From 00290ce7418c850325d84025d0dd519f1422732c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 15:25:36 -0700 Subject: [PATCH 001/154] Tune l4 --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 3d32ba0b..176a9092 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -50,7 +50,7 @@ public class ExtensionKinematics { Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.23, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); From 051b2aaa76edb0d051e54fe96b55eb790571079e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 8 Apr 2025 15:32:03 -0700 Subject: [PATCH 002/154] reenable auto release --- src/main/java/frc/robot/Robot.java | 42 +++++++++++++++--------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 241386b2..b2f43b55 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -440,27 +440,27 @@ public static enum AlgaeScoreTarget { .debounce(0.15)) // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .or(() -> Autos.autoScore && DriverStation.isAutonomous()) - // .or( - // new Trigger( - // () -> - // AutoAim.isInToleranceCoral( - // swerve.getPose(), - // Units.inchesToMeters(1.0), - // Units.degreesToRadians(1.0)) - // && MathUtil.isNear( - // 0, - // Math.hypot( - // swerve.getVelocityRobotRelative().vxMetersPerSecond, - // swerve.getVelocityRobotRelative().vyMetersPerSecond), - // AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - // && MathUtil.isNear( - // 0.0, - // swerve.getVelocityRobotRelative().omegaRadiansPerSecond, - // 3.0) - // && currentTarget != ReefTarget.L4) - // .debounce(0.08) - // .and(() -> swerve.hasFrontTags)) - , + .or( + new Trigger( + () -> + AutoAim.isInToleranceCoral( + swerve.getPose(), + Units.inchesToMeters(1.0), + Units.degreesToRadians(1.0)) + && MathUtil.isNear( + 0, + Math.hypot( + swerve.getVelocityRobotRelative().vxMetersPerSecond, + swerve.getVelocityRobotRelative().vyMetersPerSecond), + AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) + && MathUtil.isNear( + 0.0, + swerve.getVelocityRobotRelative().omegaRadiansPerSecond, + 3.0) + && currentTarget != ReefTarget.L4 + && currentTarget != ReefTarget.L1) + .debounce(0.08) + .and(() -> swerve.hasFrontTags)), driver .rightTrigger() .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) From 28120edb4248e9d6b1a909ba6508d564be080d94 Mon Sep 17 00:00:00 2001 From: SCool62 <79726681+SCool62@users.noreply.github.com> Date: Tue, 8 Apr 2025 15:51:15 -0700 Subject: [PATCH 003/154] L1 autoalign (#106) * Add target lines * Initial testing, nonworking * Make it mostly work * Allow driver to move along the line --- src/main/java/frc/robot/Robot.java | 36 ++++++- .../java/frc/robot/utils/autoaim/AutoAim.java | 88 +++++++++++++++ .../frc/robot/utils/autoaim/L1Targets.java | 100 ++++++++++++++++++ 3 files changed, 223 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/utils/autoaim/L1Targets.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b2f43b55..3a383694 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,6 +25,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; @@ -79,6 +80,7 @@ import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.CageTargets; import frc.robot.utils.autoaim.CoralTargets; +import frc.robot.utils.autoaim.L1Targets; import java.util.HashMap; import java.util.Optional; import java.util.Set; @@ -805,7 +807,7 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike()) + .and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -825,6 +827,32 @@ public Robot() { AutoAim.INITIAL_REEF_KEEPOFF_DISTANCE_METERS, 0.0, Rotation2d.kZero)), Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && currentTarget == ReefTarget.L1) + .whileTrue( + Commands.parallel( + AutoAim.alignToLine( + swerve, + () -> + modifyJoystick(driver.getLeftY()) + * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + () -> + modifyJoystick(driver.getLeftX()) + * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + () -> L1Targets.getNearestLine(swerve.getPose())), + Commands.waitUntil( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + new Pose2d( + L1Targets.getNearestLine(swerve.getPose()) + .nearest(swerve.getPose().getTranslation()), + L1Targets.getNearestLine(swerve.getPose()).getRotation()))) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + driver .rightBumper() .and( @@ -1093,6 +1121,12 @@ public Robot() { Stream.of(CageTargets.values()) .map((target) -> target.getLocation()) .toArray(Pose2d[]::new)); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Targets/L1", + Stream.of(L1Targets.values()) + .map((target) -> L1Targets.getRobotTargetLine(target.line)) + .toArray(Rectangle2d[]::new)); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( "AutoAim/Targets/Algae", diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 078fa79b..f989da2e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -39,6 +40,8 @@ public class AutoAim { public static Pose2d RED_PROCESSOR_POS = ChoreoAllianceFlipUtil.flip(BLUE_PROCESSOR_POS); public static List PROCESSOR_POSES = List.of(BLUE_PROCESSOR_POS, RED_PROCESSOR_POS); + public static final double L1_TROUGH_WIDTH_METERS = 0.935; + public static final double TRANSLATION_TOLERANCE_METERS = Units.inchesToMeters(2.0); public static final double ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(2.0); public static final double VELOCITY_TOLERANCE_METERSPERSECOND = 0.5; @@ -223,6 +226,91 @@ public static Command translateToXCoord( })); } + public static Command alignToLine( + SwerveSubsystem swerve, + DoubleSupplier xVel, + DoubleSupplier yVel, + Supplier line) { + final Pose2d cachedTarget[] = {new Pose2d()}; + final ProfiledPIDController headingController = + // assume we can accelerate to max in 2/3 of a second + new ProfiledPIDController( + Robot.ROBOT_HARDWARE.swerveConstants.getHeadingVelocityKP(), + 0.0, + 0.0, + new TrapezoidProfile.Constraints(MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCELERATION)); + headingController.enableContinuousInput(-Math.PI, Math.PI); + final ProfiledPIDController vxController = + new ProfiledPIDController( + 10.0, + 0.01, + 0.02, + new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION)); + final ProfiledPIDController vyController = + new ProfiledPIDController( + 10.0, + 0.01, + 0.02, + new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION)); + return Commands.runOnce( + () -> { + cachedTarget[0] = + new Pose2d( + line.get().nearest(swerve.getPose().getTranslation()), + line.get().getRotation()); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("AutoAim/Cached Target", cachedTarget[0]); + headingController.reset(swerve.getPose().getRotation().getRadians(), 0.0); + vxController.reset(swerve.getPose().getX(), 0.0); + vyController.reset(swerve.getPose().getY(), 0.0); + }) + .andThen( + swerve.driveVelocityFieldRelative( + () -> { + Supplier target = + () -> + new Pose2d( + line.get().nearest(swerve.getPose().getTranslation()), + line.get().getRotation()); + final var diff = swerve.getPose().minus(target.get()); + var driverReqSpeedsRobotRelative = + ChassisSpeeds.fromFieldRelativeSpeeds( + xVel.getAsDouble(), yVel.getAsDouble(), 0.0, swerve.getRotation()); + driverReqSpeedsRobotRelative.vxMetersPerSecond = 0; + final var speeds = + MathUtil.isNear(0.0, diff.getX(), Units.inchesToMeters(0.25)) + && MathUtil.isNear(0.0, diff.getY(), Units.inchesToMeters(0.25)) + && MathUtil.isNear(0.0, diff.getRotation().getDegrees(), 0.5) + ? new ChassisSpeeds() + : new ChassisSpeeds( + vxController.calculate( + swerve.getPose().getX(), target.get().getX()) + + vxController.getSetpoint().velocity, + // Use the inputted y velocity target + vyController.calculate( + swerve.getPose().getY(), target.get().getY()) + + vyController.getSetpoint().velocity, + headingController.calculate( + swerve.getPose().getRotation().getRadians(), + target.get().getRotation().getRadians()) + + headingController.getSetpoint().velocity) + .plus( + ChassisSpeeds.fromRobotRelativeSpeeds( + driverReqSpeedsRobotRelative, swerve.getRotation()) + .times(1.2)); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Target Pose", + new Pose2d( + vxController.getSetpoint().position, + vyController.getSetpoint().position, + Rotation2d.fromRadians(headingController.getSetpoint().position))); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("AutoAim/Target Speeds", speeds); + return speeds; + })); + } + public static Command approachAlgae( SwerveSubsystem swerve, Supplier target, double approachSpeed) { // This feels like a horrible way of getting around lambda final requirements diff --git a/src/main/java/frc/robot/utils/autoaim/L1Targets.java b/src/main/java/frc/robot/utils/autoaim/L1Targets.java new file mode 100644 index 00000000..d06bbfe9 --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/L1Targets.java @@ -0,0 +1,100 @@ +package frc.robot.utils.autoaim; + +import choreo.util.ChoreoAllianceFlipUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import frc.robot.Robot; +import java.util.Arrays; +import java.util.List; + +public enum L1Targets { + BLUE_AB( + new Rectangle2d( + new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_CD( + new Rectangle2d( + new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_EF( + new Rectangle2d( + new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_GH( + new Rectangle2d( + new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_IJ( + new Rectangle2d( + new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_KL( + new Rectangle2d( + new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + + RED_AB( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_CD( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_EF( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_GH( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_IJ( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_KL( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)); + + public Rectangle2d line; + + private L1Targets(Rectangle2d line) { + this.line = line; + } + + private static final List transformedLines = + Arrays.stream(values()) + .map( + (L1Targets targets) -> { + return L1Targets.getRobotTargetLine(targets.line); + }) + .toList(); + + public static Rectangle2d getRobotTargetLine(Rectangle2d original) { + return original.transformBy( + new Transform2d( + Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Rectangle2d getNearestLine(Pose2d pose) { + // It feels like there should be a better way to do this + return new Rectangle2d( + pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS); + } +} From fbe08e5eb818b3d7b2606f71a116725da038f664 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 16:23:00 -0700 Subject: [PATCH 004/154] L1 tuning at loom --- src/main/java/frc/robot/Robot.java | 4 +-- .../robot/subsystems/ExtensionKinematics.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 28 ++++++++++++------- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3a383694..5858986e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -447,8 +447,8 @@ public static enum AlgaeScoreTarget { () -> AutoAim.isInToleranceCoral( swerve.getPose(), - Units.inchesToMeters(1.0), - Units.degreesToRadians(1.0)) + Units.inchesToMeters(1.5), + Units.degreesToRadians(1.5)) && MathUtil.isNear( 0, Math.hypot( diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 176a9092..c9f6d3ce 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -35,7 +35,7 @@ public class ExtensionKinematics { // Not super accurate bc of whack public static final Pose2d L1_POSE = - new Pose2d(0.26, 0.4, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); + new Pose2d(0.33, 0.50, Rotation2d.fromDegrees(20.0)); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final ExtensionState L2_EXTENSION = new ExtensionState( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index bcdfd0cd..6244b939 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -23,8 +23,8 @@ import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.utils.autoaim.AlgaeIntakeTargets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.CoralTargets; import frc.robot.utils.autoaim.HumanPlayerTargets; +import frc.robot.utils.autoaim.L1Targets; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -416,6 +416,9 @@ private void configureStateTransitionCommands() { // PRE_L{1-4} logic + -> SCORE_CORAL stateTriggers .get(SuperState.PRE_L1) + .and( + () -> + L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) // .whileTrue( // this.extendWithClearance( // ElevatorSubsystem.L1_EXTENSION_METERS, @@ -428,10 +431,21 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L1_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())); + + stateTriggers + .get(SuperState.PRE_L1) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) + .whileTrue( + this.extendWithClearance( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.L1_EXTENSION + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.L1_EXTENSION))) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -569,10 +583,7 @@ private void configureStateTransitionCommands() { .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) .and( () -> - CoralTargets.getClosestTarget(pose.get()) - .getTranslation() - .getDistance(pose.get().getTranslation()) - > 0.3) + L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) .debounce(0.15) .onTrue(forceState(SuperState.IDLE)); @@ -583,10 +594,7 @@ private void configureStateTransitionCommands() { .and(killVisionIK) .and( () -> - CoralTargets.getClosestTarget(pose.get()) - .getTranslation() - .getDistance(pose.get().getTranslation()) - > 0.3) + L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) .onTrue(forceState(SuperState.IDLE)); stateTriggers diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index f989da2e..12139d67 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -297,7 +297,7 @@ public static Command alignToLine( .plus( ChassisSpeeds.fromRobotRelativeSpeeds( driverReqSpeedsRobotRelative, swerve.getRotation()) - .times(1.2)); + .times(1.5)); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( "AutoAim/Target Pose", From 830acc9a4e51dc2136b85333620df2db314d2b9e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 8 Apr 2025 20:41:57 -0700 Subject: [PATCH 005/154] Algae autos fr (#113) * added new auto command for circling the reef * added steps for auto command * updated auto path and scoring in auto command * updated auto command, scoring in auto, and intake in auto * updated auto commands * reintroduce handedness * add adjustable speed for different scoring levels * updated auto commands * changed runpath to use start/middle/end locations instead, turned off module force ff in sim * why did i do that * make autoaim start with velocity when that axis is towards the goal * i don't even know anymore * add starting locations * added starting paths and stuff * 4.5 L Outside * 4.5 L Outside completed * added autos 4.5 L Outside and 4.5 R Outside * added autos 4.5 L Outside and 4.5 R Outside * added autos 4.5 L Outside and 4.5 R Outside * score preload autos. also i messed up the command sequence lol * regenerate all choreo paths * fix demo cycle * added li to k, not done yet (need to pull petro changes) * respond to feedback * regen paths with correct torque and a few other fixes * feedback * change force ff back to its own thing * make outtake use volts * loom testing, things are not working! * remove dead autos, rename test * adjust test paths, regen all paths * remove removed auto from robot.java * run scoreinauto tests * make test routine use routines * adjust indexing to not spit when reenabled * lm to h works * move debug auto to LM to H * adjust choreo moi this time for sure * adjust alpha swerve tuning and choreo settings * reenable module force ff * bound elevator extension, upped i for autoaim * adjust command bindings in auto, regen auto trajs * adjust score in auto * started making auto use superstructure(??) * auto works in sim * improved autos in sim * make chooser work when disabled, extend elevator a bit later * premature optimization was the root of all evil * updated auto commands by adding run path to simplify trajectory code * added all 4.5 inner and outer routines * added bind elevator extension to autos * premature optimization was the root of all evil part 2 * standardize naming * added the push auto and updated run path command * finish merge * started adding algae auto commands * add algae reef locations * delete new path * started adding booleans for algae and scoring/intaking algae in auto commands * updating autos to include algae commands and intake/outake (still in progress) * whar * changed some algae intake logic, need to fix elevator extension binding to work for algae intake * please don't be terrible * move scoring and intaking to actually be in sequence * actually check algae * adjust coral g and h and add cmtog traj * more tuning --------- Co-authored-by: vivi-o Co-authored-by: Lewis-Seiden Co-authored-by: petropapahadjopoulos --- notes/algaeLocations.PNG | Bin 0 -> 38607 bytes notes/fieldLocations.md | 5 + src/main/deploy/choreo/CMtoG.traj | 76 +++++++ src/main/deploy/choreo/GHtoNI.traj | 65 ++++++ src/main/deploy/choreo/IJtoNI.traj | 80 +++++++ src/main/deploy/choreo/NItoEF.traj | 106 +++++++++ src/main/deploy/choreo/NItoIJ.traj | 80 +++++++ src/main/deploy/choreo/Sprint Test.traj | 158 -------------- src/main/deploy/choreo/Triangle Test.traj | 128 ----------- src/main/deploy/choreo/choreo.chor | 46 +++- .../choreo/starting location tester.traj | 206 ------------------ src/main/java/frc/robot/Autos.java | 198 +++++++++++++++-- src/main/java/frc/robot/Robot.java | 39 +++- .../subsystems/ManipulatorSubsystem.java | 6 +- .../swerve/KelpieSwerveConstants.java | 4 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 14 +- .../frc/robot/utils/autoaim/CoralTargets.java | 4 +- 17 files changed, 678 insertions(+), 537 deletions(-) create mode 100644 notes/algaeLocations.PNG create mode 100644 src/main/deploy/choreo/CMtoG.traj create mode 100644 src/main/deploy/choreo/GHtoNI.traj create mode 100644 src/main/deploy/choreo/IJtoNI.traj create mode 100644 src/main/deploy/choreo/NItoEF.traj create mode 100644 src/main/deploy/choreo/NItoIJ.traj delete mode 100644 src/main/deploy/choreo/Sprint Test.traj delete mode 100644 src/main/deploy/choreo/Triangle Test.traj delete mode 100644 src/main/deploy/choreo/starting location tester.traj diff --git a/notes/algaeLocations.PNG b/notes/algaeLocations.PNG new file mode 100644 index 0000000000000000000000000000000000000000..fbda28626f0ec5b1fbb416b024f28be071faf657 GIT binary patch literal 38607 zcmeFYg;$hc)IX{SA}x(WNp~nA($d`>1CmmLNDMp7{F&N7X`!5j~)@ZJv@(BY-k~m9;H|* z%DmHofcKZrElmczxcw6H!#f_4wV$teI~z|vs|{>Fr}v1oIA{-Vk7!rstmm2s{@7Ev zh7(MP=G7eKV~AIMv_HCjcE0;^?!98j`32)CBiWoO)uVBOoL>4L*<#**=hHoHc^^~I zHHXQpZ>j%1(%weKcI2AfPTfzepG)WmM#!OZfBkzB#PX6gIL4%zNt~s^!MR-?Y3M~# zdV|*^!fQg8_6Ya0_z;hyztcjQoM!M3$`8q9IzD-&sFHX{^WDax2p>lLTL7 z64_6&zUIgd>}EX2pm^+a`=pOo(q0ZftK%r8L38)x7+hfJ7OgNAvwD;ExlELvbBdqZ z>seZVT3|z!xasCZAY#?QVqA8gq9prqin2Q}-?RjUIvxRO>;l_;+npcZiG2UGTUec$$7XpSmjc zK0~v|5Y+y-2S4MAugr<4UMRBRrR)6oo-v|CVF!gPsg4_!UG8JGuXVl=b1Lc|;L2Im-S{5*>05bq`9!NsTP+93ETao zh_kJ-YMqySk@&gqc|7X!u}&l<85nnTd0CpI!nLWr@)d}chG-2+I`8mktG4OSG!5XKq5fKJ-Ii&6MZz>w$cub_}8;n|5eHcHevs$GQ7Sa|uvr;U91`QKleK+j9e zlqp}z&-?BL5(VNb z3_sLF>4p-@{6%hrz@`Gm<#q{k|R%kQwLL)19Jn^8`V< zuo2J9EE*Mq_G3{K1Ila?4qvWEK@n|Bw@=W@D&^EYJ5DX_o~)~gE~nwJSl8fDh2eme z%c-;hb^u)Se|{Y5DVeN8tU|A4QReDjmG+JShiEKU#5v#UGc%dxlj%bzvnqj-+N5ex zANhsSW^A5`;-e`gGZx%$-?72_pKb>UcwtY<3o^_T#9zsC#xUbNAM3_xAnni!1rt-a z%}g65PQ6B+I>Z)6hgW2^Tp9#3&hLfWRTjbIN{R8S#FcC5Q499{Ipv~I>%+Wn-BOv^871T>5sexTeR3*mTX8mIUZ6i~~LDN$kz{29H6*jmhc zf4L*ZsPXtn`G-BLMEd=t=e@q2Wv<-$%B6za!SD+CoID;tp%(wZ zC9eMvzc~_+Ojjh4Jwyo7Rp_HHAZtCKY)1K0@{mWB2V6DXlW4IGQ=dr?t@E6WV--KRw}En)1%uyyq2c^ zRHFr(DG`ZFN+gDVDdehv=hyHers>s_pBOb=>>Q9r{ei<6+ECA*UOI#-kjB|}c+B2O z<_VcE0Rm^olNX6Kohc|hF4W(1f9Cd|1hR>?TqGQmLo0l+v;)7sh`c zB+{ZBS$5a;2X8ZJzqI(53hw65pDyvaf-e{tlkITSK~9=+P241+x@qx z$q7<*rWIp%HKyOfhyxMSF08;2jo5ivbxbkh;ZHu?WQCNfbN4D3#YN+q6NM{=N z^8F7Ky|~GUSsfTx@mI`~Uxql~Fzo|FeG5ofl;tbf6a4&0^8_memi6eabZ!&>n)G@b za&tpyE#;eufOmOGX$kLGYQ*Ef4Pli3`nTj}>Yqm4G=JgZhL|?n7oZ^c72k&I_!0PLcS`t_& z0WwhEdkJzSJ+cz;SX-9&{iUe&Tpva)RWLijjHU{m&Bo6tyC+gIK~xk}N{a+pDd05u z!0~%jXR;iNMkSa-L~X*ZnFO{lEDY;$I5&r2?iB&*r;*C6=kIP^6M^&~wjSr`Ew4E= zc?>hE5L6gZn6F)b>3h0$p920WpTOlE8zraj0sCnoLTY9 z+tkeXjUR9UI|ag#9O@|@G>^EAHqr$@e*D+qwoi72^1P>Se-x|po>0?*SZcnG5_z$N za5oEe_#e#iH9ZFi+5!YQJJ^$P^>+c&baCW$(lLVcBB6T0=F7$F8Zm9uF0g?>%JPwM zmeBegVIna+3l~4HB~Z*HQ1`@tWCZr+uoeG#zenUTU~{*$ba~&uaB5M@5+2D5o>gI6 zF;Uc%uaHZSKm@FD5578<<1=##i6&TJ6R}y^bL;MvF)d?!Clbu=d~Q6INcL*Cc*li$ zlZT`DVu%DgIyyyM{Htid5RY;8r<1B!Kc;rcz}xEf(`*AbRS%dvn&u%WGEiUs4}uz; z283x0sXIXtr0ToTW6FhA+D|t}I0_R+NpKlMa9;IMY^(6;Th(szQOx1E_xK2B-RxZ4HaTC! z544vKN}FaBS?S4A#s=qjel+lPjay4v{F;H1xjUx|4D&1=0|z6r2i={N2UcNIb2J|?+Q{#t@j@$spebM!cEXQA#qm6T7} z#jjNA`|4uz<3DXV;h~JIR#vw|?uK2v&#;;GW@(I}YqfE!zh0vPvAzTy)?6EDC!zv{ zK3GMM&mO=V4Desl@gsK3oIchRn|Fhl8{$)&YETdY`I*33`GZ>>s3?^^`cvZSdBg(s zd|emc9B95a@7IXkO< zvZ>JXdugb5BL?w|pzUCIDUOR5upb;|CSBoID$F8hyfC`7pWy>Im1Qzvz-B9=`;R?h58IYK*uEJwhSg86w<+(GwpM1`;A0 zWPAd(H^tp^l}98uyL%U#ZE&0-Wy2uFJ*`&win6QsyK54Jso8mRyHU{KfdBRQ#(n{v z)6c`}>0Nl>Y_*&MuVDj}7#l1vgiibjgXeEj+&A`ijn<^fqszbY{Lv1oa+?#O1Z_8M zQdM2SRm-jZ2Hd&_|NW;V@4GGzUUcFw9*^JUH5~l5Mq*4^Lmhmwe-I$8XbWvu-R{~l zf(FS~Z&FKFU#Ja75$s^sERRKd5@Zb8I^yl$l@B{D1S$S3&ufwX0q9#^6zUzz1G%N` zVEKLpo;2>0Z)SZnbXm<@J0|9;e;aRE!gL84I#Z=>Ym`?jrragoR2$v39h+?NAFGe0 z_jQueUTSaMhCU;K1(F70Ns{}#-{{xzK)6M0xzBpyA}`<#B`N;j(Gw}yv)`sNEA08z zEL!5C;d}Sw#c7hj@c;fk{Kz0ugldrvxiDj^jAYBf^CR>B?Jv&Uno2yQiN_!ovn2X(}GDu*i2DkbNbOk3N1m z%vQ!wc$)f|iSSW`&nTsUaRZbO8=Ut9VDgPg?O;e|Jq{~M-AAjOl{ba@AkTaPIF3go zFey(UXIwrc(a+u)(gs_TYt^nE4SR+;-hOHPb9y3i>x}dc3Sg53xC$~yd)XBg1Rz8c zN*8j6kllm45}7~J`~Hp#Lj4!W3(d(^_c%WmrvSk61$;Pj zYmzizd^yZrY2LZ3?kDNDkf(`0j8Supppz_DD1=wTVYS%`p*dlIL%) zs#?zD(SXYnocCv1QK>@n5W8KP_c5f}1%7cg8Fb)B-~Kc3mZsyJ?UHqXpI6Al6?V<@ zk~0R4UaQFeJ2^Hu3lpQ}kRFf>&(F@iay&eE6emi3s_@0eW>q|mvo}VlOoG1~jn@Vj zQ`4Gip(0${{KHq&58r@5{Vf2mC=5X8v=D zUGEu!TXBM~O|K|k zvqIdnoBS?L&H`ui+Ql9V(LOMC1ftsdC>9TP>++MhStH;d8;wRo+jl6vvB3UdGDrq0 z=u{~fIrk1u!2gTMzu=P9ueYZyr4m&7E?z7+|o)z-5o$>xakwzU53BuR_bMTYRNWg_xCsrx%e2LSD)FRsx`k!DQy z+dh+v^xi%sFku30a2E}*QI9`SyOU8(MdW6+tLlE&oxkMQInG!TsGT`-8uXYTCbj59 zCr>i}bhXo)TntGIK1L{RBnoGP%imgmrVMxzc~soYHCsF<+R-2Dimg;wL-_P5MvXZS zV0J_fS<>%2gg>}hcg#p1q{llByzj&$*P=V;|O-*n)8`j5G z5kyANnViG*uajnkk3*d~vIo?aA0jWib3&v;@SSn}UtPcom;!L#>uqQDRx?Qe2Y30I z-ClO~y*izJ+K3sYa2z|td>%NR|EAEkwwVPR?5mYgn4_-mSI5L36XvPXTa4qmJb zpl~<uHo?w>r6@AAE@I`%Ww&1L7%x`luAid%&om5(Rl>Zr1L%-}uLazvWE< zZic*S(c=rumw$tjL>3$YNc-3&R+4uJVytpXI^|#Rxo}{462@0NF`SIqtD$y2+tbdT z-4X>rYoz{0EYuVH45Nlu1Hk!{KU=h4m`2RZcLTI7glqjZ(7pO}Cz}(T#Lyni#sk{I z6YAze?7+s$uLp%?y;I?d86uHwgl)x?N+8d!INRS$_3_5}n?tq^$AFOZTr?p$?+@wI zXFx^m(S<%;jeo$u+w8NfRuzQiE0X6_ax0o47M5c3t@KOUud3ZBy-FGr1nFg=-%JA> z3?R!S*~OgLR&DKLn*Evl)sAhO?s7J9{XCMm2WiH|e@>ij4H2Z$LcF{WeD_v_s%1B7 zK=WuK)J!^9pu}i_(t{$LL87i?L;4m1E5)fC_~Gficatq}OA!jRZlYf-Me$3KZ>A`g zi)d;x??2(M&77s8_ArbOaO3fe`(1~FjZYtq2|48iP+ z%8O0qIE#1>%Ir72JV><%vk+*uX>yajNu-|bvN9ACT-Nf}>WGq~be`+L>{AwCk>&bo z<|CUrFH*+f0RhbTA69^C{6AGU_J6s3=P|sS+m29SKt3S7@bl&IWN4zg5l%FR*C;&A zGb^Uz#yNYxJ1YH>8QIrLUvF8)49ku%b0qY#8P7}YMRQt~p78}4d&R+$I!{0x)IB2W zB@=*FRiPkw^7((Jr<8G>hGH1Lhyl2#k^v0%LoES{pow|#G}GysvX5(hRb5?K3_Kaxu`n&i-l_}P;^&z9`Kn%;jR)O6k-5S7|-u9h=reHMf1H0PxN z-3!gE2Z;fO^8LdCy~gYE$->`J-D2N-uGh`9Pc~lU{|NPtgcM^F*d0!U8XUJ97l{4X zq42U*VZPrMMSh}GY9K7LsC!c_5zqDPjhyE@6qy~7yTyPN%}RtiXr)I4wkX)CK7WMv zU>+V}u6?|zi0$BL|3cvYpP#Voay>po(epS*e<*CPw+TO0uo9p!VcbdoK2fg-1{@#+ zPZl0fzUI$-bUk;Gr?INfFQ^^OYl+g2o%LFqMy7q7a-JH9BW*-I@ zxGaqL3H0rxEff+Ri$7V50PYM=)As!$?~^8 zPa3%M7Y;#5=cICXuP`?B8Up&aYn~sRIZRP-^y^VQm!^J9$d+&W&ci2^vYHL?rF%EW zO@f9B-h3Bq%W_%xJJIZV&9Mh48UnE5e1KK|b$Ox9Uj?4O(tT&rC2YJxI>DTp9C`d5 zu-7{u2o<2!Uxka$zER8|NOh)#Zf8j$x5U}PBboRf65^sT25c+kA8}@mpHdN7gVO&4 z)(5I;KRY91fv~+jw_-kE$~im=t<&!x6XHEZPLd6wsFfHaY@TMk7m-Z@e(TqCh4j6m&Jq$+ZH^#>YoAss}duxI1d!D0kr zfvibpdpbvuy2Hy!Q~KB&()<{iGrv5~$L~KCD%#}&&K?Loj6oWr*n07TuM=TDH@{Si zdPHJZ@2LR(-5&Vw6(h_%r%DEIVD=bGTzp00Rc-9nFwsVL7KO=RF^b;mR}M~E4xVE2 z8~N*2+7E?f)t^7}9dKvLe*3wPH-=&jNv6@iT^;B7m;V?~^?7vXLZ~NT<*L7!qy3Qq z`Z*gVc)21iWfzy8Z`v!z?6i);4B;mNTdcLrnDHU{A-?lK5AdF6m|z5IBr zmctoo-q!h102?c+aPC58XmGS3GIA&;O5byOD5NU4HasC=idEEg+mZg4Lr*h#bwR5A zIO+z5`=)n-falL&B;{TLBJ0?Q0dNXMfKxDea0-bwBdOpU%(aR6T@unv$pDy8hM&CCA&2`2goG6ZPt-kAf9W))>SYyim}iV>D??!Qo&cH<5QBY*azT_e z#mc!w4}%_ixBKlAGL=|Ih6d?pG>XRxvS-3xmqbLbLv9H( z>QtaSxhRIwv_&#mLsfOFHd@!zG;88^wF&|$sjM{lozu%or*=kV-yL$r6s$8kWH{gxvHB!pN)=9 z_IJFFp6=;0+8N2?d~*xks$fvHgre%OMz`7lxv&62Ed0ho`4@*nOj^hC?PqFh7+W#9 zx6LTCy4i4C9z&>SN~Ldf!@hLjqv{8sT6Fl`xos`dDp3Y)wVYJZMqyOnJV8@j-*vuD zi*Q%ko@0bcLj_Hm`eK?^kcl$R*aRnT?KPlJUG^#_`4oX{HKn-=U@3XCCyJiBO263H zx$}BAe$(;6dfJUUuo^8+P3QTY>z;!FbgA&h39_o-9(-#%m4R}%Kf?yU=LLGT&1tdy zu|p1D;ZNU%QeyqHJ~~~WHgbx3erplcTl=+?0r|?YXUcy((hPq0DkT5;k6qvCNFGVi z4J{)h=1}w2r`YoAYx6J|TBI@sD5TROR9A-GE@8KilYPD$VzM#P$d-+m2MWBKOLd-* zuyG(6=r(@`G?>ws%n#$fR%+HhG~}j&k)LHvu?Zf#w@awsZKKrXFVmK|8-_S4zt2UOj<}2xfzoB8C&F4{Xv>PfFr^J$0 zh`&QGs16TtSl+xj7biH#1)^cfK&+oH|Kw3r6UPhS!k$yscy&h=9M$M?XEd&;tK6#! zMp4_f*c1-f=V4dBI#MkqY(w|-Rcl;Bb}m_VxijkOPPgMP3K56Ba_TUq=#Kzk2Y5>XReXwdAmVy;l}R1lZR>yU z++aVY_UgwtPW_R6m8mn&nLgW!W4bu|yje(U{JEkoC^!5!ou+*E1zJ*8B1bVw*n6&+ zM=00D8E-a+k&&4oy>A`}LQ29;NGv9UW=@x$R}6ql>U0MN`SK-yvmOCD`pX@U$&n2k z{0`$`@rZt&F9}oMWrBW+JioqlkS+MBJm=Y-CHOi>^fS33wlPzHRl`x}@p;Ia$5P+w zX@`9uMu{CVUB^g|8!e(D)afeF3}+o}ehKCi8To>JDvF_Rgj2O%a|9l*O1cI9@u_I> zmoy~+?HOl@2|^{n!Py4@VSIRVZn7;Bo%MZht5F5%J@(%$U$b&y8!X1p!)0S^@594SLA{ckY5D1AnNn;~QyC4QO zVB!Jyz(5buSY?eyfeV<}pzxk>_eMRAj|MlEILOWVtbIJiamntAANC0-r6(kOR%lpe zj;<%z!l0XKI?2(>n$%r&qC^PvXo6w|aYE0_n6Y%Cj}r2)+#+>e*d{b&M2je1{L1sC z3rIzB(Ng3Xm41gcZ`JG+(Y451+K)d(oTprIWT1Clh3eS6 zF&U{g60QX25s33^4N&Qa8kxe<)oe^k^iR$vdxy#jD|TNzd#dfL`h<(kF9hEfoWI<{ zVrPGf3s?W0_&y-MJY_}$v8FX-sv~3g9f!4%DHCWu_j(h=Xhfqe8v=A<2T{dAFN|veMpiM zVib;C8%kjFd{3uKU0HC2Ir{tN485$lq>BHt*G)LnGq$?OCu&8(E7N1^TjAA$T&6j1 z8p<7-q2HQRT_{di6JKT49F*~|QR9T~^4g;H#D?8SUyUs~oh0QsLtq9f4UeeqUTp^g zEz1ad>U=IVF1K5g=-st*Cg2>ZrcQ+`@o-$hJU5RKgFtQ%qztWZNK9~e7hIyj+>2a# z9(Y<)Dwz{wl3KOpNx1quvfFV2bIXmUnX3Bm91s4hk}2n@goNqB*%yvz-R9{UVj*WX zohwIID+<1Hl==t7N9xRqtLmQ7&W#$E)%HO1UpDV(vV8X)!fwNaldG6q84a7@;1Klo z4yMEl)i8e!cmrIte{_hx@n&)vVQ;BDe^&!kz6&H?tUp&1v=H7PG?bPBB$222bSVN8 z?dfiw!qgw$&Q2bFzm>K9V9}!ke*t}pq5nE_*UoxbE_2qScn_az@w?g;7&?WjOde^< zEH!s$>q}wObSWHysxR!OE#b^IVkUj0L}v$A&+*KROv)8UEe_LQ{(SM^uET8?T=ZLP z$IvDz;g+x*{k=+YMH(;V={Jr&Ng71SS&}VLQNLUo!3(UNzb;CM?N$b@-8v56$*=p1 zpFeJg*N2Qa;T7_o;o?^3ItxxT7=A}W`Y5Suc3o#I7wR`2slp^wufRYn3ISnX)K!gW z<<|_N7b^}5AD73c6%B#l*2GTvc8PCxdacV>OK6%(hQ0?d^%Da!lC8gu8vF2_lWxUV zjEE`su-_4Ok5#k4?)~*0d3kU`CHjz=rc^OyG(5DqhIz%AxQ9XiwQ!hQ%x}};c@DCK z_bHR7h(bek)e7V7=@-|+;Bm?_=P5*}GXK_y1=4$)1D}QT+MaDNu6oF&8-hnwGh&Bw z?d!+G`21f{E8^SO}~iiwfJr76{RsJuOaP?Z1x~WZ>!oacZkvqC!zbfhrukcK>3E z+~(%{ARN3|wi9Ft6w_IUtv9ZSYS=j+PkH&PgW)_3F`6G=QNl z$h7K8Q?RZ0;Zyti=reG+_&b`nIDPv%W(E`>x*6uOC4CMj7tBD)yXt^1Vd@@ZqH5as zV=sGr3QnTlQ@a0l$&E<)XQ-UYN9zg02U_bLpMy{W1i=Vbu_E;mLcWZ`0V7$6Ds@g0 zGf2NaM2@m13eg|)ISr?wkL1kV0_*0>v)?&i)*sS)igP~~o(Q+fj(>p{Oheki)1r=3 zx)3+q&2f&r(ChXZb+{Rqvnm-Mcr0mU{~5+sCKDac8*<=wYWOaKr%(d zsOFB0P_c9Hu=GeV2#D=0uG!soO@)!u7|OEP*LVg8DZh1|Y@J>#^K!>Q?zCZGW4==1 zn;2Qgal{5+dYPHl=NPSiqxY?fnLrgLk9dPhTk}G(v7=bo($7~l?i#v+?6$Oz) zN)PD@khLIf^94<$r`;_j(TQF7AL=fYkzK+5bthvjvVBZCy1HB*Q(1cAnfTUbxli1; zK3rCx!k^B>+vJ$4gn@WG&_gPa>d5II?Z4;d9En+}th?Z(g73C1{xLwUPaMXV2?hvl za*Smbi)o9cF~~nyPd>FTwbPL@=StSs(U7zG(abt5&d?Z#L9B`$h$V#o(DlV@G$$fX zRcP_TSW6YbB--sWH~#ltSzT&q(XIT7dDPdptRc^xggbcC{AXVc^?SN4!qIB*_Mxwv zN~4Xik%unqjbC^av}f+Iy6cV zIk~$lNosNr$;vY2Ul^Rt<1qsQ7km)7wl^;pUTpD35;yL?R&1eXe=&E!JrNrKaVeYlo{% zuLjSdU5hHRUGK#d&+v>2V<=4SPKCnk+h^AkAVq(7Wr+Xu|6AOF7vaulv6fI;TYJ$G z(5qxtQgYj3D+1E|_$xM}dZM>2#%#5qVkS)c!@ypFlKJ+M{MLuRr1q6|2>=65L#wK( z+QQ`8ZAqtT3o9-kyLK;Fh>^+qO>@5+VA)=oHacTWrdc=cuN@@ID$DaxUpcX3scsBS z0i-(u;RSwizj-&Fk%w9~1^xEIVY`BH-$+b0hKKk)UDa~!C=w$dCx(Z7Dy&WXoFk+r zA#dz(DfC9xCTWzkhh=79W5BG*BMc=V^wqTz03@Rz`rZ|cQrS~8sQTc2)DtB`!xmO8 znvuTw=PEye=Mj~PYFBWW6XeR-&?`ihVOAfSmHk6a+=}>l%x?AM2oBC>bnO!-qdNKD zeg-+`QajQhv*m>Y^L%}&(dwmjH>RTREPt)0xsablhz<&2c6}=`TUPQ8H5$e}oXA}t zLhiUx9##_B;UGed|6OoI!sajtYkd1p_h9~-1&}!p3Cp~^&cthvVRp`7-chcMW)BM) z9X`{HN_2Q_1I|#FkJfjTnw(jZP5~3oJNVKtjc~Ggg^U%ua~FFsonmnDkP^Ipbz`4`}yx01*=&cK2Atw_pSBPWfj|2HQ3)V z3#U%M^-WW)s?F-_-XWP5=F{B)mFc~1OxxAQw>GWy`nGJ%QhHf;(?T7Ry0|KSt}yV~ z6|vh9VO#37>VETQRf2j?|}6JQDjQ>9iQpi zTCs5P^@BAfpW`q&y~Lrr|6-2g^!)PGz2xyJ!ArBwNAL?f#q|m%S~LIS-*QuP%}`LX zV2)hLmD=J*m?^TKRC&h^6yaIIx?aH&KHJw-pfjsvcU83EAUr*((VAJK_4N}D>2^sJ zn52V?r}T%|c`d7tSh8Q@4Eaa$saVy_-M$rp`dQwWCo$^7HXi!2@P#62rn+qCbCT@FXDEJo4>b z1xh5zv()AMH<~0bh~?{sY9@P=aFvEfiYj6BLp)`<2}4Gkvn}a6BcoO{5+NBZD<^^0g+9R-Qw?5* zIh;^k2pwNFU%K{>aNNce^W2>LlBX=FjE0W4>3Ysy(>6nV1pZtLjgcdZAcfhDZ@2!H zO3K5%eWo5`xFmqj0tyv#A<}XNR=60f(LVl^R}+U|(A63?`wzQqmZmo-oDWww*8HCH zMn*u-*{IR$za}IwxUFPN@Gaj|etfD!I~94@Lg{vMY%1(~t19e&&1UL#fp6-6EGBQn+h(jQgfe?=SS zdh*+#|4$Z)O7m`Z)XO$OCAB>C;49VW2J-`KVPm+VB(&r3an)jNv=Qywk8f6r-xv@2 z54YTZfTzUkKLN%g> zr?B{UZ=_y%4?@2|ZWCtzQs>V2CJyp0*A4HUGjoY)L;rsfIV}mjfgZN!G`vA|lHlE* zOWcc0PWaz6-E)Gj%x-%8g^TiN?L`G1^A}$wO$91yxn^v494x$i8?vJ10Izo-ax$qO zD1LsXaBH364UK@s|InUV*RvN|lnKRSG6vF^6(nZ1B9eEeC-t#>Y;xW0mAN8v_|2z4w`TBa-!mSZW zy0kCffoFMXX$e{3f71yMrEt8w2%AVNUze6h<7Hx{_zSY@FZOa%@oX$WY_2>4f3|`z z2atBVR$&rUQ0TKqdrEJer0g(jl)uNYQyuuwaM_{yr#3Ye2`iNtoE@)aiPNHbk2rkwY=^MC)U<1d57LU2W^ z1_`rTAsB#~H1}KO+~KO4{Fb{qs>Wf$?LrS_M<=eOoc-!^)poDG@I0vMORW${+t1|! zowztSF`~+;p(J1V$v6A?XU;p(hVZA?UXW)+cUHtNQqz+(EYj~XOL7zm`>8kVS``XL z&m99p3Sf>M?;aNy04QUgk(hju5DL)1B5K|Ne4Vc&PvIQjoTRJP-?4_ZhSKo9y=|K+7gUu-BYJ zR6M9Z=^u?pSFpfz!-RturZC8&f7e5$^R@nh3(oigjtP%oqE`u#!rvFN&Qc+H=~}oP zvebhHC#pqVpGRs{bJ7o@t`Ze`^w@tl#aFLrd0H6NpeGS4Nw!p=Cy`gWw)u^6XKtQ& zjV8Y!Jt-U*BYm!7UJ7(_+AgmgknQ7sEKcEFdMZlX(QGT?oIw#CYU}xl3o+tSgbuO5H|HNhQ!1xnlzku?lcga(pXTQc3+DNbJ%FjCe);C4@iBQ5 ze)#2Mzy_#-rgro13RPAuuvm#ttv30PPQO+a`-Kxxb1i89D<*K$Mpn*EbOYT}W2_gl{Keai?_a2-)y=3l;50>s&MmN> ztO#nW@+qv^zaS$_y02On#>sZFY^Yoq3MnD%-?u25dG)vL2i<#KHbnTYdhN zXeHQdnMx}o=^PF}@ueO=z_nd+a1~uQ8o+K0!uWr|nko1)Jp1-S`2i_fuETaJU4P$G zsu&n=stBjKcNDz0*~R{m*rM0r&HV;-6zab5zICHsIN|P3{$lFrEL(4LP9#dZ=pba2 zGe?d68|R~1V~iFt-{;PFdn}1gLNH|=DF9R2Y$eM)?f8tn3qC^7ExzDsgKZ2L~41* zVx4~*;WnE)YU<+u232VlM8PoiB(MF97rvZ{H8pf4-2Ncxfa%K3-u~e9{^;dpeAs#9 zbSZAp$+cOBmNc-x^jVqEv28U)kXOO0Lvpj_;w@+FurezI3I%Q;gv~_;%|h4q-$ESl z@7ngn-W@#feuL3|dlp-W_iB#4+^%r)ZAGXr?|WF#!Pny!{w6o9=~@188lsOsEpant zmqS&hC3-O^Vm0zUmd|;It!`BwUpCabVF>8PrqX0~{TVSocKsS7XU&1p|;u<4NRs*7g zD?7lX<>SiERAQ?sr}lW0EJaJxP#}4FyR3gsA+#Usw@_WQ(c<5U-*2PrX7Y6#T&CrW zI{YZB=38S0_e5ok&$4p&!^S86t&<~x+X~yiLX6#xcb*7;K6zydJLhV>+HP|@nZI^B z7onXjIQLUh7r(&p1!g~{JpIp%uMp(^xwQ8?=uG&cK zmY6++tEXFREXBQrIMN`OAJ_!=VvEO&6~Cuc7KK!p6>U;N1yR848xh_uFt#Tv?SD2y zJUP*M^!Hpw+W&xBS^6F&Zx$YU=bd8+x+3tujm^)yzoflumzS3Ky(i?Vx97(#5;Gz4 z;0!#AzU!diFzw*k*G3Ye6> z2HFwCMbp1VZ#w@ec;z7?5K5zJbCbW4MK>n4m_At`g_Gxw_@B3_#nM|#K z6T%*p_1f3qX`+@XM+n9FAf@edjs+XGE>({`lP-mQDgWN|=>*D2DRAqTz1cW;9PF`SWo31k=FY$i>w|E3G&g<|=?O^(DRK zicL^`L1G%GK=(=$J`fOabaYyx+7_$uu<-JaCnznclRp@VHexSqK);$tWQwCttybnG z|F%bywSju|?)o=vUXKVGdE9TBx>?6E1ehQD++s7I3cqe(%bB&ZHz>sMTNIl$<2e&JObWlPCEFpo-I92lxO({|?7lv2zWj~@D=8zBk5QK1pP z$_MTTPj`IGMGd|s$^N>M7_df@C|l#0;z!suQ(dq%$k4ToGOsVSDtbn+wEcE*@GB*M zg(F?yAdW$h;%9f9_7qP^C}&oHw-tm~768AClFE1@u;Q*r&W)U-2J3_3PCrfj-LF!v z-HlE#`>Q*&JtO-^0zye_lbb#N7Yc&>EVX(#gZP|M9sjTcOUrCT|64kuZj9A~Mm*6^9;!l(^tpRtB=nx?1@74Ht0My3lDR_M z2TK64lrrI$wK3w`7cgoYI?9<|(pI*+35m$heakUx*d=2pw5C3EJYN(FP8;3jRqZ81_e@~0}cSZJkv@V8HG@(3}gx5j_B53OtCvh^u(aSdNw zSw}W#Hl`${BC+%|Z^M8<<76sg4K(XgmyX>}9!2+Q-iz<~-Me@a5GfDfo@T;2@8pkX zE|g*!P-?(s$6z`TKVFpSHgcTFUTdQ7&#S}$xCWF6*DfdOVAU| zizeB=nJM9UW;ofDyah>kO!bhxcDAcERDSZn3?{KnngdB*S9%Wlx`WBSpO~yrUa=Z5 zm>lqa@`xYEN@LV1iN6b+XIdrqi|JqFvNAB=X*PJ7CTbQ-lNbRlmsr|H(Sr4EM155E z{Z??31cf4Rg1R40;0kc;S)!utg#(1pk_>aA#8idG5?tt0Q0f;cX{AT4mXVj&ku_!U_|+vi&tjOP z{o@{N@54m?;OMgHf55A;~A_v#X-> zGz;X|EIu4v*Ds3ZJ-xd&FjIc1Seo~g>6ea8SlqZlM-Gp`l8dQO%iYEG26^Jap*fLR zx|F-ED*R+29poIt(j%&zg%vn>;*Q6Br6-n7z^@cr_T9Q+Q*k`+4*OXI9S^MppiE*d zEp%yf%A{S4oZK0}Eq0CdbuuLRaJs$O8!ai-43vAow;KPKr24-VI)lefnYd!8WY_;X z7TbNixnE6QDi-^HsCw(DsJ{38+dz<(kQ5N8?S#8M;HH zL24*bLYkpt==$yP{rRr-JpZ^{F4xSNefHUB@B6;4*Ojd74L@@BzakjY*p*YI`qDw|fPMiMtbI-*wbURttaJ9_<3o9;D z?wWv%LV6cE59W;bA@VT)`(GDCC7O^98WF?^T5vGXrajz4dis^6QMCv{Q|{@p*plsv zNE$7gpHsf8xG|b}iQm-vJ>yk`U6yqFo?kN)@25b86G`vswFQo0l_f|VzcABz+6&RM zU8WVv6vQZCD^RL1q_Gtqa7<%(L4TO|SB1K(JY?jA6DT8vd7cYsEc;ITL%wdEWV;dN zu50?E1^2)OJ=9Pmc5dTCpuC+69K%zoDr_y~ zZ7B1bty~WgXQ4$mIGT7g&rR3R%3{*6#F`!sCDnx8Ci+(;T@d$v*b0$gTG-bPL66(G zk|`E0j)RVrkGfcjA_^!UDMP-|CG&3f)%d#jd2*NkQF3%Q;-JDH@=&9>=$5!DK&E3D z`u)Cx^$;tLS}7Wshx=1ZHsH_NcGa!TY5W z`o@a5_{N51^N*+NWmW~tOWYcc@E_&}#k+&MXiLqJ_crI1MrXvZ|K*ht_FGzZ{2h1H zrR}>!el^5XX}BnOUhh%vA%1NL59$&DSChsg@6o|7mSmiDpc}6Lni#A_r5!R73Tnr_ z=mm1g2iki-bn@VCMG}JG#DG4EG6cfis2x5B$*RA68W1W5jM`z-Ng@tv={JSH45i1s z9y0po;XcelDU*{%Uklqw``zkY{4J4q?cP za*Q^Zq}{&t+RaWm`A|Q>hbHiyd|#vZ*{VczJ^yT_7~`5U-1x}~b=+g{-R9#Ku0GKp zl;iD7w~aRO@fcG-x?BKvCX?7YsH^y7pK&vZBt9(&GyNfQgJ%_?FjL1GrYsksU}Ae; znuS+X^Mb)0eJfg}v+)EPTTWd@AX|}9%f$JRaSGAZ`t$c&-re1kL+tl9TneQsIZZDD z^(|a2M9E1~{+qA+6pR=c^*!_+k}=4CU*0CEf!tMVCWQtnOSz_!-6T@|H&>I>rPo9P zdD+%M`M;bcXY|YlVIK%>h-L#Sc5nqPx8J1E8RU`7uC;cY0aQB5FyG-@M-IOPn9gB; z9=}EjVdcpnwZB_%=FN>szckTb@LX#p%S`qn;YTx%Dau(p-Y{CNw#dsJ*vAnnzsK7x zeF8ma7D>9020LIf@5@dTMKDhjVq-WMarz<|`ih<<@3Y@uKJGYQI^G4Tc-0kI*#1|=L{67FH>dGJhjNjm=$!s}T7JZ^FNZ(7 z+%ijo6NCah(iL4~taG}2%-Dpkbb9_xuYdANa(T5ynwn3gbYSkD>P2 zV}iTQ7qy+^U*uXapTdw{oUMiH{HzkM_)qFGr*20Rx_wk^vZPJ=Z+GPzWi8D$6leZh z#oI>L*qL3u?y_TK9U4y(Cq(?v`{2U0XqjkfA|-cuwY6^iF&N!L4c9jIE8JqnB=nta zZZp@z5393ww4LUIc%2U!N2d9bzhr!l!con@gb^`_gj^`0#qAR}9qr)ftv733UC7mB ziRT_+$YFRWW_V?aN0(Vn*k{b#j&|x?>9f=<5}hwl6&1D^I4skmMl{ki`>!S9-MVsi ziT=r|D>}9K5^i|_-LpIxruX{VF6}j;lp2(R1& zW?W6(k&6%ZVj2BB>780-mc3K2k)%V!ahc3w|E}x{Z_~{R2K;DlQCw8M=vh#Z`s(jX zJi`v~)8E!2ojq7mU?cqxS$*WF@B zsC7drLi}yzu;#hr>XC`Q1RaS{{6Rj1@8HY7oU;;bJA+Hg4M%bhpq6HQ^Op}gR=OPm zLZu`oUmi217M`bEoeAGJ(F*LOy6gla)`!z{Sa{mR8FId+Rrz+Ext?G6z`lRs@kpIZ z--hsPNx7+}g#$fqK&UZ*!00PWKGUHZ3-vm!;!L3{U&c{B(Qu-3pj^~NmV*W!>pf>y8_w~VcI)=LU(?H=c?eds6 z_s?e{(^MTiL1Q*!vbuC@P zMq_>LsjT-X3|egYhMbL6j|LLq$S@%)Qf$;Ym|D3Ca&z))xH=NSz?YTj)ksvkz=emwVPy#BD~V6}EUz}NBS zM@{2OjwDxQ$VGrcGdTReX`9@rFr)HI2ck^vmZE^{BXvC3C8*l*>78j@fbfZ9gym`_ZbI5P3be=eFr05$C8CsC*5iY&Dct+Kct14GLus&Xd zOoLl&%3Fu+!b?-I<(+xdb#)_=>Ez7dO|;=7?6Vt;^zlfa2fZ0F8z&R=kEd;R_!P!Y z)|iZ}6N z2u7f>j$R>oy>Rlnb?W@!)K>-QM45#?zi%%dBhGgPccgi8*5Hj!{+EAG%Abcb4dZMA_ZVh&uYvMXb)|X&> zzOQoKYE>+%F2|yhLQIHgkPUt%Ea7;^cRg+aRRvsK#&BIn?SN1iQa>QQDNsQKQX5_H%Jt=8ZB2I8$T0^yl;uic(uB^7(mr1hw$v7 zbZ0+e-C+?^_+YO#203E+J&hFJu@_UhWMpN0mv!|Avg+(Z*|r0%imNI3OTRNLWM-ju z!XljN^%`fnMrW9KBm~sHjO~>vN_yEonThR;2-AsY-hXZF81)wF`ZYYk!X(cJ@0Bbl z{nRoy`9p|^mlH-xB(t={VPRS|%^UPo18vOrmO*{#O}Xu(5yVf^C2}2^KX`4=#TKYG zuZPUzhToQ*HkWF_wN9UA=bKiUFTX!Le8pAS8~u<`msUfASSwxz5$9THDvO!0SDuwh zgw8#y^#T4e;c~0cM!K<3O`%YsyOfd%j*dQ_ws4z*s?}D7+4Zp%>w7=JD7NQ9NW$o@ z(OUJ3CTsGboA?5nosCv@4ZQPG=L;*x3%y8gkRusi6O#jLR?Zy6w1_U$ zrg_0lBvj%iwA|r^*2lxsa%pipEJpjfUFoRMj9_gp1TA~4ltnfRyLhmaMNVZv3HAf# zj+L8c8YR@Egw4K$#zY#E%x*Sz}$K{RUHvVB-N$5xU@htHQM+byW({~ zzgs?eGgq?1>^rh**KOXKKuny}kKZ0O9iL#JYPziDj_LslpTIjM-ngTy1AOHOZO5BC z>Ey~i!AYZ=dLCdJTc2OWc_325 zjH8@@;pDzkcA5)nEjng43?pBF5%mc(8qqPuR0n-_dSdAxR#c57lFqtcEnMYl8~6yW z3BRGmDV?@+pk64ydx}=bchkGeA1P1R3u442(fyNjpz0etLT?q6Nut4i7Ibs{7EN7u zr-)GX!4$@y^idTK;Cl)?a2R!(ycM}i>7B=gC>-0^9Mk3KkM4WV_yqolXfBiy(C`Me z@uCj;rc^6-Z&^^RFAKCT5ADB)TJTVpZ)D-R84-dS7dxjkHR`O_@MS^uKyqdMgz`_W zNH>-r4H-Qt3(*1-bfM?spidwFy)HoU9Z_!YPn~oMWpg`Q__ihLTB%w+4UIT0dkI$_ zDStCL3xB^2|JUkJNo|c6mEy?-i#)nWa)q#LqmyVJB1!Yx!6VQzWpJ(u5!w|Bd*P7# z>{`2c66}Wt5>TIetoRr5uk~^JQJSj~KsS)s9xAS+d8oEWKBs}oE-eCep?2W;;S&xd zYm(S>f0_-y_a7Mn&k?=_mkO@sGpdLifO z_}ziAkt{6ZIHW!fV*z@c=Zi_cT2xo1I}UBs!kh+;XoD1b(=-0}4VV$NZ+3J+qYbrP z0H3)?6;hzBT7Xz}>oc;?S2Vs4WJy|uZ=;wWCh+Y_C%_QzxZQQ~?el~)pg<1!?e!{l zVOEodw*z|Mi@^SdAoOA`)o4_iG(w=ADN*nL=EcGPzumAxl1vX4$h<^TxV9LSe|?95 z5)-P>E{(0E8(lfYys6e8Knp|;SISIz5m)L(h!P}w1buYr60qh>y(8bR<5Zx29W-J4kbL{_)RGR8M|FLZJi zo=4RYWUTBfbEK4X+>lWs#FtF3_U-4A!M_S{wN~l>e&KviTKy4fPfX&BzemL5^Fdjc z7^X+DEZ`W>UlTTyTN1Z0Pods$BV#Yhig|zYuTz^8Xy9e*z9NwqX5Kr}OgbHDWt8LL zj4OKYV!Z+8V6Jj@{45sgTc9@^*l_bzz5ga*RDbql8fp^OZf$tMgYY^{vw8a*74Hbe zzw5}?m{)HP>~T(hxfU3q4TNu-YRq^7F}5G>lk24_NDRFc4s`WzwLJdC+v$49pGg!R zhiezZF5z)mTt*N~l~Gd13e#E02C{6T6#nTrugt@?GqDWKCmP!hxv$+wM<*w&sd^Vd zgTX+jp=jD=4NDc5u}F)Bwpt#wSnT;6QXcj~gx*Vn73cs^PG^*KQG;)PyQSAB;BfXJ z?jjZaw|+%Q4!0H1W!kQNet>H6G+GlCB8GIw3e%=oX)UuM4m&3K&Us{=Bx6cuuW!4$ z@5*A*8_6v>i@s{Sn0dyFNJYNU{vqE=Ccsk+qr=^pMkAM434{oaVB%|2- za?rzGO5~3)*L2N86=UbE+ZiPMx1T!zGyeuDO$3cfTD>-^?Fjg3PutB%K&?$|sN6`N zsjA$Aw@Z&Fb2L@Ou;_S<*tT=dc-*jT!lgXrt9|@OcrhArU?kS@ICGz8G=$$o@7{o~ z*B6(Hq@VTu=2*wRX0rWqm6C4 z{GVbx58fZMS`{9LwyA!W(3&wnXwN0b8}U*3;2FjHPBrYKiDvrBH~B=oQ$FK7U38el z_xXB!^pK-n6ef?AHp3x=3{O+0(ux*0JrVZ4F?#%MuTT!7CrMh3^)^nO=J1qeDrr&| zJFfn8S=zDwf7SD|Er0Fuq&E2f-UL_J-)!OT2-3HLXPYCpDk!4K#`wZDwl6ZR{fmj% zp6`hWH{@!&*94C8jqkQ!a?kl)%AwwU8;5~=U*-}`+E3)SX;JLVN}kAo|eJ{ z`VtFaY0gk`(MXE{SJGB7&nv`_ivnX(wE!|(Xp$kk+hyC*2Cb0glF@QK;x1-}?VwKE#DgS5mm#Y1usmSt9vqZjsLMA!oiP567@nkv@FlOX)6+N>qU2duIX_WN7j{V?JGmIr!M#>r9x#oK zPL@hY(Ba38)d~nzkXxHP=Dqy>Oju+3^}aXDy2|esA5N7f0hQnKr*g}IBs@C~PmPtV zqk?tv%3~Vjb?k*-ak^bAB<<67hBurik7#@~d+~U)AnaJHNh1GkWhjL+5|%F$98$fO zY4plkaqs<;0_<~q+s>+tiI$QCWZZ|g;7=PI$+^~V6h=}$EtxJso0EZF6lgQOmuRg1 z+w$GPTN^MXq$Za>KmV+4XlOsqEJT67SVNtuY?!#cXX7yQKNp+reX&Ql)4g$Kw$^)6 z_LDNHuf?N&vs#7LN`@WB$y{Te2I%LLdf+qQ_szL~e)d&_xtB|ic%n|5ElPu1&4ITN zRvmO7l9_vR%!sfj`+|vI0MFNHcAVlDvT9LuJAF>jT{}z-FDfj}O>RI#2)$5$mb}72 z=fP=WONS%&cH}_*DrO4c1pi&_k8JZL77?bTfq}t{al`DOK;SN`+5fI6Z0HKh$r3X; z`#4p$@El9rGUDjvek$#82u1fk0Y<1}W5|a7nhsE}eWI{W?cQ3Vh!MS34&!&IoXD)9 zafUqQ_~jlAM-FyX;4Vz*Tj%0F5J40kl3i90!Metcp{lwE70w!kPVFDD>k#{i366-Q8j-4MR;!lqX=h{ViT0JppIV{A;F-G=&JhJY zUdy&0I?U2qZLAVU_B<~ zOjE=yNXr}9d06JGnw9W}e}?!6w75w&ysrG&vR$>xr@fbNqh8zElIP`Fhkauek$cij zp$rVr2-4gAOq6{0Ee938^I#+s66NAI4J1%5efG?Pg_}jkCx(iz#lpZyZLr?)(J?(0 z?uTbZCX!^n90x!s zirx^xSiWRWn|lar|5InwRlW{8lg{fU@jh}I3zn5 z?x!9n*Nup6jt1zZ)NNJUnv9b-#r|$IooU-1*Ead{)W2-RtnkGso2=o6&+pF2Ug6C1 zk82r5jJ{5CwqBwN$~3!>hs~}xuXt3-lG>))mY|g_S5g{xs7MIt#PHKQD3nF7r4Y;Q z*Vg}JwLhLQKYi4Z3;SoE!R-h-fUPJ0LL1QaQ*q;dGySTpWZvqX{)X*b=?=eaI#Y== z{AzhH54V!8Y^EV$__6qgCuF{(q0HQ>U7%2VaULv{Eq*rM+r2z&8M=r zJ*CwCd598C%^y=xb2ObF0SZWdeouak1pqI`73agxXNMud;f;xi4h~gj&mFsyMp^S7 zwdt2gyBi44R4RIn#a=+{s{8XYw{8Q-6Q}cGT=?`~=9Z1$0TyfCL{iwuT06{@tU`B3<2|x&|neeFDr8mzlR2s6~_eAkobyO@8;M-Xz6+M4r)2?QkZ|Uy{;*D}0iX1RaC3?7Jxf(5>h3Ov zrrgLCFVM&%N|oQcLE58k9bxQvm$zEgGNu^dMhGSw1Y#~b`QHRszp)C2)C`0V^xoqeb*7h@9KTXs|RxhF4X*}?@3yp`x-;}9)2tI_kCVcKU< zvl`oO2*UntvNe&-$&9Rf;kgtTCIv4=O6>RVl>HMI@2yF#X8(LgA4nx%3R<1|SgqHK zoO!t1`aTCk{$nVhLVSEGElEy1mD&a-$^bj@M7dP(n@lgGIs|f8$D{F%d#%K}vK-xj zs_w$0R&sdov?TD6s)|@czX>$qEI#QIlLLMjoQC>b=?v%;>F%SBk2IP0AHn*6bV#10 zK3PM^7}Or?2%GFjm(xq+geycL-fXI*YK}F=d`qzWl^+|c1viFBA3mEhU7}2p1OOqU z-%mxb1(2dd8*#8()=fjd{qT0uR{n*H4ZWpWQJ+1nrpt_|jh=BDIEXpO1BAc)7Xmcn zp=_rJl*;ojQs=kt)iXy4_td&=F;&Xeu;+W6`UlRNDJ zK}IZEoG`;jORLoe_xuR4(vB`RW{5*}F zvLLL(i`2RB&dZwClC-t_Af3rXanaC4JT`iiDmpvrW-*N0npB%bQ^1SdO$>%o`a^1d zPLE_>U3s;ay<8gNE4y@jeL9QfaJzs~6+4@caB%EpJbpBu5{ z@&{hGS8qAzHb}^IRomjBx32p{borG-=UCfgxkn^gl#l+|%IrFV-hOhh^Jdi1)d@A( za!^H>Hv#QF@JVF9+j=igX&?uVBGfx+6EjMkS!UK-+;-*8wYCjZmfZok{N1(dM>KGF zt9?c@e|Ug3FBLb0r1~plXiiRLl4Hs9r~;Ri`AC^{#yXb-Orh5;iK0uJeRLf!=V@6o za9iuqam8>R{&TW+o0@l%2BoKqa<_CwIArU4}4xss@XppTylHL@|-2FYen02isvx$ z4K5?qy2t;F%Ow9ZF3WKNbk-hHf%p+r{uAwHG3kbikrd%$Vv^9Ar@@wtcHT=}k2Nz@ zz6lj1eL-|k(b8VCv^Kq=iSW04hM@Qmj3vkQlN){|J&16fT8>;_rDdDH4GD}z)(U_8 zg|T5uH(5KO;niq);cU|&jAdv-&qW`0l~dJMW&=pIg1D%k{0^Vdq2>n5i@BZWdP$Mx zDZ6Up_rbjqewHGHHpE2om^5SBp69@i($SzXq&$CSw@bu+L5hc+H)eSBg|?tftLip; zi|=j>n6sh`-r&FupWb|k>(Qe%hb6X3ysG7{Ec!qxvYEn`4lW<}CL7QN|D^u`UQ(NX z+Yp)r$8%ZRCJVQ_)|`{+^st|Z6&kIle!c^{F#edzx7tm`VH67vG5j_6JH&`Y#m?Rg zMMRo@-%2X|U@5}4=fH4{wvMx`Pj13Jj&yTF7-_6$+qWR*6B#a1Cu@W~^}}6`A`$c4 ziB0xNwyobadfW?)Sv)fW~4Yxs#!h8T&_ev|Xk>(uuy&w*X$ zNz;88&NPe{N~UHn;6U`ktrWJbeXL^O{w4wC@kdzES&lsaHrilwUa1@NvBL8(oP#!M zwj0Ga6N;j7hjw1IN7UOmtlm8wNR|BEr;M<;^Y7Z?Z0N@O*>Y4cX+PN3EHPyBT@?$S5IApoVU5P<{9loZbufvoxhvz8e^i^c95W$^9^nS2gvalYPkm` zgMB{J+{cWL(`%zQQ`J;vACv4C#IGN!{N|6qc@Z&O!hskDv=rcWTbZoouMY4mx&RwS zq1;Dx2W2G1!lgO%zQPaW3hVC}fW?2_wv4yjT_i#`z-`3s*heloagOFY0eF8KeZ+?N z64~{)n$|vimh*GY|Fbv>LD&DnzK-w>R$dhMuYk^HEOmk@it#4bYt1Tf^afaipT(p- z44$_=OwP^31{)7Z80@d;JC_!mLojXHUaf`JxGfz-PIr!noT`Y@P3gt7hO{lI1AzNW z1W~n)NKmKP`&9b-jilvS+!t(h>-pw2i>b^TCkj-sS$>5JvO6f;=3lAbU$hSuY@gOI z4Vg3dTFpXJcE6aE%-a%m!}Mk>a2RiDY4l>Gx+&Q%BubCsJ@6qmTKnGITPnW={RH@i zoCvBdn}X4kMz{I|`{mu%o@bBCX0VMMX4`K~?{0X|I&`LSX!|Ow*%G-ivrGE2H6EtY z`yD(6V9Ov#v6r#X?e1@A)Y_#v2_C+}h1|L#&He9VV6@Ob`McwJ6{wNOR)~dJB;2mm zs>vjI#VVYCE-p_+!(-x>Rfw;zNd}GniL+mAGs2=`4VT1oou5f<4aOa~OU;SMj1(}L zaEDZC+MGg$>_y6(Utvd<#kVER^NQK$U0e>K)Yh>>d0a!7ni)I;L zCIo?`kuZYu6zXEZZlBwn=Iu*j{L{BOXHg>QeWq@L17prScQXe5S7`p!&u?@o8Eiie z9WsvXJx&^@f&wW{V^^NDXK%E~Xb7^41P3xaW!FIVMX>#GOmQ6FX<1g^hU{aVyK21G zZ@I9J=%@hnZLU5~@3)?s1QALZ#k|DMNE_y}Y4+(%PnNr{C=(cbzjpkHaMFH%FHfZ} zA4T@RY>EM*@+AIWU#Aa}UA@sWM!c9Qe4*4W5|`|?+G^W`c<4!$Q!;~L)9<&%fAX`> zE2!Yd|2vP>NA{f*2_FAF-RiM4bE6VwANrUfW)a=4Hu^tJ1lgaJY0CfL6 zUMtPv7AaPJ9acMyFabTy$wXluC+@hn3857E{9|j`V(!A%{5!+qlE8Hq+cwOn6?kdq z^Ny{I^Oq#3y^m*=V)G8xyi{i6cPuVwn#R#SvEzH}T>S0FJInayhGd?CwNNg?gvYR^ zdwRPEe=i;Z#2iv{$99Zp`uYl>y+mvJq`!X3YQ5wN+s1>p*$Fh5$ ziVEp|Q4THWD%ECX=g@u5>kZHb`Jp;mMXxdM1RGfNK9J`SWagXTg$EV0WDA6R&=@FT zWu}uzGlvthJY!I8fmP&^5y(vN8;!GswNzP!(n=zbCUdeHa!WK?^j4AmG<4a%(+j4n zKDb%r*okbnBJ$oMcjQp1T{11#AO6fTpe(zYcmmLyXdYKA=BK=2vvFy8bGuh1NtBPH zT2_f4#ef8=T1>o%-2FZ>MU}x^;8IO{`pwGc)6b!yg5-uS+Ul>OPZ5)ub2M-*Fpv$QN#P<&FNh-(98QM`K+n?x)#|# z?O%(@s4Ehg&H%`ZUKri6h8+f#Xr8C&!K4l!icW9-EO;cA7JyjqtPsSiTChd^;^+NJNdrjhjZ06@ zd-N1Cl_CVcb#*bQOPa+zylX7oVOm@Qh$dlJmDR2`XQP<(zrPS4{(Maze9?8!Z%#UN zF<9S9Tz%V#!{0=-W@ELCR!xmFRotKadOuu3end&v+KCaE+!4!naL`c9xqVsNK0QBdF=I28Vl1vx4A9<-b;Vg!keVDY zW(OgRrc6?&&-Mv_V`sObagR<8h@|<4;;cvOC2newqT8EeA0%{~_LzQr-9ET;fdV#l zmp*_29O_H&4u5Ed+;He3Rp!Tst|NwKDUhGoHSe6iDtiDtVV(X=b^We)EoRi$ystl> z6c?5fx000F+qjVUK`9ubiHV6d9gMP(wMDoXIM037C??=KU zywD3+BSyrPK3PPW44fkJQi%YJ)q&IH3B_FoK>Gys9w2V{jaWn-+kr2hg@r-o!eF9@ zP~N=DPqAem>M%#CG&lAd&@#_vN7C)(J>TP!R=Gx{ZyS9e*nNGlkIeoE42^F8ZA-9Q z3qefSZ1S5m5=~PVX0M(}7TT3~cSYvC{ukwEIvW0mE#zc&goGptmEl zA^$VB^Z1J@W_EKMVq7LhtbQnC~*!L(gh|wx>1=80EZ6>Z7yHI<#;8+i0&_ zh^fY@s#LGcxwe@237y#fX@7tJW3d-i>)MpN$D^-uV;K9Hy?dMd%SGQ<%oy6zop&d) z)+dqHU7TaEexi>jjMe%VA_K&=sP2jRNhCj)_C?E`6I8i}-~&i`GJDZN6ko`zxn-+7 z=cRP5F#YQ1)kgq?Qh<_X}LMR-<*O0=>RQ1^D(oR?LV~Z! zZLUG%_s2=`>WX&gpMy)B^Nh%MkN+Oad+O*Ca5ek-<=_f{U8iJaaS1<<^aUA6FPoXV zUy!>Vp0h8^Bq|+}sdZv-2PGHyd+$DnXDulJy}8-Nn+!s1wKi76t`#lbm&x6?o>6#T z`)k&*5NFS&L0(lMXK9Hr4bW9Zky-}2s^lp%#E*&8Ib|tj)dvG zqO{Gal8p%5zAnU^4%`1tXXLMGpv!nq0pdr?z?i40-!=MB--t3>frmNEqO6d-^4#dm zpVFDd%#!UqxAu!!Jv6GLr1m1VAgO*M;qsrT8Q;hZggO$F1uZ@yBST%rl1asDFIjka zL&<;7LJ`DaiO~=v@HXPq-~jAIuXv)8+Bj<$^OhSu=*77$Q9t^fnBUt~utk(nJ|-FL z^SEaJdD?x?7LqdFwI8|74bq?>rLeb4y#I8=5*~WtUqgwc6{@DOM@dn)V5t~9c>6AJ z;H%~66#30u$%qbnB5Y74`Si4SfAo{Hc?R-Y^7dSAJuJbim$gH>PKeI==SE7kIvVL~ zv#NKsQ+Cq##ehEnKH^>RMDRM8434--y?X%w0pAroY1#BZHdi3PjWWqiFa%kCxrhZE z`lKtnbwqb>H_W^J*E7UrP#g9ad0GJB=B865rt_dwUG{h5^pwR6tc4@mTTLexA_-oc80^+cvJ zX}E9y{0n5{bT|r8x)t*U(LQ%tr}35Tk7VkbxSGhW>4ho-d++zu836No@w&LbzXZQ) zYv)G%&IJk$hl$<{*2B6^k;MVnyNom01-G|#m%Ok1^IGrn;it@q1V#V=X`hKI=Oi?7 z_zQ)hulBZmXQ#+qh~riKN6JS}q~3>jkZt4{%KNfYGN}wt)Rm3e9Ma6$7A+zJ1o$75jk}EVR<-E`%j%HF1 zd;M;66Fyk@q{0V7Uw8T?C_VJ%($$NzS)9^2F720gWtFd1_b`MIFsb3m7;GK%(RJG* zwAVhtv2fN5=7}9|pF0pVFf5-MAc3TOamKkJgw*r4Y0WY?duL}4!pzVCa=s3iefib2 zyJ4wjv4SH7Pf4$EI6u!g53V6vEc6|{MM-3saP01U|H_K#m3)5Lko#@e%ZkMcSX#gNuVK{y4yXZC&5uq zN*Rhg!|@Phr^~-lyazao1D(Qvex3I6hnPc4FV5a@v*{{&r2%Z)jcfn~B;Q%KcPqUE z1^KDpB^&rH3jfQWTVAJm`Mbh1FUFB5qHKd3A+DVjM0|R+YHn>*o<@en)^`$e+|@|0VoNs!)sVE-qCqiN``hKtLAYTBqch<$OAj(JF*HD+#-l{DxPZf=@( zI-J$znbxCAO}h~EoBH&03Xr&Ov=?|1P#6V7CAAZXI}Cq1b*+D=4+Z9ouKk-}UEBA3 zpc5CY<=X07!H#~I35xeyQRxoy_Xj}IO&3yFE8 z_j!FtR8WEm$Uo|5*Yr3vTwh9KXYhS016WiXafcs0aY+Kwhl@y=_XP z``VfX;($`&pVg!}B!m!N5rroTa(uquLBq>>49O_E+>dnjdC>USnSTCZ1qg^lKsR0= ztT|rg@FeuKuQR?2jM$R*{KUJ^Md8Y6^k?w-Pdyp^0|iIIyFK0}kg|mOo2XE=Y&udW zg{W!yTAxn-!`^KB{d-_lz2+qZ>(b;qPdVk!Dz=Z5ejRC^h*7-n#C2u+Et5*$`fFD8 z{a1|pupgVC)I9U6;^Zsxom&N{60u-?C_5!8sYMS~xADvs#H(6<0l!9`-YQZ^_`fY} zqIUZT#C#wbMst5t!W?`>N2kd68b>Y8p3PNvjTxj<8t1co-PwB9qo|fVDz>bF$a*#x z7zV6kDT7-UlRCt^MYR&{c#L^qaBYDl;;q0Az9}Y?|087XgR-~sw{eg{)BQ;AbQ#|i z6{LkvR}0p^`h}aAq5VjI<-1-h&$9tInLgsi_+T&lhF;eeY1|tWS8f7Ch6I)LTS}Fol)4O>nr@*=VExHTI%x2WQUtKukV0pvO6N@_JP1M zA6eoKly>uD=wG^L?gh~G3sfe~V(X48)mnc*y8{gnDR*E3)s1J4GO5TjkyJ(vSPN8- z9lPh9zu1KU0PX7@D%d}kt?>D<%duQ5KRYXCvs+b*T!h_fT(o6*fFsr8j`Byv~rlO2MGrOl-Fu|uk95YRlC4eacEF_&q=9o2d*Ys+VAv@ZO<`Nxj)RBlipLHTA>&gbqs0BPjX8UUAny zQp|9Vb-Mk1Qn$`nd)wMv*+#6_7W^J$zoz{u=|%-kf&GSB?{_#NV^If3KSzcMFGH zB{rrf!C~*6_d!FIvEr}77(ZQOP%34Sv=(aBc2iV7-MP`9m0r%^kGaaJPFVLlkaG6H z`w6KsxDZE=TYd5+Yw3BWzekCa;mD^GQ^MDDtr;#WTHsRiXMbpp5wVEObMb(c6ZvTi z3v#Q-WH2Y2Y}3GsK%8CO(`jOf2(x0DyXiS}PJG4kncoM#Q@1^RS|r)&uLq8&d#o-O zJ%(bs_g4`=e)r)JP=LX$Az3L+p6eX^i=n$I_rkeq?Y}eznKLiNkn{WZ}Tmbc(A8 zhbr8PdrmpcEfi1$e&=HNCbK#!$e@j6HdwvopC>G-Z6YnR$ETm$lqbIOSm+Yg7=$m} zGhm$JJ{TJ{l(=d{is1%lQ7iBIZ1tL>qW{cR08`%+?}kCbYf1{yJwdKoV?dEiwA0D! zd$44B$n@LTNv(60V()6j*(NT*|5S>Icx7?HObCl(O$kU5r`5JJ?=<_20J znle=VFQq~spQe(fF)A)hn^vUX9~nQsQa9l;^ASC77>E@3ipr*G{TrY+@AC5{-V8AMeb|h(TSM`1Z%T z$wk%anqA_^saH3wd1F>@>Q6mJ+&qq^sn^xbdNxzohuk+{SnU4`W(e3X!Tf6G7vmo(D^?Dv$W+nVP zUD@_@WFU);6qVKq@=p-m@VvHjwz>H~uZqm3T(U-U)en{~k-IdG&i1zP^q$UComRpJ z{raZBW#tvV_f#g3BH z!u)ziGE7V@sZ0`SVLDW|F@SWCq9tH@F+!k)(&+(=ClQ_Zs~?DuVG_eLZ3l;Gt*729 zubu!r*MT+|+poC20n_vQqn*JSA-TPGFXV9UUSKFj3E%(`3D;5<_Pkfa-*@yw<$ujp z@ESq9)omO#e#Jt}k$~*mN^mRpfpUnBg0#iz+uuoBbHdnQ#}0dE!7m(!?JC})-~2Us zsuI43$WFh)0^e8Jg#{ac{Q;4ePg}`;eE$+KO`uQn4Ht(25HV5SXoV>3Uwg=i`Yx&W zRm-@Yzyk19A4D2wG`{qv4f;gR@&MMl=w=Uu18|G`*KvAA&%FgLdntoS!AHla?{6B> z75&u-7RYit-6vMsAB@ERwYQnG7wdl~%vA0k>i^se)Y#u-2UY-b4e(J1hX;9ybu(}a zv}-L5>t{3(nMm++zyeH%xA){Lh`kIArSSs8XVke;c918~e)|IP_TJ^(^{`xaffjfs z&R%kTB6#osHq57q4f(LVv#fLxOoFHkxZ#e99cbrW|ogQ^YJ7<}*N} zoBBf|A4UFa4-wV&6xa+8=%m4iZ2+|}K_E7y)tC3*2Fbf4@Dn$$D%lw7LdVXu6cSXu ze}3~n>wRFS5)5*{$hfa+C3)X(57vOf4Wb+GA_^N)3m(76G-=_dZ46@XP1}Kb*U;6? zgS3QyFj4@=QJN(`h9HaXLNy$>{ z@gV+n?EYnU_TS@10Mi&4=1im(*~mO+n~KSl7;ap&*+7e{ct{`$SO&~9$&xc}ScNW* zC#Id00l#Eqg@_Q$bi6Kjd5O_B8lXnS6}9%gHp9YV?}YY*ZmE!iVZod zZRmhl?OYM(?4_Z{@%*%D&t5E#h2PVJ%u~2(J$E9TM`U{VU+`f;F28iTm@5V&1p|Cl zP5k3Pc1XNMv5pjXuv;*t4n|z`26Gsj3dtuKVEo6W<GgW z^9u4k`;i&y`*t+*45!}_B|taL`ToDW9oCbfj(oq)B)dS&>8RYuk?i75VLMM=2!-%P zF~YWFL$cp6yQT4`C*tU?ey~SC7l<=K>wErn6cf~ukB;z(NsWObYJTqgBt#ox|ELqz z@-ln-*N^=yAk>7CNvDh-$|OfWh`6!wo>E*BQPHl+O3!v9%pJl04ph@y^)%5C`i^J( zwB#VI`+KV!D#V>ytK#_VdZZiwfr*5_GCs8=2qO19W3=}cN5CeO?)SW$lpx~_dy`W| zp!&^Y3Y6}BxLd}FLiokSt1K`^ddH`{VEM3{s|C0P_AMP2oC!#tr>K6g!swzl0~B0+ z84;YvFla7ksTkAwpkh}k)r|d}iXZ!`Y9QfO(9x93dxhTbszhQuJtY&@) z@(wV*HJ5|nsxy}6?|2?pZH*WT?|r_Di{@5?TzSM>Y6TQs*6+qZLp}l`XbKYKD=qtT zn{D1JFjw+GoFB>7ZSdXwpbtu7>!!K5AZ%yyt>-oF^Qs%bpo^y7T66*}7-GVnURs%R z@drv#1YecGM+hBZ7cDM+&wuczaw%eFb0l<}i3_-WNN`Y5Sr zONYfu3*jb!;+7$Qh{hk3g2a1a44Ub+{ujBuzJd*axd-Iy&#j)l#gEx0yW^e**dyXX zddiCl4=}8kxwLD_(?NuN9t`(8b&IITDf@oU`>JrHde>G17!@y1#Zu5V1No0d$TNH$ zkM<)1oz0&sFkE_?(kZO>>Hrz?T3hD$G%c^ADUtrE@RyeKSR?bt2fY+{|AvH0 z;`UAxh`NN2m@%ILFGwD!?!m9G8gyBc6X7?vpKpLCg4)hA{jRX{NXd)2At7X$R5 z!yq(%dLOK!Z)GEp)#}wKIBIIUIaOn0bGYnsg#jB*ys)>jZc>v^v5Q{Ir~xtm$$ch5 zkR2&#=o6cq@wEAS2`(1ISdEfQ24ybyqqaLbr@0NBYN`8w?Oo|tQ%M#NjS`_*gaAgi z$d*J=49zl;21AcB2m%6<0764p(yTV>up|KzmavPqfC3U_3y1+kP?SXiVLRe91Q3wL zAc!D~3Sv5heJXyKzhOS~r+3b)`|4KJxwYI^_x^r)Aj;o5{DPHJrNB~R?7XHa^4pKl zh8|V~@OxP$J^PrvW^5s+1V18fzov$o&k54JsvQnyI$pothz=Q%+E->Hrvbn^^lJ1A z?Vm$~3H>>QL4TN9rpKvib3bw*lbRa+-f7i5s4?(!*IdjHkMtO%2&RLu;;zuHkecb^ z>;uTFq@w|iE;#gZe*W)fEvJgHpfZ^J5jfQ+WW4c5 zj$m^E`|9zX>t5*(S{?+>r5C?y*?r}05MKsxE*}4RnBuzE-Vlk*3V-I*TXkExK1d5e#{1_ZgR9_wRevH)ZP>4|@Kv=W!7er#Z4P#2OOzhw@_#jd(v>iVTvsboYxp1l`7P*YT_g604W*a3Xb`F60 z=GbEDPc{q+3#18GcDC0|BK!U7RHchT>)$}8L)bFN2_1^$%9pGEk|gyV zYwwA}=%s;waZkkTiEQtQ_=9W0!2v+)i{Bjo>x~#uMvR?Vr(;x$RflUJ<7n~R;%1_f z#$nsuzY1KVC(v@LAS^v`tQ#BcteA}Iwktwd)qvfgc~do29;{b*KEOR5TGJbJ#bRK1h@y&R*ypp)!=a2VC9(otTpuus7(>B zjFfSG2bi6QcrNpzxfbQ3Tsx3)DO@KL zy+q(W7BhO#+WM%3Z1tbep;4#jYSASfrle9gfJUQT{`|L#UX+adt1u14qoX#rELGdD z_-jt3--!9TdIO{j$#|zS|Hse1V_%EgkWjhpmZ_#RRqnnEhb;Yrm1wJ=Vcwj>mnZH6 ze6I<2X6}LXyFx9w{PJlP^*TU4EAq&y8=U_Z3oydd=-}XR@FJbGE(y0wiNZ+4ds8;R z;et#JdXsV;Y@rM95%uII(ell<3*s2vVUu9<4bi4Am>vY+BBdBzCRX;X_JeaiSWvpI z)}9M;57-K+QInC8&zq9}4wnpg!r!hfc2goJ#J%jIPb%6W-}3(x{@?HEDh$CFlBlik5;{Y@m<^ zg;j~VTrWL~EqB2BzRg(^j2qu=BM$x>aS)WZhqQAU$u>PMHT_oW2#d$ks5Vje$pstS zu92pB1Y=DmBj&BRqGE&8eXy#R3*?EG@HPkROo^sEI(mhVDr<74ejo1n0dcaEUy$%E zMXT36z3-Z}26ok9;--w%Pr-|CwI>KN9}9u$>dIkCw!q4DGd?2+{; z+Iu~fJVM%J?-#K)-5iighqntsUY_J6o!(wOQyAR+=?gz5fd3E+GGC~4AOrURpaJe- zUfJ#upLchlg4ad6!$@ZA-WIO7VcEURn11#t&AxQzVf{Js7l%r-g9+zF>E~vgk-}F6_5mrr%$5aNo;ZzXh%j*Evh+T zpI-3|iwG+faDWKLLhcgAcd_8q(}2Zm&hW{&ppT!QzpEIjtyW2{tKa$ow0YIIlFJa^ zG&FJ29q*Yt;I_tj=84jWUhayGvD4;;^W}Ta)bEY*ma@!uS5dt^SmpU_Xq~fuzpq;J zi)|BaN6MkdC{rotb@}&!pZUd<%h1;=cV1@nM$FFF+#q(4uRe>OqU1$=8naIBCV}v5 zpm^W`8|AcEN*J7ll3um{~`G6`7@v?|ae Oh^@8b*+>5*CjJ+4{m$tC literal 0 HcmV?d00001 diff --git a/notes/fieldLocations.md b/notes/fieldLocations.md index 3ea8ec40..d9730e2f 100644 --- a/notes/fieldLocations.md +++ b/notes/fieldLocations.md @@ -10,6 +10,11 @@ These names should be used for naming auto paths and other tasks which require r Each scoring location on the reef should be identified by the letter shown in the diagram above. If there is ambiguity about what the letter refers to, it may be notated as ReefN where N is the letter for that location on the reef. +## Algae Reef Locations +![Map of the field with algae locations labeled](algaeLocations.PNG) + +Each algae removal site is identified by the letters of the two reef branches adjacent to it in alphabetical order. + ## Coral Station Locations ![A map of the field with locations marked around the coral stations](coralStationLabels.png) diff --git a/src/main/deploy/choreo/CMtoG.traj b/src/main/deploy/choreo/CMtoG.traj new file mode 100644 index 00000000..693771cb --- /dev/null +++ b/src/main/deploy/choreo/CMtoG.traj @@ -0,0 +1,76 @@ +{ + "name":"CMtoG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.228332996368408, "y":3.8562123775482178, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.813863277435303, "y":3.8562123775482178, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":1, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"CM.x", "val":7.228332996368408}, "y":{"exp":"G.y", "val":3.8562123775482178}, "heading":{"exp":"CM.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.813863277435303}, "y":{"exp":"G.y", "val":3.8562123775482178}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":1, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.67166], + "samples":[ + {"t":0.0, "x":7.22833, "y":3.85621, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.99922, "ay":0.0, "alpha":-0.00138, "fx":[-16.32226,-16.36668,-16.32629,-16.32629], "fy":[-0.00133,0.00133,0.00133,-0.00133]}, + {"t":0.05066, "x":7.22705, "y":3.85621, "heading":3.14159, "vx":-0.05062, "vy":0.0, "omega":-0.00007, "ax":-4.61608, "ay":0.0, "alpha":0.0, "fx":[-75.46408,-75.46408,-75.46408,-75.46408], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.10131, "x":7.21856, "y":3.85621, "heading":3.14159, "vx":-0.28445, "vy":0.0, "omega":-0.00007, "ax":-4.61478, "ay":0.0, "alpha":0.0, "fx":[-75.4428,-75.4428,-75.4428,-75.4428], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.15197, "x":7.19823, "y":3.85621, "heading":3.14159, "vx":-0.51822, "vy":0.0, "omega":-0.00007, "ax":-4.61217, "ay":0.0, "alpha":0.0, "fx":[-75.40013,-75.40013,-75.40013,-75.40013], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.20263, "x":7.16607, "y":3.85621, "heading":3.14158, "vx":-0.75185, "vy":0.0, "omega":-0.00007, "ax":-4.60428, "ay":0.0, "alpha":0.0, "fx":[-75.27126,-75.27126,-75.27126,-75.27126], "fy":[-0.00001,0.00001,0.00001,-0.00001]}, + {"t":0.25328, "x":7.12207, "y":3.85621, "heading":3.14158, "vx":-0.98509, "vy":0.0, "omega":-0.00007, "ax":-0.29047, "ay":0.0, "alpha":0.00031, "fx":[-4.74782,-4.74782,-4.74949,-4.74949], "fy":[-0.00087,0.00087,0.00087,-0.00087]}, + {"t":0.30394, "x":7.0718, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00005, "ax":0.0, "ay":0.0, "alpha":0.00029, "fx":[0.00079,0.00079,-0.00084,-0.00084], "fy":[-0.0008,0.0008,0.0008,-0.0008]}, + {"t":0.35459, "x":7.02115, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.00026, "fx":[0.00071,0.00071,-0.00071,-0.00071], "fy":[-0.00072,0.00072,0.00072,-0.00072]}, + {"t":0.40525, "x":6.97051, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00063,0.00063,-0.00063,-0.00063], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, + {"t":0.45591, "x":6.91986, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.0002, "fx":[0.00056,0.00056,-0.00056,-0.00056], "fy":[-0.00057,0.00057,0.00057,-0.00057]}, + {"t":0.50656, "x":6.86921, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[0.00049,0.00049,-0.00049,-0.00049], "fy":[-0.0005,0.0005,0.0005,-0.0005]}, + {"t":0.55722, "x":6.81857, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[0.00043,0.00043,-0.00043,-0.00043], "fy":[-0.00044,0.00044,0.00044,-0.00044]}, + {"t":0.60788, "x":6.76792, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[0.00037,0.00037,-0.00037,-0.00037], "fy":[-0.00037,0.00037,0.00037,-0.00037]}, + {"t":0.65853, "x":6.71727, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00031,0.00031,-0.00031,-0.00031], "fy":[-0.00032,0.00032,0.00032,-0.00032]}, + {"t":0.70919, "x":6.66663, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[0.00026,0.00026,-0.00026,-0.00026], "fy":[-0.00026,0.00026,0.00026,-0.00026]}, + {"t":0.75984, "x":6.61598, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.0002,0.0002,-0.0002,-0.0002], "fy":[-0.00021,0.00021,0.00021,-0.00021]}, + {"t":0.8105, "x":6.56533, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00015,0.00015,-0.00015,-0.00015], "fy":[-0.00016,0.00016,0.00016,-0.00016]}, + {"t":0.86116, "x":6.51469, "y":3.85621, "heading":3.14157, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.0001,0.0001,-0.0001,-0.0001], "fy":[-0.00011,0.00011,0.00011,-0.00011]}, + {"t":0.91181, "x":6.46404, "y":3.85621, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[0.00006,0.00006,-0.00006,-0.00006], "fy":[-0.00006,0.00006,0.00006,-0.00006]}, + {"t":0.96247, "x":6.4134, "y":3.85621, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,-0.00001,-0.00001], "fy":[-0.00001,0.00001,0.00001,-0.00001]}, + {"t":1.01313, "x":6.36275, "y":3.85621, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00004,-0.00004,0.00004,0.00004], "fy":[0.00004,-0.00004,-0.00004,0.00004]}, + {"t":1.06378, "x":6.3121, "y":3.85621, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00009,-0.00009,0.00009,0.00009], "fy":[0.00009,-0.00009,-0.00009,0.00009]}, + {"t":1.11444, "x":6.26146, "y":3.85621, "heading":3.14158, "vx":-0.9998, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00014,-0.00014,0.00014,0.00014], "fy":[0.00014,-0.00014,-0.00014,0.00014]}, + {"t":1.1651, "x":6.21081, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00019,-0.00019,0.00019,0.00019], "fy":[0.00019,-0.00019,-0.00019,0.00019]}, + {"t":1.21575, "x":6.16016, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.00024,-0.00024,0.00024,0.00024], "fy":[0.00024,-0.00024,-0.00024,0.00024]}, + {"t":1.26641, "x":6.10952, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[-0.00029,-0.00029,0.00029,0.00029], "fy":[0.0003,-0.0003,-0.0003,0.0003]}, + {"t":1.31706, "x":6.05887, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00035,-0.00035,0.00035,0.00035], "fy":[0.00036,-0.00036,-0.00036,0.00036]}, + {"t":1.36772, "x":6.00822, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00001, "ax":0.00001, "ay":0.0, "alpha":-0.00015, "fx":[-0.00029,-0.00029,0.00055,0.00055], "fy":[0.00041,-0.00041,-0.00041,0.00041]}, + {"t":1.41838, "x":5.95758, "y":3.85621, "heading":3.14159, "vx":-0.9998, "vy":0.0, "omega":0.00001, "ax":1.28942, "ay":0.0, "alpha":-0.00014, "fx":[21.07927,21.07927,21.07997,21.07997], "fy":[0.00042,-0.00042,-0.00042,0.00042]}, + {"t":1.46903, "x":5.90859, "y":3.85621, "heading":3.14159, "vx":-0.93449, "vy":0.0, "omega":0.0, "ax":4.60447, "ay":0.0, "alpha":0.0, "fx":[75.27429,75.27429,75.27429,75.27429], "fy":[0.00001,-0.00001,-0.00001,0.00001]}, + {"t":1.51969, "x":5.86716, "y":3.85621, "heading":3.14159, "vx":-0.70124, "vy":0.0, "omega":0.0, "ax":4.61221, "ay":0.0, "alpha":0.0, "fx":[75.40089,75.40089,75.40089,75.40089], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.57035, "x":5.83755, "y":3.85621, "heading":3.14159, "vx":-0.4676, "vy":0.0, "omega":0.0, "ax":4.6148, "ay":0.0, "alpha":0.0, "fx":[75.44314,75.44314,75.44314,75.44314], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.621, "x":5.81979, "y":3.85621, "heading":3.14159, "vx":-0.23383, "vy":0.0, "omega":0.0, "ax":4.61609, "ay":0.0, "alpha":0.0, "fx":[75.46427,75.46427,75.46427,75.46427], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.67166, "x":5.81386, "y":3.85621, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/GHtoNI.traj b/src/main/deploy/choreo/GHtoNI.traj new file mode 100644 index 00000000..90b88d1f --- /dev/null +++ b/src/main/deploy/choreo/GHtoNI.traj @@ -0,0 +1,65 @@ +{ + "name":"GHtoNI", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.46565], + "samples":[ + {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.23714, "ay":1.83157, "alpha":0.0, "fx":[69.26922,69.26922,69.26922,69.26922], "fy":[29.94265,29.94265,29.94265,29.94265]}, + {"t":0.04885, "x":5.81138, "y":4.01935, "heading":3.14159, "vx":0.20701, "vy":0.08948, "omega":0.0, "ax":4.23688, "ay":1.83145, "alpha":0.0, "fx":[69.26488,69.26488,69.26488,69.26488], "fy":[29.94077,29.94077,29.94077,29.94077]}, + {"t":0.09771, "x":5.82655, "y":4.02591, "heading":3.14159, "vx":0.414, "vy":0.17896, "omega":0.0, "ax":4.23657, "ay":1.83132, "alpha":0.0, "fx":[69.25984,69.25984,69.25984,69.25984], "fy":[29.93859,29.93859,29.93859,29.93859]}, + {"t":0.14656, "x":5.85183, "y":4.03684, "heading":3.14159, "vx":0.62097, "vy":0.26843, "omega":0.0, "ax":4.23621, "ay":1.83116, "alpha":0.0, "fx":[69.25393,69.25393,69.25393,69.25393], "fy":[29.93604,29.93604,29.93604,29.93604]}, + {"t":0.19542, "x":5.88722, "y":4.05214, "heading":3.14159, "vx":0.82793, "vy":0.35789, "omega":0.0, "ax":4.23578, "ay":1.83098, "alpha":0.0, "fx":[69.24689,69.24689,69.24689,69.24689], "fy":[29.93299,29.93299,29.93299,29.93299]}, + {"t":0.24427, "x":5.93272, "y":4.07181, "heading":3.14159, "vx":1.03487, "vy":0.44734, "omega":0.0, "ax":4.23526, "ay":1.83075, "alpha":0.0, "fx":[69.23837,69.23837,69.23837,69.23837], "fy":[29.92931,29.92931,29.92931,29.92931]}, + {"t":0.29313, "x":5.98834, "y":4.09585, "heading":3.14159, "vx":1.24179, "vy":0.53678, "omega":0.0, "ax":4.23461, "ay":1.83047, "alpha":0.0, "fx":[69.22785,69.22785,69.22785,69.22785], "fy":[29.92476,29.92476,29.92476,29.92476]}, + {"t":0.34198, "x":6.05406, "y":4.12426, "heading":3.14159, "vx":1.44867, "vy":0.62621, "omega":0.0, "ax":4.2338, "ay":1.83012, "alpha":0.0, "fx":[69.21452,69.21452,69.21452,69.21452], "fy":[29.919,29.919,29.919,29.919]}, + {"t":0.39084, "x":6.12989, "y":4.15703, "heading":3.14159, "vx":1.65551, "vy":0.71562, "omega":0.0, "ax":4.23273, "ay":1.82966, "alpha":0.0, "fx":[69.19709,69.19709,69.19709,69.19709], "fy":[29.91147,29.91147,29.91147,29.91147]}, + {"t":0.43969, "x":6.21582, "y":4.19418, "heading":3.14159, "vx":1.8623, "vy":0.80501, "omega":0.0, "ax":4.23128, "ay":1.82903, "alpha":0.0, "fx":[69.17333,69.17333,69.17333,69.17333], "fy":[29.9012,29.9012,29.9012,29.9012]}, + {"t":0.48855, "x":6.31185, "y":4.23569, "heading":3.14159, "vx":2.06902, "vy":0.89436, "omega":0.0, "ax":4.22918, "ay":1.82813, "alpha":0.0, "fx":[69.13902,69.13902,69.13902,69.13902], "fy":[29.88637,29.88637,29.88637,29.88637]}, + {"t":0.5374, "x":6.41798, "y":4.28157, "heading":3.14159, "vx":2.27563, "vy":0.98368, "omega":0.0, "ax":4.22588, "ay":1.8267, "alpha":0.0, "fx":[69.08514,69.08514,69.08514,69.08514], "fy":[29.86308,29.86308,29.86308,29.86308]}, + {"t":0.58626, "x":6.5342, "y":4.3318, "heading":3.14159, "vx":2.48209, "vy":1.07292, "omega":0.0, "ax":4.21996, "ay":1.82414, "alpha":0.0, "fx":[68.98826,68.98826,68.98826,68.98826], "fy":[29.8212,29.8212,29.8212,29.8212]}, + {"t":0.63511, "x":6.66049, "y":4.3864, "heading":3.14159, "vx":2.68825, "vy":1.16204, "omega":0.0, "ax":4.20616, "ay":1.81817, "alpha":0.0, "fx":[68.76271,68.76271,68.76271,68.76271], "fy":[29.7237,29.7237,29.7237,29.7237]}, + {"t":0.68397, "x":6.79685, "y":4.44534, "heading":3.14159, "vx":2.89375, "vy":1.25086, "omega":0.0, "ax":4.13785, "ay":1.78865, "alpha":0.0, "fx":[67.64593,67.64593,67.64593,67.64593], "fy":[29.24095,29.24095,29.24095,29.24095]}, + {"t":0.73282, "x":6.94316, "y":4.50858, "heading":3.14159, "vx":3.0959, "vy":1.33825, "omega":0.0, "ax":-4.13785, "ay":-1.78865, "alpha":0.0, "fx":[-67.64593,-67.64593,-67.64593,-67.64593], "fy":[-29.24095,-29.24095,-29.24095,-29.24095]}, + {"t":0.78168, "x":7.08947, "y":4.57183, "heading":3.14159, "vx":2.89375, "vy":1.25086, "omega":0.0, "ax":-4.20616, "ay":-1.81817, "alpha":0.0, "fx":[-68.76271,-68.76271,-68.76271,-68.76271], "fy":[-29.7237,-29.7237,-29.7237,-29.7237]}, + {"t":0.83053, "x":7.22583, "y":4.63077, "heading":3.14159, "vx":2.68825, "vy":1.16204, "omega":0.0, "ax":-4.21996, "ay":-1.82414, "alpha":0.0, "fx":[-68.98826,-68.98826,-68.98826,-68.98826], "fy":[-29.8212,-29.8212,-29.8212,-29.8212]}, + {"t":0.87939, "x":7.35212, "y":4.68536, "heading":3.14159, "vx":2.48209, "vy":1.07292, "omega":0.0, "ax":-4.22588, "ay":-1.8267, "alpha":0.0, "fx":[-69.08514,-69.08514,-69.08514,-69.08514], "fy":[-29.86308,-29.86308,-29.86308,-29.86308]}, + {"t":0.92824, "x":7.46834, "y":4.7356, "heading":3.14159, "vx":2.27563, "vy":0.98368, "omega":0.0, "ax":-4.22918, "ay":-1.82813, "alpha":0.0, "fx":[-69.13902,-69.13902,-69.13902,-69.13902], "fy":[-29.88637,-29.88637,-29.88637,-29.88637]}, + {"t":0.9771, "x":7.57447, "y":4.78148, "heading":3.14159, "vx":2.06902, "vy":0.89436, "omega":0.0, "ax":-4.23128, "ay":-1.82903, "alpha":0.0, "fx":[-69.17333,-69.17333,-69.17333,-69.17333], "fy":[-29.9012,-29.9012,-29.9012,-29.9012]}, + {"t":1.02595, "x":7.6705, "y":4.82299, "heading":3.14159, "vx":1.8623, "vy":0.80501, "omega":0.0, "ax":-4.23273, "ay":-1.82966, "alpha":0.0, "fx":[-69.19709,-69.19709,-69.19709,-69.19709], "fy":[-29.91147,-29.91147,-29.91147,-29.91147]}, + {"t":1.07481, "x":7.75643, "y":4.86013, "heading":3.14159, "vx":1.65551, "vy":0.71562, "omega":0.0, "ax":-4.2338, "ay":-1.83012, "alpha":0.0, "fx":[-69.21452,-69.21452,-69.21452,-69.21452], "fy":[-29.919,-29.919,-29.919,-29.919]}, + {"t":1.12366, "x":7.83226, "y":4.89291, "heading":3.14159, "vx":1.44867, "vy":0.62621, "omega":0.0, "ax":-4.23461, "ay":-1.83047, "alpha":0.0, "fx":[-69.22785,-69.22785,-69.22785,-69.22785], "fy":[-29.92476,-29.92476,-29.92476,-29.92476]}, + {"t":1.17252, "x":7.89798, "y":4.92132, "heading":3.14159, "vx":1.24179, "vy":0.53678, "omega":0.0, "ax":-4.23526, "ay":-1.83075, "alpha":0.0, "fx":[-69.23837,-69.23837,-69.23837,-69.23837], "fy":[-29.92931,-29.92931,-29.92931,-29.92931]}, + {"t":1.22137, "x":7.9536, "y":4.94536, "heading":3.14159, "vx":1.03487, "vy":0.44734, "omega":0.0, "ax":-4.23578, "ay":-1.83098, "alpha":0.0, "fx":[-69.24689,-69.24689,-69.24689,-69.24689], "fy":[-29.93299,-29.93299,-29.93299,-29.93299]}, + {"t":1.27023, "x":7.9991, "y":4.96503, "heading":3.14159, "vx":0.82793, "vy":0.35789, "omega":0.0, "ax":-4.23621, "ay":-1.83116, "alpha":0.0, "fx":[-69.25393,-69.25393,-69.25393,-69.25393], "fy":[-29.93604,-29.93604,-29.93604,-29.93604]}, + {"t":1.31908, "x":8.03449, "y":4.98033, "heading":3.14159, "vx":0.62097, "vy":0.26843, "omega":0.0, "ax":-4.23657, "ay":-1.83132, "alpha":0.0, "fx":[-69.25984,-69.25984,-69.25984,-69.25984], "fy":[-29.93859,-29.93859,-29.93859,-29.93859]}, + {"t":1.36794, "x":8.05977, "y":4.99126, "heading":3.14159, "vx":0.414, "vy":0.17896, "omega":0.0, "ax":-4.23688, "ay":-1.83145, "alpha":0.0, "fx":[-69.26488,-69.26488,-69.26488,-69.26488], "fy":[-29.94077,-29.94077,-29.94077,-29.94077]}, + {"t":1.41679, "x":8.07494, "y":4.99781, "heading":3.14159, "vx":0.20701, "vy":0.08948, "omega":0.0, "ax":-4.23714, "ay":-1.83157, "alpha":0.0, "fx":[-69.26922,-69.26922,-69.26922,-69.26922], "fy":[-29.94265,-29.94265,-29.94265,-29.94265]}, + {"t":1.46565, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/IJtoNI.traj b/src/main/deploy/choreo/IJtoNI.traj new file mode 100644 index 00000000..f6f12391 --- /dev/null +++ b/src/main/deploy/choreo/IJtoNI.traj @@ -0,0 +1,80 @@ +{ + "name":"IJtoNI", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.188053131103516, "y":5.171266078948975, "heading":-2.073639537722758, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"IJ.x", "val":5.188053131103516}, "y":{"exp":"IJ.y", "val":5.171266078948975}, "heading":{"exp":"IJ.heading", "val":-2.073639537722758}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.59193], + "samples":[ + {"t":0.0, "x":5.18805, "y":5.17127, "heading":-2.07364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.5723, "ay":-0.27152, "alpha":-1.65476, "fx":[74.81943,73.76345,75.44788,74.96278], "fy":[-9.5225,-15.84386,-0.82241,8.43375]}, + {"t":0.03538, "x":5.19091, "y":5.1711, "heading":-2.07364, "vx":0.16175, "vy":-0.00961, "omega":-0.05854, "ax":4.57205, "ay":-0.2715, "alpha":-1.6546, "fx":[74.81456,73.75986,75.44429,74.95857], "fy":[-9.52125,-15.842,-0.82265,8.43199]}, + {"t":0.07075, "x":5.1995, "y":5.17059, "heading":-2.07571, "vx":0.32349, "vy":-0.01921, "omega":-0.11707, "ax":4.57178, "ay":-0.27147, "alpha":-1.65429, "fx":[74.81294,73.75332,75.4401,74.95341], "fy":[-9.49032,-15.85185,-0.84523,8.43509]}, + {"t":0.10613, "x":5.2138, "y":5.16974, "heading":-2.07985, "vx":0.48522, "vy":-0.02881, "omega":-0.17559, "ax":4.57149, "ay":-0.27144, "alpha":-1.65382, "fx":[74.81447,73.74381,75.43523,74.94725], "fy":[-9.42961,-15.87326,-0.89017,8.44284]}, + {"t":0.1415, "x":5.23383, "y":5.16855, "heading":-2.08606, "vx":0.64695, "vy":-0.03842, "omega":-0.2341, "ax":4.57117, "ay":-0.2714, "alpha":-1.6532, "fx":[74.81899,73.73128,75.42958,74.94008], "fy":[-9.3389,-15.90596,-0.95752,8.45485]}, + {"t":0.17688, "x":5.25957, "y":5.16702, "heading":-2.09435, "vx":0.80866, "vy":-0.04802, "omega":-0.29258, "ax":4.57082, "ay":-0.27135, "alpha":-1.65244, "fx":[74.82632,73.71569,75.423,74.93184], "fy":[-9.2179,-15.94955,-1.04732,8.47054]}, + {"t":0.21226, "x":5.29104, "y":5.16515, "heading":-2.1047, "vx":0.97035, "vy":-0.05762, "omega":-0.35104, "ax":4.57043, "ay":-0.27129, "alpha":-1.65155, "fx":[74.8362,73.69703,75.4153,74.9225], "fy":[-9.06621,-16.00351,-1.15967,8.48916]}, + {"t":0.24763, "x":5.32823, "y":5.16294, "heading":-2.11711, "vx":1.13204, "vy":-0.06721, "omega":-0.40947, "ax":4.56998, "ay":-0.27122, "alpha":-1.65053, "fx":[74.8483,73.67523,75.40625,74.91202], "fy":[-8.88336,-16.06718,-1.29465,8.50977]}, + {"t":0.28301, "x":5.37114, "y":5.1604, "heading":-2.1316, "vx":1.29371, "vy":-0.07681, "omega":-0.46786, "ax":4.56947, "ay":-0.27113, "alpha":-1.64942, "fx":[74.86221,73.65024,75.39555,74.90032], "fy":[-8.66883,-16.13976,-1.45235,8.53126]}, + {"t":0.31839, "x":5.41976, "y":5.15751, "heading":-2.14815, "vx":1.45536, "vy":-0.0864, "omega":-0.52621, "ax":4.56887, "ay":-0.27102, "alpha":-1.64825, "fx":[74.87739,73.62198,75.38283,74.88729], "fy":[-8.42206,-16.22028,-1.63287,8.55234]}, + {"t":0.35376, "x":5.47411, "y":5.15428, "heading":-2.16677, "vx":1.61699, "vy":-0.09599, "omega":-0.58452, "ax":4.56817, "ay":-0.2709, "alpha":-1.64704, "fx":[74.89317,73.59029,75.36761,74.87276], "fy":[-8.14247,-16.30761,-1.8363,8.57157]}, + {"t":0.38914, "x":5.53417, "y":5.15072, "heading":-2.18744, "vx":1.77859, "vy":-0.10557, "omega":-0.64278, "ax":4.56734, "ay":-0.27075, "alpha":-1.64584, "fx":[74.90865,73.55495,75.34931,74.85644], "fy":[-7.82947,-16.40046,-2.0627,8.58734]}, + {"t":0.42451, "x":5.59994, "y":5.14681, "heading":-2.21018, "vx":1.94017, "vy":-0.11515, "omega":-0.70101, "ax":4.56633, "ay":-0.27058, "alpha":-1.64472, "fx":[74.92264,73.51557,75.32714,74.83789], "fy":[-7.48255,-16.49731,-2.31211,8.59786]}, + {"t":0.45989, "x":5.67144, "y":5.14257, "heading":-2.23498, "vx":2.10171, "vy":-0.12472, "omega":-0.75919, "ax":4.56508, "ay":-0.27038, "alpha":-1.64373, "fx":[74.93354,73.47152,75.30003,74.81637], "fy":[-7.10124,-16.5964,-2.58451,8.60123]}, + {"t":0.49527, "x":5.74864, "y":5.13799, "heading":-2.26184, "vx":2.2632, "vy":-0.13429, "omega":-0.81734, "ax":4.5635, "ay":-0.27014, "alpha":-1.64296, "fx":[74.93911,73.42175,75.26648,74.79072], "fy":[-6.68519,-16.69569,-2.87981,8.59536]}, + {"t":0.53064, "x":5.83156, "y":5.13307, "heading":-2.29075, "vx":2.42464, "vy":-0.14384, "omega":-0.87546, "ax":4.56144, "ay":-0.26986, "alpha":-1.6425, "fx":[74.93601,73.36443,75.22426,74.75896], "fy":[-6.23418,-16.79277,-3.19781,8.57802]}, + {"t":0.56602, "x":5.92019, "y":5.12781, "heading":-2.32172, "vx":2.58601, "vy":-0.15339, "omega":-0.93356, "ax":4.55868, "ay":-0.26951, "alpha":-1.64246, "fx":[74.91903,73.29628,75.16984,74.71765], "fy":[-5.74814,-16.88474,-3.53814,8.54681]}, + {"t":0.60139, "x":6.01453, "y":5.12222, "heading":-2.35475, "vx":2.74727, "vy":-0.16292, "omega":-0.99167, "ax":4.55478, "ay":-0.26908, "alpha":-1.643, "fx":[74.8792,73.21107,75.09705,74.66038], "fy":[-5.22716,-16.96792,-3.90013,8.49917]}, + {"t":0.63677, "x":6.11457, "y":5.11628, "heading":-2.38983, "vx":2.9084, "vy":-0.17244, "omega":-1.04979, "ax":4.5489, "ay":-0.26851, "alpha":-1.64433, "fx":[74.7993,73.09581,74.99386,74.57412], "fy":[-4.67126,-17.03732,-4.28254,8.43233]}, + {"t":0.67215, "x":6.2203, "y":5.11002, "heading":-2.42697, "vx":3.06933, "vy":-0.18194, "omega":-1.10796, "ax":4.53907, "ay":-0.26768, "alpha":-1.64689, "fx":[74.64014,72.91933,74.83274,74.42811], "fy":[-4.07983,-17.08503,-4.68282,8.34338]}, + {"t":0.70752, "x":6.33172, "y":5.10341, "heading":-2.46616, "vx":3.2299, "vy":-0.19141, "omega":-1.16622, "ax":4.51941, "ay":-0.2662, "alpha":-1.65205, "fx":[74.28619,72.58687,74.53202,74.12989], "fy":[-3.44873,-17.09458,-5.09415,8.22982]}, + {"t":0.7429, "x":6.44881, "y":5.09647, "heading":-2.50742, "vx":3.38978, "vy":-0.20083, "omega":-1.22467, "ax":4.4608, "ay":-0.26217, "alpha":-1.66716, "fx":[73.16851,71.62449,73.68889,73.22038], "fy":[-2.74894,-17.00454,-5.48612,8.09537]}, + {"t":0.77828, "x":6.57152, "y":5.08921, "heading":-2.55074, "vx":3.54759, "vy":-0.2101, "omega":-1.28364, "ax":-0.02415, "ay":0.01691, "alpha":-1.24584, "fx":[-5.15387,-1.2863,4.36804,0.49268], "fy":[1.26754,-4.50516,-0.71378,5.05716]}, + {"t":0.81365, "x":6.697, "y":5.08178, "heading":-2.59616, "vx":3.54673, "vy":-0.20951, "omega":-1.32772, "ax":-4.46317, "ay":0.26378, "alpha":1.61226, "fx":[-73.22989,-71.69651,-73.60396,-73.32708], "fy":[1.57796,16.57219,6.39697,-7.2981]}, + {"t":0.84903, "x":6.81968, "y":5.07454, "heading":-2.64312, "vx":3.38884, "vy":-0.20017, "omega":-1.27068, "ax":-4.52022, "ay":0.26657, "alpha":1.64057, "fx":[-74.37365,-72.59391,-74.37363,-74.24704], "fy":[0.86467,16.93661,6.98509,-7.35467]}, + {"t":0.8844, "x":6.93674, "y":5.06762, "heading":-2.68808, "vx":3.22893, "vy":-0.19074, "omega":-1.21264, "ax":-4.5392, "ay":0.26747, "alpha":1.6567, "fx":[-74.76107,-72.90019,-74.59937,-74.56869], "fy":[0.19245,17.03976,7.4992,-7.24102]}, + {"t":0.91978, "x":7.04812, "y":5.06104, "heading":-2.73098, "vx":3.06835, "vy":-0.18128, "omega":-1.15404, "ax":-4.54859, "ay":0.26792, "alpha":1.66912, "fx":[-74.95252,-73.06246,-74.68816,-74.74012], "fy":[-0.43599,17.05497,7.97489,-7.07375]}, + {"t":0.95516, "x":7.15382, "y":5.0548, "heading":-2.7718, "vx":2.90744, "vy":-0.1718, "omega":-1.09499, "ax":-4.55414, "ay":0.26822, "alpha":1.67959, "fx":[-75.06276,-73.16994,-74.72231,-74.85124], "fy":[-1.02016,17.02178,8.41922,-6.88103]}, + {"t":0.99053, "x":7.25383, "y":5.04889, "heading":-2.81054, "vx":2.74634, "vy":-0.16231, "omega":-1.03557, "ax":-4.55778, "ay":0.26846, "alpha":1.68868, "fx":[-75.13094,-73.25179,-74.72966,-74.93188], "fy":[-1.56037,16.95625,8.8346,-6.67492]}, + {"t":1.02591, "x":7.34813, "y":5.04331, "heading":-2.84717, "vx":2.5851, "vy":-0.15282, "omega":-0.97583, "ax":-4.56034, "ay":0.26868, "alpha":1.69664, "fx":[-75.17441,-73.32002,-74.72234,-74.99467], "fy":[-2.05746,16.8674,9.22219,-6.46239]}, + {"t":1.06128, "x":7.43673, "y":5.03807, "heading":-2.88169, "vx":2.42377, "vy":-0.14331, "omega":-0.91581, "ax":-4.56223, "ay":0.26889, "alpha":1.70359, "fx":[-75.2022,-73.38019,-74.7066,-75.04584], "fy":[-2.51268,16.76143,9.58272,-6.24818]}, + {"t":1.09666, "x":7.51962, "y":5.03317, "heading":-2.91409, "vx":2.26238, "vy":-0.1338, "omega":-0.85555, "ax":-4.56367, "ay":0.26909, "alpha":1.7096, "fx":[-75.21959,-73.43509,-74.68606,-75.08881], "fy":[-2.92754,16.64319,9.91683,-6.03589]}, + {"t":1.13204, "x":7.5968, "y":5.02861, "heading":-2.94436, "vx":2.10093, "vy":-0.12428, "omega":-0.79507, "ax":-4.56482, "ay":0.26929, "alpha":1.71474, "fx":[-75.22992,-73.48609,-74.66294,-75.12561], "fy":[-3.30374,16.51675,10.2251,-5.8284]}, + {"t":1.16741, "x":7.66826, "y":5.02438, "heading":-2.97248, "vx":1.93945, "vy":-0.11475, "omega":-0.73441, "ax":-4.56576, "ay":0.26949, "alpha":1.71908, "fx":[-75.23544,-73.53388,-74.63876,-75.15752], "fy":[-3.64312,16.38566,10.50809,-5.62808]}, + {"t":1.20279, "x":7.73402, "y":5.02049, "heading":-2.99846, "vx":1.77793, "vy":-0.10522, "omega":-0.67359, "ax":-4.56653, "ay":0.26968, "alpha":1.72269, "fx":[-75.23772,-73.57873,-74.61456,-75.1854], "fy":[-3.94754,16.25312,10.76639,-5.43694]}, + {"t":1.23817, "x":7.79405, "y":5.01694, "heading":-3.02229, "vx":1.61638, "vy":-0.09568, "omega":-0.61265, "ax":-4.56719, "ay":0.26986, "alpha":1.72565, "fx":[-75.23789,-73.6207,-74.59107,-75.20986], "fy":[-4.2189,16.12197,11.0006,-5.2567]}, + {"t":1.27354, "x":7.84838, "y":5.01372, "heading":-3.04397, "vx":1.45481, "vy":-0.08613, "omega":-0.5516, "ax":-4.56776, "ay":0.27004, "alpha":1.72804, "fx":[-75.23678,-73.65974,-74.56888,-75.23134], "fy":[-4.45902,15.9948,11.21128,-5.08883]}, + {"t":1.30892, "x":7.89699, "y":5.01084, "heading":-3.06348, "vx":1.29322, "vy":-0.07658, "omega":-0.49047, "ax":-4.56826, "ay":0.2702, "alpha":1.72993, "fx":[-75.23501,-73.69573,-74.54838,-75.25018], "fy":[-4.66965,15.87393,11.39901,-4.93457]}, + {"t":1.34429, "x":7.93988, "y":5.0083, "heading":-3.08083, "vx":1.13161, "vy":-0.06702, "omega":-0.42927, "ax":-4.5687, "ay":0.27034, "alpha":1.73139, "fx":[-75.23305,-73.72854,-74.5299,-75.26668], "fy":[-4.85245,15.7614,11.56434,-4.79501]}, + {"t":1.37967, "x":7.97705, "y":5.0061, "heading":-3.09602, "vx":0.96999, "vy":-0.05746, "omega":-0.36802, "ax":-4.56909, "ay":0.27047, "alpha":1.7325, "fx":[-75.23123,-73.75804,-74.51368,-75.28103], "fy":[-5.00891,15.65905,11.70778,-4.67104]}, + {"t":1.41505, "x":8.00851, "y":5.00424, "heading":-3.10904, "vx":0.80835, "vy":-0.04789, "omega":-0.30673, "ax":-4.56945, "ay":0.27059, "alpha":1.73332, "fx":[-75.22983,-73.7841,-74.49992,-75.29344], "fy":[-5.14039,15.56843,11.8298,-4.56343]}, + {"t":1.45042, "x":8.03424, "y":5.00271, "heading":-3.11989, "vx":0.6467, "vy":-0.03832, "omega":-0.24542, "ax":-4.56977, "ay":0.27069, "alpha":1.73389, "fx":[-75.22904,-73.80661,-74.48876,-75.30405], "fy":[-5.24805,15.49091,11.93082,-4.4728]}, + {"t":1.4858, "x":8.05426, "y":5.00153, "heading":-3.12857, "vx":0.48504, "vy":-0.02874, "omega":-0.18408, "ax":-4.57007, "ay":0.27077, "alpha":1.73428, "fx":[-75.22902,-73.82549,-74.48031,-75.313], "fy":[-5.3329,15.42759,12.01121,-4.39967]}, + {"t":1.52117, "x":8.06856, "y":5.00068, "heading":-3.13508, "vx":0.32337, "vy":-0.01916, "omega":-0.12272, "ax":-4.57034, "ay":0.27083, "alpha":1.73451, "fx":[-75.22989,-73.84068,-74.47466,-75.32038], "fy":[-5.39574,15.37934,12.07125,-4.34442]}, + {"t":1.55655, "x":8.07714, "y":5.00017, "heading":-3.13942, "vx":0.16169, "vy":-0.00958, "omega":-0.06136, "ax":-4.57059, "ay":0.27088, "alpha":1.73462, "fx":[-75.23174,-73.85212,-74.47188,-75.32628], "fy":[-5.43719,15.34685,12.11118,-4.30735]}, + {"t":1.59193, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/NItoEF.traj b/src/main/deploy/choreo/NItoEF.traj new file mode 100644 index 00000000..93a20cb3 --- /dev/null +++ b/src/main/deploy/choreo/NItoEF.traj @@ -0,0 +1,106 @@ +{ + "name":"NItoEF", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.383368492126465, "y":3.130985975265503, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.188053131103516, "y":2.863070487976074, "heading":2.0928880900706415, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.383368492126465 m", "val":6.383368492126465}, "y":{"exp":"3.130985975265503 m", "val":3.130985975265503}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"EF.x", "val":5.188053131103516}, "y":{"exp":"EF.y", "val":2.863070487976074}, "heading":{"exp":"EF.heading", "val":2.0928880900706415}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.12124,1.86587], + "samples":[ + {"t":0.0, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.28056, "ay":-3.99197, "alpha":-1.1299, "fx":[-44.80091,-39.1584,-30.00595,-35.16625], "fy":[-60.66906,-64.47141,-69.19916,-66.70489]}, + {"t":0.02875, "x":8.07906, "y":4.99835, "heading":3.14159, "vx":-0.06557, "vy":-0.11477, "omega":-0.03248, "ax":-2.29702, "ay":-3.98215, "alpha":-1.13297, "fx":[-45.07531,-39.40122,-30.26033,-35.47044], "fy":[-60.46054,-64.31924,-69.08417,-66.53858]}, + {"t":0.0575, "x":8.07622, "y":4.9934, "heading":3.14066, "vx":-0.1316, "vy":-0.22925, "omega":-0.06506, "ax":-2.31466, "ay":-3.97152, "alpha":-1.13624, "fx":[-45.36803,-39.66778,-30.53646,-35.78915], "fy":[-60.23587,-64.15082,-68.95811,-66.36224]}, + {"t":0.08625, "x":8.07148, "y":4.98517, "heading":3.13879, "vx":-0.19815, "vy":-0.34343, "omega":-0.09772, "ax":-2.33365, "ay":-3.95996, "alpha":-1.13973, "fx":[-45.68125,-39.96007,-30.8365,-36.12486], "fy":[-59.9929,-63.96443,-68.81966,-66.1742]}, + {"t":0.115, "x":8.06482, "y":4.97366, "heading":3.13598, "vx":-0.26524, "vy":-0.45728, "omega":-0.13049, "ax":-2.35412, "ay":-3.94735, "alpha":-1.14345, "fx":[-46.01749,-40.28034,-31.16293,-36.48047], "fy":[-59.72909,-63.7581,-68.66728,-65.97243]}, + {"t":0.14375, "x":8.05622, "y":4.95888, "heading":3.13223, "vx":-0.33292, "vy":-0.57077, "omega":-0.16336, "ax":-2.37625, "ay":-3.93356, "alpha":-1.14744, "fx":[-46.37959,-40.63123,-31.51864,-36.85935], "fy":[-59.4415,-63.52949,-68.49911,-65.75455]}, + {"t":0.1725, "x":8.04567, "y":4.94085, "heading":3.12753, "vx":-0.40124, "vy":-0.68386, "omega":-0.19635, "ax":-2.40027, "ay":-3.91839, "alpha":-1.15173, "fx":[-46.77088,-41.01577,-31.90705,-37.26544], "fy":[-59.1266,-63.27582,-68.31292,-65.51766]}, + {"t":0.20125, "x":8.03314, "y":4.91957, "heading":3.12189, "vx":-0.47025, "vy":-0.79651, "omega":-0.22947, "ax":-2.4264, "ay":-3.90165, "alpha":-1.15636, "fx":[-47.19521,-41.43752,-32.33218,-37.70337], "fy":[-58.7802,-62.99376,-68.10604,-65.25829]}, + {"t":0.23, "x":8.01862, "y":4.89506, "heading":3.11529, "vx":-0.54001, "vy":-0.90868, "omega":-0.26271, "ax":-2.45495, "ay":-3.88308, "alpha":-1.16136, "fx":[-47.65709,-41.90065,-32.79876,-38.17866], "fy":[-58.39723,-62.67932,-67.8752,-64.9722]}, + {"t":0.25875, "x":8.00208, "y":4.86733, "heading":3.10774, "vx":-0.61059, "vy":-1.02032, "omega":-0.2961, "ax":-2.48626, "ay":-3.86237, "alpha":-1.16679, "fx":[-48.16182,-42.4101,-33.31249,-38.69784], "fy":[-57.97156,-62.32762,-67.61642,-64.65416]}, + {"t":0.2875, "x":7.9835, "y":4.8364, "heading":3.09922, "vx":-0.68206, "vy":-1.13136, "omega":-0.32964, "ax":-2.52073, "ay":-3.83914, "alpha":-1.1727, "fx":[-48.71569,-42.97174,-33.88016,-39.26878], "fy":[-57.49563,-61.9327,-67.32474,-64.29768]}, + {"t":0.31625, "x":7.96285, "y":4.80228, "heading":3.08975, "vx":-0.75453, "vy":-1.24174, "omega":-0.36336, "ax":-2.55886, "ay":-3.81292, "alpha":-1.17917, "fx":[-49.3262,-43.59262,-34.51002,-39.90096], "fy":[-56.96008,-61.48716,-66.99399,-63.89463]}, + {"t":0.345, "x":7.9401, "y":4.76501, "heading":3.0793, "vx":-0.8281, "vy":-1.35136, "omega":-0.39726, "ax":-2.60125, "ay":-3.7831, "alpha":-1.18626, "fx":[-50.00233,-44.28126,-35.21212,-40.60597], "fy":[-56.35319,-60.98173,-66.61631,-63.43466]}, + {"t":0.37375, "x":7.91521, "y":4.72459, "heading":3.06788, "vx":-0.90289, "vy":-1.46012, "omega":-0.43137, "ax":-2.64862, "ay":-3.74892, "alpha":-1.19408, "fx":[-50.75495,-45.04799,-35.99886,-41.39801], "fy":[-55.66006,-60.40466,-66.18166,-62.90443]}, + {"t":0.4025, "x":7.88816, "y":4.68107, "heading":3.05548, "vx":-0.97903, "vy":-1.5679, "omega":-0.46569, "ax":-2.70189, "ay":-3.70939, "alpha":-1.20272, "fx":[-51.59722,-45.90554,-36.88565,-42.29468], "fy":[-54.86156,-59.74083,-65.6769,-62.28651]}, + {"t":0.43125, "x":7.8589, "y":4.63446, "heading":3.04209, "vx":-1.05671, "vy":-1.67455, "omega":-0.50027, "ax":-2.76217, "ay":-3.6632, "alpha":-1.21232, "fx":[-52.54521,-46.86962,-37.89187,-43.31799], "fy":[-53.93272,-58.97051,-65.08469,-61.55777]}, + {"t":0.46, "x":7.82738, "y":4.5848, "heading":3.0277, "vx":-1.13612, "vy":-1.77986, "omega":-0.53513, "ax":-2.83085, "ay":-3.60862, "alpha":-1.22301, "fx":[-53.61855,-47.95983,-39.04217,-44.49567], "fy":[-52.84052,-58.06753,-64.38159,-60.68686]}, + {"t":0.48875, "x":7.79354, "y":4.53214, "heading":3.01232, "vx":-1.21751, "vy":-1.88361, "omega":-0.57029, "ax":-2.90972, "ay":-3.54327, "alpha":-1.23495, "fx":[-54.84126,-49.20079,-40.36831,-45.86296], "fy":[-51.54049,-56.99652,-63.5353,-59.63055]}, + {"t":0.5175, "x":7.75734, "y":4.47652, "heading":2.99592, "vx":-1.30117, "vy":-1.98548, "omega":-0.60579, "ax":-3.001, "ay":-3.46383, "alpha":-1.24831, "fx":[-56.24255,-50.62357,-41.91161,-47.46485], "fy":[-49.97163,-55.7087,-62.50027,-58.32777]}, + {"t":0.54625, "x":7.71869, "y":4.41801, "heading":2.97851, "vx":-1.38744, "vy":-2.08506, "omega":-0.64168, "ax":-3.10755, "ay":-3.3656, "alpha":-1.26323, "fx":[-57.85731,-52.26748,-43.72649,-49.35883], "fy":[-48.04873,-54.1353,-61.21051,-56.69021]}, + {"t":0.575, "x":7.67752, "y":4.35667, "heading":2.96006, "vx":-1.47678, "vy":-2.18182, "omega":-0.678, "ax":-3.23295, "ay":-3.24168, "alpha":-1.27981, "fx":[-59.72568,-54.18191,-45.88506,-51.61764], "fy":[-45.65026,-52.17687,-59.56753,-54.58669]}, + {"t":0.60375, "x":7.63372, "y":4.2926, "heading":2.94057, "vx":-1.56973, "vy":-2.27502, "omega":-0.71479, "ax":-3.38159, "ay":-3.08173, "alpha":-1.29796, "fx":[-61.88954,-56.42752,-48.48285,-54.33019], "fy":[-42.59958,-49.68583,-57.41947,-51.81709]}, + {"t":0.6325, "x":7.5872, "y":4.22592, "heading":2.92002, "vx":-1.66695, "vy":-2.36362, "omega":-0.75211, "ax":-3.55843, "ay":-2.86983, "alpha":-1.31717, "fx":[-64.38137,-59.07407,-51.64363,-57.5951], "fy":[-38.63577,-46.43713,-54.5237,-48.06829]}, + {"t":0.66125, "x":7.5378, "y":4.15678, "heading":2.89839, "vx":-1.76926, "vy":-2.44613, "omega":-0.78998, "ax":-3.76785, "ay":-2.5809, "alpha":-1.33595, "fx":[-67.19446,-62.18757,-55.51605,-61.49054], "fy":[-33.3711,-42.079,-50.47877,-42.84219]}, + {"t":0.69, "x":7.48538, "y":4.08539, "heading":2.87568, "vx":-1.87758, "vy":-2.52033, "omega":-0.82839, "ax":-4.00973, "ay":-2.17532, "alpha":-1.35043, "fx":[-70.20939,-65.78559,-60.23491,-65.97601], "fy":[-26.2397,-36.0538,-44.60226,-35.35332]}, + {"t":0.71875, "x":7.42974, "y":4.01203, "heading":2.85187, "vx":-1.99286, "vy":-2.58287, "omega":-0.86721, "ax":-4.26846, "ay":-1.59293, "alpha":-1.35107, "fx":[-73.03144,-69.70621,-65.7603,-70.62684], "fy":[-16.47844,-27.49195,-35.73812,-24.45667]}, + {"t":0.7475, "x":7.37068, "y":3.93712, "heading":2.82693, "vx":-2.11558, "vy":-2.62866, "omega":-0.90605, "ax":-4.48732, "ay":-0.75701, "alpha":-1.31414, "fx":[-74.70254,-73.2751,-71.36715,-74.09159], "fy":[-3.29436,-15.17279,-22.11724,-8.91816]}, + {"t":0.77624, "x":7.308, "y":3.86123, "heading":2.80089, "vx":-2.24459, "vy":-2.65043, "omega":-0.94384, "ax":-4.53295, "ay":0.37671, "alpha":-1.18964, "fx":[-73.47309,-74.69638,-74.56135,-73.68943], "fy":[13.44664,2.02139,-2.06868,11.23445]}, + {"t":0.80499, "x":7.2416, "y":3.78519, "heading":2.77375, "vx":-2.37491, "vy":-2.6396, "omega":-0.97804, "ax":-4.22556, "ay":1.69385, "alpha":-0.92774, "fx":[-67.4481,-70.91208,-70.95757,-67.00153], "fy":[31.99529,23.32846,22.79592,32.64547]}, + {"t":0.83374, "x":7.17158, "y":3.71, "heading":2.74563, "vx":-2.49639, "vy":-2.5909, "omega":-1.00471, "ax":-3.53657, "ay":2.8802, "alpha":-0.57237, "fx":[-56.60953,-60.16518,-59.20563,-55.28457], "fy":[48.69612,44.19624,45.35521,50.09533]}, + {"t":0.86249, "x":7.09834, "y":3.6367, "heading":2.71675, "vx":-2.59807, "vy":-2.50809, "omega":-1.02116, "ax":-2.6886, "ay":3.69566, "alpha":-0.2311, "fx":[-43.58431,-45.3238,-44.35007,-42.556], "fy":[60.71894,59.41569,60.11675,61.41677]}, + {"t":0.89124, "x":7.02254, "y":3.56612, "heading":2.68739, "vx":-2.67536, "vy":-2.40185, "omega":-1.02781, "ax":-1.91137, "ay":4.15938, "alpha":0.03878, "fx":[-31.27624,-30.97656,-31.21881,-31.51759], "fy":[67.98188,68.12171,68.0143,67.87363]}, + {"t":0.91999, "x":6.94483, "y":3.49879, "heading":2.65784, "vx":-2.73032, "vy":-2.28226, "omega":-1.02669, "ax":-1.28681, "ay":4.39853, "alpha":0.23564, "fx":[-21.02389,-19.28233,-21.05447,-22.78672], "fy":[71.90768,72.41004,71.92885,71.38395]}, + {"t":0.94874, "x":6.8658, "y":3.43499, "heading":2.62832, "vx":-2.76731, "vy":-2.15581, "omega":-1.01992, "ax":-0.80606, "ay":4.51541, "alpha":0.37737, "fx":[-12.92254,-10.30817,-13.42086,-16.05852], "fy":[73.87158,74.30503,73.82335,73.27335]}, + {"t":0.97749, "x":6.78591, "y":3.37488, "heading":2.599, "vx":-2.79049, "vy":-2.02599, "omega":-1.00907, "ax":-0.43711, "ay":4.56881, "alpha":0.48118, "fx":[-6.60912,-3.47646,-7.63702,-10.86111], "fy":[74.76447,75.00285,74.71118,74.28657]}, + {"t":1.00624, "x":6.7055, "y":3.31852, "heading":2.56999, "vx":-2.80305, "vy":-1.89464, "omega":-0.99524, "ax":-0.15038, "ay":4.58939, "alpha":0.55924, "fx":[-1.66963,1.79002,-3.16115,-6.79283], "fy":[75.08906,75.11531,75.08664,74.82017]}, + {"t":1.03499, "x":6.62486, "y":3.26595, "heading":2.54138, "vx":-2.80738, "vy":-1.76269, "omega":-0.97916, "ax":0.07638, "ay":4.59292, "alpha":0.61952, "fx":[2.23797,5.92798,0.38443,-3.55563], "fy":[75.11684,74.94538,75.19414,75.08576]}, + {"t":1.06374, "x":6.54418, "y":3.21717, "heading":2.51323, "vx":-2.80518, "vy":-1.63065, "omega":-0.96135, "ax":0.25896, "ay":4.58761, "alpha":0.66721, "fx":[5.3704,9.24447,3.25589,-0.93667], "fy":[74.99349,74.64408,75.15867,75.19873]}, + {"t":1.09249, "x":6.46363, "y":3.17218, "heading":2.48559, "vx":-2.79773, "vy":-1.49875, "omega":-0.94216, "ax":0.40847, "ay":4.57784, "alpha":0.70574, "fx":[7.91483,11.95238,5.62801,1.2154], "fy":[74.79764,74.28777,75.04645,75.22372]}, + {"t":1.12124, "x":6.38337, "y":3.13099, "heading":2.4585, "vx":-2.78599, "vy":-1.36714, "omega":-0.92188, "ax":0.67511, "ay":4.54288, "alpha":0.76556, "fx":[12.58059,16.6488,9.7503,5.1671], "fy":[74.11506,73.34409,74.5914,75.01896]}, + {"t":1.14606, "x":6.31443, "y":3.09845, "heading":2.43562, "vx":-2.76923, "vy":-1.25438, "omega":-0.90287, "ax":1.08771, "ay":4.45963, "alpha":0.86375, "fx":[19.99711,23.8179,15.97767,11.33535], "fy":[72.45628,71.33548,73.50606,74.32814]}, + {"t":1.17088, "x":6.24602, "y":3.06869, "heading":2.41321, "vx":-2.74224, "vy":-1.14369, "omega":-0.88143, "ax":1.50305, "ay":4.33479, "alpha":0.95807, "fx":[27.51974,30.83788,22.21126,17.71896], "fy":[69.939,68.59325,71.86721,73.06262]}, + {"t":1.19571, "x":6.17842, "y":3.04164, "heading":2.39133, "vx":-2.70493, "vy":-1.0361, "omega":-0.85765, "ax":1.90969, "ay":4.16917, "alpha":1.04589, "fx":[34.89277,37.50417,28.30927,24.17304], "fy":[66.56504,65.19108,69.692,71.18364]}, + {"t":1.22053, "x":6.11187, "y":3.0172, "heading":2.37004, "vx":-2.65753, "vy":-0.93261, "omega":-0.83169, "ax":2.29651, "ay":3.96693, "alpha":1.12478, "fx":[41.85798,43.64569,34.13739,30.53352], "fy":[62.42252,61.25647,67.0337,68.69447]}, + {"t":1.24535, "x":6.04662, "y":2.99528, "heading":2.3494, "vx":-2.60053, "vy":-0.83415, "omega":-0.80378, "ax":2.65417, "ay":3.73508, "alpha":1.19279, "fx":[48.1977,49.14529,39.58318,36.63623], "fy":[57.67588,56.9497,63.97536,65.64484]}, + {"t":1.27017, "x":5.98289, "y":2.97572, "heading":2.32945, "vx":-2.53465, "vy":-0.74144, "omega":-0.77417, "ax":2.97614, "ay":3.48235, "alpha":1.24872, "fx":[53.76712,53.94675,44.56592,42.33731], "fy":[52.53582,52.43839,60.61858,62.1263]}, + {"t":1.29499, "x":5.92089, "y":2.95839, "heading":2.31023, "vx":-2.46078, "vy":-0.65501, "omega":-0.74317, "ax":3.25916, "ay":3.2179, "alpha":1.29233, "fx":[58.50555,58.04883,49.04017,47.52941], "fy":[47.2209,47.87557,57.07065,58.25917]}, + {"t":1.31981, "x":5.86082, "y":2.94313, "heading":2.29179, "vx":-2.37988, "vy":-0.57513, "omega":-0.7111, "ax":3.50288, "ay":2.95018, "alpha":1.32428, "fx":[62.42676,61.49147,52.99351,52.14966], "fy":[41.92476,43.38601,53.43313,54.17523]}, + {"t":1.34463, "x":5.80282, "y":2.92976, "heading":2.27414, "vx":-2.29294, "vy":-0.50191, "omega":-0.67823, "ax":3.70924, "ay":2.68614, "alpha":1.34589, "fx":[65.59708,64.33987,56.44018,56.17861], "fy":[36.79723,39.06124,49.79381,50.00107]}, + {"t":1.36945, "x":5.74705, "y":2.91813, "heading":2.2573, "vx":-2.20087, "vy":-0.43524, "omega":-0.64482, "ax":3.88161, "ay":2.43101, "alpha":1.35884, "fx":[68.11133,66.67048,59.41317,59.63257], "fy":[31.93971,34.96124,46.22279,45.84602]}, + {"t":1.39427, "x":5.69362, "y":2.90808, "heading":2.2413, "vx":-2.10452, "vy":-0.37489, "omega":-0.61109, "ax":4.02407, "ay":2.18829, "alpha":1.36488, "fx":[70.07309,68.56094,61.95651,62.55282], "fy":[27.4103,31.11963,42.77176,41.79583]}, + {"t":1.4191, "x":5.64262, "y":2.89944, "heading":2.22613, "vx":-2.00464, "vy":-0.32058, "omega":-0.57722, "ax":4.14084, "ay":1.96003, "alpha":1.36561, "fx":[71.58186,70.08385,64.11898,64.99485], "fy":[23.23349,27.5501,39.47567,37.91164]}, + {"t":1.44392, "x":5.59414, "y":2.89209, "heading":2.2118, "vx":-1.90186, "vy":-0.27193, "omega":-0.54332, "ax":4.23595, "ay":1.74715, "alpha":1.36241, "fx":[72.72617,71.30365,65.94959,67.01963], "fy":[19.41022,24.25234,36.35547,34.23234]}, + {"t":1.46874, "x":5.54824, "y":2.88588, "heading":2.19831, "vx":-1.79672, "vy":-0.22856, "omega":-0.5095, "ax":4.31302, "ay":1.54977, "alpha":1.35638, "fx":[73.58106,72.27561,67.49466,68.68749], "fy":[15.92656,21.21698,33.42123,30.77869]}, + {"t":1.49356, "x":5.50497, "y":2.88068, "heading":2.18567, "vx":-1.68967, "vy":-0.1901, "omega":-0.47584, "ax":4.3752, "ay":1.36747, "alpha":1.34839, "fx":[74.20818,73.04605,68.79627,70.05438], "fy":[12.76018,18.42931,30.67496,27.55777]}, + {"t":1.51838, "x":5.46438, "y":2.87639, "heading":2.17386, "vx":-1.58107, "vy":-0.15615, "omega":-0.44237, "ax":4.42516, "ay":1.19948, "alpha":1.33909, "fx":[74.65715,73.65321,69.89159,71.17004], "fy":[9.88485,15.87189,28.11311,24.56705]}, + {"t":1.5432, "x":5.4265, "y":2.87288, "heading":2.16288, "vx":-1.47123, "vy":-0.12638, "omega":-0.40913, "ax":4.46514, "ay":1.04486, "alpha":1.32899, "fx":[74.96732,74.1283,70.81284,72.07742], "fy":[7.27333,13.52629,25.72845,21.79766]}, + {"t":1.56802, "x":5.39136, "y":2.87007, "heading":2.15272, "vx":-1.3604, "vy":-0.10045, "omega":-0.37614, "ax":4.49696, "ay":0.90258, "alpha":1.31844, "fx":[75.1697,74.49671,71.58753,72.81291], "fy":[4.8991,11.37425,23.51156,19.23701]}, + {"t":1.59284, "x":5.35898, "y":2.86785, "heading":2.14339, "vx":-1.24878, "vy":-0.07804, "omega":-0.34342, "ax":4.52214, "ay":0.77162, "alpha":1.3077, "fx":[75.28856,74.77896,72.23899,73.40686], "fy":[2.7373,9.3983,21.45186,16.87049]}, + {"t":1.61766, "x":5.32937, "y":2.86615, "heading":2.13486, "vx":-1.13654, "vy":-0.05889, "omega":-0.31096, "ax":4.5419, "ay":0.65097, "alpha":1.29696, "fx":[75.34289,74.99162,72.78686,73.88442], "fy":[0.7652,7.58215,19.53835,14.6828]}, + {"t":1.64248, "x":5.30256, "y":2.86489, "heading":2.12714, "vx":-1.0238, "vy":-0.04273, "omega":-0.27877, "ax":4.55725, "ay":0.5397, "alpha":1.28636, "fx":[75.34748,75.1481,73.2476,74.26625], "fy":[-1.03768,5.91085,17.76007,12.65875]}, + {"t":1.66731, "x":5.27855, "y":2.864, "heading":2.12022, "vx":-0.91069, "vy":-0.02934, "omega":-0.24684, "ax":4.56899, "ay":0.43692, "alpha":1.27598, "fx":[75.3139,75.25921,73.63498,74.5693], "fy":[-2.68958,4.37079,16.10643,10.7837]}, + {"t":1.69213, "x":5.25736, "y":2.8634, "heading":2.1141, "vx":-0.79728, "vy":-0.01849, "omega":-0.21517, "ax":4.57779, "ay":0.34185, "alpha":1.26589, "fx":[75.2512,75.33373,73.96052,74.80743], "fy":[-4.20669,2.94973,14.56743,9.04394]}, + {"t":1.71695, "x":5.23898, "y":2.86305, "heading":2.10876, "vx":-0.68366, "vy":-0.01001, "omega":-0.18375, "ax":4.58419, "ay":0.25376, "alpha":1.25612, "fx":[75.16643,75.37877,74.23384,74.99196], "fy":[-5.60328,1.63666,13.13369,7.42677]}, + {"t":1.74177, "x":5.22342, "y":2.86288, "heading":2.1042, "vx":-0.56987, "vy":-0.00371, "omega":-0.15257, "ax":4.58861, "ay":0.17199, "alpha":1.24671, "fx":[75.06515,75.4001,74.463,75.13216], "fy":[-6.89192,0.42174,11.79652,5.92054]}, + {"t":1.76659, "x":5.21069, "y":2.86284, "heading":2.10041, "vx":-0.45598, "vy":0.00056, "omega":-0.12162, "ax":4.59143, "ay":0.09596, "alpha":1.23766, "fx":[74.9517,75.40244,74.65475,75.2356], "fy":[-8.08368,-0.70384,10.54795,4.51468]}, + {"t":1.79141, "x":5.20079, "y":2.86288, "heading":2.09739, "vx":-0.34201, "vy":0.00294, "omega":-0.0909, "ax":4.59293, "ay":0.02514, "alpha":1.22898, "fx":[74.82949,75.38964,74.81475,75.30849], "fy":[-9.18828,-1.74794,9.38067,3.19961]}, + {"t":1.81623, "x":5.19371, "y":2.86296, "heading":2.09513, "vx":-0.22801, "vy":0.00357, "omega":-0.0604, "ax":4.59334, "ay":-0.04094, "alpha":1.22067, "fx":[74.70121,75.36483,74.94776,75.35593], "fy":[-10.21429,-2.71757,8.28801,1.9667]}, + {"t":1.84105, "x":5.18947, "y":2.86304, "heading":2.09364, "vx":-0.114, "vy":0.00255, "omega":-0.0301, "ax":4.59288, "ay":-0.10271, "alpha":1.21272, "fx":[74.56897,75.33061,75.05779,75.38211], "fy":[-11.16926,-3.619,7.2639,0.80819]}, + {"t":1.86587, "x":5.18805, "y":2.86307, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/NItoIJ.traj b/src/main/deploy/choreo/NItoIJ.traj new file mode 100644 index 00000000..fd1c23df --- /dev/null +++ b/src/main/deploy/choreo/NItoIJ.traj @@ -0,0 +1,80 @@ +{ + "name":"NItoIJ", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.188053131103516, "y":5.171266078948975, "heading":-2.073639537722758, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"IJ.x", "val":5.188053131103516}, "y":{"exp":"IJ.y", "val":5.171266078948975}, "heading":{"exp":"IJ.heading", "val":-2.073639537722758}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.59192], + "samples":[ + {"t":0.0, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.57063, "ay":0.27087, "alpha":1.73375, "fx":[-75.23066,-73.85697,-74.4693,-75.32756], "fy":[-5.45257,15.32339,12.12694,-4.2852]}, + {"t":0.03538, "x":8.07714, "y":5.00017, "heading":3.14159, "vx":-0.16169, "vy":0.00958, "omega":0.06133, "ax":-4.57038, "ay":0.27085, "alpha":1.73375, "fx":[-75.22592,-73.85239,-74.46565,-75.32383], "fy":[-5.45224,15.32248,12.12638,-4.285]}, + {"t":0.07075, "x":8.06856, "y":5.00068, "heading":-3.13942, "vx":-0.32337, "vy":0.01916, "omega":0.12267, "ax":-4.5701, "ay":0.27082, "alpha":1.73364, "fx":[-75.22218,-73.84399,-74.46488,-75.31866], "fy":[-5.4311,15.3379,12.10592,-4.3032]}, + {"t":0.10613, "x":8.05426, "y":5.00153, "heading":-3.13508, "vx":-0.48504, "vy":0.02874, "omega":0.184, "ax":-4.5698, "ay":0.27077, "alpha":1.73342, "fx":[-75.21935,-73.83174,-74.46694,-75.31198], "fy":[-5.38894,15.3694,12.06548,-4.3397]}, + {"t":0.1415, "x":8.03424, "y":5.00271, "heading":-3.12857, "vx":-0.64671, "vy":0.03832, "omega":0.24532, "ax":-4.56947, "ay":0.2707, "alpha":1.73306, "fx":[-75.21732,-73.81562,-74.47174,-75.3037], "fy":[-5.32532,15.4165,12.00489,-4.39428]}, + {"t":0.17688, "x":8.00851, "y":5.00424, "heading":-3.1199, "vx":-0.80836, "vy":0.0479, "omega":0.30663, "ax":-4.5691, "ay":0.27062, "alpha":1.73253, "fx":[-75.21598,-73.79564,-74.47919,-75.29369], "fy":[-5.23961,15.47852,11.9239,-4.46664]}, + {"t":0.21226, "x":7.97705, "y":5.0061, "heading":-3.10905, "vx":-0.96999, "vy":0.05747, "omega":0.36792, "ax":-4.5687, "ay":0.27051, "alpha":1.73178, "fx":[-75.21514,-73.77184,-74.48916,-75.28182], "fy":[-5.131,15.55453,11.82222,-4.55636]}, + {"t":0.24763, "x":7.93988, "y":5.0083, "heading":-3.09603, "vx":-1.13161, "vy":0.06704, "omega":0.42918, "ax":-4.56824, "ay":0.27039, "alpha":1.73078, "fx":[-75.21458,-73.74424,-74.5015,-75.26791], "fy":[-4.99847,15.64338,11.69945,-4.66291]}, + {"t":0.28301, "x":7.89699, "y":5.01084, "heading":-3.08085, "vx":-1.29322, "vy":0.07661, "omega":0.49041, "ax":-4.56773, "ay":0.27025, "alpha":1.72946, "fx":[-75.21402,-73.71288,-74.51599,-75.25173], "fy":[-4.84081,15.74366,11.55516,-4.78561]}, + {"t":0.31838, "x":7.84838, "y":5.01372, "heading":-3.0635, "vx":-1.45481, "vy":0.08617, "omega":0.55159, "ax":-4.56714, "ay":0.2701, "alpha":1.72774, "fx":[-75.21305,-73.67778,-74.53234,-75.23301], "fy":[-4.65665,15.85374,11.38885,-4.92366]}, + {"t":0.35376, "x":7.79406, "y":5.01694, "heading":-3.04399, "vx":-1.61638, "vy":0.09572, "omega":0.61271, "ax":-4.56646, "ay":0.26993, "alpha":1.72556, "fx":[-75.21115,-73.63896,-74.5502,-75.21138], "fy":[-4.44445,15.97169,11.19998,-5.07609]}, + {"t":0.38914, "x":7.73402, "y":5.0205, "heading":-3.02231, "vx":-1.77792, "vy":0.10527, "omega":0.67375, "ax":-4.56566, "ay":0.26974, "alpha":1.72283, "fx":[-75.20761,-73.59633,-74.56905,-75.18638], "fy":[-4.20254,16.09534,10.98796,-5.24176]}, + {"t":0.42451, "x":7.66826, "y":5.02439, "heading":-2.99848, "vx":-1.93943, "vy":0.11481, "omega":0.7347, "ax":-4.5647, "ay":0.26954, "alpha":1.71946, "fx":[-75.20141,-73.5497,-74.5882,-75.15737], "fy":[-3.92911,16.22218,10.75218,-5.41928]}, + {"t":0.45989, "x":7.5968, "y":5.02862, "heading":-2.97249, "vx":-2.10092, "vy":0.12435, "omega":0.79553, "ax":-4.56353, "ay":0.26933, "alpha":1.71535, "fx":[-75.19114,-73.49868,-74.60668,-75.12346], "fy":[-3.62225,16.34938,10.49196,-5.60701]}, + {"t":0.49526, "x":7.51962, "y":5.03319, "heading":-2.94435, "vx":-2.26236, "vy":0.13388, "omega":0.85621, "ax":-4.56205, "ay":0.2691, "alpha":1.7104, "fx":[-75.17473,-73.44245,-74.62306,-75.08332], "fy":[-3.28001,16.47367,10.2066,-5.80299]}, + {"t":0.53064, "x":7.43673, "y":5.03809, "heading":-2.91406, "vx":-2.42374, "vy":0.1434, "omega":0.91672, "ax":-4.56014, "ay":0.26886, "alpha":1.70449, "fx":[-75.14903,-73.3795,-74.63518,-75.03489], "fy":[-2.90038,16.59123,9.89535,-6.00477]}, + {"t":0.56602, "x":7.34814, "y":5.04333, "heading":-2.88163, "vx":-2.58506, "vy":0.15291, "omega":0.97701, "ax":-4.55757, "ay":0.2686, "alpha":1.69749, "fx":[-75.109,-73.3069,-74.63953,-74.9747], "fy":[-2.48131,16.69738,9.55731,-6.20927]}, + {"t":0.60139, "x":7.25384, "y":5.04891, "heading":-2.84706, "vx":-2.74629, "vy":0.16241, "omega":1.03707, "ax":-4.55391, "ay":0.26829, "alpha":1.6892, "fx":[-75.04585,-73.21892,-74.62994,-74.89652], "fy":[-2.02076,16.78596,9.19134,-6.41226]}, + {"t":0.63677, "x":7.15383, "y":5.05482, "heading":-2.81038, "vx":-2.90739, "vy":0.1719, "omega":1.09682, "ax":-4.54836, "ay":0.26791, "alpha":1.67928, "fx":[-74.94252,-73.10334,-74.59435,-74.78776], "fy":[-1.51657,16.84784,8.79564,-6.60744]}, + {"t":0.67214, "x":7.04814, "y":5.06107, "heading":-2.77158, "vx":-3.06829, "vy":0.18138, "omega":1.15623, "ax":-4.53898, "ay":0.26736, "alpha":1.66702, "fx":[-74.76024,-72.93057,-74.50507,-74.61887], "fy":[-0.96608,16.86643,8.36663,-6.78355]}, + {"t":0.70752, "x":6.93675, "y":5.06766, "heading":-2.73067, "vx":-3.22886, "vy":0.19084, "omega":1.2152, "ax":-4.52004, "ay":0.26634, "alpha":1.65038, "fx":[-74.38494,-72.61024,-74.27985,-74.30095], "fy":[-0.36449,16.80048,7.89432,-6.91368]}, + {"t":0.7429, "x":6.8197, "y":5.07457, "heading":-2.68768, "vx":-3.38877, "vy":0.20026, "omega":1.27359, "ax":-4.46309, "ay":0.26334, "alpha":1.6199, "fx":[-73.25905,-71.6897,-73.51189,-73.39138], "fy":[0.30637,16.46839,7.33039,-6.88467]}, + {"t":0.77827, "x":6.69703, "y":5.08182, "heading":-2.64263, "vx":-3.54665, "vy":0.20958, "omega":1.33089, "ax":-0.02749, "ay":0.01122, "alpha":-1.3406, "fx":[-5.45065,-1.87675,4.55646,0.97341], "fy":[1.71464,-4.85157,-1.34872,5.2192]}, + {"t":0.81365, "x":6.57154, "y":5.08924, "heading":-2.59555, "vx":-3.54762, "vy":0.20997, "omega":1.28347, "ax":4.46053, "ay":-0.26213, "alpha":-1.67818, "fx":[73.21363,71.58193,73.60859,73.28022], "fy":[-1.42247,-17.05682,-6.44465,7.78251]}, + {"t":0.84902, "x":6.44883, "y":5.09651, "heading":-2.55014, "vx":-3.38983, "vy":0.2007, "omega":1.2241, "ax":4.51925, "ay":-0.26598, "alpha":-1.66015, "fx":[74.33742,72.55253,74.46251,74.17183], "fy":[-2.16249,-17.17994,-6.0133,7.96237]}, + {"t":0.8844, "x":6.33174, "y":5.10344, "heading":-2.50684, "vx":-3.22996, "vy":0.19129, "omega":1.16537, "ax":4.53897, "ay":-0.2674, "alpha":-1.65284, "fx":[74.69855,72.88273,74.77219,74.46083], "fy":[-2.84389,-17.20304,-5.56114,8.12247]}, + {"t":0.91978, "x":6.22032, "y":5.11004, "heading":-2.46561, "vx":-3.06938, "vy":0.18183, "omega":1.1069, "ax":4.54886, "ay":-0.2682, "alpha":-1.64841, "fx":[74.86401,73.05551,74.94129,74.59998], "fy":[-3.49162,-17.18295,-5.11862,8.25504]}, + {"t":0.95515, "x":6.11458, "y":5.1163, "heading":-2.42646, "vx":-2.90846, "vy":0.17234, "omega":1.04858, "ax":4.55479, "ay":-0.26876, "alpha":-1.64546, "fx":[74.94891,73.1674,75.05167,74.68057], "fy":[-4.10806,-17.13559,-4.69283,8.36164]}, + {"t":0.99053, "x":6.01454, "y":5.12223, "heading":-2.38936, "vx":-2.74733, "vy":0.16283, "omega":0.99037, "ax":4.55873, "ay":-0.2692, "alpha":-1.64352, "fx":[74.9923,73.25002,75.13095,74.73297], "fy":[-4.69258,-17.06903,-4.28662,8.44486]}, + {"t":1.02591, "x":5.9202, "y":5.12783, "heading":-2.35433, "vx":-2.58606, "vy":0.15331, "omega":0.93223, "ax":4.56153, "ay":-0.26956, "alpha":-1.64238, "fx":[75.01143,73.31651,75.19117,74.77008], "fy":[-5.24418,-16.9886,-3.90145,8.50736]}, + {"t":1.06128, "x":5.83157, "y":5.13308, "heading":-2.32135, "vx":-2.4247, "vy":0.14378, "omega":0.87413, "ax":4.56361, "ay":-0.26986, "alpha":-1.64189, "fx":[75.01531,73.37309,75.23855,74.79826], "fy":[-5.76202,-16.89837,-3.53817,8.55172]}, + {"t":1.09666, "x":5.74865, "y":5.138, "heading":-2.29042, "vx":-2.26325, "vy":0.13423, "omega":0.81605, "ax":4.56521, "ay":-0.27012, "alpha":-1.6419, "fx":[75.00926,73.42301,75.27665,74.82088], "fy":[-6.24552,-16.80164,-3.1973,8.58042]}, + {"t":1.13203, "x":5.67145, "y":5.14258, "heading":-2.26156, "vx":-2.10175, "vy":0.12467, "omega":0.75796, "ax":4.56647, "ay":-0.27035, "alpha":-1.64233, "fx":[74.99671,73.468,75.30775,74.83991], "fy":[-6.69439,-16.70121,-2.87917,8.5958]}, + {"t":1.16741, "x":5.59995, "y":5.14682, "heading":-2.23474, "vx":-1.94021, "vy":0.11511, "omega":0.69986, "ax":4.56749, "ay":-0.27055, "alpha":-1.64305, "fx":[74.98005,73.50904,75.33339,74.85648], "fy":[-7.10857,-16.59948,-2.58398,8.60011]}, + {"t":1.20279, "x":5.53417, "y":5.15072, "heading":-2.20998, "vx":-1.77863, "vy":0.10554, "omega":0.64174, "ax":4.56832, "ay":-0.27072, "alpha":-1.64399, "fx":[74.961,73.54669,75.35467,74.87129], "fy":[-7.48824,-16.49856,-2.31182,8.59549]}, + {"t":1.23816, "x":5.47411, "y":5.15429, "heading":-2.18728, "vx":-1.61702, "vy":0.09596, "omega":0.58358, "ax":4.56902, "ay":-0.27087, "alpha":-1.64506, "fx":[74.94086,73.58123,75.37242,74.88475], "fy":[-7.83371,-16.40032,-2.06271,8.58393]}, + {"t":1.27354, "x":5.41976, "y":5.15751, "heading":-2.16664, "vx":-1.45539, "vy":0.08638, "omega":0.52539, "ax":4.56961, "ay":-0.271, "alpha":-1.64621, "fx":[74.92063,73.61284,75.38728,74.8971], "fy":[-8.14542,-16.30637,-1.83664,8.5673]}, + {"t":1.30891, "x":5.37114, "y":5.1604, "heading":-2.14805, "vx":-1.29373, "vy":0.07679, "omega":0.46715, "ax":4.57012, "ay":-0.27111, "alpha":-1.64737, "fx":[74.90111,73.64157,75.39977,74.90849], "fy":[-8.42388,-16.21815,-1.63354,8.54733]}, + {"t":1.34429, "x":5.32823, "y":5.16294, "heading":-2.13152, "vx":-1.13206, "vy":0.0672, "omega":0.40887, "ax":4.57056, "ay":-0.2712, "alpha":-1.6485, "fx":[74.88293,73.66747,75.4103,74.91897], "fy":[-8.66965,-16.13691,-1.45334,8.52561]}, + {"t":1.37967, "x":5.29104, "y":5.16515, "heading":-2.11706, "vx":-0.97037, "vy":0.05761, "omega":0.35056, "ax":4.57094, "ay":-0.27128, "alpha":-1.64956, "fx":[74.86658,73.69054,75.41921,74.92858], "fy":[-8.8833,-16.06372,-1.29593,8.50356]}, + {"t":1.41504, "x":5.25958, "y":5.16702, "heading":-2.10466, "vx":-0.80867, "vy":0.04801, "omega":0.2922, "ax":4.57129, "ay":-0.27134, "alpha":-1.65053, "fx":[74.85247,73.71075,75.42677,74.93733], "fy":[-9.06539,-15.99953,-1.16121,8.48245]}, + {"t":1.45042, "x":5.23383, "y":5.16855, "heading":-2.09432, "vx":-0.64696, "vy":0.03841, "omega":0.23381, "ax":4.57159, "ay":-0.2714, "alpha":-1.65139, "fx":[74.84092,73.7281,75.43323,74.94522], "fy":[-9.21643,-15.9451,-1.04908,8.46339]}, + {"t":1.48579, "x":5.2138, "y":5.16974, "heading":-2.08605, "vx":-0.48523, "vy":0.02881, "omega":0.17539, "ax":4.57187, "ay":-0.27144, "alpha":-1.65211, "fx":[74.83218,73.74258,75.43875,74.95222], "fy":[-9.33688,-15.90109,-0.95946,8.44729]}, + {"t":1.52117, "x":5.1995, "y":5.17059, "heading":-2.07985, "vx":-0.3235, "vy":0.01921, "omega":0.11695, "ax":4.57213, "ay":-0.27148, "alpha":-1.65269, "fx":[74.82645,73.75418,75.44351,74.95835], "fy":[-9.42712,-15.868,-0.89226,8.43491]}, + {"t":1.55655, "x":5.19091, "y":5.1711, "heading":-2.07571, "vx":-0.16175, "vy":0.0096, "omega":0.05848, "ax":4.57237, "ay":-0.2715, "alpha":-1.65313, "fx":[74.82388,73.76291,75.4476,74.9636], "fy":[-9.48746,-15.84623,-0.84743,8.42682]}, + {"t":1.59192, "x":5.18805, "y":5.17127, "heading":-2.07364, "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/Sprint Test.traj b/src/main/deploy/choreo/Sprint Test.traj deleted file mode 100644 index acefb3b4..00000000 --- a/src/main/deploy/choreo/Sprint Test.traj +++ /dev/null @@ -1,158 +0,0 @@ -{ - "name":"Sprint Test", - "version":1, - "snapshot":{ - "waypoints":[ - {"x":0.5679131746292114, "y":1.726379156112671, "heading":0.0, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":8.309528350830078, "y":1.726379156112671, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.358970642089844, "y":1.726379156112671, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4679131746292118, "y":1.726379156112671, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"0.5679131746292114 m", "val":0.5679131746292114}, "y":{"exp":"1.726379156112671 m", "val":1.726379156112671}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"8.309528350830078 m", "val":8.309528350830078}, "y":{"exp":"1.726379156112671 m", "val":1.726379156112671}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.358970642089844 m", "val":4.358970642089844}, "y":{"exp":"1.726379156112671 m", "val":1.726379156112671}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4679131746292118 m", "val":1.4679131746292118}, "y":{"exp":"1.726379156112671 m", "val":1.726379156112671}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":"Swerve", - "waypoints":[0.0,2.66512,4.01133,5.11578], - "samples":[ - {"t":0.0, "x":0.56791, "y":1.72638, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.9712, "ay":0.0, "alpha":0.0, "fx":[81.26964,81.26964,81.26964,81.26964], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.04935, "x":0.57397, "y":1.72638, "heading":0.0, "vx":0.24535, "vy":0.0, "omega":0.0, "ax":4.97094, "ay":0.0, "alpha":0.0, "fx":[81.2654,81.2654,81.2654,81.2654], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.09871, "x":0.59213, "y":1.72638, "heading":0.0, "vx":0.49068, "vy":0.0, "omega":0.0, "ax":4.97065, "ay":0.0, "alpha":0.0, "fx":[81.26059,81.26059,81.26059,81.26059], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.14806, "x":0.6224, "y":1.72638, "heading":0.0, "vx":0.73601, "vy":0.0, "omega":0.0, "ax":4.97031, "ay":0.0, "alpha":0.0, "fx":[81.2551,81.2551,81.2551,81.2551], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.19742, "x":0.66478, "y":1.72638, "heading":0.0, "vx":0.98131, "vy":0.0, "omega":0.0, "ax":4.96992, "ay":0.0, "alpha":0.0, "fx":[81.24876,81.24876,81.24876,81.24876], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.24677, "x":0.71926, "y":1.72638, "heading":0.0, "vx":1.2266, "vy":0.0, "omega":0.0, "ax":4.96947, "ay":0.0, "alpha":0.0, "fx":[81.24136,81.24136,81.24136,81.24136], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.29612, "x":0.78585, "y":1.72638, "heading":0.0, "vx":1.47186, "vy":0.0, "omega":0.0, "ax":4.96893, "ay":0.0, "alpha":0.0, "fx":[81.23262,81.23262,81.23262,81.23262], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.34548, "x":0.86455, "y":1.72638, "heading":0.0, "vx":1.7171, "vy":0.0, "omega":0.0, "ax":4.96829, "ay":0.0, "alpha":0.0, "fx":[81.22213,81.22213,81.22213,81.22213], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.39483, "x":0.95534, "y":1.72638, "heading":0.0, "vx":1.9623, "vy":0.0, "omega":0.0, "ax":4.96751, "ay":0.0, "alpha":0.0, "fx":[81.20931,81.20931,81.20931,81.20931], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.44419, "x":1.05824, "y":1.72638, "heading":0.0, "vx":2.20747, "vy":0.0, "omega":0.0, "ax":4.96653, "ay":0.0, "alpha":0.0, "fx":[81.1933,81.1933,81.1933,81.1933], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.49354, "x":1.17324, "y":1.72638, "heading":0.0, "vx":2.45259, "vy":0.0, "omega":0.0, "ax":4.96527, "ay":0.0, "alpha":0.0, "fx":[81.17271,81.17271,81.17271,81.17271], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.54289, "x":1.30033, "y":1.72638, "heading":0.0, "vx":2.69764, "vy":0.0, "omega":0.0, "ax":4.96359, "ay":0.0, "alpha":0.0, "fx":[81.14528,81.14528,81.14528,81.14528], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.59225, "x":1.43951, "y":1.72638, "heading":0.0, "vx":2.94261, "vy":0.0, "omega":0.0, "ax":4.96125, "ay":0.0, "alpha":0.0, "fx":[81.10691,81.10691,81.10691,81.10691], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.6416, "x":1.59079, "y":1.72638, "heading":0.0, "vx":3.18747, "vy":0.0, "omega":0.0, "ax":4.95773, "ay":0.0, "alpha":0.0, "fx":[81.04944,81.04944,81.04944,81.04944], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.69096, "x":1.75414, "y":1.72638, "heading":0.0, "vx":3.43216, "vy":0.0, "omega":0.0, "ax":4.95189, "ay":0.0, "alpha":0.0, "fx":[80.95392,80.95392,80.95392,80.95392], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.74031, "x":1.92956, "y":1.72638, "heading":0.0, "vx":3.67655, "vy":0.0, "omega":0.0, "ax":4.94027, "ay":0.0, "alpha":0.0, "fx":[80.76404,80.76404,80.76404,80.76404], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.78966, "x":2.11703, "y":1.72638, "heading":0.0, "vx":3.92037, "vy":0.0, "omega":0.0, "ax":4.90611, "ay":0.0, "alpha":0.0, "fx":[80.20548,80.20548,80.20548,80.20548], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.83902, "x":2.31649, "y":1.72638, "heading":0.0, "vx":4.16251, "vy":0.0, "omega":0.0, "ax":3.23791, "ay":0.0, "alpha":0.0, "fx":[52.9336,52.9336,52.9336,52.9336], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.88837, "x":2.52587, "y":1.72638, "heading":0.0, "vx":4.32231, "vy":0.0, "omega":0.0, "ax":0.00233, "ay":0.0, "alpha":0.0, "fx":[0.03804,0.03804,0.03804,0.03804], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.93773, "x":2.7392, "y":1.72638, "heading":0.0, "vx":4.32243, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.98708, "x":2.95253, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.03643, "x":3.16586, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.08579, "x":3.37919, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.13514, "x":3.59251, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.1845, "x":3.80584, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.23385, "x":4.01917, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.2832, "x":4.2325, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.33256, "x":4.44583, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.38191, "x":4.65916, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.43127, "x":4.87249, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.48062, "x":5.08582, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.52997, "x":5.29915, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.57933, "x":5.51248, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.62868, "x":5.72581, "y":1.72638, "heading":0.0, "vx":4.32243, "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":1.67804, "x":5.93914, "y":1.72638, "heading":0.0, "vx":4.32243, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00002,-0.00002,-0.00002,-0.00002], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.72739, "x":6.15246, "y":1.72638, "heading":0.0, "vx":4.32243, "vy":0.0, "omega":0.0, "ax":-0.00308, "ay":0.0, "alpha":0.0, "fx":[-0.05033,-0.05033,-0.05033,-0.05033], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.77674, "x":6.36579, "y":1.72638, "heading":0.0, "vx":4.32228, "vy":0.0, "omega":0.0, "ax":-3.56959, "ay":0.0, "alpha":0.0, "fx":[-58.35604,-58.35604,-58.35604,-58.35604], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.8261, "x":6.57476, "y":1.72638, "heading":0.0, "vx":4.1461, "vy":0.0, "omega":0.0, "ax":-4.9069, "ay":0.0, "alpha":0.0, "fx":[-80.2185,-80.2185,-80.2185,-80.2185], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.87545, "x":6.77341, "y":1.72638, "heading":0.0, "vx":3.90393, "vy":0.0, "omega":0.0, "ax":-4.94048, "ay":0.0, "alpha":0.0, "fx":[-80.7675,-80.7675,-80.7675,-80.7675], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.92481, "x":6.96007, "y":1.72638, "heading":0.0, "vx":3.66009, "vy":0.0, "omega":0.0, "ax":-4.95198, "ay":0.0, "alpha":0.0, "fx":[-80.95549,-80.95549,-80.95549,-80.95549], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.97416, "x":7.13468, "y":1.72638, "heading":0.0, "vx":3.41569, "vy":0.0, "omega":0.0, "ax":-4.95778, "ay":0.0, "alpha":0.0, "fx":[-81.05033,-81.05033,-81.05033,-81.05033], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.02351, "x":7.29722, "y":1.72638, "heading":0.0, "vx":3.17101, "vy":0.0, "omega":0.0, "ax":-4.96128, "ay":0.0, "alpha":0.0, "fx":[-81.10749,-81.10749,-81.10749,-81.10749], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.07287, "x":7.44768, "y":1.72638, "heading":0.0, "vx":2.92615, "vy":0.0, "omega":0.0, "ax":-4.96362, "ay":0.0, "alpha":0.0, "fx":[-81.14568,-81.14568,-81.14568,-81.14568], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.12222, "x":7.58605, "y":1.72638, "heading":0.0, "vx":2.68117, "vy":0.0, "omega":0.0, "ax":-4.96529, "ay":0.0, "alpha":0.0, "fx":[-81.17301,-81.17301,-81.17301,-81.17301], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.17158, "x":7.71233, "y":1.72638, "heading":0.0, "vx":2.43612, "vy":0.0, "omega":0.0, "ax":-4.96654, "ay":0.0, "alpha":0.0, "fx":[-81.19352,-81.19352,-81.19352,-81.19352], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.22093, "x":7.82652, "y":1.72638, "heading":0.0, "vx":2.191, "vy":0.0, "omega":0.0, "ax":-4.96752, "ay":0.0, "alpha":0.0, "fx":[-81.20949,-81.20949,-81.20949,-81.20949], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.27028, "x":7.9286, "y":1.72638, "heading":0.0, "vx":1.94583, "vy":0.0, "omega":0.0, "ax":-4.9683, "ay":0.0, "alpha":0.0, "fx":[-81.22228,-81.22228,-81.22228,-81.22228], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.31964, "x":8.01858, "y":1.72638, "heading":0.0, "vx":1.70063, "vy":0.0, "omega":0.0, "ax":-4.96894, "ay":0.0, "alpha":0.0, "fx":[-81.23274,-81.23274,-81.23274,-81.23274], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.36899, "x":8.09646, "y":1.72638, "heading":0.0, "vx":1.45539, "vy":0.0, "omega":0.0, "ax":-4.96948, "ay":0.0, "alpha":0.0, "fx":[-81.24146,-81.24146,-81.24146,-81.24146], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.41835, "x":8.16224, "y":1.72638, "heading":0.0, "vx":1.21013, "vy":0.0, "omega":0.0, "ax":-4.96993, "ay":0.0, "alpha":0.0, "fx":[-81.24884,-81.24884,-81.24884,-81.24884], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.4677, "x":8.21591, "y":1.72638, "heading":0.0, "vx":0.96484, "vy":0.0, "omega":0.0, "ax":-4.97031, "ay":0.0, "alpha":0.0, "fx":[-81.25517,-81.25517,-81.25517,-81.25517], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.51705, "x":8.25748, "y":1.72638, "heading":0.0, "vx":0.71953, "vy":0.0, "omega":0.0, "ax":-4.97065, "ay":0.0, "alpha":0.0, "fx":[-81.26066,-81.26066,-81.26066,-81.26066], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.56641, "x":8.28694, "y":1.72638, "heading":0.0, "vx":0.47421, "vy":0.0, "omega":0.0, "ax":-4.97094, "ay":0.0, "alpha":0.0, "fx":[-81.26546,-81.26546,-81.26546,-81.26546], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.61576, "x":8.30429, "y":1.72638, "heading":0.0, "vx":0.22888, "vy":0.0, "omega":0.0, "ax":-4.9712, "ay":0.0, "alpha":0.0, "fx":[-81.26969,-81.26969,-81.26969,-81.26969], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.66512, "x":8.30953, "y":1.72638, "heading":0.0, "vx":-0.01647, "vy":0.0, "omega":0.0, "ax":-4.96992, "ay":0.0, "alpha":0.0, "fx":[-81.24868,-81.24868,-81.24868,-81.24868], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.70251, "x":8.30544, "y":1.72638, "heading":0.0, "vx":-0.20232, "vy":0.0, "omega":0.0, "ax":-4.96967, "ay":0.0, "alpha":0.0, "fx":[-81.24461,-81.24461,-81.24461,-81.24461], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.73991, "x":8.2944, "y":1.72638, "heading":0.0, "vx":-0.38816, "vy":0.0, "omega":0.0, "ax":-4.9694, "ay":0.0, "alpha":0.0, "fx":[-81.24016,-81.24016,-81.24016,-81.24016], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.7773, "x":8.27641, "y":1.72638, "heading":0.0, "vx":-0.57399, "vy":0.0, "omega":0.0, "ax":-4.9691, "ay":0.0, "alpha":0.0, "fx":[-81.23526,-81.23526,-81.23526,-81.23526], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.8147, "x":8.25147, "y":1.72638, "heading":0.0, "vx":-0.75981, "vy":0.0, "omega":0.0, "ax":-4.96877, "ay":0.0, "alpha":0.0, "fx":[-81.22985,-81.22985,-81.22985,-81.22985], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.85209, "x":8.21958, "y":1.72638, "heading":0.0, "vx":-0.94561, "vy":0.0, "omega":0.0, "ax":-4.9684, "ay":0.0, "alpha":0.0, "fx":[-81.22382,-81.22382,-81.22382,-81.22382], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.88949, "x":8.18075, "y":1.72638, "heading":0.0, "vx":-1.13141, "vy":0.0, "omega":0.0, "ax":-4.96798, "ay":0.0, "alpha":0.0, "fx":[-81.21709,-81.21709,-81.21709,-81.21709], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.92688, "x":8.13496, "y":1.72638, "heading":0.0, "vx":-1.31718, "vy":0.0, "omega":0.0, "ax":-4.96752, "ay":0.0, "alpha":0.0, "fx":[-81.2095,-81.2095,-81.2095,-81.2095], "fy":[0.0,0.0,0.0,0.0]}, - {"t":2.96427, "x":8.08224, "y":1.72638, "heading":0.0, "vx":-1.50294, "vy":0.0, "omega":0.0, "ax":-4.96699, "ay":0.0, "alpha":0.0, "fx":[-81.2009,-81.2009,-81.2009,-81.2009], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.00167, "x":8.02256, "y":1.72638, "heading":0.0, "vx":-1.68868, "vy":0.0, "omega":0.0, "ax":-4.96639, "ay":0.0, "alpha":0.0, "fx":[-81.19106,-81.19106,-81.19106,-81.19106], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.03906, "x":7.95594, "y":1.72638, "heading":0.0, "vx":-1.8744, "vy":0.0, "omega":0.0, "ax":-4.9657, "ay":0.0, "alpha":0.0, "fx":[-81.17969,-81.17969,-81.17969,-81.17969], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.07646, "x":7.88238, "y":1.72638, "heading":0.0, "vx":-2.06009, "vy":0.0, "omega":0.0, "ax":-4.96489, "ay":0.0, "alpha":0.0, "fx":[-81.16641,-81.16641,-81.16641,-81.16641], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.11385, "x":7.80187, "y":1.72638, "heading":0.0, "vx":-2.24575, "vy":0.0, "omega":0.0, "ax":-4.96392, "ay":0.0, "alpha":0.0, "fx":[-81.1507,-81.1507,-81.1507,-81.1507], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.15125, "x":7.71442, "y":1.72638, "heading":0.0, "vx":-2.43138, "vy":0.0, "omega":0.0, "ax":-4.96277, "ay":0.0, "alpha":0.0, "fx":[-81.13181,-81.13181,-81.13181,-81.13181], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.18864, "x":7.62003, "y":1.72638, "heading":0.0, "vx":-2.61696, "vy":0.0, "omega":0.0, "ax":-4.96135, "ay":0.0, "alpha":0.0, "fx":[-81.10868,-81.10868,-81.10868,-81.10868], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.22604, "x":7.5187, "y":1.72638, "heading":0.0, "vx":-2.80249, "vy":0.0, "omega":0.0, "ax":-4.95958, "ay":0.0, "alpha":0.0, "fx":[-81.07969,-81.07969,-81.07969,-81.07969], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.26343, "x":7.41043, "y":1.72638, "heading":0.0, "vx":-2.98795, "vy":0.0, "omega":0.0, "ax":-4.95729, "ay":0.0, "alpha":0.0, "fx":[-81.04232,-81.04232,-81.04232,-81.04232], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.30083, "x":7.29523, "y":1.72638, "heading":0.0, "vx":-3.17333, "vy":0.0, "omega":0.0, "ax":-4.95424, "ay":0.0, "alpha":0.0, "fx":[-80.99231,-80.99231,-80.99231,-80.99231], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.33822, "x":7.1731, "y":1.72638, "heading":0.0, "vx":-3.35859, "vy":0.0, "omega":0.0, "ax":-4.94993, "ay":0.0, "alpha":0.0, "fx":[-80.92197,-80.92197,-80.92197,-80.92197], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.37562, "x":7.04405, "y":1.72638, "heading":0.0, "vx":-3.54369, "vy":0.0, "omega":0.0, "ax":-4.94344, "ay":0.0, "alpha":0.0, "fx":[-80.81576,-80.81576,-80.81576,-80.81576], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.41301, "x":6.90807, "y":1.72638, "heading":0.0, "vx":-3.72855, "vy":0.0, "omega":0.0, "ax":-4.93251, "ay":0.0, "alpha":0.0, "fx":[-80.63708,-80.63708,-80.63708,-80.63708], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.45041, "x":6.7652, "y":1.72638, "heading":0.0, "vx":-3.913, "vy":0.0, "omega":0.0, "ax":-4.91031, "ay":0.0, "alpha":0.0, "fx":[-80.2742,-80.2742,-80.2742,-80.2742], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.4878, "x":6.61544, "y":1.72638, "heading":0.0, "vx":-4.09662, "vy":0.0, "omega":0.0, "ax":-4.8417, "ay":0.0, "alpha":0.0, "fx":[-79.1526,-79.1526,-79.1526,-79.1526], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.5252, "x":6.45886, "y":1.72638, "heading":0.0, "vx":-4.27768, "vy":0.0, "omega":0.0, "ax":-1.16408, "ay":0.0, "alpha":0.0, "fx":[-19.03054,-19.03054,-19.03054,-19.03054], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.56259, "x":6.29808, "y":1.72638, "heading":0.0, "vx":-4.32121, "vy":0.0, "omega":0.0, "ax":-0.00155, "ay":0.0, "alpha":0.0, "fx":[-0.02531,-0.02531,-0.02531,-0.02531], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.59999, "x":6.13649, "y":1.72638, "heading":0.0, "vx":-4.32126, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.63738, "x":5.9749, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.67478, "x":5.8133, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.71217, "x":5.65171, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.74957, "x":5.49012, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.78696, "x":5.32853, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.82436, "x":5.16693, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.86175, "x":5.00534, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.89915, "x":4.84375, "y":1.72638, "heading":0.0, "vx":-4.32126, "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":3.93654, "x":4.68215, "y":1.72638, "heading":0.0, "vx":-4.32126, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[0.0,0.0,0.0,0.0]}, - {"t":3.97393, "x":4.52056, "y":1.72638, "heading":0.0, "vx":-4.32126, "vy":0.0, "omega":0.0, "ax":0.00268, "ay":0.0, "alpha":0.0, "fx":[0.04382,0.04382,0.04382,0.04382], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.01133, "x":4.35897, "y":1.72638, "heading":0.0, "vx":-4.32116, "vy":0.0, "omega":0.0, "ax":0.00294, "ay":0.0, "alpha":0.0, "fx":[0.048,0.048,0.048,0.048], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.04696, "x":4.20502, "y":1.72638, "heading":0.0, "vx":-4.32106, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.08258, "x":4.05107, "y":1.72638, "heading":0.0, "vx":-4.32106, "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":4.11821, "x":3.89713, "y":1.72638, "heading":0.0, "vx":-4.32106, "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":4.15384, "x":3.74318, "y":1.72638, "heading":0.0, "vx":-4.32106, "vy":0.0, "omega":0.0, "ax":0.00001, "ay":0.0, "alpha":0.0, "fx":[0.00012,0.00012,0.00012,0.00012], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.18947, "x":3.58923, "y":1.72638, "heading":0.0, "vx":-4.32106, "vy":0.0, "omega":0.0, "ax":0.00479, "ay":0.0, "alpha":0.0, "fx":[0.07837,0.07837,0.07837,0.07837], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.22509, "x":3.43529, "y":1.72638, "heading":0.0, "vx":-4.32089, "vy":0.0, "omega":0.0, "ax":2.38959, "ay":0.0, "alpha":0.0, "fx":[39.06518,39.06518,39.06518,39.06518], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.26072, "x":3.28286, "y":1.72638, "heading":0.0, "vx":-4.23575, "vy":0.0, "omega":0.0, "ax":4.83852, "ay":0.0, "alpha":0.0, "fx":[79.1006,79.1006,79.1006,79.1006], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.29635, "x":3.13502, "y":1.72638, "heading":0.0, "vx":-4.06337, "vy":0.0, "omega":0.0, "ax":4.90649, "ay":0.0, "alpha":0.0, "fx":[80.21181,80.21181,80.21181,80.21181], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.33198, "x":2.99337, "y":1.72638, "heading":0.0, "vx":-3.88857, "vy":0.0, "omega":0.0, "ax":4.92946, "ay":0.0, "alpha":0.0, "fx":[80.5873,80.5873,80.5873,80.5873], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.3676, "x":2.85796, "y":1.72638, "heading":0.0, "vx":-3.71294, "vy":0.0, "omega":0.0, "ax":4.94097, "ay":0.0, "alpha":0.0, "fx":[80.77541,80.77541,80.77541,80.77541], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.40323, "x":2.72881, "y":1.72638, "heading":0.0, "vx":-3.53691, "vy":0.0, "omega":0.0, "ax":4.94787, "ay":0.0, "alpha":0.0, "fx":[80.88825,80.88825,80.88825,80.88825], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.43886, "x":2.60594, "y":1.72638, "heading":0.0, "vx":-3.36063, "vy":0.0, "omega":0.0, "ax":4.95247, "ay":0.0, "alpha":0.0, "fx":[80.96343,80.96343,80.96343,80.96343], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.47448, "x":2.48935, "y":1.72638, "heading":0.0, "vx":-3.18419, "vy":0.0, "omega":0.0, "ax":4.95575, "ay":0.0, "alpha":0.0, "fx":[81.0171,81.0171,81.0171,81.0171], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.51011, "x":2.37906, "y":1.72638, "heading":0.0, "vx":-3.00763, "vy":0.0, "omega":0.0, "ax":4.95821, "ay":0.0, "alpha":0.0, "fx":[81.05731,81.05731,81.05731,81.05731], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.54574, "x":2.27505, "y":1.72638, "heading":0.0, "vx":-2.83098, "vy":0.0, "omega":0.0, "ax":4.96012, "ay":0.0, "alpha":0.0, "fx":[81.08857,81.08857,81.08857,81.08857], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.58137, "x":2.17734, "y":1.72638, "heading":0.0, "vx":-2.65426, "vy":0.0, "omega":0.0, "ax":4.96165, "ay":0.0, "alpha":0.0, "fx":[81.11357,81.11357,81.11357,81.11357], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.61699, "x":2.08592, "y":1.72638, "heading":0.0, "vx":-2.47749, "vy":0.0, "omega":0.0, "ax":4.9629, "ay":0.0, "alpha":0.0, "fx":[81.134,81.134,81.134,81.134], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.65262, "x":2.0008, "y":1.72638, "heading":0.0, "vx":-2.30068, "vy":0.0, "omega":0.0, "ax":4.96394, "ay":0.0, "alpha":0.0, "fx":[81.15103,81.15103,81.15103,81.15103], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.68825, "x":1.92199, "y":1.72638, "heading":0.0, "vx":-2.12382, "vy":0.0, "omega":0.0, "ax":4.96482, "ay":0.0, "alpha":0.0, "fx":[81.16542,81.16542,81.16542,81.16542], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.72388, "x":1.84947, "y":1.72638, "heading":0.0, "vx":-1.94694, "vy":0.0, "omega":0.0, "ax":4.96558, "ay":0.0, "alpha":0.0, "fx":[81.17776,81.17776,81.17776,81.17776], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.7595, "x":1.78326, "y":1.72638, "heading":0.0, "vx":-1.77003, "vy":0.0, "omega":0.0, "ax":4.96623, "ay":0.0, "alpha":0.0, "fx":[81.18844,81.18844,81.18844,81.18844], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.79513, "x":1.72335, "y":1.72638, "heading":0.0, "vx":-1.5931, "vy":0.0, "omega":0.0, "ax":4.9668, "ay":0.0, "alpha":0.0, "fx":[81.19779,81.19779,81.19779,81.19779], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.83076, "x":1.66974, "y":1.72638, "heading":0.0, "vx":-1.41614, "vy":0.0, "omega":0.0, "ax":4.96731, "ay":0.0, "alpha":0.0, "fx":[81.20604,81.20604,81.20604,81.20604], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.86639, "x":1.62244, "y":1.72638, "heading":0.0, "vx":-1.23917, "vy":0.0, "omega":0.0, "ax":4.96776, "ay":0.0, "alpha":0.0, "fx":[81.21336,81.21336,81.21336,81.21336], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.90201, "x":1.58145, "y":1.72638, "heading":0.0, "vx":-1.06218, "vy":0.0, "omega":0.0, "ax":4.96816, "ay":0.0, "alpha":0.0, "fx":[81.21992,81.21992,81.21992,81.21992], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.93764, "x":1.54676, "y":1.72638, "heading":0.0, "vx":-0.88518, "vy":0.0, "omega":0.0, "ax":4.96852, "ay":0.0, "alpha":0.0, "fx":[81.22582,81.22582,81.22582,81.22582], "fy":[0.0,0.0,0.0,0.0]}, - {"t":4.97327, "x":1.51838, "y":1.72638, "heading":0.0, "vx":-0.70817, "vy":0.0, "omega":0.0, "ax":4.96884, "ay":0.0, "alpha":0.0, "fx":[81.23115,81.23115,81.23115,81.23115], "fy":[0.0,0.0,0.0,0.0]}, - {"t":5.00889, "x":1.4963, "y":1.72638, "heading":0.0, "vx":-0.53114, "vy":0.0, "omega":0.0, "ax":4.96914, "ay":0.0, "alpha":0.0, "fx":[81.236,81.236,81.236,81.236], "fy":[0.0,0.0,0.0,0.0]}, - {"t":5.04452, "x":1.48053, "y":1.72638, "heading":0.0, "vx":-0.3541, "vy":0.0, "omega":0.0, "ax":4.96941, "ay":0.0, "alpha":0.0, "fx":[81.24043,81.24043,81.24043,81.24043], "fy":[0.0,0.0,0.0,0.0]}, - {"t":5.08015, "x":1.47107, "y":1.72638, "heading":0.0, "vx":-0.17706, "vy":0.0, "omega":0.0, "ax":4.96966, "ay":0.0, "alpha":0.0, "fx":[81.24448,81.24448,81.24448,81.24448], "fy":[0.0,0.0,0.0,0.0]}, - {"t":5.11578, "x":1.46791, "y":1.72638, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} diff --git a/src/main/deploy/choreo/Triangle Test.traj b/src/main/deploy/choreo/Triangle Test.traj deleted file mode 100644 index b876f80b..00000000 --- a/src/main/deploy/choreo/Triangle Test.traj +++ /dev/null @@ -1,128 +0,0 @@ -{ - "name":"Triangle Test", - "version":1, - "snapshot":{ - "waypoints":[ - {"x":3.0798263549804688, "y":2.2565953731536865, "heading":0.0, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.089874744415283, "y":5.504938125610352, "heading":1.5707963267948966, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":0.9011527895927428, "y":3.063164234161377, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"3.0798263549804688 m", "val":3.0798263549804688}, "y":{"exp":"2.2565953731536865 m", "val":2.2565953731536865}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.089874744415283 m", "val":2.089874744415283}, "y":{"exp":"5.504938125610352 m", "val":5.504938125610352}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"0.9011527895927429 m", "val":0.9011527895927428}, "y":{"exp":"3.063164234161377 m", "val":3.063164234161377}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":"Swerve", - "waypoints":[0.0,1.62111,3.06323], - "samples":[ - {"t":0.0, "x":3.07983, "y":2.2566, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.11463, "ay":4.86819, "alpha":2.96068, "fx":[-14.43306,-21.07748,16.67613,11.33824], "fy":[79.96303,78.43454,79.4851,80.45968]}, - {"t":0.03377, "x":3.07976, "y":2.25937, "heading":0.0, "vx":-0.00387, "vy":0.16441, "omega":0.09999, "ax":-0.12183, "ay":4.86925, "alpha":2.93746, "fx":[-14.45635,-21.0532,16.37941,11.16358], "fy":[79.95492,78.4356,79.54123,80.48017]}, - {"t":0.06755, "x":3.07956, "y":2.2677, "heading":0.00338, "vx":-0.00799, "vy":0.32886, "omega":0.1992, "ax":-0.12979, "ay":4.87038, "alpha":2.91226, "fx":[-14.5319,-20.98303,16.10467,10.92267], "fy":[79.93689,78.44832,79.59139,80.50893]}, - {"t":0.10132, "x":3.07922, "y":2.28159, "heading":0.0101, "vx":-0.01237, "vy":0.49335, "omega":0.29756, "ax":-0.13863, "ay":4.87157, "alpha":2.88471, "fx":[-14.65835,-20.8673,15.84601,10.61419], "fy":[79.90898,78.47239,79.63691,80.5455]}, - {"t":0.13509, "x":3.07872, "y":2.30103, "heading":0.02015, "vx":-0.01705, "vy":0.65788, "omega":0.39498, "ax":-0.14845, "ay":4.87285, "alpha":2.85434, "fx":[-14.83396,-20.70561,15.59559,10.23628], "fy":[79.87118,78.5076,79.67943,80.58925]}, - {"t":0.16887, "x":3.07806, "y":2.32602, "heading":0.03349, "vx":-0.02207, "vy":0.82245, "omega":0.49138, "ax":-0.1594, "ay":4.87423, "alpha":2.82054, "fx":[-15.0565,-20.49705,15.34348,9.78646], "fy":[79.82348,78.55378,79.72085,80.63942]}, - {"t":0.20264, "x":3.07722, "y":2.35658, "heading":0.05009, "vx":-0.02745, "vy":0.98707, "omega":0.58664, "ax":-0.17165, "ay":4.87572, "alpha":2.78258, "fx":[-15.32326,-20.24041,15.07741,9.26153], "fy":[79.76587,78.6107,79.76336,80.69504]}, - {"t":0.23641, "x":3.0762, "y":2.3927, "heading":0.0699, "vx":-0.03325, "vy":1.15174, "omega":0.68062, "ax":-0.18543, "ay":4.87734, "alpha":2.73959, "fx":[-15.63091,-19.93449,14.78247,8.65745], "fy":[79.69838,78.678,79.80944,80.75495]}, - {"t":0.27019, "x":3.07497, "y":2.43438, "heading":0.09289, "vx":-0.03951, "vy":1.31646, "omega":0.77314, "ax":-0.20101, "ay":4.8791, "alpha":2.69053, "fx":[-15.97544,-19.57855,14.4406,7.96911], "fy":[79.62111,78.75504,79.86185,80.81768]}, - {"t":0.30396, "x":3.07352, "y":2.48162, "heading":0.119, "vx":-0.0463, "vy":1.48125, "omega":0.86401, "ax":-0.21876, "ay":4.881, "alpha":2.63417, "fx":[-16.352,-19.17283,14.02986,7.19], "fy":[79.53432,78.84079,79.92359,80.88145]}, - {"t":0.33773, "x":3.07183, "y":2.53443, "heading":0.14818, "vx":-0.05369, "vy":1.64609, "omega":0.95297, "ax":-0.23916, "ay":4.88304, "alpha":2.569, "fx":[-16.75473,-18.7194,13.52335,6.3117], "fy":[79.43842,78.93363,79.99782,80.94398]}, - {"t":0.37151, "x":3.06988, "y":2.59281, "heading":0.18037, "vx":-0.06176, "vy":1.81101, "omega":1.03974, "ax":-0.26286, "ay":4.88521, "alpha":2.49318, "fx":[-17.17646,-18.22321,12.88752,5.32311], "fy":[79.33409,79.03115,80.08769,81.00239]}, - {"t":0.40528, "x":3.06765, "y":2.65676, "heading":0.21548, "vx":-0.07064, "vy":1.976, "omega":1.12394, "ax":-0.29076, "ay":4.88744, "alpha":2.40434, "fx":[-17.60834,-17.69371,12.07939,4.20909], "fy":[79.22236,79.12986,80.19598,81.05287]}, - {"t":0.43905, "x":3.0651, "y":2.72628, "heading":0.25344, "vx":-0.08046, "vy":2.14106, "omega":1.20514, "ax":-0.32413, "ay":4.88963, "alpha":2.29927, "fx":[-18.03924,-17.14699,11.04222,2.94832], "fy":[79.10471,79.22487,80.32446,81.09025]}, - {"t":0.47282, "x":3.06219, "y":2.80138, "heading":0.29414, "vx":-0.09141, "vy":2.3062, "omega":1.2828, "ax":-0.36482, "ay":4.89158, "alpha":2.17342, "fx":[-18.45479,-16.60915,9.69803,1.5094], "fy":[78.98327,79.30937,80.47247,81.10713]}, - {"t":0.5066, "x":3.0589, "y":2.88206, "heading":0.33746, "vx":-0.10373, "vy":2.4714, "omega":1.3562, "ax":-0.41562, "ay":4.89294, "alpha":2.01986, "fx":[-18.83571,-16.12124,7.9347,-0.15604], "fy":[78.86099,79.37382,80.634,81.09215]}, - {"t":0.54037, "x":3.05516, "y":2.96832, "heading":0.38327, "vx":-0.11776, "vy":2.63665, "omega":1.42442, "ax":-0.48089, "ay":4.89298, "alpha":1.82749, "fx":[-19.15484,-15.74726,5.5823,-2.12701], "fy":[78.74205,79.40448,80.79086,81.02634]}, - {"t":0.57414, "x":3.05091, "y":3.06016, "heading":0.43138, "vx":-0.13401, "vy":2.80191, "omega":1.48614, "ax":-0.56785, "ay":4.89022, "alpha":1.57734, "fx":[-19.37124,-15.58779,2.36769,-4.54173], "fy":[78.63239,79.38017,80.89603,80.87447]}, - {"t":0.60792, "x":3.04606, "y":3.15757, "heading":0.48157, "vx":-0.15318, "vy":2.96706, "omega":1.53941, "ax":-0.68914, "ay":4.88124, "alpha":1.23483, "fx":[-19.41738,-15.80479,-2.17796,-7.66459], "fy":[78.54069,79.26503,80.82862,80.56169]}, - {"t":0.64169, "x":3.04049, "y":3.26057, "heading":0.53356, "vx":-0.17646, "vy":3.13192, "omega":1.58111, "ax":-0.86895, "ay":4.85725, "alpha":0.73139, "fx":[-19.1682,-16.67091,-8.91749,-12.06629], "fy":[78.47998,78.99001,80.25888,79.89801]}, - {"t":0.67546, "x":3.03404, "y":3.36911, "heading":0.58696, "vx":-0.20581, "vy":3.29596, "omega":1.60581, "ax":-1.15871, "ay":4.79139, "alpha":-0.08528, "fx":[-18.35213,-18.67762,-19.52887,-19.21241], "fy":[78.46927,78.40093,78.19023,78.25997]}, - {"t":0.70924, "x":3.02642, "y":3.48316, "heading":0.64119, "vx":-0.24494, "vy":3.45778, "omega":1.60293, "ax":-1.68281, "ay":4.57889, "alpha":-1.63056, "fx":[-16.23361,-22.80331,-37.0706,-33.93562], "fy":[78.51912,77.08762,71.27679,72.54122]}, - {"t":0.74301, "x":3.01719, "y":3.60255, "heading":0.69533, "vx":-0.30177, "vy":3.61243, "omega":1.54786, "ax":-2.67794, "ay":3.6221, "alpha":-5.7837, "fx":[-10.58881,-31.3845,-62.73003,-70.4138], "fy":[78.24965,73.63206,49.66913,35.30662]}, - {"t":0.77678, "x":3.00547, "y":3.72662, "heading":0.7476, "vx":-0.39222, "vy":3.73476, "omega":1.35253, "ax":-4.15538, "ay":1.71688, "alpha":-5.20773, "fx":[-59.81301,-57.51679,-76.6625,-77.73812], "fy":[47.66233,54.41356,21.02853,-10.83322]}, - {"t":0.81056, "x":2.98986, "y":3.85373, "heading":0.79328, "vx":-0.53256, "vy":3.79274, "omega":1.17665, "ax":-4.64718, "ay":-1.21456, "alpha":1.08037, "fx":[-76.86467,-73.95511,-75.30165,-77.76859], "fy":[-18.1269,-27.24227,-22.03806,-12.01545]}, - {"t":0.84433, "x":2.96922, "y":3.98113, "heading":0.83302, "vx":-0.68951, "vy":3.75172, "omega":1.21314, "ax":-3.47988, "ay":-2.98984, "alpha":5.07478, "fx":[-69.90119,-45.02702,-36.38194,-76.24739], "fy":[-38.833,-66.00659,-69.81085,-20.86256]}, - {"t":0.8781, "x":2.94395, "y":4.10614, "heading":0.87399, "vx":-0.80703, "vy":3.65075, "omega":1.38453, "ax":-2.50265, "ay":-4.08336, "alpha":3.18847, "fx":[-54.43045,-31.92846,-21.8051,-55.4902], "fy":[-59.05114,-73.73512,-76.89249,-57.34165]}, - {"t":0.91188, "x":2.91526, "y":4.2271, "heading":0.92075, "vx":-0.89156, "vy":3.51284, "omega":1.49221, "ax":-1.75967, "ay":-4.58976, "alpha":0.89909, "fx":[-34.13628,-25.91375,-22.8969,-32.12194], "fy":[-72.94979,-76.28391,-77.15028,-73.75153]}, - {"t":0.94565, "x":2.88415, "y":4.34313, "heading":0.97115, "vx":-0.95099, "vy":3.35783, "omega":1.52258, "ax":-1.3306, "ay":-4.75414, "alpha":-0.24587, "fx":[-20.1069,-22.58378,-23.36226,-20.9583], "fy":[-78.16837,-77.4819,-77.27085,-77.96371]}, - {"t":0.97942, "x":2.85127, "y":4.45382, "heading":1.02257, "vx":-0.99592, "vy":3.19727, "omega":1.51427, "ax":-1.06607, "ay":-4.81907, "alpha":-0.93084, "fx":[-10.92127,-20.79967,-23.41962,-14.57237], "fy":[-80.08817,-78.08289,-77.40042,-79.55913]}, - {"t":1.0132, "x":2.81703, "y":4.55905, "heading":1.07371, "vx":-1.03193, "vy":3.03451, "omega":1.48284, "ax":-0.88978, "ay":-4.84796, "alpha":-1.38611, "fx":[-4.69988,-19.94799,-23.2434,-10.29325], "fy":[-80.77347,-78.38129,-77.54751,-80.31715]}, - {"t":1.04697, "x":2.78167, "y":4.65877, "heading":1.12379, "vx":-1.06198, "vy":2.87078, "omega":1.43602, "ax":-0.7648, "ay":-4.86154, "alpha":-1.71253, "fx":[-0.30353,-19.67029,-22.92409,-7.1144], "fy":[-80.96771,-78.5105,-77.70755,-80.72193]}, - {"t":1.08074, "x":2.74537, "y":4.75296, "heading":1.17229, "vx":-1.08781, "vy":2.70659, "omega":1.37818, "ax":-0.67186, "ay":-4.86786, "alpha":-1.96012, "fx":[2.91418,-19.7455,-22.51378,-4.58971], "fy":[-80.95944,-78.53865,-77.87495,-80.94805]}, - {"t":1.11452, "x":2.70825, "y":4.84159, "heading":1.21884, "vx":-1.1105, "vy":2.54219, "omega":1.31198, "ax":-0.60008, "ay":-4.87046, "alpha":-2.15584, "fx":[5.33197,-20.03188,-22.0459,-2.49486], "fy":[-80.87005,-78.50443,-78.04481,-81.07194]}, - {"t":1.14829, "x":2.6704, "y":4.92467, "heading":1.26315, "vx":-1.13077, "vy":2.3777, "omega":1.23918, "ax":-0.54292, "ay":-4.87107, "alpha":-2.31515, "fx":[7.18342,-20.43641,-21.54393,-0.70586], "fy":[-80.75365,-78.43193,-78.21316,-81.13221]}, - {"t":1.18206, "x":2.6319, "y":5.00219, "heading":1.305, "vx":-1.1491, "vy":2.21318, "omega":1.16099, "ax":-0.49625, "ay":-4.87059, "alpha":-2.44748, "fx":[8.62051,-20.89737,-21.02543,0.85109], "fy":[-80.63497,-78.3372,-78.37683,-81.15071]}, - {"t":1.21583, "x":2.59281, "y":5.07416, "heading":1.34421, "vx":-1.16586, "vy":2.04869, "omega":1.07833, "ax":-0.45737, "ay":-4.86954, "alpha":-2.5589, "fx":[9.74726,-21.37401,-20.50419,2.22257], "fy":[-80.52492,-78.23137,-78.53338,-81.14095]}, - {"t":1.24961, "x":2.55317, "y":5.14058, "heading":1.38063, "vx":-1.18131, "vy":1.88423, "omega":0.9919, "ax":-0.42441, "ay":-4.8682, "alpha":-2.65361, "fx":[10.63823,-21.83982,-19.99136,3.43952], "fy":[-80.42767,-78.1223,-78.68092,-81.11198]}, - {"t":1.28338, "x":2.51303, "y":5.20144, "heading":1.41413, "vx":-1.19564, "vy":1.71981, "omega":0.90228, "ax":-0.3961, "ay":-4.86674, "alpha":-2.73463, "fx":[11.34914,-22.27815,-19.49619,4.52341], "fy":[-80.34389,-78.0155,-78.81809,-81.07017]}, - {"t":1.31715, "x":2.47243, "y":5.25674, "heading":1.4446, "vx":-1.20902, "vy":1.55545, "omega":0.80993, "ax":-0.37149, "ay":-4.86527, "alpha":-2.80433, "fx":[11.92311,-22.67914,-19.0264,5.4897], "fy":[-80.27247,-77.91478,-78.9439,-81.02026]}, - {"t":1.35093, "x":2.43138, "y":5.3065, "heading":1.47195, "vx":-1.22157, "vy":1.39113, "omega":0.71521, "ax":-0.34991, "ay":-4.86383, "alpha":-2.86463, "fx":[12.39452,-23.03753,-18.58851,6.34993], "fy":[-80.21141,-77.82264,-79.05771,-80.96588]}, - {"t":1.3847, "x":2.38993, "y":5.35071, "heading":1.49611, "vx":-1.23339, "vy":1.22687, "omega":0.61847, "ax":-0.33085, "ay":-4.86246, "alpha":-2.91713, "fx":[12.79138,-23.35111,-18.18802,7.11293], "fy":[-80.15835,-77.74066,-79.15911,-80.90988]}, - {"t":1.41847, "x":2.34808, "y":5.38937, "heading":1.517, "vx":-1.24456, "vy":1.06264, "omega":0.51995, "ax":-0.3139, "ay":-4.86116, "alpha":-2.96319, "fx":[13.13684,-23.61955,-17.82959,7.78561], "fy":[-80.11083,-77.66972,-79.24788,-80.85454]}, - {"t":1.45225, "x":2.30587, "y":5.42249, "heading":1.53456, "vx":-1.25516, "vy":0.89847, "omega":0.41987, "ax":-0.29877, "ay":-4.85993, "alpha":-3.00398, "fx":[13.4502,-23.84361,-17.51716,8.37344], "fy":[-80.06642,-77.61029,-79.32395,-80.80169]}, - {"t":1.48602, "x":2.26331, "y":5.45006, "heading":1.54874, "vx":-1.26525, "vy":0.73433, "omega":0.31842, "ax":-0.2852, "ay":-4.85875, "alpha":-3.04051, "fx":[13.74754,-24.02452,-17.25401,8.88078], "fy":[-80.02287,-77.56255,-79.38729,-80.75283]}, - {"t":1.51979, "x":2.22042, "y":5.47209, "heading":1.55949, "vx":-1.27488, "vy":0.57024, "omega":0.21573, "ax":-0.27301, "ay":-4.85763, "alpha":-3.07359, "fx":[14.04221,-24.16348,-17.04289,9.31108], "fy":[-79.97813,-77.52658,-79.43797,-80.70918]}, - {"t":1.55357, "x":2.1772, "y":5.48858, "heading":1.56678, "vx":-1.2841, "vy":0.40618, "omega":0.11192, "ax":-0.26204, "ay":-4.85654, "alpha":-3.10393, "fx":[14.34506,-24.26139,-16.88602,9.66698], "fy":[-79.93034,-77.50243,-79.47602,-80.67175]}, - {"t":1.58734, "x":2.13369, "y":5.49953, "heading":1.57056, "vx":-1.29295, "vy":0.24216, "omega":0.00709, "ax":-0.25215, "ay":-4.85547, "alpha":-3.13206, "fx":[14.66468,-24.31856,-16.78519,9.95047], "fy":[-79.87791,-77.49028,-79.50148,-80.64133]}, - {"t":1.62111, "x":2.08987, "y":5.50494, "heading":1.5708, "vx":-1.30147, "vy":0.07817, "omega":-0.09869, "ax":-0.24282, "ay":-4.86536, "alpha":-2.98289, "fx":[13.87574,-23.13458,-16.19853,9.57871], "fy":[-80.013,-77.84588,-79.61767,-80.68098]}, - {"t":1.65316, "x":2.04804, "y":5.50494, "heading":1.56763, "vx":-1.30925, "vy":-0.07775, "omega":-0.19428, "ax":-0.23081, "ay":-4.86574, "alpha":-2.98127, "fx":[14.14443,-22.85739,-16.07373,9.69342], "fy":[-79.95982,-77.92118,-79.63853,-80.663]}, - {"t":1.68521, "x":2.00597, "y":5.49995, "heading":1.56141, "vx":-1.31665, "vy":-0.23368, "omega":-0.28982, "ax":-0.21769, "ay":-4.86612, "alpha":-2.97969, "fx":[14.47845,-22.51879,-15.97585,9.78089], "fy":[-79.8934,-78.01238,-79.65332,-80.64777]}, - {"t":1.71725, "x":1.96366, "y":5.48997, "heading":1.55212, "vx":-1.32362, "vy":-0.38962, "omega":-0.38531, "ax":-0.20327, "ay":-4.86648, "alpha":-2.97789, "fx":[14.87829,-22.11211,-15.90138,9.8431], "fy":[-79.81273,-78.1204,-79.66274,-80.63508]}, - {"t":1.7493, "x":1.92114, "y":5.47498, "heading":1.53977, "vx":-1.33014, "vy":-0.54558, "omega":-0.48074, "ax":-0.1873, "ay":-4.86686, "alpha":-2.97555, "fx":[15.34408,-21.62869,-15.84588,9.88236], "fy":[-79.71666,-78.24634,-79.66766,-80.62464]}, - {"t":1.78135, "x":1.87841, "y":5.455, "heading":1.52437, "vx":-1.33614, "vy":-0.70155, "omega":-0.5761, "ax":-0.1695, "ay":-4.86724, "alpha":-2.97223, "fx":[15.87573,-21.05756,-15.80375,9.90149], "fy":[-79.60387,-78.39146,-79.6691,-80.61602]}, - {"t":1.81339, "x":1.83551, "y":5.43002, "heading":1.5059, "vx":-1.34157, "vy":-0.85753, "omega":-0.67135, "ax":-0.14949, "ay":-4.86765, "alpha":-2.96742, "fx":[16.47319,-20.38496,-15.76787,9.90411], "fy":[-79.47285,-78.55708,-79.66834,-80.60868]}, - {"t":1.84544, "x":1.79244, "y":5.40004, "heading":1.48439, "vx":-1.34636, "vy":-1.01352, "omega":-0.76645, "ax":-0.12679, "ay":-4.86808, "alpha":-2.96046, "fx":[17.13672,-19.59376,-15.72917,9.89494], "fy":[-79.32179,-78.74445,-79.66698,-80.6019]}, - {"t":1.87749, "x":1.74923, "y":5.36506, "heading":1.45983, "vx":-1.35043, "vy":-1.16953, "omega":-0.86132, "ax":-0.10079, "ay":-4.86853, "alpha":-2.95062, "fx":[17.86737,-18.66255,-15.67592,9.88039], "fy":[-79.14844,-78.95452,-79.66706,-80.59467]}, - {"t":1.90954, "x":1.7059, "y":5.32508, "heading":1.43222, "vx":-1.35366, "vy":-1.32555, "omega":-0.95588, "ax":-0.07066, "ay":-4.86898, "alpha":-2.93704, "fx":[18.66766,-17.56449,-15.59281,9.86922], "fy":[-78.9499,-79.18766,-79.67123,-80.58566]}, - {"t":1.94158, "x":1.66248, "y":5.2801, "heading":1.40159, "vx":-1.35592, "vy":-1.48158, "omega":-1.05, "ax":-0.03531, "ay":-4.86939, "alpha":-2.91877, "fx":[19.54257,-16.26552,-15.45949,9.87363], "fy":[-78.72223,-79.4431,-79.68287,-80.57303]}, - {"t":1.97363, "x":1.61901, "y":5.23012, "heading":1.36794, "vx":-1.35705, "vy":-1.63763, "omega":-1.14354, "ax":0.00676, "ay":-4.86966, "alpha":-2.89478, "fx":[20.50107,-14.7217,-15.24829,9.91095], "fy":[-78.45979,-79.71809,-79.70642,-80.55416]}, - {"t":2.00568, "x":1.57552, "y":5.17513, "heading":1.33129, "vx":-1.35684, "vy":-1.79369, "omega":-1.23631, "ax":0.05763, "ay":-4.86958, "alpha":-2.86397, "fx":[21.55847,-12.8751,-14.92069,10.00619], "fy":[-78.15416,-80.00642,-79.74758,-80.52522]}, - {"t":2.03772, "x":1.53207, "y":5.11515, "heading":1.29167, "vx":-1.35499, "vy":-1.94975, "omega":-1.32809, "ax":0.12032, "ay":-4.8688, "alpha":-2.82522, "fx":[22.74024,-10.6471,-14.42121,10.19636], "fy":[-77.79228,-80.29589,-79.81365,-80.48034]}, - {"t":2.06977, "x":1.48871, "y":5.05017, "heading":1.24911, "vx":-1.35113, "vy":-2.10578, "omega":-1.41863, "ax":0.19929, "ay":-4.86662, "alpha":-2.77732, "fx":[24.08835,-7.9271,-13.66677,10.53789], "fy":[-77.35297,-80.56345,-79.91367,-80.40998]}, - {"t":2.10182, "x":1.44551, "y":4.98019, "heading":1.20365, "vx":-1.34475, "vy":-2.26174, "omega":-1.50763, "ax":0.30147, "ay":-4.86174, "alpha":-2.71895, "fx":[25.6725,-4.55239,-12.52659,11.12026], "fy":[-76.80036,-80.76535,-80.05782,-80.29749]}, - {"t":2.13386, "x":1.40257, "y":4.90521, "heading":1.15534, "vx":-1.33508, "vy":-2.41754, "omega":-1.59477, "ax":0.43815, "ay":-4.8515, "alpha":-2.64836, "fx":[27.61119,-0.27079,-10.7817,12.09308], "fy":[-76.07022,-80.81571,-80.25405,-80.11116]}, - {"t":2.16591, "x":1.36001, "y":4.82524, "heading":1.10423, "vx":-1.32104, "vy":-2.57302, "omega":-1.67964, "ax":0.62906, "ay":-4.83009, "alpha":-2.56259, "fx":[30.11443,5.33251,-8.03522,13.72416], "fy":[-75.03878,-80.53455,-80.49413,-79.78373]}, - {"t":2.19796, "x":1.318, "y":4.7403, "heading":1.0504, "vx":-1.30088, "vy":-2.72781, "omega":-1.76176, "ax":0.91157, "ay":-4.7834, "alpha":-2.45464, "fx":[33.57801,12.98344,-3.49129,16.53956], "fy":[-73.44248,-79.5082,-80.69593,-79.15123]}, - {"t":2.23001, "x":1.27678, "y":4.65043, "heading":0.99394, "vx":-1.27167, "vy":-2.8811, "omega":-1.84043, "ax":1.36401, "ay":-4.67124, "alpha":-2.30231, "fx":[38.81297,24.00982,4.66404,21.70926], "fy":[-70.63428,-76.66621,-80.43243,-77.73064]}, - {"t":2.26205, "x":1.23672, "y":4.5557, "heading":0.93496, "vx":-1.22796, "vy":-3.0308, "omega":-1.91421, "ax":2.16723, "ay":-4.35153, "alpha":-2.00943, "fx":[47.63819,40.67197,21.13352,32.27637], "fy":[-64.66708,-68.88273,-77.35553,-73.65139]}, - {"t":2.2941, "x":1.19848, "y":4.45634, "heading":0.87362, "vx":-1.15851, "vy":-3.17025, "omega":-1.9786, "ax":3.66167, "ay":-3.19288, "alpha":-1.13067, "fx":[63.78516,64.00296,55.308,56.34931], "fy":[-47.93389,-47.20595,-57.15612,-56.49425]}, - {"t":2.32615, "x":1.16324, "y":4.3531, "heading":0.81021, "vx":-1.04116, "vy":-3.27257, "omega":-2.01484, "ax":4.83454, "ay":0.28582, "alpha":0.88147, "fx":[78.97724,79.2959,79.35253,78.51618], "fy":[4.992,-2.04242,4.40658,11.33436]}, - {"t":2.35819, "x":1.13236, "y":4.24837, "heading":0.74564, "vx":-0.88623, "vy":-3.26341, "omega":-1.98659, "ax":3.68443, "ay":3.1155, "alpha":2.39147, "fx":[50.93747,69.40339,67.27911,53.31358], "fy":[61.26041,39.18453,43.49924,59.78589]}, - {"t":2.39024, "x":1.10585, "y":4.14539, "heading":0.68198, "vx":-0.76815, "vy":-3.16357, "omega":-1.90995, "ax":2.66652, "ay":4.03049, "alpha":2.7515, "fx":[26.15134,53.80269,56.83093,37.58506], "fy":[75.91766,59.37489,57.01291,71.25788]}, - {"t":2.42229, "x":1.0826, "y":4.04608, "heading":0.62077, "vx":-0.6827, "vy":-3.03441, "omega":-1.82177, "ax":2.09639, "ay":4.36836, "alpha":2.77316, "fx":[14.16479,41.85567,50.75194,30.31548], "fy":[79.36326,68.6609,62.75222,74.88086]}, - {"t":2.45433, "x":1.0618, "y":3.95108, "heading":0.56238, "vx":-0.61552, "vy":-2.89441, "omega":-1.7329, "ax":1.75227, "ay":4.52736, "alpha":2.75439, "fx":[7.81991,33.19855,46.98847,26.57837], "fy":[80.41711,73.44693,65.77098,76.42013]}, - {"t":2.48638, "x":1.04297, "y":3.86064, "heading":0.50685, "vx":-0.55936, "vy":-2.74933, "omega":-1.64463, "ax":1.52551, "ay":4.61514, "alpha":2.74299, "fx":[4.10176,26.69064,44.45513,24.5089], "fy":[80.80359,76.19139,67.60767,77.19216]}, - {"t":2.51843, "x":1.02583, "y":3.77491, "heading":0.45415, "vx":-0.51047, "vy":-2.60143, "omega":-1.55673, "ax":1.3658, "ay":4.66893, "alpha":2.74335, "fx":[1.77403,21.59095,42.62191,23.32593], "fy":[80.96319,77.88756,68.84657,77.61483]}, - {"t":2.55047, "x":1.01017, "y":3.69394, "heading":0.40426, "vx":-0.4667, "vy":-2.4518, "omega":-1.46881, "ax":1.24767, "ay":4.70435, "alpha":2.75293, "fx":[0.25891,17.46261,41.21226,22.65434], "fy":[81.03589,78.98784,69.74981,77.85484]}, - {"t":2.58252, "x":0.99586, "y":3.61778, "heading":0.35719, "vx":-0.42672, "vy":-2.30104, "omega":-1.38059, "ax":1.15702, "ay":4.72892, "alpha":2.76854, "fx":[-0.74661,14.04174,40.07182,22.29358], "fy":[81.07301,79.72263,70.44968,77.99022]}, - {"t":2.61457, "x":0.98277, "y":3.54647, "heading":0.31294, "vx":-0.38964, "vy":-2.14949, "omega":-1.29187, "ax":1.08547, "ay":4.74668, "alpha":2.7875, "fx":[-1.41666,11.16093,39.10976,22.1273], "fy":[81.09514,80.2207,71.01871,78.0621]}, - {"t":2.64662, "x":0.97084, "y":3.48002, "heading":0.27154, "vx":-0.35485, "vy":-1.99738, "omega":-1.20254, "ax":1.0277, "ay":4.75994, "alpha":2.80773, "fx":[-1.85874,8.70828,38.27076,22.08369], "fy":[81.11091,80.55955,71.49905,78.09418]}, - {"t":2.67866, "x":0.96, "y":3.41845, "heading":0.233, "vx":-0.32192, "vy":-1.84483, "omega":-1.11256, "ax":0.98021, "ay":4.77013, "alpha":2.82772, "fx":[-2.14317,6.60508,37.52037,22.11619], "fy":[81.12394,80.78866,71.91618,78.1012]}, - {"t":2.71071, "x":0.95019, "y":3.36178, "heading":0.19735, "vx":-0.29051, "vy":-1.69197, "omega":-1.02194, "ax":0.94057, "ay":4.77816, "alpha":2.84647, "fx":[-2.31813,4.79346,36.83709,22.19351], "fy":[81.1357,80.94106,72.28589,78.09284]}, - {"t":2.74276, "x":0.94136, "y":3.31001, "heading":0.1646, "vx":-0.26036, "vy":-1.53884, "omega":-0.93072, "ax":0.90703, "ay":4.78466, "alpha":2.86334, "fx":[-2.41793,3.22917,36.20768,22.29395], "fy":[81.14669,81.03951,72.61809,78.07577]}, - {"t":2.7748, "x":0.93348, "y":3.26315, "heading":0.13477, "vx":-0.2313, "vy":-1.38551, "omega":-0.83895, "ax":0.87833, "ay":4.79002, "alpha":2.878, "fx":[-2.46773,1.87731,35.62424,22.40214], "fy":[81.157,81.10005,72.91904,78.05472]}, - {"t":2.80685, "x":0.92652, "y":3.22121, "heading":0.10789, "vx":-0.20315, "vy":-1.232, "omega":-0.74672, "ax":0.85351, "ay":4.79454, "alpha":2.89034, "fx":[-2.48644,0.70974,35.08242,22.50701], "fy":[81.16658,81.1342,73.19266,78.03319]}, - {"t":2.8389, "x":0.92045, "y":3.18419, "heading":0.08396, "vx":-0.1758, "vy":-1.07835, "omega":-0.6541, "ax":0.83183, "ay":4.79843, "alpha":2.90041, "fx":[-2.48858,-0.29666,34.58018,22.60042], "fy":[81.17531,81.15029,73.44144,78.01379]}, - {"t":2.87094, "x":0.91524, "y":3.1521, "heading":0.063, "vx":-0.14914, "vy":-0.92458, "omega":-0.56115, "ax":0.81274, "ay":4.80183, "alpha":2.90836, "fx":[-2.48545,-1.16103,34.1169,22.67638], "fy":[81.1831,81.15438,73.66692,77.99849]}, - {"t":2.90299, "x":0.91088, "y":3.12493, "heading":0.04501, "vx":-0.12309, "vy":-0.77069, "omega":-0.46794, "ax":0.79578, "ay":4.80484, "alpha":2.91439, "fx":[-2.48591,-1.89933,33.69283,22.73035], "fy":[81.18986,81.1509,73.87009,77.98881]}, - {"t":2.93504, "x":0.90734, "y":3.1027, "heading":0.03002, "vx":-0.09759, "vy":-0.61671, "omega":-0.37455, "ax":0.78061, "ay":4.80754, "alpha":2.91874, "fx":[-2.49702,-2.52482,33.30861,22.7589], "fy":[81.19552,81.1431,74.05167,77.98591]}, - {"t":2.96708, "x":0.90462, "y":3.08541, "heading":0.01801, "vx":-0.07258, "vy":-0.46265, "omega":-0.28101, "ax":0.76693, "ay":4.80998, "alpha":2.92162, "fx":[-2.52438,-3.04852,32.965,22.75935], "fy":[81.20003,81.13335,74.21218,77.9907]}, - {"t":2.99913, "x":0.90269, "y":3.07305, "heading":0.00901, "vx":-0.048, "vy":-0.3085, "omega":-0.18738, "ax":0.75453, "ay":4.81222, "alpha":2.92325, "fx":[-2.57245,-3.4794,32.66264,22.72958], "fy":[81.20332,81.12336,74.35215,78.00387]}, - {"t":3.03118, "x":0.90153, "y":3.06564, "heading":0.003, "vx":-0.02382, "vy":-0.15428, "omega":-0.0937, "ax":0.74321, "ay":4.81429, "alpha":2.9238, "fx":[-2.64473,-3.82469,32.40185,22.66785], "fy":[81.20531,81.1144,74.4721,78.02594]}, - {"t":3.06323, "x":0.90115, "y":3.06316, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index 6f163365..b8c97aea 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -55,6 +55,20 @@ "val":1.0349851207246612 } }, + "CM":{ + "x":{ + "exp":"7.228332996368408 m", + "val":7.228332996368408 + }, + "y":{ + "exp":"GH.y", + "val":4.017168045043945 + }, + "heading":{ + "exp":"pi rad", + "val":3.141592653589793 + } + }, "D":{ "x":{ "exp":"3.967747211456299 m", @@ -111,6 +125,20 @@ "val":3.141592653589793 } }, + "GH":{ + "x":{ + "exp":"5.806319713592529 m", + "val":5.806319713592529 + }, + "y":{ + "exp":"4.017168045043945 m", + "val":4.017168045043945 + }, + "heading":{ + "exp":"pi rad", + "val":3.141592653589793 + } + }, "H":{ "x":{ "exp":"5.808465480804443 m", @@ -237,6 +265,20 @@ "val":1.5940473094638272 } }, + "N":{ + "x":{ + "exp":"8.08 m", + "val":8.08 + }, + "y":{ + "exp":"5 m", + "val":5.0 + }, + "heading":{ + "exp":"pi rad", + "val":3.141592653589793 + } + }, "PL":{ "x":{ "exp":"7.814075469970703 m", @@ -473,8 +515,8 @@ "val":0.0508 }, "vmax":{ - "exp":"4800 RPM", - "val":502.6548245743669 + "exp":"5000 RPM", + "val":523.5987755982989 }, "tmax":{ "exp":"0.65 N * m", diff --git a/src/main/deploy/choreo/starting location tester.traj b/src/main/deploy/choreo/starting location tester.traj deleted file mode 100644 index 4d0d178e..00000000 --- a/src/main/deploy/choreo/starting location tester.traj +++ /dev/null @@ -1,206 +0,0 @@ -{ - "name":"starting location tester", - "version":1, - "snapshot":{ - "waypoints":[ - {"x":7.1, "y":7.216524124145508, "heading":-2.095116885713791, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1, "y":5.889204978942871, "heading":-1.0455529770312977, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.2, "y":4.717027187347412, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.2, "y":3.303518533706665, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1, "y":2.0451512336730957, "heading":1.0349851207246612, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1, "y":0.7523078918457031, "heading":2.0928880900706415, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"LO.x", "val":7.1}, "y":{"exp":"LO.y", "val":7.216524124145508}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"LI.x", "val":7.1}, "y":{"exp":"LI.y", "val":5.889204978942871}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"LM.x", "val":7.2}, "y":{"exp":"LM.y", "val":4.717027187347412}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"RM.x", "val":7.2}, "y":{"exp":"RM.y", "val":3.303518533706665}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"RI.x", "val":7.1}, "y":{"exp":"RI.y", "val":2.0451512336730957}, "heading":{"exp":"1.0349851207246612 rad", "val":1.0349851207246612}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"RO.x", "val":7.1}, "y":{"exp":"RO.y", "val":0.7523078918457031}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":"Swerve", - "waypoints":[0.0,0.81385,1.29347,1.75879,2.25932,3.05285], - "samples":[ - {"t":0.0, "x":7.1, "y":7.21652, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.62908, "ay":-3.83932, "alpha":10.01675, "fx":[59.32677,-64.93321,-45.92361,10.39307], "fy":[-55.43899,-48.03908,-67.0007,-80.58319]}, - {"t":0.02466, "x":7.09981, "y":7.21536, "heading":-2.09512, "vx":-0.01551, "vy":-0.09469, "omega":0.24703, "ax":-0.56911, "ay":-3.94032, "alpha":9.51655, "fx":[58.39956,-60.82818,-45.11492,10.32822], "fy":[-56.40527,-53.1333,-67.54117,-80.58707]}, - {"t":0.04932, "x":7.09925, "y":7.21182, "heading":-2.08902, "vx":-0.02955, "vy":-0.19186, "omega":0.48173, "ax":-0.50491, "ay":-4.02637, "alpha":9.09421, "fx":[57.54957,-56.96985,-44.08237,10.48533], "fy":[-57.26121,-57.25811,-68.21268,-80.56187]}, - {"t":0.07399, "x":7.09837, "y":7.20587, "heading":-2.07714, "vx":-0.042, "vy":-0.29116, "omega":0.70601, "ax":-0.43982, "ay":-4.09947, "alpha":8.73927, "fx":[56.74207,-53.51218,-42.83163,10.84093], "fy":[-58.04888,-60.51839,-68.99775,-80.50936]}, - {"t":0.09865, "x":7.0972, "y":7.19744, "heading":-2.05973, "vx":-0.05285, "vy":-0.39226, "omega":0.92154, "ax":-0.37506, "ay":-4.16286, "alpha":8.43422, "fx":[55.93657,-50.47,-41.36464,11.372], "fy":[-58.81106,-63.09851,-69.87964,-80.43002]}, - {"t":0.12331, "x":7.09578, "y":7.1865, "heading":-2.03701, "vx":-0.0621, "vy":-0.49493, "omega":1.12954, "ax":-0.31054, "ay":-4.21991, "alpha":8.15934, "fx":[55.0849,-47.76696,-39.67985,12.05495], "fy":[-59.59312,-65.19082,-70.84202,-80.32371]}, - {"t":0.14797, "x":7.09416, "y":7.17301, "heading":-2.00915, "vx":-0.06976, "vy":-0.599, "omega":1.33077, "ax":-0.24562, "ay":-4.27368, "alpha":7.8949, "fx":[54.1285,-45.28195,-37.77262,12.86433], "fy":[-60.44451,-66.96248,-71.86867,-80.1904]}, - {"t":0.17263, "x":7.09236, "y":7.15694, "heading":-1.97633, "vx":-0.07581, "vy":-0.70439, "omega":1.52547, "ax":-0.17967, "ay":-4.32679, "alpha":7.62155, "fx":[52.99495,-42.87981,-35.63532,13.7711], "fy":[-61.41964,-68.54568,-72.94296,-80.03092]}, - {"t":0.1973, "x":7.09044, "y":7.13825, "heading":-1.93871, "vx":-0.08025, "vy":-0.8111, "omega":1.71344, "ax":-0.1124, "ay":-4.38141, "alpha":7.31997, "fx":[51.59394,-40.42707,-33.25743,14.74058], "fy":[-62.5773,-70.03824,-74.04741,-79.84796]}, - {"t":0.22196, "x":7.08843, "y":7.11692, "heading":-1.89645, "vx":-0.08302, "vy":-0.91916, "omega":1.89396, "ax":-0.04404, "ay":-4.43927, "alpha":6.97047, "fx":[49.8134,-37.79781,-30.6253,15.72971], "fy":[-63.97711,-71.50687,-75.16313,-79.64728]}, - {"t":0.24662, "x":7.08636, "y":7.0929, "heading":-1.84974, "vx":-0.0841, "vy":-1.02864, "omega":2.06587, "ax":0.02455, "ay":-4.50159, "alpha":6.55285, "fx":[47.51753,-34.87386,-27.72168,16.6834], "fy":[-65.67099,-72.99021,-76.26919,-79.43917]}, - {"t":0.27128, "x":7.0843, "y":7.06616, "heading":-1.79879, "vx":-0.0835, "vy":-1.13966, "omega":2.22747, "ax":0.09196, "ay":-4.56888, "alpha":6.04725, "fx":[44.55105,-31.54244,-24.52446,17.52929], "fy":[-67.68701,-74.50033,-77.34194,-79.24034]}, - {"t":0.29594, "x":7.08227, "y":7.03666, "heading":-1.74386, "vx":-0.08123, "vy":-1.25233, "omega":2.37661, "ax":0.15644, "ay":-4.64055, "alpha":5.43624, "fx":[40.7573,-27.69284,-21.00444,18.16995], "fy":[-70.00422,-76.02236,-78.35396,-79.07602]}, - {"t":0.32061, "x":7.08031, "y":7.00437, "heading":-1.68525, "vx":-0.07737, "vy":-1.36678, "omega":2.51068, "ax":0.2165, "ay":-4.71445, "alpha":4.70834, "fx":[36.02062,-23.21319,-17.12089,18.47082], "fy":[-72.52303,-77.51189,-79.27243,-78.98196]}, - {"t":0.34527, "x":7.07847, "y":6.96923, "heading":-1.62333, "vx":-0.07203, "vy":-1.48305, "omega":2.6268, "ax":0.27185, "ay":-4.78648, "alpha":3.8617, "fx":[30.33813,-17.98837,-12.81325,18.24056], "fy":[-75.04841,-78.88979,-80.05597,-79.0053]}, - {"t":0.36993, "x":7.07677, "y":6.9312, "heading":-1.55855, "vx":-0.06533, "vy":-1.60109, "omega":2.72204, "ax":0.32443, "ay":-4.85068, "alpha":2.90374, "fx":[23.90236,-11.90049,-7.98518,17.19839], "fy":[-77.31523,-80.03363,-80.64805,-79.20078]}, - {"t":0.39459, "x":7.07526, "y":6.89023, "heading":-1.49142, "vx":-0.05733, "vy":-1.72072, "omega":2.79365, "ax":0.37847, "ay":-4.89987, "alpha":1.84038, "fx":[17.13841,-4.83485,-2.47376,14.91925], "fy":[-79.07385,-80.76553,-80.96154,-79.61334]}, - {"t":0.41925, "x":7.07396, "y":6.84631, "heading":-1.42252, "vx":-0.04799, "vy":-1.84156, "omega":2.83903, "ax":0.43883, "ay":-4.92564, "alpha":0.65158, "fx":[10.63345,3.30315,4.01315,10.74657], "fy":[-80.20045,-80.83729,-80.84028,-80.22133]}, - {"t":0.44392, "x":7.07291, "y":6.79939, "heading":-1.3525, "vx":-0.03717, "vy":-1.96304, "omega":2.8551, "ax":0.5083, "ay":-4.9149, "alpha":-0.74309, "fx":[4.96943,12.55289,12.0321,3.68469], "fy":[-80.75001,-79.91841,-79.95298,-80.77532]}, - {"t":0.46858, "x":7.07215, "y":6.74949, "heading":-1.28209, "vx":-0.02464, "vy":-2.08425, "omega":2.83678, "ax":0.58922, "ay":-4.83805, "alpha":-2.52129, "fx":[0.56069,22.83838,22.7143,-7.58284], "fy":[-80.90755,-77.60103,-77.45674,-80.40642]}, - {"t":0.49324, "x":7.07172, "y":6.69661, "heading":-1.21213, "vx":-0.0101, "vy":-2.20356, "omega":2.7746, "ax":0.70262, "ay":-4.61955, "alpha":-4.99298, "fx":[-2.41222,33.88768,38.42571,-23.95542], "fy":[-80.88421,-73.44709,-70.72851,-77.02366]}, - {"t":0.5179, "x":7.07169, "y":6.64086, "heading":-1.1437, "vx":0.00722, "vy":-2.31749, "omega":2.65146, "ax":0.91676, "ay":-4.06696, "alpha":-8.69335, "fx":[-3.97596,45.16108,62.40374,-43.63995], "fy":[-80.83517,-67.10764,-50.24548,-67.75979]}, - {"t":0.54256, "x":7.07214, "y":6.58247, "heading":-1.07831, "vx":0.02983, "vy":-2.41779, "omega":2.43707, "ax":1.07538, "ay":-2.89187, "alpha":-14.02203, "fx":[-4.29958,55.84038,79.8059,-61.02528], "fy":[-80.83198,-58.53277,2.91594,-52.65735]}, - {"t":0.56723, "x":7.07321, "y":6.52197, "heading":-1.01821, "vx":0.05635, "vy":-2.48911, "omega":2.09125, "ax":0.77826, "ay":-1.74403, "alpha":-17.95447, "fx":[-3.65993,64.98983,61.55456,-71.99205], "fy":[-80.8765,-48.19943,51.42513,-36.39569]}, - {"t":0.59189, "x":7.07483, "y":6.46005, "heading":-0.96663, "vx":0.07555, "vy":-2.53212, "omega":1.64846, "ax":0.50986, "ay":-1.0957, "alpha":-19.5568, "fx":[-2.46273,71.92197,41.36973,-77.48783], "fy":[-80.93394,-37.12075,69.13124,-22.72674]}, - {"t":0.61655, "x":7.07685, "y":6.39727, "heading":-0.92598, "vx":0.08812, "vy":-2.55914, "omega":1.16615, "ax":0.37871, "ay":-0.677, "alpha":-20.2401, "fx":[-1.1342,76.51609,29.30019,-79.91732], "fy":[-80.97457,-26.46989,75.27016,-12.09653]}, - {"t":0.64121, "x":7.07914, "y":6.33395, "heading":-0.89722, "vx":0.09746, "vy":-2.57584, "omega":0.66699, "ax":0.31473, "ay":-0.36697, "alpha":-20.54706, "fx":[-0.03381,79.16468,22.25832,-80.80818], "fy":[-80.99265,-17.1237,77.7745,-3.65497]}, - {"t":0.66587, "x":7.08164, "y":6.27031, "heading":-0.88077, "vx":0.10522, "vy":-2.58489, "omega":0.16026, "ax":0.28003, "ay":-0.12454, "alpha":-20.66978, "fx":[0.52999,80.4626,18.18767,-80.86812], "fy":[-81.00016,-9.52657,78.90941,3.47343]}, - {"t":0.69054, "x":7.08432, "y":6.20653, "heading":-0.87682, "vx":0.11213, "vy":-2.58796, "omega":-0.3495, "ax":0.25907, "ay":0.06945, "alpha":-20.69508, "fx":[0.28646,80.96269,16.06961,-80.37733], "fy":[-81.00978,-3.78321,79.42571,9.90866]}, - {"t":0.7152, "x":7.08716, "y":6.14272, "heading":-0.88544, "vx":0.11852, "vy":-2.58625, "omega":-0.85988, "ax":0.24504, "ay":0.2266, "alpha":-20.66841, "fx":[-1.00651,81.07391,15.37551,-79.41921], "fy":[-81.01161,0.18665,79.60536,16.03754]}, - {"t":0.73986, "x":7.09016, "y":6.07901, "heading":-0.90664, "vx":0.12456, "vy":-2.58066, "omega":-1.36961, "ax":0.23405, "ay":0.355, "alpha":-20.61535, "fx":[-3.56856,81.05436,15.80803,-77.98886], "fy":[-80.9461,2.5288,79.55295,22.07864]}, - {"t":0.76452, "x":7.0933, "y":6.01547, "heading":-0.94042, "vx":0.13033, "vy":-2.5719, "omega":-1.87802, "ax":0.22304, "ay":0.46092, "alpha":-20.55065, "fx":[-7.59606,81.03943,17.18579,-76.04378], "fy":[-80.6742,3.38786,79.29223,28.13481]}, - {"t":0.78918, "x":7.09659, "y":5.95219, "heading":-0.98674, "vx":0.13583, "vy":-2.56054, "omega":-2.38485, "ax":0.20928, "ay":0.54998, "alpha":-20.48228, "fx":[-13.24642,81.07257,19.38853,-73.52905], "fy":[-79.94713,2.87811,78.8032,34.23009]}, - {"t":0.81385, "x":7.1, "y":5.8892, "heading":-1.04555, "vx":0.141, "vy":-2.54697, "omega":-2.88998, "ax":0.21073, "ay":0.65647, "alpha":-20.30739, "fx":[-20.03277,80.8652,22.51376,-69.56609], "fy":[-78.1472,2.05257,77.77716,41.24569]}, - {"t":0.82681, "x":7.10185, "y":5.85624, "heading":-1.08302, "vx":0.14373, "vy":-2.53846, "omega":-3.15322, "ax":0.19772, "ay":0.70968, "alpha":-20.23738, "fx":[-24.93206,80.83601,24.30115,-67.2757], "fy":[-76.63282,1.01048,77.20644,44.82337]}, - {"t":0.83977, "x":7.10373, "y":5.8234, "heading":-1.12389, "vx":0.14629, "vy":-2.52926, "omega":-3.41555, "ax":0.17899, "ay":0.777, "alpha":-20.14385, "fx":[-30.60896,80.78134,26.11213,-64.58022], "fy":[-74.42691,0.09922,76.57602,48.56161]}, - {"t":0.85273, "x":7.10564, "y":5.79068, "heading":-1.16816, "vx":0.14861, "vy":-2.51919, "omega":-3.67667, "ax":0.15208, "ay":0.86547, "alpha":-20.01257, "fx":[-37.21076,80.70192,27.88676,-61.43322], "fy":[-71.19193,-0.53159,75.90303,52.41595]}, - {"t":0.8657, "x":7.10758, "y":5.75809, "heading":-1.21582, "vx":0.15058, "vy":-2.50797, "omega":-3.93609, "ax":0.11364, "ay":0.98715, "alpha":-19.81653, "fx":[-44.92517,80.59903,29.5429,-57.78556], "fy":[-66.35832,-0.64308,75.21866,56.33462]}, - {"t":0.87866, "x":7.10954, "y":5.72567, "heading":-1.26685, "vx":0.15206, "vy":-2.49518, "omega":-4.19297, "ax":0.0597, "ay":1.16374, "alpha":-19.50033, "fx":[-53.93773,80.46254,30.9632,-53.5842], "fy":[-58.9023,0.1684,74.57557,60.25773]}, - {"t":0.89162, "x":7.11151, "y":5.69342, "heading":-1.3212, "vx":0.15283, "vy":-2.48009, "omega":-4.44574, "ax":-0.01154, "ay":1.43599, "alpha":-18.94145, "fx":[-64.17853,80.2224,31.97071,-48.76935], "fy":[-46.90368,2.63148,74.05923,64.11606]}, - {"t":0.90459, "x":7.11349, "y":5.66139, "heading":-1.37883, "vx":0.15268, "vy":-2.46148, "omega":-4.69128, "ax":-0.08677, "ay":1.87858, "alpha":-17.85716, "fx":[-74.24321,79.55505,32.28008,-43.26584], "fy":[-26.94937,8.15824,73.80541,67.83061]}, - {"t":0.91755, "x":7.11547, "y":5.62964, "heading":-1.43964, "vx":0.15156, "vy":-2.43713, "omega":-4.92276, "ax":-0.10376, "ay":2.59286, "alpha":-15.66217, "fx":[-78.29854,77.07437,31.38983,-36.95082], "fy":[4.62971,19.58232,74.02484,71.31639]}, - {"t":0.93051, "x":7.11742, "y":5.59827, "heading":-1.50345, "vx":0.15021, "vy":-2.40352, "omega":-5.12578, "ax":0.00483, "ay":3.56187, "alpha":-11.68367, "fx":[-65.79404,67.34236,28.34242,-29.57496], "fy":[42.30123,41.12606,75.01315,74.4784]}, - {"t":0.94347, "x":7.11937, "y":5.56741, "heading":-1.5699, "vx":0.15027, "vy":-2.35734, "omega":-5.27723, "ax":0.02624, "ay":4.42413, "alpha":-6.42804, "fx":[-39.38679,40.4586,21.41644,-20.77245], "fy":[67.92232,67.32251,76.94366,77.11581]}, - {"t":0.95644, "x":7.12132, "y":5.53723, "heading":-1.6383, "vx":0.15061, "vy":-2.29999, "omega":-5.36056, "ax":-0.05612, "ay":4.75966, "alpha":-2.67193, "fx":[-17.90452,14.11134,11.68832,-11.5649], "fy":[76.69437,77.38127,78.55712,78.61283]}, - {"t":0.9694, "x":7.12327, "y":5.50781, "heading":-1.70779, "vx":0.14989, "vy":-2.2383, "omega":-5.39519, "ax":0.15126, "ay":4.72078, "alpha":-2.61864, "fx":[-14.33507,16.66889,15.13482,-7.57749], "fy":[76.70762,76.06654,77.38232,78.5464]}, - {"t":0.98236, "x":7.12522, "y":5.47919, "heading":-1.77773, "vx":0.15185, "vy":-2.1771, "omega":-5.42914, "ax":0.52829, "ay":4.58158, "alpha":-3.14093, "fx":[-11.24985,25.66771,22.89601,-2.76744], "fy":[75.61263,71.72379,74.39148,77.87222]}, - {"t":0.99533, "x":7.12723, "y":5.45136, "heading":-1.84811, "vx":0.15869, "vy":-2.11771, "omega":-5.46985, "ax":1.23369, "ay":4.258, "alpha":-3.1258, "fx":[2.55114,36.85276,32.72474,8.54508], "fy":[72.74681,62.79946,67.9594,74.935]}, - {"t":1.00829, "x":7.1294, "y":5.42426, "heading":-1.91901, "vx":0.17469, "vy":-2.06252, "omega":-5.51037, "ax":3.14388, "ay":2.41261, "alpha":0.9651, "fx":[53.28531,47.99329,49.47349,54.83393], "fy":[39.25341,44.26101,39.84249,34.40924]}, - {"t":1.02125, "x":7.13192, "y":5.39773, "heading":-1.99044, "vx":0.21544, "vy":-2.03124, "omega":-5.49786, "ax":2.16393, "ay":-2.28966, "alpha":9.79446, "fx":[68.98179,47.09526,-12.3926,37.81997], "fy":[-22.72896,5.22103,-68.47873,-63.73961]}, - {"t":1.03421, "x":7.1349, "y":5.37121, "heading":-2.06171, "vx":0.24349, "vy":-2.06092, "omega":-5.3709, "ax":0.89519, "ay":-3.18963, "alpha":9.92872, "fx":[65.95091,3.72344,-34.08459,22.94906], "fy":[-38.60514,-27.2841,-68.34241,-74.34617]}, - {"t":1.04718, "x":7.13813, "y":5.34422, "heading":-2.13133, "vx":0.25509, "vy":-2.10227, "omega":-5.2422, "ax":0.18598, "ay":-3.56947, "alpha":9.51952, "fx":[62.52338,-24.51605,-41.34997,15.50432], "fy":[-46.65381,-42.92589,-66.34396,-77.49238]}, - {"t":1.06014, "x":7.14145, "y":5.31667, "heading":-2.19928, "vx":0.25751, "vy":-2.14854, "omega":-5.1188, "ax":-0.0856, "ay":-3.75348, "alpha":9.01995, "fx":[59.38207,-29.23879,-45.88868,10.14776], "fy":[-51.90404,-50.26375,-64.30223,-78.97892]}, - {"t":1.0731, "x":7.14478, "y":5.28851, "heading":-2.26564, "vx":0.2564, "vy":-2.1972, "omega":-5.00187, "ax":-0.19575, "ay":-3.87768, "alpha":8.52847, "fx":[56.60077,-25.54988,-49.5431,5.69175], "fy":[-55.68322,-55.97447,-62.13186,-79.78147]}, - {"t":1.08606, "x":7.14809, "y":5.2597, "heading":-2.33047, "vx":0.25386, "vy":-2.24746, "omega":-4.89132, "ax":-0.18209, "ay":-3.98201, "alpha":8.06209, "fx":[53.99155,-14.95237,-52.66857,1.72237], "fy":[-58.70366,-61.58112,-59.90792,-80.20051]}, - {"t":1.09903, "x":7.15137, "y":5.23023, "heading":-2.39388, "vx":0.2515, "vy":-2.29908, "omega":-4.78681, "ax":-0.06378, "ay":-4.08763, "alpha":7.63849, "fx":[50.99927,1.67768,-54.93049,-1.91731], "fy":[-61.65067,-67.16335,-58.12566,-80.35989]}, - {"t":1.11199, "x":7.15462, "y":5.20009, "heading":-2.45593, "vx":0.25067, "vy":-2.35207, "omega":-4.6878, "ax":-0.01487, "ay":-4.19226, "alpha":7.27459, "fx":[47.00481,13.05007,-55.79022,-5.23728], "fy":[-64.97707,-71.33015,-57.50508,-80.3296]}, - {"t":1.12495, "x":7.15787, "y":5.16924, "heading":-2.5167, "vx":0.25048, "vy":-2.40641, "omega":-4.5935, "ax":-0.07394, "ay":-4.28401, "alpha":6.93401, "fx":[42.40847,16.63711,-55.65044,-8.23009], "fy":[-68.23324,-73.94848,-57.79293,-80.16682]}, - {"t":1.13792, "x":7.16111, "y":5.13769, "heading":-2.57624, "vx":0.24952, "vy":-2.46194, "omega":-4.50362, "ax":-0.16484, "ay":-4.35442, "alpha":6.64191, "fx":[37.88276,17.46195,-55.18227,-10.94175], "fy":[-70.97395,-75.499,-58.36068,-79.91209]}, - {"t":1.15088, "x":7.16433, "y":5.10541, "heading":-2.63462, "vx":0.24738, "vy":-2.51839, "omega":-4.41752, "ax":-0.21509, "ay":-4.35861, "alpha":6.70915, "fx":[34.94627,20.74852,-56.16231,-13.59793], "fy":[-72.56938,-75.36644,-57.51966,-79.56437]}, - {"t":1.16384, "x":7.16752, "y":5.0724, "heading":-2.69188, "vx":0.24459, "vy":-2.57489, "omega":-4.33055, "ax":-0.02792, "ay":-4.10043, "alpha":8.35254, "fx":[36.44567,40.7289,-62.16819,-16.8321], "fy":[-71.92468,-66.1389,-51.06767,-79.00589]}, - {"t":1.1768, "x":7.17069, "y":5.03868, "heading":-2.74802, "vx":0.24423, "vy":-2.62804, "omega":-4.22228, "ax":0.32487, "ay":-3.34765, "alpha":12.24692, "fx":[39.0588,71.80981,-69.16795,-20.45636], "fy":[-70.62863,-28.86785,-41.21157,-78.20243]}, - {"t":1.18977, "x":7.17388, "y":5.00433, "heading":-2.80275, "vx":0.24844, "vy":-2.67143, "omega":-4.06352, "ax":0.30447, "ay":-2.55194, "alpha":15.58856, "fx":[40.79571,77.73219,-74.48039,-24.13749], "fy":[-69.71699,10.77219,-30.73926,-77.19334]}, - {"t":1.20273, "x":7.17713, "y":4.96949, "heading":-2.85543, "vx":0.25239, "vy":-2.70451, "omega":-3.86145, "ax":0.15327, "ay":-2.10108, "alpha":17.1782, "fx":[41.6519,74.08767,-77.96814,-27.74862], "fy":[-69.27418,28.44153,-20.55157,-76.01033]}, - {"t":1.21569, "x":7.18041, "y":4.93425, "heading":-2.90548, "vx":0.25438, "vy":-2.73175, "omega":-3.63878, "ax":0.03409, "ay":-1.82477, "alpha":18.02562, "fx":[41.8644,71.51541,-79.92997,-31.22061], "fy":[-69.2008,35.5674,-11.00629,-74.68607]}, - {"t":1.22865, "x":7.18371, "y":4.89869, "heading":-2.95265, "vx":0.25482, "vy":-2.7554, "omega":-3.40511, "ax":-0.04962, "ay":-1.62722, "alpha":18.56395, "fx":[41.63023,70.33961,-80.70112,-34.51359], "fy":[-69.38799,38.49394,-2.26057,-73.2529]}, - {"t":1.24162, "x":7.18701, "y":4.86283, "heading":-2.99679, "vx":0.25418, "vy":-2.7765, "omega":-3.16447, "ax":-0.10851, "ay":-1.47292, "alpha":18.94304, "fx":[41.09452,69.99352,-80.58044,-37.60303], "fy":[-69.7455,39.53921,5.63124,-71.74287]}, - {"t":1.25458, "x":7.1903, "y":4.82672, "heading":-3.03781, "vx":0.25277, "vy":-2.79559, "omega":-2.91892, "ax":-0.1504, "ay":-1.34665, "alpha":19.22613, "fx":[40.36423,70.09214,-79.81717,-40.47397], "fy":[-70.20373,39.65777,12.67279,-70.18734]}, - {"t":1.26754, "x":7.19356, "y":4.79037, "heading":-3.07565, "vx":0.25082, "vy":-2.81305, "omega":-2.6697, "ax":-0.18043, "ay":-1.24055, "alpha":19.44537, "fx":[39.51986,70.41248,-78.61303,-43.11812], "fy":[-70.71082,39.3046,18.89998,-68.61649]}, - {"t":1.28051, "x":7.1968, "y":4.7538, "heading":-3.11025, "vx":0.24848, "vy":-2.82913, "omega":-2.41763, "ax":-0.20203, "ay":-1.14995, "alpha":19.61945, "fx":[38.62344,70.82582,-77.12857,-45.53218], "fy":[-71.22886,38.72447,24.36534,-67.05881]}, - {"t":1.29347, "x":7.2, "y":4.71703, "heading":3.14159, "vx":0.24586, "vy":-2.84403, "omega":-2.16331, "ax":-0.22986, "ay":-1.23018, "alpha":19.49753, "fx":[35.71676,73.453,-77.20124,-46.99942], "fy":[-72.84777,33.94046,24.61864,-66.15586]}, - {"t":1.31462, "x":7.20515, "y":4.6566, "heading":3.09584, "vx":0.241, "vy":-2.87005, "omega":-1.75092, "ax":-0.2688, "ay":-1.25075, "alpha":19.44003, "fx":[32.56919,75.42025,-76.1352,-49.43177], "fy":[-74.28714,29.19742,27.63486,-64.33456]}, - {"t":1.33577, "x":7.21019, "y":4.59561, "heading":3.0588, "vx":0.23532, "vy":-2.89651, "omega":-1.33975, "ax":-0.31068, "ay":-1.26977, "alpha":19.37766, "fx":[29.69101,76.7478,-75.27893,-51.47603], "fy":[-75.45722,25.32988,29.77499,-62.68105]}, - {"t":1.35692, "x":7.21509, "y":4.53407, "heading":3.03047, "vx":0.22874, "vy":-2.92336, "omega":-0.92989, "ax":-0.35628, "ay":-1.289, "alpha":19.30513, "fx":[27.02545,77.59001,-74.73438,-53.17929], "fy":[-76.41888,22.34559,30.98783,-61.20515]}, - {"t":1.37807, "x":7.21985, "y":4.47195, "heading":3.0108, "vx":0.22121, "vy":-2.95063, "omega":-0.52157, "ax":-0.40772, "ay":-1.30996, "alpha":19.21521, "fx":[24.46758,78.04813,-74.57801,-54.59951], "fy":[-77.23187,20.25538,31.20878,-59.89345]}, - {"t":1.39922, "x":7.22444, "y":4.40925, "heading":2.99977, "vx":0.21258, "vy":-2.97833, "omega":-0.11516, "ax":-0.46917, "ay":-1.33374, "alpha":19.0967, "fx":[21.82494,78.16535,-74.8547,-55.81549], "fy":[-77.95987,19.09685,30.34441,-58.69789]}, - {"t":1.42037, "x":7.22883, "y":4.34595, "heading":2.99733, "vx":0.20266, "vy":-3.00654, "omega":0.28875, "ax":-0.549, "ay":-1.3603, "alpha":18.92955, "fx":[18.72404,77.89523,-75.5711,-56.94833], "fy":[-78.6759,18.98639,28.24655,-57.51051]}, - {"t":1.44152, "x":7.23299, "y":4.28206, "heading":3.00344, "vx":0.19105, "vy":-3.03532, "omega":0.68913, "ax":-0.6665, "ay":-1.38523, "alpha":18.67053, "fx":[14.359,76.95103,-76.68397,-58.21041], "fy":[-79.45458,20.31133,24.66084,-56.10077]}, - {"t":1.46268, "x":7.23689, "y":4.21755, "heading":3.01801, "vx":0.17695, "vy":-3.06461, "omega":1.08403, "ax":-0.88354, "ay":-1.37663, "alpha":18.19808, "fx":[6.68572,73.63557,-78.06787,-60.02999], "fy":[-80.23257,25.03582,19.10849,-53.93258]}, - {"t":1.48383, "x":7.24043, "y":4.15242, "heading":3.04094, "vx":0.15827, "vy":-3.09373, "omega":1.46893, "ax":-2.89613, "ay":-1.10669, "alpha":12.51072, "fx":[-10.64721,-35.94249,-79.38436,-63.41106], "fy":[-79.32744,45.80608,10.64101,-49.48871]}, - {"t":1.50498, "x":7.24313, "y":4.08674, "heading":3.07201, "vx":0.09701, "vy":-3.11714, "omega":1.73354, "ax":-4.32508, "ay":-1.65216, "alpha":4.40097, "fx":[-55.12723,-77.23766,-79.5354,-70.92692], "fy":[-56.8011,-11.25006,-3.31725,-36.67025]}, - {"t":1.52613, "x":7.24422, "y":4.02044, "heading":3.10868, "vx":0.00553, "vy":-3.15208, "omega":1.82663, "ax":-4.65093, "ay":-0.23155, "alpha":-4.10695, "fx":[-78.40199,-77.43061,-72.52261,-75.78039], "fy":[13.46466,-18.4403,-30.32352,20.15773]}, - {"t":1.54728, "x":7.24329, "y":3.95372, "heading":-3.13587, "vx":-0.09284, "vy":-3.15698, "omega":1.73976, "ax":-2.75798, "ay":0.36122, "alpha":-14.86509, "fx":[-68.22935,-76.69389,-23.12792,-12.2998], "fy":[42.29748,-23.23953,-73.85396,78.41707]}, - {"t":1.56843, "x":7.24071, "y":3.88703, "heading":-3.09908, "vx":-0.15117, "vy":-3.14934, "omega":1.42535, "ax":-1.26049, "ay":0.56499, "alpha":-18.9067, "fx":[-62.28566,-75.6229,42.66293,12.81917], "fy":[51.17898,-27.34055,-66.14103,79.24894]}, - {"t":1.58958, "x":7.23723, "y":3.82054, "heading":-3.06893, "vx":-0.17783, "vy":-3.13739, "omega":1.02546, "ax":-0.76392, "ay":0.81454, "alpha":-19.52217, "fx":[-59.34799,-74.47826,63.1327,20.73884], "fy":[54.82143,-30.76562,-48.71249,77.9212]}, - {"t":1.61073, "x":7.2333, "y":3.75437, "heading":-3.04724, "vx":-0.19399, "vy":-3.12016, "omega":0.61255, "ax":-0.56198, "ay":0.93793, "alpha":-19.64883, "fx":[-57.67598,-73.45064,70.08668,24.2906], "fy":[56.73228,-33.42473,-39.05014,77.0761]}, - {"t":1.63188, "x":7.22907, "y":3.68858, "heading":-3.03428, "vx":-0.20588, "vy":-3.10032, "omega":0.19696, "ax":-0.45318, "ay":1.00198, "alpha":-19.69173, "fx":[-56.49429,-72.66849,73.11091,26.41744], "fy":[58.01033,-35.28892,-33.68559,76.4862]}, - {"t":1.65303, "x":7.22462, "y":3.62323, "heading":-3.03012, "vx":-0.21546, "vy":-3.07913, "omega":-0.21953, "ax":-0.38276, "ay":1.03801, "alpha":-19.71412, "fx":[-55.43315,-72.21958,74.535,28.08839], "fy":[59.09651,-36.34715,-30.83511,75.9635]}, - {"t":1.67418, "x":7.21997, "y":3.55834, "heading":-3.03476, "vx":-0.22356, "vy":-3.05718, "omega":-0.6365, "ax":-0.33085, "ay":1.05955, "alpha":-19.72995, "fx":[-54.29504,-72.155,75.10598,29.70875], "fy":[60.19673,-36.59464,-29.71366,75.39789]}, - {"t":1.69533, "x":7.21517, "y":3.49391, "heading":-3.04822, "vx":-0.23056, "vy":-3.03477, "omega":-1.05381, "ax":-0.28871, "ay":1.0729, "alpha":-19.74322, "fx":[-52.96201,-72.48953,75.09699,31.475], "fy":[61.41351,-36.02882,-29.94312,74.71768]}, - {"t":1.71648, "x":7.21023, "y":3.42997, "heading":-3.07051, "vx":-0.23666, "vy":-3.01207, "omega":-1.47139, "ax":-0.25195, "ay":1.081, "alpha":-19.75562, "fx":[-51.35536,-73.2022,74.59146,33.49078], "fy":[62.79538,-34.64804,-31.32545,73.86703]}, - {"t":1.73764, "x":7.20517, "y":3.3665, "heading":-3.10163, "vx":-0.24199, "vy":-2.98921, "omega":-1.88924, "ax":-0.21822, "ay":1.08501, "alpha":-19.76844, "fx":[-49.41679,-74.23824,73.57294,35.81201], "fy":[64.35802,-32.4526,-33.74868,72.79465]}, - {"t":1.75879, "x":7.2, "y":3.30352, "heading":3.14159, "vx":-0.24661, "vy":-2.96626, "omega":-2.30736, "ax":-0.19989, "ay":1.26357, "alpha":-19.3975, "fx":[-46.10856,-77.06868,73.90306,36.20279], "fy":[66.60732,-24.23213,-32.1854,72.43777]}, - {"t":1.77196, "x":7.19673, "y":3.26456, "heading":3.1112, "vx":-0.24924, "vy":-2.94962, "omega":-2.56286, "ax":-0.17552, "ay":1.405, "alpha":-19.10507, "fx":[-43.66984,-78.74578,74.39888,36.53913], "fy":[68.20681,-17.83783,-30.73293,72.24017]}, - {"t":1.78513, "x":7.19344, "y":3.22583, "heading":3.07744, "vx":-0.25155, "vy":-2.93111, "omega":-2.81452, "ax":-0.13841, "ay":1.57939, "alpha":-18.7097, "fx":[-40.96972,-80.0259,75.18484,36.75956], "fy":[69.83539,-10.3143,-28.33612,72.09527]}, - {"t":1.7983, "x":7.19011, "y":3.18735, "heading":3.04037, "vx":-0.25338, "vy":-2.91031, "omega":-3.06096, "ax":-0.08341, "ay":1.8005, "alpha":-18.1533, "fx":[-38.00903,-80.61449,76.39325,36.77611], "fy":[71.46038,-1.55714,-24.21281,72.04845]}, - {"t":1.81147, "x":7.18677, "y":3.14918, "heading":3.00005, "vx":-0.25447, "vy":-2.88659, "omega":-3.30008, "ax":-0.00609, "ay":2.09283, "alpha":-17.32347, "fx":[-34.79381,-80.11963,78.04281,36.47236], "fy":[73.04666,8.4756,-16.82478,72.15782]}, - {"t":1.82465, "x":7.18341, "y":3.11134, "heading":2.95658, "vx":-0.25455, "vy":-2.85902, "omega":-3.52826, "ax":0.08616, "ay":2.50222, "alpha":-15.98358, "fx":[-31.33761,-78.05527,79.33396,35.69326], "fy":[74.55703,19.69318,-3.11794,72.49377]}, - {"t":1.83782, "x":7.18007, "y":3.07389, "heading":2.91011, "vx":-0.25342, "vy":-2.82606, "omega":-3.7388, "ax":0.13154, "ay":3.09079, "alpha":-13.69661, "fx":[-27.66708,-73.90064,75.92918,34.24019], "fy":[75.95205,31.76058,21.27169,73.12973]}, - {"t":1.85099, "x":7.17674, "y":3.03694, "heading":2.86086, "vx":-0.25169, "vy":-2.78535, "omega":-3.91921, "ax":-0.00826, "ay":3.78442, "alpha":-10.39509, "fx":[-23.84171,-67.31845,58.70068,31.91909], "fy":[77.18737,43.91788,52.26598,74.10099]}, - {"t":1.86416, "x":7.17343, "y":3.00058, "heading":2.80924, "vx":-0.2518, "vy":-2.7355, "omega":-4.05613, "ax":-0.23895, "ay":4.26603, "alpha":-7.53879, "fx":[-20.0451,-58.94503,34.42831,28.93631], "fy":[78.19995,54.57086,70.95936,75.23572]}, - {"t":1.87733, "x":7.17009, "y":2.96491, "heading":2.75581, "vx":-0.25494, "vy":-2.67931, "omega":-4.15543, "ax":-0.29456, "ay":4.43258, "alpha":-6.34527, "fx":[-16.96409,-53.35336,23.02339,28.0322], "fy":[78.86362,59.97542,75.52657,75.49155]}, - {"t":1.89051, "x":7.1667, "y":2.93001, "heading":2.70107, "vx":-0.25882, "vy":-2.62093, "omega":-4.23901, "ax":-0.21578, "ay":4.41953, "alpha":-6.37178, "fx":[-14.68998,-52.90743,22.61932,30.86763], "fy":[79.25736,60.26476,75.20019,74.28146]}, - {"t":1.90368, "x":7.16328, "y":2.89587, "heading":2.64524, "vx":-0.26167, "vy":-2.56271, "omega":-4.32294, "ax":-0.10654, "ay":4.36641, "alpha":-6.6348, "fx":[-12.41175,-53.49238,24.21573,34.72161], "fy":[79.57476,59.61615,73.89705,72.44228]}, - {"t":1.91685, "x":7.15982, "y":2.86249, "heading":2.58829, "vx":-0.26307, "vy":-2.5052, "omega":-4.41034, "ax":0.01412, "ay":4.29692, "alpha":-6.94048, "fx":[-9.92988,-54.01749,26.02312,38.84758], "fy":[79.83475,58.98013,72.00515,70.16549]}, - {"t":1.93002, "x":7.15636, "y":2.82986, "heading":2.5302, "vx":-0.26288, "vy":-2.4486, "omega":-4.50176, "ax":0.14076, "ay":4.21093, "alpha":-7.2692, "fx":[-7.19053,-54.24692,27.56953,43.07266], "fy":[80.01629,58.56515,69.32564,67.45534]}, - {"t":1.94319, "x":7.15291, "y":2.79798, "heading":2.4709, "vx":-0.26103, "vy":-2.39313, "omega":-4.59751, "ax":0.263, "ay":4.10591, "alpha":-7.61919, "fx":[-4.14708,-53.97306,28.09333,47.22527], "fy":[80.08387,58.54883,65.51913,64.34344]}, - {"t":1.95637, "x":7.14949, "y":2.76681, "heading":2.41035, "vx":-0.25756, "vy":-2.33905, "omega":-4.69787, "ax":0.36113, "ay":3.98067, "alpha":-7.99188, "fx":[-0.74516,-52.93334,26.19958,51.09436], "fy":[79.98254,59.11998,60.27915,60.9235]}, - {"t":1.96954, "x":7.14613, "y":2.73635, "heading":2.34847, "vx":-0.25281, "vy":-2.28661, "omega":-4.80314, "ax":0.42724, "ay":3.83667, "alpha":-8.39249, "fx":[3.09438,-50.84041,21.1688,54.51523], "fy":[79.62292,60.3887,53.5947,57.28256]}, - {"t":1.98271, "x":7.14284, "y":2.70656, "heading":2.2852, "vx":-0.24718, "vy":-2.23608, "omega":-4.91368, "ax":0.53388, "ay":3.66767, "alpha":-8.82247, "fx":[7.54529,-47.42648,17.26393,57.52891], "fy":[78.83702,62.25108,45.48835,53.26154]}, - {"t":1.99588, "x":7.13963, "y":2.67742, "heading":2.22048, "vx":-0.24015, "vy":-2.18777, "omega":-5.02989, "ax":0.82951, "ay":3.43073, "alpha":-9.25104, "fx":[13.12574,-42.03886,22.79152,60.36533], "fy":[77.20057,64.46039,34.53023,48.15235]}, - {"t":2.00905, "x":7.13654, "y":2.6489, "heading":2.15422, "vx":-0.22922, "vy":-2.14258, "omega":-5.15175, "ax":1.42386, "ay":3.00453, "alpha":-9.53447, "fx":[21.7948,-31.24056,39.328,63.22706], "fy":[73.02268,66.6114,17.03533,39.80414]}, - {"t":2.02223, "x":7.13364, "y":2.62094, "heading":2.08636, "vx":-0.21047, "vy":-2.103, "omega":-5.27734, "ax":2.72587, "ay":1.44014, "alpha":-7.77718, "fx":[45.95922,14.81473,51.2051,66.27227], "fy":[48.94771,50.35162,-16.87457,11.74932]}, - {"t":2.0354, "x":7.1311, "y":2.59337, "heading":2.01685, "vx":-0.17456, "vy":-2.08403, "omega":-5.37978, "ax":1.89037, "ay":-3.61571, "alpha":3.0001, "fx":[21.4585,42.0978,44.0814,15.97789], "fy":[-66.98851,-56.47633,-49.1833,-63.7916]}, - {"t":2.04857, "x":7.12897, "y":2.5656, "heading":1.94599, "vx":-0.14966, "vy":-2.13166, "omega":-5.34026, "ax":0.75812, "ay":-4.33678, "alpha":4.11242, "fx":[1.01297,31.26475,32.083,-14.78572], "fy":[-76.46559,-69.43853,-65.20397,-72.48427]}, - {"t":2.06174, "x":7.12706, "y":2.53715, "heading":1.87564, "vx":-0.13968, "vy":-2.18878, "omega":-5.28609, "ax":0.2934, "ay":-4.55712, "alpha":3.81859, "fx":[-6.11594,23.68503,23.21368,-21.59651], "fy":[-77.95925,-74.27372,-72.16034,-73.60755]}, - {"t":2.07491, "x":7.12525, "y":2.50792, "heading":1.80602, "vx":-0.13581, "vy":-2.24881, "omega":-5.23579, "ax":0.00324, "ay":-4.67649, "alpha":3.28036, "fx":[-10.02575,16.99078,15.95765,-22.71056], "fy":[-78.35456,-76.98068,-75.83715,-74.63447]}, - {"t":2.08809, "x":7.12346, "y":2.47789, "heading":1.73705, "vx":-0.13577, "vy":-2.31041, "omega":-5.19258, "ax":-0.1818, "ay":-4.72146, "alpha":3.13302, "fx":[-13.24212,13.01753,12.7968,-24.46062], "fy":[-78.3534,-78.28157,-77.31271,-74.80013]}, - {"t":2.10126, "x":7.12166, "y":2.44705, "heading":1.66865, "vx":-0.13816, "vy":-2.3726, "omega":-5.15132, "ax":-0.08898, "ay":-4.40509, "alpha":6.50618, "fx":[-20.53468,22.13326,36.54151,-43.95898], "fy":[-77.16083,-76.64105,-69.04345,-65.21378]}, - {"t":2.11443, "x":7.11983, "y":2.41542, "heading":1.6008, "vx":-0.13933, "vy":-2.43062, "omega":-5.06562, "ax":0.0391, "ay":-3.44822, "alpha":12.21322, "fx":[-28.57261,30.57929,68.75165,-68.20179], "fy":[-74.85035,-74.03594,-37.59724,-39.0035]}, - {"t":2.1276, "x":7.118, "y":2.3831, "heading":1.53408, "vx":-0.13882, "vy":-2.47604, "omega":-4.90474, "ax":-0.01865, "ay":-2.43988, "alpha":16.2084, "fx":[-35.68787,34.5861,78.57412,-78.6922], "fy":[-71.93983,-72.50759,-9.90403,-5.1981]}, - {"t":2.14077, "x":7.11617, "y":2.35028, "heading":1.46947, "vx":-0.13907, "vy":-2.50818, "omega":-4.69125, "ax":-0.03583, "ay":-1.75527, "alpha":18.21106, "fx":[-42.00517,35.98687,79.72274,-76.04758], "fy":[-68.60281,-72.0051,3.28177,22.54474]}, - {"t":2.15395, "x":7.11433, "y":2.31709, "heading":1.40768, "vx":-0.13954, "vy":-2.5313, "omega":-4.45137, "ax":-0.00426, "ay":-1.33006, "alpha":19.19251, "fx":[-47.63457,35.92068,79.65777,-68.22259], "fy":[-64.94888,-72.17085,8.90265,41.24099]}, - {"t":2.16712, "x":7.11249, "y":2.28363, "heading":1.34904, "vx":-0.13959, "vy":-2.54882, "omega":-4.19856, "ax":0.03702, "ay":-1.06063, "alpha":19.70702, "fx":[-52.63465,34.99592,79.62924,-59.56992], "fy":[-61.07492,-72.7231,11.00992,53.43095]}, - {"t":2.18029, "x":7.11066, "y":2.24996, "heading":1.29374, "vx":-0.13911, "vy":-2.56279, "omega":-3.93898, "ax":0.07316, "ay":-0.88106, "alpha":20.00116, "fx":[-57.04607,33.55588,79.74009,-51.46593], "fy":[-57.06995,-73.47509,11.36469,61.56598]}, - {"t":2.19346, "x":7.10883, "y":2.21613, "heading":1.24186, "vx":-0.13814, "vy":-2.5744, "omega":-3.67553, "ax":0.10143, "ay":-0.75486, "alpha":20.18347, "fx":[-60.90651,31.81023,79.93516,-44.20642], "fy":[-53.01425,-74.30778,10.79184,67.16806]}, - {"t":2.20664, "x":7.10702, "y":2.18215, "heading":1.19344, "vx":-0.13681, "vy":-2.58434, "omega":-3.40967, "ax":0.12265, "ay":-0.66187, "alpha":20.30453, "fx":[-64.25479,29.89658,80.15905,-37.7806], "fy":[-48.97911,-75.14671,9.72006,71.12468]}, - {"t":2.21981, "x":7.10523, "y":2.14806, "heading":1.14853, "vx":-0.13519, "vy":-2.59306, "omega":-3.14222, "ax":0.13834, "ay":-0.5905, "alpha":20.3896, "fx":[-67.13165,27.91086,80.37578,-32.10889], "fy":[-45.02702,-75.94654,8.39315,73.96614]}, - {"t":2.23298, "x":7.10346, "y":2.11385, "heading":1.10714, "vx":-0.13337, "vy":-2.60084, "omega":-2.87365, "ax":0.14988, "ay":-0.53383, "alpha":20.45222, "fx":[-69.57934,25.92325,80.56575,-27.10866], "fy":[-41.212,-76.68111,6.96012,76.02466]}, - {"t":2.24615, "x":7.10172, "y":2.07954, "heading":1.06929, "vx":-0.13139, "vy":-2.60787, "omega":-2.60425, "ax":0.15839, "ay":-0.48753, "alpha":20.50004, "fx":[-71.64083,23.98704,80.72066,-22.70944], "fy":[-37.5799,-77.33697,5.51745,77.51894]}, - {"t":2.25932, "x":7.1, "y":2.04515, "heading":1.03499, "vx":-0.12931, "vy":-2.61429, "omega":-2.33422, "ax":0.18158, "ay":-0.45282, "alpha":20.56754, "fx":[-73.33795,22.54599,80.9921,-18.32631], "fy":[-34.56285,-77.924,3.96343,78.91253]}, - {"t":2.28337, "x":7.09694, "y":1.98216, "heading":0.97886, "vx":-0.12494, "vy":-2.62518, "omega":-1.83965, "ax":0.19476, "ay":-0.34249, "alpha":20.633, "fx":[-76.36356,19.83915,81.00902,-11.74898], "fy":[-27.14454,-78.62967,3.22783,80.14998]}, - {"t":2.30742, "x":7.094, "y":1.91893, "heading":0.93462, "vx":-0.12026, "vy":-2.63341, "omega":-1.3435, "ax":0.20595, "ay":-0.21093, "alpha":20.685, "fx":[-78.65077,17.90677,80.95729,-6.74568], "fy":[-19.41245,-79.05768,3.95786,80.7192]}, - {"t":2.33146, "x":7.09116, "y":1.85555, "heading":0.90231, "vx":-0.11531, "vy":-2.63849, "omega":-0.8461, "ax":0.21869, "ay":-0.051, "alpha":20.70903, "fx":[-80.17814,16.8906,80.7864,-3.19831], "fy":[-11.31926,-79.2357,6.28848,80.93115]}, - {"t":2.35551, "x":7.08845, "y":1.79209, "heading":0.88197, "vx":-0.11005, "vy":-2.63971, "omega":-0.34812, "ax":0.23765, "ay":0.14485, "alpha":20.67927, "fx":[-80.88374,17.01997,80.33545,-0.93096], "fy":[-2.74697,-79.1458,10.38291,80.98217]}, - {"t":2.37955, "x":7.08588, "y":1.72865, "heading":0.8736, "vx":-0.10433, "vy":-2.63623, "omega":0.14914, "ax":0.26993, "ay":0.38553, "alpha":20.55202, "fx":[-80.61915,18.71132,79.29676,0.26245], "fy":[6.51325,-78.67236,16.39049,80.97963]}, - {"t":2.4036, "x":7.08345, "y":1.66537, "heading":0.87718, "vx":-0.09784, "vy":-2.62696, "omega":0.64334, "ax":0.32952, "ay":0.68274, "alpha":20.25192, "fx":[-79.07061,22.82004,77.19,0.60885], "fy":[16.76016,-77.43577,24.35195,80.96963]}, - {"t":2.42765, "x":7.08119, "y":1.6024, "heading":0.89265, "vx":-0.08992, "vy":-2.61054, "omega":1.13032, "ax":0.45079, "ay":1.05801, "alpha":19.63385, "fx":[-75.64683,31.36956,73.39945,0.35574], "fy":[28.30418,-74.11958,34.03867,80.96233]}, - {"t":2.45169, "x":7.07916, "y":1.53993, "heading":0.91983, "vx":-0.07908, "vy":-2.5851, "omega":1.60244, "ax":0.72347, "ay":1.59274, "alpha":18.28991, "fx":[-69.40966,49.58957,67.35606,-0.22676], "fy":[41.19058,-62.76664,44.77577,80.95341]}, - {"t":2.47574, "x":7.07746, "y":1.47823, "heading":0.95836, "vx":-0.06168, "vy":-2.5468, "omega":2.04225, "ax":1.172, "ay":2.70447, "alpha":14.38994, "fx":[-59.34451,77.95032,58.87988,-0.8455], "fy":[54.64301,-14.15667,55.42572,80.93953]}, - {"t":2.49979, "x":7.07632, "y":1.41777, "heading":1.00747, "vx":-0.0335, "vy":-2.48177, "omega":2.38828, "ax":0.93273, "ay":4.06812, "alpha":8.43313, "fx":[-45.27618,59.02152,48.41762,-1.16984], "fy":[66.75545,53.58508,64.75825,80.92532]}, - {"t":2.52383, "x":7.07578, "y":1.35927, "heading":1.0649, "vx":-0.01107, "vy":-2.38394, "omega":2.59106, "ax":0.63192, "ay":4.60065, "alpha":5.16098, "fx":[-29.00093,34.20889,36.94248,-0.82738], "fy":[75.30376,72.69472,71.9297,80.91916]}, - {"t":2.54788, "x":7.0757, "y":1.30327, "heading":1.12721, "vx":0.00412, "vy":-2.27332, "omega":2.71516, "ax":0.50486, "ay":4.82008, "alpha":2.93896, "fx":[-13.69108,20.69689,25.49731,0.51089], "fy":[79.59228,77.94382,76.74859,80.91188]}, - {"t":2.57193, "x":7.07594, "y":1.25, "heading":1.1925, "vx":0.01626, "vy":-2.15741, "omega":2.78583, "ax":0.43298, "ay":4.91042, "alpha":1.22127, "fx":[-1.53349,12.03358,14.78857,3.02491], "fy":[80.82293,79.90847,79.52415,80.84866]}, - {"t":2.59597, "x":7.07646, "y":1.19955, "heading":1.25949, "vx":0.02668, "vy":-2.03933, "omega":2.8152, "ax":0.37619, "ay":4.93434, "alpha":-0.15046, "fx":[7.12394,5.54953,5.16151,6.7652], "fy":[80.59429,80.71997,80.73744,80.6168]}, - {"t":2.62002, "x":7.07721, "y":1.15193, "heading":1.32718, "vx":0.03572, "vy":-1.92068, "omega":2.81158, "ax":0.3275, "ay":4.92142, "alpha":-1.29802, "fx":[12.88832,0.20008,-3.29548,11.62288], "fy":[79.93645,80.97793,80.84843,80.06085]}, - {"t":2.64406, "x":7.07816, "y":1.10717, "heading":1.39479, "vx":0.0436, "vy":-1.80234, "omega":2.78037, "ax":0.28624, "ay":4.88555, "alpha":-2.31166, "fx":[16.5186,-4.47493,-10.63298,17.30704], "fy":[79.31616,80.90269,80.22494,79.03387]}, - {"t":2.66811, "x":7.0793, "y":1.06524, "heading":1.46165, "vx":0.05048, "vy":-1.68486, "omega":2.72478, "ax":0.2501, "ay":4.83373, "alpha":-3.24053, "fx":[18.63556,-8.69851,-16.95902,23.37682], "fy":[78.8865,80.59419,79.138,77.47017]}, - {"t":2.69216, "x":7.08058, "y":1.02613, "heading":1.52717, "vx":0.05649, "vy":-1.56862, "omega":2.64686, "ax":0.21488, "ay":4.77096, "alpha":-4.09801, "fx":[19.68995,-12.58597,-22.39582,29.34358], "fy":[78.66319,80.10722,77.77998,75.43425]}, - {"t":2.7162, "x":7.082, "y":0.98979, "heading":1.59082, "vx":0.06166, "vy":-1.4539, "omega":2.54832, "ax":0.17646, "ay":4.70208, "alpha":-4.87653, "fx":[19.99972,-16.19935,-27.06135,34.79995], "fy":[78.61215,79.47773,76.28495,73.10512]}, - {"t":2.74025, "x":7.08354, "y":0.95618, "heading":1.65209, "vx":0.06591, "vy":-1.34083, "omega":2.43106, "ax":0.1324, "ay":4.63159, "alpha":-5.56384, "fx":[19.79065,-19.57286,-31.06358,39.50377], "fy":[78.68743,78.7332,74.74448,70.70542]}, - {"t":2.7643, "x":7.08516, "y":0.92528, "heading":1.71055, "vx":0.06909, "vy":-1.22946, "omega":2.29727, "ax":0.08232, "ay":4.56303, "alpha":-6.15364, "fx":[19.22623,-22.72599,-34.50056,43.38314], "fy":[78.84585,77.89691,73.21851,68.42633]}, - {"t":2.78834, "x":7.08684, "y":0.89704, "heading":1.76579, "vx":0.07107, "vy":-1.11974, "omega":2.14929, "ax":0.02725, "ay":4.49858, "alpha":-6.64857, "fx":[18.42745,-25.67043,-37.4626,46.48753], "fy":[79.05201,76.98987,71.74225,66.38848]}, - {"t":2.81239, "x":7.08856, "y":0.87141, "heading":1.81748, "vx":0.07172, "vy":-1.01156, "omega":1.98942, "ax":-0.03112, "ay":4.43907, "alpha":-7.05865, "fx":[17.48564,-28.41402,-40.03535,48.92851], "fy":[79.27893,76.03143,70.33029,64.64093]}, - {"t":2.83644, "x":7.09028, "y":0.84837, "heading":1.86531, "vx":0.07098, "vy":-0.90482, "omega":1.81969, "ax":-0.09113, "ay":4.38439, "alpha":-7.39815, "fx":[16.4709,-30.96275,-42.30302,50.83564], "fy":[79.50734,75.03961,68.97869,63.18008]}, - {"t":2.86048, "x":7.09196, "y":0.82788, "heading":1.90907, "vx":0.06878, "vy":-0.79939, "omega":1.64179, "ax":-0.15145, "ay":4.33372, "alpha":-7.68312, "fx":[15.43786,-33.32172,-44.3517,52.3322], "fy":[79.72423,74.03125,67.66555,61.97147]}, - {"t":2.88453, "x":7.09357, "y":0.80991, "heading":1.94855, "vx":0.06514, "vy":-0.69518, "omega":1.45704, "ax":-0.21123, "ay":4.28583, "alpha":-7.92996, "fx":[14.42969,-35.49556,-46.27243,53.5253], "fy":[79.92147,73.02205,66.35024,60.9666]}, - {"t":2.90857, "x":7.09507, "y":0.79443, "heading":1.98359, "vx":0.06006, "vy":-0.59212, "omega":1.26635, "ax":-0.27017, "ay":4.23912, "alpha":-8.15481, "fx":[13.48108,-37.48841,-48.16359,54.50399], "fy":[80.09453,72.02679,64.97148,60.11338]}, - {"t":2.93262, "x":7.09644, "y":0.78142, "heading":2.01404, "vx":0.05357, "vy":-0.49019, "omega":1.07026, "ax":-0.32839, "ay":4.19172, "alpha":-8.37362, "fx":[12.6204,-39.30385,-50.13166,55.34091], "fy":[80.2414,71.05947,63.44427,59.36164]}, - {"t":2.95667, "x":7.09763, "y":0.77085, "heading":2.03977, "vx":0.04567, "vy":-0.38939, "omega":0.8689, "ax":-0.38639, "ay":4.14141, "alpha":-8.60246, "fx":[11.87145,-40.94463,-52.28875,56.09505], "fy":[80.36166,70.13355,61.65614,58.66531]}, - {"t":2.98071, "x":7.09862, "y":0.76268, "heading":2.06067, "vx":0.03638, "vy":-0.2898, "omega":0.66204, "ax":-0.44481, "ay":4.08557, "alpha":-8.85792, "fx":[11.25486,-42.41244,-54.74389,56.81458], "fy":[80.45578,69.26217,59.46417,57.98274]}, - {"t":3.00476, "x":7.09937, "y":0.75689, "heading":2.07659, "vx":0.02568, "vy":-0.19156, "omega":0.44904, "ax":-0.50409, "ay":4.02119, "alpha":-9.15714, "fx":[10.78923,-43.70757,-57.58438,57.53928], "fy":[80.52449,68.45844,56.69651,57.27603]}, - {"t":3.02881, "x":7.09984, "y":0.75345, "heading":2.08739, "vx":0.01356, "vy":-0.09487, "omega":0.22885, "ax":-0.56395, "ay":3.94512, "alpha":-9.51688, "fx":[10.49208,-44.82845,-60.84391,58.30244], "fy":[80.56826,67.73578,53.16646,56.51]}, - {"t":3.05285, "x":7.1, "y":0.75231, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f6cdaa9e..1f28e41c 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -17,11 +17,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.Robot.ReefTarget; import frc.robot.Robot.RobotType; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.wrist.WristSubsystem; +import frc.robot.utils.autoaim.AlgaeIntakeTargets; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.CoralTargets; import java.util.HashMap; @@ -35,15 +40,28 @@ public class Autos { private final ManipulatorSubsystem manipulator; private final FunnelSubsystem funnel; private final AutoFactory factory; + private final ElevatorSubsystem elevator; + private final ShoulderSubsystem shoulder; + private final WristSubsystem wrist; @AutoLogOutput public static boolean autoPreScore = true; @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static - @AutoLogOutput public static boolean autoGroundIntake = false; + @AutoLogOutput public static boolean autoGroundCoralIntake = false; + @AutoLogOutput public static boolean autoAlgaeIntake = false; - public Autos(SwerveSubsystem swerve, ManipulatorSubsystem manipulator, FunnelSubsystem funnel) { + public Autos( + SwerveSubsystem swerve, + ManipulatorSubsystem manipulator, + FunnelSubsystem funnel, + ElevatorSubsystem elevator, + ShoulderSubsystem shoulder, + WristSubsystem wrist) { this.swerve = swerve; this.manipulator = manipulator; this.funnel = funnel; + this.elevator = elevator; + this.shoulder = shoulder; + this.wrist = wrist; factory = new AutoFactory( swerve::getPose, @@ -91,7 +109,7 @@ public Command LMtoH() { final var traj = routine.trajectory("LMtoH"); routine.active().whileTrue(Commands.sequence(traj.resetOdometry(), traj.cmd())); bindElevatorExtension(routine); - routine.observe(traj.done()).onTrue(scoreInAuto()); + routine.observe(traj.done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } @@ -100,11 +118,11 @@ public Command RMtoG() { final var traj = routine.trajectory("RMtoG"); routine.active().whileTrue(Commands.sequence(traj.resetOdometry(), traj.cmd())); bindElevatorExtension(routine); - routine.observe(traj.done()).onTrue(scoreInAuto()); + routine.observe(traj.done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } - public void runPath( + public void runCoralPath( AutoRoutine routine, String startPos, String endPos, @@ -120,10 +138,10 @@ public void runPath( .onTrue( Commands.sequence( endPos.length() == 3 - ? intakeInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()) + ? intakeCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()) : Commands.sequence( endPos.length() == 1 - ? scoreInAuto( + ? scoreCoralInAuto( () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) : AutoAim.translateToPose( swerve, @@ -164,7 +182,7 @@ public Command LOtoJ() { String startPos = stops[i]; String endPos = stops[i + 1]; String nextPos = stops[i + 2]; - runPath(routine, startPos, endPos, nextPos, steps); + runCoralPath(routine, startPos, endPos, nextPos, steps); } // final var groundTraj = routine.trajectory("LtoAGround"); @@ -212,7 +230,7 @@ public Command ROtoE() { String startPos = stops[i]; String endPos = stops[i + 1]; String nextPos = stops[i + 2]; - runPath(routine, startPos, endPos, nextPos, steps); + runCoralPath(routine, startPos, endPos, nextPos, steps); } routine @@ -245,10 +263,10 @@ public Command LItoK() { String startPos = stops[i]; String endPos = stops[i + 1]; String nextPos = stops[i + 2]; - runPath(routine, startPos, endPos, nextPos, steps); + runCoralPath(routine, startPos, endPos, nextPos, steps); } // final path - routine.observe(steps.get("PLItoB").done()).onTrue(scoreInAuto()); + routine.observe(steps.get("PLItoB").done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } @@ -275,10 +293,10 @@ public Command RItoD() { String startPos = stops[i]; String endPos = stops[i + 1]; String nextPos = stops[i + 2]; - runPath(routine, startPos, endPos, nextPos, steps); + runCoralPath(routine, startPos, endPos, nextPos, steps); } // final path - routine.observe(steps.get("PRItoA").done()).onTrue(scoreInAuto()); + routine.observe(steps.get("PRItoA").done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } @@ -306,14 +324,93 @@ public Command PMtoPL() { String startPos = stops[i]; String endPos = stops[i + 1]; String nextPos = stops[i + 2]; - runPath(routine, startPos, endPos, nextPos, steps); + runCoralPath(routine, startPos, endPos, nextPos, steps); } // final path - routine.observe(steps.get("PLOtoL").done()).onTrue(scoreInAuto()); + routine.observe(steps.get("PLOtoL").done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } - public Command scoreInAuto(Supplier trajEndPose) { + public Command CMtoGH() { + final var routine = factory.newRoutine("CM to GH"); + bindElevatorExtension(routine, 2.0); + HashMap steps = + new HashMap(); // key - name of path, value - traj + String[] stops = { + "CM", "G", "GH", "NI", "IJ", "NI", "EF" // each stop we are going to, in order + }; + for (int i = 0; i < stops.length - 1; i++) { + String name = stops[i] + "to" + stops[i + 1]; // concatenate the names of the stops + steps.put( + name, routine.trajectory(name)); // and puts that name + corresponding traj to the map + } + routine + // run first path + .active() + .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), steps.get("CMtoG").cmd())); + + routine + .observe(steps.get("CMtoG").done()) + .onTrue(Commands.sequence(scoreCoralInAuto(() -> steps.get("CMtoG").getFinalPose().get()))); + routine + .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) + .onTrue( + Commands.sequence( + swerve.driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0)).withTimeout(0.2), + Commands.runOnce( + () -> { + autoAlgaeIntake = true; + Robot.setCurrentAlgaeIntakeTarget( + AlgaeIntakeTargets.getClosestTarget( + steps.get("CMtoG").getFinalPose().get()) + .height); + }), + AutoAim.translateToPose( + swerve, + () -> + AlgaeIntakeTargets.getOffsetLocation( + AlgaeIntakeTargets.getClosestTargetPose( + steps.get("CMtoG").getFinalPose().get()))) + .until( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + AlgaeIntakeTargets.getOffsetLocation( + AlgaeIntakeTargets.getClosestTargetPose( + swerve.getPose())), + swerve.getVelocityFieldRelative(), + Units.inchesToMeters(1.0), + Units.degreesToRadians(1.0)) + && elevator.isNearTarget() + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + && wrist.isNearAngle( + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + AutoAim.approachAlgae( + swerve, + () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), + 1) + .withTimeout(2)) + .andThen( + Commands.runOnce( + () -> { + autoAlgaeIntake = false; + }), + steps.get("GHtoNI").cmd())); + routine.observe(steps.get("GHtoNI").done()).onTrue(scoreAlgaeInAuto()); + // for (int i = 0; i < stops.length - 2; i++) { + // String startPos = stops[i]; + // String endPos = stops[i + 1]; + // String nextPos = stops[i + 2]; + // runAlgaePath(routine, startPos, endPos, nextPos, steps); + // } + // routine + // .observe(steps.get("NItoEF").done()) + // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); + return routine.cmd(); + } + + public Command scoreCoralInAuto(Supplier trajEndPose) { return Commands.sequence( Commands.waitUntil( new Trigger( @@ -356,15 +453,10 @@ public Command scoreInAuto(Supplier trajEndPose) { swerve, () -> CoralTargets.getClosestTarget(trajEndPose.get()), ChassisSpeeds::new, - new Constraints(1.5, 2.0))); - } - - // TODO: REMOVE THIS OVERLOAD - public Command scoreInAuto() { - return scoreInAuto(() -> swerve.getPose()); + new Constraints(1.5, 1.0))); } - public Command intakeInAuto(Supplier> pose) { + public Command intakeCoralInAuto(Supplier> pose) { if (!pose.get().isPresent()) { return Commands.none(); } else { @@ -410,4 +502,64 @@ public void bindElevatorExtension(AutoRoutine routine, double toleranceMeters) { .whileTrue(Commands.run(() -> autoPreScore = true)) .whileFalse(Commands.run(() -> autoPreScore = false)); } + + public void runAlgaePath( + AutoRoutine routine, + String startPos, + String endPos, + String nextPos, + HashMap steps) { + routine + .observe(steps.get(startPos + "to" + endPos).done()) + .onTrue( + Commands.sequence( + endPos.equals("NI") + ? scoreAlgaeInAuto() + : intakeAlgaeInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()), + steps.get(endPos + "to" + nextPos).cmd())); + } + + public Command intakeAlgaeInAuto(Supplier> pose) { + if (!pose.get().isPresent()) { + return Commands.none(); + } else { + return Commands.sequence( + Commands.runOnce( + () -> { + autoAlgaeIntake = true; + Robot.setCurrentAlgaeIntakeTarget( + AlgaeIntakeTargets.getClosestTarget(pose.get().get()).height); // are you serios + }), + Commands.waitUntil(() -> manipulator.hasAlgae()) + .alongWith( + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setHasAlgae(true)) + : Commands.none()), + Commands.runOnce( + () -> { + autoAlgaeIntake = false; + })); + } + } + + public Command scoreAlgaeInAuto() { + return Commands.runOnce( + () -> { + autoScore = true; + Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); + }) + .andThen( + Commands.waitUntil( + () -> !manipulator.hasAlgae()) // TODO maybe check state directly instead + .alongWith( + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) + : Commands.none()) + .andThen( + Commands.runOnce( + () -> { + autoScore = false; + autoPreScore = false; + }))); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5858986e..a3ec268d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -189,8 +189,8 @@ public static enum AlgaeScoreTarget { } private static ReefTarget currentTarget = ReefTarget.L4; - private AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; - private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; + private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; @AutoLogOutput private boolean killVisionIK = true; @@ -479,8 +479,8 @@ public static enum AlgaeScoreTarget { .getNorm() < 3.25 && DriverStation.isAutonomous()), - driver.leftTrigger(), - driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), + driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()), + driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()), driver .x() .and(driver.pov(-1).negate()) @@ -594,7 +594,7 @@ public Robot() { carriageLigament.append(shoulderLigament); shoulderLigament.append(wristLigament); - autos = new Autos(swerve, manipulator, funnel); + autos = new Autos(swerve, manipulator, funnel, elevator, shoulder, wrist); autoChooser.addDefaultOption("None", autos.getNoneAuto()); SmartDashboard.putData( @@ -700,7 +700,7 @@ public Robot() { .onTrue(Commands.runOnce(() -> Autos.autoPreScore = false)); new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) - .onTrue(Commands.runOnce(() -> Autos.autoGroundIntake = false)); + .onTrue(Commands.runOnce(() -> Autos.autoGroundCoralIntake = false)); new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) .onTrue( @@ -1157,6 +1157,7 @@ private void addAutos() { autoChooser.addOption("4.5 L Inside", autos.LItoK()); autoChooser.addOption("4.5 R Inside", autos.RItoD()); autoChooser.addOption("Push Auto", autos.PMtoPL()); + autoChooser.addDefaultOption("Algae auto", autos.CMtoGH()); } /** Scales a joystick value for teleop driving */ @@ -1314,6 +1315,8 @@ public void robotPeriodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Mechanism/Elevator", elevatorMech2d); superstructure.periodic(); + Logger.recordOutput("Autos/Coral Pre Score", Autos.autoPreScore); + Logger.recordOutput("Autos/Coral Score", Autos.autoScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Pre Score", Autos.autoPreScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); @@ -1331,6 +1334,30 @@ public void robotPeriodic() { state = superstructure::getState; } + public static void setCurrentCoralTarget(ReefTarget target) { + currentTarget = target; + } + + public ReefTarget getCurrentCoralTarget() { + return currentTarget; + } + + public static void setCurrentAlgaeIntakeTarget(AlgaeIntakeTarget target) { + algaeIntakeTarget = target; + } + + public AlgaeIntakeTarget getCurrentAlgaeIntakeTarget() { + return algaeIntakeTarget; + } + + public static void setCurrentAlgaeScoreTarget(AlgaeScoreTarget target) { + algaeScoreTarget = target; + } + + public AlgaeScoreTarget getCurrentAlgaeScoreTarget() { + return algaeScoreTarget; + } + public static void setCurrentTarget(ReefTarget target) { currentTarget = target; } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 4dae0079..a99c15ca 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -14,12 +14,14 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.beambreak.BeambreakIO; import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; import frc.robot.utils.Tracer; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class ManipulatorSubsystem extends RollerSubsystem { @@ -42,7 +44,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { private boolean bb1 = false; private boolean bb2 = false; - private boolean hasAlgae = false; + @AutoLogOutput private boolean hasAlgae = false; private LinearFilter currentFilter = LinearFilter.movingAverage(20); private double currentFilterValue = 0.0; @@ -205,6 +207,6 @@ public void setHasAlgae(boolean state) { } public boolean hasAlgae() { - return hasAlgae; + return hasAlgae || Robot.state.get() == SuperState.READY_ALGAE; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java index b3b441bd..e4d032c0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java @@ -251,9 +251,7 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(-30))), FRONT_LEFT_CAMERA_MATRIX, FRONT_LEFT_DIST_COEFFS); - return new VisionConstants[] { - backLeftCamConstants, frontRightCamConstants, frontLeftCamConstants - }; + return new VisionConstants[] {frontRightCamConstants, frontLeftCamConstants}; } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 12139d67..5b625042 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -150,13 +150,13 @@ public static Command translateToPose( cachedTarget[0].getRotation().getRadians()) + headingController.getSetpoint().velocity) .plus(speedsModifier.get()); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "AutoAim/Target Pose", - new Pose2d( - vxController.getSetpoint().position, - vyController.getSetpoint().position, - Rotation2d.fromRadians(headingController.getSetpoint().position))); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Target Pose", + new Pose2d( + vxController.getSetpoint().position, + vyController.getSetpoint().position, + Rotation2d.fromRadians(headingController.getSetpoint().position))); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("AutoAim/Target Speeds", speeds); return speeds; diff --git a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java b/src/main/java/frc/robot/utils/autoaim/CoralTargets.java index 8937c9bf..04893800 100644 --- a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java +++ b/src/main/java/frc/robot/utils/autoaim/CoralTargets.java @@ -18,8 +18,8 @@ public enum CoralTargets { BLUE_D(new Pose2d(4.35, 3.49, Rotation2d.fromDegrees(240)), false), BLUE_E(new Pose2d(4.60, 3.50, Rotation2d.fromDegrees(300)), false), BLUE_F(new Pose2d(4.88, 3.66, Rotation2d.fromDegrees(300)), true), - BLUE_G(new Pose2d(5.00, 3.90, Rotation2d.fromDegrees(0)), false), - BLUE_H(new Pose2d(5.00, 4.20, Rotation2d.fromDegrees(0)), true), + BLUE_G(new Pose2d(5.00, 3.86, Rotation2d.fromDegrees(0)), false), + BLUE_H(new Pose2d(5.00, 4.18, Rotation2d.fromDegrees(0)), true), BLUE_I(new Pose2d(4.88, 4.41, Rotation2d.fromDegrees(60)), false), BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), From 593d27e756b637f1002ddb2433101e0c1ab39cf8 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 8 Apr 2025 20:46:06 -0700 Subject: [PATCH 006/154] steepen barge shot angle again --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index bb0fae39..853f00b2 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -40,7 +40,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(100); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = From 3a83f517e37f0c752ffd33f03a0aa329482ac59d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 8 Apr 2025 21:15:19 -0700 Subject: [PATCH 007/154] reintroduce toss, use vel threshold --- src/main/java/frc/robot/subsystems/Superstructure.java | 10 +++++++--- .../robot/subsystems/shoulder/ShoulderSubsystem.java | 6 +++++- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 6244b939..5d5dfde8 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -791,10 +791,10 @@ private void configureStateTransitionCommands() { Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), // Make it initially extend to the full 90 degrees - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), wrist.setSlowTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS))) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) + .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .and(scoreReq) .onTrue(forceState(SuperState.SCORE_ALGAE_NET)); @@ -802,7 +802,11 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_ALGAE_NET) .onTrue(Commands.runOnce(() -> stateTimer.reset())) - .whileTrue(manipulator.setVoltage(-13.0)) + .whileTrue( + manipulator + .hold() + .until(() -> shoulder.getVelocity() > 0.4) + .andThen(manipulator.setVoltage(-13.0))) .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 5ed27160..341edec3 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -45,7 +45,7 @@ public class ShoulderSubsystem extends SubsystemBase { ExtensionKinematics.L1_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(40); + public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); @@ -158,4 +158,8 @@ public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { public ShoulderIOInputsAutoLogged getInputs() { return inputs; } + + public double getVelocity() { + return inputs.angularVelocityRPS; + } } From 261c5bbdc3f96aa4a7724c533288afd3cca6bf06 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 8 Apr 2025 21:20:25 -0700 Subject: [PATCH 008/154] add algae intaking to graph --- .../frc/robot/subsystems/ExtensionPathing.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index 50c47e74..345dc6ce 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -110,6 +110,22 @@ public class ExtensionPathing { final var l1 = ExtensionKinematics.L1_EXTENSION; graph.addNode(l1); graph.putEdge(l1, betweenTucked); + + final var algaeLow = + new ExtensionState( + ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + graph.addNode(algaeLow); + graph.putEdge(untucked, algaeLow); + + final var algaeHigh = + new ExtensionState( + ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + graph.addNode(algaeHigh); + graph.putEdge(untucked, algaeHigh); } private ExtensionPathing() {} From 00ba4e604b276605156279d9b68d9ded27b02031 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 9 Apr 2025 12:35:05 -0700 Subject: [PATCH 009/154] tune accel/vel constraints, extend elevator earlier --- src/main/deploy/choreo/LOtoJ.traj | 138 ++++++------- src/main/deploy/choreo/PLMtoA.traj | 184 +++++++++--------- src/main/deploy/choreo/PLOtoK.traj | 129 ++++++------ src/main/deploy/choreo/PLOtoL.traj | 131 +++++++------ src/main/deploy/choreo/PRMtoB.traj | 139 ++++++------- src/main/deploy/choreo/PROtoC.traj | 114 +++++------ src/main/deploy/choreo/PROtoD.traj | 110 +++++------ src/main/deploy/choreo/ROtoE.traj | 106 +++++----- src/main/deploy/choreo/choreo.chor | 11 +- src/main/java/frc/robot/Autos.java | 6 +- .../shoulder/ShoulderSubsystem.java | 2 +- 11 files changed, 553 insertions(+), 517 deletions(-) diff --git a/src/main/deploy/choreo/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 8c038d3c..3c2a93e5 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -4,31 +4,31 @@ "snapshot":{ "waypoints":[ {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":4.484625190496445, "y":4.0126113295555115, "r":3.25}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.484625190496445 m", "val":4.484625190496445}, "y":{"exp":"4.0126113295555115 m", "val":4.0126113295555115}, "r":{"exp":"3.25 m", "val":3.25}}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"slowaccel", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,68 +36,72 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.9336,1.90766], + "waypoints":[0.0,0.94616,2.04867], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.1785, "ay":-1.95764, "alpha":0.00825, "fx":[-68.29881,-68.33496,-68.32225,-68.28611], "fy":[-32.02858,-31.95148,-31.97888,-32.05588]}, - {"t":0.03334, "x":7.10899, "y":6.23448, "heading":-2.35619, "vx":-0.13932, "vy":-0.06527, "omega":0.00028, "ax":-4.1782, "ay":-1.95751, "alpha":0.00851, "fx":[-68.29346,-68.33072,-68.31763,-68.28037], "fy":[-32.02716,-31.94768,-31.97592,-32.05531]}, - {"t":0.06669, "x":7.10202, "y":6.23121, "heading":-2.35619, "vx":-0.27864, "vy":-0.13054, "omega":0.00056, "ax":-4.17785, "ay":-1.95736, "alpha":0.0088, "fx":[-68.28742,-68.32595,-68.31242,-68.2739], "fy":[-32.02556,-31.94339,-31.97259,-32.05466]}, - {"t":0.10003, "x":7.09041, "y":6.22577, "heading":-2.35617, "vx":-0.41794, "vy":-0.19581, "omega":0.00085, "ax":-4.17746, "ay":-1.95719, "alpha":0.00913, "fx":[-68.28058,-68.32053,-68.30651,-68.26656], "fy":[-32.02374,-31.93852,-31.96882,-32.05393]}, - {"t":0.13337, "x":7.07415, "y":6.21815, "heading":-2.35614, "vx":-0.55723, "vy":-0.26106, "omega":0.00116, "ax":-4.17702, "ay":-1.95699, "alpha":0.0095, "fx":[-68.27274,-68.31434,-68.29975,-68.25816], "fy":[-32.02166,-31.93295,-31.9645,-32.05309]}, - {"t":0.16671, "x":7.05325, "y":6.20836, "heading":-2.3561, "vx":-0.6965, "vy":-0.32632, "omega":0.00147, "ax":-4.1765, "ay":-1.95676, "alpha":0.00993, "fx":[-68.26369,-68.30718,-68.29194,-68.24845], "fy":[-32.01926,-31.92652,-31.95951,-32.05212]}, - {"t":0.20006, "x":7.02771, "y":6.19639, "heading":-2.35605, "vx":-0.83575, "vy":-0.39156, "omega":0.0018, "ax":-4.1759, "ay":-1.9565, "alpha":0.01044, "fx":[-68.25311,-68.29882,-68.28281,-68.23711], "fy":[-32.01645,-31.91899,-31.95368,-32.05099]}, - {"t":0.2334, "x":6.99752, "y":6.18225, "heading":-2.35599, "vx":-0.97499, "vy":-0.45679, "omega":0.00215, "ax":-4.17518, "ay":-1.95619, "alpha":0.01104, "fx":[-68.24058,-68.28891,-68.27201,-68.22368], "fy":[-32.01312,-31.91009,-31.94677,-32.04965]}, - {"t":0.26674, "x":6.96269, "y":6.16593, "heading":-2.35592, "vx":-1.1142, "vy":-0.52202, "omega":0.00252, "ax":-4.17432, "ay":-1.95581, "alpha":0.01176, "fx":[-68.22551,-68.277,-68.25901,-68.20752], "fy":[-32.00912,-31.89937,-31.93847,-32.04804]}, - {"t":0.30009, "x":6.92322, "y":6.14744, "heading":-2.35583, "vx":-1.25339, "vy":-0.58723, "omega":0.00291, "ax":-4.17327, "ay":-1.95535, "alpha":0.01265, "fx":[-68.20704,-68.2624,-68.24308,-68.18773], "fy":[-32.00422,-31.88624,-31.9283,-32.04607]}, - {"t":0.33343, "x":6.87911, "y":6.12677, "heading":-2.35574, "vx":-1.39254, "vy":-0.65243, "omega":0.00333, "ax":-4.17195, "ay":-1.95477, "alpha":0.01376, "fx":[-68.18386,-68.24408,-68.2231,-68.16289], "fy":[-31.99806,-31.86976,-31.91554,-32.04359]}, - {"t":0.36677, "x":6.83036, "y":6.10393, "heading":-2.35563, "vx":-1.53164, "vy":-0.71761, "omega":0.00379, "ax":-4.17025, "ay":-1.95402, "alpha":0.01519, "fx":[-68.15392,-68.22042,-68.19731,-68.13082], "fy":[-31.99011,-31.84848,-31.89907,-32.0404]}, - {"t":0.40011, "x":6.77697, "y":6.07892, "heading":-2.3555, "vx":-1.67069, "vy":-0.78276, "omega":0.0043, "ax":-4.16796, "ay":-1.95302, "alpha":0.01712, "fx":[-68.11375,-68.18868,-68.16273,-68.08782], "fy":[-31.97943,-31.81993,-31.87698,-32.03611]}, - {"t":0.43346, "x":6.71895, "y":6.05173, "heading":-2.35536, "vx":-1.80966, "vy":-0.84788, "omega":0.00487, "ax":-4.16474, "ay":-1.9516, "alpha":0.01984, "fx":[-68.05705,-68.14388,-68.11395,-68.02714], "fy":[-31.96437,-31.77963,-31.84583,-32.03006]}, - {"t":0.4668, "x":6.65629, "y":6.02238, "heading":-2.35519, "vx":-1.94852, "vy":-0.91295, "omega":0.00553, "ax":-4.15984, "ay":-1.94945, "alpha":0.02398, "fx":[-67.97094,-68.07586,-68.03996,-67.93508], "fy":[-31.94148,-31.71845,-31.79858,-32.02087]}, - {"t":0.50014, "x":6.58901, "y":5.99085, "heading":-2.35501, "vx":-2.08722, "vy":-0.97795, "omega":0.00633, "ax":-4.15152, "ay":-1.9458, "alpha":0.03103, "fx":[-67.82455,-67.96026,-67.91443,-67.77878], "fy":[-31.90255,-31.61452,-31.71843,-32.00523]}, - {"t":0.53348, "x":6.51711, "y":5.95717, "heading":-2.3548, "vx":-2.22565, "vy":-1.04283, "omega":0.00737, "ax":-4.13428, "ay":-1.93824, "alpha":0.0457, "fx":[-67.5205,-67.72031,-67.65469,-67.45502], "fy":[-31.82161,-31.39891,-31.55262,-31.97267]}, - {"t":0.56683, "x":6.4406, "y":5.92132, "heading":-2.35455, "vx":-2.3635, "vy":-1.10745, "omega":0.00889, "ax":-4.07711, "ay":-1.91312, "alpha":0.09507, "fx":[-66.507,-66.9221,-66.79863,-66.38426], "fy":[-31.55122,-30.68244,-31.00625,-31.86384]}, - {"t":0.60017, "x":6.35953, "y":5.88333, "heading":-2.35426, "vx":-2.49944, "vy":-1.17124, "omega":0.01206, "ax":3.61219, "ay":1.67604, "alpha":0.59454, "fx":[60.18933,57.70457,57.87735,60.43836], "fy":[26.12223,30.74728,28.89646,23.83423]}, - {"t":0.63351, "x":6.2782, "y":5.84521, "heading":-2.35385, "vx":-2.379, "vy":-1.11536, "omega":0.03188, "ax":4.10597, "ay":1.92116, "alpha":0.06392, "fx":[67.22021,66.94197,67.02907,67.30759], "fy":[31.22369,31.80561,31.5935,31.00648]}, - {"t":0.66686, "x":6.20116, "y":5.80909, "heading":-2.35279, "vx":-2.24209, "vy":-1.0513, "omega":0.03402, "ax":4.14198, "ay":1.93911, "alpha":0.03232, "fx":[67.76045,67.61947,67.66663,67.80767], "fy":[31.60625,31.90391,31.79608,31.4971]}, - {"t":0.7002, "x":6.12871, "y":5.77511, "heading":-2.35166, "vx":-2.10399, "vy":-0.98665, "omega":0.03509, "ax":4.15518, "ay":1.9457, "alpha":0.02083, "fx":[67.95919,67.86828,67.89949,67.99042], "fy":[31.74716,31.93979,31.87001,31.67683]}, - {"t":0.73354, "x":6.06086, "y":5.74329, "heading":-2.35049, "vx":-1.96544, "vy":-0.92177, "omega":0.03579, "ax":4.16203, "ay":1.94911, "alpha":0.01489, "fx":[68.06248,67.9975,68.02014,68.08513], "fy":[31.82043,31.95835,31.90829,31.77009]}, - {"t":0.76688, "x":5.99764, "y":5.71364, "heading":-2.34929, "vx":-1.82667, "vy":-0.85678, "omega":0.03628, "ax":4.16623, "ay":1.9512, "alpha":0.01125, "fx":[68.12576,68.07665,68.09394,68.14305], "fy":[31.86533,31.96968,31.93171,31.8272]}, - {"t":0.80023, "x":5.93905, "y":5.68616, "heading":-2.34808, "vx":-1.68776, "vy":-0.79172, "omega":0.03666, "ax":4.16906, "ay":1.95262, "alpha":0.0088, "fx":[68.16851,68.13011,68.14374,68.18214], "fy":[31.89566,31.97731,31.94752,31.86576]}, - {"t":0.83357, "x":5.8851, "y":5.66085, "heading":-2.34686, "vx":-1.54875, "vy":-0.72662, "omega":0.03695, "ax":4.1711, "ay":1.95363, "alpha":0.00703, "fx":[68.19932,68.16864,68.17961,68.21029], "fy":[31.91752,31.9828,31.9589,31.89356]}, - {"t":0.86691, "x":5.83577, "y":5.63771, "heading":-2.34563, "vx":-1.40967, "vy":-0.66148, "omega":0.03719, "ax":4.17263, "ay":1.9544, "alpha":0.0057, "fx":[68.22259,68.19774,68.20668,68.23153], "fy":[31.93403,31.98693,31.9675,31.91455]}, - {"t":0.90026, "x":5.79109, "y":5.61674, "heading":-2.34439, "vx":-1.27054, "vy":-0.59631, "omega":0.03738, "ax":4.17384, "ay":1.955, "alpha":0.00466, "fx":[68.24078,68.22048,68.22782,68.24813], "fy":[31.94693,31.99015,31.97422,31.93097]}, - {"t":0.9336, "x":5.75105, "y":5.59794, "heading":-2.34314, "vx":-1.13138, "vy":-0.53113, "omega":0.03753, "ax":0.05238, "ay":-0.09348, "alpha":1.49741, "fx":[6.57224,0.57467,-4.83962,1.1178], "fy":[-1.40773,4.45521,-1.65746,-7.50296]}, - {"t":0.96502, "x":5.71552, "y":5.58121, "heading":-2.34196, "vx":-1.12973, "vy":-0.53407, "omega":0.08458, "ax":0.01787, "ay":-0.03775, "alpha":1.37278, "fx":[5.52189,0.04856,-4.93597,0.53432], "fy":[-0.49098,4.86812,-0.7473,-6.09862]}, - {"t":0.99644, "x":5.68004, "y":5.56441, "heading":-2.33931, "vx":-1.12917, "vy":-0.53525, "omega":0.12772, "ax":0.00401, "ay":-0.00847, "alpha":1.25114, "fx":[4.82917,-0.17508,-4.69552,0.30395], "fy":[-0.00375,4.86175,-0.27407,-5.13758]}, - {"t":1.02786, "x":5.64456, "y":5.54758, "heading":-2.33529, "vx":-1.12904, "vy":-0.53552, "omega":0.16703, "ax":0.00059, "ay":-0.00124, "alpha":1.13272, "fx":[4.31864,-0.22756,-4.29777,0.24515], "fy":[0.12336,4.5082,-0.1641,-4.54854]}, - {"t":1.05928, "x":5.60908, "y":5.53076, "heading":-2.33004, "vx":-1.12902, "vy":-0.53556, "omega":0.20262, "ax":-0.00015, "ay":0.00032, "alpha":1.01731, "fx":[3.86339,-0.23564,-3.86766,0.23007], "fy":[0.15717,4.07396,-0.1468,-4.06359]}, - {"t":1.09071, "x":5.57361, "y":5.51393, "heading":-2.32368, "vx":-1.12903, "vy":-0.53555, "omega":0.23459, "ax":-0.00031, "ay":0.00066, "alpha":0.90457, "fx":[3.42794,-0.23321,-3.43816,0.223], "fy":[0.16923,3.63045,-0.14766,-3.60895]}, - {"t":1.12213, "x":5.53813, "y":5.4971, "heading":-2.31631, "vx":-1.12904, "vy":-0.53553, "omega":0.26301, "ax":-0.00039, "ay":0.00083, "alpha":0.79418, "fx":[3.00298,-0.22702,-3.01615,0.21457], "fy":[0.17535,3.19342,-0.14831,-3.16644]}, - {"t":1.15355, "x":5.50265, "y":5.48027, "heading":-2.30804, "vx":-1.12905, "vy":-0.5355, "omega":0.28796, "ax":-0.00041, "ay":0.00086, "alpha":0.6858, "fx":[2.58689,-0.21619,-2.60083,0.20333], "fy":[0.17516,2.76239,-0.1469,-2.73416]}, - {"t":1.18497, "x":5.46718, "y":5.46345, "heading":-2.29899, "vx":-1.12906, "vy":-0.53547, "omega":0.30951, "ax":-0.00008, "ay":0.00016, "alpha":0.57911, "fx":[2.1833,-0.19517,-2.18627,0.19313], "fy":[0.15774,2.32621,-0.15247,-2.32092]}, - {"t":1.21639, "x":5.4317, "y":5.44662, "heading":-2.28927, "vx":-1.12907, "vy":-0.53547, "omega":0.32771, "ax":0.00078, "ay":-0.00165, "alpha":0.47381, "fx":[1.79501,-0.16358,-1.76737,0.18714], "fy":[0.11628,1.87773,-0.17021,-1.93165]}, - {"t":1.24781, "x":5.39622, "y":5.4298, "heading":-2.27897, "vx":-1.12904, "vy":-0.53552, "omega":0.3426, "ax":-0.00109, "ay":0.0046, "alpha":0.37056, "fx":[1.41513,-0.3206,-1.34619,0.18036], "fy":[0.19979,1.56608,-0.04929,-1.41557]}, - {"t":1.27923, "x":5.36075, "y":5.41297, "heading":-2.26821, "vx":-1.12908, "vy":-0.53538, "omega":0.35424, "ax":1.63703, "ay":0.77583, "alpha":0.2267, "fx":[27.53843,26.66109,25.95436,26.89534], "fy":[12.72101,13.65064,12.65271,11.70923]}, - {"t":1.31066, "x":5.32608, "y":5.39653, "heading":-2.25708, "vx":-1.07764, "vy":-0.511, "omega":0.36136, "ax":1.79641, "ay":0.8519, "alpha":0.14028, "fx":[29.83813,29.31506,28.86792,29.45041], "fy":[13.95483,14.53003,13.90458,13.31815]}, - {"t":1.34208, "x":5.2931, "y":5.3809, "heading":-2.24572, "vx":-1.02119, "vy":-0.48423, "omega":0.36577, "ax":1.8016, "ay":0.85434, "alpha":0.06247, "fx":[29.66372,29.44902,29.22378,29.47448], "fy":[13.99472,14.23186,13.94242,13.69856]}, - {"t":1.3735, "x":5.26191, "y":5.3661, "heading":-2.23423, "vx":-0.96458, "vy":-0.45738, "omega":0.36773, "ax":1.80341, "ay":0.85519, "alpha":-0.0157, "fx":[29.43169,29.54313,29.51962,29.43451], "fy":[14.0026,13.90469,13.96141,14.05401]}, - {"t":1.40492, "x":5.23249, "y":5.35215, "heading":-2.22267, "vx":-0.90792, "vy":-0.43051, "omega":0.36724, "ax":1.80432, "ay":0.85561, "alpha":-0.09445, "fx":[29.18347,29.63266,29.79954,29.37326], "fy":[13.99707,13.56787,13.981,14.40455]}, - {"t":1.43634, "x":5.20485, "y":5.33905, "heading":-2.21113, "vx":-0.85122, "vy":-0.40363, "omega":0.36427, "ax":1.80488, "ay":0.85587, "alpha":-0.17389, "fx":[28.92765,29.72458,30.07323,29.29981], "fy":[13.98195,13.22527,14.00523,14.75469]}, - {"t":1.46776, "x":5.17899, "y":5.32679, "heading":-2.19969, "vx":-0.79451, "vy":-0.37674, "omega":0.35881, "ax":1.80525, "ay":0.85604, "alpha":-0.25415, "fx":[28.66697,29.82122,30.34421,29.21725], "fy":[13.95852,12.87801,14.03541,15.10624]}, - {"t":1.49918, "x":5.15492, "y":5.31537, "heading":-2.18841, "vx":-0.73779, "vy":-0.34984, "omega":0.35082, "ax":1.80552, "ay":0.85616, "alpha":-0.33534, "fx":[28.40242,29.92354,30.61423,29.12698], "fy":[13.9274,12.52633,14.07203,15.46025]}, - {"t":1.53061, "x":5.13263, "y":5.30481, "heading":-2.17739, "vx":-0.68106, "vy":-0.32294, "omega":0.34029, "ax":1.80572, "ay":0.85624, "alpha":-0.41757, "fx":[28.13428,30.03206,30.88432,29.02971], "fy":[13.88904,12.17025,14.11513,15.81742]}, - {"t":1.56203, "x":5.11212, "y":5.29508, "heading":-2.1667, "vx":-0.62432, "vy":-0.29603, "omega":0.32717, "ax":1.80588, "ay":0.85631, "alpha":-0.50095, "fx":[27.86255,30.1469,31.15523,28.92598], "fy":[13.8438,11.80952,14.16464,16.17839]}, - {"t":1.59345, "x":5.0934, "y":5.2862, "heading":-2.15642, "vx":-0.56757, "vy":-0.26913, "omega":0.31143, "ax":1.806, "ay":0.85637, "alpha":-0.58559, "fx":[27.587,30.2681,31.42761,28.81623], "fy":[13.79208,11.44382,14.22026,16.54375]}, - {"t":1.62487, "x":5.07645, "y":5.27817, "heading":-2.14663, "vx":-0.51083, "vy":-0.24222, "omega":0.29303, "ax":1.80611, "ay":0.85641, "alpha":-0.67163, "fx":[27.3073,30.39553,31.70202,28.70087], "fy":[13.73437,11.07275,14.28163,16.91406]}, - {"t":1.65629, "x":5.06129, "y":5.27098, "heading":-2.13743, "vx":-0.45408, "vy":-0.21531, "omega":0.27192, "ax":1.8062, "ay":0.85645, "alpha":-0.75916, "fx":[27.02301,30.52901,31.97904,28.58033], "fy":[13.67123,10.69588,14.34819,17.2899]}, - {"t":1.68771, "x":5.04792, "y":5.26464, "heading":-2.12888, "vx":-0.39732, "vy":-0.1884, "omega":0.24807, "ax":1.80627, "ay":0.85648, "alpha":-0.84832, "fx":[26.73359,30.66819,32.25928,28.45514], "fy":[13.60332,10.31264,14.41936,17.6719]}, - {"t":1.71913, "x":5.03633, "y":5.25914, "heading":-2.12109, "vx":-0.34057, "vy":-0.16149, "omega":0.22141, "ax":1.80633, "ay":0.85651, "alpha":-0.93923, "fx":[26.43843,30.8127,32.54334,28.32585], "fy":[13.53144,9.92242,14.49438,18.06071]}, - {"t":1.75056, "x":5.02652, "y":5.25449, "heading":-2.11413, "vx":-0.28381, "vy":-0.13457, "omega":0.1919, "ax":1.80639, "ay":0.85653, "alpha":-1.03201, "fx":[26.13683,30.96207,32.83191,28.1931], "fy":[13.45654,9.52455,14.57236,18.45699]}, - {"t":1.78198, "x":5.01849, "y":5.25068, "heading":-2.1081, "vx":-0.22705, "vy":-0.10766, "omega":0.15947, "ax":1.80643, "ay":0.85655, "alpha":-1.12681, "fx":[25.82798,31.1157,33.12571,28.05765], "fy":[13.37969,9.11826,14.6523,18.86148]}, - {"t":1.8134, "x":5.01225, "y":5.24772, "heading":-2.10309, "vx":-0.17029, "vy":-0.08075, "omega":0.12407, "ax":1.80648, "ay":0.85657, "alpha":-1.22375, "fx":[25.51101,31.2729,33.42555,27.92036], "fy":[13.30214,8.70268,14.73307,19.27497]}, - {"t":1.84482, "x":5.00779, "y":5.24561, "heading":-2.09919, "vx":-0.11353, "vy":-0.05383, "omega":0.08562, "ax":1.80651, "ay":0.85658, "alpha":-1.32299, "fx":[25.18491,31.43287,33.73227,27.78223], "fy":[13.22537,8.27686,14.81337,19.69828]}, - {"t":1.87624, "x":5.00511, "y":5.24434, "heading":-2.0965, "vx":-0.05676, "vy":-0.02692, "omega":0.04405, "ax":1.80655, "ay":0.8566, "alpha":-1.40176, "fx":[24.83872,31.57646,34.04764,27.67166], "fy":[13.20488,8.00309,14.85831,19.94854]}, - {"t":1.90766, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17853, "ay":-1.9575, "alpha":0.00521, "fx":[-68.30368,-68.32648,-68.31847,-68.29567], "fy":[-32.01702,-31.96839,-31.98566,-32.03425]}, + {"t":0.03379, "x":7.10893, "y":6.23445, "heading":-2.35619, "vx":-0.1412, "vy":-0.06615, "omega":0.00018, "ax":-4.17821, "ay":-1.95735, "alpha":0.00538, "fx":[-68.29818,-68.32175,-68.31347,-68.2899], "fy":[-32.01517,-31.96489,-31.98275,-32.033]}, + {"t":0.06758, "x":7.10177, "y":6.23109, "heading":-2.35619, "vx":-0.28239, "vy":-0.13229, "omega":0.00036, "ax":-4.17785, "ay":-1.95719, "alpha":0.00559, "fx":[-68.29194,-68.3164,-68.30781,-68.28335], "fy":[-32.01308,-31.96092,-31.97945,-32.03157]}, + {"t":0.10137, "x":7.08984, "y":6.22551, "heading":-2.35618, "vx":-0.42356, "vy":-0.19842, "omega":0.00055, "ax":-4.17743, "ay":-1.957, "alpha":0.00582, "fx":[-68.28481,-68.31028,-68.30134,-68.27588], "fy":[-32.01069,-31.95639,-31.97568,-32.02994]}, + {"t":0.13517, "x":7.07315, "y":6.21768, "heading":-2.35616, "vx":-0.56472, "vy":-0.26455, "omega":0.00074, "ax":-4.17695, "ay":-1.95678, "alpha":0.00608, "fx":[-68.27659,-68.30322,-68.29388,-68.26725], "fy":[-32.00793,-31.95115,-31.97134,-32.02807]}, + {"t":0.16896, "x":7.05168, "y":6.20763, "heading":-2.35613, "vx":-0.70587, "vy":-0.33068, "omega":0.00095, "ax":-4.17639, "ay":-1.95653, "alpha":0.00639, "fx":[-68.267,-68.29498,-68.28518,-68.2572], "fy":[-32.00471,-31.94505,-31.96627,-32.02588]}, + {"t":0.20275, "x":7.02544, "y":6.19534, "heading":-2.3561, "vx":-0.84699, "vy":-0.39679, "omega":0.00116, "ax":-4.17573, "ay":-1.95623, "alpha":0.00676, "fx":[-68.25567,-68.28526,-68.2749,-68.24531], "fy":[-32.00092,-31.93784,-31.96027,-32.02329]}, + {"t":0.23654, "x":6.99444, "y":6.18081, "heading":-2.35606, "vx":-0.9881, "vy":-0.46289, "omega":0.00139, "ax":-4.17494, "ay":-1.95587, "alpha":0.0072, "fx":[-68.24208,-68.27359,-68.26257,-68.23106], "fy":[-31.99636,-31.92918,-31.95309,-32.0202]}, + {"t":0.27033, "x":6.95866, "y":6.16405, "heading":-2.35601, "vx":-1.12917, "vy":-0.52898, "omega":0.00164, "ax":-4.17397, "ay":-1.95543, "alpha":0.00774, "fx":[-68.22547,-68.25935,-68.24752,-68.21365], "fy":[-31.99079,-31.91861,-31.94431,-32.01642]}, + {"t":0.30412, "x":6.91813, "y":6.14506, "heading":-2.35596, "vx":-1.27022, "vy":-0.59506, "omega":0.0019, "ax":-4.17276, "ay":-1.95488, "alpha":0.00841, "fx":[-68.20473,-68.24155,-68.22872,-68.19189], "fy":[-31.98385,-31.90539,-31.93334,-32.01171]}, + {"t":0.33791, "x":6.87282, "y":6.12384, "heading":-2.35589, "vx":-1.41122, "vy":-0.66112, "omega":0.00218, "ax":-4.17121, "ay":-1.95418, "alpha":0.00928, "fx":[-68.17807,-68.2187,-68.20457,-68.16395], "fy":[-31.97492,-31.8884,-31.91925,-32.00566]}, + {"t":0.37171, "x":6.82275, "y":6.10038, "heading":-2.35582, "vx":-1.55217, "vy":-0.72715, "omega":0.0025, "ax":-4.16914, "ay":-1.95324, "alpha":0.01044, "fx":[-68.14256,-68.18826,-68.17242,-68.12673], "fy":[-31.96304,-31.86576,-31.90049,-31.99763]}, + {"t":0.4055, "x":6.76792, "y":6.07469, "heading":-2.35574, "vx":-1.69305, "vy":-0.79316, "omega":0.00285, "ax":-4.16625, "ay":-1.95193, "alpha":0.01207, "fx":[-68.09291,-68.14573,-68.1275,-68.07469], "fy":[-31.94645,-31.83407,-31.87425,-31.98644]}, + {"t":0.43929, "x":6.70833, "y":6.04678, "heading":-2.35564, "vx":-1.83384, "vy":-0.85911, "omega":0.00326, "ax":-4.16192, "ay":-1.94997, "alpha":0.01453, "fx":[-68.01857,-68.08212,-68.06032,-67.99679], "fy":[-31.92166,-31.78656,-31.83495,-31.96978]}, + {"t":0.47308, "x":6.64399, "y":6.01663, "heading":-2.35553, "vx":-1.97447, "vy":-0.92501, "omega":0.00375, "ax":-4.15473, "ay":-1.94671, "alpha":0.01865, "fx":[-67.895,-67.97658,-67.94891,-67.86736], "fy":[-31.88059,-31.7074,-31.76964,-31.94238]}, + {"t":0.50687, "x":6.5749, "y":5.98427, "heading":-2.3554, "vx":-2.11487, "vy":-0.99079, "omega":0.00438, "ax":-4.14046, "ay":-1.94024, "alpha":0.02709, "fx":[-67.64906,-67.76748,-67.72818,-67.60982], "fy":[-31.79959,-31.54882,-31.63949,-31.88928]}, + {"t":0.54066, "x":6.50107, "y":5.94968, "heading":-2.35526, "vx":-2.25478, "vy":-1.05635, "omega":0.00529, "ax":-4.09842, "ay":-1.92123, "alpha":0.05556, "fx":[-66.91789,-67.1608,-67.08457,-66.84247], "fy":[-31.57129,-31.06019,-31.24828,-31.75425]}, + {"t":0.57445, "x":6.42254, "y":5.91288, "heading":-2.35508, "vx":-2.39327, "vy":-1.12127, "omega":0.00717, "ax":-1.55891, "ay":-0.75503, "alpha":1.69471, "fx":[-19.10513,-26.10422,-31.66581,-25.06558], "fy":[-12.8562,-5.34424,-11.94818,-19.22492]}, + {"t":0.60824, "x":6.34077, "y":5.87456, "heading":-2.35483, "vx":-2.44595, "vy":-1.14679, "omega":0.06444, "ax":4.09634, "ay":1.91743, "alpha":0.05401, "fx":[67.0479,66.81312,66.8864,67.12227], "fy":[31.19128,31.68256,31.50421,31.00736]}, + {"t":0.64204, "x":6.26046, "y":5.83691, "heading":-2.35266, "vx":-2.30753, "vy":-1.08199, "omega":0.06626, "ax":4.14018, "ay":1.93876, "alpha":0.02311, "fx":[67.7176,67.61678,67.6505,67.75137], "fy":[31.62739,31.8403,31.76307,31.54944]}, + {"t":0.67583, "x":6.18485, "y":5.80145, "heading":-2.35042, "vx":-2.16763, "vy":-1.01648, "omega":0.06704, "ax":4.15472, "ay":1.94581, "alpha":0.01441, "fx":[67.94239,67.87949,67.90117,67.96408], "fy":[31.76789,31.90119,31.85269,31.71912]}, + {"t":0.70962, "x":6.11397, "y":5.76822, "heading":-2.34815, "vx":-2.02723, "vy":-0.95073, "omega":0.06753, "ax":4.16198, "ay":1.94932, "alpha":0.0102, "fx":[68.05483,68.01035,68.02597,68.07046], "fy":[31.83774,31.93218,31.89765,31.80308]}, + {"t":0.74341, "x":6.04785, "y":5.7372, "heading":-2.34587, "vx":-1.88659, "vy":-0.88486, "omega":0.06787, "ax":4.16633, "ay":1.95143, "alpha":0.0077, "fx":[68.1223,68.08875,68.1007,68.13425], "fy":[31.87956,31.95087,31.92464,31.85327]}, + {"t":0.7772, "x":5.98648, "y":5.70842, "heading":-2.34358, "vx":-1.74581, "vy":-0.81892, "omega":0.06813, "ax":4.16922, "ay":1.95283, "alpha":0.00604, "fx":[68.16727,68.14097,68.15045,68.17675], "fy":[31.90741,31.96335,31.94265,31.88666]}, + {"t":0.81099, "x":5.92986, "y":5.68186, "heading":-2.34127, "vx":-1.60492, "vy":-0.75293, "omega":0.06834, "ax":4.17129, "ay":1.95383, "alpha":0.00485, "fx":[68.19939,68.17826,68.18595,68.20709], "fy":[31.92728,31.97226,31.95551,31.9105]}, + {"t":0.84478, "x":5.87801, "y":5.65753, "heading":-2.33897, "vx":-1.46397, "vy":-0.68691, "omega":0.0685, "ax":4.17284, "ay":1.95458, "alpha":0.00397, "fx":[68.22349,68.20622,68.21256,68.22983], "fy":[31.94217,31.97894,31.96516,31.92837]}, + {"t":0.87858, "x":5.83092, "y":5.63543, "heading":-2.33665, "vx":-1.32296, "vy":-0.62086, "omega":0.06864, "ax":4.17405, "ay":1.95516, "alpha":0.00328, "fx":[68.24222,68.22796,68.23325,68.24751], "fy":[31.95375,31.98413,31.97267,31.94227]}, + {"t":0.91237, "x":5.7886, "y":5.61557, "heading":-2.33433, "vx":-1.18192, "vy":-0.55479, "omega":0.06875, "ax":4.17501, "ay":1.95563, "alpha":0.00273, "fx":[68.25722,68.24535,68.24979,68.26165], "fy":[31.963,31.98828,31.97868,31.95339]}, + {"t":0.94616, "x":5.75105, "y":5.59794, "heading":-2.33201, "vx":-1.04084, "vy":-0.48871, "omega":0.06884, "ax":0.04202, "ay":-0.07239, "alpha":0.93694, "fx":[4.19677,0.45363,-2.79,0.88718], "fy":[-1.07029,2.63185,-1.29798,-4.9976]}, + {"t":0.97766, "x":5.71828, "y":5.58251, "heading":-2.32984, "vx":-1.03951, "vy":-0.49099, "omega":0.09835, "ax":0.02045, "ay":-0.04321, "alpha":0.86648, "fx":[3.56488,0.15184,-2.89367,0.51414], "fy":[-0.59039,2.82384,-0.82406,-4.2351]}, + {"t":1.00916, "x":5.68555, "y":5.56702, "heading":-2.32674, "vx":-1.03887, "vy":-0.49235, "omega":0.12565, "ax":0.00518, "ay":-0.01093, "alpha":0.7981, "fx":[3.05749,-0.09563,-2.88501,0.26217], "fy":[-0.05861,3.07465,-0.29951,-3.43157]}, + {"t":1.04066, "x":5.65283, "y":5.55151, "heading":-2.32278, "vx":-1.03871, "vy":-0.49269, "omega":0.15079, "ax":0.00079, "ay":-0.00167, "alpha":0.73168, "fx":[2.73474,-0.16382,-2.70671,0.18759], "fy":[0.0968,2.95724,-0.15163,-3.01164]}, + {"t":1.07216, "x":5.62011, "y":5.53599, "heading":-2.31803, "vx":-1.03868, "vy":-0.49275, "omega":0.17384, "ax":-0.00034, "ay":0.00071, "alpha":0.6671, "fx":[2.47224,-0.17794,-2.48218,0.16588], "fy":[0.13903,2.73466,-0.11591,-2.71141]}, + {"t":1.10366, "x":5.58739, "y":5.52047, "heading":-2.31256, "vx":-1.03869, "vy":-0.49272, "omega":0.19485, "ax":-0.00063, "ay":0.00133, "alpha":0.60419, "fx":[2.23004,-0.17849,-2.25005,0.15726], "fy":[0.1514,2.49,-0.10794,-2.44653]}, + {"t":1.13516, "x":5.55467, "y":5.50495, "heading":-2.30642, "vx":-1.03871, "vy":-0.49268, "omega":0.21388, "ax":-0.00093, "ay":0.00196, "alpha":0.54278, "fx":[1.99356,-0.17814,-2.02359,0.14734], "fy":[0.16243,2.25152,-0.09831,-2.18742]}, + {"t":1.16666, "x":5.52195, "y":5.48943, "heading":-2.29968, "vx":-1.03874, "vy":-0.49262, "omega":0.23098, "ax":-0.00123, "ay":0.0026, "alpha":0.48269, "fx":[1.76272,-0.178,-1.80161,0.13634], "fy":[0.17162,2.01847,-0.08671,-1.93357]}, + {"t":1.19816, "x":5.48923, "y":5.47391, "heading":-2.29241, "vx":-1.03878, "vy":-0.49254, "omega":0.24618, "ax":-0.00019, "ay":0.0004, "alpha":0.42377, "fx":[1.55897,-0.15619,-1.56129,0.14604], "fy":[0.13223,1.74382,-0.1192,-1.73073]}, + {"t":1.22966, "x":5.4565, "y":5.4584, "heading":-2.28465, "vx":-1.03879, "vy":-0.49253, "omega":0.25953, "ax":0.01067, "ay":-0.02223, "alpha":0.366, "fx":[1.5288,0.00027,-1.1546,0.32339], "fy":[-0.24376,1.13922,-0.48323,-1.86598]}, + {"t":1.26116, "x":5.42379, "y":5.44287, "heading":-2.27648, "vx":-1.03845, "vy":-0.49323, "omega":0.27106, "ax":0.4775, "ay":0.25378, "alpha":0.30245, "fx":[8.9563,7.48784,6.78051,8.00041], "fy":[4.24571,5.40027,4.05161,2.898]}, + {"t":1.29266, "x":5.39131, "y":5.42746, "heading":-2.26794, "vx":-1.02341, "vy":-0.48523, "omega":0.28059, "ax":1.34486, "ay":0.63723, "alpha":0.2277, "fx":[22.75287,21.97093,21.14287,22.07725], "fy":[10.4761,11.38538,10.36327,9.44511]}, + {"t":1.32416, "x":5.35974, "y":5.41249, "heading":-2.2591, "vx":-0.98104, "vy":-0.46516, "omega":0.28776, "ax":1.35012, "ay":0.64, "alpha":0.17795, "fx":[22.67537,22.02869,21.42819,22.155], "fy":[10.51629,11.22252,10.41201,9.70032]}, + {"t":1.35566, "x":5.32951, "y":5.39816, "heading":-2.25004, "vx":-0.93852, "vy":-0.445, "omega":0.29337, "ax":1.35186, "ay":0.64091, "alpha":0.12911, "fx":[22.53581,22.06655,21.63821,22.16083], "fy":[10.5247,11.03191,10.43238,9.92175]}, + {"t":1.38716, "x":5.30062, "y":5.38446, "heading":-2.24079, "vx":-0.89593, "vy":-0.42481, "omega":0.29743, "ax":1.35273, "ay":0.64136, "alpha":0.0804, "fx":[22.38055,22.10293,21.82875,22.14614], "fy":[10.52289,10.83395,10.44845,10.13501]}, + {"t":1.41866, "x":5.27307, "y":5.37139, "heading":-2.23142, "vx":-0.85332, "vy":-0.40461, "omega":0.29997, "ax":1.35326, "ay":0.64163, "alpha":0.03159, "fx":[22.21868,22.14084,22.01162,22.12142], "fy":[10.51488,10.63253,10.465,10.34544]}, + {"t":1.45016, "x":5.24686, "y":5.35897, "heading":-2.22198, "vx":-0.81069, "vy":-0.3844, "omega":0.30096, "ax":1.3536, "ay":0.64181, "alpha":-0.01746, "fx":[22.05308,22.18123,22.19096,22.09011], "fy":[10.50198,10.42878,10.48365,10.55504]}, + {"t":1.48166, "x":5.22199, "y":5.34718, "heading":-2.2125, "vx":-0.76805, "vy":-0.36418, "omega":0.30041, "ax":1.35385, "ay":0.64193, "alpha":-0.06686, "fx":[21.88479,22.22458,22.36869,22.05361], "fy":[10.48471,10.22302,10.50509,10.76483]}, + {"t":1.51316, "x":5.19847, "y":5.33602, "heading":-2.20303, "vx":-0.72541, "vy":-0.34396, "omega":0.29831, "ax":1.35404, "ay":0.64203, "alpha":-0.11671, "fx":[21.71426,22.27117,22.54584,22.01266], "fy":[10.46331,10.01517,10.52973,10.97551]}, + {"t":1.54466, "x":5.17629, "y":5.32551, "heading":-2.19364, "vx":-0.68275, "vy":-0.32373, "omega":0.29463, "ax":1.35419, "ay":0.6421, "alpha":-0.1671, "fx":[21.54157,22.32107,22.72336,21.96748], "fy":[10.43794,9.80516,10.55769,11.18762]}, + {"t":1.57616, "x":5.15546, "y":5.31563, "heading":-2.18435, "vx":-0.6401, "vy":-0.30351, "omega":0.28937, "ax":1.3543, "ay":0.64216, "alpha":-0.21814, "fx":[21.36645,22.37453,22.90166,21.91849], "fy":[10.40871,9.59268,10.58912,11.40161]}, + {"t":1.60766, "x":5.13597, "y":5.30639, "heading":-2.17524, "vx":-0.59744, "vy":-0.28328, "omega":0.2825, "ax":1.3544, "ay":0.6422, "alpha":-0.2699, "fx":[21.189,22.43151,23.08118,21.8657], "fy":[10.37565,9.37749,10.62407,11.61791]}, + {"t":1.63916, "x":5.11782, "y":5.29778, "heading":-2.16634, "vx":-0.55477, "vy":-0.26305, "omega":0.27399, "ax":1.35448, "ay":0.64224, "alpha":-0.32247, "fx":[21.00891,22.49211,23.26236,21.80925], "fy":[10.33888,9.15933,10.66249,11.8369]}, + {"t":1.67066, "x":5.10101, "y":5.28981, "heading":-2.15771, "vx":-0.51211, "vy":-0.24282, "omega":0.26384, "ax":1.35455, "ay":0.64227, "alpha":-0.37596, "fx":[20.82592,22.55633,23.44559,21.74923], "fy":[10.29852,8.93784,10.70431,12.059]}, + {"t":1.70216, "x":5.08556, "y":5.28248, "heading":-2.1494, "vx":-0.46944, "vy":-0.22259, "omega":0.25199, "ax":1.35461, "ay":0.6423, "alpha":-0.43045, "fx":[20.63971,22.62415,23.63125,21.68576], "fy":[10.25468,8.71266,10.74948,12.28463]}, + {"t":1.73366, "x":5.07144, "y":5.27579, "heading":-2.14146, "vx":-0.42677, "vy":-0.20235, "omega":0.23843, "ax":1.35466, "ay":0.64232, "alpha":-0.48604, "fx":[20.44996,22.69551,23.81975,21.61896], "fy":[10.20751,8.4834,10.79784,12.51421]}, + {"t":1.76516, "x":5.05867, "y":5.26973, "heading":-2.13395, "vx":-0.3841, "vy":-0.18212, "omega":0.22312, "ax":1.3547, "ay":0.64234, "alpha":-0.54283, "fx":[20.25629,22.77037,24.01145,21.54896], "fy":[10.15723,8.24968,10.8492,12.74817]}, + {"t":1.79666, "x":5.04724, "y":5.26432, "heading":-2.12692, "vx":-0.34142, "vy":-0.16189, "omega":0.20602, "ax":1.35474, "ay":0.64236, "alpha":-0.60092, "fx":[20.0583,22.84863,24.20677,21.47592], "fy":[10.10408,8.01105,10.90334,12.98698]}, + {"t":1.82816, "x":5.03716, "y":5.25954, "heading":-2.12043, "vx":-0.29875, "vy":-0.14165, "omega":0.18709, "ax":1.35477, "ay":0.64238, "alpha":-0.66042, "fx":[19.85555,22.93013,24.40617,21.40005], "fy":[10.04831,7.76701,10.96002,13.23113]}, + {"t":1.85967, "x":5.02842, "y":5.25539, "heading":-2.11454, "vx":-0.25607, "vy":-0.12142, "omega":0.16629, "ax":1.35481, "ay":0.64239, "alpha":-0.72143, "fx":[19.64755,23.01471,24.61009,21.32158], "fy":[9.99027,7.51707,11.01889,13.48114]}, + {"t":1.89117, "x":5.02103, "y":5.25189, "heading":-2.1093, "vx":-0.2134, "vy":-0.10118, "omega":0.14357, "ax":1.35483, "ay":0.6424, "alpha":-0.78406, "fx":[19.43377,23.10216,24.81904,21.24081], "fy":[9.93036,7.26067,11.07959,13.73758]}, + {"t":1.92267, "x":5.01498, "y":5.24902, "heading":-2.10478, "vx":-0.17072, "vy":-0.08095, "omega":0.11887, "ax":1.35486, "ay":0.64241, "alpha":-0.84843, "fx":[19.21364,23.19221,25.03353,21.15804], "fy":[9.86904,6.99721,11.14169,14.001]}, + {"t":1.95417, "x":5.01027, "y":5.24679, "heading":-2.10103, "vx":-0.12804, "vy":-0.06071, "omega":0.09214, "ax":1.35488, "ay":0.64242, "alpha":-0.91466, "fx":[18.98657,23.28456,25.25412,21.07369], "fy":[9.80683,6.72605,11.20468,14.27206]}, + {"t":1.98567, "x":5.00691, "y":5.24519, "heading":-2.09813, "vx":-0.08536, "vy":-0.04047, "omega":0.06333, "ax":1.3549, "ay":0.64243, "alpha":-0.98289, "fx":[18.75183,23.37884,25.48146,20.98818], "fy":[9.74439,6.44649,11.26796,14.55139]}, + {"t":2.01717, "x":5.00489, "y":5.24424, "heading":-2.09614, "vx":-0.04268, "vy":-0.02024, "omega":0.03237, "ax":1.35492, "ay":0.64244, "alpha":-1.02758, "fx":[18.5059,23.46284,25.71728,20.91556], "fy":[9.72826,6.35308,11.28328,14.64617]}, + {"t":2.04867, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLMtoA.traj b/src/main/deploy/choreo/PLMtoA.traj index bc5527c4..1d78e8d1 100644 --- a/src/main/deploy/choreo/PLMtoA.traj +++ b/src/main/deploy/choreo/PLMtoA.traj @@ -11,8 +11,9 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -25,8 +26,9 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"slowaccel", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -34,94 +36,94 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.37594,1.84423], + "waypoints":[0.0,1.42603,2.20774], "samples":[ - {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.84746, "ay":-3.56868, "alpha":1.97071, "fx":[56.00692,37.39941,39.22004,53.57608], "fy":[-50.44518,-65.4405,-64.40864,-53.07005]}, - {"t":0.02293, "x":1.03088, "y":6.97847, "heading":-0.93501, "vx":0.0653, "vy":-0.08184, "omega":0.04519, "ax":2.8474, "ay":-3.56872, "alpha":1.96587, "fx":[55.98103,37.42306,39.23404,53.56042], "fy":[-50.46756,-65.42203,-64.39619,-53.08112]}, - {"t":0.04586, "x":1.03313, "y":6.97566, "heading":-0.93397, "vx":0.1306, "vy":-0.16368, "omega":0.09028, "ax":2.84735, "ay":-3.56875, "alpha":1.9608, "fx":[55.94744,37.43877,39.25742,53.55129], "fy":[-50.49805,-65.40783,-64.37779,-53.08531]}, - {"t":0.0688, "x":1.03687, "y":6.97097, "heading":-0.9319, "vx":0.19589, "vy":-0.24552, "omega":0.13524, "ax":2.8473, "ay":-3.56877, "alpha":1.95546, "fx":[55.90602,37.44682,39.29019,53.5485], "fy":[-50.53671,-65.39775,-64.35342,-53.08277]}, - {"t":0.09173, "x":1.04211, "y":6.9644, "heading":-0.9288, "vx":0.26119, "vy":-0.32736, "omega":0.18008, "ax":2.84725, "ay":-3.56879, "alpha":1.94982, "fx":[55.85659,37.44753,39.33237,53.55187], "fy":[-50.58367,-65.39157,-64.32301,-53.07367]}, - {"t":0.11466, "x":1.04885, "y":6.95595, "heading":-0.92467, "vx":0.32648, "vy":-0.4092, "omega":0.2248, "ax":2.8472, "ay":-3.56881, "alpha":1.94385, "fx":[55.79892,37.4413,39.38398,53.56115], "fy":[-50.63909,-65.38904,-64.28651,-53.05821]}, - {"t":0.13759, "x":1.05709, "y":6.94563, "heading":-0.91951, "vx":0.39178, "vy":-0.49104, "omega":0.26938, "ax":2.84716, "ay":-3.56882, "alpha":1.9375, "fx":[55.73274,37.42861,39.44505,53.57608], "fy":[-50.70317,-65.38984,-64.24382,-53.03662]}, - {"t":0.16053, "x":1.06682, "y":6.93343, "heading":-0.91334, "vx":0.45707, "vy":-0.57288, "omega":0.31381, "ax":2.84712, "ay":-3.56882, "alpha":1.9307, "fx":[55.65769,37.41001,39.51564,53.59632], "fy":[-50.77617,-65.39364,-64.19485,-53.00917]}, - {"t":0.18346, "x":1.07805, "y":6.91936, "heading":-0.90614, "vx":0.52236, "vy":-0.65472, "omega":0.35808, "ax":2.84707, "ay":-3.56883, "alpha":1.92341, "fx":[55.57339,37.38614,39.59582,53.62151], "fy":[-50.85836,-65.40001,-64.13946,-52.97617]}, - {"t":0.20639, "x":1.09078, "y":6.9034, "heading":-0.89793, "vx":0.58765, "vy":-0.73657, "omega":0.40219, "ax":2.84703, "ay":-3.56883, "alpha":1.91555, "fx":[55.4794,37.35771,39.68567,53.65124], "fy":[-50.95007,-65.40848,-64.07752,-52.93796]}, - {"t":0.22932, "x":1.105, "y":6.88557, "heading":-0.8887, "vx":0.65294, "vy":-0.81841, "omega":0.44612, "ax":2.84698, "ay":-3.56883, "alpha":1.90705, "fx":[55.3752,37.32556,39.78526,53.68502], "fy":[-51.05167,-65.41854,-64.00886,-52.89495]}, - {"t":0.25226, "x":1.12072, "y":6.86587, "heading":-0.87847, "vx":0.71823, "vy":-0.90025, "omega":0.48985, "ax":2.84693, "ay":-3.56883, "alpha":1.8978, "fx":[55.26022,37.29059,39.89469,53.7223], "fy":[-51.16356,-65.42958,-63.9333,-52.84759]}, - {"t":0.27519, "x":1.13794, "y":6.84428, "heading":-0.86724, "vx":0.78351, "vy":-0.98209, "omega":0.53337, "ax":2.84688, "ay":-3.56883, "alpha":1.88771, "fx":[55.13384,37.25386,40.01407,53.76246], "fy":[-51.28615,-65.44093,-63.85064,-52.79643]}, - {"t":0.29812, "x":1.15666, "y":6.82082, "heading":-0.85501, "vx":0.8488, "vy":-1.06393, "omega":0.57666, "ax":2.84682, "ay":-3.56883, "alpha":1.87667, "fx":[54.99535,37.21651,40.1435,53.80477], "fy":[-51.41992,-65.45186,-63.76063,-52.74207]}, - {"t":0.32105, "x":1.17687, "y":6.79549, "heading":-0.84178, "vx":0.91408, "vy":-1.14577, "omega":0.6197, "ax":2.84674, "ay":-3.56884, "alpha":1.86455, "fx":[54.84398,37.17986,40.28308,53.84839], "fy":[-51.56535,-65.4615,-63.66304,-52.68522]}, - {"t":0.34399, "x":1.19858, "y":6.76827, "heading":-0.82757, "vx":0.97937, "vy":-1.22762, "omega":0.66246, "ax":2.84666, "ay":-3.56886, "alpha":1.85118, "fx":[54.67889,37.14536,40.43293,53.89237], "fy":[-51.72294,-65.46893,-63.55757,-52.62671]}, - {"t":0.36692, "x":1.22179, "y":6.73918, "heading":-0.81238, "vx":1.04465, "vy":-1.30946, "omega":0.70491, "ax":2.84655, "ay":-3.56888, "alpha":1.83641, "fx":[54.49914,37.11467,40.59317,53.93558], "fy":[-51.89323,-65.47305,-63.44391,-52.56751]}, - {"t":0.38985, "x":1.2465, "y":6.70821, "heading":-0.79622, "vx":1.10993, "vy":-1.3913, "omega":0.74702, "ax":2.84642, "ay":-3.56892, "alpha":1.82003, "fx":[54.30371,37.08966,40.7639,53.9767], "fy":[-52.07677,-65.47265,-63.32169,-52.50875]}, - {"t":0.41278, "x":1.2727, "y":6.67537, "heading":-0.77908, "vx":1.1752, "vy":-1.47315, "omega":0.78876, "ax":2.84625, "ay":-3.56896, "alpha":1.80181, "fx":[54.09147,37.07246,40.94524,54.01418], "fy":[-52.27412,-65.46632,-63.19051,-52.45179]}, - {"t":0.43572, "x":1.3004, "y":6.64065, "heading":-0.761, "vx":1.24047, "vy":-1.55499, "omega":0.83008, "ax":2.84605, "ay":-3.56901, "alpha":1.78147, "fx":[53.86116,37.06553,41.1373,54.04616], "fy":[-52.48584,-65.45241,-63.04991,-52.39822]}, - {"t":0.45865, "x":1.32959, "y":6.60405, "heading":-0.74196, "vx":1.30574, "vy":-1.63684, "omega":0.87094, "ax":2.8458, "ay":-3.56908, "alpha":1.75869, "fx":[53.61137,37.0717,41.34017,54.07037], "fy":[-52.71252,-65.42898,-62.89933,-52.35]}, - {"t":0.48158, "x":1.36028, "y":6.56558, "heading":-0.72199, "vx":1.371, "vy":-1.71868, "omega":0.91127, "ax":2.84548, "ay":-3.56916, "alpha":1.73304, "fx":[53.34053,37.09436,41.55398,54.08406], "fy":[-52.95474,-65.39371,-62.73814,-52.3095]}, - {"t":0.50451, "x":1.39247, "y":6.52522, "heading":-0.70109, "vx":1.43625, "vy":-1.80053, "omega":0.95101, "ax":2.84509, "ay":-3.56926, "alpha":1.70402, "fx":[53.04681,37.13751,41.77884,54.08378], "fy":[-53.21311,-65.34377,-62.56556,-52.27965]}, - {"t":0.52744, "x":1.42616, "y":6.48299, "heading":-0.67928, "vx":1.5015, "vy":-1.88238, "omega":0.99009, "ax":2.84459, "ay":-3.56935, "alpha":1.671, "fx":[52.7281,37.20607,42.01487,54.06515], "fy":[-53.48822,-65.27561,-62.38063,-52.26416]}, - {"t":0.55038, "x":1.46134, "y":6.43889, "heading":-0.65658, "vx":1.56673, "vy":-1.96424, "omega":1.02841, "ax":2.84395, "ay":-3.56946, "alpha":1.63313, "fx":[52.38187,37.30615,42.26224,54.02251], "fy":[-53.78073,-65.18472,-62.18211,-52.26774]}, - {"t":0.57331, "x":1.49802, "y":6.39291, "heading":-0.63299, "vx":1.63195, "vy":-2.04609, "omega":1.06586, "ax":2.84315, "ay":-3.56955, "alpha":1.58928, "fx":[52.005,37.44558,42.52113,53.94835], "fy":[-54.09135,-65.06517,-61.96839,-52.29658]}, - {"t":0.59624, "x":1.53619, "y":6.34504, "heading":-0.60855, "vx":1.69715, "vy":-2.12795, "omega":1.1023, "ax":2.84211, "ay":-3.56962, "alpha":1.53791, "fx":[51.59357,37.63464,42.79184,53.83241], "fy":[-54.42086,-64.90895,-61.73729,-52.35896]}, - {"t":0.61917, "x":1.57585, "y":6.29531, "heading":-0.58327, "vx":1.76233, "vy":-2.20981, "omega":1.13757, "ax":2.84077, "ay":-3.56964, "alpha":1.47683, "fx":[51.14238,37.88731,43.07478,53.66023], "fy":[-54.77026,-64.70484,-61.48569,-52.46628]}, - {"t":0.64211, "x":1.61702, "y":6.24369, "heading":-0.55718, "vx":1.82747, "vy":-2.29167, "omega":1.17144, "ax":2.839, "ay":-3.56955, "alpha":1.40282, "fx":[50.64431,38.2233,43.37063,53.41067], "fy":[-55.14086,-64.43653,-61.20902,-52.63482]}, - {"t":0.66504, "x":1.65967, "y":6.1902, "heading":-0.53032, "vx":1.89258, "vy":-2.37353, "omega":1.20361, "ax":2.83661, "ay":-3.56926, "alpha":1.31095, "fx":[50.08907,38.67164,43.6805,53.05132], "fy":[-55.5345,-64.07916,-60.90019,-52.88874]}, - {"t":0.68797, "x":1.70382, "y":6.13483, "heading":-0.50272, "vx":1.95763, "vy":-2.45538, "omega":1.23367, "ax":2.83327, "ay":-3.56861, "alpha":1.19324, "fx":[49.461,39.27729,44.00632,52.52963], "fy":[-55.95404,-63.59266,-60.54748,-53.2657]}, - {"t":0.7109, "x":1.74946, "y":6.07758, "heading":-0.47443, "vx":2.0226, "vy":-2.53722, "omega":1.26104, "ax":2.82838, "ay":-3.56723, "alpha":1.03598, "fx":[48.73468,40.11432,44.35161,51.75411], "fy":[-56.4041,-62.90776,-60.13006,-53.82806]}, - {"t":0.73384, "x":1.79658, "y":6.01846, "heading":-0.44551, "vx":2.08746, "vy":-2.61903, "omega":1.28479, "ax":2.82071, "ay":-3.56433, "alpha":0.81345, "fx":[47.86556,41.31437,44.72329,50.54982], "fy":[-56.89267,-61.8935,-59.60696,-54.68721]}, - {"t":0.75677, "x":1.84519, "y":5.95746, "heading":-0.41605, "vx":2.15215, "vy":-2.70076, "omega":1.30345, "ax":2.80729, "ay":-3.55778, "alpha":0.47144, "fx":[46.76749,43.1358,45.13627,48.53575], "fy":[-57.43441,-60.26955,-58.88517,-56.06235]}, - {"t":0.7797, "x":1.89529, "y":5.89459, "heading":-0.38616, "vx":2.21653, "vy":-2.78235, "omega":1.31426, "ax":2.77921, "ay":-3.54041, "alpha":-0.12581, "fx":[45.24959,46.15594,45.62785,44.70603], "fy":[-58.05767,-57.31781,-57.70033,-58.44002]}, - {"t":0.80263, "x":1.94685, "y":5.82986, "heading":-0.35602, "vx":2.28026, "vy":-2.86354, "omega":1.31137, "ax":2.69785, "ay":-3.47607, "alpha":-1.42739, "fx":[42.79261,51.878,46.3071,35.44099], "fy":[-58.81331,-50.59895,-54.90841,-62.98769]}, - {"t":0.82557, "x":1.99985, "y":5.76327, "heading":-0.32594, "vx":2.34213, "vy":-2.94326, "omega":1.27864, "ax":2.25038, "ay":-2.89227, "alpha":-5.93088, "fx":[37.43235,62.87124,45.27871,1.57551], "fy":[-59.51265,-28.1726,-33.56796,-67.87898]}, - {"t":0.8485, "x":2.05415, "y":5.69502, "heading":-0.29662, "vx":2.39373, "vy":-3.00958, "omega":1.14263, "ax":-2.20073, "ay":1.89305, "alpha":-4.66547, "fx":[-36.40407,-16.26707,-38.63591,-52.60404], "fy":[14.43765,47.79963,43.4055,18.1482]}, - {"t":0.87143, "x":2.10847, "y":5.6265, "heading":-0.27042, "vx":2.34327, "vy":-2.96617, "omega":1.03564, "ax":-2.77421, "ay":3.28603, "alpha":-2.29399, "fx":[-48.36229,-31.59466,-44.36483,-57.09052], "fy":[50.81218,63.9323,56.8864,43.25012]}, - {"t":0.89436, "x":2.16147, "y":5.55934, "heading":-0.24667, "vx":2.27965, "vy":-2.89081, "omega":0.98303, "ax":-2.81317, "ay":3.41575, "alpha":-2.13892, "fx":[-48.6441,-32.74653,-45.10485,-57.46466], "fy":[53.95053,65.52683,58.24959,45.63682]}, - {"t":0.9173, "x":2.21301, "y":5.49395, "heading":-0.22412, "vx":2.21513, "vy":-2.81248, "omega":0.93398, "ax":-2.82683, "ay":3.46391, "alpha":-2.09048, "fx":[-48.54228,-33.1184,-45.51352,-57.67914], "fy":[55.26291,66.14958,58.66764,46.43348]}, - {"t":0.94023, "x":2.26307, "y":5.43036, "heading":-0.20271, "vx":2.15031, "vy":-2.73305, "omega":0.88604, "ax":-2.83369, "ay":3.48902, "alpha":-2.06782, "fx":[-48.34239,-33.30337,-45.82405,-57.83211], "fy":[56.06472,66.47878,58.81242,46.79952]}, - {"t":0.96316, "x":2.31163, "y":5.3686, "heading":-0.18239, "vx":2.08533, "vy":-2.65304, "omega":0.83862, "ax":-2.83777, "ay":3.50444, "alpha":-2.05486, "fx":[-48.10822,-33.42151,-46.08798,-57.95074], "fy":[56.64692,66.67826,58.84433,46.99438]}, - {"t":0.98609, "x":2.35871, "y":5.30868, "heading":-0.16316, "vx":2.02025, "vy":-2.57267, "omega":0.7915, "ax":-2.84044, "ay":3.51489, "alpha":-2.04653, "fx":[-47.86297,-33.51028,-46.32294,-58.04685], "fy":[57.11027,66.80841,58.82131,47.10698]}, - {"t":1.00903, "x":2.40429, "y":5.25061, "heading":-0.145, "vx":1.95511, "vy":-2.49207, "omega":0.74457, "ax":-2.8423, "ay":3.52244, "alpha":-2.04078, "fx":[-47.61743,-33.58424,-46.53652,-58.12671], "fy":[57.49882,66.89707,58.76943,47.1755]}, - {"t":1.03196, "x":2.44838, "y":5.19439, "heading":-0.12793, "vx":1.88993, "vy":-2.41129, "omega":0.69777, "ax":-2.84366, "ay":3.52816, "alpha":-2.03666, "fx":[-47.37741,-33.64977,-46.7325,-58.19411], "fy":[57.83485,66.95898,58.70221,47.21873]}, - {"t":1.05489, "x":2.49097, "y":5.14002, "heading":-0.11193, "vx":1.82472, "vy":-2.33038, "omega":0.65106, "ax":-2.84469, "ay":3.53264, "alpha":-2.03363, "fx":[-47.1464,-33.70985,-46.91301,-58.25155], "fy":[58.13091,67.00277,58.62738,47.24687]}, - {"t":1.07782, "x":2.53207, "y":5.08751, "heading":-0.097, "vx":1.75948, "vy":-2.24937, "omega":0.60443, "ax":-2.84548, "ay":3.53625, "alpha":-2.03139, "fx":[-46.92665,-33.76587,-47.07933,-58.30082], "fy":[58.39459,67.03388,58.54969,47.26578]}, - {"t":1.10075, "x":2.57167, "y":5.03685, "heading":-0.08314, "vx":1.69423, "vy":-2.16827, "omega":0.55784, "ax":-2.84611, "ay":3.53922, "alpha":-2.02973, "fx":[-46.7197,-33.81845,-47.23228,-58.34323], "fy":[58.63081,67.05596,58.47224,47.27901]}, - {"t":1.12369, "x":2.60977, "y":4.98806, "heading":-0.07034, "vx":1.62896, "vy":-2.08711, "omega":0.5113, "ax":-2.84661, "ay":3.5417, "alpha":-2.02853, "fx":[-46.52662,-33.8678,-47.37243,-58.37983], "fy":[58.84291,67.07155,58.39713,47.28876]}, - {"t":1.14662, "x":2.64638, "y":4.94113, "heading":-0.05862, "vx":1.56368, "vy":-2.00589, "omega":0.46478, "ax":-2.84702, "ay":3.54381, "alpha":-2.02768, "fx":[-46.34821,-33.91389,-47.50019,-58.41143], "fy":[59.03326,67.08251,58.32585,47.29644]}, - {"t":1.16955, "x":2.68149, "y":4.89606, "heading":-0.04796, "vx":1.49839, "vy":-1.92462, "omega":0.41828, "ax":-2.84737, "ay":3.54562, "alpha":-2.02709, "fx":[-46.18504,-33.9566,-47.61585,-58.43875], "fy":[59.2036,67.09024,58.25949,47.30294]}, - {"t":1.19248, "x":2.71511, "y":4.85286, "heading":-0.03837, "vx":1.43309, "vy":-1.84331, "omega":0.37179, "ax":-2.84766, "ay":3.54718, "alpha":-2.02672, "fx":[-46.03756,-33.99574,-47.71964,-58.46235], "fy":[59.35527,67.0958,58.19885,47.30882]}, - {"t":1.21542, "x":2.74722, "y":4.81152, "heading":-0.02984, "vx":1.36779, "vy":-1.76197, "omega":0.32531, "ax":-2.84791, "ay":3.54855, "alpha":-2.02651, "fx":[-45.90609,-34.03111,-47.81175,-58.48273], "fy":[59.48928,67.10004,58.14457,47.31445]}, - {"t":1.23835, "x":2.77784, "y":4.77205, "heading":-0.02238, "vx":1.30248, "vy":-1.68059, "omega":0.27884, "ax":-2.84813, "ay":3.54976, "alpha":-2.02641, "fx":[-45.79091,-34.06251,-47.89233,-58.5003], "fy":[59.60644,67.10364,58.09711,47.32005]}, - {"t":1.26128, "x":2.80696, "y":4.73444, "heading":-0.01599, "vx":1.23717, "vy":-1.59919, "omega":0.23237, "ax":-2.84833, "ay":3.55083, "alpha":-2.02641, "fx":[-45.6922,-34.08973,-47.96149,-58.51542], "fy":[59.70743,67.10713,58.05685,47.32576]}, - {"t":1.28421, "x":2.83458, "y":4.6987, "heading":-0.01066, "vx":1.17185, "vy":-1.51776, "omega":0.1859, "ax":-2.8485, "ay":3.55178, "alpha":-2.02646, "fx":[-45.61012,-34.1126,-48.01932,-58.5284], "fy":[59.79276,67.11095,58.0241,47.33167]}, - {"t":1.30715, "x":2.86071, "y":4.66483, "heading":-0.0064, "vx":1.10653, "vy":-1.43631, "omega":0.13943, "ax":-2.84867, "ay":3.55264, "alpha":-2.02656, "fx":[-45.54479,-34.13097,-48.06592,-58.53947], "fy":[59.86287,67.11547,57.9991,47.33783]}, - {"t":1.33008, "x":2.88533, "y":4.63282, "heading":-0.0032, "vx":1.0412, "vy":-1.35484, "omega":0.09296, "ax":-2.84882, "ay":3.5534, "alpha":-2.02667, "fx":[-45.4963,-34.14472,-48.10133,-58.54886], "fy":[59.91811,67.12098,57.98205,47.3443]}, - {"t":1.35301, "x":2.90846, "y":4.60269, "heading":-0.00107, "vx":0.97587, "vy":-1.27335, "omega":0.04648, "ax":-2.84897, "ay":3.55409, "alpha":-2.0268, "fx":[-45.46475,-34.15375,-48.12561,-58.55671], "fy":[59.95877,67.1277,57.97311,47.3511]}, - {"t":1.37594, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.91053, "vy":-1.19184, "omega":0.0, "ax":-3.0537, "ay":-2.17772, "alpha":0.0, "fx":[-49.92223,-49.92223,-49.92223,-49.92223], "fy":[-35.60165,-35.60165,-35.60165,-35.60165]}, - {"t":1.39468, "x":2.94661, "y":4.55171, "heading":0.0, "vx":0.85333, "vy":-1.23264, "omega":0.0, "ax":-2.55521, "ay":-1.69767, "alpha":0.0, "fx":[-41.77279,-41.77279,-41.77279,-41.77279], "fy":[-27.75365,-27.75365,-27.75365,-27.75365]}, - {"t":1.41341, "x":2.96215, "y":4.52833, "heading":0.0, "vx":0.80547, "vy":-1.26444, "omega":0.0, "ax":-1.67897, "ay":-1.04076, "alpha":0.0, "fx":[-27.44795,-27.44795,-27.44795,-27.44795], "fy":[-17.01448,-17.01448,-17.01448,-17.01448]}, - {"t":1.43214, "x":2.97694, "y":4.50446, "heading":0.0, "vx":0.77402, "vy":-1.28393, "omega":0.0, "ax":-0.8017, "ay":-0.47699, "alpha":0.0, "fx":[-13.10631,-13.10631,-13.10631,-13.10631], "fy":[-7.79793,-7.79793,-7.79793,-7.79793]}, - {"t":1.45087, "x":2.9913, "y":4.48033, "heading":0.0, "vx":0.759, "vy":-1.29287, "omega":0.0, "ax":-0.29898, "ay":-0.17466, "alpha":0.0, "fx":[-4.88782,-4.88782,-4.88782,-4.88782], "fy":[-2.85538,-2.85538,-2.85538,-2.85538]}, - {"t":1.4696, "x":3.00546, "y":4.45608, "heading":0.0, "vx":0.7534, "vy":-1.29614, "omega":0.0, "ax":-0.07115, "ay":-0.04131, "alpha":0.0, "fx":[-1.16325,-1.16325,-1.16325,-1.16325], "fy":[-0.67533,-0.67533,-0.67533,-0.67533]}, - {"t":1.48833, "x":3.01956, "y":4.43179, "heading":0.0, "vx":0.75207, "vy":-1.29691, "omega":0.0, "ax":0.03416, "ay":0.02199, "alpha":0.0, "fx":[0.55852,0.55852,0.55852,0.55852], "fy":[0.35944,0.35944,0.35944,0.35944]}, - {"t":1.50706, "x":3.03365, "y":4.4075, "heading":0.0, "vx":0.75271, "vy":-1.2965, "omega":0.0, "ax":-0.95471, "ay":1.84534, "alpha":0.0, "fx":[-15.60768,-15.60768,-15.60768,-15.60768], "fy":[30.16783,30.16783,30.16783,30.16783]}, - {"t":1.5258, "x":3.04759, "y":4.38354, "heading":0.0, "vx":0.73483, "vy":-1.26193, "omega":0.0, "ax":-2.22741, "ay":3.85822, "alpha":0.0, "fx":[-36.41395,-36.41395,-36.41395,-36.41395], "fy":[63.07447,63.07447,63.07447,63.07447]}, - {"t":1.54453, "x":3.06096, "y":4.36058, "heading":0.0, "vx":0.6931, "vy":-1.18966, "omega":0.0, "ax":-2.27315, "ay":3.92483, "alpha":0.0, "fx":[-37.1617,-37.1617,-37.1617,-37.1617], "fy":[64.16345,64.16345,64.16345,64.16345]}, - {"t":1.56326, "x":3.07354, "y":4.33898, "heading":0.0, "vx":0.65052, "vy":-1.11614, "omega":0.0, "ax":-2.28986, "ay":3.94721, "alpha":0.0, "fx":[-37.43477,-37.43477,-37.43477,-37.43477], "fy":[64.52941,64.52941,64.52941,64.52941]}, - {"t":1.58199, "x":3.08533, "y":4.31877, "heading":0.0, "vx":0.60763, "vy":-1.04221, "omega":0.0, "ax":-2.29891, "ay":3.95818, "alpha":0.0, "fx":[-37.58276,-37.58276,-37.58276,-37.58276], "fy":[64.70866,64.70866,64.70866,64.70866]}, - {"t":1.60072, "x":3.09631, "y":4.29994, "heading":0.0, "vx":0.56457, "vy":-0.96806, "omega":0.0, "ax":-2.30478, "ay":3.96455, "alpha":0.0, "fx":[-37.67881,-37.67881,-37.67881,-37.67881], "fy":[64.81287,64.81287,64.81287,64.81287]}, - {"t":1.61945, "x":3.10648, "y":4.2825, "heading":0.0, "vx":0.5214, "vy":-0.8938, "omega":0.0, "ax":-2.309, "ay":3.96865, "alpha":0.0, "fx":[-37.74782,-37.74782,-37.74782,-37.74782], "fy":[64.87979,64.87979,64.87979,64.87979]}, - {"t":1.63819, "x":3.11584, "y":4.26646, "heading":0.0, "vx":0.47815, "vy":-0.81946, "omega":0.0, "ax":-2.31224, "ay":3.97145, "alpha":0.0, "fx":[-37.80064,-37.80064,-37.80064,-37.80064], "fy":[64.9257,64.9257,64.9257,64.9257]}, - {"t":1.65692, "x":3.12439, "y":4.2518, "heading":0.0, "vx":0.43483, "vy":-0.74507, "omega":0.0, "ax":-2.31481, "ay":3.97347, "alpha":0.0, "fx":[-37.84279,-37.84279,-37.84279,-37.84279], "fy":[64.95874,64.95874,64.95874,64.95874]}, - {"t":1.67565, "x":3.13213, "y":4.23855, "heading":0.0, "vx":0.39147, "vy":-0.67064, "omega":0.0, "ax":-2.31693, "ay":3.97498, "alpha":0.0, "fx":[-37.8774,-37.8774,-37.8774,-37.8774], "fy":[64.98341,64.98341,64.98341,64.98341]}, - {"t":1.69438, "x":3.13906, "y":4.22668, "heading":0.0, "vx":0.34807, "vy":-0.59618, "omega":0.0, "ax":-2.3187, "ay":3.97615, "alpha":0.0, "fx":[-37.90639,-37.90639,-37.90639,-37.90639], "fy":[65.00242,65.00242,65.00242,65.00242]}, - {"t":1.71311, "x":3.14517, "y":4.21621, "heading":0.0, "vx":0.30464, "vy":-0.5217, "omega":0.0, "ax":-2.32021, "ay":3.97707, "alpha":0.0, "fx":[-37.93102,-37.93102,-37.93102,-37.93102], "fy":[65.01745,65.01745,65.01745,65.01745]}, - {"t":1.73184, "x":3.15047, "y":4.20714, "heading":0.0, "vx":0.26118, "vy":-0.44721, "omega":0.0, "ax":-2.3215, "ay":3.97781, "alpha":0.0, "fx":[-37.95218,-37.95218,-37.95218,-37.95218], "fy":[65.02963,65.02963,65.02963,65.02963]}, - {"t":1.75058, "x":3.15495, "y":4.19946, "heading":0.0, "vx":0.21769, "vy":-0.3727, "omega":0.0, "ax":-2.32263, "ay":3.97843, "alpha":0.0, "fx":[-37.9705,-37.9705,-37.9705,-37.9705], "fy":[65.03969,65.03969,65.03969,65.03969]}, - {"t":1.76931, "x":3.15862, "y":4.19317, "heading":0.0, "vx":0.17419, "vy":-0.29817, "omega":0.0, "ax":-2.3236, "ay":3.97894, "alpha":0.0, "fx":[-37.98647,-37.98647,-37.98647,-37.98647], "fy":[65.04817,65.04817,65.04817,65.04817]}, - {"t":1.78804, "x":3.16148, "y":4.18829, "heading":0.0, "vx":0.13066, "vy":-0.22364, "omega":0.0, "ax":-2.32446, "ay":3.97939, "alpha":0.0, "fx":[-38.00045,-38.00045,-38.00045,-38.00045], "fy":[65.05543,65.05543,65.05543,65.05543]}, - {"t":1.80677, "x":3.16352, "y":4.1848, "heading":0.0, "vx":0.08712, "vy":-0.1491, "omega":0.0, "ax":-2.32521, "ay":3.97978, "alpha":0.0, "fx":[-38.01276,-38.01276,-38.01276,-38.01276], "fy":[65.06175,65.06175,65.06175,65.06175]}, - {"t":1.8255, "x":3.16474, "y":4.1827, "heading":0.0, "vx":0.04357, "vy":-0.07455, "omega":0.0, "ax":-2.32588, "ay":3.98012, "alpha":0.0, "fx":[-38.02365,-38.02365,-38.02365,-38.02365], "fy":[65.06731,65.06731,65.06731,65.06731]}, - {"t":1.84423, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.8986, "ay":-3.53745, "alpha":1.80025, "fx":[55.97527,39.29158,40.54674,53.73245], "fy":[-50.49105,-64.33028,-63.58517,-52.91606]}, + {"t":0.02377, "x":1.03095, "y":6.97841, "heading":-0.93501, "vx":0.06889, "vy":-0.08408, "omega":0.04279, "ax":2.89823, "ay":-3.53755, "alpha":1.79848, "fx":[55.95996,39.2934,40.54703,53.72196], "fy":[-50.50162,-64.32408,-63.58094,-52.9219]}, + {"t":0.04753, "x":1.03341, "y":6.97542, "heading":-0.93399, "vx":0.13777, "vy":-0.16815, "omega":0.08553, "ax":2.89785, "ay":-3.53764, "alpha":1.79665, "fx":[55.9382,39.28688,40.55493,53.7176], "fy":[-50.51892,-64.3227,-63.57162,-52.92119]}, + {"t":0.0713, "x":1.0375, "y":6.97042, "heading":-0.93196, "vx":0.20665, "vy":-0.25223, "omega":0.12823, "ax":2.89746, "ay":-3.53773, "alpha":1.79475, "fx":[55.90986,39.27208,40.57041,53.71929], "fy":[-50.54301,-64.32606,-63.55721,-52.91401]}, + {"t":0.09507, "x":1.04323, "y":6.96343, "heading":-0.92891, "vx":0.27551, "vy":-0.33631, "omega":0.17089, "ax":2.89704, "ay":-3.53781, "alpha":1.79276, "fx":[55.8748,39.24913,40.59349,53.72689], "fy":[-50.57398,-64.33406,-63.53766,-52.90045]}, + {"t":0.11884, "x":1.0506, "y":6.95443, "heading":-0.92485, "vx":0.34437, "vy":-0.4204, "omega":0.2135, "ax":2.8966, "ay":-3.53791, "alpha":1.79066, "fx":[55.83282,39.21821,40.62415,53.74025], "fy":[-50.61199,-64.34653,-63.51293,-52.88061]}, + {"t":0.1426, "x":1.0596, "y":6.94344, "heading":-0.91977, "vx":0.41321, "vy":-0.50448, "omega":0.25606, "ax":2.89613, "ay":-3.538, "alpha":1.78843, "fx":[55.78368,39.17954,40.66242,53.7592], "fy":[-50.65721,-64.36331,-63.48297,-52.85463]}, + {"t":0.16637, "x":1.07024, "y":6.93045, "heading":-0.91369, "vx":0.48204, "vy":-0.58857, "omega":0.29856, "ax":2.89563, "ay":-3.53809, "alpha":1.78604, "fx":[55.72707,39.13337,40.70832,53.78352], "fy":[-50.70986,-64.38416,-63.44769,-52.82266]}, + {"t":0.19014, "x":1.08251, "y":6.91547, "heading":-0.90659, "vx":0.55086, "vy":-0.67266, "omega":0.34101, "ax":2.8951, "ay":-3.53819, "alpha":1.78346, "fx":[55.66265,39.08001,40.76187,53.81296], "fy":[-50.77019,-64.40883,-63.40703,-52.78486]}, + {"t":0.2139, "x":1.09642, "y":6.89848, "heading":-0.89849, "vx":0.61967, "vy":-0.75675, "omega":0.3834, "ax":2.89453, "ay":-3.5383, "alpha":1.78065, "fx":[55.59003,39.01983,40.82311,53.84724], "fy":[-50.83851,-64.43703,-63.36086,-52.74146]}, + {"t":0.23767, "x":1.11197, "y":6.87949, "heading":-0.88937, "vx":0.68847, "vy":-0.84085, "omega":0.42572, "ax":2.89392, "ay":-3.53841, "alpha":1.77757, "fx":[55.50874,38.95322,40.89208,53.88603], "fy":[-50.91513,-64.46842,-63.30909,-52.69267]}, + {"t":0.26144, "x":1.12915, "y":6.85851, "heading":-0.87926, "vx":0.75725, "vy":-0.92495, "omega":0.46797, "ax":2.89325, "ay":-3.53854, "alpha":1.77418, "fx":[55.41827,38.88064,40.9688,53.92895], "fy":[-51.00043,-64.50262,-63.25158,-52.63879]}, + {"t":0.28521, "x":1.14796, "y":6.83553, "heading":-0.86813, "vx":0.82601, "vy":-1.00905, "omega":0.51014, "ax":2.89253, "ay":-3.53867, "alpha":1.77041, "fx":[55.31802,38.80262,41.05331,53.97556], "fy":[-51.09483,-64.5392,-63.18818,-52.58013]}, + {"t":0.30897, "x":1.16841, "y":6.81054, "heading":-0.85601, "vx":0.89476, "vy":-1.09315, "omega":0.55221, "ax":2.89174, "ay":-3.53883, "alpha":1.76621, "fx":[55.20733,38.71972,41.14561,54.02534], "fy":[-51.19877,-64.57769,-63.11875,-52.51705]}, + {"t":0.33274, "x":1.19049, "y":6.78356, "heading":-0.84288, "vx":0.96349, "vy":-1.17726, "omega":0.59419, "ax":2.89088, "ay":-3.539, "alpha":1.7615, "fx":[55.08545,38.63258,41.24571,54.07771], "fy":[-51.31273,-64.61755,-63.04312,-52.45]}, + {"t":0.35651, "x":1.21421, "y":6.75458, "heading":-0.82876, "vx":1.0322, "vy":-1.26137, "omega":0.63606, "ax":2.88992, "ay":-3.53919, "alpha":1.75621, "fx":[54.95154,38.5419,41.3536,54.13198], "fy":[-51.43725,-64.6582,-62.9611,-52.37947]}, + {"t":0.38027, "x":1.23956, "y":6.72361, "heading":-0.81364, "vx":1.10088, "vy":-1.34549, "omega":0.6778, "ax":2.88886, "ay":-3.53941, "alpha":1.75024, "fx":[54.80465,38.44846,41.46921,54.18735], "fy":[-51.57289,-64.69897,-62.87249,-52.30607]}, + {"t":0.40404, "x":1.26654, "y":6.69063, "heading":-0.79754, "vx":1.16954, "vy":-1.42961, "omega":0.71939, "ax":2.88768, "ay":-3.53966, "alpha":1.74348, "fx":[54.6437,38.35309,41.59245,54.24285], "fy":[-51.72026,-64.73911,-62.77709,-52.23049]}, + {"t":0.42781, "x":1.29515, "y":6.65565, "heading":-0.78044, "vx":1.23817, "vy":-1.51374, "omega":0.76083, "ax":2.88634, "ay":-3.53995, "alpha":1.7358, "fx":[54.46744,38.25673,41.72318,54.29735], "fy":[-51.88002,-64.77779,-62.67466,-52.15358]}, + {"t":0.45158, "x":1.3254, "y":6.61867, "heading":-0.76235, "vx":1.30677, "vy":-1.59787, "omega":0.80209, "ax":2.88482, "ay":-3.54029, "alpha":1.72707, "fx":[54.27444,38.16043,41.86116,54.34948], "fy":[-52.0529,-64.81405,-62.56495,-52.07636]}, + {"t":0.47534, "x":1.35727, "y":6.5797, "heading":-0.74329, "vx":1.37534, "vy":-1.68202, "omega":0.84313, "ax":2.88309, "ay":-3.54069, "alpha":1.7171, "fx":[54.06302,38.0653,42.00608,54.39753], "fy":[-52.23968,-64.84675,-62.44768,-52.00004]}, + {"t":0.49911, "x":1.39077, "y":6.53872, "heading":-0.72325, "vx":1.44386, "vy":-1.76617, "omega":0.88395, "ax":2.88108, "ay":-3.54115, "alpha":1.70568, "fx":[53.83115,37.97264,42.15746,54.43941], "fy":[-52.44124,-64.87459,-62.32256,-51.92617]}, + {"t":0.52288, "x":1.4259, "y":6.49574, "heading":-0.70224, "vx":1.51233, "vy":-1.85033, "omega":0.92448, "ax":2.87874, "ay":-3.5417, "alpha":1.69252, "fx":[53.57641,37.88388,42.31464,54.47244], "fy":[-52.65855,-64.89598,-62.18923,-51.85666]}, + {"t":0.54664, "x":1.46266, "y":6.45076, "heading":-0.68027, "vx":1.58075, "vy":-1.93451, "omega":0.96471, "ax":2.87597, "ay":-3.54235, "alpha":1.67728, "fx":[53.29571,37.80065,42.47671,54.49315], "fy":[-52.89277,-64.90897,-62.04729,-51.79397]}, + {"t":0.57041, "x":1.50104, "y":6.40379, "heading":-0.65734, "vx":1.64911, "vy":-2.0187, "omega":1.00457, "ax":2.87265, "ay":-3.54313, "alpha":1.65948, "fx":[52.98512,37.7249,42.64235,54.4969], "fy":[-53.14528,-64.91106,-61.89627,-51.74134]}, + {"t":0.59418, "x":1.54105, "y":6.35481, "heading":-0.63347, "vx":1.71738, "vy":-2.10291, "omega":1.04402, "ax":2.86861, "ay":-3.54407, "alpha":1.63849, "fx":[52.6394,37.65894,42.80966,54.47733], "fy":[-53.41781,-64.899,-61.73554,-51.70314]}, + {"t":0.61795, "x":1.58267, "y":6.30383, "heading":-0.60865, "vx":1.78556, "vy":-2.18714, "omega":1.08296, "ax":2.86361, "ay":-3.54523, "alpha":1.61339, "fx":[52.25125,37.60565,42.97583,54.42534], "fy":[-53.71266,-64.86826,-61.56432,-51.68548]}, + {"t":0.64171, "x":1.62592, "y":6.25084, "heading":-0.58291, "vx":1.85362, "vy":-2.2714, "omega":1.1213, "ax":2.85726, "ay":-3.54665, "alpha":1.58279, "fx":[51.81011,37.56884,43.13655,54.32743], "fy":[-54.03304,-64.81238,-61.38145,-51.69727]}, + {"t":0.66548, "x":1.67078, "y":6.19586, "heading":-0.55626, "vx":1.92153, "vy":-2.3557, "omega":1.15892, "ax":2.84897, "ay":-3.54846, "alpha":1.54454, "fx":[51.29976,37.55377,43.28496,54.16238], "fy":[-54.38369,-64.72148,-61.18524,-51.75213]}, + {"t":0.68925, "x":1.71726, "y":6.13887, "heading":-0.52872, "vx":1.98924, "vy":-2.44003, "omega":1.19563, "ax":2.83773, "ay":-3.55082, "alpha":1.495, "fx":[50.69359,37.56831,43.4093,53.89458], "fy":[-54.77225,-64.57941,-60.97284,-51.87228]}, + {"t":0.71301, "x":1.76534, "y":6.07987, "heading":-0.5003, "vx":2.05669, "vy":-2.52443, "omega":1.23116, "ax":2.82168, "ay":-3.55401, "alpha":1.42759, "fx":[49.9443,37.62537,43.48787,53.45897], "fy":[-55.21186,-64.35744,-60.73913,-52.09697]}, + {"t":0.73678, "x":1.81501, "y":6.01887, "heading":-0.47104, "vx":2.12375, "vy":-2.60889, "omega":1.26509, "ax":2.79702, "ay":-3.55856, "alpha":1.32885, "fx":[48.95797,37.74875,43.47551,52.7216], "fy":[-55.72764,-63.99813,-60.47345,-52.5034]}, + {"t":0.76055, "x":1.86628, "y":5.95586, "heading":-0.44097, "vx":2.19023, "vy":-2.69347, "omega":1.29668, "ax":2.75446, "ay":-3.56553, "alpha":1.16643, "fx":[47.51681,37.9901,43.26008,51.354], "fy":[-56.37434,-63.36582,-60.14917,-53.26888]}, + {"t":0.78432, "x":1.91911, "y":5.89083, "heading":-0.41016, "vx":2.25569, "vy":-2.77821, "omega":1.3244, "ax":2.66404, "ay":-3.57719, "alpha":0.83723, "fx":[44.97245,38.49435,42.46575,48.27533], "fy":[-57.29614,-62.0386,-59.67447,-54.91158]}, + {"t":0.80808, "x":1.97348, "y":5.82379, "heading":-0.37868, "vx":2.31901, "vy":-2.86323, "omega":1.3443, "ax":2.34638, "ay":-3.58924, "alpha":-0.26471, "fx":[38.15582,39.93996,38.58672,36.75277], "fy":[-58.98598,-57.65918,-58.37545,-59.68839]}, + {"t":0.83185, "x":2.02926, "y":5.75473, "heading":-0.34673, "vx":2.37478, "vy":-2.94854, "omega":1.33801, "ax":-2.38245, "ay":1.10304, "alpha":-9.27248, "fx":[-39.93023,-5.81514,-45.9627,-64.08617], "fy":[-28.46057,53.71773,45.32362,1.54961]}, + {"t":0.85562, "x":2.08502, "y":5.68496, "heading":-0.31493, "vx":2.31815, "vy":-2.92232, "omega":1.11763, "ax":-2.97197, "ay":3.08456, "alpha":-3.00658, "fx":[-54.89912,-31.10253,-46.37822,-61.96401], "fy":[44.04423,64.58846,55.94169,37.13237]}, + {"t":0.87938, "x":2.13928, "y":5.61638, "heading":-0.28837, "vx":2.24752, "vy":-2.84901, "omega":1.04617, "ax":-2.95385, "ay":3.29262, "alpha":-2.41945, "fx":[-52.74706,-33.65165,-46.52318,-60.23746], "fy":[50.25818,65.27966,57.40323,42.37127]}, + {"t":0.90315, "x":2.19186, "y":5.54959, "heading":-0.2635, "vx":2.17731, "vy":-2.77076, "omega":0.98866, "ax":-2.94378, "ay":3.3693, "alpha":-2.20459, "fx":[-51.71432,-34.59126,-46.69666,-59.49859], "fy":[52.54636,65.53258,57.89546,44.3524]}, + {"t":0.92692, "x":2.24278, "y":5.48469, "heading":-0.24, "vx":2.10735, "vy":-2.69068, "omega":0.93627, "ax":-2.93776, "ay":3.40919, "alpha":-2.09306, "fx":[-51.03854,-35.08104,-46.87698,-59.11031], "fy":[53.81416,65.66282,58.09082,45.36736]}, + {"t":0.95069, "x":2.29204, "y":5.42171, "heading":-0.21775, "vx":2.03752, "vy":-2.60965, "omega":0.88652, "ax":-2.93377, "ay":3.43365, "alpha":-2.02471, "fx":[-50.5236,-35.3861,-47.05593,-58.88047], "fy":[54.66323,65.73961,58.15945,45.97255]}, + {"t":0.97445, "x":2.33963, "y":5.36065, "heading":-0.19668, "vx":1.9678, "vy":-2.52804, "omega":0.8384, "ax":-2.93093, "ay":3.4502, "alpha":-1.9785, "fx":[-50.09804,-35.59893,-47.22991,-58.73353], "fy":[55.2962,65.78756,58.16448,46.36849]}, + {"t":0.99822, "x":2.38558, "y":5.30154, "heading":-0.17675, "vx":1.89814, "vy":-2.44604, "omega":0.79138, "ax":-2.9288, "ay":3.46214, "alpha":-1.94519, "fx":[-49.72994,-35.75972,-47.39711,-58.63421], "fy":[55.80026,65.81787,58.13477,46.64463]}, + {"t":1.02199, "x":2.42986, "y":5.24438, "heading":-0.15795, "vx":1.82853, "vy":-2.36376, "omega":0.74515, "ax":-2.92713, "ay":3.47117, "alpha":-1.92008, "fx":[-49.40299,-35.88846,-47.55654,-58.56404], "fy":[56.21914,65.83655,58.08541,46.84671]}, + {"t":1.04575, "x":2.47249, "y":5.18919, "heading":-0.14024, "vx":1.75896, "vy":-2.28126, "omega":0.69951, "ax":-2.92579, "ay":3.47823, "alpha":-1.90053, "fx":[-49.10806,-35.99601,-47.70762,-58.51254], "fy":[56.57716,65.84719,58.02508,47.00037]}, + {"t":1.06952, "x":2.51347, "y":5.13595, "heading":-0.12361, "vx":1.68942, "vy":-2.19859, "omega":0.65434, "ax":-2.92468, "ay":3.48391, "alpha":-1.88491, "fx":[-48.83968,-36.08871,-47.85001,-58.47337], "fy":[56.88898,65.85212,57.95914,47.12102]}, + {"t":1.09329, "x":2.5528, "y":5.08468, "heading":-0.10806, "vx":1.61991, "vy":-2.11579, "omega":0.60954, "ax":-2.92375, "ay":3.48858, "alpha":-1.8722, "fx":[-48.59437,-36.17036,-47.98349,-58.44255], "fy":[57.16395,65.85296,57.89107,47.21843]}, + {"t":1.11706, "x":2.59047, "y":5.03538, "heading":-0.09357, "vx":1.55042, "vy":-2.03287, "omega":0.56504, "ax":-2.92295, "ay":3.49248, "alpha":-1.86171, "fx":[-48.36982,-36.24339,-48.10793,-58.4175], "fy":[57.40837,65.85089,57.82325,47.29899]}, + {"t":1.14082, "x":2.6265, "y":4.98805, "heading":-0.08014, "vx":1.48095, "vy":-1.94987, "omega":0.5208, "ax":-2.92226, "ay":3.49579, "alpha":-1.85293, "fx":[-48.16446,-36.30931,-48.22326,-58.3965], "fy":[57.62662,65.84682,57.75735,47.36708]}, + {"t":1.16459, "x":2.66087, "y":4.94269, "heading":-0.06776, "vx":1.4115, "vy":-1.86678, "omega":0.47676, "ax":-2.92166, "ay":3.49863, "alpha":-1.84551, "fx":[-47.97716,-36.36911,-48.32942,-58.37837], "fy":[57.82188,65.84144,57.69459,47.42572]}, + {"t":1.18836, "x":2.69359, "y":4.89931, "heading":-0.05643, "vx":1.34206, "vy":-1.78363, "omega":0.4329, "ax":-2.92112, "ay":3.50109, "alpha":-1.83918, "fx":[-47.80708,-36.42346,-48.4264,-58.36232], "fy":[57.99651,65.83534,57.63588,47.47707]}, + {"t":1.21212, "x":2.72466, "y":4.85791, "heading":-0.04614, "vx":1.27263, "vy":-1.70042, "omega":0.38918, "ax":-2.92065, "ay":3.50325, "alpha":-1.83374, "fx":[-47.65362,-36.47276,-48.5142,-58.34781], "fy":[58.15233,65.82897,57.5819,47.52268]}, + {"t":1.23589, "x":2.75409, "y":4.81849, "heading":-0.03689, "vx":1.20321, "vy":-1.61716, "omega":0.3456, "ax":-2.92023, "ay":3.50515, "alpha":-1.82904, "fx":[-47.51628,-36.51729,-48.59282,-58.33445], "fy":[58.29072,65.82272,57.53319,47.5637]}, + {"t":1.25966, "x":2.78186, "y":4.78104, "heading":-0.02868, "vx":1.13381, "vy":-1.53385, "omega":0.30213, "ax":-2.91985, "ay":3.50684, "alpha":-1.82494, "fx":[-47.39469,-36.55722,-48.66228,-58.32198], "fy":[58.41281,65.81692,57.49017,47.60097]}, + {"t":1.28343, "x":2.80798, "y":4.74558, "heading":-0.0215, "vx":1.06441, "vy":-1.4505, "omega":0.25876, "ax":-2.91951, "ay":3.50835, "alpha":-1.82135, "fx":[-47.28853,-36.59267,-48.72258,-58.31021], "fy":[58.5195,65.81184,57.45316,47.63514]}, + {"t":1.30719, "x":2.83246, "y":4.71209, "heading":-0.01535, "vx":0.99502, "vy":-1.36712, "omega":0.21547, "ax":-2.91921, "ay":3.50971, "alpha":-1.81819, "fx":[-47.19756,-36.62369,-48.77375,-58.29901], "fy":[58.61151,65.80771,57.42245,47.6667]}, + {"t":1.33096, "x":2.85528, "y":4.68059, "heading":-0.01023, "vx":0.92564, "vy":-1.2837, "omega":0.17225, "ax":-2.91893, "ay":3.51094, "alpha":-1.81538, "fx":[-47.12158,-36.65035,-48.81581,-58.28829], "fy":[58.68944,65.80471,57.39823,47.69605]}, + {"t":1.35473, "x":2.87645, "y":4.65107, "heading":-0.00613, "vx":0.85627, "vy":-1.20026, "omega":0.12911, "ax":-2.91869, "ay":3.51205, "alpha":-1.81289, "fx":[-47.06039,-36.67267,-48.84875,-58.27796], "fy":[58.7538,65.80302,57.38069,47.7235]}, + {"t":1.37849, "x":2.89598, "y":4.62354, "heading":-0.00307, "vx":0.7869, "vy":-1.11678, "omega":0.08602, "ax":-2.91846, "ay":3.51305, "alpha":-1.81066, "fx":[-47.01386,-36.69069,-48.87261,-58.26796], "fy":[58.80497,65.80274,57.36998,47.74932]}, + {"t":1.40226, "x":2.91386, "y":4.59799, "heading":-0.00102, "vx":0.71754, "vy":-1.03329, "omega":0.04299, "ax":-2.91826, "ay":3.51398, "alpha":-1.80866, "fx":[-46.98186,-36.70446,-48.88737,-58.25822], "fy":[58.84331,65.804,57.36622,47.77374]}, + {"t":1.42603, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.64818, "vy":-0.94977, "omega":0.0, "ax":-1.45495, "ay":0.1374, "alpha":0.0, "fx":[-23.76062,-23.76537,-23.80728,-23.80917], "fy":[2.26938,2.11025,2.33807,2.2675]}, + {"t":1.4573, "x":2.94965, "y":4.54479, "heading":0.0, "vx":0.60268, "vy":-0.94548, "omega":0.0, "ax":-1.13183, "ay":0.97154, "alpha":0.0, "fx":[-18.50161,-18.50243,-18.50467,-18.5046], "fy":[15.88443,15.88021,15.88303,15.88394]}, + {"t":1.48857, "x":2.96794, "y":4.5157, "heading":0.0, "vx":0.56729, "vy":-0.9151, "omega":0.0, "ax":-0.97536, "ay":1.133, "alpha":0.0, "fx":[-15.94693,-15.94569,-15.94442,-15.94388], "fy":[18.5211,18.52937,18.51745,18.52151]}, + {"t":1.51983, "x":2.9852, "y":4.48764, "heading":0.0, "vx":0.53679, "vy":-0.87967, "omega":0.0, "ax":-0.9023, "ay":1.19383, "alpha":0.0, "fx":[-14.75263,-14.75142,-14.75,-14.74955], "fy":[19.51556,19.52317,19.51277,19.51599]}, + {"t":1.5511, "x":3.00154, "y":4.46072, "heading":0.0, "vx":0.50858, "vy":-0.84234, "omega":0.0, "ax":-0.8609, "ay":1.22501, "alpha":0.0, "fx":[-14.07551,-14.07454,-14.0734,-14.07304], "fy":[20.02546,20.03148,20.02334,20.02582]}, + {"t":1.58237, "x":3.01702, "y":4.43498, "heading":0.0, "vx":0.48166, "vy":-0.80404, "omega":0.0, "ax":-0.83448, "ay":1.24377, "alpha":0.0, "fx":[-13.6432,-13.64244,-13.64151,-13.64124], "fy":[20.33245,20.3371,20.33093,20.33274]}, + {"t":1.61364, "x":3.03168, "y":4.41045, "heading":0.0, "vx":0.45557, "vy":-0.76515, "omega":0.0, "ax":-0.81625, "ay":1.25623, "alpha":0.0, "fx":[-13.34496,-13.34435,-13.34359,-13.34338], "fy":[20.53626,20.53992,20.53512,20.53649]}, + {"t":1.64491, "x":3.04552, "y":4.38714, "heading":0.0, "vx":0.43005, "vy":-0.72587, "omega":0.0, "ax":-0.80297, "ay":1.26506, "alpha":0.0, "fx":[-13.12779,-13.12728,-13.12665,-13.12648], "fy":[20.68071,20.68373,20.67978,20.68091]}, + {"t":1.67618, "x":3.05858, "y":4.36506, "heading":0.0, "vx":0.40494, "vy":-0.68631, "omega":0.0, "ax":-0.79291, "ay":1.27162, "alpha":0.0, "fx":[-12.96324,-12.9628,-12.96226,-12.96211], "fy":[20.788,20.79059,20.78722,20.78817]}, + {"t":1.70744, "x":3.07085, "y":4.34422, "heading":0.0, "vx":0.38015, "vy":-0.64655, "omega":0.0, "ax":-0.78505, "ay":1.27666, "alpha":0.0, "fx":[-12.83471,-12.83431,-12.83381,-12.83368], "fy":[20.87054,20.87287,20.86985,20.87069]}, + {"t":1.73871, "x":3.08235, "y":4.32463, "heading":0.0, "vx":0.3556, "vy":-0.60663, "omega":0.0, "ax":-0.77876, "ay":1.28065, "alpha":0.0, "fx":[-12.73182,-12.73144,-12.73097,-12.73085], "fy":[20.93583,20.93803,20.93518,20.93597]}, + {"t":1.76998, "x":3.09309, "y":4.30629, "heading":0.0, "vx":0.33125, "vy":-0.56659, "omega":0.0, "ax":-0.77362, "ay":1.28388, "alpha":0.0, "fx":[-12.6478,-12.64744,-12.64698,-12.64686], "fy":[20.98863,20.99078,20.98798,20.98877]}, + {"t":1.80125, "x":3.10307, "y":4.2892, "heading":0.0, "vx":0.30706, "vy":-0.52644, "omega":0.0, "ax":-0.76936, "ay":1.28654, "alpha":0.0, "fx":[-12.57805,-12.57768,-12.57722,-12.5771], "fy":[21.03212,21.03429,21.03147,21.03226]}, + {"t":1.83252, "x":3.1123, "y":4.27337, "heading":0.0, "vx":0.283, "vy":-0.48621, "omega":0.0, "ax":-0.76576, "ay":1.28877, "alpha":0.0, "fx":[-12.51929,-12.51892,-12.51845,-12.51833], "fy":[21.06852,21.07075,21.06784,21.06866]}, + {"t":1.86379, "x":3.12077, "y":4.25879, "heading":0.0, "vx":0.25906, "vy":-0.44592, "omega":0.0, "ax":-0.7627, "ay":1.29066, "alpha":0.0, "fx":[-12.46919,-12.4688,-12.46831,-12.46818], "fy":[21.09939,21.10172,21.09868,21.09954]}, + {"t":1.89505, "x":3.1285, "y":4.24548, "heading":0.0, "vx":0.23521, "vy":-0.40556, "omega":0.0, "ax":-0.76005, "ay":1.29228, "alpha":0.0, "fx":[-12.42597,-12.42558,-12.42507,-12.42493], "fy":[21.12589,21.12832,21.12514,21.12604]}, + {"t":1.92632, "x":3.13548, "y":4.23343, "heading":0.0, "vx":0.21144, "vy":-0.36515, "omega":0.0, "ax":-0.75775, "ay":1.29369, "alpha":0.0, "fx":[-12.38833,-12.38794,-12.38739,-12.38727], "fy":[21.14889,21.15133,21.14816,21.14903]}, + {"t":1.95759, "x":3.14172, "y":4.22265, "heading":0.0, "vx":0.18775, "vy":-0.3247, "omega":0.0, "ax":-0.75572, "ay":1.29492, "alpha":0.0, "fx":[-12.35526,-12.35485,-12.35428,-12.35415], "fy":[21.16902,21.17158,21.16824,21.16916]}, + {"t":1.98886, "x":3.14722, "y":4.21313, "heading":0.0, "vx":0.16412, "vy":-0.28421, "omega":0.0, "ax":-0.75393, "ay":1.29601, "alpha":0.0, "fx":[-12.32596,-12.32554,-12.32495,-12.32481], "fy":[21.1868,21.18947,21.18597,21.18694]}, + {"t":2.02013, "x":3.15199, "y":4.20487, "heading":0.0, "vx":0.14055, "vy":-0.24369, "omega":0.0, "ax":-0.75233, "ay":1.29698, "alpha":0.0, "fx":[-12.29981,-12.29937,-12.29876,-12.29862], "fy":[21.20261,21.20541,21.20174,21.20277]}, + {"t":2.0514, "x":3.15601, "y":4.19789, "heading":0.0, "vx":0.11702, "vy":-0.20313, "omega":0.0, "ax":-0.75089, "ay":1.29785, "alpha":0.0, "fx":[-12.27631,-12.27586,-12.27523,-12.27508], "fy":[21.2168,21.2197,21.21588,21.21695]}, + {"t":2.08266, "x":3.15931, "y":4.19217, "heading":0.0, "vx":0.09354, "vy":-0.16255, "omega":0.0, "ax":-0.74959, "ay":1.29863, "alpha":0.0, "fx":[-12.25507,-12.2546,-12.25395,-12.25379], "fy":[21.22959,21.23259,21.22863,21.22975]}, + {"t":2.11393, "x":3.16186, "y":4.18772, "heading":0.0, "vx":0.07011, "vy":-0.12194, "omega":0.0, "ax":-0.74841, "ay":1.29934, "alpha":0.0, "fx":[-12.23575,-12.23527,-12.2346,-12.23444], "fy":[21.24119,21.24429,21.2402,21.24136]}, + {"t":2.1452, "x":3.16369, "y":4.18455, "heading":0.0, "vx":0.0467, "vy":-0.08132, "omega":0.0, "ax":-0.74733, "ay":1.29999, "alpha":0.0, "fx":[-12.2181,-12.21761,-12.21692,-12.21676], "fy":[21.25178,21.25496,21.25075,21.25195]}, + {"t":2.17647, "x":3.16479, "y":4.18264, "heading":0.0, "vx":0.02334, "vy":-0.04067, "omega":0.0, "ax":-0.74633, "ay":1.30058, "alpha":0.0, "fx":[-12.20051,-12.20099,-12.20304,-12.19999], "fy":[21.26472,21.26183,21.26156,21.26017]}, + {"t":2.20774, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index ec584baa..aef7eecf 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"slowaccel", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,62 +30,73 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.27566,1.99673], + "waypoints":[0.0,1.2456,2.23311], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42195, "ay":-3.09664, "alpha":-0.01591, "fx":[55.87399,55.98829,56.01089,55.8964], "fy":[-50.69989,-50.57369,-50.54841,-50.67492]}, - {"t":0.03866, "x":1.55271, "y":7.52108, "heading":-0.93501, "vx":0.13228, "vy":-0.1197, "omega":-0.00061, "ax":3.4217, "ay":-3.09653, "alpha":-0.01631, "fx":[55.86814,55.98534,56.0085,55.89111], "fy":[-50.69993,-50.57055,-50.54461,-50.67433]}, - {"t":0.07731, "x":1.56038, "y":7.51414, "heading":-0.93503, "vx":0.26455, "vy":-0.2394, "omega":-0.00125, "ax":3.42142, "ay":-3.0964, "alpha":-0.01676, "fx":[55.86162,55.98205,56.00584,55.88521], "fy":[-50.69998,-50.56704,-50.54038,-50.67367]}, - {"t":0.11597, "x":1.57316, "y":7.50258, "heading":-0.93508, "vx":0.39681, "vy":-0.3591, "omega":-0.00189, "ax":3.4211, "ay":-3.09626, "alpha":-0.01727, "fx":[55.85431,55.97836,56.00286,55.8786], "fy":[-50.70004,-50.56311,-50.53563,-50.67293]}, - {"t":0.15462, "x":1.59106, "y":7.48638, "heading":-0.93515, "vx":0.52906, "vy":-0.47879, "omega":-0.00256, "ax":3.42075, "ay":-3.0961, "alpha":-0.01784, "fx":[55.84605,55.97419,55.9995,55.87113], "fy":[-50.70011,-50.55868,-50.53027,-50.6721]}, - {"t":0.19328, "x":1.61406, "y":7.46556, "heading":-0.93525, "vx":0.66129, "vy":-0.59847, "omega":-0.00325, "ax":3.42034, "ay":-3.09591, "alpha":-0.01848, "fx":[55.83666,55.96945,55.99568,55.86264], "fy":[-50.70018,-50.55364,-50.52418,-50.67114]}, - {"t":0.23194, "x":1.64218, "y":7.44011, "heading":-0.93538, "vx":0.79351, "vy":-0.71815, "omega":-0.00396, "ax":3.41988, "ay":-3.0957, "alpha":-0.01923, "fx":[55.82588,55.964,55.99129,55.8529], "fy":[-50.70027,-50.54786,-50.51717,-50.67004]}, - {"t":0.27059, "x":1.67541, "y":7.41004, "heading":-0.93553, "vx":0.92571, "vy":-0.83782, "omega":-0.00471, "ax":3.41934, "ay":-3.09546, "alpha":-0.02009, "fx":[55.81338,55.95769,55.98621,55.8416], "fy":[-50.70037,-50.54116,-50.50906,-50.66877]}, - {"t":0.30925, "x":1.71375, "y":7.37534, "heading":-0.93571, "vx":1.05789, "vy":-0.95748, "omega":-0.00548, "ax":3.41871, "ay":-3.09517, "alpha":-0.02111, "fx":[55.79872,55.95028,55.98024,55.82835], "fy":[-50.70049,-50.53331,-50.49953,-50.66727]}, - {"t":0.34791, "x":1.7572, "y":7.33601, "heading":-0.93592, "vx":1.19004, "vy":-1.07712, "omega":-0.0063, "ax":3.41796, "ay":-3.09483, "alpha":-0.02231, "fx":[55.78127,55.94147,55.97314,55.81257], "fy":[-50.70064,-50.52396,-50.48819,-50.66549]}, - {"t":0.38656, "x":1.80575, "y":7.29206, "heading":-0.93617, "vx":1.32216, "vy":-1.19676, "omega":-0.00716, "ax":3.41705, "ay":-3.09442, "alpha":-0.02377, "fx":[55.76016,55.93082,55.96456,55.79349], "fy":[-50.70081,-50.51266,-50.47446,-50.66333]}, - {"t":0.42522, "x":1.85942, "y":7.24349, "heading":-0.93644, "vx":1.45426, "vy":-1.31638, "omega":-0.00808, "ax":3.41593, "ay":-3.09391, "alpha":-0.02558, "fx":[55.73411,55.91767,55.95397,55.76993], "fy":[-50.70103,-50.49871,-50.45752,-50.66066]}, - {"t":0.46387, "x":1.91819, "y":7.19029, "heading":-0.93676, "vx":1.5863, "vy":-1.43598, "omega":-0.00907, "ax":3.41451, "ay":-3.09326, "alpha":-0.02786, "fx":[55.70115,55.90104,55.94056,55.74011], "fy":[-50.7013,-50.48107,-50.43608,-50.65727]}, - {"t":0.50253, "x":1.98206, "y":7.13247, "heading":-0.93711, "vx":1.71829, "vy":-1.55555, "omega":-0.01015, "ax":3.41266, "ay":-3.09242, "alpha":-0.03084, "fx":[55.65813,55.87934,55.92306,55.70116], "fy":[-50.70165,-50.45806,-50.40806,-50.65285]}, - {"t":0.54119, "x":2.05103, "y":7.07003, "heading":-0.9375, "vx":1.85021, "vy":-1.67509, "omega":-0.01134, "ax":3.41013, "ay":-3.09127, "alpha":-0.03491, "fx":[55.59958,55.84983,55.89925,55.64811], "fy":[-50.70211,-50.42675,-50.36992,-50.64681]}, - {"t":0.57984, "x":2.1251, "y":7.00297, "heading":-0.93794, "vx":1.98204, "vy":-1.79459, "omega":-0.01269, "ax":3.4065, "ay":-3.08962, "alpha":-0.04077, "fx":[55.5153,55.80737,55.86494,55.57166], "fy":[-50.70274,-50.38169,-50.31495,-50.63808]}, - {"t":0.6185, "x":2.20426, "y":6.93129, "heading":-0.93843, "vx":2.11372, "vy":-1.91402, "omega":-0.01427, "ax":3.40082, "ay":-3.08703, "alpha":-0.04995, "fx":[55.38354,55.74106,55.81124,55.4519], "fy":[-50.70365,-50.31129,-50.22888,-50.62435]}, - {"t":0.65716, "x":2.28851, "y":6.85499, "heading":-0.93898, "vx":2.24518, "vy":-2.03335, "omega":-0.0162, "ax":3.39067, "ay":-3.0824, "alpha":-0.0664, "fx":[55.14851,55.62294,55.71521,55.23757], "fy":[-50.70499,-50.18582,-50.07494,-50.59957]}, - {"t":0.69581, "x":2.37784, "y":6.77409, "heading":-0.93961, "vx":2.37625, "vy":-2.15251, "omega":-0.01876, "ax":3.3674, "ay":-3.07173, "alpha":-0.1043, "fx":[54.61085,55.35329,55.49416,54.74381], "fy":[-50.70645,-49.89932,-49.72087,-50.54129]}, - {"t":0.73447, "x":2.47221, "y":6.68858, "heading":-0.94033, "vx":2.50642, "vy":-2.27125, "omega":-0.02279, "ax":3.25888, "ay":-3.02124, "alpha":-0.28362, "fx":[52.13558,54.11858,54.4465,52.40523], "fy":[-50.68105,-48.58911,-48.05582,-50.2401]}, - {"t":0.77312, "x":2.57153, "y":6.59853, "heading":-0.94121, "vx":2.6324, "vy":-2.38804, "omega":-0.03376, "ax":-3.31806, "ay":2.91526, "alpha":-0.31165, "fx":[-55.50223,-53.34113,-53.01702,-55.11565], "fy":[46.13397,48.56598,49.12806,46.80776]}, - {"t":0.81178, "x":2.67081, "y":6.50839, "heading":-0.94252, "vx":2.50414, "vy":-2.27535, "omega":-0.04581, "ax":-3.39222, "ay":3.03976, "alpha":-0.09365, "fx":[-55.85326,-55.1882,-55.06256,-55.72154], "fy":[49.24229,49.98196,50.14076,49.41186]}, - {"t":0.85044, "x":2.76508, "y":6.42271, "heading":-0.94429, "vx":2.37301, "vy":-2.15784, "omega":-0.04943, "ax":-3.4062, "ay":3.06361, "alpha":-0.05249, "fx":[-55.90952,-55.53524,-55.4612,-55.83355], "fy":[49.83143,50.24669,50.33524,49.92339]}, - {"t":0.88909, "x":2.85426, "y":6.34158, "heading":-0.9462, "vx":2.24133, "vy":-2.03941, "omega":-0.05145, "ax":-3.41211, "ay":3.07373, "alpha":-0.03505, "fx":[-55.93219,-55.68186,-55.6311,-55.88057], "fy":[50.08086,50.35831,50.41763,50.14171]}, - {"t":0.92775, "x":2.93836, "y":6.26504, "heading":-0.94819, "vx":2.10944, "vy":-1.92059, "omega":-0.05281, "ax":-3.41537, "ay":3.07933, "alpha":-0.02541, "fx":[-55.94435,-55.76279,-55.7253,-55.90641], "fy":[50.2187,50.41983,50.46309,50.26277]}, - {"t":0.96641, "x":3.01735, "y":6.1931, "heading":-0.95023, "vx":1.97741, "vy":-1.80156, "omega":-0.05379, "ax":-3.41743, "ay":3.08288, "alpha":-0.01928, "fx":[-55.95191,-55.81408,-55.78519,-55.92276], "fy":[50.30619,50.4588,50.49187,50.33973]}, - {"t":1.00506, "x":3.09123, "y":6.12576, "heading":-0.95231, "vx":1.84531, "vy":-1.68239, "omega":-0.05454, "ax":-3.41886, "ay":3.08533, "alpha":-0.01504, "fx":[-55.95703,-55.8495,-55.82665,-55.93403], "fy":[50.36666,50.4857,50.51172,50.39296]}, - {"t":1.04372, "x":3.16001, "y":6.06303, "heading":-0.95442, "vx":1.71315, "vy":-1.56312, "omega":-0.05512, "ax":-3.4199, "ay":3.08713, "alpha":-0.01194, "fx":[-55.96073,-55.87541,-55.85706,-55.94227], "fy":[50.41097,50.50539,50.52621,50.43196]}, - {"t":1.08237, "x":3.22368, "y":6.00492, "heading":-0.95655, "vx":1.58095, "vy":-1.44378, "omega":-0.05558, "ax":-3.4207, "ay":3.0885, "alpha":-0.00956, "fx":[-55.96351,-55.89519,-55.88032,-55.94857], "fy":[50.44483,50.52044,50.53726,50.46177]}, - {"t":1.12103, "x":3.28224, "y":5.95141, "heading":-0.9587, "vx":1.44871, "vy":-1.32439, "omega":-0.05595, "ax":-3.42133, "ay":3.08958, "alpha":-0.00768, "fx":[-55.96568,-55.91078,-55.89869,-55.95354], "fy":[50.47157,50.53231,50.54595,50.48528]}, - {"t":1.15969, "x":3.33568, "y":5.90253, "heading":-0.96086, "vx":1.31646, "vy":-1.20496, "omega":-0.05625, "ax":-3.42183, "ay":3.09046, "alpha":-0.00616, "fx":[-55.96741,-55.92338,-55.91357,-55.95757], "fy":[50.49321,50.54192,50.55296,50.5043]}, - {"t":1.19834, "x":3.38402, "y":5.85826, "heading":-0.96303, "vx":1.18418, "vy":-1.0855, "omega":-0.05648, "ax":-3.42225, "ay":3.09118, "alpha":-0.00491, "fx":[-55.96881,-55.93377,-55.92587,-55.9609], "fy":[50.51109,50.54987,50.55874,50.51999]}, - {"t":1.237, "x":3.42724, "y":5.8186, "heading":-0.96522, "vx":1.05189, "vy":-0.966, "omega":-0.05667, "ax":-3.42261, "ay":3.09179, "alpha":-0.00385, "fx":[-55.96998,-55.94248,-55.93621,-55.96371], "fy":[50.52612,50.55655,50.56358,50.53316]}, - {"t":1.27566, "x":3.46534, "y":5.78357, "heading":-0.96741, "vx":0.91959, "vy":-0.84649, "omega":-0.05682, "ax":-1.5087, "ay":-1.53167, "alpha":-1.13942, "fx":[-24.65478,-20.00114,-24.84409,-29.15736], "fy":[-29.11667,-26.86294,-20.74079,-23.43943]}, - {"t":1.31171, "x":3.49751, "y":5.75206, "heading":-0.96946, "vx":0.86519, "vy":-0.90171, "omega":-0.0979, "ax":-0.20145, "ay":-0.19175, "alpha":-1.43419, "fx":[-4.36719,2.23451,-2.2412,-8.79968], "fy":[-8.60231,-4.14368,2.37787,-2.17111]}, - {"t":1.34776, "x":3.52858, "y":5.71942, "heading":-0.97299, "vx":0.85793, "vy":-0.90862, "omega":-0.14961, "ax":-0.02066, "ay":-0.01949, "alpha":-1.12338, "fx":[-1.20182,3.9795,0.52494,-4.65371], "fy":[-4.61642,-1.09214,3.98192,0.45197]}, - {"t":1.38382, "x":3.5595, "y":5.68665, "heading":-0.97838, "vx":0.85718, "vy":-0.90932, "omega":-0.19012, "ax":-0.0021, "ay":-0.00198, "alpha":-0.80656, "fx":[-0.67094,3.06192,0.60235,-3.13037], "fy":[-3.11552,-0.60327,3.05108,0.53854]}, - {"t":1.41987, "x":3.5904, "y":5.65386, "heading":-0.98523, "vx":0.85711, "vy":-0.9094, "omega":-0.21919, "ax":-0.00021, "ay":-0.0002, "alpha":-0.49278, "fx":[-0.40534,1.88575,0.39841,-1.89268], "fy":[-1.88435,-0.36503,1.87783,0.35849]}, - {"t":1.45593, "x":3.6213, "y":5.62108, "heading":-0.99314, "vx":0.8571, "vy":-0.9094, "omega":-0.23696, "ax":-0.00002, "ay":-0.00002, "alpha":-0.18094, "fx":[-0.15337,0.69228,0.15267,-0.69297], "fy":[-0.68986,-0.13864,0.6892,0.13798]}, - {"t":1.49198, "x":3.6522, "y":5.58829, "heading":-1.00168, "vx":0.8571, "vy":-0.9094, "omega":-0.24349, "ax":0.0, "ay":0.0, "alpha":0.13018, "fx":[0.11428,-0.49747,-0.11436,0.4974], "fy":[0.49508,0.10373,-0.49515,-0.10379]}, - {"t":1.52803, "x":3.6831, "y":5.5555, "heading":-1.01046, "vx":0.8571, "vy":-0.9094, "omega":-0.23879, "ax":0.0, "ay":0.0, "alpha":0.44181, "fx":[0.40273,-1.6851,-0.40273,1.68509], "fy":[1.67691,0.36696,-1.67691,-0.36697]}, - {"t":1.56409, "x":3.71401, "y":5.52271, "heading":-1.01907, "vx":0.8571, "vy":-0.9094, "omega":-0.22286, "ax":0.0, "ay":0.0, "alpha":0.75519, "fx":[0.71304,-2.87483,-0.71304,2.87482], "fy":[2.8603,0.65202,-2.8603,-0.65203]}, - {"t":1.60014, "x":3.74491, "y":5.48993, "heading":-1.0271, "vx":0.8571, "vy":-0.9094, "omega":-0.19563, "ax":0.0, "ay":0.0, "alpha":1.07151, "fx":[1.04427,-4.07141,-1.04428,4.07141], "fy":[4.05011,0.95788,-4.05011,-0.95788]}, - {"t":1.6362, "x":3.77581, "y":5.45714, "heading":-1.03416, "vx":0.8571, "vy":-0.9094, "omega":-0.157, "ax":0.0, "ay":0.0, "alpha":1.39194, "fx":[1.39361,-5.28005,-1.39365,5.28001], "fy":[5.25155,1.28158,-5.25158,-1.28161]}, - {"t":1.67225, "x":3.80671, "y":5.42435, "heading":-1.03982, "vx":0.8571, "vy":-0.9094, "omega":-0.10682, "ax":-0.00009, "ay":0.00007, "alpha":1.71758, "fx":[1.7548,-6.50772,-1.75785,6.50472], "fy":[6.47153,1.6195,-6.46911,-1.61706]}, - {"t":1.7083, "x":3.83761, "y":5.39156, "heading":-1.04367, "vx":0.8571, "vy":-0.9094, "omega":-0.04489, "ax":-1.6595, "ay":1.76081, "alpha":1.17929, "fx":[-24.82136,-31.20566,-29.70566,-22.78626], "fy":[33.35883,28.76988,24.01327,29.00162]}, - {"t":1.74436, "x":3.86744, "y":5.35992, "heading":-1.04529, "vx":0.79726, "vy":-0.84592, "omega":-0.00237, "ax":-3.1427, "ay":3.33448, "alpha":0.02031, "fx":[-51.28155,-51.43802,-51.47288,-51.31608], "fy":[54.60327,54.45646,54.42113,54.56843]}, - {"t":1.78041, "x":3.89414, "y":5.33159, "heading":-1.04537, "vx":0.68396, "vy":-0.7257, "omega":-0.00164, "ax":-3.15561, "ay":3.34818, "alpha":0.01168, "fx":[-51.5329,-51.62314,-51.64356,-51.55321], "fy":[54.78867,54.70382,54.68385,54.76886]}, - {"t":1.81646, "x":3.91675, "y":5.3076, "heading":-1.04543, "vx":0.57019, "vy":-0.60498, "omega":-0.00122, "ax":-3.15995, "ay":3.35279, "alpha":0.00877, "fx":[-51.61756,-51.68546,-51.70091,-51.63295], "fy":[54.85105,54.78716,54.77223,54.83622]}, - {"t":1.85252, "x":3.93525, "y":5.28797, "heading":-1.04548, "vx":0.45626, "vy":-0.4841, "omega":-0.0009, "ax":-3.16213, "ay":3.3551, "alpha":0.00732, "fx":[-51.66005,-51.71673,-51.72966,-51.67294], "fy":[54.88234,54.82898,54.81656,54.86999]}, - {"t":1.88857, "x":3.94965, "y":5.27269, "heading":-1.04551, "vx":0.34225, "vy":-0.36313, "omega":-0.00064, "ax":-3.16344, "ay":3.35649, "alpha":0.00645, "fx":[-51.68559,-51.73553,-51.74694,-51.69697], "fy":[54.90115,54.85413,54.84321,54.89028]}, - {"t":1.92463, "x":3.95993, "y":5.26178, "heading":-1.04553, "vx":0.22819, "vy":-0.24212, "omega":-0.00041, "ax":-3.16432, "ay":3.35742, "alpha":0.00587, "fx":[-51.70264,-51.74808,-51.75847,-51.71301], "fy":[54.9137,54.87091,54.86099,54.90382]}, - {"t":1.96068, "x":3.9661, "y":5.25524, "heading":-1.04555, "vx":0.11411, "vy":-0.12107, "omega":-0.0002, "ax":-3.16494, "ay":3.35808, "alpha":0.00545, "fx":[-51.71482,-51.75704,-51.76671,-51.72447], "fy":[54.92267,54.88291,54.8737,54.91349]}, - {"t":1.99673, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.48118, "ay":-3.02968, "alpha":-0.00331, "fx":[56.89659,56.91977,56.92479,56.90161], "fy":[-49.54565,-49.51904,-49.51321,-49.53984]}, + {"t":0.03775, "x":1.55263, "y":7.52124, "heading":-0.93501, "vx":0.1314, "vy":-0.11436, "omega":-0.00012, "ax":3.48069, "ay":-3.02983, "alpha":-0.00339, "fx":[56.88825,56.91203,56.91718,56.89339], "fy":[-49.54865,-49.52134,-49.51536,-49.54268]}, + {"t":0.07549, "x":1.56007, "y":7.51477, "heading":-0.93501, "vx":0.26278, "vy":-0.22872, "omega":-0.00025, "ax":3.48015, "ay":-3.03001, "alpha":-0.00349, "fx":[56.87898,56.90344,56.90873,56.88426], "fy":[-49.55197,-49.52389,-49.51774,-49.54584]}, + {"t":0.11324, "x":1.57247, "y":7.50397, "heading":-0.93502, "vx":0.39414, "vy":-0.34309, "omega":-0.00038, "ax":3.47955, "ay":-3.0302, "alpha":-0.0036, "fx":[56.86861,56.89384,56.89929,56.87406], "fy":[-49.55568,-49.52674,-49.52041,-49.54937]}, + {"t":0.15098, "x":1.58982, "y":7.48887, "heading":-0.93503, "vx":0.52548, "vy":-0.45747, "omega":-0.00052, "ax":3.47886, "ay":-3.03042, "alpha":-0.00372, "fx":[56.85696,56.88303,56.88867,56.86258], "fy":[-49.55986,-49.52995,-49.5234,-49.55333]}, + {"t":0.18873, "x":1.61214, "y":7.46944, "heading":-0.93505, "vx":0.65679, "vy":-0.57185, "omega":-0.00066, "ax":3.47809, "ay":-3.03067, "alpha":-0.00386, "fx":[56.84375,56.8708,56.87663,56.84957], "fy":[-49.56459,-49.53358,-49.52679,-49.55782]}, + {"t":0.22647, "x":1.63941, "y":7.4457, "heading":-0.93508, "vx":0.78807, "vy":-0.68624, "omega":-0.00081, "ax":3.47721, "ay":-3.03095, "alpha":-0.00401, "fx":[56.82866,56.85681,56.86288,56.83471], "fy":[-49.57,-49.53772,-49.53066,-49.56296]}, + {"t":0.26422, "x":1.67163, "y":7.41763, "heading":-0.93511, "vx":0.91932, "vy":-0.80065, "omega":-0.00096, "ax":3.47619, "ay":-3.03128, "alpha":-0.0042, "fx":[56.81125,56.84068,56.84702,56.81757], "fy":[-49.57623,-49.54251,-49.53513,-49.56887]}, + {"t":0.30196, "x":1.7088, "y":7.38525, "heading":-0.93515, "vx":1.05053, "vy":-0.91507, "omega":-0.00112, "ax":3.47501, "ay":-3.03166, "alpha":-0.00441, "fx":[56.79095,56.82188,56.82852,56.79758], "fy":[-49.58349,-49.54808,-49.54034,-49.57577]}, + {"t":0.33971, "x":1.75093, "y":7.34855, "heading":-0.93519, "vx":1.1817, "vy":-1.0295, "omega":-0.00128, "ax":3.4736, "ay":-3.03211, "alpha":-0.00466, "fx":[56.76697,56.79966,56.80666,56.77396], "fy":[-49.59206,-49.55466,-49.54648,-49.58391]}, + {"t":0.37746, "x":1.79801, "y":7.30753, "heading":-0.93524, "vx":1.31281, "vy":-1.14395, "omega":-0.00146, "ax":3.47192, "ay":-3.03265, "alpha":-0.00496, "fx":[56.73821,56.77301,56.78045,56.74564], "fy":[-49.60233,-49.56254,-49.55384,-49.59367]}, + {"t":0.4152, "x":1.85004, "y":7.2622, "heading":-0.93529, "vx":1.44386, "vy":-1.25842, "omega":-0.00165, "ax":3.46987, "ay":-3.03331, "alpha":-0.00532, "fx":[56.70308,56.74046,56.74844,56.71103], "fy":[-49.61487,-49.57216,-49.56283,-49.60558]}, + {"t":0.45295, "x":1.90701, "y":7.21254, "heading":-0.93535, "vx":1.57483, "vy":-1.37291, "omega":-0.00185, "ax":3.4673, "ay":-3.03413, "alpha":-0.00578, "fx":[56.6592,56.69982,56.70845,56.66781], "fy":[-49.63051,-49.58416,-49.57404,-49.62043]}, + {"t":0.49069, "x":1.96892, "y":7.15855, "heading":-0.93542, "vx":1.70571, "vy":-1.48744, "omega":-0.00206, "ax":3.46401, "ay":-3.03519, "alpha":-0.00637, "fx":[56.60285,56.64763,56.6571,56.61229], "fy":[-49.65056,-49.59954,-49.58841,-49.63948]}, + {"t":0.52844, "x":2.03577, "y":7.10025, "heading":-0.9355, "vx":1.83646, "vy":-1.602, "omega":-0.00231, "ax":3.45962, "ay":-3.03658, "alpha":-0.00716, "fx":[56.52782,56.57815,56.58872,56.53836], "fy":[-49.6772,-49.61996,-49.60749,-49.66479]}, + {"t":0.56618, "x":2.10755, "y":7.03761, "heading":-0.93559, "vx":1.96704, "vy":-1.71662, "omega":-0.00258, "ax":3.4535, "ay":-3.03853, "alpha":-0.00825, "fx":[56.423,56.4811,56.4932,56.43505], "fy":[-49.7143,-49.64841,-49.63408,-49.70006]}, + {"t":0.60393, "x":2.18426, "y":6.97066, "heading":-0.93569, "vx":2.0974, "vy":-1.83131, "omega":-0.00289, "ax":3.44433, "ay":-3.04143, "alpha":-0.00989, "fx":[56.26626,56.33602,56.35034,56.28051], "fy":[-49.76953,-49.69074,-49.67365,-49.75257]}, + {"t":0.64167, "x":2.26588, "y":6.89937, "heading":-0.93579, "vx":2.22741, "vy":-1.94611, "omega":-0.00326, "ax":3.42914, "ay":-3.04621, "alpha":-0.01262, "fx":[56.00633,56.09553,56.11342,56.02411], "fy":[-49.86046,-49.7604,-49.73879,-49.83906]}, + {"t":0.67942, "x":2.3524, "y":6.82374, "heading":-0.93592, "vx":2.35684, "vy":-2.06109, "omega":-0.00374, "ax":3.39903, "ay":-3.05554, "alpha":-0.01803, "fx":[55.49145,55.61952,55.64399,55.51569], "fy":[-50.03812,-49.89636,-49.86606,-50.00822]}, + {"t":0.71717, "x":2.44378, "y":6.74376, "heading":-0.93606, "vx":2.48514, "vy":-2.17642, "omega":-0.00442, "ax":3.31097, "ay":-3.08185, "alpha":-0.03392, "fx":[53.9863,54.23041,54.27039,54.0254], "fy":[-50.53895,-50.279,-50.22501,-50.48633]}, + {"t":0.75491, "x":2.53994, "y":6.65942, "heading":-0.93623, "vx":2.61011, "vy":-2.29275, "omega":-0.0057, "ax":0.22135, "ay":-3.01768, "alpha":-0.64591, "fx":[2.84089,6.99076,4.46698,0.17601], "fy":[-50.86345,-49.39344,-47.75772,-49.31855]}, + {"t":0.79266, "x":2.63862, "y":6.57073, "heading":-0.93644, "vx":2.61847, "vy":-2.40665, "omega":-0.03008, "ax":-3.52195, "ay":2.827, "alpha":-0.03365, "fx":[-57.71276,-57.49259,-57.44203,-57.66159], "fy":[46.04252,46.31378,46.3888,46.11893]}, + {"t":0.8304, "x":2.73495, "y":6.4819, "heading":-0.93758, "vx":2.48553, "vy":-2.29995, "omega":-0.03135, "ax":-3.50697, "ay":2.9282, "alpha":-0.0159, "fx":[-57.39841,-57.29076,-57.26634,-57.37383], "fy":[47.7903,47.91857,47.9506,47.82264]}, + {"t":0.86815, "x":2.82627, "y":6.39718, "heading":-0.93876, "vx":2.35316, "vy":-2.18942, "omega":-0.03195, "ax":-3.50169, "ay":2.96127, "alpha":-0.0101, "fx":[-57.28836,-57.21928,-57.20362,-57.27264], "fy":[48.36056,48.44198,48.46166,48.38037]}, + {"t":0.90589, "x":2.91259, "y":6.31664, "heading":-0.93996, "vx":2.22098, "vy":-2.07764, "omega":-0.03233, "ax":-3.49899, "ay":2.97769, "alpha":-0.0072, "fx":[-57.23227,-57.18272,-57.17147,-57.22098], "fy":[48.64352,48.7016,48.71545,48.65743]}, + {"t":0.94364, "x":2.99393, "y":6.24034, "heading":-0.94118, "vx":2.08891, "vy":-1.96525, "omega":-0.0326, "ax":-3.49735, "ay":2.9875, "alpha":-0.00547, "fx":[-57.19824,-57.16052,-57.15192,-57.18962], "fy":[48.81265,48.85672,48.86717,48.82313]}, + {"t":0.98138, "x":3.07029, "y":6.16829, "heading":-0.94242, "vx":1.9569, "vy":-1.85249, "omega":-0.03281, "ax":-3.49625, "ay":2.99403, "alpha":-0.00431, "fx":[-57.17536,-57.14559,-57.13877,-57.16854], "fy":[48.92516,48.95988,48.96809,48.9334]}, + {"t":1.01913, "x":3.14166, "y":6.1005, "heading":-0.94365, "vx":1.82494, "vy":-1.73947, "omega":-0.03297, "ax":-3.49546, "ay":2.99868, "alpha":-0.00348, "fx":[-57.15893,-57.13486,-57.12932,-57.15338], "fy":[49.00542,49.03344,49.04007,49.01206]}, + {"t":1.05688, "x":3.20806, "y":6.03698, "heading":-0.9449, "vx":1.693, "vy":-1.62629, "omega":-0.0331, "ax":-3.49486, "ay":3.00217, "alpha":-0.00285, "fx":[-57.14654,-57.12677,-57.1222,-57.14196], "fy":[49.06556,49.08855,49.09399,49.07102]}, + {"t":1.09462, "x":3.26947, "y":5.97773, "heading":-0.94615, "vx":1.56108, "vy":-1.51297, "omega":-0.03321, "ax":-3.4944, "ay":3.00488, "alpha":-0.00237, "fx":[-57.13686,-57.12045,-57.11664,-57.13304], "fy":[49.11232,49.13138,49.1359,49.11685]}, + {"t":1.13237, "x":3.3259, "y":5.92277, "heading":-0.9474, "vx":1.42918, "vy":-1.39955, "omega":-0.0333, "ax":-3.49402, "ay":3.00705, "alpha":-0.00198, "fx":[-57.12909,-57.11537,-57.11217,-57.12588], "fy":[49.1497,49.16562,49.16941,49.1535]}, + {"t":1.17011, "x":3.37736, "y":5.87208, "heading":-0.94866, "vx":1.2973, "vy":-1.28605, "omega":-0.03337, "ax":-3.49372, "ay":3.00882, "alpha":-0.00166, "fx":[-57.12271,-57.11121,-57.10851,-57.12001], "fy":[49.18029,49.19363,49.19681,49.18348]}, + {"t":1.20786, "x":3.42384, "y":5.82568, "heading":-0.94992, "vx":1.16543, "vy":-1.17248, "omega":-0.03344, "ax":-3.49346, "ay":3.0103, "alpha":-0.00139, "fx":[-57.11738,-57.10773,-57.10545,-57.1151], "fy":[49.20577,49.21696,49.21964,49.20845]}, + {"t":1.2456, "x":3.46534, "y":5.78357, "heading":-0.95118, "vx":1.03357, "vy":-1.05885, "omega":-0.03349, "ax":-1.12411, "ay":0.98635, "alpha":-0.42679, "fx":[-18.794,-16.80009,-17.95851,-19.95541], "fy":[14.41941,15.97602,17.82529,16.27887]}, + {"t":1.27746, "x":3.49769, "y":5.75034, "heading":-0.95225, "vx":0.99776, "vy":-1.02743, "omega":-0.04708, "ax":-1.11126, "ay":1.00453, "alpha":-0.39388, "fx":[-18.56966,-16.65637,-17.77713,-19.66488], "fy":[14.89258,16.30103,17.93894,16.55601]}, + {"t":1.30931, "x":3.52891, "y":5.71812, "heading":-0.95375, "vx":0.96236, "vy":-0.99543, "omega":-0.05963, "ax":-1.09943, "ay":1.01776, "alpha":-0.3607, "fx":[-18.33368,-16.63113,-17.61685,-19.31238], "fy":[15.20568,16.51089,18.06697,16.7704]}, + {"t":1.34117, "x":3.55901, "y":5.68693, "heading":-0.95565, "vx":0.92734, "vy":-0.96301, "omega":-0.07112, "ax":-1.08961, "ay":1.0285, "alpha":-0.32917, "fx":[-18.1538,-16.5481,-17.48123,-19.06894], "fy":[15.54129,16.70765,18.07708,16.93033]}, + {"t":1.37302, "x":3.588, "y":5.65678, "heading":-0.95791, "vx":0.89263, "vy":-0.93025, "omega":-0.08161, "ax":-1.08134, "ay":1.03739, "alpha":-0.29739, "fx":[-17.97869,-16.57007,-17.37846,-18.78402], "fy":[15.78195,16.84841,18.13404,17.07323]}, + {"t":1.40488, "x":3.61589, "y":5.62767, "heading":-0.96051, "vx":0.85818, "vy":-0.8972, "omega":-0.09108, "ax":-1.07428, "ay":1.04487, "alpha":-0.26705, "fx":[-17.84277,-16.53508,-17.28802,-18.58367], "fy":[16.05275,16.98958,18.10297,17.1812]}, + {"t":1.43673, "x":3.64268, "y":5.59962, "heading":-0.96341, "vx":0.82396, "vy":-0.86392, "omega":-0.09959, "ax":-1.06818, "ay":1.05124, "alpha":-0.23632, "fx":[-17.70491,-16.58292,-17.22043,-18.34258], "fy":[16.25241,17.09122,18.11761,17.28203]}, + {"t":1.46859, "x":3.66838, "y":5.57263, "heading":-0.96658, "vx":0.78993, "vy":-0.83043, "omega":-0.10712, "ax":-1.06286, "ay":1.05674, "alpha":-0.20684, "fx":[-17.59603,-16.57945,-17.15929,-18.16838], "fy":[16.48157,17.19903,18.06402,17.35816]}, + {"t":1.50044, "x":3.69301, "y":5.54671, "heading":-0.97, "vx":0.75607, "vy":-0.79677, "omega":-0.1137, "ax":-1.05819, "ay":1.06153, "alpha":-0.17685, "fx":[-17.48233,-16.64222,-17.11503,-17.95771], "fy":[16.65664,17.27691,18.05067,17.43179]}, + {"t":1.5323, "x":3.71655, "y":5.52187, "heading":-0.97362, "vx":0.72237, "vy":-0.76295, "omega":-0.11934, "ax":-1.05404, "ay":1.06574, "alpha":-0.14791, "fx":[-17.39072,-16.66124,-17.07445,-17.79976], "fy":[16.85716,17.36347,17.98398,17.48672]}, + {"t":1.56415, "x":3.73903, "y":5.49811, "heading":-0.97742, "vx":0.68879, "vy":-0.729, "omega":-0.12405, "ax":-1.05034, "ay":1.06947, "alpha":-0.11838, "fx":[-17.29332,-16.73298,-17.04669,-17.6112], "fy":[17.01717,17.42629,17.95037,17.54141]}, + {"t":1.59601, "x":3.76044, "y":5.47543, "heading":-0.98137, "vx":0.65533, "vy":-0.69493, "omega":-0.12782, "ax":-1.04702, "ay":1.0728, "alpha":-0.08971, "fx":[-17.21289,-16.76924,-17.02158,-17.46321], "fy":[17.19709,17.49874,17.87567,17.58129]}, + {"t":1.62786, "x":3.78078, "y":5.45384, "heading":-0.98544, "vx":0.62198, "vy":-0.66076, "omega":-0.13068, "ax":-1.04402, "ay":1.07578, "alpha":-0.06037, "fx":[-17.12679,-16.84661,-17.00614,-17.29124], "fy":[17.34809,17.55185,17.82619,17.62193]}, + {"t":1.65972, "x":3.80007, "y":5.43333, "heading":-0.98961, "vx":0.58872, "vy":-0.62649, "omega":-0.1326, "ax":-1.0413, "ay":1.07848, "alpha":-0.03171, "fx":[-17.05352,-16.89699,-16.9934,-17.14891], "fy":[17.51275,17.61464,17.74629,17.65056]}, + {"t":1.69157, "x":3.81829, "y":5.41392, "heading":-0.99383, "vx":0.55555, "vy":-0.59214, "omega":-0.13361, "ax":-1.03882, "ay":1.08092, "alpha":-0.00233, "fx":[-16.97536,-16.97809,-16.98757,-16.98959], "fy":[17.65886,17.66153,17.68346,17.6802]}, + {"t":1.72343, "x":3.83546, "y":5.39561, "heading":-0.99809, "vx":0.52246, "vy":-0.5577, "omega":-0.13369, "ax":-1.03655, "ay":1.08315, "alpha":0.02657, "fx":[-16.90644,-17.04074,-16.98523,-16.84977], "fy":[17.81206,17.71757,17.59991,17.70008]}, + {"t":1.75528, "x":3.85158, "y":5.37839, "heading":-1.00235, "vx":0.48944, "vy":-0.5232, "omega":-0.13284, "ax":-1.03446, "ay":1.08518, "alpha":0.05624, "fx":[-16.83393,-17.12455,-16.98718,-16.70019], "fy":[17.95632,17.76061,17.52514,17.72074]}, + {"t":1.78714, "x":3.86665, "y":5.36228, "heading":-1.00658, "vx":0.45649, "vy":-0.48863, "omega":-0.13105, "ax":-1.03254, "ay":1.08706, "alpha":0.08562, "fx":[-16.76735,-17.19845,-16.99399,-16.56039], "fy":[18.1009,17.81185,17.43876,17.7336]}, + {"t":1.81899, "x":3.88066, "y":5.34726, "heading":-1.01075, "vx":0.4236, "vy":-0.454, "omega":-0.12832, "ax":-1.03076, "ay":1.08878, "alpha":0.1158, "fx":[-16.69886,-17.28447,-17.00241,-16.41824], "fy":[18.24569,17.85273,17.35275,17.74666]}, + {"t":1.85085, "x":3.89363, "y":5.33335, "heading":-1.01484, "vx":0.39076, "vy":-0.41932, "omega":-0.12463, "ax":-1.02911, "ay":1.09037, "alpha":0.14589, "fx":[-16.63313,-17.36914,-17.01756,-16.2764], "fy":[18.38389,17.90045,17.26388,17.75378]}, + {"t":1.8827, "x":3.90556, "y":5.32055, "heading":-1.01881, "vx":0.35798, "vy":-0.38459, "omega":-0.11998, "ax":-1.02758, "ay":1.09185, "alpha":0.17681, "fx":[-16.56751,-17.45724,-17.03145,-16.13981], "fy":[18.53119,17.94036,17.16683,17.76023]}, + {"t":1.91456, "x":3.91644, "y":5.30885, "heading":-1.02263, "vx":0.32525, "vy":-0.34981, "omega":-0.11435, "ax":-1.02615, "ay":1.09322, "alpha":0.20784, "fx":[-16.50152,-17.55263,-17.05438,-15.99407], "fy":[18.66486,17.98542,17.0755,17.76263]}, + {"t":1.94641, "x":3.92628, "y":5.29826, "heading":-1.02627, "vx":0.29256, "vy":-0.31498, "omega":-0.10773, "ax":-1.02482, "ay":1.0945, "alpha":0.23975, "fx":[-16.43798,-17.64294,-17.07296,-15.86142], "fy":[18.81645,18.02521,16.9672,17.76326]}, + {"t":1.97827, "x":3.93508, "y":5.28879, "heading":-1.02971, "vx":0.25991, "vy":-0.28012, "omega":-0.10009, "ax":-1.02357, "ay":1.0957, "alpha":0.27197, "fx":[-16.37091,-17.7493,-17.10328,-15.71004], "fy":[18.94718,18.06814,16.87323,17.76179]}, + {"t":2.01012, "x":3.94284, "y":5.28042, "heading":-1.03289, "vx":0.22731, "vy":-0.24521, "omega":-0.09143, "ax":-1.02239, "ay":1.09682, "alpha":0.3051, "fx":[-16.3089,-17.84215,-17.12588,-15.57987], "fy":[19.10472,18.10839,16.7532,17.75727]}, + {"t":2.04198, "x":3.94956, "y":5.27316, "heading":-1.03581, "vx":0.19474, "vy":-0.21027, "omega":-0.08171, "ax":-1.02129, "ay":1.09787, "alpha":0.33872, "fx":[-16.24026,-17.96003,-17.16326,-15.42109], "fy":[19.23356,18.14955,16.65634,17.75285]}, + {"t":2.07383, "x":3.95525, "y":5.26702, "heading":-1.03841, "vx":0.1622, "vy":-0.1753, "omega":-0.07092, "ax":-1.02025, "ay":1.09886, "alpha":0.37336, "fx":[-16.1794,-18.05593,-17.18927,-15.29206], "fy":[19.39908,18.19047,16.52366,17.74371]}, + {"t":2.10569, "x":3.9599, "y":5.262, "heading":-1.04067, "vx":0.1297, "vy":-0.1403, "omega":-0.05903, "ax":-1.01927, "ay":1.09979, "alpha":0.40869, "fx":[-16.10875,-18.18624,-17.23343,-15.12409], "fy":[19.52786,18.22943,16.42339,17.7371]}, + {"t":2.13754, "x":3.96351, "y":5.25809, "heading":-1.04255, "vx":0.09724, "vy":-0.10526, "omega":-0.04601, "ax":-1.01834, "ay":1.10067, "alpha":0.44508, "fx":[-16.04902,-18.28574,-17.26224,-14.99489], "fy":[19.70254,18.2716,16.27701,17.72406]}, + {"t":2.1694, "x":3.96609, "y":5.25529, "heading":-1.04401, "vx":0.0648, "vy":-0.0702, "omega":-0.03183, "ax":-1.01747, "ay":1.1015, "alpha":0.48241, "fx":[-15.97628,-18.4295,-17.31286,-14.81583], "fy":[19.83248,18.30855,16.17254,17.71593]}, + {"t":2.20125, "x":3.96764, "y":5.25361, "heading":-1.04503, "vx":0.03238, "vy":-0.03511, "omega":-0.01646, "ax":-1.01663, "ay":1.10228, "alpha":0.51683, "fx":[-15.919,-18.53043,-17.34202,-14.68862], "fy":[19.99151,18.34685,16.04057,17.70193]}, + {"t":2.23311, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index ee67414c..36d866ab 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"slowaccel", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,63 +30,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.235,2.011], + "waypoints":[0.0,1.19635,2.22924], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.08941, "ay":-3.42846, "alpha":-0.00579, "fx":[50.48043,50.52718,50.53164,50.48485], "fy":[-56.07195,-56.02984,-56.02573,-56.06788]}, - {"t":0.03859, "x":1.55245, "y":7.52085, "heading":-0.93501, "vx":0.11923, "vy":-0.13232, "omega":-0.00022, "ax":3.08922, "ay":-3.42827, "alpha":-0.00593, "fx":[50.47666,50.52451,50.52907,50.48118], "fy":[-56.06936,-56.02626,-56.02205,-56.06519]}, - {"t":0.07719, "x":1.55935, "y":7.51319, "heading":-0.93501, "vx":0.23846, "vy":-0.26463, "omega":-0.00045, "ax":3.089, "ay":-3.42806, "alpha":-0.00608, "fx":[50.47244,50.52152,50.52619,50.47708], "fy":[-56.06646,-56.02226,-56.01794,-56.06218]}, - {"t":0.11578, "x":1.57086, "y":7.50042, "heading":-0.93503, "vx":0.35767, "vy":-0.39693, "omega":-0.00069, "ax":3.08876, "ay":-3.42782, "alpha":-0.00625, "fx":[50.4677,50.51815,50.52296,50.47246], "fy":[-56.0632,-56.01776,-56.01331,-56.0588]}, - {"t":0.15437, "x":1.58696, "y":7.48255, "heading":-0.93506, "vx":0.47688, "vy":-0.52922, "omega":-0.00093, "ax":3.08848, "ay":-3.42754, "alpha":-0.00645, "fx":[50.46233,50.51434,50.51929,50.46723], "fy":[-56.05951,-56.01266,-56.00807,-56.05497]}, - {"t":0.19297, "x":1.60767, "y":7.45957, "heading":-0.93509, "vx":0.59608, "vy":-0.6615, "omega":-0.00118, "ax":3.08817, "ay":-3.42723, "alpha":-0.00667, "fx":[50.45618,50.50998,50.5151,50.46126], "fy":[-56.05529,-56.00683,-56.00207,-56.05058]}, - {"t":0.23156, "x":1.63297, "y":7.43149, "heading":-0.93514, "vx":0.71526, "vy":-0.79377, "omega":-0.00143, "ax":3.0878, "ay":-3.42687, "alpha":-0.00693, "fx":[50.4491,50.50496,50.51027,50.45436], "fy":[-56.05042,-56.0001,-55.99516,-56.04553]}, - {"t":0.27016, "x":1.66288, "y":7.3983, "heading":-0.9352, "vx":0.83443, "vy":-0.92603, "omega":-0.0017, "ax":3.08738, "ay":-3.42645, "alpha":-0.00722, "fx":[50.44083,50.49909,50.50463,50.44631], "fy":[-56.04474,-55.99226,-55.98709,-56.03963]}, - {"t":0.30875, "x":1.69738, "y":7.36001, "heading":-0.93526, "vx":0.95358, "vy":-1.05827, "omega":-0.00198, "ax":3.08687, "ay":-3.42595, "alpha":-0.00758, "fx":[50.43106,50.49216,50.49797,50.4368], "fy":[-56.03803,-55.98299,-55.97756,-56.03266]}, - {"t":0.34734, "x":1.73648, "y":7.31662, "heading":-0.93534, "vx":1.07272, "vy":-1.19049, "omega":-0.00227, "ax":3.08627, "ay":-3.42536, "alpha":-0.008, "fx":[50.41933,50.48385,50.48997,50.42539], "fy":[-56.02997,-55.97187,-55.96612,-56.02429]}, - {"t":0.38594, "x":1.78018, "y":7.26812, "heading":-0.93542, "vx":1.19183, "vy":-1.32268, "omega":-0.00258, "ax":3.08553, "ay":-3.42463, "alpha":-0.00852, "fx":[50.40501,50.47369,50.4802,50.41144], "fy":[-56.02013,-55.95828,-55.95213,-56.01406]}, - {"t":0.42453, "x":1.82847, "y":7.21452, "heading":-0.93552, "vx":1.31091, "vy":-1.45485, "omega":-0.00291, "ax":3.08461, "ay":-3.42372, "alpha":-0.00917, "fx":[50.3871,50.46099,50.46799,50.39401], "fy":[-56.00783,-55.9413,-55.93466,-56.00128]}, - {"t":0.46312, "x":1.88136, "y":7.15582, "heading":-0.93564, "vx":1.42996, "vy":-1.58699, "omega":-0.00326, "ax":3.08343, "ay":-3.42255, "alpha":-0.01, "fx":[50.36409,50.44468,50.4523,50.3716], "fy":[-55.99203,-55.91948,-55.91219,-55.98486]}, - {"t":0.50172, "x":1.93885, "y":7.09203, "heading":-0.93576, "vx":1.54896, "vy":-1.71908, "omega":-0.00365, "ax":3.08185, "ay":-3.42099, "alpha":-0.01112, "fx":[50.33342,50.42293,50.43137,50.34173], "fy":[-55.97096,-55.89039,-55.88224,-55.96295]}, - {"t":0.54031, "x":2.00092, "y":7.02313, "heading":-0.9359, "vx":1.6679, "vy":-1.85111, "omega":-0.00408, "ax":3.07964, "ay":-3.41881, "alpha":-0.01268, "fx":[50.29051,50.39252,50.40209,50.29992], "fy":[-55.94149,-55.84969,-55.84032,-55.9323]}, - {"t":0.57891, "x":2.06759, "y":6.94915, "heading":-0.93606, "vx":1.78675, "vy":-1.98305, "omega":-0.00457, "ax":3.07633, "ay":-3.41553, "alpha":-0.01502, "fx":[50.22621,50.34694,50.3582,50.23724], "fy":[-55.89732,-55.78872,-55.77747,-55.88633]}, - {"t":0.6175, "x":2.13883, "y":6.87007, "heading":-0.93624, "vx":1.90548, "vy":-2.11487, "omega":-0.00515, "ax":3.07082, "ay":-3.41009, "alpha":-0.01892, "fx":[50.11924,50.27112,50.2851,50.13286], "fy":[-55.82382,-55.68727,-55.67283,-55.80977]}, - {"t":0.65609, "x":2.21466, "y":6.78591, "heading":-0.93644, "vx":2.02399, "vy":-2.24648, "omega":-0.00588, "ax":3.05983, "ay":-3.39922, "alpha":-0.02674, "fx":[49.90607,50.11998,50.13916,49.92452], "fy":[-55.67721,-55.48511,-55.46394,-55.65684]}, - {"t":0.69469, "x":2.29505, "y":6.69668, "heading":-0.93666, "vx":2.14208, "vy":-2.37766, "omega":-0.00691, "ax":3.02711, "ay":-3.36684, "alpha":-0.05019, "fx":[49.27296,49.67066,49.70323,49.30303], "fy":[-55.24051,-54.8846,-54.84084,-55.19947]}, - {"t":0.73328, "x":2.37998, "y":6.60241, "heading":-0.93693, "vx":2.25891, "vy":-2.5076, "omega":-0.00885, "ax":-0.03913, "ay":-0.15143, "alpha":-2.2462, "fx":[-2.05033,8.04867,0.7743,-9.33143], "fy":[-11.08514,-3.74474,6.20253,-1.27508]}, - {"t":0.77187, "x":2.46713, "y":6.50552, "heading":-0.93727, "vx":2.2574, "vy":-2.51345, "omega":-0.09554, "ax":-3.03493, "ay":3.3596, "alpha":-0.04548, "fx":[-49.81085,-49.44917,-49.42086,-49.78051], "fy":[54.74055,55.06615,55.10447,54.78112]}, - {"t":0.81047, "x":2.55199, "y":6.41102, "heading":-0.94096, "vx":2.14027, "vy":-2.38379, "omega":-0.09729, "ax":-3.06381, "ay":3.39559, "alpha":-0.02173, "fx":[-50.18269,-50.0086,-49.9926,-50.16622], "fy":[55.42409,55.58088,55.59839,55.44212]}, - {"t":0.84906, "x":2.63231, "y":6.32155, "heading":-0.94471, "vx":2.02203, "vy":-2.25274, "omega":-0.09813, "ax":-3.07349, "ay":3.40766, "alpha":-0.01372, "fx":[-50.30635,-50.1962,-50.1853,-50.29526], "fy":[55.6535,55.75272,55.76385,55.66484]}, - {"t":0.88766, "x":2.70806, "y":6.23714, "heading":-0.9485, "vx":1.90341, "vy":-2.12123, "omega":-0.09866, "ax":-3.07834, "ay":3.41371, "alpha":-0.00966, "fx":[-50.36797,-50.2903,-50.28219,-50.35977], "fy":[55.76862,55.83859,55.84659,55.77672]}, - {"t":0.92625, "x":2.77923, "y":6.15782, "heading":-0.95231, "vx":1.7846, "vy":-1.98948, "omega":-0.09903, "ax":-3.08126, "ay":3.41734, "alpha":-0.00721, "fx":[-50.40481,-50.34688,-50.34055,-50.39843], "fy":[55.83788,55.89007,55.89619,55.84405]}, - {"t":0.96484, "x":2.84581, "y":6.08358, "heading":-0.95613, "vx":1.66569, "vy":-1.85759, "omega":-0.09931, "ax":-3.0832, "ay":3.41977, "alpha":-0.00555, "fx":[-50.42928,-50.38466,-50.37958,-50.42418], "fy":[55.88416,55.92436,55.9292,55.88903]}, - {"t":1.00344, "x":2.90779, "y":6.01444, "heading":-0.95996, "vx":1.5467, "vy":-1.72561, "omega":-0.09953, "ax":-3.08459, "ay":3.4215, "alpha":-0.00436, "fx":[-50.44669,-50.41167,-50.40754,-50.44254], "fy":[55.91728,55.94884,55.95274,55.92121]}, - {"t":1.04203, "x":2.96519, "y":5.95039, "heading":-0.9638, "vx":1.42765, "vy":-1.59356, "omega":-0.09969, "ax":-3.08563, "ay":3.4228, "alpha":-0.00345, "fx":[-50.45971,-50.43195,-50.42856,-50.45631], "fy":[55.94217,55.96718,55.97036,55.94536]}, - {"t":1.08062, "x":3.01799, "y":5.89144, "heading":-0.96765, "vx":1.30856, "vy":-1.46146, "omega":-0.09983, "ax":-3.08644, "ay":3.42381, "alpha":-0.00275, "fx":[-50.46981,-50.44773,-50.44494,-50.46701], "fy":[55.96156,55.98145,55.98405,55.96417]}, - {"t":1.11922, "x":3.06619, "y":5.83758, "heading":-0.9715, "vx":1.18945, "vy":-1.32932, "omega":-0.09993, "ax":-3.08709, "ay":3.42462, "alpha":-0.00218, "fx":[-50.47785,-50.46036,-50.45808,-50.47557], "fy":[55.97709,55.99286,55.99497,55.97921]}, - {"t":1.15781, "x":3.1098, "y":5.78883, "heading":-0.97536, "vx":1.0703, "vy":-1.19715, "omega":-0.10002, "ax":-3.08762, "ay":3.42528, "alpha":-0.00171, "fx":[-50.48442,-50.47069,-50.46884,-50.48257], "fy":[55.98982,56.00219,56.0039,55.99153]}, - {"t":1.19641, "x":3.14881, "y":5.74518, "heading":-0.97922, "vx":0.95114, "vy":-1.06496, "omega":-0.10008, "ax":-3.08806, "ay":3.42584, "alpha":-0.00132, "fx":[-50.48987,-50.47929,-50.47783,-50.48841], "fy":[56.00044,56.00997,56.01132,56.0018]}, - {"t":1.235, "x":3.18322, "y":5.70663, "heading":-0.98308, "vx":0.83196, "vy":-0.93274, "omega":-0.10013, "ax":-0.9744, "ay":-0.82976, "alpha":-0.497, "fx":[-16.18986,-14.0089,-15.68417,-17.83521], "fy":[-15.44631,-14.08815,-11.65841,-13.06725]}, - {"t":1.27027, "x":3.21196, "y":5.67321, "heading":-0.98662, "vx":0.79759, "vy":-0.96201, "omega":-0.11766, "ax":-0.11311, "ay":-0.09339, "alpha":-0.4615, "fx":[-2.22683,-0.07972,-1.47285,-3.61746], "fy":[-3.28669,-1.87101,0.23565,-1.18487]}, - {"t":1.30554, "x":3.24002, "y":5.63922, "heading":-0.99077, "vx":0.7936, "vy":-0.96531, "omega":-0.13394, "ax":-0.01201, "ay":-0.00987, "alpha":-0.34675, "fx":[-0.48638,1.13169,0.09378,-1.52415], "fy":[-1.48326,-0.42327,1.16084,0.10057]}, - {"t":1.34082, "x":3.268, "y":5.60517, "heading":-0.99549, "vx":0.79318, "vy":-0.96565, "omega":-0.14617, "ax":-0.00127, "ay":-0.00104, "alpha":-0.23282, "fx":[-0.21974,0.87001,0.1782,-0.91154], "fy":[-0.90379,-0.19712,0.86969,0.163]}, - {"t":1.37609, "x":3.29598, "y":5.5711, "heading":-1.00065, "vx":0.79313, "vy":-0.96569, "omega":-0.15439, "ax":-0.00013, "ay":-0.00011, "alpha":-0.12006, "fx":[-0.10716,0.45668,0.10277,-0.46107], "fy":[-0.45855,-0.09703,0.45494,0.09342]}, - {"t":1.41136, "x":3.32396, "y":5.53704, "heading":-1.00609, "vx":0.79313, "vy":-0.9657, "omega":-0.15862, "ax":-0.00001, "ay":-0.00001, "alpha":-0.00791, "fx":[-0.00731,0.02998,0.00685,-0.03044], "fy":[-0.03026,-0.00663,0.02987,0.00625]}, - {"t":1.44664, "x":3.35193, "y":5.50298, "heading":-1.01169, "vx":0.79313, "vy":-0.9657, "omega":-0.1589, "ax":0.0, "ay":0.0, "alpha":0.10419, "fx":[0.09544,-0.39732,-0.09549,0.39727], "fy":[0.39534,0.08701,-0.39538,-0.08705]}, - {"t":1.48191, "x":3.37991, "y":5.46892, "heading":-1.01729, "vx":0.79313, "vy":-0.9657, "omega":-0.15522, "ax":0.0, "ay":0.0, "alpha":0.21683, "fx":[0.20327,-0.82576,-0.20327,0.82575], "fy":[0.82161,0.18574,-0.82162,-0.18575]}, - {"t":1.51718, "x":3.40788, "y":5.43485, "heading":-1.02277, "vx":0.79313, "vy":-0.9657, "omega":-0.14758, "ax":0.0, "ay":0.0, "alpha":0.33057, "fx":[0.31674,-1.25732,-0.31674,1.25732], "fy":[1.25086,0.29006,-1.25086,-0.29006]}, - {"t":1.55245, "x":3.43586, "y":5.40079, "heading":-1.02797, "vx":0.79313, "vy":-0.9657, "omega":-0.13592, "ax":0.0, "ay":0.0, "alpha":0.44597, "fx":[0.4361,-1.69421,-0.4361,1.69421], "fy":[1.68532,0.40015,-1.68532,-0.40015]}, - {"t":1.58773, "x":3.46384, "y":5.36673, "heading":-1.03277, "vx":0.79313, "vy":-0.9657, "omega":-0.12019, "ax":0.0, "ay":0.0, "alpha":0.56363, "fx":[0.56136,-2.13873,-0.56136,2.13873], "fy":[2.12727,0.51598,-2.12727,-0.51598]}, - {"t":1.623, "x":3.49181, "y":5.33266, "heading":-1.03701, "vx":0.79313, "vy":-0.9657, "omega":-0.10031, "ax":0.0, "ay":0.0, "alpha":0.68412, "fx":[0.69231,-2.59328,-0.69231,2.59327], "fy":[2.57913,0.63728,-2.57913,-0.63729]}, - {"t":1.65827, "x":3.51979, "y":5.2986, "heading":-1.04054, "vx":0.79313, "vy":-0.9657, "omega":-0.07617, "ax":0.0, "ay":0.0, "alpha":0.80805, "fx":[0.82849,-3.06034,-0.82849,3.06033], "fy":[3.0434,0.76355,-3.0434,-0.76356]}, - {"t":1.69355, "x":3.54776, "y":5.26454, "heading":-1.04323, "vx":0.79313, "vy":-0.9657, "omega":-0.04767, "ax":-0.00015, "ay":0.00018, "alpha":0.936, "fx":[0.96671,-3.54498,-0.97159,3.54012], "fy":[3.52564,0.8969,-3.51982,-0.89107]}, - {"t":1.72882, "x":3.57574, "y":5.23048, "heading":-1.04491, "vx":0.79312, "vy":-0.96569, "omega":-0.01466, "ax":-2.02463, "ay":2.46515, "alpha":0.38307, "fx":[-32.021,-34.47573,-34.21918,-31.67969], "fy":[41.76193,39.97843,38.80229,40.65941]}, - {"t":1.76409, "x":3.60246, "y":5.19795, "heading":-1.04543, "vx":0.72171, "vy":-0.87874, "omega":-0.00115, "ax":-2.90753, "ay":3.54014, "alpha":0.01041, "fx":[-47.48284,-47.56901,-47.58228,-47.496], "fy":[57.91577,57.84522,57.83311,57.90379]}, - {"t":1.79936, "x":3.6261, "y":5.16915, "heading":-1.04547, "vx":0.61915, "vy":-0.75387, "omega":-0.00078, "ax":-2.91971, "ay":3.55497, "alpha":0.00585, "fx":[-47.70365,-47.75227,-47.75991,-47.71126], "fy":[58.1403,58.10043,58.09381,58.13371]}, - {"t":1.83464, "x":3.64613, "y":5.14477, "heading":-1.0455, "vx":0.51617, "vy":-0.62847, "omega":-0.00057, "ax":-2.92385, "ay":3.56001, "alpha":0.00431, "fx":[-47.77856,-47.81441,-47.82008,-47.78422], "fy":[58.21642,58.18701,58.18218,58.21161]}, - {"t":1.86991, "x":3.66251, "y":5.12482, "heading":-1.04552, "vx":0.41303, "vy":-0.5029, "omega":-0.00042, "ax":-2.92592, "ay":3.56254, "alpha":0.00353, "fx":[-47.81626,-47.84568,-47.85035,-47.82092], "fy":[58.25472,58.23058,58.22664,58.2508]}, - {"t":1.90518, "x":3.67526, "y":5.1093, "heading":-1.04553, "vx":0.30983, "vy":-0.37724, "omega":-0.00029, "ax":-2.92718, "ay":3.56406, "alpha":0.00307, "fx":[-47.83895,-47.8645,-47.86857,-47.84301], "fy":[58.27778,58.25681,58.2534,58.27438]}, - {"t":1.94045, "x":3.68437, "y":5.09821, "heading":-1.04554, "vx":0.20658, "vy":-0.25153, "omega":-0.00019, "ax":-2.92801, "ay":3.56508, "alpha":0.00276, "fx":[-47.85411,-47.87707,-47.88073,-47.85776], "fy":[58.29318,58.27433,58.27127,58.29013]}, - {"t":1.97573, "x":3.68984, "y":5.09156, "heading":-1.04555, "vx":0.1033, "vy":-0.12578, "omega":-0.00009, "ax":-2.92861, "ay":3.56581, "alpha":0.00253, "fx":[-47.86495,-47.88607,-47.88943,-47.86832], "fy":[58.3042,58.28686,58.28405,58.30139]}, - {"t":2.011, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.12866, "ay":-3.39248, "alpha":-0.00255, "fx":[51.13646,51.1568,51.15897,51.13862], "fy":[-55.47103,-55.45227,-55.45024,-55.469]}, + {"t":0.03739, "x":1.55234, "y":7.52103, "heading":-0.93501, "vx":0.11697, "vy":-0.12683, "omega":-0.0001, "ax":3.12833, "ay":-3.39243, "alpha":-0.00261, "fx":[51.13076,51.15161,51.15383,51.13297], "fy":[-55.47038,-55.45115,-55.44906,-55.4683]}, + {"t":0.07477, "x":1.5589, "y":7.51392, "heading":-0.93501, "vx":0.23392, "vy":-0.25366, "omega":-0.00019, "ax":3.12796, "ay":-3.39237, "alpha":-0.00268, "fx":[51.12442,51.14584,51.14812,51.12669], "fy":[-55.46965,-55.44991,-55.44776,-55.46751]}, + {"t":0.11216, "x":1.56983, "y":7.50206, "heading":-0.93502, "vx":0.35087, "vy":-0.38049, "omega":-0.00029, "ax":3.12755, "ay":-3.3923, "alpha":-0.00276, "fx":[51.11734,51.1394,51.14174,51.11968], "fy":[-55.46884,-55.44852,-55.44631,-55.46664]}, + {"t":0.14954, "x":1.58513, "y":7.48547, "heading":-0.93503, "vx":0.46779, "vy":-0.50731, "omega":-0.0004, "ax":3.12709, "ay":-3.39222, "alpha":-0.00285, "fx":[51.10938,51.13214,51.13456,51.11178], "fy":[-55.46793,-55.44695,-55.44467,-55.46566]}, + {"t":0.18693, "x":1.60481, "y":7.46413, "heading":-0.93504, "vx":0.5847, "vy":-0.63413, "omega":-0.0005, "ax":3.12656, "ay":-3.39213, "alpha":-0.00295, "fx":[51.10035,51.12392,51.12642,51.10284], "fy":[-55.4669,-55.44518,-55.44281,-55.46454]}, + {"t":0.22432, "x":1.62885, "y":7.43805, "heading":-0.93506, "vx":0.70159, "vy":-0.76095, "omega":-0.00061, "ax":3.12596, "ay":-3.39203, "alpha":-0.00307, "fx":[51.09003,51.11453,51.11712,51.09261], "fy":[-55.46572,-55.44315,-55.44069,-55.46327]}, + {"t":0.2617, "x":1.65726, "y":7.40723, "heading":-0.93508, "vx":0.81846, "vy":-0.88776, "omega":-0.00073, "ax":3.12527, "ay":-3.39191, "alpha":-0.0032, "fx":[51.07812,51.10368,51.10638,51.08081], "fy":[-55.46435,-55.44081,-55.43824,-55.4618]}, + {"t":0.29909, "x":1.69005, "y":7.37167, "heading":-0.93511, "vx":0.9353, "vy":-1.01457, "omega":-0.00085, "ax":3.12446, "ay":-3.39178, "alpha":-0.00336, "fx":[51.06423,51.09104,51.09386,51.06705], "fy":[-55.46276,-55.43808,-55.43539,-55.46008]}, + {"t":0.33647, "x":1.7272, "y":7.33137, "heading":-0.93514, "vx":1.05211, "vy":-1.14138, "omega":-0.00097, "ax":3.12351, "ay":-3.39162, "alpha":-0.00354, "fx":[51.04782,51.07609,51.07906,51.05078], "fy":[-55.46088,-55.43486,-55.43201,-55.45805]}, + {"t":0.37386, "x":1.76871, "y":7.28633, "heading":-0.93518, "vx":1.16889, "vy":-1.26818, "omega":-0.00111, "ax":3.12236, "ay":-3.39142, "alpha":-0.00377, "fx":[51.02812,51.05815,51.0613,51.03125], "fy":[-55.45863,-55.43098,-55.42796,-55.45562]}, + {"t":0.41125, "x":1.8146, "y":7.23655, "heading":-0.93522, "vx":1.28562, "vy":-1.39497, "omega":-0.00125, "ax":3.12096, "ay":-3.39119, "alpha":-0.00404, "fx":[51.00404,51.03623,51.0396,51.00739], "fy":[-55.45587,-55.42625,-55.42301,-55.45264]}, + {"t":0.44863, "x":1.86484, "y":7.18202, "heading":-0.93527, "vx":1.4023, "vy":-1.52175, "omega":-0.0014, "ax":3.11921, "ay":-3.39089, "alpha":-0.00437, "fx":[50.97394,51.00884,51.01247,50.97756], "fy":[-55.45242,-55.42034,-55.41682,-55.44892]}, + {"t":0.48602, "x":1.91945, "y":7.12276, "heading":-0.93532, "vx":1.51891, "vy":-1.64852, "omega":-0.00156, "ax":3.11696, "ay":-3.39051, "alpha":-0.00481, "fx":[50.93525,50.97361,50.97759,50.9392], "fy":[-55.44798,-55.41274,-55.40887,-55.44413]}, + {"t":0.5234, "x":1.97841, "y":7.05876, "heading":-0.93538, "vx":1.63544, "vy":-1.77528, "omega":-0.00174, "ax":3.11396, "ay":-3.39001, "alpha":-0.00539, "fx":[50.88367,50.92666,50.93108,50.88806], "fy":[-55.44205,-55.4026,-55.39825,-55.43774]}, + {"t":0.56079, "x":2.04173, "y":6.99002, "heading":-0.93544, "vx":1.75186, "vy":-1.90202, "omega":-0.00194, "ax":3.10976, "ay":-3.3893, "alpha":-0.0062, "fx":[50.81148,50.86095,50.86598,50.81647], "fy":[-55.43374,-55.3884,-55.38339,-55.42877]}, + {"t":0.59817, "x":2.1094, "y":6.91654, "heading":-0.93552, "vx":1.86812, "vy":-2.02873, "omega":-0.00218, "ax":3.10347, "ay":-3.38823, "alpha":-0.00742, "fx":[50.70324,50.76244,50.76836,50.70911], "fy":[-55.42125,-55.3671,-55.36109,-55.4153]}, + {"t":0.63556, "x":2.18141, "y":6.83833, "heading":-0.9356, "vx":1.98415, "vy":-2.1554, "omega":-0.00245, "ax":3.09298, "ay":-3.38646, "alpha":-0.00946, "fx":[50.52299,50.5984,50.60572,50.53022], "fy":[-55.40034,-55.33158,-55.3239,-55.39276]}, + {"t":0.67295, "x":2.25775, "y":6.75538, "heading":-0.93569, "vx":2.09978, "vy":-2.28201, "omega":-0.00281, "ax":3.07203, "ay":-3.3829, "alpha":-0.01354, "fx":[50.16305,50.2709,50.28076,50.17273], "fy":[-55.35824,-55.26054,-55.24945,-55.34736]}, + {"t":0.71033, "x":2.3384, "y":6.6677, "heading":-0.93579, "vx":2.21463, "vy":-2.40848, "omega":-0.00331, "ax":3.00938, "ay":-3.37215, "alpha":-0.02578, "fx":[49.08777,49.29279,49.30798,49.1023], "fy":[-55.22982,-55.04768,-55.02615,-55.209]}, + {"t":0.74772, "x":2.4233, "y":6.5753, "heading":-0.93592, "vx":2.32714, "vy":-2.53455, "omega":-0.00428, "ax":-1.20515, "ay":-0.89187, "alpha":-0.89784, "fx":[-20.09712,-16.24615,-19.34566,-23.11871], "fy":[-18.05042,-15.29237,-11.05849,-13.92032]}, + {"t":0.7851, "x":2.50946, "y":6.47992, "heading":-0.93608, "vx":2.28209, "vy":-2.5679, "omega":-0.03784, "ax":-3.12386, "ay":3.26686, "alpha":-0.02366, "fx":[-51.16996,-50.98763,-50.96863,-51.15044], "fy":[53.30733,53.48077,53.50631,53.33351]}, + {"t":0.82249, "x":2.59259, "y":6.3862, "heading":-0.93749, "vx":2.1653, "vy":-2.44576, "omega":-0.03873, "ax":-3.12956, "ay":3.32986, "alpha":-0.01141, "fx":[-51.21202,-51.12254,-51.11286,-51.20221], "fy":[54.38951,54.47339,54.48428,54.40054]}, + {"t":0.85988, "x":2.67136, "y":6.29709, "heading":-0.93894, "vx":2.0483, "vy":-2.32127, "omega":-0.03915, "ax":-3.13144, "ay":3.35098, "alpha":-0.00729, "fx":[-51.22498,-51.16749,-51.16114,-51.21858], "fy":[54.752,54.80564,54.81234,54.75876]}, + {"t":0.89726, "x":2.74575, "y":6.21265, "heading":-0.9404, "vx":1.93122, "vy":-2.19599, "omega":-0.03943, "ax":-3.13237, "ay":3.36157, "alpha":-0.00521, "fx":[-51.23115,-51.18992,-51.18529,-51.22649], "fy":[54.93367,54.97204,54.97677,54.93843]}, + {"t":0.93465, "x":2.81576, "y":6.1329, "heading":-0.94188, "vx":1.81412, "vy":-2.07032, "omega":-0.03962, "ax":-3.13292, "ay":3.36793, "alpha":-0.00396, "fx":[-51.2347,-51.20336,-51.19978,-51.23111], "fy":[55.04285,55.07198,55.07555,55.04644]}, + {"t":0.97203, "x":2.88139, "y":6.05785, "heading":-0.94336, "vx":1.69699, "vy":-1.9444, "omega":-0.03977, "ax":-3.13328, "ay":3.37217, "alpha":-0.00312, "fx":[-51.23699,-51.2123,-51.20944,-51.23413], "fy":[55.11571,55.13864,55.14146,55.11854]}, + {"t":1.00942, "x":2.94265, "y":5.98752, "heading":-0.94485, "vx":1.57985, "vy":-1.81833, "omega":-0.03989, "ax":-3.13354, "ay":3.37521, "alpha":-0.00251, "fx":[-51.23858,-51.21868,-51.21634,-51.23624], "fy":[55.16781,55.18628,55.18856,55.1701]}, + {"t":1.04681, "x":2.99952, "y":5.9219, "heading":-0.94634, "vx":1.4627, "vy":-1.69215, "omega":-0.03998, "ax":-3.13374, "ay":3.37748, "alpha":-0.00205, "fx":[-51.23974,-51.22345,-51.22152,-51.2378], "fy":[55.20692,55.22202,55.22389,55.2088]}, + {"t":1.08419, "x":3.05201, "y":5.86099, "heading":-0.94783, "vx":1.34554, "vy":-1.56588, "omega":-0.04006, "ax":-3.13389, "ay":3.37925, "alpha":-0.00169, "fx":[-51.24061,-51.22716,-51.22554,-51.23899], "fy":[55.23736,55.24982,55.25139,55.23892]}, + {"t":1.12158, "x":3.10013, "y":5.80481, "heading":-0.94933, "vx":1.22838, "vy":-1.43954, "omega":-0.04012, "ax":-3.13401, "ay":3.38067, "alpha":-0.00141, "fx":[-51.24129,-51.23012,-51.22876,-51.23993], "fy":[55.26172,55.27208,55.27338,55.26303]}, + {"t":1.15896, "x":3.14386, "y":5.75336, "heading":-0.95083, "vx":1.11121, "vy":-1.31315, "omega":-0.04017, "ax":-3.1341, "ay":3.38183, "alpha":-0.00117, "fx":[-51.24184,-51.23254,-51.23139,-51.24069], "fy":[55.28167,55.29029,55.29138,55.28277]}, + {"t":1.19635, "x":3.18322, "y":5.70663, "heading":-0.95233, "vx":0.99404, "vy":-1.18672, "omega":-0.04022, "ax":-1.01055, "ay":1.10185, "alpha":-0.35139, "fx":[-16.87622,-15.19648,-16.16097,-17.84881], "fy":[16.63621,17.90516,19.38537,18.12558]}, + {"t":1.22765, "x":3.21383, "y":5.67002, "heading":-0.95359, "vx":0.96241, "vy":-1.15223, "omega":-0.05121, "ax":-1.00296, "ay":1.11244, "alpha":-0.32555, "fx":[-16.7391,-15.13652,-16.0639,-17.64674], "fy":[16.93242,18.10031,19.427,18.28529]}, + {"t":1.25895, "x":3.24347, "y":5.63451, "heading":-0.95519, "vx":0.93102, "vy":-1.11741, "omega":-0.0614, "ax":-0.99553, "ay":1.11939, "alpha":-0.29851, "fx":[-16.58362,-15.14258,-15.96819,-17.40591], "fy":[17.13643,18.20619,19.45951,18.39743]}, + {"t":1.29025, "x":3.27212, "y":5.60008, "heading":-0.95712, "vx":0.89986, "vy":-1.08237, "omega":-0.07075, "ax":-0.98943, "ay":1.12503, "alpha":-0.27374, "fx":[-16.46648,-15.11528,-15.89127,-17.22829], "fy":[17.34078,18.31483,19.43315,18.4794]}, + {"t":1.32155, "x":3.2998, "y":5.56675, "heading":-0.95933, "vx":0.86889, "vy":-1.04716, "omega":-0.07932, "ax":-0.98433, "ay":1.12969, "alpha":-0.24803, "fx":[-16.35148,-15.15094,-15.83288,-17.03267], "fy":[17.50393,18.3855,19.43006,18.55353]}, + {"t":1.35285, "x":3.32651, "y":5.53453, "heading":-0.96181, "vx":0.83808, "vy":-1.0118, "omega":-0.08708, "ax":-0.98001, "ay":1.13361, "alpha":-0.22422, "fx":[-16.26289,-15.15315,-15.78463,-16.88459], "fy":[17.67322,18.46434,19.38354,18.60825]}, + {"t":1.38415, "x":3.35226, "y":5.50342, "heading":-0.96454, "vx":0.80741, "vy":-0.97632, "omega":-0.0941, "ax":-0.9763, "ay":1.13695, "alpha":-0.19951, "fx":[-16.17203,-15.20445,-15.74853,-16.71752], "fy":[17.81268,18.51535,19.35978,18.66006]}, + {"t":1.41545, "x":3.37706, "y":5.47341, "heading":-0.96748, "vx":0.77685, "vy":-0.94073, "omega":-0.10034, "ax":-0.97308, "ay":1.13983, "alpha":-0.1764, "fx":[-16.10062,-15.22525,-15.71848,-16.58748], "fy":[17.95955,18.57639,19.3024,18.698]}, + {"t":1.44675, "x":3.4009, "y":5.44453, "heading":-0.97062, "vx":0.74639, "vy":-0.90506, "omega":-0.10586, "ax":-0.97025, "ay":1.14234, "alpha":-0.15241, "fx":[-16.02509,-15.28542,-15.69698,-16.43975], "fy":[18.08437,18.61561,19.26525,18.73535]}, + {"t":1.47805, "x":3.42378, "y":5.41676, "heading":-0.97394, "vx":0.71602, "vy":-0.8693, "omega":-0.11063, "ax":-0.96776, "ay":1.14455, "alpha":-0.12974, "fx":[-15.96445,-15.31905,-15.67955,-16.32113], "fy":[18.21602,18.66542,19.20151,18.762]}, + {"t":1.50935, "x":3.44572, "y":5.39011, "heading":-0.9774, "vx":0.68573, "vy":-0.83348, "omega":-0.11469, "ax":-0.96554, "ay":1.14651, "alpha":-0.10623, "fx":[-15.89906,-15.38461,-15.66837,-16.18707], "fy":[18.33155,18.69732,19.15477,18.78924]}, + {"t":1.54065, "x":3.46671, "y":5.36459, "heading":-0.98099, "vx":0.65551, "vy":-0.79759, "omega":-0.11802, "ax":-0.96356, "ay":1.14825, "alpha":-0.08378, "fx":[-15.84527,-15.42784,-15.66038,-16.07571], "fy":[18.45252,18.7398,19.08695,18.80772]}, + {"t":1.57195, "x":3.48676, "y":5.34018, "heading":-0.98468, "vx":0.62535, "vy":-0.76165, "omega":-0.12064, "ax":-0.96177, "ay":1.14982, "alpha":-0.06052, "fx":[-15.7867,-15.49701,-15.65696,-15.95153], "fy":[18.56224,18.76711,19.03268,18.8274]}, + {"t":1.60325, "x":3.50586, "y":5.31691, "heading":-0.98846, "vx":0.59525, "vy":-0.72566, "omega":-0.12254, "ax":-0.96015, "ay":1.15123, "alpha":-0.03808, "fx":[-15.73717,-15.548,-15.65653,-15.84456], "fy":[18.67563,18.80473,18.96191,18.83962]}, + {"t":1.63454, "x":3.52402, "y":5.29476, "heading":-0.99229, "vx":0.5652, "vy":-0.68963, "omega":-0.12373, "ax":-0.95867, "ay":1.15252, "alpha":-0.01485, "fx":[-15.68323,-15.61985,-15.65929,-15.72755], "fy":[18.78196,18.82924,18.90121,18.85333]}, + {"t":1.66584, "x":3.54124, "y":5.27374, "heading":-0.99617, "vx":0.53519, "vy":-0.65356, "omega":-0.12419, "ax":-0.95733, "ay":1.15368, "alpha":0.00776, "fx":[-15.63618,-15.6776,-15.66524,-15.62286], "fy":[18.89004,18.86364,18.82799,18.8605]}, + {"t":1.69714, "x":3.55752, "y":5.25385, "heading":-1.00005, "vx":0.50523, "vy":-0.61745, "omega":-0.12395, "ax":-0.95609, "ay":1.15475, "alpha":0.03115, "fx":[-15.58533,-15.75172,-15.67314,-15.51098], "fy":[18.99485,18.88657,18.76139,18.86928]}, + {"t":1.72844, "x":3.57287, "y":5.23509, "heading":-1.00393, "vx":0.4753, "vy":-0.5813, "omega":-0.12297, "ax":-0.95496, "ay":1.15574, "alpha":0.05414, "fx":[-15.53946,-15.81574,-15.68477,-15.4069], "fy":[19.09935,18.91887,18.68585,18.87225]}, + {"t":1.75974, "x":3.58728, "y":5.21746, "heading":-1.00778, "vx":0.44541, "vy":-0.54513, "omega":-0.12128, "ax":-0.95391, "ay":1.15664, "alpha":0.07789, "fx":[-15.4906,-15.89205,-15.69709,-15.29853], "fy":[19.20412,18.94107,18.61349,18.87681]}, + {"t":1.79104, "x":3.60075, "y":5.20096, "heading":-1.01158, "vx":0.41555, "vy":-0.50893, "omega":-0.11884, "ax":-0.95294, "ay":1.15748, "alpha":0.10143, "fx":[-15.44494,-15.96218,-15.71396,-15.19363], "fy":[19.30646,18.97204,18.53555,18.87617]}, + {"t":1.82234, "x":3.61329, "y":5.1856, "heading":-1.0153, "vx":0.38573, "vy":-0.4727, "omega":-0.11567, "ax":-0.95203, "ay":1.15825, "alpha":0.12573, "fx":[-15.39726,-16.04081,-15.73021,-15.08741], "fy":[19.41248,18.99416,18.45731,18.87701]}, + {"t":1.85364, "x":3.6249, "y":5.17137, "heading":-1.01892, "vx":0.35593, "vy":-0.43645, "omega":-0.11173, "ax":-0.95119, "ay":1.15898, "alpha":0.14998, "fx":[-15.35118,-16.11712,-15.75201,-14.98039], "fy":[19.51342,19.02446,18.3769,18.87338]}, + {"t":1.88494, "x":3.63557, "y":5.15828, "heading":-1.02242, "vx":0.32616, "vy":-0.40017, "omega":-0.10704, "ax":-0.95041, "ay":1.15965, "alpha":0.17504, "fx":[-15.30401,-16.19838,-15.77183,-14.87517], "fy":[19.62231,19.0468,18.29228,18.87075]}, + {"t":1.91624, "x":3.64531, "y":5.14632, "heading":-1.02577, "vx":0.29641, "vy":-0.36387, "omega":-0.10156, "ax":-0.94967, "ay":1.16028, "alpha":0.20024, "fx":[-15.25681,-16.28132,-15.79841,-14.76482], "fy":[19.72351,19.07621,18.20902,18.86452]}, + {"t":1.94754, "x":3.65413, "y":5.1355, "heading":-1.02895, "vx":0.26669, "vy":-0.32756, "omega":-0.09529, "ax":-0.94898, "ay":1.16087, "alpha":0.22623, "fx":[-15.20985,-16.36546,-15.82149,-14.65953], "fy":[19.83579,19.09966,18.11756,18.85875]}, + {"t":1.97884, "x":3.66201, "y":5.12582, "heading":-1.03193, "vx":0.23698, "vy":-0.29122, "omega":-0.08821, "ax":-0.94834, "ay":1.16142, "alpha":0.25256, "fx":[-15.16113,-16.45544,-15.85278,-14.54466], "fy":[19.9381,19.12879,18.03085,18.85014]}, + {"t":2.01014, "x":3.66896, "y":5.11727, "heading":-1.03469, "vx":0.2073, "vy":-0.25487, "omega":-0.08031, "ax":-0.94773, "ay":1.16194, "alpha":0.27969, "fx":[-15.11409,-16.54304,-15.8788,-14.43826], "fy":[20.05504,19.1531,17.93203,18.84169]}, + {"t":2.04144, "x":3.67499, "y":5.10986, "heading":-1.0372, "vx":0.17764, "vy":-0.2185, "omega":-0.07155, "ax":-0.94715, "ay":1.16243, "alpha":0.30737, "fx":[-15.06354,-16.64071,-15.91469,-14.31769], "fy":[20.1595,19.18215,17.8415,18.83071]}, + {"t":2.07274, "x":3.68008, "y":5.10359, "heading":-1.03944, "vx":0.14799, "vy":-0.18212, "omega":-0.06193, "ax":-0.94661, "ay":1.16289, "alpha":0.33584, "fx":[-15.01626,-16.73237,-15.94341,-14.20909], "fy":[20.28214,19.20729,17.73434,18.8203]}, + {"t":2.10404, "x":3.68425, "y":5.09846, "heading":-1.04138, "vx":0.11836, "vy":-0.14572, "omega":-0.05142, "ax":-0.9461, "ay":1.16333, "alpha":0.3651, "fx":[-14.96373,-16.8386,-15.98385,-14.0814], "fy":[20.38959,19.23604,17.63924,18.80775]}, + {"t":2.13534, "x":3.68749, "y":5.09447, "heading":-1.04299, "vx":0.08875, "vy":-0.10931, "omega":-0.03999, "ax":-0.94561, "ay":1.16374, "alpha":0.39516, "fx":[-14.91616,-16.93498,-16.01494,-13.96968], "fy":[20.51928,19.26209,17.5229,18.79537]}, + {"t":2.16664, "x":3.68981, "y":5.09162, "heading":-1.04424, "vx":0.05915, "vy":-0.07289, "omega":-0.02762, "ax":-0.94515, "ay":1.16413, "alpha":0.42626, "fx":[-14.86159,-17.05077,-16.05985,-13.83337], "fy":[20.63078,19.29046,17.42217,18.78186]}, + {"t":2.19794, "x":3.69119, "y":5.08991, "heading":-1.04511, "vx":0.02957, "vy":-0.03645, "omega":-0.01428, "ax":-0.94471, "ay":1.1645, "alpha":0.45627, "fx":[-14.8142,-17.1495,-16.09199,-13.7212], "fy":[20.75853,19.31598,17.30837,18.76671]}, + {"t":2.22924, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PRMtoB.traj b/src/main/deploy/choreo/PRMtoB.traj index a863e89e..7e8a0c8d 100644 --- a/src/main/deploy/choreo/PRMtoB.traj +++ b/src/main/deploy/choreo/PRMtoB.traj @@ -11,7 +11,7 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -24,7 +24,7 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,74 +32,75 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.36526,2.00258], + "waypoints":[0.0,1.37983,2.04992], "samples":[ - {"t":0.0, "x":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.91939, "ay":3.57297, "alpha":-0.16326, "fx":[48.37039,47.00801,47.06734,48.45991], "fy":[57.88742,58.99896,58.94891,57.80972]}, - {"t":0.0333, "x":1.123, "y":0.996, "heading":0.942, "vx":0.09721, "vy":0.11898, "omega":-0.00544, "ax":2.91887, "ay":3.57306, "alpha":-0.16632, "fx":[48.37405,46.98637,47.04632,48.46533], "fy":[57.87922,59.01114,58.96045,57.79985]}, - {"t":0.0666, "x":1.12786, "y":1.00194, "heading":0.94182, "vx":0.19441, "vy":0.23796, "omega":-0.01097, "ax":2.91831, "ay":3.57315, "alpha":-0.16966, "fx":[48.37817,46.9629,47.02325,48.47114], "fy":[57.87017,59.02432,58.97315,57.78918]}, - {"t":0.0999, "x":1.13595, "y":1.01185, "heading":0.94145, "vx":0.29159, "vy":0.35694, "omega":-0.01662, "ax":2.9177, "ay":3.57325, "alpha":-0.17332, "fx":[48.38281,46.93731,46.99783,48.47738], "fy":[57.86016,59.03864,58.98717,57.77758]}, - {"t":0.1332, "x":1.14728, "y":1.02571, "heading":0.9409, "vx":0.38874, "vy":0.47592, "omega":-0.0224, "ax":2.91702, "ay":3.57337, "alpha":-0.17734, "fx":[48.38804,46.90929,46.96971,48.48413], "fy":[57.84904,59.05429,59.00269,57.76491]}, - {"t":0.1665, "x":1.16184, "y":1.04354, "heading":0.94015, "vx":0.48588, "vy":0.59491, "omega":-0.0283, "ax":2.91628, "ay":3.57349, "alpha":-0.18179, "fx":[48.39394,46.87846,46.93848,48.49149], "fy":[57.83665,59.07146,59.01995,57.75099]}, - {"t":0.19979, "x":1.17963, "y":1.06533, "heading":0.93921, "vx":0.58299, "vy":0.71391, "omega":-0.03435, "ax":2.91545, "ay":3.57363, "alpha":-0.18674, "fx":[48.40062,46.84433,46.90361,48.49955], "fy":[57.82278,59.09042,59.03922,57.7356]}, - {"t":0.23309, "x":1.20066, "y":1.09109, "heading":0.93807, "vx":0.68007, "vy":0.83291, "omega":-0.04057, "ax":2.91452, "ay":3.57378, "alpha":-0.19226, "fx":[48.40821,46.80632,46.86449,48.50846], "fy":[57.8072,59.11149,59.06085,57.71848]}, - {"t":0.26639, "x":1.22492, "y":1.12081, "heading":0.93672, "vx":0.77712, "vy":0.95191, "omega":-0.04698, "ax":2.91348, "ay":3.57395, "alpha":-0.19848, "fx":[48.41685,46.76369,46.82031,48.51838], "fy":[57.78958,59.13506,59.08528,57.6993]}, - {"t":0.29969, "x":1.25242, "y":1.15448, "heading":0.93515, "vx":0.87413, "vy":1.07092, "omega":-0.05358, "ax":2.91229, "ay":3.57414, "alpha":-0.20553, "fx":[48.42674,46.71553,46.77006,48.52952], "fy":[57.76953,59.16162,59.11304,57.67763]}, - {"t":0.33299, "x":1.28314, "y":1.19213, "heading":0.93337, "vx":0.97111, "vy":1.18993, "omega":-0.06043, "ax":2.91094, "ay":3.57436, "alpha":-0.21358, "fx":[48.43815,46.66063,46.71244,48.54216], "fy":[57.74655,59.19182,59.14484,57.65291]}, - {"t":0.36629, "x":1.31709, "y":1.23373, "heading":0.93136, "vx":1.06804, "vy":1.30896, "omega":-0.06754, "ax":2.90938, "ay":3.57461, "alpha":-0.22288, "fx":[48.45139,46.59746,46.64574,48.55666], "fy":[57.71997,59.22647,59.18161,57.62445]}, - {"t":0.39959, "x":1.35427, "y":1.2793, "heading":0.92911, "vx":1.16492, "vy":1.42799, "omega":-0.07496, "ax":2.90756, "ay":3.5749, "alpha":-0.23372, "fx":[48.46692,46.52394,46.5677,48.57349], "fy":[57.68893,59.26669,59.22454,57.59127]}, - {"t":0.43289, "x":1.39467, "y":1.32883, "heading":0.92661, "vx":1.26174, "vy":1.54703, "omega":-0.08274, "ax":2.9054, "ay":3.57524, "alpha":-0.24654, "fx":[48.48532,46.43728,46.47519,48.59331], "fy":[57.65222,59.31394,59.27532,57.55207]}, - {"t":0.46619, "x":1.4383, "y":1.38233, "heading":0.92386, "vx":1.35849, "vy":1.66608, "omega":-0.09095, "ax":2.90281, "ay":3.57564, "alpha":-0.26192, "fx":[48.50742,46.33356,46.36383,48.61703], "fy":[57.60818,59.3703,59.33625,57.50501]}, - {"t":0.49949, "x":1.48514, "y":1.43979, "heading":0.92083, "vx":1.45515, "vy":1.78515, "omega":-0.09968, "ax":2.89965, "ay":3.57612, "alpha":-0.28073, "fx":[48.53443,46.20715,46.22726,48.64598], "fy":[57.55442,59.43869,59.41068,57.44743]}, - {"t":0.53278, "x":1.5352, "y":1.50122, "heading":0.91751, "vx":1.5517, "vy":1.90423, "omega":-0.10902, "ax":2.89569, "ay":3.57671, "alpha":-0.30423, "fx":[48.56813,46.04964,46.05591,48.68215], "fy":[57.48737,59.52348,59.50359,57.3753]}, - {"t":0.56608, "x":1.58848, "y":1.56661, "heading":0.91388, "vx":1.64813, "vy":2.02333, "omega":-0.11915, "ax":2.89059, "ay":3.57745, "alpha":-0.33445, "fx":[48.61127,45.84793,45.83462,48.72867], "fy":[57.40145,59.63138,59.62279,57.2823]}, - {"t":0.59938, "x":1.64496, "y":1.63597, "heading":0.90991, "vx":1.74438, "vy":2.14245, "omega":-0.13029, "ax":2.88378, "ay":3.5784, "alpha":-0.37474, "fx":[48.66843,45.58035,45.53794,48.79077], "fy":[57.28746,59.77335,59.78119,57.15777]}, - {"t":0.63268, "x":1.70465, "y":1.70929, "heading":0.90557, "vx":1.84041, "vy":2.26161, "omega":-0.14277, "ax":2.87424, "ay":3.57965, "alpha":-0.43113, "fx":[48.74767,45.20835,45.1196,48.87788], "fy":[57.12903,59.96852,60.00179,56.98242]}, - {"t":0.66598, "x":1.76753, "y":1.78659, "heading":0.90082, "vx":1.93612, "vy":2.38081, "omega":-0.15713, "ax":2.8599, "ay":3.58138, "alpha":-0.51565, "fx":[48.86471,44.65614,44.48577,49.00888], "fy":[56.89399,60.25358,60.32991,56.71718]}, - {"t":0.69928, "x":1.83358, "y":1.86785, "heading":0.89558, "vx":2.03135, "vy":2.50007, "omega":-0.1743, "ax":2.83592, "ay":3.58384, "alpha":-0.6563, "fx":[49.05481,43.75159,43.41327,49.22789], "fy":[56.50937,60.70859,60.86854,56.26934]}, - {"t":0.73258, "x":1.9028, "y":1.95309, "heading":0.88978, "vx":2.12578, "vy":2.61941, "omega":-0.19615, "ax":2.78774, "ay":3.58722, "alpha":-0.93678, "fx":[49.41616,42.00279,41.21131,49.667], "fy":[55.76691,61.547,61.91043,55.35276]}, - {"t":0.76588, "x":1.97513, "y":2.0423, "heading":0.88325, "vx":2.21861, "vy":2.73886, "omega":-0.22734, "ax":2.64217, "ay":3.58506, "alpha":-1.77334, "fx":[50.36004,37.23191,34.20914,50.9767], "fy":[53.74271,63.56893,64.68414,52.43971]}, - {"t":0.79918, "x":2.05047, "y":2.13549, "heading":0.87568, "vx":2.30659, "vy":2.85824, "omega":-0.2864, "ax":-0.19101, "ay":0.85926, "alpha":-15.65567, "fx":[57.11884,-8.79183,-62.54868,1.73121], "fy":[29.27025,66.40467,17.93266,-57.41844]}, - {"t":0.83248, "x":2.12717, "y":2.23114, "heading":0.86614, "vx":2.30023, "vy":2.88685, "omega":-0.80771, "ax":-3.00913, "ay":-3.26547, "alpha":-1.80257, "fx":[-41.43195,-56.89172,-55.36033,-43.09038], "fy":[-60.00068,-45.66056,-48.3497,-59.52585]}, - {"t":0.86578, "x":2.2021, "y":2.32546, "heading":0.83925, "vx":2.20003, "vy":2.77811, "omega":-0.86774, "ax":-2.9793, "ay":-3.42861, "alpha":-0.83721, "fx":[-45.0095,-52.18838,-52.0042,-45.62139], "fy":[-59.15017,-52.92053,-53.28736,-58.84676]}, - {"t":0.89907, "x":2.27371, "y":2.41607, "heading":0.81035, "vx":2.10082, "vy":2.66394, "omega":-0.89562, "ax":-2.96513, "ay":-3.47831, "alpha":-0.52854, "fx":[-46.08497,-50.59106,-50.69646,-46.52426], "fy":[-58.85298,-55.02293,-55.00215,-58.57711]}, - {"t":0.93237, "x":2.34202, "y":2.50285, "heading":0.78053, "vx":2.00209, "vy":2.54812, "omega":-0.91322, "ax":-2.95735, "ay":-3.50231, "alpha":-0.3753, "fx":[-46.61042,-49.79256,-49.99694,-46.98811], "fy":[-58.69666,-56.01868,-55.87638,-58.43248]}, - {"t":0.96567, "x":2.40705, "y":2.58576, "heading":0.75012, "vx":1.90361, "vy":2.43149, "omega":-0.92571, "ax":-2.95247, "ay":-3.51643, "alpha":-0.28334, "fx":[-46.92589,-49.31514,-49.55815,-47.26959], "fy":[-58.59725,-56.59804,-56.40927,-58.34311]}, - {"t":0.99897, "x":2.4688, "y":2.66477, "heading":0.71929, "vx":1.8053, "vy":2.3144, "omega":-0.93515, "ax":-2.94912, "ay":-3.52573, "alpha":-0.22191, "fx":[-47.13875,-48.99893,-49.25506,-47.45739], "fy":[-58.52664,-56.97582,-56.76998,-58.28338]}, - {"t":1.03227, "x":2.52728, "y":2.73989, "heading":0.68815, "vx":1.70709, "vy":2.197, "omega":-0.94254, "ax":-2.94669, "ay":-3.53232, "alpha":-0.17791, "fx":[-47.29367,-48.7753,-49.03165,-47.59046], "fy":[-58.47273,-57.24057,-57.03164,-58.24157]}, - {"t":1.06557, "x":2.58249, "y":2.81108, "heading":0.65677, "vx":1.60897, "vy":2.07937, "omega":-0.94846, "ax":-2.94484, "ay":-3.53722, "alpha":-0.14481, "fx":[-47.41258,-48.60988,-48.8591,-47.68866], "fy":[-58.42945,-57.43549,-57.23104,-58.21153]}, - {"t":1.09887, "x":2.63443, "y":2.87836, "heading":0.62519, "vx":1.51091, "vy":1.96159, "omega":-0.95328, "ax":-2.94339, "ay":-3.54102, "alpha":-0.11901, "fx":[-47.50751,-48.48351,-48.72107,-47.76317], "fy":[-58.3934,-57.58418,-57.38868,-58.18969]}, - {"t":1.13217, "x":2.68311, "y":2.94172, "heading":0.59344, "vx":1.4129, "vy":1.84368, "omega":-0.95725, "ax":-2.94222, "ay":-3.54405, "alpha":-0.09831, "fx":[-47.58562,-48.38468,-48.6076,-47.8208], "fy":[-58.36255,-57.70064,-57.51691,-58.17385]}, - {"t":1.16547, "x":2.72853, "y":3.00115, "heading":0.56157, "vx":1.31493, "vy":1.72566, "omega":-0.96052, "ax":-2.94125, "ay":-3.54652, "alpha":-0.08135, "fx":[-47.65142,-48.30604,-48.51226,-47.86594], "fy":[-58.33558,-57.79369,-57.62362,-58.16256]}, - {"t":1.19877, "x":2.77069, "y":3.05664, "heading":0.52958, "vx":1.21699, "vy":1.60757, "omega":-0.96323, "ax":-2.94045, "ay":-3.54857, "alpha":-0.06718, "fx":[-47.70789,-48.24266,-48.43074,-47.90154], "fy":[-58.31164,-57.86917,-57.71405,-58.15481]}, - {"t":1.23206, "x":2.80958, "y":3.10821, "heading":0.49751, "vx":1.11907, "vy":1.4894, "omega":-0.96547, "ax":-2.93976, "ay":-3.55031, "alpha":-0.05519, "fx":[-47.7571,-48.19112,-48.36002,-47.92968], "fy":[-58.29011,-57.93113,-57.79185,-58.1499]}, - {"t":1.26536, "x":2.84521, "y":3.15584, "heading":0.46536, "vx":1.02118, "vy":1.37118, "omega":-0.9673, "ax":-2.93917, "ay":-3.55179, "alpha":-0.04489, "fx":[-47.80051,-48.14899,-48.29795,-47.95185], "fy":[-58.27058,-57.98242,-57.85962,-58.14731]}, - {"t":1.29866, "x":2.87759, "y":3.19953, "heading":0.43315, "vx":0.92331, "vy":1.25291, "omega":-0.9688, "ax":-2.93866, "ay":-3.55307, "alpha":-0.03597, "fx":[-47.83919,-48.11443,-48.24292,-47.96916], "fy":[-58.25274,-58.02516,-57.91928,-58.14665]}, - {"t":1.33196, "x":2.90671, "y":3.23928, "heading":0.40089, "vx":0.82546, "vy":1.1346, "omega":-0.97, "ax":-2.9382, "ay":-3.55419, "alpha":-0.02816, "fx":[-47.87393,-48.0861,-48.19374,-47.98244], "fy":[-58.23634,-58.06092,-57.97226,-58.1476]}, - {"t":1.36526, "x":2.93256, "y":3.27509, "heading":0.36859, "vx":0.72762, "vy":1.01624, "omega":-0.97093, "ax":-3.70783, "ay":2.38395, "alpha":-0.46714, "fx":[-60.22353,-58.75353,-61.04054,-62.44625], "fy":[39.34989,41.85646,38.67566,36.01005]}, - {"t":1.39182, "x":2.95058, "y":3.30291, "heading":0.3428, "vx":0.62915, "vy":1.07955, "omega":-0.98334, "ax":-3.64564, "ay":1.9166, "alpha":-0.92419, "fx":[-58.93726,-56.53613,-60.4259,-62.49761], "fy":[31.23966,37.01893,31.56461,25.508]}, - {"t":1.41837, "x":2.966, "y":3.33226, "heading":0.31669, "vx":0.53234, "vy":1.13045, "omega":-1.00788, "ax":-2.64566, "ay":1.14839, "alpha":-2.67439, "fx":[-37.78713,-37.21506,-48.73048,-49.27361], "fy":[15.09043,32.01479,22.13747,5.85301]}, - {"t":1.44493, "x":2.9792, "y":3.36268, "heading":0.28993, "vx":0.46209, "vy":1.16094, "omega":-1.0789, "ax":-0.54339, "ay":0.21241, "alpha":-4.2358, "fx":[5.70356,-1.48195,-22.76198,-16.99302], "fy":[-4.75829,18.58393,11.18526,-11.12092]}, - {"t":1.47148, "x":2.99128, "y":3.39358, "heading":0.26128, "vx":0.44766, "vy":1.16658, "omega":-1.19138, "ax":0.07099, "ay":-0.02731, "alpha":-2.71268, "fx":[10.2494,6.38144,-7.96615,-4.0223], "fy":[-5.82701,8.76402,4.96478,-9.68773]}, - {"t":1.49804, "x":3.0032, "y":3.42455, "heading":0.22964, "vx":0.44954, "vy":1.16586, "omega":-1.26342, "ax":0.13728, "ay":-0.05318, "alpha":-0.94102, "fx":[5.33597,4.15372,-0.85591,0.34306], "fy":[-2.83596,2.27121,1.10482,-4.01772]}, - {"t":1.52459, "x":3.01518, "y":3.45549, "heading":0.19609, "vx":0.45319, "vy":1.16444, "omega":-1.28841, "ax":0.11671, "ay":-0.0456, "alpha":0.87217, "fx":[-0.90229,0.04857,4.71259,3.77323], "fy":[1.17926,-3.60115,-2.66432,2.10409]}, - {"t":1.55115, "x":3.02726, "y":3.4864, "heading":0.16188, "vx":0.45629, "vy":1.16323, "omega":-1.26525, "ax":0.092, "ay":-0.0362, "alpha":2.64552, "fx":[-6.82958,-4.41718,9.79886,7.46405], "fy":[5.54942,-9.06648,-6.68681,7.83664]}, - {"t":1.5777, "x":3.03941, "y":3.51728, "heading":0.12828, "vx":0.45873, "vy":1.16227, "omega":-1.19499, "ax":0.07318, "ay":-0.02896, "alpha":4.30333, "fx":[-12.02652,-8.88468,14.34405,11.35276], "fy":[9.9795,-13.93708,-10.8238,12.88791]}, - {"t":1.60426, "x":3.05161, "y":3.54813, "heading":0.09654, "vx":0.46067, "vy":1.1615, "omega":-1.08072, "ax":0.05927, "ay":-0.02355, "alpha":5.79995, "fx":[-16.40121,-13.18429,18.23763,15.22342], "fy":[14.27013,-18.09674,-14.883,17.16943]}, - {"t":1.63081, "x":3.06387, "y":3.57897, "heading":0.06785, "vx":0.46225, "vy":1.16088, "omega":-0.9267, "ax":0.04871, "ay":-0.01943, "alpha":7.1206, "fx":[-20.00344,-17.19814,21.48148,18.90534], "fy":[18.28186,-21.55554,-18.71567,20.71894]}, - {"t":1.65737, "x":3.07616, "y":3.60979, "heading":0.04324, "vx":0.46354, "vy":1.16036, "omega":-0.73761, "ax":0.04085, "ay":-0.01634, "alpha":8.27167, "fx":[-22.95211,-20.84037,24.1691,22.29472], "fy":[21.92538,-24.41363,-22.22593,23.64557]}, - {"t":1.68392, "x":3.08848, "y":3.64059, "heading":0.02365, "vx":0.46463, "vy":1.15993, "omega":-0.51796, "ax":0.03643, "ay":-0.01486, "alpha":9.27005, "fx":[-25.38219,-24.03554,26.44848,25.35141], "fy":[25.14297,-26.82686,-25.36231,26.07468]}, - {"t":1.71048, "x":3.10084, "y":3.67139, "heading":0.0099, "vx":0.46559, "vy":1.15953, "omega":-0.27179, "ax":-0.3495, "ay":-1.00775, "alpha":9.30609, "fx":[-36.79367,-28.04746,17.75166,24.23462], "fy":[7.25963,-39.86105,-42.4654,9.16752]}, - {"t":1.73703, "x":3.11308, "y":3.70183, "heading":0.00268, "vx":0.45631, "vy":1.13277, "omega":-0.02467, "ax":-1.69457, "ay":-4.22029, "alpha":0.24467, "fx":[-29.3347,-28.37449,-26.09739,-27.00538], "fy":[-68.31646,-68.76259,-69.64284,-69.25266]}, - {"t":1.76359, "x":3.1246, "y":3.73042, "heading":0.00202, "vx":0.41131, "vy":1.0207, "omega":-0.01817, "ax":-1.71015, "ay":-4.25293, "alpha":0.13735, "fx":[-28.87903,-28.33795,-27.04476,-27.56918], "fy":[-69.15032,-69.38571,-69.89531,-69.67801]}, - {"t":1.79014, "x":3.13492, "y":3.75603, "heading":0.00154, "vx":0.3659, "vy":0.90777, "omega":-0.01452, "ax":-1.7159, "ay":-4.2636, "alpha":0.10155, "fx":[-28.73412,-28.33373,-27.37383,-27.76507], "fy":[-69.42361,-69.59388,-69.97472,-69.81443]}, - {"t":1.8167, "x":3.14403, "y":3.77863, "heading":0.00115, "vx":0.32033, "vy":0.79454, "omega":-0.01182, "ax":-1.71909, "ay":-4.2688, "alpha":0.08367, "fx":[-28.6665,-28.33658,-27.54422,-27.86792], "fy":[-69.55789,-69.69659,-70.0121,-69.88019]}, - {"t":1.84325, "x":3.15193, "y":3.79822, "heading":0.00084, "vx":0.27468, "vy":0.68119, "omega":-0.0096, "ax":-1.72119, "ay":-4.27184, "alpha":0.07294, "fx":[-28.6289,-28.34129,-27.64973,-27.93261], "fy":[-69.6371,-69.7572,-70.0332,-69.91828]}, - {"t":1.86981, "x":3.15861, "y":3.81481, "heading":0.00059, "vx":0.22898, "vy":0.56775, "omega":-0.00767, "ax":-1.7227, "ay":-4.27382, "alpha":0.06579, "fx":[-28.60562,-28.34623,-27.72198,-27.97752], "fy":[-69.68913,-69.79696,-70.04649,-69.94288]}, - {"t":1.89636, "x":3.16409, "y":3.82838, "heading":0.00038, "vx":0.18323, "vy":0.45426, "omega":-0.00592, "ax":-1.72384, "ay":-4.27521, "alpha":0.06068, "fx":[-28.59003,-28.35078,-27.77466,-28.01063], "fy":[-69.72584,-69.82498,-70.05554,-69.96]}, - {"t":1.92292, "x":3.16835, "y":3.83893, "heading":0.00023, "vx":0.13745, "vy":0.34073, "omega":-0.00431, "ax":-1.72473, "ay":-4.27624, "alpha":0.05685, "fx":[-28.57888,-28.35474,-27.81474,-28.036], "fy":[-69.75313,-69.84579,-70.06209,-69.97259]}, - {"t":1.94947, "x":3.17139, "y":3.84647, "heading":0.00011, "vx":0.09165, "vy":0.22717, "omega":-0.0028, "ax":-1.72544, "ay":-4.27703, "alpha":0.05388, "fx":[-28.57046,-28.35806,-27.84619,-28.056], "fy":[-69.77424,-69.86189,-70.06708,-69.98227]}, - {"t":1.97603, "x":3.17321, "y":3.851, "heading":0.00004, "vx":0.04583, "vy":0.11359, "omega":-0.00137, "ax":-1.72601, "ay":-4.27767, "alpha":0.05149, "fx":[-28.56382,-28.3608,-27.87143,-28.07209], "fy":[-69.79109,-69.87474,-70.07103,-69.98997]}, - {"t":2.00258, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.91146, "ay":3.57956, "alpha":-0.15883, "fx":[48.22621,46.89788,46.95317,48.3102], "fy":[58.00934,59.08827,59.04173,57.93676]}, + {"t":0.03365, "x":1.12303, "y":0.99605, "heading":0.942, "vx":0.09798, "vy":0.12047, "omega":-0.00535, "ax":2.911, "ay":3.57961, "alpha":-0.162, "fx":[48.23113,46.87651,46.93243,48.31691], "fy":[58.00015,59.10021,59.05303,57.92588]}, + {"t":0.06731, "x":1.12798, "y":1.00213, "heading":0.94182, "vx":0.19595, "vy":0.24094, "omega":-0.0108, "ax":2.91049, "ay":3.57966, "alpha":-0.16547, "fx":[48.23663,46.8533,46.90964,48.32414], "fy":[57.99001,59.11314,59.06547,57.91409]}, + {"t":0.10096, "x":1.13622, "y":1.01226, "heading":0.94146, "vx":0.2939, "vy":0.36141, "omega":-0.01637, "ax":2.90993, "ay":3.57972, "alpha":-0.16927, "fx":[48.24278,46.82798,46.88451,48.33194], "fy":[57.9788,59.12721,59.07921,57.90126]}, + {"t":0.13462, "x":1.14776, "y":1.02645, "heading":0.94091, "vx":0.39184, "vy":0.48188, "omega":-0.02206, "ax":2.90931, "ay":3.57978, "alpha":-0.17346, "fx":[48.24968,46.80023,46.85669,48.34042], "fy":[57.96635,59.14259,59.09444,57.88721]}, + {"t":0.16827, "x":1.16259, "y":1.0447, "heading":0.94016, "vx":0.48975, "vy":0.60236, "omega":-0.0279, "ax":2.90863, "ay":3.57985, "alpha":-0.17809, "fx":[48.25743,46.76965,46.82576,48.3497], "fy":[57.95248,59.1595,59.11139,57.87174]}, + {"t":0.20193, "x":1.18072, "y":1.067, "heading":0.93922, "vx":0.58764, "vy":0.72284, "omega":-0.03389, "ax":2.90788, "ay":3.57993, "alpha":-0.18325, "fx":[48.26616,46.73577,46.79119,48.35992], "fy":[57.93696,59.17818,59.13034,57.85462]}, + {"t":0.23558, "x":1.20215, "y":1.09335, "heading":0.93808, "vx":0.6855, "vy":0.84332, "omega":-0.04006, "ax":2.90703, "ay":3.58001, "alpha":-0.18902, "fx":[48.27605,46.69799,46.75235,48.37124], "fy":[57.91949,59.19896,59.15165,57.83554]}, + {"t":0.26924, "x":1.22686, "y":1.12376, "heading":0.93674, "vx":0.78333, "vy":0.9638, "omega":-0.04642, "ax":2.90607, "ay":3.58011, "alpha":-0.19552, "fx":[48.28729,46.65556,46.70841,48.3839], "fy":[57.89974,59.22224,59.17574,57.81411]}, + {"t":0.30289, "x":1.25487, "y":1.15823, "heading":0.93517, "vx":0.88114, "vy":1.08429, "omega":-0.053, "ax":2.90499, "ay":3.58021, "alpha":-0.2029, "fx":[48.30016,46.60755,46.65835,48.39816], "fy":[57.87723,59.24852,59.20317,57.78984]}, + {"t":0.33655, "x":1.28617, "y":1.19674, "heading":0.93339, "vx":0.9789, "vy":1.20478, "omega":-0.05983, "ax":2.90375, "ay":3.58033, "alpha":-0.21135, "fx":[48.31498,46.55273,46.60083,48.4144], "fy":[57.85139,59.27845,59.23467,57.76212]}, + {"t":0.3702, "x":1.32076, "y":1.23932, "heading":0.93138, "vx":1.07663, "vy":1.32527, "omega":-0.06694, "ax":2.90231, "ay":3.58047, "alpha":-0.22113, "fx":[48.3322,46.48951,46.5341,48.43308], "fy":[57.82146,59.31286,59.27115,57.7301]}, + {"t":0.40385, "x":1.35864, "y":1.28595, "heading":0.92912, "vx":1.1743, "vy":1.44577, "omega":-0.07439, "ax":2.90063, "ay":3.58063, "alpha":-0.23256, "fx":[48.35241,46.41578,46.4558,48.45485], "fy":[57.7864,59.35288,59.31389,57.69267]}, + {"t":0.43751, "x":1.3998, "y":1.33663, "heading":0.92662, "vx":1.27192, "vy":1.56628, "omega":-0.08221, "ax":2.89863, "ay":3.58081, "alpha":-0.24611, "fx":[48.3764,46.32861,46.36268,48.48056], "fy":[57.74483,59.40004,59.36459,57.64832]}, + {"t":0.47116, "x":1.44425, "y":1.39137, "heading":0.92385, "vx":1.36947, "vy":1.68679, "omega":-0.0905, "ax":2.89622, "ay":3.58103, "alpha":-0.26244, "fx":[48.4053,46.22394,46.25017,48.51146], "fy":[57.69478,59.45645,59.42566,57.59488]}, + {"t":0.50482, "x":1.49198, "y":1.45017, "heading":0.92081, "vx":1.46694, "vy":1.80731, "omega":-0.09933, "ax":2.89326, "ay":3.58128, "alpha":-0.28248, "fx":[48.44074,46.09585,46.11155,48.54932], "fy":[57.63343,59.52519,59.5006,57.52919]}, + {"t":0.53847, "x":1.54299, "y":1.51302, "heading":0.91746, "vx":1.56432, "vy":1.92783, "omega":-0.10883, "ax":2.88954, "ay":3.58159, "alpha":-0.30766, "fx":[48.48515,45.93545,45.93663,48.59686], "fy":[57.55649,59.61081,59.59468,57.44646]}, + {"t":0.57213, "x":1.59727, "y":1.57993, "heading":0.9138, "vx":1.66156, "vy":2.04837, "omega":-0.11919, "ax":2.88472, "ay":3.58196, "alpha":-0.34026, "fx":[48.54236,45.72872,45.70908,48.65839], "fy":[57.45722,59.72045,59.71624,57.33902]}, + {"t":0.60578, "x":1.65482, "y":1.65089, "heading":0.90979, "vx":1.75865, "vy":2.16892, "omega":-0.13064, "ax":2.87821, "ay":3.58243, "alpha":-0.38411, "fx":[48.61876,45.45214,45.40106,48.7412], "fy":[57.32434,59.86587,59.87928,57.19382]}, + {"t":0.63944, "x":1.71564, "y":1.72592, "heading":0.90539, "vx":1.85551, "vy":2.28948, "omega":-0.14357, "ax":2.86897, "ay":3.58301, "alpha":-0.44625, "fx":[48.72583,45.0631,44.96091,48.85869], "fy":[57.13739,60.06802,60.10924,56.98666]}, + {"t":0.67309, "x":1.77971, "y":1.805, "heading":0.90056, "vx":1.95206, "vy":2.41007, "omega":-0.15859, "ax":2.85479, "ay":3.58372, "alpha":-0.54111, "fx":[48.88649,44.47566,44.28082,49.03845], "fy":[56.85511,60.36799,60.45761,56.66711]}, + {"t":0.70674, "x":1.84702, "y":1.88814, "heading":0.89522, "vx":2.04814, "vy":2.53067, "omega":-0.1768, "ax":2.83031, "ay":3.58444, "alpha":-0.70378, "fx":[49.15398,43.48669,43.09237,49.34762], "fy":[56.38005,60.85888,61.04632,56.10991]}, + {"t":0.7404, "x":1.91755, "y":1.97533, "heading":0.88927, "vx":2.14339, "vy":2.65131, "omega":-0.20048, "ax":2.77795, "ay":3.5839, "alpha":-1.04752, "fx":[49.68616,41.47451,40.49334,50.003], "fy":[55.4139,61.8041,62.24709,54.89434]}, + {"t":0.77405, "x":1.99126, "y":2.06659, "heading":0.88253, "vx":2.23688, "vy":2.77192, "omega":-0.23574, "ax":2.5888, "ay":3.5591, "alpha":-2.2628, "fx":[51.24305,35.21624,30.53655,52.29214], "fy":[52.3983,64.30957,65.8273,50.20265]}, + {"t":0.80771, "x":2.06801, "y":2.1619, "heading":0.87459, "vx":2.32401, "vy":2.8917, "omega":-0.31189, "ax":-2.06511, "ay":-1.751, "alpha":-10.94875, "fx":[17.67974,-59.00451,-65.14143,-28.57611], "fy":[-53.34197,25.50101,-24.07399,-62.58683]}, + {"t":0.84136, "x":2.14505, "y":2.25822, "heading":0.8641, "vx":2.25451, "vy":2.83277, "omega":-0.68036, "ax":-2.97065, "ay":-3.35731, "alpha":-1.4465, "fx":[-42.21032,-54.76559,-53.81776,-43.46429], "fy":[-60.17385,-49.03713,-50.60462,-59.72698]}, + {"t":0.87502, "x":2.21924, "y":2.35166, "heading":0.8412, "vx":2.15453, "vy":2.71978, "omega":-0.72904, "ax":-2.95431, "ay":-3.46373, "alpha":-0.74148, "fx":[-44.99506,-51.38044,-51.28143,-45.53255], "fy":[-59.35489,-53.91825,-54.15515,-59.07341]}, + {"t":0.90867, "x":2.29008, "y":2.44123, "heading":0.81666, "vx":2.05511, "vy":2.60321, "omega":-0.754, "ax":-2.94565, "ay":-3.50107, "alpha":-0.48363, "fx":[-45.96339,-50.10095,-50.20739,-46.35152], "fy":[-59.04139,-55.56963,-55.53635,-58.79589]}, + {"t":0.94233, "x":2.35758, "y":2.52686, "heading":0.79129, "vx":1.95597, "vy":2.48539, "omega":-0.77028, "ax":-2.94061, "ay":-3.52007, "alpha":-0.34936, "fx":[-46.45932,-49.43225,-49.61205,-46.7897], "fy":[-58.87304,-56.39684,-56.27295,-58.64323]}, + {"t":0.97598, "x":2.42174, "y":2.60851, "heading":0.76537, "vx":1.85701, "vy":2.36692, "omega":-0.78203, "ax":-2.93733, "ay":-3.53158, "alpha":-0.26682, "fx":[-46.76331,-49.02268,-49.23162,-47.06156], "fy":[-58.76605,-56.89241,-56.73261,-58.54747]}, + {"t":1.00964, "x":2.48257, "y":2.68616, "heading":0.73905, "vx":1.75815, "vy":2.24807, "omega":-0.79101, "ax":-2.93504, "ay":-3.5393, "alpha":-0.21088, "fx":[-46.9704,-48.74717,-48.96604,-47.24572], "fy":[-58.69075,-57.22152,-57.04808,-58.48259]}, + {"t":1.04329, "x":2.54008, "y":2.75982, "heading":0.71243, "vx":1.65938, "vy":2.12895, "omega":-0.79811, "ax":-2.93335, "ay":-3.54482, "alpha":-0.17043, "fx":[-47.12173,-48.55008,-48.76905,-47.37784], "fy":[-58.63399,-57.45518,-57.27894,-58.43644]}, + {"t":1.07694, "x":2.59426, "y":2.82946, "heading":0.68557, "vx":1.56065, "vy":2.00965, "omega":-0.80385, "ax":-2.93205, "ay":-3.54898, "alpha":-0.1398, "fx":[-47.23798,-48.40293,-48.61632,-47.47647], "fy":[-58.58905,-57.62897,-57.45586,-58.40256]}, + {"t":1.1106, "x":2.64513, "y":2.89508, "heading":0.65851, "vx":1.46198, "vy":1.89021, "omega":-0.80855, "ax":-2.93102, "ay":-3.55222, "alpha":-0.1158, "fx":[-47.33069,-48.28957,-48.49388,-47.55223], "fy":[-58.55214,-57.7627,-57.59626,-58.37722]}, + {"t":1.14425, "x":2.69267, "y":2.95669, "heading":0.6313, "vx":1.36334, "vy":1.77067, "omega":-0.81245, "ax":-2.93018, "ay":-3.55482, "alpha":-0.09648, "fx":[-47.40681,-48.2002,-48.39308,-47.61162], "fy":[-58.52097,-57.86827,-57.71078,-58.35809]}, + {"t":1.17791, "x":2.73689, "y":3.01426, "heading":0.60396, "vx":1.26472, "vy":1.65103, "omega":-0.81569, "ax":-2.92949, "ay":-3.55695, "alpha":-0.08059, "fx":[-47.47077,-48.12849,-48.30833,-47.65888], "fy":[-58.49405,-57.95326,-57.80623,-58.34362]}, + {"t":1.21156, "x":2.7778, "y":3.06781, "heading":0.57651, "vx":1.16613, "vy":1.53132, "omega":-0.81841, "ax":-2.92891, "ay":-3.55872, "alpha":-0.06729, "fx":[-47.52551,-48.07018,-48.23583,-47.69688], "fy":[-58.4704,-58.02275,-57.88724,-58.33278]}, + {"t":1.24522, "x":2.81538, "y":3.11733, "heading":0.54896, "vx":1.06756, "vy":1.41156, "omega":-0.82067, "ax":-2.92841, "ay":-3.56022, "alpha":-0.056, "fx":[-47.57309,-48.0223,-48.17291,-47.72761], "fy":[-58.44933,-58.08025,-57.95701,-58.32482]}, + {"t":1.27887, "x":2.84965, "y":3.16282, "heading":0.52135, "vx":0.96901, "vy":1.29174, "omega":-0.82256, "ax":-2.92798, "ay":-3.56151, "alpha":-0.0463, "fx":[-47.61497,-47.98271,-48.11766,-47.75255], "fy":[-58.43036,-58.12827,-58.01784,-58.31919]}, + {"t":1.31253, "x":2.88061, "y":3.20428, "heading":0.49366, "vx":0.87047, "vy":1.17188, "omega":-0.82411, "ax":-2.92761, "ay":-3.56263, "alpha":-0.03787, "fx":[-47.65221,-47.94982,-48.06865,-47.77276], "fy":[-58.41312,-58.16867,-58.07145,-58.31548]}, + {"t":1.34618, "x":2.90824, "y":3.2417, "heading":0.46593, "vx":0.77194, "vy":1.05198, "omega":-0.82539, "ax":-2.92728, "ay":-3.56361, "alpha":-0.03048, "fx":[-47.68562,-47.92243,-48.0248,-47.78908], "fy":[-58.39734,-58.20285,-58.1191,-58.31339]}, + {"t":1.37983, "x":2.93256, "y":3.27509, "heading":0.43815, "vx":0.67342, "vy":0.93205, "omega":-0.82641, "ax":-3.68118, "ay":2.36939, "alpha":-0.6121, "fx":[-59.46602,-57.80934,-60.92268,-62.52297], "fy":[39.50811,42.42668,38.12333,34.88219]}, + {"t":1.40664, "x":2.94929, "y":3.30092, "heading":0.416, "vx":0.57475, "vy":0.99556, "omega":-0.84282, "ax":-3.51949, "ay":1.82095, "alpha":-1.44249, "fx":[-55.90333,-53.10175,-59.41608,-61.72662], "fy":[30.18303,38.29714,29.79038,20.80558]}, + {"t":1.43344, "x":2.96343, "y":3.32826, "heading":0.39341, "vx":0.48042, "vy":1.04436, "omega":-0.88148, "ax":-2.0264, "ay":0.86988, "alpha":-4.62252, "fx":[-19.03619,-25.72589,-45.46977,-42.27939], "fy":[8.33577,34.62355,19.50638,-5.5819]}, + {"t":1.46025, "x":2.97558, "y":3.35656, "heading":0.36978, "vx":0.4261, "vy":1.06768, "omega":-1.00538, "ax":-0.36101, "ay":0.1422, "alpha":-5.81347, "fx":[15.14954,2.57215,-25.94317,-15.38577], "fy":[-7.27197,23.54261,11.42583,-18.39794]}, + {"t":1.48705, "x":2.98687, "y":3.38523, "heading":0.34283, "vx":0.41643, "vy":1.07149, "omega":-1.16121, "ax":0.03617, "ay":-0.01408, "alpha":-4.51441, "fx":[16.40293,7.99845,-15.28058,-6.7558], "fy":[-7.92853,15.75729,7.50218,-16.25139]}, + {"t":1.51385, "x":2.99805, "y":3.41395, "heading":0.31171, "vx":0.4174, "vy":1.07111, "omega":-1.28221, "ax":0.08988, "ay":-0.03514, "alpha":-2.99614, "fx":[11.78311,6.72631,-8.90787,-3.72424], "fy":[-5.99829,9.87319,4.89034,-11.06324]}, + {"t":1.54066, "x":3.00927, "y":3.44264, "heading":0.27734, "vx":0.41981, "vy":1.07017, "omega":-1.36252, "ax":0.08211, "ay":-0.03231, "alpha":-1.37064, "fx":[5.98115,3.90152,-3.308,-1.2052], "fy":[-3.17577,4.17118,2.12804,-5.23621]}, + {"t":1.56746, "x":3.02055, "y":3.47132, "heading":0.24082, "vx":0.42201, "vy":1.06931, "omega":-1.39925, "ax":0.06814, "ay":-0.02696, "alpha":0.31118, "fx":[0.08174,0.49588,2.14556,1.73237], "fy":[0.19971,-1.48681,-1.08073,0.60498]}, + {"t":1.59426, "x":3.03189, "y":3.49997, "heading":0.20332, "vx":0.42383, "vy":1.06858, "omega":-1.39091, "ax":0.05637, "ay":-0.0224, "alpha":1.98061, "fx":[-5.49697,-3.25277,7.32529,5.11044], "fy":[3.95968,-6.87772,-4.6775,6.13052]}, + {"t":1.62107, "x":3.04327, "y":3.5286, "heading":0.16603, "vx":0.42534, "vy":1.06798, "omega":-1.33783, "ax":0.04708, "ay":-0.01878, "alpha":3.5719, "fx":[-10.51326,-7.18478,12.01572,8.76099], "fy":[7.92846,-11.76822,-8.49987,11.11137]}, + {"t":1.64787, "x":3.05468, "y":3.55722, "heading":0.13018, "vx":0.42661, "vy":1.06748, "omega":-1.24209, "ax":0.0396, "ay":-0.01585, "alpha":5.0382, "fx":[-14.84459,-11.14103,16.08295,12.49253], "fy":[11.9273,-16.01863,-12.36991,15.42473]}, + {"t":1.67467, "x":3.06613, "y":3.58583, "heading":0.09688, "vx":0.42767, "vy":1.06705, "omega":-1.10704, "ax":0.03339, "ay":-0.0134, "alpha":6.35664, "fx":[-18.47872,-14.98038,19.50137,16.14137], "fy":[15.80295,-19.60019,-16.13485,19.05582]}, + {"t":1.70148, "x":3.07761, "y":3.61442, "heading":0.06721, "vx":0.42856, "vy":1.0667, "omega":-0.93666, "ax":0.02806, "ay":-0.01128, "alpha":7.52415, "fx":[-21.48606,-18.59374,22.32943,19.58518], "fy":[19.44084,-22.5702,-19.68028,22.07171]}, + {"t":1.72828, "x":3.0891, "y":3.64301, "heading":0.0421, "vx":0.42932, "vy":1.06639, "omega":-0.73499, "ax":0.02328, "ay":-0.00938, "alpha":8.54992, "fx":[-23.98461,-21.905,24.6734,22.73859], "fy":[22.76306,-25.03563,-22.92736,24.5865]}, + {"t":1.75508, "x":3.10062, "y":3.67159, "heading":0.0224, "vx":0.42994, "vy":1.06614, "omega":-0.50582, "ax":0.01851, "ay":-0.00776, "alpha":9.4488, "fx":[-26.1155,-24.86772,26.65362,25.54019], "fy":[25.71523,-27.12508,-25.8281,26.73076]}, + {"t":1.78189, "x":3.11215, "y":3.70016, "heading":0.00885, "vx":0.43044, "vy":1.06593, "omega":-0.25256, "ax":-0.52658, "ay":-1.39553, "alpha":8.64442, "fx":[-39.82016,-28.93129,13.54591,20.7713], "fy":[-0.99528,-42.86129,-46.76058,-0.63957]}, + {"t":1.80869, "x":3.1235, "y":3.72823, "heading":0.00208, "vx":0.41632, "vy":1.02853, "omega":-0.02086, "ax":-1.70701, "ay":-4.22378, "alpha":0.21947, "fx":[-29.37144,-28.50696,-26.46209,-27.28474], "fy":[-68.44092,-68.83898,-69.63781,-69.28513]}, + {"t":1.8355, "x":3.13404, "y":3.75428, "heading":0.00152, "vx":0.37057, "vy":0.91532, "omega":-0.01498, "ax":-1.71999, "ay":-4.25307, "alpha":0.12273, "fx":[-28.94209,-28.45726,-27.30166,-27.77326], "fy":[-69.19124,-69.40158,-69.86055,-69.66471]}, + {"t":1.8623, "x":3.14336, "y":3.77729, "heading":0.00112, "vx":0.32447, "vy":0.80132, "omega":-0.01169, "ax":-1.72469, "ay":-4.26275, "alpha":0.09029, "fx":[-28.80219,-28.44533,-27.5922,-27.94187], "fy":[-69.43957,-69.59122,-69.93202,-69.78829]}, + {"t":1.8891, "x":3.15144, "y":3.79724, "heading":0.0008, "vx":0.27824, "vy":0.68706, "omega":-0.00927, "ax":-1.72724, "ay":-4.26752, "alpha":0.07404, "fx":[-28.73497,-28.44233,-27.74152,-28.02931], "fy":[-69.56249,-69.68556,-69.96636,-69.84864]}, + {"t":1.91591, "x":3.15827, "y":3.81412, "heading":0.00055, "vx":0.23194, "vy":0.57268, "omega":-0.00728, "ax":-1.72888, "ay":-4.27034, "alpha":0.06428, "fx":[-28.69631,-28.44226,-27.83318,-28.08357], "fy":[-69.63551,-69.74168,-69.98619,-69.88406]}, + {"t":1.94271, "x":3.16387, "y":3.82794, "heading":0.00036, "vx":0.1856, "vy":0.45822, "omega":-0.00556, "ax":-1.73003, "ay":-4.27219, "alpha":0.05777, "fx":[-28.67149,-28.44319,-27.8954,-28.12074], "fy":[-69.68377,-69.77878,-69.99898,-69.90723]}, + {"t":1.96951, "x":3.16822, "y":3.83868, "heading":0.00021, "vx":0.13923, "vy":0.34371, "omega":-0.00401, "ax":-1.73089, "ay":-4.2735, "alpha":0.05312, "fx":[-28.65427,-28.44437,-27.94041,-28.14782], "fy":[-69.71803,-69.80513,-70.00789,-69.92356]}, + {"t":1.99632, "x":3.17133, "y":3.84636, "heading":0.0001, "vx":0.09284, "vy":0.22916, "omega":-0.00259, "ax":-1.73155, "ay":-4.27448, "alpha":0.04963, "fx":[-28.64158,-28.44547,-27.97444,-28.16837], "fy":[-69.74362,-69.82482,-70.01447,-69.9357]}, + {"t":2.02312, "x":3.1732, "y":3.85097, "heading":0.00003, "vx":0.04643, "vy":0.11459, "omega":-0.00126, "ax":-1.73206, "ay":-4.27524, "alpha":0.04691, "fx":[-28.63179,-28.4464,-28.001,-28.18444], "fy":[-69.7635,-69.84012,-70.01956,-69.9451]}, + {"t":2.04992, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PROtoC.traj b/src/main/deploy/choreo/PROtoC.traj index b079e800..bbb22130 100644 --- a/src/main/deploy/choreo/PROtoC.traj +++ b/src/main/deploy/choreo/PROtoC.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.6333351135253906, "y":0.6246773600578308, "heading":0.9420001549844138, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.274733781814575, "y":2.497971296310425, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.274733781814575, "y":2.497971296310425, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.681653261184693, "y":2.970940351486206, "heading":1.0349851207246612, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PRO.x", "val":1.6333351135253906}, "y":{"exp":"PRO.y", "val":0.6246773600578308}, "heading":{"exp":"0.9420001549844138 rad", "val":0.9420001549844138}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.274733781814575 m", "val":3.274733781814575}, "y":{"exp":"2.497971296310425 m", "val":2.497971296310425}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.274733781814575 m", "val":3.274733781814575}, "y":{"exp":"2.497971296310425 m", "val":2.497971296310425}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"C.x", "val":3.681653261184693}, "y":{"exp":"C.y", "val":2.970940351486206}, "heading":{"exp":"1.0349851207246612 rad", "val":1.0349851207246612}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,60 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.2487,1.88374], + "waypoints":[0.0,1.26269,1.93029], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0423, "ay":3.47042, "alpha":0.0068, "fx":[49.7105,49.76616,49.76122,49.70561], "fy":[56.75687,56.70808,56.71252,56.76125]}, - {"t":0.03902, "x":1.63565, "y":0.62732, "heading":0.942, "vx":0.11872, "vy":0.13542, "omega":0.00027, "ax":3.04212, "ay":3.47021, "alpha":0.00695, "fx":[49.70701,49.76388,49.75883,49.70201], "fy":[56.75407,56.70422,56.70876,56.75856]}, - {"t":0.07804, "x":1.6426, "y":0.63525, "heading":0.94201, "vx":0.23743, "vy":0.27084, "omega":0.00054, "ax":3.04192, "ay":3.46999, "alpha":0.00712, "fx":[49.70311,49.76133,49.75617,49.698], "fy":[56.75095,56.69991,56.70457,56.75555]}, - {"t":0.11707, "x":1.65418, "y":0.64846, "heading":0.94203, "vx":0.35613, "vy":0.40624, "omega":0.00081, "ax":3.0417, "ay":3.46974, "alpha":0.00731, "fx":[49.69872,49.75847,49.75317,49.69347], "fy":[56.74744,56.69507,56.69984,56.75216]}, - {"t":0.15609, "x":1.67039, "y":0.66695, "heading":0.94206, "vx":0.47482, "vy":0.54164, "omega":0.0011, "ax":3.04144, "ay":3.46945, "alpha":0.00752, "fx":[49.69374,49.75522,49.74977,49.68835], "fy":[56.74346,56.68957,56.6945,56.74832]}, - {"t":0.19511, "x":1.69124, "y":0.69073, "heading":0.94211, "vx":0.5935, "vy":0.67702, "omega":0.00139, "ax":3.04115, "ay":3.46913, "alpha":0.00776, "fx":[49.68805,49.75151,49.74588,49.68249], "fy":[56.73891,56.68329,56.68838,56.74393]}, - {"t":0.23413, "x":1.71671, "y":0.71979, "heading":0.94216, "vx":0.71218, "vy":0.8124, "omega":0.0017, "ax":3.04081, "ay":3.46875, "alpha":0.00804, "fx":[49.68149,49.74722,49.7414,49.67574], "fy":[56.73365,56.67604,56.68133,56.73887]}, - {"t":0.27315, "x":1.74682, "y":0.75413, "heading":0.94223, "vx":0.83083, "vy":0.94775, "omega":0.00201, "ax":3.04042, "ay":3.46831, "alpha":0.00836, "fx":[49.67384,49.74223,49.73617,49.66785], "fy":[56.72753,56.66759,56.6731,56.73296]}, - {"t":0.31218, "x":1.78155, "y":0.79375, "heading":0.94231, "vx":0.94948, "vy":1.08309, "omega":0.00234, "ax":3.03995, "ay":3.46779, "alpha":0.00875, "fx":[49.66479,49.73632,49.72999,49.65854], "fy":[56.72029,56.65759,56.66338,56.72599]}, - {"t":0.3512, "x":1.82092, "y":0.83866, "heading":0.9424, "vx":1.0681, "vy":1.21841, "omega":0.00268, "ax":3.03939, "ay":3.46717, "alpha":0.00921, "fx":[49.65393,49.72924,49.72257,49.64736], "fy":[56.7116,56.6456,56.65171,56.71761]}, - {"t":0.39022, "x":1.86491, "y":0.88884, "heading":0.9425, "vx":1.18671, "vy":1.35371, "omega":0.00304, "ax":3.03871, "ay":3.46641, "alpha":0.00978, "fx":[49.64066,49.72058,49.71351,49.63369], "fy":[56.70098,56.63094,56.63745,56.70738]}, - {"t":0.42924, "x":1.91353, "y":0.94431, "heading":0.94262, "vx":1.30528, "vy":1.48898, "omega":0.00342, "ax":3.03786, "ay":3.46546, "alpha":0.01049, "fx":[49.62407,49.70975,49.70218,49.61662], "fy":[56.6877,56.61261,56.61963,56.69459]}, - {"t":0.46826, "x":1.96678, "y":1.00505, "heading":0.94275, "vx":1.42382, "vy":1.62421, "omega":0.00383, "ax":3.03676, "ay":3.46423, "alpha":0.0114, "fx":[49.60274,49.69584,49.68762,49.59466], "fy":[56.67063,56.58905,56.59672,56.67816]}, - {"t":0.50729, "x":2.02465, "y":1.07107, "heading":0.9429, "vx":1.54233, "vy":1.75939, "omega":0.00427, "ax":3.0353, "ay":3.4626, "alpha":0.01261, "fx":[49.5743,49.67729,49.66822,49.5654], "fy":[56.64787,56.55764,56.56619,56.65625]}, - {"t":0.54631, "x":2.08715, "y":1.14236, "heading":0.94307, "vx":1.66077, "vy":1.8945, "omega":0.00476, "ax":3.03325, "ay":3.46032, "alpha":0.01431, "fx":[49.53449,49.65132,49.64107,49.52446], "fy":[56.61602,56.51367,56.52347,56.6256]}, - {"t":0.58533, "x":2.15427, "y":1.21892, "heading":0.94325, "vx":1.77913, "vy":2.02953, "omega":0.00532, "ax":3.03018, "ay":3.4569, "alpha":0.01687, "fx":[49.47478,49.61237,49.60038,49.46309], "fy":[56.56824,56.44772,56.45944,56.57965]}, - {"t":0.62435, "x":2.226, "y":1.30075, "heading":0.94346, "vx":1.89738, "vy":2.16443, "omega":0.00598, "ax":3.02507, "ay":3.4512, "alpha":0.02113, "fx":[49.37531,49.54749,49.53265,49.36094], "fy":[56.48862,56.33785,56.35287,56.50316]}, - {"t":0.66337, "x":2.30234, "y":1.38783, "heading":0.9437, "vx":2.01542, "vy":2.2991, "omega":0.00681, "ax":3.01486, "ay":3.43983, "alpha":0.02968, "fx":[49.17655,49.41778,49.39753,49.15721], "fy":[56.32947,56.11836,56.14035,56.35051]}, - {"t":0.7024, "x":2.38328, "y":1.48017, "heading":0.94396, "vx":2.13307, "vy":2.43333, "omega":0.00796, "ax":2.9844, "ay":3.40587, "alpha":0.05538, "fx":[48.58215,49.02911,48.99472,48.55088], "fy":[55.85258,55.46212,55.50793,55.89512]}, - {"t":0.74142, "x":2.46879, "y":1.57772, "heading":0.94427, "vx":2.24952, "vy":2.56623, "omega":0.01012, "ax":-0.19624, "ay":-0.18478, "alpha":2.46195, "fx":[-12.64153,-1.61938,6.35428,-4.92602], "fy":[-4.43821,-12.46876,-1.66449,6.48822]}, - {"t":0.78044, "x":2.55642, "y":1.67771, "heading":0.94467, "vx":2.24186, "vy":2.55902, "omega":0.10619, "ax":-2.98627, "ay":-3.40482, "alpha":0.04907, "fx":[-49.00166,-48.60835,-48.6367,-49.03245], "fy":[-55.51037,-55.85381,-55.81557,-55.46956]}, - {"t":0.81946, "x":2.64163, "y":1.77498, "heading":0.94881, "vx":2.12533, "vy":2.42616, "omega":0.10811, "ax":-3.01574, "ay":-3.43922, "alpha":0.02327, "fx":[-49.38747,-49.19912,-49.21535,-49.40425], "fy":[-56.15114,-56.31592,-56.29848,-56.13311]}, - {"t":0.85848, "x":2.72227, "y":1.86704, "heading":0.95303, "vx":2.00765, "vy":2.29195, "omega":0.10902, "ax":-3.02564, "ay":-3.45078, "alpha":0.01453, "fx":[-49.51683,-49.3988,-49.40986,-49.5281], "fy":[-56.36753,-56.47086,-56.45982,-56.35626]}, - {"t":0.89751, "x":2.79831, "y":1.95384, "heading":0.95728, "vx":1.88959, "vy":2.1573, "omega":0.10958, "ax":-3.0306, "ay":-3.45657, "alpha":0.01011, "fx":[-49.58154,-49.49933,-49.50752,-49.58983], "fy":[-56.47634,-56.54834,-56.54046,-56.46836]}, - {"t":0.93653, "x":2.86974, "y":2.03539, "heading":0.96156, "vx":1.77133, "vy":2.02242, "omega":0.10998, "ax":-3.03358, "ay":-3.46006, "alpha":0.00742, "fx":[-49.62034,-49.55994,-49.56627,-49.62672], "fy":[-56.54186,-56.59476,-56.5888,-56.53584]}, - {"t":0.97555, "x":2.93655, "y":2.11168, "heading":0.96585, "vx":1.65295, "vy":1.8874, "omega":0.11027, "ax":-3.03557, "ay":-3.46238, "alpha":0.00561, "fx":[-49.64618,-49.6005,-49.60552,-49.65122], "fy":[-56.58564,-56.62565,-56.62099,-56.58095]}, - {"t":1.01457, "x":2.99874, "y":2.18269, "heading":0.97015, "vx":1.5345, "vy":1.75229, "omega":0.11049, "ax":-3.037, "ay":-3.46404, "alpha":0.00431, "fx":[-49.66462,-49.62957,-49.63359,-49.66865], "fy":[-56.61696,-56.64766,-56.64397,-56.61325]}, - {"t":1.05359, "x":3.0563, "y":2.24843, "heading":0.97447, "vx":1.41599, "vy":1.61712, "omega":0.11066, "ax":-3.03806, "ay":-3.46529, "alpha":0.00332, "fx":[-49.67845,-49.65144,-49.65466,-49.68167], "fy":[-56.64048,-56.66414,-56.6612,-56.63753]}, - {"t":1.09262, "x":3.10925, "y":2.3089, "heading":0.97878, "vx":1.29744, "vy":1.48189, "omega":0.11079, "ax":-3.03889, "ay":-3.46626, "alpha":0.00254, "fx":[-49.6892,-49.6685,-49.67106,-49.69176], "fy":[-56.65878,-56.67692,-56.67459,-56.65645]}, - {"t":1.13164, "x":3.15756, "y":2.36409, "heading":0.98311, "vx":1.17885, "vy":1.34663, "omega":0.11088, "ax":-3.03956, "ay":-3.46703, "alpha":0.00192, "fx":[-49.6978,-49.68217,-49.68418,-49.69981], "fy":[-56.67343,-56.68712,-56.68531,-56.67162]}, - {"t":1.17066, "x":3.20125, "y":2.41399, "heading":0.98743, "vx":1.06024, "vy":1.21134, "omega":0.11096, "ax":-3.0401, "ay":-3.46767, "alpha":0.00141, "fx":[-49.70484,-49.69339,-49.69491,-49.70636], "fy":[-56.68542,-56.69545,-56.69408,-56.68405]}, - {"t":1.20968, "x":3.24031, "y":2.45862, "heading":0.99176, "vx":0.94161, "vy":1.07603, "omega":0.11101, "ax":-3.04055, "ay":-3.4682, "alpha":0.00098, "fx":[-49.71071,-49.70276,-49.70385,-49.71181], "fy":[-56.6954,-56.70237,-56.70139,-56.69442]}, - {"t":1.2487, "x":3.27473, "y":2.49797, "heading":0.9961, "vx":0.82296, "vy":0.94069, "omega":0.11105, "ax":-0.21883, "ay":0.18009, "alpha":0.35826, "fx":[-4.94745,-3.27644,-2.2061,-3.87997], "fy":[2.66244,1.5785,3.22883,4.30706]}, - {"t":1.28213, "x":3.30212, "y":2.52951, "heading":0.99981, "vx":0.81565, "vy":0.94671, "omega":0.12303, "ax":-0.0265, "ay":0.02281, "alpha":0.22906, "fx":[-1.30883,-0.23381,0.4424,-0.63275], "fy":[0.192,-0.49872,0.55397,1.2444]}, - {"t":1.31555, "x":3.32936, "y":2.56117, "heading":1.00392, "vx":0.81476, "vy":0.94747, "omega":0.13068, "ax":-0.00326, "ay":0.0028, "alpha":0.0982, "fx":[-0.42832,0.0338,0.32177,-0.14034], "fy":[-0.0333,-0.32747,0.12492,0.41908]}, - {"t":1.34897, "x":3.35659, "y":2.59284, "heading":1.00829, "vx":0.81466, "vy":0.94757, "omega":0.13396, "ax":-0.0004, "ay":0.00034, "alpha":-0.03225, "fx":[0.11653,-0.03568,-0.12962,0.02259], "fy":[0.03215,0.12811,-0.02089,-0.11685]}, - {"t":1.38239, "x":3.38382, "y":2.62451, "heading":1.01276, "vx":0.81464, "vy":0.94758, "omega":0.13289, "ax":-0.00005, "ay":0.00004, "alpha":-0.16285, "fx":[0.61999,-0.15067,-0.62159,0.14906], "fy":[0.13738,0.61843,-0.136,-0.61705]}, - {"t":1.41582, "x":3.41105, "y":2.65618, "heading":1.01721, "vx":0.81464, "vy":0.94758, "omega":0.12744, "ax":-0.00001, "ay":0.00001, "alpha":-0.29417, "fx":[1.12019,-0.27577,-1.12038,0.27557], "fy":[0.25198,1.11476,-0.25181,-1.11459]}, - {"t":1.44924, "x":3.43828, "y":2.68785, "heading":1.02147, "vx":0.81464, "vy":0.94758, "omega":0.11761, "ax":0.0, "ay":0.0, "alpha":-0.42679, "fx":[1.6238,-0.40686,-1.62382,0.40683], "fy":[0.37239,1.61553,-0.37237,-1.61551]}, - {"t":1.48266, "x":3.4655, "y":2.71952, "heading":1.0254, "vx":0.81464, "vy":0.94758, "omega":0.10335, "ax":0.0, "ay":0.0, "alpha":-0.56132, "fx":[2.13368,-0.54343,-2.13369,0.54343], "fy":[0.49815,2.12261,-0.49815,-2.1226]}, - {"t":1.51609, "x":3.49273, "y":2.75119, "heading":1.02885, "vx":0.81464, "vy":0.94758, "omega":0.08459, "ax":0.0, "ay":0.0, "alpha":-0.69832, "fx":[2.65232,-0.68519,-2.65232,0.68518], "fy":[0.6289,2.63834,-0.6289,-2.63834]}, - {"t":1.54951, "x":3.51996, "y":2.78286, "heading":1.03168, "vx":0.81464, "vy":0.94758, "omega":0.06125, "ax":-0.00002, "ay":-0.00002, "alpha":-0.83841, "fx":[3.18193,-0.83189,-3.18254,0.83128], "fy":[0.76373,3.16493,-0.76439,-3.16559]}, - {"t":1.58293, "x":3.54719, "y":2.81453, "heading":1.03372, "vx":0.81464, "vy":0.94758, "omega":0.03322, "ax":-0.36075, "ay":-0.41962, "alpha":-0.95474, "fx":[-2.26053,-6.92545,-9.49346,-4.91087], "fy":[-6.0444,-3.24466,-7.69221,-10.45887]}, - {"t":1.61635, "x":3.57421, "y":2.84597, "heading":1.03484, "vx":0.80258, "vy":0.93355, "omega":0.00131, "ax":-2.9817, "ay":-3.46828, "alpha":-0.01161, "fx":[-48.70576,-48.79994,-48.78449,-48.69044], "fy":[-56.73271,-56.65201,-56.66692,-56.74747]}, - {"t":1.64978, "x":3.59937, "y":2.87523, "heading":1.03488, "vx":0.70293, "vy":0.81764, "omega":0.00093, "ax":-2.99684, "ay":-3.48588, "alpha":-0.00652, "fx":[-48.97048,-49.02356,-49.01463,-48.96159], "fy":[-57.00629,-56.96073,-56.96886,-57.01438]}, - {"t":1.6832, "x":3.62119, "y":2.90061, "heading":1.03491, "vx":0.60276, "vy":0.70113, "omega":0.00071, "ax":-3.00185, "ay":-3.49171, "alpha":-0.00484, "fx":[-49.05813,-49.09757,-49.09088,-49.05146], "fy":[-57.09683,-57.06296,-57.06894,-57.10278]}, - {"t":1.71662, "x":3.63966, "y":2.9221, "heading":1.03493, "vx":0.50243, "vy":0.58442, "omega":0.00055, "ax":-3.00435, "ay":-3.49462, "alpha":-0.004, "fx":[-49.10182,-49.13445,-49.12889,-49.09627], "fy":[-57.14195,-57.11392,-57.11884,-57.14686]}, - {"t":1.75004, "x":3.65478, "y":2.93968, "heading":1.03495, "vx":0.40202, "vy":0.46762, "omega":0.00041, "ax":-3.00584, "ay":-3.49636, "alpha":-0.0035, "fx":[-49.12798,-49.15654,-49.15166,-49.12312], "fy":[-57.16898,-57.14445,-57.14874,-57.17326]}, - {"t":1.78347, "x":3.66653, "y":2.95335, "heading":1.03497, "vx":0.30156, "vy":0.35077, "omega":0.0003, "ax":-3.00684, "ay":-3.49752, "alpha":-0.00317, "fx":[-49.14541,-49.17125,-49.16683,-49.141], "fy":[-57.18698,-57.16477,-57.16865,-57.19084]}, - {"t":1.81689, "x":3.67493, "y":2.96312, "heading":1.03498, "vx":0.20106, "vy":0.23387, "omega":0.00019, "ax":-3.00755, "ay":-3.49835, "alpha":-0.00293, "fx":[-49.15785,-49.18174,-49.17765,-49.15376], "fy":[-57.19982,-57.17928,-57.18286,-57.20339]}, - {"t":1.85031, "x":3.67997, "y":2.96899, "heading":1.03498, "vx":0.10054, "vy":0.11695, "omega":0.00009, "ax":-3.00809, "ay":-3.49897, "alpha":-0.00275, "fx":[-49.16717,-49.18961,-49.18576,-49.16333], "fy":[-57.20944,-57.19016,-57.19351,-57.21279]}, - {"t":1.88374, "x":3.68165, "y":2.97094, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.04234, "ay":3.47052, "alpha":0.00935, "fx":[49.70155,49.77802,49.77122,49.69484], "fy":[56.76695,56.69992,56.70602,56.77296]}, + {"t":0.03946, "x":1.6357, "y":0.62738, "heading":0.942, "vx":0.12005, "vy":0.13694, "omega":0.00037, "ax":3.04216, "ay":3.47033, "alpha":0.00958, "fx":[49.69778,49.77616,49.76919,49.69091], "fy":[56.76451,56.69581,56.70207,56.77067]}, + {"t":0.07892, "x":1.64281, "y":0.63548, "heading":0.94201, "vx":0.24009, "vy":0.27388, "omega":0.00075, "ax":3.04196, "ay":3.47011, "alpha":0.00984, "fx":[49.69356,49.77407,49.76692,49.68651], "fy":[56.76178,56.69121,56.69765,56.76812]}, + {"t":0.11838, "x":1.65465, "y":0.64899, "heading":0.94204, "vx":0.36012, "vy":0.41081, "omega":0.00114, "ax":3.04174, "ay":3.46986, "alpha":0.01014, "fx":[49.68881,49.77173,49.76436,49.68155], "fy":[56.75871,56.68604,56.69268,56.76524]}, + {"t":0.15784, "x":1.67123, "y":0.6679, "heading":0.94209, "vx":0.48015, "vy":0.54773, "omega":0.00154, "ax":3.04148, "ay":3.46959, "alpha":0.01047, "fx":[49.68343,49.76907,49.76146,49.67594], "fy":[56.75523,56.68017,56.68704,56.76199]}, + {"t":0.1973, "x":1.69254, "y":0.69222, "heading":0.94215, "vx":0.60016, "vy":0.68463, "omega":0.00195, "ax":3.04119, "ay":3.46927, "alpha":0.01085, "fx":[49.67728,49.76603,49.75814,49.66952], "fy":[56.75126,56.67346,56.6806,56.75826]}, + {"t":0.23676, "x":1.71859, "y":0.72193, "heading":0.94223, "vx":0.72016, "vy":0.82153, "omega":0.00238, "ax":3.04086, "ay":3.4689, "alpha":0.01129, "fx":[49.67018,49.76252,49.75432,49.6621], "fy":[56.74666,56.66572,56.67317,56.75397]}, + {"t":0.27621, "x":1.74938, "y":0.75705, "heading":0.94232, "vx":0.84015, "vy":0.95841, "omega":0.00282, "ax":3.04047, "ay":3.46847, "alpha":0.0118, "fx":[49.66189,49.75843,49.74985,49.65345], "fy":[56.7413,56.65668,56.66449,56.74895]}, + {"t":0.31567, "x":1.7849, "y":0.79757, "heading":0.94243, "vx":0.96013, "vy":1.09527, "omega":0.00329, "ax":3.04001, "ay":3.46796, "alpha":0.01241, "fx":[49.65208,49.75359,49.74456,49.64322], "fy":[56.73495,56.64599,56.65422,56.74302]}, + {"t":0.35513, "x":1.82515, "y":0.84349, "heading":0.94256, "vx":1.08009, "vy":1.23211, "omega":0.00378, "ax":3.03946, "ay":3.46735, "alpha":0.01314, "fx":[49.6403,49.74777,49.73822,49.63093], "fy":[56.72733,56.63315,56.6419,56.7359]}, + {"t":0.39459, "x":1.87013, "y":0.89481, "heading":0.94271, "vx":1.20002, "vy":1.36893, "omega":0.0043, "ax":3.03878, "ay":3.4666, "alpha":0.01404, "fx":[49.6259,49.74066,49.73046,49.6159], "fy":[56.71801,56.61743,56.62683,56.72719]}, + {"t":0.43405, "x":1.91985, "y":0.95152, "heading":0.94288, "vx":1.31993, "vy":1.50572, "omega":0.00485, "ax":3.03793, "ay":3.46567, "alpha":0.01516, "fx":[49.60786,49.73176,49.72074,49.59709], "fy":[56.70634,56.59777,56.60797,56.71629]}, + {"t":0.47351, "x":1.9743, "y":1.01363, "heading":0.94307, "vx":1.4398, "vy":1.64247, "omega":0.00545, "ax":3.03684, "ay":3.46447, "alpha":0.0166, "fx":[49.58464,49.7203,49.70824,49.57288], "fy":[56.69131,56.57244,56.58369,56.70226]}, + {"t":0.51297, "x":2.03348, "y":1.08114, "heading":0.94329, "vx":1.55963, "vy":1.77918, "omega":0.0061, "ax":3.03538, "ay":3.46286, "alpha":0.01853, "fx":[49.55362,49.70498,49.69155,49.54054], "fy":[56.67124,56.5386,56.55127,56.68352]}, + {"t":0.55243, "x":2.09738, "y":1.15404, "heading":0.94353, "vx":1.67941, "vy":1.91582, "omega":0.00683, "ax":3.03333, "ay":3.4606, "alpha":0.02124, "fx":[49.51006,49.68349,49.66812,49.49517], "fy":[56.64304,56.4911,56.50579,56.65723]}, + {"t":0.59189, "x":2.16601, "y":1.23233, "heading":0.9438, "vx":1.7991, "vy":2.05237, "omega":0.00767, "ax":3.03025, "ay":3.45721, "alpha":0.02532, "fx":[49.44446,49.6511,49.63288,49.4269], "fy":[56.60058,56.41956,56.43736,56.61767]}, + {"t":0.63135, "x":2.23936, "y":1.31601, "heading":0.9441, "vx":1.91867, "vy":2.18879, "omega":0.00867, "ax":3.02509, "ay":3.45152, "alpha":0.03218, "fx":[49.33443,49.59677,49.57384,49.31257], "fy":[56.52932,56.29958,56.32277,56.55138]}, + {"t":0.67081, "x":2.31743, "y":1.40507, "heading":0.94444, "vx":2.03804, "vy":2.32499, "omega":0.00994, "ax":3.01465, "ay":3.44002, "alpha":0.04608, "fx":[49.11178,49.48669,49.45461,49.08188], "fy":[56.38497,56.05683,56.0916,56.41743]}, + {"t":0.71027, "x":2.40019, "y":1.49949, "heading":0.94483, "vx":2.15699, "vy":2.46073, "omega":0.01176, "ax":2.98243, "ay":3.40451, "alpha":0.08928, "fx":[48.42266,49.1445,49.08735,48.37358], "fy":[55.93649,55.30586,55.3821,56.00425]}, + {"t":0.74972, "x":2.48763, "y":1.59924, "heading":0.9453, "vx":2.27468, "vy":2.59507, "omega":0.01528, "ax":-2.08877, "ay":-2.35101, "alpha":1.46878, "fx":[-39.38208,-30.87268,-28.30233,-38.03225], "fy":[-36.9213,-43.88656,-40.50442,-32.42602]}, + {"t":0.78918, "x":2.57576, "y":1.6998, "heading":0.9459, "vx":2.19226, "vy":2.5023, "omega":0.07324, "ax":-2.99066, "ay":-3.40924, "alpha":0.07191, "fx":[-49.15731,-48.58116,-48.62334,-49.20474], "fy":[-55.51192,-56.01507,-55.95996,-55.45128]}, + {"t":0.82864, "x":2.65994, "y":1.79589, "heading":0.94879, "vx":2.07425, "vy":2.36777, "omega":0.07608, "ax":-3.01731, "ay":-3.4407, "alpha":0.03469, "fx":[-49.45527,-49.17453,-49.19866,-49.48063], "fy":[-56.1393,-56.38492,-56.35925,-56.11232]}, + {"t":0.8681, "x":2.73944, "y":1.88664, "heading":0.95179, "vx":1.95519, "vy":2.232, "omega":0.07745, "ax":-3.02654, "ay":-3.4516, "alpha":0.02182, "fx":[-49.5584,-49.38119,-49.39755,-49.57525], "fy":[-56.35778,-56.51291,-56.49666,-56.341]}, + {"t":0.90756, "x":2.81423, "y":1.97203, "heading":0.95485, "vx":1.83576, "vy":2.09581, "omega":0.07831, "ax":-3.03122, "ay":-3.45713, "alpha":0.01529, "fx":[-49.61067,-49.48635,-49.49842,-49.62298], "fy":[-56.46888,-56.57775,-56.56616,-56.45703]}, + {"t":0.94702, "x":2.88431, "y":2.05203, "heading":0.95794, "vx":1.71615, "vy":1.95939, "omega":0.07891, "ax":-3.03405, "ay":-3.46047, "alpha":0.01133, "fx":[-49.64226,-49.55008,-49.5594,-49.65171], "fy":[-56.53616,-56.61689,-56.60812,-56.52724]}, + {"t":0.98648, "x":2.94966, "y":2.12666, "heading":0.96105, "vx":1.59643, "vy":1.82284, "omega":0.07936, "ax":-3.03594, "ay":-3.46271, "alpha":0.00867, "fx":[-49.6634,-49.59285,-49.60025,-49.67088], "fy":[-56.58126,-56.64307,-56.63619,-56.5743]}, + {"t":1.02594, "x":3.01029, "y":2.19589, "heading":0.96418, "vx":1.47664, "vy":1.68621, "omega":0.0797, "ax":-3.0373, "ay":-3.46431, "alpha":0.00675, "fx":[-49.67856,-49.62356,-49.62953,-49.68457], "fy":[-56.61361,-56.66179,-56.6563,-56.60806]}, + {"t":1.0654, "x":3.0662, "y":2.25973, "heading":0.96733, "vx":1.35679, "vy":1.54951, "omega":0.07996, "ax":-3.03832, "ay":-3.46552, "alpha":0.00531, "fx":[-49.68995,-49.64668,-49.65153,-49.69483], "fy":[-56.63793,-56.67584,-56.67141,-56.63347]}, + {"t":1.10486, "x":3.11737, "y":2.31817, "heading":0.97048, "vx":1.2369, "vy":1.41276, "omega":0.08017, "ax":-3.03912, "ay":-3.46646, "alpha":0.00419, "fx":[-49.69883,-49.66473,-49.66866,-49.70279], "fy":[-56.65688,-56.68677,-56.68319,-56.65328]}, + {"t":1.14432, "x":3.16381, "y":2.37122, "heading":0.97365, "vx":1.11698, "vy":1.27598, "omega":0.08034, "ax":-3.03975, "ay":-3.46721, "alpha":0.00328, "fx":[-49.70596,-49.67921,-49.68239,-49.70915], "fy":[-56.67206,-56.6955,-56.69263,-56.66917]}, + {"t":1.18378, "x":3.20552, "y":2.41887, "heading":0.97682, "vx":0.99703, "vy":1.13917, "omega":0.08047, "ax":-3.04028, "ay":-3.46783, "alpha":0.00254, "fx":[-49.7118,-49.69109,-49.69362,-49.71433], "fy":[-56.68449,-56.70264,-56.70036,-56.68221]}, + {"t":1.22324, "x":3.24249, "y":2.46112, "heading":0.97999, "vx":0.87706, "vy":1.00233, "omega":0.08057, "ax":-3.04071, "ay":-3.46835, "alpha":0.00192, "fx":[-49.71668,-49.70101,-49.70298,-49.71865], "fy":[-56.69486,-56.70858,-56.70682,-56.69309]}, + {"t":1.26269, "x":3.27473, "y":2.49797, "heading":0.98317, "vx":0.75708, "vy":0.86547, "omega":0.08065, "ax":-0.19506, "ay":0.16154, "alpha":0.83838, "fx":[-6.40085,-2.52519,0.03041,-3.85981], "fy":[2.02908,-0.56773,3.26656,5.83582]}, + {"t":1.29783, "x":3.30121, "y":2.52848, "heading":0.98601, "vx":0.75023, "vy":0.87115, "omega":0.1101, "ax":-0.01841, "ay":0.01584, "alpha":0.65245, "fx":[-2.80173,0.23279,2.20024,-0.83508], "fy":[-0.2216,-2.23161,0.74036,2.74875]}, + {"t":1.33297, "x":3.32756, "y":2.5591, "heading":0.98987, "vx":0.74958, "vy":0.8717, "omega":0.13303, "ax":-0.00176, "ay":0.00151, "alpha":0.46413, "fx":[-1.80651,0.35796,1.74903,-0.41548], "fy":[-0.32424,-1.74524,0.37372,1.79464]}, + {"t":1.3681, "x":3.3539, "y":2.58973, "heading":0.99455, "vx":0.74952, "vy":0.87175, "omega":0.14934, "ax":-0.00017, "ay":0.00014, "alpha":0.27766, "fx":[-1.06528,0.23355,1.05979,-0.23904], "fy":[-0.21138,-1.05539,0.2161,1.0601]}, + {"t":1.40324, "x":3.38024, "y":2.62036, "heading":0.9998, "vx":0.74951, "vy":0.87176, "omega":0.15909, "ax":-0.00002, "ay":0.00001, "alpha":0.09232, "fx":[-0.35317,0.08015,0.35264,-0.08067], "fy":[-0.0727,-0.35105,0.07314,0.3515]}, + {"t":1.43838, "x":3.40657, "y":2.65099, "heading":1.00539, "vx":0.74951, "vy":0.87176, "omega":0.16234, "ax":0.0, "ay":0.0, "alpha":-0.09264, "fx":[0.3537,-0.08269,-0.35375,0.08264], "fy":[0.07518,0.35207,-0.07513,-0.35203]}, + {"t":1.47351, "x":3.43291, "y":2.68162, "heading":1.01109, "vx":0.74951, "vy":0.87176, "omega":0.15908, "ax":0.0, "ay":0.0, "alpha":-0.27798, "fx":[1.06009,-0.25406,-1.06009,0.25405], "fy":[0.23156,1.05493,-0.23155,-1.05492]}, + {"t":1.50865, "x":3.45924, "y":2.71225, "heading":1.01668, "vx":0.74951, "vy":0.87176, "omega":0.14931, "ax":0.0, "ay":0.0, "alpha":-0.46446, "fx":[1.76902,-0.43433,-1.76902,0.43433], "fy":[0.39678,1.76018,-0.39678,-1.76018]}, + {"t":1.54379, "x":3.48558, "y":2.74288, "heading":1.02193, "vx":0.74951, "vy":0.87176, "omega":0.13299, "ax":0.0, "ay":0.0, "alpha":-0.65282, "fx":[2.48349,-0.62344,-2.48349,0.62344], "fy":[0.57074,2.47079,-0.57074,-2.47079]}, + {"t":1.57892, "x":3.51191, "y":2.77351, "heading":1.0266, "vx":0.74951, "vy":0.87176, "omega":0.11006, "ax":0.0, "ay":0.0, "alpha":-0.84382, "fx":[3.20663,-0.82076,-3.20663,0.82076], "fy":[0.75272,3.18989,-0.75271,-3.18989]}, + {"t":1.61406, "x":3.53825, "y":2.80414, "heading":1.03047, "vx":0.74951, "vy":0.87176, "omega":0.08041, "ax":-0.00001, "ay":-0.00001, "alpha":-1.0382, "fx":[3.94152,-1.0252,-3.9419,1.02481], "fy":[0.94117,3.9206,-0.94156,-3.92099]}, + {"t":1.6492, "x":3.56458, "y":2.83478, "heading":1.03329, "vx":0.74951, "vy":0.87176, "omega":0.04393, "ax":-0.31251, "ay":-0.36348, "alpha":-1.21074, "fx":[-0.49172,-6.39622,-9.66871,-3.87885], "fy":[-4.89386,-1.35757,-7.01334,-10.50406]}, + {"t":1.68433, "x":3.59073, "y":2.86518, "heading":1.03483, "vx":0.73853, "vy":0.85899, "omega":0.00139, "ax":-2.98683, "ay":-3.474, "alpha":-0.01235, "fx":[-48.78712,-48.88743,-48.87082,-48.77064], "fy":[-56.82848,-56.74247,-56.7582,-56.84405]}, + {"t":1.71947, "x":3.61483, "y":2.89322, "heading":1.03488, "vx":0.63358, "vy":0.73692, "omega":0.00095, "ax":-2.99946, "ay":-3.48868, "alpha":-0.007, "fx":[-49.01167,-49.06871,-49.05907,-49.00208], "fy":[-57.05349,-57.00452,-57.01322,-57.06213]}, + {"t":1.75461, "x":3.63524, "y":2.91696, "heading":1.03492, "vx":0.52819, "vy":0.61434, "omega":0.00071, "ax":-3.00363, "ay":-3.49354, "alpha":-0.00523, "fx":[-49.08596,-49.12864,-49.12138,-49.07872], "fy":[-57.12789,-57.09122,-57.09768,-57.13431]}, + {"t":1.78974, "x":3.65195, "y":2.93639, "heading":1.03494, "vx":0.42265, "vy":0.49159, "omega":0.00052, "ax":-3.00571, "ay":-3.49596, "alpha":-0.00435, "fx":[-49.12298,-49.1585,-49.15244,-49.11693], "fy":[-57.16496,-57.13444,-57.13978,-57.17028]}, + {"t":1.82488, "x":3.66494, "y":2.9515, "heading":1.03496, "vx":0.31704, "vy":0.36875, "omega":0.00037, "ax":-3.00696, "ay":-3.49742, "alpha":-0.00383, "fx":[-49.14515,-49.17639,-49.17104,-49.13982], "fy":[-57.18716,-57.16032,-57.165,-57.19183]}, + {"t":1.86002, "x":3.67423, "y":2.9623, "heading":1.03497, "vx":0.21139, "vy":0.24587, "omega":0.00024, "ax":-3.00779, "ay":-3.49838, "alpha":-0.00348, "fx":[-49.15992,-49.18829,-49.18343,-49.15506], "fy":[-57.20194,-57.17755,-57.1818,-57.20618]}, + {"t":1.89515, "x":3.6798, "y":2.96878, "heading":1.03498, "vx":0.1057, "vy":0.12295, "omega":0.00011, "ax":-3.00838, "ay":-3.49907, "alpha":-0.00323, "fx":[-49.17045,-49.19679,-49.19227,-49.16594], "fy":[-57.21249,-57.18985,-57.19379,-57.21642]}, + {"t":1.93029, "x":3.68165, "y":2.97094, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PROtoD.traj b/src/main/deploy/choreo/PROtoD.traj index 67573fa4..2fcf76a6 100644 --- a/src/main/deploy/choreo/PROtoD.traj +++ b/src/main/deploy/choreo/PROtoD.traj @@ -10,7 +10,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,60 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.29973,1.85258], + "waypoints":[0.0,1.31406,1.89238], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37099, "ay":3.15195, "alpha":0.00824, "fx":[55.08474,55.14519,55.13385,55.07345], "fy":[51.55463,51.49,51.50228,51.56683]}, - {"t":0.03823, "x":1.6358, "y":0.62698, "heading":0.942, "vx":0.12886, "vy":0.12049, "omega":0.00032, "ax":3.3708, "ay":3.15177, "alpha":0.0084, "fx":[55.08117,55.14279,55.13123,55.06966], "fy":[51.55221,51.48632,51.49884,51.56465]}, - {"t":0.07645, "x":1.64319, "y":0.63389, "heading":0.94201, "vx":0.25772, "vy":0.24097, "omega":0.00064, "ax":3.37059, "ay":3.15158, "alpha":0.00858, "fx":[55.0772,55.14013,55.12833,55.06545], "fy":[51.54952,51.48223,51.49503,51.56223]}, - {"t":0.11468, "x":1.6555, "y":0.6454, "heading":0.94204, "vx":0.38657, "vy":0.36145, "omega":0.00096, "ax":3.37036, "ay":3.15135, "alpha":0.00878, "fx":[55.07276,55.13715,55.12508,55.06075], "fy":[51.54652,51.47767,51.49077,51.55952]}, - {"t":0.15291, "x":1.67274, "y":0.66152, "heading":0.94207, "vx":0.51541, "vy":0.48192, "omega":0.0013, "ax":3.37009, "ay":3.1511, "alpha":0.009, "fx":[55.06777,55.1338,55.12142,55.05545], "fy":[51.54314,51.47253,51.48597,51.55648]}, - {"t":0.19114, "x":1.69491, "y":0.68225, "heading":0.94212, "vx":0.64424, "vy":0.60238, "omega":0.00164, "ax":3.36979, "ay":3.15082, "alpha":0.00926, "fx":[55.06211,55.13001,55.11728,55.04945], "fy":[51.5393,51.46671,51.48054,51.55303]}, - {"t":0.22936, "x":1.722, "y":0.70758, "heading":0.94219, "vx":0.77306, "vy":0.72282, "omega":0.002, "ax":3.36945, "ay":3.1505, "alpha":0.00955, "fx":[55.05564,55.12567,55.11255,55.04259], "fy":[51.53492,51.46006,51.47433,51.54908]}, - {"t":0.26759, "x":1.75401, "y":0.73751, "heading":0.94226, "vx":0.90186, "vy":0.84326, "omega":0.00236, "ax":3.36906, "ay":3.15012, "alpha":0.00988, "fx":[55.04819,55.12066,55.10709,55.03468], "fy":[51.52986,51.45238,51.46717,51.54454]}, - {"t":0.30582, "x":1.79095, "y":0.77205, "heading":0.94235, "vx":1.03065, "vy":0.96368, "omega":0.00274, "ax":3.3686, "ay":3.14969, "alpha":0.01028, "fx":[55.03948,55.11483,55.10071,55.02545], "fy":[51.52397,51.44342,51.45882,51.53923]}, - {"t":0.34405, "x":1.83281, "y":0.81119, "heading":0.94246, "vx":1.15942, "vy":1.08408, "omega":0.00313, "ax":3.36805, "ay":3.14917, "alpha":0.01074, "fx":[55.0292,55.10793,55.09318,55.01454], "fy":[51.51699,51.43283,51.44895,51.53296]}, - {"t":0.38227, "x":1.87959, "y":0.85493, "heading":0.94258, "vx":1.28817, "vy":1.20447, "omega":0.00354, "ax":3.3674, "ay":3.14855, "alpha":0.0113, "fx":[55.01686,55.09965,55.08415,55.00146], "fy":[51.50863,51.42013,51.4371,51.52545]}, - {"t":0.4205, "x":1.93129, "y":0.90328, "heading":0.94271, "vx":1.4169, "vy":1.32483, "omega":0.00398, "ax":3.3666, "ay":3.1478, "alpha":0.01198, "fx":[55.00177,55.08954,55.07312,54.98546], "fy":[51.49841,51.4046,51.42263,51.51626]}, - {"t":0.45873, "x":1.98792, "y":0.95622, "heading":0.94286, "vx":1.5456, "vy":1.44516, "omega":0.00443, "ax":3.3656, "ay":3.14685, "alpha":0.01283, "fx":[54.98292,55.0769,55.05933,54.96548], "fy":[51.48563,51.3852,51.40455,51.50478]}, - {"t":0.49695, "x":2.04946, "y":1.01376, "heading":0.94303, "vx":1.67425, "vy":1.56545, "omega":0.00492, "ax":3.36432, "ay":3.14564, "alpha":0.01392, "fx":[54.95869,55.06065,55.04161,54.93979], "fy":[51.46921,51.36025,51.38131,51.49003]}, - {"t":0.53518, "x":2.11592, "y":1.0759, "heading":0.94322, "vx":1.80286, "vy":1.6857, "omega":0.00546, "ax":3.36261, "ay":3.14402, "alpha":0.01538, "fx":[54.92638,55.03899,55.01799,54.90556], "fy":[51.44732,51.32699,51.35035,51.47038]}, - {"t":0.57341, "x":2.1873, "y":1.14264, "heading":0.94343, "vx":1.93141, "vy":1.80589, "omega":0.00604, "ax":3.36022, "ay":3.14176, "alpha":0.01742, "fx":[54.88117,55.00869,54.98497,54.85768], "fy":[51.41668,51.28045,51.30703,51.44289]}, - {"t":0.61164, "x":2.26359, "y":1.21397, "heading":0.94366, "vx":2.05986, "vy":1.92599, "omega":0.00671, "ax":3.35663, "ay":3.13837, "alpha":0.02049, "fx":[54.81339,54.96327,54.93551,54.78595], "fy":[51.37076,51.2107,51.24216,51.4017]}, - {"t":0.64986, "x":2.34478, "y":1.28989, "heading":0.94392, "vx":2.18817, "vy":2.04596, "omega":0.00749, "ax":3.35067, "ay":3.13273, "alpha":0.02561, "fx":[54.70054,54.88766,54.85328,54.66665], "fy":[51.2943,51.09458,51.1343,51.33321]}, - {"t":0.68809, "x":2.43088, "y":1.37039, "heading":0.9442, "vx":2.31626, "vy":2.16572, "omega":0.00847, "ax":3.33878, "ay":3.12148, "alpha":0.03586, "fx":[54.47535,54.73675,54.68954,54.42911], "fy":[51.14169,50.86299,50.91962,51.19675]}, - {"t":0.72632, "x":2.52186, "y":1.45546, "heading":0.94453, "vx":2.44389, "vy":2.28504, "omega":0.00984, "ax":3.30345, "ay":3.08806, "alpha":0.06656, "fx":[53.80435,54.2864,54.20408,53.72533], "fy":[50.68611,50.17385,50.2843,50.79129]}, - {"t":0.76455, "x":2.6177, "y":1.54507, "heading":0.94491, "vx":2.57017, "vy":2.40309, "omega":0.01239, "ax":0.29815, "ay":0.26122, "alpha":2.73059, "fx":[-5.76398,6.87286,15.27546,3.1122], "fy":[2.8179,-6.30561,5.82634,14.74326]}, - {"t":0.80277, "x":2.71617, "y":1.63712, "heading":0.94538, "vx":2.58157, "vy":2.41308, "omega":0.11677, "ax":-3.30212, "ay":-3.08838, "alpha":0.06006, "fx":[-54.16293,-53.73074,-53.80239,-54.23727], "fy":[-50.30918,-50.76667,-50.67122,-50.20944]}, - {"t":0.841, "x":2.81244, "y":1.72711, "heading":0.94984, "vx":2.45534, "vy":2.29502, "omega":0.11907, "ax":-3.33825, "ay":-3.12177, "alpha":0.02849, "fx":[-54.65826,-54.45149,-54.48942,-54.6968], "fy":[-50.94784,-51.16783,-51.12277,-50.90179]}, - {"t":0.87923, "x":2.90386, "y":1.81256, "heading":0.95439, "vx":2.32773, "vy":2.17568, "omega":0.12016, "ax":-3.35034, "ay":-3.13295, "alpha":0.0178, "fx":[-54.82396,-54.69444,-54.71936,-54.84912], "fy":[-51.16315,-51.30118,-51.27264,-51.13422]}, - {"t":0.91745, "x":2.9904, "y":1.89344, "heading":0.95899, "vx":2.19965, "vy":2.05592, "omega":0.12084, "ax":-3.3564, "ay":-3.13855, "alpha":0.01237, "fx":[-54.90677,-54.81666,-54.83463,-54.92485], "fy":[-51.27138,-51.36749,-51.3473,-51.251]}, - {"t":0.95568, "x":3.07203, "y":1.96974, "heading":0.96361, "vx":2.07135, "vy":1.93594, "omega":0.12131, "ax":-3.36004, "ay":-3.14191, "alpha":0.00907, "fx":[-54.95641,-54.89034,-54.90391,-54.97004], "fy":[-51.33654,-51.40706,-51.39197,-51.32135]}, - {"t":0.99391, "x":3.14876, "y":2.04145, "heading":0.96824, "vx":1.9429, "vy":1.81583, "omega":0.12166, "ax":-3.36246, "ay":-3.14415, "alpha":0.00685, "fx":[-54.98947,-54.93964,-54.95015,-55.00002], "fy":[-51.38009,-51.43329,-51.42168,-51.36842]}, - {"t":1.03214, "x":3.22057, "y":2.10857, "heading":0.97289, "vx":1.81437, "vy":1.69564, "omega":0.12192, "ax":-3.36419, "ay":-3.14575, "alpha":0.00524, "fx":[-55.01307,-54.97497,-54.98321,-55.02133], "fy":[-51.41124,-51.45192,-51.44286,-51.40214]}, - {"t":1.07036, "x":3.28747, "y":2.17109, "heading":0.97756, "vx":1.68576, "vy":1.57539, "omega":0.12212, "ax":-3.36549, "ay":-3.14695, "alpha":0.00402, "fx":[-55.03076,-55.00154,-55.00802,-55.03725], "fy":[-51.43462,-51.46582,-51.45874,-51.42751]}, - {"t":1.10859, "x":3.34946, "y":2.22902, "heading":0.98222, "vx":1.55711, "vy":1.45509, "omega":0.12227, "ax":-3.3665, "ay":-3.14789, "alpha":0.00306, "fx":[-55.04452,-55.02226,-55.02731,-55.04957], "fy":[-51.45282,-51.47659,-51.47108,-51.4473]}, - {"t":1.14682, "x":3.40652, "y":2.28234, "heading":0.9869, "vx":1.42842, "vy":1.33475, "omega":0.12239, "ax":-3.36731, "ay":-3.14863, "alpha":0.00229, "fx":[-55.05553,-55.03888,-55.04274,-55.05939], "fy":[-51.46737,-51.48515,-51.48095,-51.46316]}, - {"t":1.18505, "x":3.45866, "y":2.33106, "heading":0.99158, "vx":1.29969, "vy":1.21439, "omega":0.12248, "ax":-3.36797, "ay":-3.14924, "alpha":0.00166, "fx":[-55.06454,-55.05251,-55.05536,-55.0674], "fy":[-51.47928,-51.49213,-51.48903,-51.47618]}, - {"t":1.22327, "x":3.50589, "y":2.37518, "heading":0.99626, "vx":1.17094, "vy":1.094, "omega":0.12254, "ax":-3.36853, "ay":-3.14975, "alpha":0.00113, "fx":[-55.07206,-55.06389,-55.06587,-55.07404], "fy":[-51.48919,-51.49792,-51.49578,-51.48705]}, - {"t":1.2615, "x":3.54819, "y":2.4147, "heading":1.00094, "vx":1.04217, "vy":0.97359, "omega":0.12258, "ax":-3.36899, "ay":-3.15019, "alpha":0.00068, "fx":[-55.07844,-55.07355,-55.07476,-55.07965], "fy":[-51.49757,-51.5028,-51.50149,-51.49626]}, - {"t":1.29973, "x":3.58557, "y":2.44962, "heading":1.00563, "vx":0.91339, "vy":0.85317, "omega":0.12261, "ax":0.09524, "ay":-0.11403, "alpha":0.18612, "fx":[0.84597,1.72255,2.26779,1.39144], "fy":[-2.01624,-2.5709,-1.71253,-1.157]}, - {"t":1.33225, "x":3.61532, "y":2.47731, "heading":1.00962, "vx":0.91648, "vy":0.84946, "omega":0.12866, "ax":0.01311, "ay":-0.01415, "alpha":0.03201, "fx":[0.09221,0.2434,0.33644,0.18525], "fy":[-0.25785,-0.35288,-0.20488,-0.10984]}, - {"t":1.36477, "x":3.64513, "y":2.50492, "heading":1.0138, "vx":0.91691, "vy":0.849, "omega":0.1297, "ax":0.0017, "ay":-0.00184, "alpha":-0.12236, "fx":[0.49415,-0.08526,-0.4385,0.14091], "fy":[0.07313,0.43397,-0.13324,-0.49408]}, - {"t":1.39729, "x":3.67495, "y":2.53253, "heading":1.01802, "vx":0.91697, "vy":0.84894, "omega":0.12572, "ax":0.00022, "ay":-0.00024, "alpha":-0.27722, "fx":[1.05919,-0.25703,-1.05196,0.26426], "fy":[0.23434,1.04636,-0.24215,-1.05417]}, - {"t":1.42981, "x":3.70477, "y":2.56014, "heading":1.02211, "vx":0.91697, "vy":0.84894, "omega":0.11671, "ax":0.00003, "ay":-0.00003, "alpha":-0.43323, "fx":[1.64851,-0.41356,-1.64757,0.4145], "fy":[0.37855,1.6391,-0.37956,-1.64011]}, - {"t":1.46233, "x":3.73459, "y":2.58775, "heading":1.0259, "vx":0.91697, "vy":0.84893, "omega":0.10262, "ax":0.0, "ay":0.0, "alpha":-0.591, "fx":[2.2463,-0.57323,-2.24618,0.57335], "fy":[0.52556,2.23449,-0.52569,-2.23462]}, - {"t":1.49485, "x":3.76441, "y":2.61536, "heading":1.02924, "vx":0.91697, "vy":0.84893, "omega":0.0834, "ax":0.0, "ay":0.0, "alpha":-0.75117, "fx":[2.85277,-0.73814,-2.85277,0.73814], "fy":[0.6776,2.83771,-0.6776,-2.83771]}, - {"t":1.52737, "x":3.79423, "y":2.64296, "heading":1.03195, "vx":0.91697, "vy":0.84893, "omega":0.05897, "ax":-0.00007, "ay":-0.00006, "alpha":-0.91438, "fx":[3.46913,-0.90911,-3.47157,0.90665], "fy":[0.83322,3.4508,-0.83527,-3.45284]}, - {"t":1.55989, "x":3.82406, "y":2.67057, "heading":1.03387, "vx":0.91697, "vy":0.84893, "omega":0.02923, "ax":-1.16985, "ay":-1.08306, "alpha":-0.85354, "fx":[-15.96955,-20.39596,-22.18884,-17.94513], "fy":[-17.32205,-14.3404,-18.13458,-21.02687]}, - {"t":1.59242, "x":3.85326, "y":2.69761, "heading":1.03482, "vx":0.87893, "vy":0.81371, "omega":0.00148, "ax":-3.35541, "ay":-3.10643, "alpha":-0.01318, "fx":[-54.82079,-54.9143,-54.88826,-54.79485], "fy":[-50.81945,-50.71901,-50.74923,-50.84946]}, - {"t":1.62494, "x":3.88007, "y":2.72243, "heading":1.03487, "vx":0.76981, "vy":0.71269, "omega":0.00105, "ax":-3.37272, "ay":-3.12246, "alpha":-0.0075, "fx":[-55.11842,-55.17182,-55.15664,-55.10327], "fy":[-51.06657,-51.00905,-51.02605,-51.0835]}, - {"t":1.65746, "x":3.90332, "y":2.74395, "heading":1.0349, "vx":0.66012, "vy":0.61114, "omega":0.0008, "ax":-3.37852, "ay":-3.12783, "alpha":-0.0056, "fx":[-55.21816,-55.25809,-55.24666,-55.20674], "fy":[-51.14934,-51.10628,-51.11893,-51.16195]}, - {"t":1.68998, "x":3.923, "y":2.76217, "heading":1.03493, "vx":0.55025, "vy":0.50942, "omega":0.00062, "ax":-3.38143, "ay":-3.13053, "alpha":-0.00466, "fx":[-55.2681,-55.30129,-55.29176,-55.25858], "fy":[-51.19079,-51.15499,-51.16547,-51.20125]}, - {"t":1.7225, "x":3.93911, "y":2.77709, "heading":1.03495, "vx":0.44028, "vy":0.40761, "omega":0.00047, "ax":-3.38318, "ay":-3.13214, "alpha":-0.00409, "fx":[-55.29809,-55.32723,-55.31884,-55.28971], "fy":[-51.21567,-51.18423,-51.19342,-51.22484]}, - {"t":1.75502, "x":3.95164, "y":2.78869, "heading":1.03496, "vx":0.33026, "vy":0.30575, "omega":0.00034, "ax":-3.38434, "ay":-3.13322, "alpha":-0.00371, "fx":[-55.31809,-55.34453,-55.33691,-55.31048], "fy":[-51.23227,-51.20374,-51.21207,-51.24058]}, - {"t":1.78754, "x":3.96059, "y":2.79697, "heading":1.03497, "vx":0.2202, "vy":0.20386, "omega":0.00022, "ax":-3.38517, "ay":-3.13399, "alpha":-0.00343, "fx":[-55.33238,-55.35689,-55.34982,-55.32532], "fy":[-51.24412,-51.21767,-51.22539,-51.25183]}, - {"t":1.82006, "x":3.96596, "y":2.80194, "heading":1.03498, "vx":0.11011, "vy":0.10194, "omega":0.00011, "ax":-3.3858, "ay":-3.13457, "alpha":-0.00323, "fx":[-55.3431,-55.36616,-55.3595,-55.33645], "fy":[-51.25302,-51.22813,-51.23539,-51.26026]}, - {"t":1.85258, "x":3.96775, "y":2.8036, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37108, "ay":3.15201, "alpha":0.00992, "fx":[55.0812,55.154,55.14033,55.0676], "fy":[51.56096,51.48311,51.49791,51.57564]}, + {"t":0.03865, "x":1.63585, "y":0.62703, "heading":0.942, "vx":0.13029, "vy":0.12182, "omega":0.00038, "ax":3.3709, "ay":3.15184, "alpha":0.01014, "fx":[55.07751,55.15192,55.13796,55.06362], "fy":[51.55879,51.47921,51.49434,51.57379]}, + {"t":0.0773, "x":1.64341, "y":0.63409, "heading":0.94201, "vx":0.26057, "vy":0.24364, "omega":0.00078, "ax":3.37069, "ay":3.15164, "alpha":0.01039, "fx":[55.07341,55.14962,55.13532,55.05919], "fy":[51.55637,51.47488,51.49038,51.57174]}, + {"t":0.11595, "x":1.65599, "y":0.64586, "heading":0.94204, "vx":0.39084, "vy":0.36544, "omega":0.00118, "ax":3.37046, "ay":3.15142, "alpha":0.01066, "fx":[55.06883,55.14704,55.13237,55.05424], "fy":[51.55366,51.47003,51.48595,51.56944]}, + {"t":0.15459, "x":1.67362, "y":0.66234, "heading":0.94209, "vx":0.52111, "vy":0.48724, "omega":0.00159, "ax":3.3702, "ay":3.15118, "alpha":0.01097, "fx":[55.06367,55.14414,55.12905,55.04867], "fy":[51.55062,51.46458,51.48097,51.56686]}, + {"t":0.19324, "x":1.69627, "y":0.68353, "heading":0.94215, "vx":0.65136, "vy":0.60903, "omega":0.00201, "ax":3.36991, "ay":3.1509, "alpha":0.01132, "fx":[55.05783,55.14085,55.12528,55.04236], "fy":[51.54717,51.45839,51.47532,51.56393]}, + {"t":0.23189, "x":1.72397, "y":0.70942, "heading":0.94223, "vx":0.7816, "vy":0.73081, "omega":0.00245, "ax":3.36957, "ay":3.15058, "alpha":0.01172, "fx":[55.05115,55.1371,55.12098,55.03514], "fy":[51.54322,51.45132,51.46886,51.56059]}, + {"t":0.27054, "x":1.75669, "y":0.74002, "heading":0.94232, "vx":0.91183, "vy":0.85257, "omega":0.0029, "ax":3.36918, "ay":3.15021, "alpha":0.01218, "fx":[55.04344,55.13276,55.11601,55.0268], "fy":[51.53866,51.44316,51.46141,51.55673]}, + {"t":0.30919, "x":1.79445, "y":0.77532, "heading":0.94244, "vx":1.04205, "vy":0.97433, "omega":0.00337, "ax":3.36873, "ay":3.14978, "alpha":0.01272, "fx":[55.03444,55.1277,55.11021,55.01708], "fy":[51.53334,51.43364,51.45272,51.55222]}, + {"t":0.34784, "x":1.83724, "y":0.81533, "heading":0.94257, "vx":1.17224, "vy":1.09606, "omega":0.00387, "ax":3.3682, "ay":3.14928, "alpha":0.01336, "fx":[55.0238,55.12172,55.10336,55.00558], "fy":[51.52705,51.42237,51.44244,51.5469]}, + {"t":0.38649, "x":1.88506, "y":0.86004, "heading":0.94272, "vx":1.30242, "vy":1.21778, "omega":0.00438, "ax":3.36756, "ay":3.14867, "alpha":0.01412, "fx":[55.01103,55.11454,55.09513,54.99177], "fy":[51.5195,51.40885,51.4301,51.54051]}, + {"t":0.42514, "x":1.93791, "y":0.90946, "heading":0.94289, "vx":1.43257, "vy":1.33947, "omega":0.00493, "ax":3.36677, "ay":3.14792, "alpha":0.01506, "fx":[54.9954,55.10575,55.08507,54.97489], "fy":[51.51026,51.3923,51.41501,51.53269]}, + {"t":0.46378, "x":1.99579, "y":0.96358, "heading":0.94308, "vx":1.56269, "vy":1.46113, "omega":0.00551, "ax":3.36579, "ay":3.14699, "alpha":0.01623, "fx":[54.97585,55.09477,55.07248,54.95378], "fy":[51.49871,51.3716,51.39615,51.52292]}, + {"t":0.50243, "x":2.0587, "y":1.0224, "heading":0.94329, "vx":1.69278, "vy":1.58276, "omega":0.00614, "ax":3.36452, "ay":3.14579, "alpha":0.01773, "fx":[54.95069,55.08062,55.05629,54.9266], "fy":[51.48383,51.34496,51.37187,51.51035]}, + {"t":0.54108, "x":2.12664, "y":1.08592, "heading":0.94353, "vx":1.82281, "vy":1.70434, "omega":0.00682, "ax":3.36284, "ay":3.1442, "alpha":0.01975, "fx":[54.9171,55.06175,55.03469,54.89034], "fy":[51.46397,51.30939,51.33948,51.49357]}, + {"t":0.57973, "x":2.1996, "y":1.15414, "heading":0.94379, "vx":1.95278, "vy":1.82586, "omega":0.00759, "ax":3.36047, "ay":3.14195, "alpha":0.02258, "fx":[54.86997,55.03527,55.00441,54.83951], "fy":[51.43611,51.25951,51.29408,51.47005]}, + {"t":0.61838, "x":2.27758, "y":1.22705, "heading":0.94408, "vx":2.08266, "vy":1.94729, "omega":0.00846, "ax":3.35691, "ay":3.13858, "alpha":0.02684, "fx":[54.7991,54.99544,54.95892,54.76313], "fy":[51.39421,51.18449,51.22587,51.4347]}, + {"t":0.65703, "x":2.36058, "y":1.30466, "heading":0.94441, "vx":2.2124, "vy":2.06859, "omega":0.0095, "ax":3.35097, "ay":3.13294, "alpha":0.03397, "fx":[54.68047,54.92878,54.88291,54.63549], "fy":[51.32406,51.05897,51.11194,51.37561]}, + {"t":0.69568, "x":2.44859, "y":1.38695, "heading":0.94478, "vx":2.34191, "vy":2.18968, "omega":0.01081, "ax":3.339, "ay":3.1216, "alpha":0.04839, "fx":[54.44139,54.79431,54.73015,54.379], "fy":[51.18256,50.80615,50.88314,51.25669]}, + {"t":0.73433, "x":2.5416, "y":1.47391, "heading":0.94519, "vx":2.47096, "vy":2.31032, "omega":0.01268, "ax":3.30248, "ay":3.08699, "alpha":0.09267, "fx":[53.7096,54.38142,54.26571,53.6003], "fy":[50.74814,50.0338,50.18967,50.89378]}, + {"t":0.77297, "x":2.63956, "y":1.5655, "heading":0.94568, "vx":2.5986, "vy":2.42963, "omega":0.01626, "ax":-1.84533, "ay":-1.74106, "alpha":2.08087, "fx":[-37.25332,-26.85919,-22.24049,-34.31739], "fy":[-27.64157,-36.57058,-30.05287,-19.58702]}, + {"t":0.81162, "x":2.73862, "y":1.65811, "heading":0.94631, "vx":2.52728, "vy":2.36234, "omega":0.09668, "ax":-3.30636, "ay":-3.09241, "alpha":0.07717, "fx":[-54.28273,-53.72751,-53.82044,-54.3801], "fy":[-50.32392,-50.91163,-50.78981,-50.19499]}, + {"t":0.85027, "x":2.83382, "y":1.7471, "heading":0.95005, "vx":2.39949, "vy":2.24283, "omega":0.09967, "ax":-3.33975, "ay":-3.12321, "alpha":0.037, "fx":[-54.70797,-54.43941,-54.48877,-54.75835], "fy":[-50.94534,-51.23103,-51.17273,-50.88536]}, + {"t":0.88892, "x":2.92407, "y":1.83145, "heading":0.9539, "vx":2.27041, "vy":2.12212, "omega":0.1011, "ax":-3.35121, "ay":-3.13378, "alpha":0.02323, "fx":[-54.85402,-54.685,-54.71746,-54.88687], "fy":[-51.1599,-51.34001,-51.30297,-51.1222]}, + {"t":0.92757, "x":3.00931, "y":1.91112, "heading":0.95781, "vx":2.14089, "vy":2.001, "omega":0.10199, "ax":-3.35699, "ay":-3.13911, "alpha":0.01624, "fx":[-54.92782,-54.80954,-54.83298,-54.95146], "fy":[-51.26867,-51.39483,-51.36854,-51.24205]}, + {"t":0.96622, "x":3.08955, "y":1.98612, "heading":0.96175, "vx":2.01115, "vy":1.87968, "omega":0.10262, "ax":-3.36049, "ay":-3.14233, "alpha":0.01201, "fx":[-54.97234,-54.88488,-54.90268,-54.99025], "fy":[-51.33443,-51.42777,-51.408,-51.31448]}, + {"t":1.00487, "x":3.16477, "y":2.05642, "heading":0.96572, "vx":1.88127, "vy":1.75823, "omega":0.10309, "ax":-3.36282, "ay":-3.14449, "alpha":0.00916, "fx":[-55.00213,-54.9354,-54.94931,-55.0161], "fy":[-51.37847,-51.44971,-51.43436,-51.36303]}, + {"t":1.04352, "x":3.23496, "y":2.12202, "heading":0.9697, "vx":1.7513, "vy":1.6367, "omega":0.10344, "ax":-3.36449, "ay":-3.14603, "alpha":0.00712, "fx":[-55.02345,-54.97165,-54.98269,-55.03453], "fy":[-51.41003,-51.46535,-51.45323,-51.39784]}, + {"t":1.08216, "x":3.30014, "y":2.18293, "heading":0.9737, "vx":1.62127, "vy":1.51511, "omega":0.10371, "ax":-3.36575, "ay":-3.14719, "alpha":0.00557, "fx":[-55.03948,-54.99893,-55.00776,-55.04833], "fy":[-51.43374,-51.47705,-51.46739,-51.42404]}, + {"t":1.12081, "x":3.36028, "y":2.23913, "heading":0.97771, "vx":1.49119, "vy":1.39347, "omega":0.10393, "ax":-3.36673, "ay":-3.14809, "alpha":0.00437, "fx":[-55.05197,-55.02022,-55.02727,-55.05904], "fy":[-51.4522,-51.48613,-51.47843,-51.44448]}, + {"t":1.15946, "x":3.4154, "y":2.29064, "heading":0.98172, "vx":1.36107, "vy":1.27181, "omega":0.1041, "ax":-3.36751, "ay":-3.14881, "alpha":0.0034, "fx":[-55.06199,-55.03729,-55.04289,-55.06759], "fy":[-51.46699,-51.49336,-51.48727,-51.46088]}, + {"t":1.19811, "x":3.46549, "y":2.33744, "heading":0.98575, "vx":1.23092, "vy":1.15011, "omega":0.10423, "ax":-3.36816, "ay":-3.14941, "alpha":0.0026, "fx":[-55.0702,-55.0513,-55.05567,-55.07457], "fy":[-51.47908,-51.49927,-51.49453,-51.47433]}, + {"t":1.23676, "x":3.51055, "y":2.37954, "heading":0.98978, "vx":1.10074, "vy":1.02839, "omega":0.10433, "ax":-3.36869, "ay":-3.1499, "alpha":0.00194, "fx":[-55.07705,-55.063,-55.06631,-55.08036], "fy":[-51.48916,-51.50417,-51.50058,-51.48557]}, + {"t":1.27541, "x":3.55057, "y":2.41693, "heading":0.99381, "vx":0.97054, "vy":0.90665, "omega":0.10441, "ax":-3.36914, "ay":-3.15032, "alpha":0.00137, "fx":[-55.08287,-55.07292,-55.07531,-55.08525], "fy":[-51.49768,-51.5083,-51.50572,-51.49509]}, + {"t":1.31406, "x":3.58557, "y":2.44962, "heading":0.99784, "vx":0.84033, "vy":0.78489, "omega":0.10446, "ax":0.08606, "ay":-0.10179, "alpha":0.54028, "fx":[-0.66042,1.87226,3.47317,0.94235], "fy":[-2.09005,-3.71846,-1.24133,0.39362]}, + {"t":1.34808, "x":3.6142, "y":2.47626, "heading":1.0014, "vx":0.84326, "vy":0.78143, "omega":0.12284, "ax":0.00927, "ay":-0.01001, "alpha":0.34804, "fx":[-1.17844,0.45686,1.48156,-0.15365], "fy":[-0.44076,-1.48737,0.11333,1.16022]}, + {"t":1.38209, "x":3.64289, "y":2.50284, "heading":1.00558, "vx":0.84357, "vy":0.78109, "omega":0.13468, "ax":0.00095, "ay":-0.00102, "alpha":0.15621, "fx":[-0.58091,0.155,0.61191,-0.12399], "fy":[-0.14358,-0.61032,0.11009,0.57683]}, + {"t":1.41611, "x":3.67159, "y":2.52941, "heading":1.01016, "vx":0.84361, "vy":0.78105, "omega":0.13999, "ax":0.0001, "ay":-0.0001, "alpha":-0.03504, "fx":[0.13524,-0.03032,-0.13207,0.03349], "fy":[0.02735,0.1313,-0.03078,-0.13472]}, + {"t":1.45013, "x":3.70029, "y":2.55598, "heading":1.01492, "vx":0.84361, "vy":0.78105, "omega":0.1388, "ax":0.00001, "ay":-0.00001, "alpha":-0.22642, "fx":[0.8629,-0.21006,-0.86257,0.21039], "fy":[0.19174,0.85828,-0.19209,-0.85863]}, + {"t":1.48415, "x":3.72899, "y":2.58255, "heading":1.01964, "vx":0.84361, "vy":0.78105, "omega":0.1311, "ax":0.0, "ay":0.0, "alpha":-0.41865, "fx":[1.59351,-0.39618,-1.59348,0.39621], "fy":[0.36236,1.58541,-0.36239,-1.58545]}, + {"t":1.51817, "x":3.75769, "y":2.60912, "heading":1.0241, "vx":0.84361, "vy":0.78105, "omega":0.11685, "ax":0.0, "ay":0.0, "alpha":-0.61245, "fx":[2.32875,-0.58993,-2.32875,0.58993], "fy":[0.54051,2.31672,-0.54051,-2.31673]}, + {"t":1.55219, "x":3.78639, "y":2.63569, "heading":1.02808, "vx":0.84361, "vy":0.78105, "omega":0.09602, "ax":0.0, "ay":0.0, "alpha":-0.80853, "fx":[3.07144,-0.79096,-3.07145,0.79094], "fy":[0.72578,3.05532,-0.72577,-3.0553]}, + {"t":1.58621, "x":3.81509, "y":2.66226, "heading":1.03134, "vx":0.84361, "vy":0.78105, "omega":0.06851, "ax":-0.00005, "ay":-0.00004, "alpha":-1.00759, "fx":[3.82382,-0.99899,-3.82554,0.99725], "fy":[0.91628,3.80363,-0.91764,-3.80499]}, + {"t":1.62023, "x":3.84378, "y":2.68883, "heading":1.03367, "vx":0.84361, "vy":0.78105, "omega":0.03424, "ax":-1.14372, "ay":-1.05891, "alpha":-0.96571, "fx":[-15.11489,-20.12216,-22.16563,-17.38769], "fy":[-16.86101,-13.50744,-17.81626,-21.06001]}, + {"t":1.65424, "x":3.87182, "y":2.71479, "heading":1.03484, "vx":0.8047, "vy":0.74502, "omega":0.00139, "ax":-3.36083, "ay":-3.1116, "alpha":-0.01256, "fx":[-54.91109,-55.00033,-54.97531,-54.88617], "fy":[-50.90234,-50.8064,-50.83511,-50.93085]}, + {"t":1.68826, "x":3.89725, "y":2.73833, "heading":1.03489, "vx":0.69037, "vy":0.63917, "omega":0.00096, "ax":-3.37542, "ay":-3.1251, "alpha":-0.00721, "fx":[-55.1633,-55.21466,-55.20001,-55.14869], "fy":[-51.10897,-51.05363,-51.06994,-51.12522]}, + {"t":1.72228, "x":3.91878, "y":2.75827, "heading":1.03492, "vx":0.57554, "vy":0.53286, "omega":0.00071, "ax":-3.3803, "ay":-3.12962, "alpha":-0.00542, "fx":[-55.24772,-55.28638,-55.27529,-55.23665], "fy":[-51.17811,-51.13642,-51.14864,-51.1903]}, + {"t":1.7563, "x":3.93641, "y":2.77459, "heading":1.03494, "vx":0.46055, "vy":0.42639, "omega":0.00053, "ax":-3.38275, "ay":-3.13189, "alpha":-0.00453, "fx":[-55.28998,-55.32228,-55.31299,-55.28071], "fy":[-51.21272,-51.17787,-51.18806,-51.22288]}, + {"t":1.79032, "x":3.95012, "y":2.78728, "heading":1.03496, "vx":0.34547, "vy":0.31985, "omega":0.00037, "ax":-3.38422, "ay":-3.13325, "alpha":-0.00399, "fx":[-55.31536,-55.34383,-55.33563,-55.30716], "fy":[-51.23349,-51.20276,-51.21174,-51.24245]}, + {"t":1.82434, "x":3.95991, "y":2.79635, "heading":1.03497, "vx":0.23034, "vy":0.21326, "omega":0.00024, "ax":-3.3852, "ay":-3.13415, "alpha":-0.00363, "fx":[-55.33228,-55.3582,-55.35072,-55.3248], "fy":[-51.24735,-51.21936,-51.22752,-51.25549]}, + {"t":1.85836, "x":3.96579, "y":2.80179, "heading":1.03498, "vx":0.11518, "vy":0.10664, "omega":0.00011, "ax":-3.3859, "ay":-3.1348, "alpha":-0.00338, "fx":[-55.34436,-55.36847,-55.36151,-55.33741], "fy":[-51.25724,-51.23122,-51.2388,-51.26481]}, + {"t":1.89238, "x":3.96775, "y":2.8036, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/ROtoE.traj b/src/main/deploy/choreo/ROtoE.traj index b30213f4..bff576b2 100644 --- a/src/main/deploy/choreo/ROtoE.traj +++ b/src/main/deploy/choreo/ROtoE.traj @@ -10,7 +10,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.15}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.15}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,58 +30,58 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.98844,1.5904], + "waypoints":[0.0,1.00084,1.63262], "samples":[ - {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29685, "ay":1.68221, "alpha":-0.03642, "fx":[-70.14995,-70.28385,-70.34056,-70.20685], "fy":[27.74423,27.40376,27.25669,27.59874]}, - {"t":0.03408, "x":7.0441, "y":1.88284, "heading":2.35619, "vx":-0.14645, "vy":0.05734, "omega":-0.00124, "ax":-4.29649, "ay":1.68224, "alpha":-0.03731, "fx":[-70.14166,-70.27889,-70.33696,-70.19994], "fy":[27.75069,27.40188,27.25118,27.60166]}, - {"t":0.06817, "x":7.03661, "y":1.88578, "heading":2.35615, "vx":-0.2929, "vy":0.11467, "omega":-0.00251, "ax":-4.29608, "ay":1.68227, "alpha":-0.03832, "fx":[-70.13234,-70.2733,-70.33291,-70.19217], "fy":[27.75796,27.39978,27.24498,27.60492]}, - {"t":0.10225, "x":7.02413, "y":1.89066, "heading":2.35607, "vx":-0.43933, "vy":0.17201, "omega":-0.00382, "ax":-4.29561, "ay":1.68231, "alpha":-0.03947, "fx":[-70.12178,-70.26696,-70.32833,-70.18337], "fy":[27.76621,27.3974,27.23794,27.60861]}, - {"t":0.13634, "x":7.00666, "y":1.8975, "heading":2.35594, "vx":-0.58574, "vy":0.22935, "omega":-0.00516, "ax":-4.29508, "ay":1.68235, "alpha":-0.04078, "fx":[-70.1097,-70.25972,-70.32309,-70.17331], "fy":[27.77563,27.3947,27.2299,27.61282]}, - {"t":0.17042, "x":6.9842, "y":1.9063, "heading":2.35576, "vx":-0.73213, "vy":0.2867, "omega":-0.00655, "ax":-4.29447, "ay":1.6824, "alpha":-0.04229, "fx":[-70.09576,-70.25136,-70.31704,-70.16171], "fy":[27.78651,27.39159,27.22061,27.61767]}, - {"t":0.20451, "x":6.95676, "y":1.91704, "heading":2.35554, "vx":-0.87851, "vy":0.34404, "omega":-0.008, "ax":-4.29376, "ay":1.68246, "alpha":-0.04405, "fx":[-70.0795,-70.2416,-70.30999,-70.14817], "fy":[27.7992,27.38798,27.20977,27.62331]}, - {"t":0.23859, "x":6.92432, "y":1.92975, "heading":2.35526, "vx":-1.02486, "vy":0.40138, "omega":-0.0095, "ax":-4.29292, "ay":1.68253, "alpha":-0.04613, "fx":[-70.06028,-70.23008,-70.30166,-70.13217], "fy":[27.8142,27.38372,27.19695,27.62997]}, - {"t":0.27267, "x":6.88689, "y":1.94441, "heading":2.35494, "vx":-1.17118, "vy":0.45873, "omega":-0.01107, "ax":-4.29191, "ay":1.68262, "alpha":-0.04863, "fx":[-70.03721,-70.21625,-70.29166,-70.11296], "fy":[27.83219,27.37862,27.18157,27.63796]}, - {"t":0.30676, "x":6.84448, "y":1.96102, "heading":2.35456, "vx":-1.31746, "vy":0.51608, "omega":-0.01273, "ax":-4.29067, "ay":1.68272, "alpha":-0.05169, "fx":[-70.00901,-70.19936,-70.27944,-70.08948], "fy":[27.85416,27.37239,27.16276,27.64772]}, - {"t":0.34084, "x":6.79708, "y":1.97959, "heading":2.35413, "vx":-1.46371, "vy":0.57344, "omega":-0.01449, "ax":-4.28913, "ay":1.68285, "alpha":-0.05551, "fx":[-69.97376,-70.17826,-70.26415,-70.06011], "fy":[27.88162,27.36462,27.13924,27.65991]}, - {"t":0.37493, "x":6.7447, "y":2.00011, "heading":2.35364, "vx":-1.6099, "vy":0.6308, "omega":-0.01638, "ax":-4.28714, "ay":1.68301, "alpha":-0.06042, "fx":[-69.92844,-70.15116,-70.2445,-70.02231], "fy":[27.91688,27.35465,27.10899,27.67557]}, - {"t":0.40901, "x":6.68734, "y":2.02259, "heading":2.35308, "vx":-1.75602, "vy":0.68816, "omega":-0.01844, "ax":-4.28449, "ay":1.68323, "alpha":-0.06698, "fx":[-69.86801,-70.11507,-70.21828,-69.97187], "fy":[27.96384,27.34137,27.06865,27.69645]}, - {"t":0.4431, "x":6.625, "y":2.04702, "heading":2.35245, "vx":-1.90206, "vy":0.74553, "omega":-0.02072, "ax":-4.28079, "ay":1.68353, "alpha":-0.07616, "fx":[-69.78341,-70.06464,-70.18155,-69.90115], "fy":[28.02945,27.32281,27.01217,27.72568]}, - {"t":0.47718, "x":6.55768, "y":2.07341, "heading":2.35174, "vx":-2.04797, "vy":0.80291, "omega":-0.02332, "ax":-4.27522, "ay":1.68398, "alpha":-0.08995, "fx":[-69.65652,-69.98921,-70.12639,-69.79486], "fy":[28.12757,27.29505,26.92743,27.76952]}, - {"t":0.51126, "x":6.4854, "y":2.10175, "heading":2.35095, "vx":-2.19368, "vy":0.86031, "omega":-0.02638, "ax":-4.26595, "ay":1.68472, "alpha":-0.11294, "fx":[-69.4451,-69.86403,-70.0343,-69.61714], "fy":[28.29029,27.24895,26.78619,27.84255]}, - {"t":0.54535, "x":6.40815, "y":2.13206, "heading":2.35005, "vx":-2.33908, "vy":0.91773, "omega":-0.03023, "ax":-4.2474, "ay":1.68616, "alpha":-0.15898, "fx":[-69.02254,-69.61574,-69.84949,-69.25958], "fy":[28.61252,27.15746,26.50378,27.98841]}, - {"t":0.57943, "x":6.32595, "y":2.16432, "heading":2.34902, "vx":-2.48385, "vy":0.9752, "omega":-0.03565, "ax":-4.1917, "ay":1.69016, "alpha":-0.29744, "fx":[-67.7581,-68.88695,-69.28989,-68.17009], "fy":[29.55318,26.8887,25.65784,28.4239]}, - {"t":0.61352, "x":6.23886, "y":2.19854, "heading":2.3478, "vx":-2.62672, "vy":1.03281, "omega":-0.04579, "ax":0.48096, "ay":0.5564, "alpha":-9.13365, "fx":[9.21905,-29.62251,10.04063,41.81374], "fy":[42.62723,12.16863,-28.58004,10.16854]}, - {"t":0.6476, "x":6.14961, "y":2.23406, "heading":2.34624, "vx":-2.61033, "vy":1.05178, "omega":-0.3571, "ax":4.22778, "ay":-1.60063, "alpha":-0.25597, "fx":[69.74402,68.83973,68.48275,69.39804], "fy":[-24.44306,-26.81238,-27.85534,-25.55844]}, - {"t":0.68168, "x":6.06309, "y":2.26898, "heading":2.33407, "vx":-2.46623, "vy":0.99722, "omega":-0.36583, "ax":4.26547, "ay":-1.6408, "alpha":-0.11893, "fx":[70.03598,69.61101,69.42723,69.85489], "fy":[-26.01974,-27.11939,-27.62037,-26.53659]}, - {"t":0.71577, "x":5.98151, "y":2.30202, "heading":2.3216, "vx":-2.32085, "vy":0.94129, "omega":-0.36988, "ax":4.27801, "ay":-1.65434, "alpha":-0.073, "fx":[70.1268,69.86591,69.74724,70.00926], "fy":[-26.54946,-27.2216,-27.53813,-26.87191]}, - {"t":0.74985, "x":5.90489, "y":2.33314, "heading":2.30899, "vx":-2.17503, "vy":0.88491, "omega":-0.37237, "ax":4.28428, "ay":-1.66113, "alpha":-0.04995, "fx":[70.17077,69.99276,69.90842,70.08702], "fy":[-26.81558,-27.27316,-27.49575,-27.0409]}, - {"t":0.78394, "x":5.83325, "y":2.36234, "heading":2.2963, "vx":-2.02901, "vy":0.82829, "omega":-0.37407, "ax":4.28803, "ay":-1.66522, "alpha":-0.03608, "fx":[70.19658,70.06854,70.00561,70.13398], "fy":[-26.97594,-27.3046,-27.46964,-27.14238]}, - {"t":0.81802, "x":5.76658, "y":2.3896, "heading":2.28355, "vx":-1.88285, "vy":0.77153, "omega":-0.3753, "ax":4.29054, "ay":-1.66795, "alpha":-0.0268, "fx":[70.21347,70.11882,70.07067,70.16552], "fy":[-27.0833,-27.32603,-27.45179,-27.20981]}, - {"t":0.85211, "x":5.7049, "y":2.41493, "heading":2.27076, "vx":-1.73661, "vy":0.71468, "omega":-0.37622, "ax":4.29233, "ay":-1.6699, "alpha":-0.02016, "fx":[70.22534,70.15454,70.11731,70.18824], "fy":[-27.16031,-27.34176,-27.43871,-27.25768]}, - {"t":0.88619, "x":5.6482, "y":2.43832, "heading":2.25794, "vx":-1.59031, "vy":0.65776, "omega":-0.3769, "ax":4.29367, "ay":-1.67136, "alpha":-0.01517, "fx":[70.23411,70.18116,70.15241,70.20544], "fy":[-27.21832,-27.35396,-27.42866,-27.29325]}, - {"t":0.92027, "x":5.59649, "y":2.45977, "heading":2.24509, "vx":-1.44397, "vy":0.6008, "omega":-0.37742, "ax":4.29471, "ay":-1.6725, "alpha":-0.01128, "fx":[70.24083,70.20171,70.1798,70.21897], "fy":[-27.26364,-27.3638,-27.42064,-27.3206]}, - {"t":0.95436, "x":5.54977, "y":2.47927, "heading":2.23223, "vx":-1.29758, "vy":0.54379, "omega":-0.3778, "ax":4.29554, "ay":-1.67341, "alpha":-0.00816, "fx":[70.24613,70.21801,70.20178,70.22992], "fy":[-27.30005,-27.37202,-27.41407,-27.34217]}, - {"t":0.98844, "x":5.50803, "y":2.49684, "heading":2.21935, "vx":-1.15117, "vy":0.48675, "omega":-0.37808, "ax":1.69706, "ay":3.5182, "alpha":-0.42607, "fx":[27.20457,25.18554,28.35205,30.23272], "fy":[58.29849,58.56483,56.73115,56.46929]}, - {"t":1.01854, "x":5.47416, "y":2.51308, "heading":2.20797, "vx":-1.1001, "vy":0.59264, "omega":-0.39091, "ax":0.99104, "ay":1.73812, "alpha":-1.28091, "fx":[16.3411,10.58259,16.20995,21.67312], "fy":[32.66817,29.93993,23.95521,27.09652]}, - {"t":1.04864, "x":5.44149, "y":2.5317, "heading":2.1962, "vx":-1.07027, "vy":0.64496, "omega":-0.42946, "ax":0.20087, "ay":0.32987, "alpha":-1.17008, "fx":[4.03949,-1.25698,2.53927,7.81385], "fy":[9.85294,6.12057,0.88796,4.70939]}, - {"t":1.07874, "x":5.40937, "y":2.55127, "heading":2.18328, "vx":-1.06422, "vy":0.65489, "omega":-0.46468, "ax":0.03202, "ay":0.05195, "alpha":-0.49616, "fx":[0.87684,-1.38813,0.17038,2.43481], "fy":[2.75261,1.1631,-1.05534,0.53673]}, - {"t":1.10883, "x":5.37736, "y":2.571, "heading":2.16929, "vx":-1.06326, "vy":0.65645, "omega":-0.47961, "ax":0.005, "ay":0.0081, "alpha":0.19584, "fx":[-0.06822,0.83439,0.23183,-0.6708], "fy":[-0.61713,-0.00157,0.88201,0.26652]}, - {"t":1.13893, "x":5.34536, "y":2.59076, "heading":2.15486, "vx":-1.06311, "vy":0.65669, "omega":-0.47371, "ax":0.00078, "ay":0.00126, "alpha":0.88726, "fx":[-0.71591,3.41331,0.7414,-3.38785], "fy":[-3.36528,-0.63579,3.40642,0.67713]}, - {"t":1.16903, "x":5.31336, "y":2.61053, "heading":2.1406, "vx":-1.06308, "vy":0.65673, "omega":-0.44701, "ax":0.00012, "ay":0.0002, "alpha":1.57533, "fx":[-1.37731,6.02246,1.38129,-6.01849], "fy":[-5.9893,-1.24827,5.99568,1.25475]}, - {"t":1.19913, "x":5.28136, "y":2.63029, "heading":2.12715, "vx":-1.06308, "vy":0.65674, "omega":-0.3996, "ax":0.00002, "ay":0.00003, "alpha":2.25748, "fx":[-2.09161,8.6029,2.09224,-8.60227], "fy":[-8.55952,-1.90883,8.56052,1.90986]}, - {"t":1.22922, "x":5.24937, "y":2.65006, "heading":2.11512, "vx":-1.06308, "vy":0.65674, "omega":-0.33165, "ax":0.0, "ay":0.00001, "alpha":2.93129, "fx":[-2.84973,11.1397,2.84987,-11.13957], "fy":[-11.08141,-2.6133,11.08162,2.61352]}, - {"t":1.25932, "x":5.21737, "y":2.66983, "heading":2.10514, "vx":-1.06308, "vy":0.65674, "omega":-0.24343, "ax":0.00001, "ay":0.00001, "alpha":3.59446, "fx":[-3.62976,13.62744,3.63025,-13.62697], "fy":[-13.55294,-3.34074,13.55315,3.34097]}, - {"t":1.28942, "x":5.18538, "y":2.68959, "heading":2.09781, "vx":-1.06308, "vy":0.65674, "omega":-0.13524, "ax":0.09125, "ay":-0.05639, "alpha":4.24059, "fx":[-2.83029,17.45034,5.98188,-14.63487], "fy":[-16.8811,-4.97731,15.04351,3.12734]}, - {"t":1.31952, "x":5.15342, "y":2.70933, "heading":2.09374, "vx":-1.06033, "vy":0.65504, "omega":-0.00761, "ax":3.87672, "ay":-2.39492, "alpha":0.07248, "fx":[63.09996,63.46942,63.6538,63.28479], "fy":[-39.60327,-39.0181,-38.69873,-39.28939]}, - {"t":1.34962, "x":5.12326, "y":2.72796, "heading":2.09351, "vx":-0.94365, "vy":0.58296, "omega":-0.00543, "ax":3.9051, "ay":-2.41245, "alpha":0.03901, "fx":[63.6903,63.88924,63.99157,63.79278], "fy":[-39.68345,-39.36484,-39.19373,-39.51397]}, - {"t":1.37971, "x":5.09663, "y":2.74442, "heading":2.09335, "vx":-0.82612, "vy":0.51035, "omega":-0.00425, "ax":3.91387, "ay":-2.41787, "alpha":0.02871, "fx":[63.87309,64.0195,64.09552,63.94919], "fy":[-39.70785,-39.47257,-39.34682,-39.58298]}, - {"t":1.40981, "x":5.07354, "y":2.75868, "heading":2.09322, "vx":-0.70832, "vy":0.43758, "omega":-0.00339, "ax":3.91813, "ay":-2.4205, "alpha":0.0237, "fx":[63.96201,64.0829,64.14597,64.02513], "fy":[-39.71965,-39.52504,-39.42125,-39.61647]}, - {"t":1.43991, "x":5.05399, "y":2.77075, "heading":2.09312, "vx":-0.59039, "vy":0.36473, "omega":-0.00268, "ax":3.92065, "ay":-2.42206, "alpha":0.02075, "fx":[64.01458,64.1204,64.17577,64.06998], "fy":[-39.7266,-39.55608,-39.46525,-39.63624]}, - {"t":1.47001, "x":5.038, "y":2.78064, "heading":2.09304, "vx":-0.47239, "vy":0.29183, "omega":-0.00205, "ax":3.92231, "ay":-2.42309, "alpha":0.0188, "fx":[64.0493,64.14518,64.19543,64.09959], "fy":[-39.73118,-39.57659,-39.49432,-39.64929]}, - {"t":1.5001, "x":5.02556, "y":2.78832, "heading":2.09298, "vx":-0.35434, "vy":0.2189, "omega":-0.00149, "ax":3.9235, "ay":-2.42382, "alpha":0.01742, "fx":[64.07395,64.16277,64.20939,64.1206], "fy":[-39.73443,-39.59115,-39.51494,-39.65855]}, - {"t":1.5302, "x":5.01667, "y":2.79381, "heading":2.09293, "vx":-0.23625, "vy":0.14595, "omega":-0.00096, "ax":3.92438, "ay":-2.42436, "alpha":0.01638, "fx":[64.09235,64.1759,64.2198,64.13628], "fy":[-39.73685,-39.60202,-39.53034,-39.66546]}, - {"t":1.5603, "x":5.01134, "y":2.79711, "heading":2.0929, "vx":-0.11814, "vy":0.07298, "omega":-0.00047, "ax":3.92506, "ay":-2.42478, "alpha":0.01558, "fx":[64.10661,64.18608,64.22787,64.14842], "fy":[-39.73872,-39.61045,-39.54227,-39.67081]}, - {"t":1.5904, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29582, "ay":1.68514, "alpha":-0.05154, "fx":[-70.09324,-70.28317,-70.36339,-70.17384], "fy":[27.89278,27.41141,27.20319,27.68775]}, + {"t":0.03451, "x":7.04404, "y":1.88287, "heading":2.35619, "vx":-0.14826, "vy":0.05816, "omega":-0.00178, "ax":-4.29544, "ay":1.68522, "alpha":-0.05295, "fx":[-70.08332,-70.27848,-70.36085,-70.16609], "fy":[27.90355,27.4091,27.19517,27.69298]}, + {"t":0.06902, "x":7.03636, "y":1.88588, "heading":2.35613, "vx":-0.2965, "vy":0.11632, "omega":-0.00361, "ax":-4.29501, "ay":1.68532, "alpha":-0.05453, "fx":[-70.07215,-70.27319,-70.35799,-70.15738], "fy":[27.91568,27.40653,27.18613,27.69886]}, + {"t":0.10354, "x":7.02357, "y":1.8909, "heading":2.35601, "vx":-0.44473, "vy":0.17448, "omega":-0.00549, "ax":-4.29452, "ay":1.68543, "alpha":-0.05633, "fx":[-70.05948,-70.26719,-70.35475,-70.1475], "fy":[27.92944,27.40363,27.17587,27.7055]}, + {"t":0.13805, "x":7.00567, "y":1.89792, "heading":2.35582, "vx":-0.59294, "vy":0.23265, "omega":-0.00743, "ax":-4.29396, "ay":1.68556, "alpha":-0.05839, "fx":[-70.04498,-70.26032,-70.35105,-70.1362], "fy":[27.94519,27.40033,27.16413,27.71307]}, + {"t":0.17256, "x":6.98264, "y":1.90696, "heading":2.35556, "vx":-0.74113, "vy":0.29082, "omega":-0.00945, "ax":-4.29332, "ay":1.68571, "alpha":-0.06077, "fx":[-70.02822,-70.25237,-70.34677,-70.12314], "fy":[27.96338,27.39655,27.15055,27.7218]}, + {"t":0.20707, "x":6.95451, "y":1.918, "heading":2.35524, "vx":-0.8893, "vy":0.349, "omega":-0.01154, "ax":-4.29257, "ay":1.68588, "alpha":-0.06355, "fx":[-70.00864,-70.24309,-70.34177,-70.1079], "fy":[27.98462,27.39215,27.13468,27.73198]}, + {"t":0.24158, "x":6.92126, "y":1.93105, "heading":2.35484, "vx":-1.03745, "vy":0.40718, "omega":-0.01374, "ax":-4.29168, "ay":1.68608, "alpha":-0.06684, "fx":[-69.98546,-70.23211,-70.33586,-70.08986], "fy":[28.00976,27.38698,27.11589,27.74401]}, + {"t":0.2761, "x":6.8829, "y":1.9461, "heading":2.35436, "vx":-1.18556, "vy":0.46537, "omega":-0.01604, "ax":-4.29061, "ay":1.68632, "alpha":-0.07079, "fx":[-69.95758,-70.2189,-70.32875,-70.06816], "fy":[28.03997,27.38078,27.09329,27.75846]}, + {"t":0.31061, "x":6.83943, "y":1.96317, "heading":2.35381, "vx":-1.33364, "vy":0.52357, "omega":-0.01849, "ax":-4.2893, "ay":1.68662, "alpha":-0.07563, "fx":[-69.92343,-70.20274,-70.32004,-70.04156], "fy":[28.07693,27.37321,27.06559,27.77614]}, + {"t":0.34512, "x":6.79085, "y":1.98224, "heading":2.35317, "vx":-1.48167, "vy":0.58178, "omega":-0.0211, "ax":-4.28766, "ay":1.68699, "alpha":-0.0817, "fx":[-69.88063,-70.18251,-70.30912,-70.0082], "fy":[28.12322,27.36376,27.03086,27.79828]}, + {"t":0.37963, "x":6.73716, "y":2.00332, "heading":2.35244, "vx":-1.62965, "vy":0.64, "omega":-0.02392, "ax":-4.28554, "ay":1.68746, "alpha":-0.08954, "fx":[-69.82539,-70.15644,-70.29501,-69.96512], "fy":[28.18286,27.35161,26.98605,27.82683]}, + {"t":0.41414, "x":6.67837, "y":2.02642, "heading":2.35162, "vx":-1.77755, "vy":0.69823, "omega":-0.02701, "ax":-4.28271, "ay":1.6881, "alpha":-0.10003, "fx":[-69.75139,-70.1216,-70.27609,-69.90733], "fy":[28.26257,27.33539,26.92601,27.86508]}, + {"t":0.44865, "x":6.61447, "y":2.05152, "heading":2.35069, "vx":-1.92535, "vy":0.75649, "omega":-0.03046, "ax":-4.27871, "ay":1.689, "alpha":-0.11481, "fx":[-69.64713,-70.07266,-70.24937,-69.82573], "fy":[28.37453,27.31262,26.84142,27.91896]}, + {"t":0.48317, "x":6.54547, "y":2.07863, "heading":2.34964, "vx":-2.07302, "vy":0.81478, "omega":-0.03442, "ax":-4.27265, "ay":1.69034, "alpha":-0.13719, "fx":[-69.48929,-69.99892,-70.20879,-69.70182], "fy":[28.54326,27.27833,26.71337,28.00054]}, + {"t":0.51768, "x":6.47139, "y":2.10776, "heading":2.34845, "vx":-2.22048, "vy":0.87312, "omega":-0.03916, "ax":-4.2624, "ay":1.69259, "alpha":-0.17502, "fx":[-69.22231,-69.87515,-70.13978,-69.49116], "fy":[28.8265,27.22078,26.49683,28.13861]}, + {"t":0.55219, "x":6.39221, "y":2.1389, "heading":2.3471, "vx":-2.36758, "vy":0.93154, "omega":-0.0452, "ax":-4.24128, "ay":1.69713, "alpha":-0.25278, "fx":[-68.67341,-69.62423,-69.99629,-69.05358], "fy":[29.40049,27.10419,26.05179,28.42278]}, + {"t":0.5867, "x":6.30798, "y":2.17206, "heading":2.34554, "vx":-2.51396, "vy":0.99011, "omega":-0.05392, "ax":-4.17259, "ay":1.71093, "alpha":-0.504, "fx":[-66.89976,-68.84361,-69.51674,-67.59543], "fy":[31.18114,26.74191,24.61414,29.34469]}, + {"t":0.62121, "x":6.21873, "y":2.20725, "heading":2.34368, "vx":-2.65796, "vy":1.04915, "omega":-0.07132, "ax":3.49335, "ay":-0.85471, "alpha":-3.91224, "fx":[60.79332,50.30297,52.76413,64.57815], "fy":[11.24046,-20.85936,-35.41451,-10.85812]}, + {"t":0.65573, "x":6.12908, "y":2.24295, "heading":2.34121, "vx":-2.5374, "vy":1.01966, "omega":-0.20633, "ax":4.24078, "ay":-1.59601, "alpha":-0.36007, "fx":[70.21131,68.95447,68.434,69.7149], "fy":[-23.64346,-26.99822,-28.46873,-25.25644]}, + {"t":0.69024, "x":6.04404, "y":2.27719, "heading":2.33409, "vx":-2.39104, "vy":0.96458, "omega":-0.21876, "ax":4.27044, "ay":-1.63749, "alpha":-0.17626, "fx":[70.26263,69.63598,69.36141,69.99409], "fy":[-25.57339,-27.21022,-27.94899,-26.34702]}, + {"t":0.72475, "x":5.96406, "y":2.3095, "heading":2.32654, "vx":-2.24366, "vy":0.90806, "omega":-0.22484, "ax":4.2808, "ay":-1.65232, "alpha":-0.11081, "fx":[70.2696,69.87393,69.69492,70.09312], "fy":[-26.25912,-27.28458,-27.75847,-26.74677]}, + {"t":0.75926, "x":5.88918, "y":2.33986, "heading":2.31878, "vx":-2.09592, "vy":0.85104, "omega":-0.22867, "ax":4.28607, "ay":-1.65993, "alpha":-0.07724, "fx":[70.27078,69.99486,69.86674,70.14396], "fy":[-26.61072,-27.32292,-27.65935,-26.95379]}, + {"t":0.79377, "x":5.8194, "y":2.36824, "heading":2.31089, "vx":-1.948, "vy":0.79375, "omega":-0.23133, "ax":4.28926, "ay":-1.66456, "alpha":-0.05681, "fx":[70.27063,70.06794,69.97157,70.17501], "fy":[-26.82472,-27.3466,-27.59842,-27.08009]}, + {"t":0.82829, "x":5.75472, "y":2.39464, "heading":2.30291, "vx":-1.79997, "vy":0.7363, "omega":-0.23329, "ax":4.2914, "ay":-1.66768, "alpha":-0.04306, "fx":[70.27012,70.11679,70.04225,70.19603], "fy":[-26.96878,-27.36291,-27.55707,-27.16497]}, + {"t":0.8628, "x":5.69516, "y":2.41906, "heading":2.29486, "vx":-1.65187, "vy":0.67875, "omega":-0.23478, "ax":4.29294, "ay":-1.66992, "alpha":-0.03318, "fx":[70.26952,70.15168,70.09314,70.21127], "fy":[-27.07245,-27.37497,-27.5271,-27.22576]}, + {"t":0.89731, "x":5.6407, "y":2.44149, "heading":2.28675, "vx":-1.50371, "vy":0.62112, "omega":-0.23593, "ax":4.2941, "ay":-1.67161, "alpha":-0.02574, "fx":[70.26892,70.1778,70.13157,70.22287], "fy":[-27.15068,-27.3844,-27.50432,-27.27131]}, + {"t":0.93182, "x":5.59137, "y":2.46193, "heading":2.27861, "vx":-1.35551, "vy":0.56343, "omega":-0.23681, "ax":4.295, "ay":-1.67293, "alpha":-0.01992, "fx":[70.26834,70.19803,70.16162,70.23204], "fy":[-27.21185,-27.39206,-27.48639,-27.3066]}, + {"t":0.96633, "x":5.54714, "y":2.48038, "heading":2.27044, "vx":-1.20728, "vy":0.50569, "omega":-0.2375, "ax":4.29572, "ay":-1.67399, "alpha":-0.01526, "fx":[70.2678,70.21414,70.18578,70.23951], "fy":[-27.26102,-27.39848,-27.47188,-27.33466]}, + {"t":1.00084, "x":5.50803, "y":2.49684, "heading":2.26224, "vx":-1.05903, "vy":0.44792, "omega":-0.23803, "ax":1.68569, "ay":3.4484, "alpha":-0.99455, "fx":[26.19758,21.62206,29.31377,33.09796], "fy":[58.3778,58.70455,54.32137,54.09533]}, + {"t":1.03243, "x":5.47542, "y":2.51271, "heading":2.25472, "vx":-1.00578, "vy":0.55685, "omega":-0.26944, "ax":0.72588, "ay":1.25179, "alpha":-3.54213, "fx":[12.64877,-2.99851,11.82359,25.99334], "fy":[32.83909,23.49966,6.76474,18.7539]}, + {"t":1.06402, "x":5.44401, "y":2.53092, "heading":2.24621, "vx":-0.98285, "vy":0.59639, "omega":-0.38134, "ax":0.10653, "ay":0.17446, "alpha":-3.33855, "fx":[3.32058,-11.27119,0.22741,14.68949], "fy":[15.68283,4.2605,-10.16662,1.63159]}, + {"t":1.09561, "x":5.41302, "y":2.54985, "heading":2.23417, "vx":-0.97948, "vy":0.6019, "omega":-0.4868, "ax":0.01356, "ay":0.02205, "alpha":-2.38042, "fx":[1.4525,-9.01463,-1.00522,9.45416], "fy":[9.56442,1.39974,-8.85563,-0.66653]}, + {"t":1.1272, "x":5.38209, "y":2.56887, "heading":2.21879, "vx":-0.97906, "vy":0.6026, "omega":-0.56199, "ax":0.00167, "ay":0.00272, "alpha":-1.38122, "fx":[0.82268,-5.32096,-0.76784,5.3755], "fy":[5.37668,0.72632,-5.28834,-0.63696]}, + {"t":1.15879, "x":5.35116, "y":2.58791, "heading":2.20104, "vx":-0.979, "vy":0.60268, "omega":-0.60562, "ax":0.0002, "ay":0.00033, "alpha":-0.36453, "fx":[0.23815,-1.40478,-0.23153,1.4114], "fy":[1.40879,0.21031,-1.39804,-0.19955]}, + {"t":1.19038, "x":5.32023, "y":2.60695, "heading":2.18191, "vx":-0.979, "vy":0.60269, "omega":-0.61714, "ax":0.00002, "ay":0.00004, "alpha":0.65683, "fx":[-0.47104,2.53002,0.47183,-2.52923], "fy":[-2.51952,-0.41707,2.52081,0.41836]}, + {"t":1.22197, "x":5.28931, "y":2.62598, "heading":2.16241, "vx":-0.979, "vy":0.6027, "omega":-0.59639, "ax":0.0, "ay":0.0, "alpha":1.66979, "fx":[-1.3231,6.40897,1.32319,-6.40887], "fy":[-6.38214,-1.18699,6.38229,1.18715]}, + {"t":1.25355, "x":5.25838, "y":2.64502, "heading":2.14357, "vx":-0.979, "vy":0.6027, "omega":-0.54364, "ax":0.0, "ay":0.0, "alpha":2.66203, "fx":[-2.30068,10.1798,2.3007,-10.17979], "fy":[-10.13313,-2.08458,10.13315,2.08461]}, + {"t":1.28514, "x":5.22746, "y":2.66406, "heading":2.1264, "vx":-0.979, "vy":0.6027, "omega":-0.45955, "ax":0.0, "ay":0.0, "alpha":3.62288, "fx":[-3.36746,13.80342,3.36748,-13.8034], "fy":[-13.73486,-3.07449,13.7349,3.07453]}, + {"t":1.31673, "x":5.19653, "y":2.6831, "heading":2.11188, "vx":-0.979, "vy":0.6027, "omega":-0.34511, "ax":0.00001, "ay":0.00001, "alpha":4.54403, "fx":[-4.47323,17.25535,4.4734,-17.25518], "fy":[-17.16386,-4.10701,17.16411,4.1073]}, + {"t":1.34832, "x":5.16561, "y":2.70214, "heading":2.10098, "vx":-0.979, "vy":0.6027, "omega":-0.20157, "ax":0.00106, "ay":-0.00057, "alpha":5.4197, "fx":[-5.53933,20.54166,5.57709,-20.51006], "fy":[-20.42158,-5.13198,20.40289,5.11341]}, + {"t":1.37991, "x":5.13468, "y":2.72118, "heading":2.09461, "vx":-0.97896, "vy":0.60268, "omega":-0.03037, "ax":3.5464, "ay":-2.18338, "alpha":0.71716, "fx":[55.57178,59.18125,60.38504,56.76953], "fy":[-39.77801,-34.78412,-31.39744,-36.81688]}, + {"t":1.4115, "x":5.10553, "y":2.73912, "heading":2.09365, "vx":-0.86694, "vy":0.53371, "omega":-0.00771, "ax":3.89898, "ay":-2.40031, "alpha":0.07329, "fx":[63.45939,63.83199,64.02219,63.65011], "fy":[-39.69877,-39.1033,-38.77926,-39.38039]}, + {"t":1.44309, "x":5.08009, "y":2.75479, "heading":2.09341, "vx":-0.74377, "vy":0.45789, "omega":-0.0054, "ax":3.91577, "ay":-2.41065, "alpha":0.0435, "fx":[63.84726,64.06844,64.18342,63.96245], "fy":[-39.68281,-39.32674,-39.13511,-39.4932]}, + {"t":1.47468, "x":5.05855, "y":2.76805, "heading":2.09324, "vx":-0.62008, "vy":0.38174, "omega":-0.00403, "ax":3.92172, "ay":-2.41431, "alpha":0.03296, "fx":[63.98505,64.15262,64.24032,64.07286], "fy":[-39.67686,-39.40639,-39.26137,-39.53301]}, + {"t":1.50626, "x":5.04092, "y":2.7789, "heading":2.09311, "vx":-0.4962, "vy":0.30547, "omega":-0.00298, "ax":3.92477, "ay":-2.41619, "alpha":0.02757, "fx":[64.05564,64.19579,64.26939,64.12933], "fy":[-39.67376,-39.44724,-39.32602,-39.55335]}, + {"t":1.53785, "x":5.0272, "y":2.78735, "heading":2.09302, "vx":-0.37222, "vy":0.22915, "omega":-0.00211, "ax":3.92662, "ay":-2.41733, "alpha":0.02429, "fx":[64.09856,64.22205,64.28704,64.16361], "fy":[-39.67185,-39.4721,-39.36531,-39.5657]}, + {"t":1.56944, "x":5.0174, "y":2.79338, "heading":2.09295, "vx":-0.24818, "vy":0.15279, "omega":-0.00135, "ax":3.92787, "ay":-2.4181, "alpha":0.02209, "fx":[64.1274,64.2397,64.29889,64.18664], "fy":[-39.67056,-39.48881,-39.39172,-39.57399]}, + {"t":1.60103, "x":5.01152, "y":2.797, "heading":2.09291, "vx":-0.1241, "vy":0.0764, "omega":-0.00065, "ax":3.92876, "ay":-2.41865, "alpha":0.02051, "fx":[64.14812,64.25238,64.30739,64.20318], "fy":[-39.66963,-39.50082,-39.41068,-39.57995]}, + {"t":1.63262, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index b8c97aea..fc2f4d8f 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -7,8 +7,15 @@ "slow":{ "dimension":"LinVel", "var":{ - "exp":"1.25 m / s", - "val":1.25 + "exp":"1.15 m / s", + "val":1.15 + } + }, + "slowaccel":{ + "dimension":"LinAcc", + "var":{ + "exp":"1.5 m / s ^ 2", + "val":1.5 } } }, diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1f28e41c..82181798 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -134,7 +134,7 @@ public void runCoralPath( .get(startPos + "to" + endPos) .atTime( steps.get(startPos + "to" + endPos).getRawTrajectory().getTotalTime() - - (endPos.length() == 1 ? 0.1 : 0.0))) + - (endPos.length() == 1 ? 0.3 : 0.0))) .onTrue( Commands.sequence( endPos.length() == 3 @@ -431,7 +431,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) - .debounce(0.10)), + .debounce(0.06)), Commands.print("Scoring!"), Commands.runOnce( () -> { @@ -482,7 +482,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } public void bindElevatorExtension(AutoRoutine routine) { - bindElevatorExtension(routine, 3.5); // TODO tune + bindElevatorExtension(routine, 3.75); // TODO tune } public void bindElevatorExtension(AutoRoutine routine, double toleranceMeters) { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 341edec3..d4b5755c 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -52,7 +52,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); private static final MotionMagicConfigs TOSS_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.5).withMotionMagicAcceleration(4.0); From fe7d418f5a4ddc0ca18e91b6fb4e48cbd77399c1 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 9 Apr 2025 13:30:10 -0700 Subject: [PATCH 010/154] increase accel --- src/main/deploy/choreo/AtoPLO.traj | 99 ++++++++-------- src/main/deploy/choreo/JtoPLO.traj | 176 ++++++++++++++--------------- src/main/deploy/choreo/KtoPLO.traj | 81 +++++++------ src/main/deploy/choreo/LOtoJ.traj | 135 +++++++++++----------- src/main/deploy/choreo/LtoPLM.traj | 82 +++++++------- src/main/deploy/choreo/PLMtoA.traj | 174 ++++++++++++++-------------- src/main/deploy/choreo/PLOtoK.traj | 138 +++++++++++----------- src/main/deploy/choreo/PLOtoL.traj | 139 +++++++++++------------ src/main/deploy/choreo/choreo.chor | 4 +- src/main/java/frc/robot/Autos.java | 16 +-- 10 files changed, 514 insertions(+), 530 deletions(-) diff --git a/src/main/deploy/choreo/AtoPLO.traj b/src/main/deploy/choreo/AtoPLO.traj index ed1b3476..5fe806fb 100644 --- a/src/main/deploy/choreo/AtoPLO.traj +++ b/src/main/deploy/choreo/AtoPLO.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,55 +26,54 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.73378], + "waypoints":[0.0,1.67509], "samples":[ - {"t":0.0, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.14834, "ay":4.44845, "alpha":-1.6039, "fx":[-30.99718,-24.89414,-38.47569,-46.11821], "fy":[75.08718,77.34672,71.5742,66.88695]}, - {"t":0.03769, "x":3.16362, "y":4.18516, "heading":0.0, "vx":-0.08097, "vy":0.16767, "omega":-0.06045, "ax":-2.14854, "ay":4.44877, "alpha":-1.58687, "fx":[-31.05019,-25.0,-38.44546,-46.00248], "fy":[75.06013,77.30832,71.58613,66.96128]}, - {"t":0.07538, "x":3.15905, "y":4.19464, "heading":-0.00228, "vx":-0.16195, "vy":0.33534, "omega":-0.12026, "ax":-2.14876, "ay":4.44912, "alpha":-1.56806, "fx":[-31.13649,-25.10478,-38.39126,-45.87991], "fy":[75.01871,77.26965,71.61048,67.03954]}, - {"t":0.11307, "x":3.15142, "y":4.21044, "heading":-0.00681, "vx":-0.24294, "vy":0.50304, "omega":-0.17936, "ax":-2.149, "ay":4.44949, "alpha":-1.54723, "fx":[-31.2553,-25.211,-38.31347,-45.74843], "fy":[74.96303,77.22981,71.64688,67.12298]}, - {"t":0.15076, "x":3.14073, "y":4.23256, "heading":-0.01357, "vx":-0.32394, "vy":0.67074, "omega":-0.23768, "ax":-2.14926, "ay":4.44989, "alpha":-1.52408, "fx":[-31.4058,-25.32179,-38.21252,-45.60541], "fy":[74.89316,77.18769,71.69494,67.21326]}, - {"t":0.18845, "x":3.127, "y":4.261, "heading":-0.02253, "vx":-0.40495, "vy":0.83846, "omega":-0.29512, "ax":-2.14955, "ay":4.45033, "alpha":-1.49824, "fx":[-31.58706,-25.44101,-38.08886,-45.4475], "fy":[74.80915,77.14188,71.75418,67.31238]}, - {"t":0.22615, "x":3.11021, "y":4.29577, "heading":-0.03365, "vx":-0.48597, "vy":1.0062, "omega":-0.35159, "ax":-2.14987, "ay":4.4508, "alpha":-1.46925, "fx":[-31.79798,-25.57348,-37.94302,-45.27055], "fy":[74.71106,77.09058,71.82403,67.42286]}, - {"t":0.26384, "x":3.09036, "y":4.33685, "heading":-0.04691, "vx":-0.567, "vy":1.17395, "omega":-0.40697, "ax":-2.15021, "ay":4.45132, "alpha":-1.43651, "fx":[-32.0372,-25.72521,-37.77564,-45.06938], "fy":[74.599,77.03156,71.90382,67.54776]}, - {"t":0.30153, "x":3.06747, "y":4.38426, "heading":-0.06224, "vx":-0.64804, "vy":1.34173, "omega":-0.46112, "ax":-2.15058, "ay":4.45187, "alpha":-1.39924, "fx":[-32.303,-25.90372,-37.58749,-44.83746], "fy":[74.47314,76.9619,71.99274,67.69088]}, - {"t":0.33922, "x":3.04151, "y":4.438, "heading":-0.07962, "vx":-0.7291, "vy":1.50952, "omega":-0.51385, "ax":-2.15098, "ay":4.45248, "alpha":-1.35643, "fx":[-32.59319,-26.11863,-37.37956,-44.56646], "fy":[74.33384,76.87784,72.08976,67.85692]}, - {"t":0.37691, "x":3.0125, "y":4.49805, "heading":-0.09899, "vx":-0.81017, "vy":1.67734, "omega":-0.56498, "ax":-2.15141, "ay":4.45314, "alpha":-1.30668, "fx":[-32.90486,-26.38238,-37.15313,-44.24557], "fy":[74.18163,76.77437,72.19362,68.05188]}, - {"t":0.4146, "x":2.98044, "y":4.56444, "heading":-0.12029, "vx":-0.89126, "vy":1.84518, "omega":-0.61423, "ax":-2.15187, "ay":4.45385, "alpha":-1.24807, "fx":[-33.23422,-26.71144,-36.9099,-43.86036], "fy":[74.0174,76.64462,72.30267,68.2835]}, - {"t":0.45229, "x":2.94532, "y":4.63715, "heading":-0.14344, "vx":-0.97237, "vy":2.01305, "omega":-0.66127, "ax":-2.15235, "ay":4.45462, "alpha":-1.17791, "fx":[-33.57615,-27.12814,-36.6522,-43.39105], "fy":[73.84247,76.47895,72.41473,68.56203]}, - {"t":0.48998, "x":2.90714, "y":4.71619, "heading":-0.16836, "vx":-1.05349, "vy":2.18095, "omega":-0.70567, "ax":-2.15285, "ay":4.45542, "alpha":-1.09222, "fx":[-33.92371,-27.66381,-36.38329,-42.80948], "fy":[73.65885,76.26326,72.5268,68.90156]}, - {"t":0.52767, "x":2.86591, "y":4.80155, "heading":-0.19496, "vx":-1.13463, "vy":2.34888, "omega":-0.74683, "ax":-2.15335, "ay":4.45621, "alpha":-0.98501, "fx":[-34.26728,-28.36398,-36.10793,-42.07371], "fy":[73.46951,75.97601,72.63456,69.32209]}, - {"t":0.56536, "x":2.82161, "y":4.89325, "heading":-0.22311, "vx":-1.21579, "vy":2.51684, "omega":-0.78396, "ax":-2.15381, "ay":4.45688, "alpha":-0.84682, "fx":[-34.59313,-29.29802,-35.83337,-41.11798], "fy":[73.2789,75.58242,72.7314,69.85335]}, - {"t":0.60305, "x":2.77426, "y":4.99128, "heading":-0.25266, "vx":-1.29697, "vy":2.68483, "omega":-0.81588, "ax":-2.15411, "ay":4.45716, "alpha":-0.66183, "fx":[-34.88078,-30.57765,-35.57144,-39.83219], "fy":[73.09375,75.02244,72.80639,70.54172]}, - {"t":0.64075, "x":2.72384, "y":5.09564, "heading":-0.28341, "vx":-1.37816, "vy":2.85282, "omega":-0.84082, "ax":-2.15397, "ay":4.45634, "alpha":-0.40171, "fx":[-35.09803,-32.39592,-35.34326,-38.0162], "fy":[72.9246,74.18293,72.83958,71.46375]}, - {"t":0.67844, "x":2.67037, "y":5.20633, "heading":-0.3151, "vx":-1.45935, "vy":3.02078, "omega":-0.85596, "ax":-2.1526, "ay":4.45244, "alpha":-0.01051, "fx":[-35.19005,-35.11747,-35.19185,-35.26439], "fy":[72.7888,72.8244,72.78902,72.75334]}, - {"t":0.71613, "x":2.61383, "y":5.32335, "heading":-0.34736, "vx":-1.54048, "vy":3.1886, "omega":-0.85636, "ax":-2.14724, "ay":4.43883, "alpha":0.63833, "fx":[-35.05282,-39.51565,-35.22485,-30.61984], "fy":[72.71721,70.35036,72.54542,74.65294]}, - {"t":0.75382, "x":2.55425, "y":5.44668, "heading":-0.37964, "vx":-1.62141, "vy":3.3559, "omega":-0.8323, "ax":-2.12595, "ay":4.38655, "alpha":1.89331, "fx":[-34.45542,-47.47899,-35.82962,-21.25692], "fy":[72.77117,64.86829,71.69381,77.51397]}, - {"t":0.79151, "x":2.49162, "y":5.57628, "heading":-0.41101, "vx":-1.70154, "vy":3.52124, "omega":-0.76094, "ax":-2.03162, "ay":4.07305, "alpha":5.04181, "fx":[-32.7489,-63.7093,-40.53374,4.13948], "fy":[73.08634,47.88386,65.87862,79.49749]}, - {"t":0.8292, "x":2.42605, "y":5.71189, "heading":-0.43969, "vx":-1.77812, "vy":3.67475, "omega":-0.57091, "ax":-1.87052, "ay":3.16229, "alpha":8.63028, "fx":[-30.19527,-68.81788,-45.13224,21.82778], "fy":[71.72949,33.75642,28.29546,73.00844]}, - {"t":0.86689, "x":2.3577, "y":5.85265, "heading":-0.46121, "vx":-1.84862, "vy":3.79394, "omega":-0.24563, "ax":1.86218, "ay":-3.15924, "alpha":-8.67077, "fx":[29.24097,68.24617,46.75587,-22.47056], "fy":[-72.03004,-34.72308,-27.19735,-72.63971]}, - {"t":0.90458, "x":2.28935, "y":5.9934, "heading":-0.47046, "vx":-1.77843, "vy":3.67487, "omega":-0.57243, "ax":2.04311, "ay":-4.06595, "alpha":-5.08, "fx":[31.20309,62.73253,44.30548,-4.63728], "fy":[-73.74551,-49.19538,-63.52848,-79.41292]}, - {"t":0.94227, "x":2.22377, "y":6.12902, "heading":-0.49204, "vx":-1.70142, "vy":3.52162, "omega":-0.7639, "ax":2.12395, "ay":-4.38804, "alpha":-1.88624, "fx":[33.16246,46.97329,37.52697,21.22742], "fy":[-73.36451,-65.25455,-70.82561,-77.49947]}, - {"t":0.97996, "x":2.16115, "y":6.25864, "heading":-0.52083, "vx":-1.62137, "vy":3.35623, "omega":-0.835, "ax":2.1454, "ay":-4.43961, "alpha":-0.64385, "fx":[34.29718,39.38423,36.00785,30.60388], "fy":[-73.07512,-70.43075,-72.15995,-74.65109]}, - {"t":1.01766, "x":2.10156, "y":6.38198, "heading":-0.5523, "vx":-1.54051, "vy":3.1889, "omega":-0.85927, "ax":2.15125, "ay":-4.45306, "alpha":-0.00285, "fx":[35.16437,35.18809,35.17316,35.14943], "fy":[-72.80137,-72.7898,-72.79684,-72.80841]}, - {"t":1.05535, "x":2.04503, "y":6.49901, "heading":-0.58469, "vx":-1.45943, "vy":3.02106, "omega":-0.85937, "ax":2.15295, "ay":-4.457, "alpha":0.38538, "fx":[35.911,32.56847,34.54925,37.75791], "fy":[-72.5289,-74.10222,-73.21793,-71.605]}, - {"t":1.09304, "x":1.99155, "y":6.60971, "heading":-0.61708, "vx":-1.37828, "vy":2.85307, "omega":-0.84485, "ax":2.15333, "ay":-4.45787, "alpha":0.64562, "fx":[36.5871,30.79517,34.01933,39.40965], "fy":[-72.25663,-74.9252,-73.54267,-70.7865]}, - {"t":1.13073, "x":1.94113, "y":6.71408, "heading":-0.64892, "vx":-1.29712, "vy":2.68505, "omega":-0.82051, "ax":2.15323, "ay":-4.45762, "alpha":0.83258, "fx":[37.21224,29.52766,33.54336,40.52133], "fy":[-71.98661,-75.48261,-73.81368,-70.21139]}, - {"t":1.16842, "x":1.89377, "y":6.81212, "heading":-0.67985, "vx":-1.21596, "vy":2.51704, "omega":-0.78913, "ax":2.15295, "ay":-4.45693, "alpha":0.97382, "fx":[37.79471,28.5871,33.10504,41.29939], "fy":[-71.72146,-75.88039,-74.0496,-69.79814]}, - {"t":1.20611, "x":1.84947, "y":6.90382, "heading":-0.70959, "vx":-1.13481, "vy":2.34905, "omega":-0.75243, "ax":2.1526, "ay":-4.4561, "alpha":1.08459, "fx":[38.33823,27.87055,32.69704,41.85794], "fy":[-71.46365,-76.17488,-74.25973,-69.49694]}, - {"t":1.2438, "x":1.80822, "y":6.98919, "heading":-0.73795, "vx":-1.05368, "vy":2.18109, "omega":-0.71155, "ax":2.15224, "ay":-4.45524, "alpha":1.17402, "fx":[38.8446,27.31426,32.31591,42.26547], "fy":[-71.21533,-76.39881,-74.44918,-69.27567]}, - {"t":1.28149, "x":1.77004, "y":7.06824, "heading":-0.76477, "vx":-0.97256, "vy":2.01317, "omega":-0.6673, "ax":2.15189, "ay":-4.45441, "alpha":1.24789, "fx":[39.31483,26.87631,31.95997,42.56581], "fy":[-70.97826,-76.57254,-74.62103,-69.1126]}, - {"t":1.31918, "x":1.73491, "y":7.14095, "heading":-0.78992, "vx":-0.89145, "vy":1.84528, "omega":-0.62027, "ax":2.15154, "ay":-4.45362, "alpha":1.30999, "fx":[39.74968,26.5277,31.62843,42.78855], "fy":[-70.75384,-76.7095,-74.77726,-68.99242]}, - {"t":1.35687, "x":1.70284, "y":7.20734, "heading":-0.8133, "vx":-0.81036, "vy":1.67742, "omega":-0.57089, "ax":2.15121, "ay":-4.45289, "alpha":1.36296, "fx":[40.14985,26.24749,31.32092,42.95449], "fy":[-70.54314,-76.81896,-74.91922,-68.90396]}, - {"t":1.39456, "x":1.67382, "y":7.2674, "heading":-0.83482, "vx":-0.72928, "vy":1.50959, "omega":-0.51952, "ax":2.1509, "ay":-4.45222, "alpha":1.40866, "fx":[40.51608,26.02001,31.0373,43.07882], "fy":[-70.34699,-76.9076,-75.04789,-68.83888]}, - {"t":1.43226, "x":1.64786, "y":7.32113, "heading":-0.8544, "vx":-0.64821, "vy":1.34178, "omega":-0.46643, "ax":2.1506, "ay":-4.45161, "alpha":1.44847, "fx":[40.84918,25.83315,30.77754,43.17293], "fy":[-70.16595,-76.98038,-75.16403,-68.79075]}, - {"t":1.46995, "x":1.62496, "y":7.36854, "heading":-0.87198, "vx":-0.56715, "vy":1.17399, "omega":-0.41183, "ax":2.15032, "ay":-4.45104, "alpha":1.48341, "fx":[41.14999,25.67733,30.54162,43.24559], "fy":[-70.00043,-77.04111,-75.26822,-68.75453]}, - {"t":1.50764, "x":1.60511, "y":7.40963, "heading":-0.8875, "vx":-0.4861, "vy":1.00623, "omega":-0.35592, "ax":2.15006, "ay":-4.45053, "alpha":1.51427, "fx":[41.41937,25.54482,30.32958,43.30371], "fy":[-69.85066,-77.0928,-75.36096,-68.72623]}, - {"t":1.54533, "x":1.58832, "y":7.4444, "heading":-0.90092, "vx":-0.40507, "vy":0.83848, "omega":-0.29885, "ax":2.14982, "ay":-4.45006, "alpha":1.54168, "fx":[41.6582,25.42926,30.14144,43.35276], "fy":[-69.71679,-77.13783,-75.44265,-68.70262]}, - {"t":1.58302, "x":1.57458, "y":7.47284, "heading":-0.91218, "vx":-0.32404, "vy":0.67076, "omega":-0.24074, "ax":2.14959, "ay":-4.44963, "alpha":1.56613, "fx":[41.86726,25.32542,29.97725,43.39719], "fy":[-69.59889,-77.17814,-75.51361,-68.68108]}, - {"t":1.62071, "x":1.56389, "y":7.49496, "heading":-0.92126, "vx":-0.24302, "vy":0.50305, "omega":-0.18171, "ax":2.14939, "ay":-4.44923, "alpha":1.58802, "fx":[42.04731,25.22894,29.83704,43.44059], "fy":[-69.49698,-77.21531,-75.57412,-68.65946]}, - {"t":1.6584, "x":1.55626, "y":7.51076, "heading":-0.9281, "vx":-0.16201, "vy":0.33535, "omega":-0.12186, "ax":2.14921, "ay":-4.44887, "alpha":1.60768, "fx":[42.19896,25.13619,29.7209,43.48594], "fy":[-69.41108,-77.25063,-75.62439,-68.63599]}, - {"t":1.69609, "x":1.55168, "y":7.52024, "heading":-0.9327, "vx":-0.081, "vy":0.16767, "omega":-0.06126, "ax":2.14905, "ay":-4.44853, "alpha":1.62538, "fx":[42.32273,25.0442,29.6289,43.53563], "fy":[-69.34121,-77.28516,-75.66459,-68.60922]}, - {"t":1.73378, "x":1.55015, "y":7.5234, "heading":-0.93501, "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.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.30254, "ay":4.76798, "alpha":-1.66398, "fx":[-33.38171,-27.0115,-41.12927,-49.04623], "fy":[80.37914,82.76017,76.73591,71.9143]}, + {"t":0.03722, "x":3.16356, "y":4.18531, "heading":0.0, "vx":-0.08571, "vy":0.17748, "omega":-0.06194, "ax":-2.3027, "ay":4.76823, "alpha":-1.64784, "fx":[-33.431,-27.11184,-41.10006,-48.93598], "fy":[80.35295,82.72265,76.7468,71.98351]}, + {"t":0.07445, "x":3.15877, "y":4.19522, "heading":-0.00231, "vx":-0.17143, "vy":0.35498, "omega":-0.12328, "ax":-2.30287, "ay":4.7685, "alpha":-1.62999, "fx":[-33.51466,-27.2102,-41.04576,-48.81962], "fy":[80.3118,82.68513,76.7706,72.05607]}, + {"t":0.11167, "x":3.15079, "y":4.21173, "heading":-0.00689, "vx":-0.25715, "vy":0.53248, "omega":-0.18395, "ax":-2.30306, "ay":4.76879, "alpha":-1.61019, "fx":[-33.63198,-27.30903,-40.96674,-48.69508], "fy":[80.25578,82.6467,76.80697,72.13327]}, + {"t":0.1489, "x":3.13963, "y":4.23486, "heading":-0.01374, "vx":-0.34288, "vy":0.70999, "omega":-0.24389, "ax":-2.30328, "ay":4.76911, "alpha":-1.58814, "fx":[-33.78221,-27.41144,-40.86337,-48.55968], "fy":[80.18491,82.60626,76.85553,72.21672]}, + {"t":0.18612, "x":3.12527, "y":4.26459, "heading":-0.02282, "vx":-0.42862, "vy":0.88752, "omega":-0.30301, "ax":-2.30351, "ay":4.76945, "alpha":-1.56346, "fx":[-33.9645,-27.52127,-40.73606,-48.41006], "fy":[80.0992,82.56236,76.91579,72.30846]}, + {"t":0.22334, "x":3.10772, "y":4.30093, "heading":-0.0341, "vx":-0.51436, "vy":1.06506, "omega":-0.36121, "ax":-2.30376, "ay":4.76982, "alpha":-1.5357, "fx":[-34.17785,-27.64335,-40.58531,-48.24199], "fy":[79.99864,82.51319,76.9872,72.41102]}, + {"t":0.26057, "x":3.08697, "y":4.34388, "heading":-0.04755, "vx":-0.60012, "vy":1.24261, "omega":-0.41837, "ax":-2.30404, "ay":4.77022, "alpha":-1.50424, "fx":[-34.42099,-27.7837,-40.41169,-48.05016], "fy":[79.88327,82.45646,77.06908,72.52752]}, + {"t":0.29779, "x":3.06304, "y":4.39344, "heading":-0.06312, "vx":-0.68588, "vy":1.42018, "omega":-0.47437, "ax":-2.30434, "ay":4.77066, "alpha":-1.46831, "fx":[-34.69233,-27.94995,-40.2159,-47.82787], "fy":[79.75318,82.3892,77.1606,72.66182]}, + {"t":0.33502, "x":3.03591, "y":4.44961, "heading":-0.08078, "vx":-0.77166, "vy":1.59776, "omega":-0.52902, "ax":-2.30466, "ay":4.77113, "alpha":-1.42685, "fx":[-34.98976,-28.1519,-39.99887,-47.56651], "fy":[79.60859,82.30752,77.26073,72.81881]}, + {"t":0.37224, "x":3.00559, "y":4.5124, "heading":-0.10047, "vx":-0.85745, "vy":1.77537, "omega":-0.58214, "ax":-2.305, "ay":4.77164, "alpha":-1.37843, "fx":[-35.31048,-28.40234,-39.7618,-47.25483], "fy":[79.44993,82.20625,77.36814,73.00468]}, + {"t":0.40947, "x":2.97207, "y":4.58179, "heading":-0.12214, "vx":-0.94325, "vy":1.95299, "omega":-0.63345, "ax":-2.30536, "ay":4.77219, "alpha":-1.32106, "fx":[-35.65073,-28.71834,-39.50632,-46.87773], "fy":[79.27789,82.07822,77.4811,73.22756]}, + {"t":0.44669, "x":2.93536, "y":4.65779, "heading":-0.14572, "vx":-1.02907, "vy":2.13063, "omega":-0.68262, "ax":-2.30574, "ay":4.77277, "alpha":-1.25188, "fx":[-36.0054,-29.12337,-39.23468,-46.41426], "fy":[79.09363,81.9133,77.59729,73.49833]}, + {"t":0.48391, "x":2.89546, "y":4.74041, "heading":-0.17113, "vx":-1.1149, "vy":2.30829, "omega":-0.72922, "ax":-2.30612, "ay":4.77335, "alpha":-1.16666, "fx":[-36.36736,-29.65079,-38.9501,-45.8342], "fy":[78.89891,81.69649,77.71345,73.83218]}, + {"t":0.52114, "x":2.85236, "y":4.82964, "heading":-0.19827, "vx":-1.20074, "vy":2.48597, "omega":-0.77265, "ax":-2.30647, "ay":4.7739, "alpha":-1.05888, "fx":[-36.72655,-30.35003,-38.65734,-45.09185], "fy":[78.69646,81.40452,77.82477,74.25107]}, + {"t":0.55836, "x":2.80607, "y":4.92549, "heading":-0.22704, "vx":-1.28659, "vy":2.66368, "omega":-0.81207, "ax":-2.30675, "ay":4.77428, "alpha":-0.91802, "fx":[-37.06815,-31.29816,-38.3638,-44.11399], "fy":[78.49045,80.99908,77.92372,74.78848]}, + {"t":0.59559, "x":2.75658, "y":5.02795, "heading":-0.25726, "vx":-1.37246, "vy":2.8414, "omega":-0.84624, "ax":-2.30683, "ay":4.77418, "alpha":-0.72597, "fx":[-37.36936,-32.62317,-38.08192,-42.77453], "fy":[78.28741,80.41219,77.99746,75.49827]}, + {"t":0.63281, "x":2.70389, "y":5.13702, "heading":-0.28876, "vx":-1.45833, "vy":3.01911, "omega":-0.87326, "ax":-2.30636, "ay":4.77278, "alpha":-0.44898, "fx":[-37.59265,-34.55541,-37.83472,-40.83534], "fy":[78.09792,79.51071,78.02154,76.47316]}, + {"t":0.67003, "x":2.64801, "y":5.25271, "heading":-0.32127, "vx":-1.54418, "vy":3.19677, "omega":-0.88998, "ax":-2.3043, "ay":4.76757, "alpha":-0.01653, "fx":[-37.67023,-37.55551,-37.67173,-37.78638], "fy":[77.94022,77.99654,77.9414,77.88491]}, + {"t":0.70726, "x":2.58893, "y":5.37501, "heading":-0.3544, "vx":-1.62996, "vy":3.37424, "omega":-0.89059, "ax":-2.29679, "ay":4.74942, "alpha":0.74512, "fx":[-37.46217,-42.68521,-37.72877,-32.31645], "fy":[77.84883,75.05702,77.60059,80.06916]}, + {"t":0.74448, "x":2.52666, "y":5.50391, "heading":-0.38755, "vx":-1.71546, "vy":3.55104, "omega":-0.86286, "ax":-2.26378, "ay":4.66967, "alpha":2.3867, "fx":[-36.61534,-52.87595,-38.59601,-19.94679], "fy":[77.898,67.67623,76.27467,83.51192]}, + {"t":0.78171, "x":2.46124, "y":5.63933, "heading":-0.41967, "vx":-1.79972, "vy":3.72486, "omega":-0.77401, "ax":-2.15343, "ay":3.84956, "alpha":7.82262, "fx":[-33.94597,-74.91965,-52.09911,20.14693], "fy":[78.21308,39.30854,52.23521,81.97523]}, + {"t":0.81893, "x":2.39275, "y":5.78065, "heading":-0.44848, "vx":-1.87988, "vy":3.86816, "omega":-0.48282, "ax":-0.04439, "ay":-0.00008, "alpha":0.0184, "fx":[-0.74877,-0.79328,-0.70273,-0.65823], "fy":[0.0668,-0.02571,-0.06928,0.02322]}, + {"t":0.85616, "x":2.32275, "y":5.92464, "heading":-0.46645, "vx":-1.88153, "vy":3.86815, "omega":-0.48214, "ax":2.19166, "ay":-3.83892, "alpha":-7.88693, "fx":[32.36044,73.73388,57.62718,-20.40332], "fy":[-78.87306,-41.58254,-48.74051,-81.83993]}, + {"t":0.89338, "x":2.25423, "y":6.06597, "heading":-0.4844, "vx":-1.79995, "vy":3.72525, "omega":-0.77572, "ax":2.26404, "ay":-4.67198, "alpha":-2.35715, "fx":[35.28255,52.18655,40.50455,20.07756], "fy":[-78.50873,-68.24285,-75.30309,-83.45747]}, + {"t":0.9306, "x":2.18879, "y":6.2014, "heading":-0.51328, "vx":-1.71567, "vy":3.55134, "omega":-0.86346, "ax":2.29597, "ay":-4.75, "alpha":-0.74435, "fx":[36.68332,42.51577,38.57892,32.36067], "fy":[-78.2195,-75.16458,-77.18548,-80.04414]}, + {"t":0.96783, "x":2.12652, "y":6.3303, "heading":-0.54542, "vx":-1.63021, "vy":3.37453, "omega":-0.89117, "ax":2.30369, "ay":-4.76794, "alpha":0.00347, "fx":[37.66613,37.63739,37.65576,37.68449], "fy":[-77.94403,-77.95807,-77.94942,-77.93539]}, + {"t":1.00505, "x":2.06743, "y":6.45261, "heading":-0.57859, "vx":-1.54446, "vy":3.19705, "omega":-0.89104, "ax":2.30595, "ay":-4.77323, "alpha":0.43067, "fx":[38.4791,34.75591,36.99349,40.56283], "fy":[-77.66751,-79.41835,-78.42302,-76.62429]}, + {"t":1.04228, "x":2.01154, "y":6.56832, "heading":-0.61176, "vx":-1.45862, "vy":3.01937, "omega":-0.87501, "ax":2.30658, "ay":-4.77475, "alpha":0.70689, "fx":[39.19972,32.87438,36.43891,42.31963], "fy":[-77.39012,-80.30115,-78.77705,-75.76383]}, + {"t":1.0795, "x":1.95884, "y":6.6774, "heading":-0.64433, "vx":-1.37276, "vy":2.84163, "omega":-0.8487, "ax":2.30664, "ay":-4.77491, "alpha":0.90059, "fx":[39.85745,31.56364,35.94369,43.47198], "fy":[-77.11479,-80.88485,-79.06756,-75.17547]}, + {"t":1.11672, "x":1.90934, "y":6.77987, "heading":-0.67592, "vx":-1.2869, "vy":2.66389, "omega":-0.81517, "ax":2.30649, "ay":-4.77455, "alpha":1.04438, "fx":[40.46493,30.61022,35.48907,44.26243], "fy":[-76.84468,-81.29416,-79.3179,-74.76212]}, + {"t":1.15395, "x":1.86303, "y":6.87572, "heading":-0.70627, "vx":-1.20104, "vy":2.48616, "omega":-0.7763, "ax":2.30625, "ay":-4.77398, "alpha":1.1557, "fx":[41.02814,29.89594,35.06685,44.8201], "fy":[-76.58253,-81.59302,-79.53939,-74.46697]}, + {"t":1.19117, "x":1.81992, "y":6.96496, "heading":-0.73517, "vx":-1.11519, "vy":2.30845, "omega":-0.73328, "ax":2.30598, "ay":-4.77335, "alpha":1.24467, "fx":[41.55017,29.34969,34.67318,45.22032], "fy":[-76.33061,-81.81758,-79.73814,-74.2545]}, + {"t":1.2284, "x":1.78001, "y":7.04758, "heading":-0.76246, "vx":-1.02935, "vy":2.13077, "omega":-0.68695, "ax":2.3057, "ay":-4.77272, "alpha":1.31755, "fx":[42.03288,28.92573,34.30623,45.51034], "fy":[-76.09072,-81.98991,-79.91777,-74.10127]}, + {"t":1.26562, "x":1.74329, "y":7.12359, "heading":-0.78803, "vx":-0.94353, "vy":1.95311, "omega":-0.6379, "ax":2.30542, "ay":-4.77212, "alpha":1.3784, "fx":[42.47761,28.59298,33.96508,45.72152], "fy":[-75.86425,-82.12434,-80.08057,-73.99109]}, + {"t":1.30285, "x":1.70977, "y":7.19299, "heading":-0.81178, "vx":-0.85771, "vy":1.77547, "omega":-0.58659, "ax":2.30516, "ay":-4.77156, "alpha":1.43001, "fx":[42.88549,28.32929,33.64928,45.87562], "fy":[-75.65225,-82.23067,-80.22811,-73.91234]}, + {"t":1.34007, "x":1.67944, "y":7.25577, "heading":-0.83361, "vx":-0.7719, "vy":1.59785, "omega":-0.53336, "ax":2.3049, "ay":-4.77103, "alpha":1.47431, "fx":[43.25759,28.11828,33.35863,45.98838], "fy":[-75.45547,-82.31588,-80.36152,-73.85641]}, + {"t":1.37729, "x":1.6523, "y":7.31195, "heading":-0.85347, "vx":-0.6861, "vy":1.42025, "omega":-0.47848, "ax":2.30465, "ay":-4.77056, "alpha":1.51272, "fx":[43.59497,27.94739,33.093,46.07151], "fy":[-75.27444,-82.38516,-80.48166,-73.81676]}, + {"t":1.41452, "x":1.62836, "y":7.36151, "heading":-0.87128, "vx":-0.60031, "vy":1.24267, "omega":-0.42217, "ax":2.30442, "ay":-4.77012, "alpha":1.5463, "fx":[43.89868,27.80678,32.85235,46.13395], "fy":[-75.10947,-82.44245,-80.5892,-73.78832]}, + {"t":1.45174, "x":1.60761, "y":7.40446, "heading":-0.88699, "vx":-0.51453, "vy":1.06511, "omega":-0.36461, "ax":2.30421, "ay":-4.76972, "alpha":1.57585, "fx":[44.16975,27.68853,32.63664,46.18269], "fy":[-74.96077,-82.49082,-80.68468,-73.76706]}, + {"t":1.48897, "x":1.59005, "y":7.44081, "heading":-0.90057, "vx":-0.42876, "vy":0.88756, "omega":-0.30595, "ax":2.30401, "ay":-4.76936, "alpha":1.602, "fx":[44.40918,27.5862,32.44583,46.22328], "fy":[-74.82842,-82.53275,-80.76857,-73.74977]}, + {"t":1.52619, "x":1.57569, "y":7.47054, "heading":-0.91195, "vx":-0.343, "vy":0.71003, "omega":-0.24632, "ax":2.30382, "ay":-4.76902, "alpha":1.62526, "fx":[44.61788,27.49449,32.27992,46.26017], "fy":[-74.71244,-82.57022,-80.84122,-73.73386]}, + {"t":1.56341, "x":1.56451, "y":7.49367, "heading":-0.92112, "vx":-0.25724, "vy":0.5325, "omega":-0.18582, "ax":2.30366, "ay":-4.76872, "alpha":1.64602, "fx":[44.79667,27.40899,32.13891,46.29698], "fy":[-74.61281,-82.60485,-80.90294,-73.71721]}, + {"t":1.60064, "x":1.55653, "y":7.51018, "heading":-0.92804, "vx":-0.17149, "vy":0.35499, "omega":-0.12455, "ax":2.30351, "ay":-4.76844, "alpha":1.66461, "fx":[44.94625,27.32611,32.02282,46.33665], "fy":[-74.52952,-82.63795,-80.95397,-73.69807]}, + {"t":1.63786, "x":1.55175, "y":7.52009, "heading":-0.93268, "vx":-0.08574, "vy":0.17749, "omega":-0.06258, "ax":2.30338, "ay":-4.76818, "alpha":1.6813, "fx":[45.06718,27.24287,31.93171,46.38156], "fy":[-74.46254,-82.67059,-80.99451,-73.67503]}, + {"t":1.67509, "x":1.55015, "y":7.5234, "heading":-0.93501, "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/JtoPLO.traj b/src/main/deploy/choreo/JtoPLO.traj index a51ec500..2ddd5bd3 100644 --- a/src/main/deploy/choreo/JtoPLO.traj +++ b/src/main/deploy/choreo/JtoPLO.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.807075500488281, "y":5.671792030334473, "heading":-1.7084131860016765, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.906598448753357, "y":7.005744457244873, "heading":-1.3217094862895618, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.807075500488281, "y":5.671792030334473, "heading":-1.7084131860016765, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.906598448753357, "y":7.005744457244873, "heading":-1.3217094862895618, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,9 +15,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.807075500488281 m", "val":4.807075500488281}, "y":{"exp":"5.671792030334473 m", "val":5.671792030334473}, "heading":{"exp":"-1.7084131860016765 rad", "val":-1.7084131860016765}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"1.906598448753357 m", "val":1.906598448753357}, "y":{"exp":"7.005744457244873 m", "val":7.005744457244873}, "heading":{"exp":"-1.3217094862895618 rad", "val":-1.3217094862895618}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.807075500488281 m", "val":4.807075500488281}, "y":{"exp":"5.671792030334473 m", "val":5.671792030334473}, "heading":{"exp":"-1.7084131860016765 rad", "val":-1.7084131860016765}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.906598448753357 m", "val":1.906598448753357}, "y":{"exp":"7.005744457244873 m", "val":7.005744457244873}, "heading":{"exp":"-1.3217094862895618 rad", "val":-1.3217094862895618}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,91 +30,87 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.45469,1.49476,2.01196], + "waypoints":[0.0,0.43931,1.44395,1.94358], "samples":[ - {"t":0.0, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.15527, "ay":4.94396, "alpha":1.32442, "fx":[7.13167,-4.73841,-12.76196,0.21552], "fy":[80.88154,81.06816,80.17815,81.16964]}, - {"t":0.02273, "x":5.00418, "y":5.2452, "heading":-2.09512, "vx":-0.00353, "vy":0.1124, "omega":0.03011, "ax":-0.3484, "ay":4.9325, "alpha":1.35882, "fx":[4.35873,-7.60339,-16.21245,-3.32542], "fy":[81.06958,80.84281,79.54424,81.09119]}, - {"t":0.04547, "x":5.00401, "y":5.24903, "heading":-2.09443, "vx":-0.01145, "vy":0.22454, "omega":0.061, "ax":-0.5639, "ay":4.91077, "alpha":1.39476, "fx":[1.22227,-10.77982,-19.99954,-7.31781], "fy":[81.16791,80.47326,78.66888,80.81715]}, - {"t":0.0682, "x":5.0036, "y":5.2554, "heading":-2.09305, "vx":-0.02427, "vy":0.33618, "omega":0.09271, "ax":-0.80443, "ay":4.87525, "alpha":1.43192, "fx":[-2.33559,-14.30236,-24.1452,-11.82053], "fy":[81.13297,79.91419,77.48773,80.26953]}, - {"t":0.09094, "x":5.00284, "y":5.2643, "heading":-2.09094, "vx":-0.04256, "vy":0.44702, "omega":0.12527, "ax":-1.07253, "ay":4.82134, "alpha":1.46984, "fx":[-6.37921,-18.2049,-28.66199,-16.88887], "fy":[80.9039,79.10756,75.92319,79.34414]}, - {"t":0.11367, "x":5.0016, "y":5.27571, "heading":-2.08809, "vx":-0.06694, "vy":0.55663, "omega":0.15868, "ax":-1.37025, "ay":4.74317, "alpha":1.50783, "fx":[-10.97659,-22.51694,-33.5464,-22.56379], "fy":[80.39651,77.9802,73.88464,77.90555]}, - {"t":0.13641, "x":4.99972, "y":5.28959, "heading":-2.08448, "vx":-0.09809, "vy":0.66446, "omega":0.19296, "ax":-1.6986, "ay":4.63349, "alpha":1.54485, "fx":[-16.19286,-27.25771,-38.77014,-28.85467], "fy":[79.49628,76.44192,71.27152,75.78524]}, - {"t":0.15914, "x":4.99706, "y":5.30589, "heading":-2.0801, "vx":-0.13671, "vy":0.7698, "omega":0.22808, "ax":-2.05667, "ay":4.48379, "alpha":1.57937, "fx":[-22.07789,-32.42754,-44.27038,-35.71442], "fy":[78.05175,74.38534,67.98077,72.78791]}, - {"t":0.18188, "x":4.99342, "y":5.32455, "heading":-2.07491, "vx":-0.18347, "vy":0.87174, "omega":0.26399, "ax":-2.44054, "ay":4.28482, "alpha":1.60904, "fx":[-28.64528,-37.99635,-49.94085,-43.00992], "fy":[75.87171,71.68938,63.92004,68.71314]}, - {"t":0.20461, "x":4.98861, "y":5.34548, "heading":-2.06891, "vx":-0.23895, "vy":0.96915, "omega":0.30057, "ax":-2.84219, "ay":4.02779, "alpha":1.6303, "fx":[-35.84162,-43.89015,-55.62784,-50.49793], "fy":[72.73254,68.22874,59.02712,63.39854]}, - {"t":0.22734, "x":4.98245, "y":5.36855, "heading":-2.06208, "vx":-0.30357, "vy":1.06072, "omega":0.33763, "ax":-3.24884, "ay":3.70643, "alpha":1.63813, "fx":[-43.50956,-49.97885,-61.13589,-57.82532], "fy":[68.40544,63.89186,53.29286,56.78232]}, - {"t":0.25008, "x":4.97471, "y":5.39363, "heading":-2.0544, "vx":-0.37743, "vy":1.14498, "omega":0.37488, "ax":-3.64339, "ay":3.31943, "alpha":1.62659, "fx":[-51.35884,-56.07116,-66.24698,-64.57329], "fy":[62.71174,58.60765,46.78121,48.96491]}, - {"t":0.27281, "x":4.96518, "y":5.42052, "heading":-2.04588, "vx":-0.46026, "vy":1.22045, "omega":0.41186, "ax":-4.00649, "ay":2.87264, "alpha":1.59061, "fx":[-58.9713,-61.92418,-70.75209,-70.34657], "fy":[55.60187,52.37678,39.63715,40.23301]}, - {"t":0.29555, "x":4.95368, "y":5.449, "heading":-2.03651, "vx":-0.55134, "vy":1.28576, "omega":0.44802, "ax":-4.32006, "ay":2.37973, "alpha":1.5285, "fx":[-65.86478,-67.27284,-74.48742,-74.87426], "fy":[47.22779,45.29688,32.07528,31.01654]}, - {"t":0.31828, "x":4.94003, "y":5.47885, "heading":-2.02633, "vx":-0.64956, "vy":1.33986, "omega":0.48277, "ax":-4.57115, "ay":1.86052, "alpha":1.44337, "fx":[-71.61063,-71.87553,-77.36246,-78.07007], "fy":[37.95851,37.56756,24.34919,21.78889]}, - {"t":0.34102, "x":4.92408, "y":5.50979, "heading":-2.01535, "vx":-0.75348, "vy":1.38216, "omega":0.51558, "ax":-4.75448, "ay":1.33724, "alpha":1.34201, "fx":[-75.95213,-75.56173,-79.36965,-80.02325], "fy":[28.30729,29.46646,16.71127,12.96017]}, - {"t":0.36375, "x":4.90573, "y":5.54156, "heading":-2.00363, "vx":-0.86157, "vy":1.41256, "omega":0.54609, "ax":-4.87258, "ay":0.83021, "alpha":1.23227, "fx":[-78.85652,-78.26206,-80.57371,-80.9371], "fy":[18.79939,21.30136,9.37671,4.81221]}, - {"t":0.38649, "x":4.88488, "y":5.57389, "heading":-1.99122, "vx":-0.97235, "vy":1.43143, "omega":0.57411, "ax":-4.93378, "ay":0.35478, "alpha":1.1207, "fx":[-80.479,-80.00934,-81.08761,-81.05598], "fy":[9.85227,13.35633,2.50213,-2.51107]}, - {"t":0.40922, "x":4.8615, "y":5.60652, "heading":-1.97816, "vx":-1.08451, "vy":1.4395, "omega":0.59958, "ax":-4.94931, "ay":-0.07986, "alpha":1.01182, "fx":[-81.07629,-80.91393,-81.04563,-80.61142], "fy":[1.72084,5.85203,-3.81901,-8.9764]}, - {"t":0.43195, "x":4.83556, "y":5.63923, "heading":-1.96453, "vx":-1.19703, "vy":1.43768, "omega":0.62259, "ax":-4.93066, "ay":-0.46998, "alpha":0.90833, "fx":[-80.92344,-81.12786,-80.5814,-79.79522], "fy":[-5.49241,-1.07036,-9.54846,-14.62219]}, - {"t":0.45469, "x":4.80708, "y":5.67179, "heading":-1.95038, "vx":-1.30913, "vy":1.427, "omega":0.64324, "ax":-4.9132, "ay":-0.64917, "alpha":0.84826, "fx":[-80.6678,-81.04147,-80.24295,-79.33361], "fy":[-8.74371,-4.39494,-12.2281,-17.0839]}, - {"t":0.48069, "x":4.77137, "y":5.70868, "heading":-1.93365, "vx":-1.43688, "vy":1.41012, "omega":0.66529, "ax":-4.91276, "ay":-0.65052, "alpha":0.83237, "fx":[-80.6637,-81.02301,-80.22021,-79.35009], "fy":[-8.69198,-4.56756,-12.31812,-16.96117]}, - {"t":0.50669, "x":4.73235, "y":5.74512, "heading":-1.91635, "vx":-1.56462, "vy":1.3932, "omega":0.68694, "ax":-4.91226, "ay":-0.652, "alpha":0.81482, "fx":[-80.65769,-81.00233,-80.19665,-79.36801], "fy":[-8.64842,-4.75547,-12.4063,-16.82578]}, - {"t":0.53269, "x":4.69001, "y":5.78113, "heading":-1.89849, "vx":-1.69235, "vy":1.37625, "omega":0.70812, "ax":-4.91171, "ay":-0.65365, "alpha":0.79532, "fx":[-80.64939,-80.97901,-80.17229,-79.38748], "fy":[-8.61521,-4.96068,-12.49148,-16.67608]}, - {"t":0.5587, "x":4.64435, "y":5.81669, "heading":-1.88008, "vx":-1.82006, "vy":1.35925, "omega":0.7288, "ax":-4.91107, "ay":-0.65547, "alpha":0.77353, "fx":[-80.63833,-80.95255,-80.14714,-79.40863], "fy":[-8.59502,-5.18563,-12.57224,-16.5101]}, - {"t":0.5847, "x":4.59536, "y":5.85181, "heading":-1.86113, "vx":-1.94776, "vy":1.34221, "omega":0.74892, "ax":-4.91034, "ay":-0.65752, "alpha":0.74901, "fx":[-80.6239,-80.92231,-80.12122,-79.43157], "fy":[-8.59109,-5.43336,-12.64684,-16.32547]}, - {"t":0.6107, "x":4.54306, "y":5.88649, "heading":-1.84166, "vx":-2.07544, "vy":1.32511, "omega":0.76839, "ax":-4.9095, "ay":-0.65982, "alpha":0.72122, "fx":[-80.60537,-80.88748,-80.09455,-79.45642], "fy":[-8.60743,-5.7077,-12.71309,-16.11925]}, - {"t":0.6367, "x":4.48743, "y":5.92072, "heading":-1.82168, "vx":-2.20309, "vy":1.30796, "omega":0.78714, "ax":-4.90851, "ay":-0.66244, "alpha":0.68946, "fx":[-80.58178,-80.84698,-80.06712,-79.48334], "fy":[-8.64906,-6.01355,-12.7682,-15.88776]}, - {"t":0.6627, "x":4.42849, "y":5.95451, "heading":-1.80121, "vx":-2.33072, "vy":1.29073, "omega":0.80507, "ax":-4.90734, "ay":-0.66544, "alpha":0.65278, "fx":[-80.55184,-80.79934,-80.03895,-79.51245], "fy":[-8.72239,-6.35733,-12.80853,-15.62628]}, - {"t":0.6887, "x":4.36623, "y":5.98784, "heading":-1.78028, "vx":-2.45832, "vy":1.27343, "omega":0.82205, "ax":-4.90593, "ay":-0.6689, "alpha":0.60996, "fx":[-80.51385,-80.74256,-80.01002,-79.54392], "fy":[-8.83577,-6.74765,-12.82921,-15.32864]}, - {"t":0.71471, "x":4.30065, "y":6.02073, "heading":-1.7589, "vx":-2.58588, "vy":1.25604, "omega":0.83791, "ax":-4.9042, "ay":-0.67296, "alpha":0.55931, "fx":[-80.46541,-80.67379,-79.98027,-79.57788], "fy":[-9.00031,-7.19633,-12.82359,-14.98653]}, - {"t":0.74071, "x":4.23175, "y":6.05316, "heading":-1.73712, "vx":-2.7134, "vy":1.23854, "omega":0.85245, "ax":-4.90204, "ay":-0.67779, "alpha":0.49844, "fx":[-80.4031,-80.58887,-79.94958,-79.61445], "fy":[-9.23116,-7.72008,-12.7823,-14.5885]}, - {"t":0.76671, "x":4.15954, "y":6.08514, "heading":-1.71495, "vx":-2.84086, "vy":1.22092, "omega":0.86541, "ax":-4.89927, "ay":-0.68361, "alpha":0.42392, "fx":[-80.32187,-80.48142,-79.91767,-79.65367], "fy":[-9.54967,-8.34341,-12.69156,-14.11821]}, - {"t":0.79271, "x":4.08402, "y":6.11665, "heading":-1.69245, "vx":-2.96825, "vy":1.20314, "omega":0.87643, "ax":-4.89559, "ay":-0.69079, "alpha":0.33053, "fx":[-80.21387,-80.34121,-79.88395,-79.69537], "fy":[-9.98695,-9.10376,-12.53032,-13.55142]}, - {"t":0.81871, "x":4.00518, "y":6.1477, "heading":-1.66966, "vx":-3.09554, "vy":1.18518, "omega":0.88503, "ax":-4.89053, "ay":-0.69988, "alpha":0.21002, "fx":[-80.06634,-80.15083,-79.84714,-79.73886], "fy":[-10.59031,-10.06134,-12.26445,-12.85056]}, - {"t":0.84471, "x":3.92304, "y":6.17828, "heading":-1.64665, "vx":-3.22271, "vy":1.16698, "omega":0.89049, "ax":-4.88316, "ay":-0.71176, "alpha":0.04846, "fx":[-79.85713,-79.8781,-79.80434,-79.78216], "fy":[-11.43564,-11.31948,-11.83475,-11.95402]}, - {"t":0.87072, "x":3.83759, "y":6.20838, "heading":-1.62349, "vx":-3.34968, "vy":1.14847, "omega":0.89175, "ax":-4.87166, "ay":-0.728, "alpha":-0.17977, "fx":[-79.54453,-79.457,-79.74822,-79.81969], "fy":[-12.65283,-13.07129,-11.12824,-10.75327]}, - {"t":0.89672, "x":3.74885, "y":6.238, "heading":-1.60031, "vx":-3.47635, "vy":1.12955, "omega":0.88707, "ax":-4.85169, "ay":-0.75153, "alpha":-0.52755, "fx":[-79.04096,-78.73041,-79.65728,-79.83508], "fy":[-14.48394,-15.72238,-9.89944,-9.03839]}, - {"t":0.92272, "x":3.65682, "y":6.26712, "heading":-1.57724, "vx":-3.6025, "vy":1.11, "omega":0.87336, "ax":-4.81075, "ay":-0.78846, "alpha":-1.12562, "fx":[-78.1324,-77.22984,-79.45065,-79.77332], "fy":[-17.43385,-20.28151,-7.49744,-6.34674]}, - {"t":0.94872, "x":3.56152, "y":6.29571, "heading":-1.55453, "vx":-3.72759, "vy":1.0895, "omega":0.84409, "ax":-4.69669, "ay":-0.8509, "alpha":-2.41319, "fx":[-76.16715,-72.91742,-78.64439,-79.39866], "fy":[-22.71682,-29.98175,-1.49584,-1.44791]}, - {"t":0.97472, "x":3.46301, "y":6.32375, "heading":-1.53259, "vx":-3.84971, "vy":1.06738, "omega":0.78134, "ax":-4.07994, "ay":-0.83818, "alpha":-6.88791, "fx":[-70.68864,-49.97906,-68.95087,-77.17863], "fy":[-32.97377,-56.24612,24.83469,9.57478]}, - {"t":1.00073, "x":3.36153, "y":6.35122, "heading":-1.51227, "vx":-3.9558, "vy":1.04558, "omega":0.60224, "ax":-1.12338, "ay":2.14843, "alpha":-4.4657, "fx":[-35.44562,-3.78429,-4.39807,-29.83246], "fy":[21.67116,28.0251,48.19665,42.59822]}, - {"t":1.02673, "x":3.2583, "y":6.37914, "heading":-1.49661, "vx":-3.98501, "vy":1.10145, "omega":0.48613, "ax":3.74498, "ay":1.64454, "alpha":7.06948, "fx":[64.07807,33.2155,70.22952,77.36984], "fy":[44.31708,67.89512,-6.10466,1.43313]}, - {"t":1.05273, "x":3.15594, "y":6.40833, "heading":-1.48397, "vx":-3.88763, "vy":1.14421, "omega":0.66995, "ax":4.46621, "ay":1.39193, "alpha":3.63244, "fx":[71.83288,63.14381,77.93148,79.14809], "fy":[33.99124,46.71442,4.70436,5.61128]}, - {"t":1.07873, "x":3.05637, "y":6.43855, "heading":-1.46655, "vx":-3.7715, "vy":1.1804, "omega":0.76439, "ax":4.70867, "ay":1.15092, "alpha":1.78638, "fx":[75.72536,73.70379,79.03698,79.44484], "fy":[25.89989,30.31046,9.65417,9.39683]}, - {"t":1.10473, "x":2.9599, "y":6.46964, "heading":-1.44667, "vx":-3.64907, "vy":1.21033, "omega":0.81084, "ax":4.79233, "ay":1.01786, "alpha":0.95236, "fx":[77.54916,76.98915,79.33568,79.50805], "fy":[20.98664,22.46845,11.64978,11.45559]}, - {"t":1.13073, "x":2.86663, "y":6.50145, "heading":-1.42559, "vx":-3.52446, "vy":1.23679, "omega":0.83561, "ax":4.83141, "ay":0.93758, "alpha":0.4817, "fx":[78.55497,78.40506,79.45516,79.52219], "fy":[17.72929,18.13558,12.75447,12.69095]}, - {"t":1.15674, "x":2.77663, "y":6.53393, "heading":-1.40386, "vx":-3.39883, "vy":1.26117, "omega":0.84813, "ax":4.85323, "ay":0.88428, "alpha":0.17879, "fx":[79.17727,79.15067,79.50961,79.52678], "fy":[15.40519,15.45257,13.48316,13.48395]}, - {"t":1.18274, "x":2.68989, "y":6.56702, "heading":-1.38181, "vx":-3.27264, "vy":1.28416, "omega":0.85278, "ax":4.86687, "ay":0.84637, "alpha":-0.03292, "fx":[79.59467,79.59609,79.53371,79.53203], "fy":[13.65324,13.66069,14.01909,14.01329]}, - {"t":1.20874, "x":2.60644, "y":6.60069, "heading":-1.35964, "vx":-3.14609, "vy":1.30617, "omega":0.85192, "ax":4.87609, "ay":0.81804, "alpha":-0.18943, "fx":[79.89155,79.88569,79.54139,79.54054], "fy":[12.27667,12.40132,14.44274,14.37288]}, - {"t":1.23474, "x":2.52629, "y":6.63493, "heading":-1.33749, "vx":-3.01931, "vy":1.32744, "omega":0.847, "ax":4.88268, "ay":0.79606, "alpha":-0.30995, "fx":[80.11221,80.08555,79.53933,79.55271], "fy":[11.16006,11.48454,14.79467,14.61671]}, - {"t":1.26074, "x":2.44943, "y":6.66972, "heading":-1.31546, "vx":-2.89235, "vy":1.34814, "omega":0.83894, "ax":4.88759, "ay":0.7785, "alpha":-0.40565, "fx":[80.28191,80.22964,79.53113,79.56822], "fy":[10.23149,10.80071,15.09742,14.77802]}, - {"t":1.28674, "x":2.37588, "y":6.70503, "heading":-1.29365, "vx":-2.76526, "vy":1.36838, "omega":0.82839, "ax":4.89137, "ay":0.76414, "alpha":-0.48353, "fx":[80.41596,80.33696,79.51894,79.5866], "fy":[9.4439,10.28216,15.36442,14.8785]}, - {"t":1.31275, "x":2.30563, "y":6.74087, "heading":-1.27211, "vx":-2.63808, "vy":1.38825, "omega":0.81582, "ax":4.89437, "ay":0.75218, "alpha":-0.54814, "fx":[80.52416,80.4189,79.50411,79.60735], "fy":[8.76523,9.88483,15.60417,14.93292]}, - {"t":1.33875, "x":2.23869, "y":6.77722, "heading":-1.2509, "vx":-2.51082, "vy":1.40781, "omega":0.80157, "ax":4.8968, "ay":0.74207, "alpha":-0.60262, "fx":[80.61304,80.48265,79.48753,79.63001], "fy":[8.17289,9.57881,15.82228,14.95175]}, - {"t":1.36475, "x":2.17506, "y":6.81408, "heading":-1.23005, "vx":-2.38349, "vy":1.4271, "omega":0.7859, "ax":4.8988, "ay":0.7334, "alpha":-0.64918, "fx":[80.68714,80.53295,79.46984,79.65418], "fy":[7.65044,9.34303,16.02262,14.94268]}, - {"t":1.39075, "x":2.11474, "y":6.85144, "heading":-1.20962, "vx":-2.25612, "vy":1.44617, "omega":0.76902, "ax":4.90048, "ay":0.72588, "alpha":-0.68942, "fx":[80.74967,80.57308,79.4515,79.67951], "fy":[7.18568,9.16221,16.20794,14.91147]}, - {"t":1.41675, "x":2.05773, "y":6.88928, "heading":-1.18962, "vx":-2.1287, "vy":1.46505, "omega":0.75109, "ax":4.9019, "ay":0.71931, "alpha":-0.72456, "fx":[80.80298,80.60532,79.43282,79.7057], "fy":[6.76929,9.02498,16.38026,14.86262]}, - {"t":1.44275, "x":2.00404, "y":6.92762, "heading":-1.17009, "vx":-2.00124, "vy":1.48375, "omega":0.73225, "ax":4.90312, "ay":0.7135, "alpha":-0.75549, "fx":[80.84885,80.63136,79.41407,79.73248], "fy":[6.39402,8.92268,16.54111,14.79965]}, - {"t":1.46876, "x":1.95366, "y":6.96644, "heading":-1.15105, "vx":-1.87375, "vy":1.5023, "omega":0.71261, "ax":4.90418, "ay":0.70834, "alpha":-0.78293, "fx":[80.88862,80.65243,79.39543,79.75963], "fy":[6.05413,8.84859,16.69168,14.72542]}, - {"t":1.49476, "x":1.9066, "y":7.00574, "heading":-1.13252, "vx":-1.74623, "vy":1.52072, "omega":0.69225, "ax":4.92568, "ay":0.51763, "alpha":-0.83829, "fx":[81.06028,80.90712,79.87067,80.2637], "fy":[2.35383,5.8573,14.13291,11.5052]}, - {"t":1.51827, "x":1.86691, "y":7.04164, "heading":-1.11625, "vx":-1.63043, "vy":1.53289, "omega":0.67254, "ax":4.94899, "ay":0.12044, "alpha":-0.93109, "fx":[80.92809,81.11373,80.64728,80.93736], "fy":[-5.09118,-0.30934,8.59135,4.6848]}, - {"t":1.54178, "x":1.82994, "y":7.07771, "heading":-1.10044, "vx":-1.51409, "vy":1.53572, "omega":0.65065, "ax":4.93892, "ay":-0.30012, "alpha":-1.02285, "fx":[80.06361,80.8351,81.0534,81.01566], "fy":[-12.82127,-6.68418,2.64218,-2.76224]}, - {"t":1.56528, "x":1.79571, "y":7.11373, "heading":-1.08514, "vx":-1.39798, "vy":1.52867, "omega":0.62661, "ax":4.89048, "ay":-0.7364, "alpha":-1.11126, "fx":[78.41402,80.03484,81.00769,80.34349], "fy":[-20.62719,-13.15661,-3.65867,-10.71243]}, - {"t":1.58879, "x":1.7642, "y":7.14946, "heading":-1.07041, "vx":-1.283, "vy":1.51135, "omega":0.60048, "ax":4.80074, "ay":-1.17859, "alpha":-1.19429, "fx":[75.99144,78.70392,80.43819,78.79847], "fy":[-28.2776,-19.60318,-10.22424,-18.96608]}, - {"t":1.6123, "x":1.73536, "y":7.18467, "heading":-1.0563, "vx":-1.17014, "vy":1.48365, "omega":0.57241, "ax":4.6696, "ay":-1.61561, "alpha":-1.27032, "fx":[72.87589,76.86361,79.29308,76.324], "fy":[-35.5515,-25.89866,-16.93818,-27.26048]}, - {"t":1.63581, "x":1.70915, "y":7.2191, "heading":-1.04284, "vx":-1.06036, "vy":1.44566, "omega":0.54254, "ax":4.50008, "ay":-2.03637, "alpha":-1.33795, "fx":[69.20246,74.56383,77.55094,72.95342], "fy":[-42.26896,-31.92783,-23.66197,-35.30446]}, - {"t":1.65932, "x":1.68546, "y":7.25253, "heading":-1.03008, "vx":-0.95457, "vy":1.39779, "omega":0.51109, "ax":4.29797, "ay":-2.43117, "alpha":-1.39582, "fx":[65.13715,71.87716,75.22747,68.81264], "fy":[-48.31079,-37.59561,-30.24751,-42.82613]}, - {"t":1.68283, "x":1.66421, "y":7.28471, "heading":-1.01807, "vx":-0.85353, "vy":1.34064, "omega":0.47827, "ax":4.07103, "ay":-2.79277, "alpha":-1.44264, "fx":[60.85,68.89005,72.37635,64.09781], "fy":[-53.62285,-42.83341,-36.55264,-49.61703]}, - {"t":1.70634, "x":1.64527, "y":7.31546, "heading":-1.00682, "vx":-0.75782, "vy":1.27498, "omega":0.44436, "ax":3.82777, "ay":-3.1169, "alpha":-1.47751, "fx":[56.49354,65.6933,69.08349,59.03637], "fy":[-58.20743,-47.60103,-42.45623,-55.55697]}, - {"t":1.72985, "x":1.62851, "y":7.34457, "heading":-0.99638, "vx":-0.66783, "vy":1.2017, "omega":0.40962, "ax":3.57637, "ay":-3.40216, "alpha":-1.50027, "fx":[52.19041,62.37373,65.45598,53.84738], "fy":[-62.10757,-51.88465,-47.86906,-60.61419]}, - {"t":1.75336, "x":1.6138, "y":7.37188, "heading":-0.98675, "vx":-0.58376, "vy":1.12172, "omega":0.37435, "ax":3.32394, "ay":-3.64948, "alpha":-1.51165, "fx":[48.02979,59.00833,61.6088,48.71338], "fy":[-65.39045,-55.69218,-52.73857,-64.8269]}, - {"t":1.77687, "x":1.60099, "y":7.39725, "heading":-0.97795, "vx":-0.50561, "vy":1.03593, "omega":0.33881, "ax":3.07607, "ay":-3.8614, "alpha":-1.51313, "fx":[44.06997,55.66097,57.65251,43.76824], "fy":[-68.13348,-59.04744,-57.04739,-68.27754]}, - {"t":1.80038, "x":1.58996, "y":7.42053, "heading":-0.96998, "vx":-0.4333, "vy":0.94515, "omega":0.30324, "ax":2.83684, "ay":-4.04138, "alpha":-1.50658, "fx":[40.34388,52.38147,53.68422,39.09794], "fy":[-70.41459,-61.98451,-60.80736,-71.06886]}, - {"t":1.82389, "x":1.58055, "y":7.44164, "heading":-0.96285, "vx":-0.3666, "vy":0.85014, "omega":0.26782, "ax":2.60891, "ay":-4.19326, "alpha":-1.49398, "fx":[36.86549,49.20635,49.78269,34.74862], "fy":[-72.30644,-64.54287,-64.0514,-73.30661]}, - {"t":1.8474, "x":1.57266, "y":7.46046, "heading":-0.95656, "vx":-0.30527, "vy":0.75156, "omega":0.2327, "ax":2.39386, "ay":-4.32087, "alpha":-1.47712, "fx":[33.63552,46.16047,46.00701,30.73689], "fy":[-73.87356,-66.76364,-66.82545,-75.08911]}, - {"t":1.8709, "x":1.56614, "y":7.47694, "heading":-0.95109, "vx":-0.24899, "vy":0.64998, "omega":0.19798, "ax":2.19235, "ay":-4.42777, "alpha":-1.45749, "fx":[30.64611,43.25913,42.39796,27.05954], "fy":[-75.17148,-68.68706,-69.18147,-76.50238]}, - {"t":1.89441, "x":1.56089, "y":7.49099, "heading":-0.94643, "vx":-0.19745, "vy":0.54588, "omega":0.16371, "ax":2.00446, "ay":-4.51718, "alpha":-1.43629, "fx":[27.88427,40.51013,38.98072,23.70117], "fy":[-76.24699,-70.35089,-71.17242,-77.6188]}, - {"t":1.91792, "x":1.5568, "y":7.50258, "heading":-0.94258, "vx":-0.15033, "vy":0.43969, "omega":0.12994, "ax":1.82984, "ay":-4.5919, "alpha":-1.41439, "fx":[25.33432,37.91564,35.76814,20.63978], "fy":[-77.13903,-71.78948,-72.84882,-78.49779]}, - {"t":1.94143, "x":1.55378, "y":7.51165, "heading":-0.93953, "vx":-0.10731, "vy":0.33174, "omega":0.09669, "ax":1.6679, "ay":-4.65432, "alpha":-1.39241, "fx":[22.97955,35.47383,32.76386,17.85053], "fy":[-77.87967,-73.0335,-74.25689,-79.18732]}, - {"t":1.96494, "x":1.55171, "y":7.51816, "heading":-0.93726, "vx":-0.0681, "vy":0.22232, "omega":0.06396, "ax":1.51786, "ay":-4.70649, "alpha":-1.37079, "fx":[20.80325,33.18009,29.965,15.30806], "fy":[-78.49523,-74.10991,-75.43767,-79.72581]}, - {"t":1.98845, "x":1.55053, "y":7.52209, "heading":-0.93575, "vx":-0.03242, "vy":0.11167, "omega":0.03173, "ax":1.3789, "ay":-4.75009, "alpha":-1.3498, "fx":[18.78937,31.028,27.36432,12.9879], "fy":[-79.00722,-75.04214,-76.42683,-80.14379]}, - {"t":2.01196, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.16523, "ay":5.29814, "alpha":1.39064, "fx":[7.45874,-5.01736,-13.43147,0.18531], "fy":[86.67692,86.86406,85.94588,86.97084]}, + {"t":0.02312, "x":5.00418, "y":5.24533, "heading":-2.09512, "vx":-0.00382, "vy":0.1225, "omega":0.03215, "ax":-0.38503, "ay":5.28492, "alpha":1.4295, "fx":[4.29601,-8.28362,-17.35529,-3.83495], "fy":[86.88142,86.60582,85.23127,86.87468]}, + {"t":0.04624, "x":5.00399, "y":5.24958, "heading":-2.09437, "vx":-0.01272, "vy":0.2447, "omega":0.06521, "ax":-0.63178, "ay":5.25917, "alpha":1.47017, "fx":[0.69455,-11.92542,-21.68473,-8.39769], "fy":[86.97407,86.17136,84.22423,86.53988]}, + {"t":0.06936, "x":5.00352, "y":5.25664, "heading":-2.09287, "vx":-0.02733, "vy":0.3663, "omega":0.0992, "ax":-0.9088, "ay":5.21631, "alpha":1.51228, "fx":[-3.41903,-15.98637,-26.44684,-13.57657], "fy":[86.89774,85.50186,82.84195,85.86559]}, + {"t":0.09249, "x":5.00265, "y":5.26651, "heading":-2.09057, "vx":-0.04834, "vy":0.48691, "omega":0.13417, "ax":-1.21923, "ay":5.15032, "alpha":1.55519, "fx":[-8.12616,-20.50846,-31.65513,-19.43893], "fy":[86.57134,84.52118,80.9838,84.71548]}, + {"t":0.11561, "x":5.0012, "y":5.27914, "heading":-2.08747, "vx":-0.07653, "vy":0.60599, "omega":0.17012, "ax":-1.56543, "ay":5.05345, "alpha":1.59796, "fx":[-13.51154,-25.52639,-37.29972,-26.02959], "fy":[85.8812,83.13252,78.53222,82.91113]}, + {"t":0.13873, "x":4.99902, "y":5.2945, "heading":-2.08354, "vx":-0.11273, "vy":0.72284, "omega":0.20707, "ax":-1.94809, "ay":4.91613, "alpha":1.63914, "fx":[-19.65231,-31.05855,-43.33462,-33.3446], "fy":[84.67136,81.21625,75.35817,80.23137]}, + {"t":0.16185, "x":4.99589, "y":5.31253, "heading":-2.07875, "vx":-0.15777, "vy":0.8365, "omega":0.24497, "ax":-2.36495, "ay":4.72725, "alpha":1.67653, "fx":[-26.59775,-37.09375,-49.66396,-41.29432], "fy":[82.73555,78.63092,71.3336,76.4259]}, + {"t":0.18497, "x":4.99161, "y":5.33314, "heading":-2.07308, "vx":-0.21245, "vy":0.94581, "omega":0.28373, "ax":-2.80925, "ay":4.47522, "alpha":1.70672, "fx":[-34.33549,-43.57397,-56.13108,-49.66303], "fy":[79.8172,75.22068,66.353,71.25439]}, + {"t":0.20809, "x":4.98595, "y":5.3562, "heading":-2.06652, "vx":-0.27741, "vy":1.04928, "omega":0.3232, "ax":-3.26831, "ay":4.15008, "alpha":1.72454, "fx":[-42.7447,-50.37564,-62.51731,-58.08519], "fy":[75.62941,70.83307,60.36322,64.55764]}, + {"t":0.23122, "x":4.97866, "y":5.38157, "heading":-2.05905, "vx":-0.35298, "vy":1.14524, "omega":0.36307, "ax":-3.72323, "ay":3.74665, "alpha":1.72315, "fx":[-51.54738,-57.29618,-68.55797,-66.06962], "fy":[69.9094,65.34975,53.39519,56.34819]}, + {"t":0.25434, "x":4.9695, "y":5.40905, "heading":-2.05066, "vx":-0.43906, "vy":1.23187, "omega":0.40291, "ax":-4.15058, "ay":3.26802, "alpha":1.69555, "fx":[-60.28818,-64.05639,-73.97854,-73.09297], "fy":[62.51356,58.72813,45.58517,46.87655]}, + {"t":0.27746, "x":4.95824, "y":5.43841, "heading":-2.04134, "vx":-0.53503, "vy":1.30743, "omega":0.44212, "ax":-4.52648, "ay":2.7275, "alpha":1.6377, "fx":[-68.38471,-70.32913,-78.54434,-78.7392], "fy":[53.5273,51.04196,37.17169,36.61704]}, + {"t":0.30058, "x":4.94466, "y":5.46937, "heading":-2.03112, "vx":-0.63969, "vy":1.37049, "omega":0.47998, "ax":-4.83206, "ay":2.14771, "alpha":1.5513, "fx":[-75.26486,-75.79561,-82.1064,-82.81329], "fy":[43.32332,42.5007,28.46252,26.15712]}, + {"t":0.3237, "x":4.92858, "y":5.50163, "heading":-2.02002, "vx":-0.75142, "vy":1.42015, "omega":0.51585, "ax":-5.05778, "ay":1.55612, "alpha":1.44356, "fx":[-80.5381,-80.21309,-84.62485,-85.36409], "fy":[32.50252,33.42972,19.78103,16.04515]}, + {"t":0.34682, "x":4.90985, "y":5.53488, "heading":-2.00809, "vx":-0.86836, "vy":1.45613, "omega":0.54923, "ax":-5.20463, "ay":0.97917, "alpha":1.32398, "fx":[-84.09881,-83.46522,-86.16172,-86.61715], "fy":[21.72853,24.21123,11.41235,6.67838]}, + {"t":0.36995, "x":4.88838, "y":5.56881, "heading":-1.99539, "vx":-0.9887, "vy":1.47877, "omega":0.57984, "ax":-5.28196, "ay":0.43736, "alpha":1.20101, "fx":[-86.10345,-85.57263,-86.85058,-86.87335], "fy":[11.55202,15.2094,3.56797,-1.72935]}, + {"t":0.39307, "x":4.86411, "y":5.60312, "heading":-1.98199, "vx":-1.11083, "vy":1.48888, "omega":0.60761, "ax":-5.30353, "ay":-0.0568, "alpha":1.08055, "fx":[-86.86022,-86.664,-86.85863,-86.42731], "fy":[2.31739,6.71079,-3.62467,-9.11799]}, + {"t":0.41619, "x":4.83701, "y":5.63753, "heading":-1.96794, "vx":-1.23345, "vy":1.48757, "omega":0.6326, "ax":-5.28377, "ay":-0.49818, "alpha":0.96612, "fx":[-86.71175,-86.92717,-86.35475,-85.52444], "fy":[-5.83385,-1.10174,-10.11293,-15.52836]}, + {"t":0.43931, "x":4.80708, "y":5.67179, "heading":-1.95331, "vx":-1.35562, "vy":1.47605, "omega":0.65493, "ax":-5.26396, "ay":-0.69983, "alpha":0.90056, "fx":[-86.42101,-86.82689,-85.97304,-85.00198], "fy":[-9.48599,-4.83007,-13.13303,-18.3142]}, + {"t":0.46575, "x":4.7694, "y":5.71057, "heading":-1.936, "vx":-1.49479, "vy":1.45755, "omega":0.67874, "ax":-5.26347, "ay":-0.70113, "alpha":0.88439, "fx":[-86.41724,-86.80698,-85.94792,-85.01841], "fy":[-9.42218,-5.00749,-13.23225,-18.18689]}, + {"t":0.49219, "x":4.72804, "y":5.74886, "heading":-1.91805, "vx":-1.63394, "vy":1.43901, "omega":0.70212, "ax":-5.26291, "ay":-0.70258, "alpha":0.86645, "fx":[-86.41136,-86.7846,-85.92179,-85.03638], "fy":[-9.36653,-5.20146,-13.32977,-18.04566]}, + {"t":0.51862, "x":4.683, "y":5.78666, "heading":-1.89949, "vx":-1.77308, "vy":1.42044, "omega":0.72503, "ax":-5.26228, "ay":-0.70419, "alpha":0.8464, "fx":[-86.40292,-86.75926,-85.89465,-85.05601], "fy":[-9.32151,-5.41424,-13.42425,-17.88858]}, + {"t":0.54506, "x":4.63429, "y":5.82397, "heading":-1.88032, "vx":-1.91221, "vy":1.40182, "omega":0.74741, "ax":-5.26156, "ay":-0.70599, "alpha":0.82386, "fx":[-86.39136,-86.7304,-85.86649,-85.07742], "fy":[-9.29015,-5.64863,-13.51408,-17.71334]}, + {"t":0.5715, "x":4.58189, "y":5.86078, "heading":-1.86056, "vx":-2.05131, "vy":1.38316, "omega":0.76919, "ax":-5.26072, "ay":-0.70801, "alpha":0.79833, "fx":[-86.37597,-86.69725,-85.83731,-85.10076], "fy":[-9.27614,-5.90815,-13.59722,-17.51711]}, + {"t":0.59794, "x":4.52582, "y":5.8971, "heading":-1.84023, "vx":-2.19039, "vy":1.36444, "omega":0.79029, "ax":-5.25975, "ay":-0.71031, "alpha":0.76914, "fx":[-86.35583,-86.65883,-85.80709,-85.12615], "fy":[-9.28415,-6.19727,-13.67109,-17.29634]}, + {"t":0.62438, "x":4.46607, "y":5.93292, "heading":-1.81933, "vx":-2.32945, "vy":1.34566, "omega":0.81063, "ax":-5.25861, "ay":-0.71294, "alpha":0.73547, "fx":[-86.32976,-86.61383,-85.7758,-85.15375], "fy":[-9.32011,-6.52183,-13.73233,-17.0465]}, + {"t":0.65081, "x":4.40265, "y":5.96825, "heading":-1.7979, "vx":-2.46847, "vy":1.32681, "omega":0.83007, "ax":-5.25724, "ay":-0.71598, "alpha":0.69617, "fx":[-86.29616,-86.56044,-85.74339,-85.18372], "fy":[-9.39173,-6.8896,-13.77646,-16.76168]}, + {"t":0.67725, "x":4.33555, "y":6.00308, "heading":-1.77596, "vx":-2.60746, "vy":1.30788, "omega":0.84848, "ax":-5.25558, "ay":-0.71953, "alpha":0.6497, "fx":[-86.25281,-86.49612,-85.70976,-85.2162], "fy":[-9.50928,-7.31123,-13.79739,-16.43394]}, + {"t":0.70369, "x":4.26478, "y":6.03741, "heading":-1.75352, "vx":-2.74641, "vy":1.28886, "omega":0.86565, "ax":-5.25351, "ay":-0.72374, "alpha":0.59389, "fx":[-86.19659,-86.41717,-85.67472,-85.25134], "fy":[-9.68675,-7.80174,-13.78656,-16.05236]}, + {"t":0.73013, "x":4.19034, "y":6.07123, "heading":-1.73064, "vx":-2.8853, "vy":1.26972, "omega":0.88136, "ax":-5.25089, "ay":-0.72883, "alpha":0.52558, "fx":[-86.12285,-86.318,-85.63793,-85.2892], "fy":[-9.94387,-8.38304,-13.73147,-15.60136]}, + {"t":0.75656, "x":4.11222, "y":6.10454, "heading":-1.70734, "vx":-3.02412, "vy":1.25046, "omega":0.89525, "ax":-5.24744, "ay":-0.73508, "alpha":0.44005, "fx":[-86.02446,-86.18971,-85.59872,-85.3297], "fy":[-10.30943,-9.0885,-13.61307,-15.05782]}, + {"t":0.783, "x":4.03044, "y":6.13734, "heading":-1.68367, "vx":-3.16285, "vy":1.23102, "omega":0.90689, "ax":-5.24274, "ay":-0.74298, "alpha":0.32976, "fx":[-85.88987,-86.01729,-85.55578,-85.37235], "fy":[-10.82732,-9.97159,-13.4007,-14.38586]}, + {"t":0.80944, "x":3.94498, "y":6.16963, "heading":-1.65969, "vx":-3.30146, "vy":1.21138, "omega":0.9156, "ax":-5.236, "ay":-0.7533, "alpha":0.18208, "fx":[-85.69908,-85.7734,-85.50627,-85.41559], "fy":[-11.56821,-11.1237,-13.04144,-13.52644]}, + {"t":0.83588, "x":3.85587, "y":6.20139, "heading":-1.63549, "vx":-3.43989, "vy":1.19146, "omega":0.92042, "ax":-5.22562, "ay":-0.76735, "alpha":-0.02615, "fx":[-85.41459,-85.40284,-85.44345,-85.45488], "fy":[-12.65391,-12.71489,-12.43519,-12.37507]}, + {"t":0.86231, "x":3.7631, "y":6.23262, "heading":-1.61115, "vx":-3.57804, "vy":1.17118, "omega":0.91973, "ax":-5.20794, "ay":-0.7877, "alpha":-0.34249, "fx":[-84.95764,-84.77676,-85.34866,-85.4766], "fy":[-14.31405,-15.10094,-11.36667,-10.72797]}, + {"t":0.88875, "x":3.66669, "y":6.26331, "heading":-1.58684, "vx":-3.71573, "vy":1.15035, "omega":0.91067, "ax":-5.17253, "ay":-0.81976, "alpha":-0.88337, "fx":[-84.13459,-83.52007,-85.15571,-85.43389], "fy":[-17.03784,-19.16549,-9.27336,-8.12929]}, + {"t":0.91519, "x":3.56664, "y":6.29344, "heading":-1.56276, "vx":-3.85248, "vy":1.12868, "omega":0.88732, "ax":-5.07694, "ay":-0.87592, "alpha":-2.03507, "fx":[-82.33995,-80.03839,-84.49641,-85.11846], "fy":[-22.0632,-27.7924,-4.09812,-3.3247]}, + {"t":0.94163, "x":3.46302, "y":6.32297, "heading":-1.5393, "vx":-3.9867, "vy":1.10552, "omega":0.83351, "ax":-4.53984, "ay":-0.91054, "alpha":-6.24542, "fx":[-76.83341,-59.96197,-77.06635,-83.00894], "fy":[-33.09589,-54.55347,19.68172,8.425]}, + {"t":0.96807, "x":3.35603, "y":6.35188, "heading":-1.51727, "vx":-4.10672, "vy":1.08145, "omega":0.6684, "ax":-0.46443, "ay":2.63676, "alpha":-3.22757, "fx":[-19.92837,4.48023,2.58014,-17.50187], "fy":[35.23941,37.73549,50.74799,48.70139]}, + {"t":0.9945, "x":3.2473, "y":6.38139, "heading":-1.49959, "vx":-4.119, "vy":1.15116, "omega":0.58307, "ax":4.17725, "ay":1.74527, "alpha":6.86193, "fx":[70.24556,42.04698,77.59264,83.27536], "fy":[45.63929,69.836,-3.49805,2.15002]}, + {"t":1.02094, "x":3.13986, "y":6.41244, "heading":-1.48418, "vx":-4.00856, "vy":1.1973, "omega":0.76448, "ax":4.91902, "ay":1.37684, "alpha":2.78348, "fx":[78.98037,74.05017,83.87533,84.76066], "fy":[32.11366,40.83872,8.71514,8.36756]}, + {"t":1.04738, "x":3.0356, "y":6.44457, "heading":-1.46397, "vx":-3.87852, "vy":1.2337, "omega":0.83807, "ax":5.09652, "ay":1.14968, "alpha":1.30255, "fx":[82.30458,81.24616,84.70761,85.01556], "fy":[24.37327,26.97418,12.13474,11.69824]}, + {"t":1.07382, "x":2.93484, "y":6.47759, "heading":-1.44181, "vx":-3.74378, "vy":1.2641, "omega":0.87251, "ax":5.16088, "ay":1.03116, "alpha":0.61517, "fx":[83.83776,83.58494,84.97266,85.08669], "fy":[19.81963,20.5182,13.63854,13.4535]}, + {"t":1.10025, "x":2.83767, "y":6.51137, "heading":-1.41874, "vx":-3.60733, "vy":1.29136, "omega":0.88877, "ax":5.19226, "ay":0.95983, "alpha":0.2185, "fx":[84.68669,84.64241,85.08757,85.11743], "fy":[16.82605,16.92956,14.52331,14.48645]}, + {"t":1.12669, "x":2.74411, "y":6.54585, "heading":-1.39525, "vx":-3.47006, "vy":1.31673, "omega":0.89455, "ax":5.21035, "ay":0.91232, "alpha":-0.04032, "fx":[85.21619,85.2193,85.14251,85.139], "fy":[14.69342,14.69617,15.13468,15.13423]}, + {"t":1.15313, "x":2.6542, "y":6.58098, "heading":-1.3716, "vx":-3.33231, "vy":1.34085, "omega":0.89348, "ax":5.22194, "ay":0.8784, "alpha":-0.22281, "fx":[85.57445,85.57288,85.1678,85.15981], "fy":[13.084,13.20304,15.60116,15.55279]}, + {"t":1.17957, "x":2.56792, "y":6.61673, "heading":-1.34798, "vx":-3.19426, "vy":1.36408, "omega":0.88759, "ax":5.22992, "ay":0.85298, "alpha":-0.35855, "fx":[85.83139,85.8071,85.1765,85.18222], "fy":[11.81668,12.15679,15.98083,15.82379]}, + {"t":1.20601, "x":2.4853, "y":6.65309, "heading":-1.32451, "vx":-3.05599, "vy":1.38663, "omega":0.87811, "ax":5.23573, "ay":0.83319, "alpha":-0.46351, "fx":[86.02383,85.97092,85.17511,85.20676], "fy":[10.78616,11.40036,16.30354,15.99417]}, + {"t":1.23244, "x":2.40634, "y":6.69004, "heading":-1.30129, "vx":-2.91757, "vy":1.40865, "omega":0.86586, "ax":5.24011, "ay":0.81735, "alpha":-0.54715, "fx":[86.17283,86.09011,85.16732,85.23332], "fy":[9.92713,10.8422,16.58611,16.09291]}, + {"t":1.25888, "x":2.33103, "y":6.72757, "heading":-1.2784, "vx":-2.77903, "vy":1.43026, "omega":0.85139, "ax":5.24354, "ay":0.80437, "alpha":-0.61536, "fx":[86.29126,86.17937,85.15532,85.2616], "fy":[9.197,10.42531,16.83874,16.13892]}, + {"t":1.28532, "x":2.25939, "y":6.76567, "heading":-1.25589, "vx":-2.6404, "vy":1.45153, "omega":0.83512, "ax":5.24628, "ay":0.79355, "alpha":-0.67207, "fx":[86.38737,86.24768,85.14055,85.29126], "fy":[8.56677,10.11231,17.06789,16.14518]}, + {"t":1.31176, "x":2.19142, "y":6.80432, "heading":-1.23382, "vx":-2.5017, "vy":1.47251, "omega":0.81736, "ax":5.24852, "ay":0.78438, "alpha":-0.71996, "fx":[86.46671,86.30079,85.12397,85.32198], "fy":[8.01601,9.87762,17.27793,16.12092]}, + {"t":1.33819, "x":2.12712, "y":6.84352, "heading":-1.21221, "vx":-2.36294, "vy":1.49325, "omega":0.79832, "ax":5.25039, "ay":0.77651, "alpha":-0.76094, "fx":[86.53314,86.34255,85.10625,85.35345], "fy":[7.52986,9.70314,17.47188,16.07295]}, + {"t":1.36463, "x":2.06648, "y":6.88327, "heading":-1.1911, "vx":-2.22414, "vy":1.51377, "omega":0.77821, "ax":5.25196, "ay":0.76968, "alpha":-0.7964, "fx":[86.58941,86.37564,85.08788,85.3854], "fy":[7.09723,9.57564,17.65192,16.00641]}, + {"t":1.39107, "x":2.00951, "y":6.92356, "heading":-1.17053, "vx":-2.08529, "vy":1.53412, "omega":0.75715, "ax":5.25331, "ay":0.7637, "alpha":-0.82738, "fx":[86.63757,86.40197,85.06923,85.4176], "fy":[6.70965,9.48525,17.81967,15.92532]}, + {"t":1.41751, "x":1.95622, "y":6.96439, "heading":-1.15051, "vx":-1.9464, "vy":1.55431, "omega":0.73528, "ax":5.25447, "ay":0.75841, "alpha":-0.85469, "fx":[86.67913,86.42295,85.05055,85.44983], "fy":[6.36051,9.42439,17.9764,15.83289]}, + {"t":1.44395, "x":1.9066, "y":7.00574, "heading":-1.13107, "vx":-1.80748, "vy":1.57436, "omega":0.71268, "ax":5.27843, "ay":0.54435, "alpha":-0.91419, "fx":[86.86122,86.70287,85.58754,86.01762], "fy":[2.22333,6.08214,15.09134,12.19922]}, + {"t":1.46774, "x":1.86509, "y":7.04336, "heading":-1.11411, "vx":-1.6819, "vy":1.58731, "omega":0.69093, "ax":5.30292, "ay":0.09702, "alpha":-1.01632, "fx":[86.66542,86.90686,86.44859,86.74989], "fy":[-6.14146,-0.84067,8.83778,4.48866]}, + {"t":1.49153, "x":1.82657, "y":7.08115, "heading":-1.09768, "vx":-1.55573, "vy":1.58962, "omega":0.66675, "ax":5.28768, "ay":-0.37719, "alpha":-1.11681, "fx":[85.60486,86.5382,86.86579,86.76513], "fy":[-14.81987,-8.00152,2.10504,-3.94907]}, + {"t":1.51532, "x":1.79106, "y":7.11886, "heading":-1.08181, "vx":-1.42993, "vy":1.58065, "omega":0.64018, "ax":5.22678, "ay":-0.86859, "alpha":-1.21288, "fx":[83.6231,85.55451,86.73864,85.87523], "fy":[-23.55173,-15.26311,-5.03399,-12.95059]}, + {"t":1.53911, "x":1.75851, "y":7.15622, "heading":-1.06658, "vx":-1.30557, "vy":1.55998, "omega":0.61132, "ax":5.11708, "ay":-1.36481, "alpha":-1.30218, "fx":[80.74802,83.94917,85.98047,83.94043], "fy":[-32.05397,-22.47281,-12.46584,-22.25537]}, + {"t":1.56291, "x":1.7289, "y":7.19295, "heading":-1.05204, "vx":-1.18383, "vy":1.52751, "omega":0.58034, "ax":4.95919, "ay":-1.85206, "alpha":-1.38283, "fx":[77.09247,81.75451,84.53329,80.913], "fy":[-40.06411,-29.47783,-20.03982,-31.52854]}, + {"t":1.5867, "x":1.70214, "y":7.22877, "heading":-1.03823, "vx":-1.06584, "vy":1.48345, "omega":0.54744, "ax":4.7577, "ay":-2.31697, "alpha":-1.45316, "fx":[72.8335,79.03867,82.38109,76.86435], "fy":[-47.37901,-36.14095,-27.57943,-40.41258]}, + {"t":1.61049, "x":1.67813, "y":7.26341, "heading":-1.0252, "vx":-0.95264, "vy":1.42832, "omega":0.51287, "ax":4.52059, "ay":-2.74837, "alpha":-1.51152, "fx":[68.17835,75.89649,79.55738,71.97964], "fy":[-53.87617,-42.35318,-34.90089,-48.59241]}, + {"t":1.63428, "x":1.65674, "y":7.29661, "heading":-1.013, "vx":-0.84509, "vy":1.36293, "omega":0.4769, "ax":4.25783, "ay":-3.13856, "alpha":-1.55649, "fx":[63.32998,72.43716,76.14363,66.51901], "fy":[-59.51402,-48.04084,-41.83475,-55.84836]}, + {"t":1.65807, "x":1.63784, "y":7.32815, "heading":-1.00166, "vx":-0.74379, "vy":1.28826, "omega":0.43987, "ax":3.9799, "ay":-3.48367, "alpha":-1.58732, "fx":[58.46211,68.77175,72.2586,60.76274], "fy":[-64.31641,-53.16641,-48.24498,-62.07765]}, + {"t":1.68187, "x":1.62127, "y":7.35782, "heading":-0.99119, "vx":-0.6491, "vy":1.20538, "omega":0.40211, "ax":3.69637, "ay":-3.7833, "alpha":-1.60437, "fx":[53.70743,65.00297,68.04148,54.96277], "fy":[-68.35059,-57.72437,-54.04076,-67.28315]}, + {"t":1.70566, "x":1.60687, "y":7.38542, "heading":-0.98162, "vx":-0.56115, "vy":1.11536, "omega":0.36394, "ax":3.41513, "ay":-4.03964, "alpha":-1.60906, "fx":[49.15672,61.21866,63.63387,49.31452], "fy":[-71.70617,-61.73411,-59.179,-71.5424]}, + {"t":1.72945, "x":1.59449, "y":7.41082, "heading":-0.97296, "vx":-0.4799, "vy":1.01925, "omega":0.32565, "ax":3.14208, "ay":-4.25651, "alpha":-1.60355, "fx":[44.86454,57.48891,59.16491,43.94989], "fy":[-74.4792,-65.23203,-63.65896,-74.97315]}, + {"t":1.75324, "x":1.58396, "y":7.43386, "heading":-0.96522, "vx":-0.40514, "vy":0.91798, "omega":0.2875, "ax":2.88122, "ay":-4.4385, "alpha":-1.59026, "fx":[40.85743,53.86596,54.74193,38.94442], "fy":[-76.76192,-68.26435,-67.5121,-77.70571]}, + {"t":1.77703, "x":1.57514, "y":7.45445, "heading":-0.95838, "vx":-0.33659, "vy":0.81238, "omega":0.24967, "ax":2.63495, "ay":-4.59036, "alpha":-1.57146, "fx":[37.14207,50.38599,50.44685,34.33053], "fy":[-78.6374,-70.88133,-70.79073,-79.86514]}, + {"t":1.80083, "x":1.56787, "y":7.47248, "heading":-0.95244, "vx":-0.2739, "vy":0.70317, "omega":0.21228, "ax":2.40444, "ay":-4.71662, "alpha":-1.54912, "fx":[33.71208,47.07178,46.33689,30.11108], "fy":[-80.17744,-73.13324,-73.55781,-81.56227]}, + {"t":1.82462, "x":1.56204, "y":7.48787, "heading":-0.94739, "vx":-0.2167, "vy":0.59095, "omega":0.17542, "ax":2.18997, "ay":-4.82136, "alpha":-1.52476, "fx":[30.55321,43.9355,42.44786,26.27063], "fy":[-81.44247,-75.06769,-75.87919,-82.89062]}, + {"t":1.84841, "x":1.5575, "y":7.50057, "heading":-0.94321, "vx":-0.16459, "vy":0.47624, "omega":0.13914, "ax":1.99122, "ay":-4.90815, "alpha":-1.49952, "fx":[27.64698,40.98146,38.7985,22.78366], "fy":[-82.48254,-76.72819,-77.81843,-83.92666]}, + {"t":1.8722, "x":1.55415, "y":7.51051, "heading":-0.9399, "vx":-0.11722, "vy":0.35947, "omega":0.10347, "ax":1.8075, "ay":-4.98006, "alpha":-1.47421, "fx":[24.97315,38.20825,35.39485,19.62016], "fy":[-83.33859,-78.15354,-79.4338,-84.73177]}, + {"t":1.89599, "x":1.55187, "y":7.51765, "heading":-0.93744, "vx":-0.07421, "vy":0.24098, "omega":0.06839, "ax":1.63788, "ay":-5.03963, "alpha":-1.44934, "fx":[22.51124,35.61063,32.23403,16.74904], "fy":[-84.04396,-79.3777,-80.7769,-85.3546]}, + {"t":1.91979, "x":1.55057, "y":7.52196, "heading":-0.93581, "vx":-0.03524, "vy":0.12108, "omega":0.03391, "ax":1.48136, "ay":-5.089, "alpha":-1.42527, "fx":[20.24149,33.18088,29.30731,14.14021], "fy":[-84.62567,-80.43007,-81.89237,-85.83349]}, + {"t":1.94358, "x":1.55015, "y":7.5234, "heading":-0.93501, "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/KtoPLO.traj b/src/main/deploy/choreo/KtoPLO.traj index 8bccc47a..ba40ea86 100644 --- a/src/main/deploy/choreo/KtoPLO.traj +++ b/src/main/deploy/choreo/KtoPLO.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,46 +26,45 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.63506], + "waypoints":[0.0,1.57909], "samples":[ - {"t":0.0, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.62351, "ay":3.40224, "alpha":0.1664, "fx":[-58.46885,-59.65655,-60.01369,-58.81116], "fy":[56.43578,55.17949,54.78851,56.07664]}, - {"t":0.04419, "x":3.96462, "y":5.25638, "heading":-1.04555, "vx":-0.16013, "vy":0.15035, "omega":0.00735, "ax":-3.62332, "ay":3.40205, "alpha":0.16632, "fx":[-58.46605,-59.65313,-60.01001,-58.80811], "fy":[56.43232,55.17673,54.78591,56.07331]}, - {"t":0.08838, "x":3.954, "y":5.26634, "heading":-1.04523, "vx":-0.32024, "vy":0.30069, "omega":0.0147, "ax":-3.62309, "ay":3.40184, "alpha":0.16623, "fx":[-58.46303,-59.64953,-60.00573,-58.80443], "fy":[56.42829,55.17335,54.78311,56.06982]}, - {"t":0.13257, "x":3.93632, "y":5.28295, "heading":-1.04458, "vx":-0.48035, "vy":0.45102, "omega":0.02205, "ax":-3.62284, "ay":3.40161, "alpha":0.16613, "fx":[-58.45973,-59.64566,-60.00079,-58.80003], "fy":[56.42361,55.16929,54.78006,56.0661]}, - {"t":0.17676, "x":3.91155, "y":5.3062, "heading":-1.0436, "vx":-0.64045, "vy":0.60134, "omega":0.02939, "ax":-3.62255, "ay":3.40133, "alpha":0.16602, "fx":[-58.45604,-59.64141,-59.99505,-58.79483], "fy":[56.41817,55.16447,54.77666,56.06204]}, - {"t":0.22095, "x":3.87971, "y":5.3361, "heading":-1.04231, "vx":-0.80053, "vy":0.75164, "omega":0.03673, "ax":-3.62222, "ay":3.40102, "alpha":0.16589, "fx":[-58.45185,-59.63664,-59.98837,-58.78868], "fy":[56.41182,55.15876,54.7728,56.05751]}, - {"t":0.26514, "x":3.8408, "y":5.37263, "heading":-1.04068, "vx":-0.9606, "vy":0.90194, "omega":0.04406, "ax":-3.62183, "ay":3.40066, "alpha":0.16575, "fx":[-58.447,-59.63116,-59.98054,-58.78142], "fy":[56.40437,55.152,54.76833,56.0523]}, - {"t":0.30934, "x":3.79481, "y":5.41581, "heading":-1.03874, "vx":-1.12065, "vy":1.05221, "omega":0.05138, "ax":-3.62137, "ay":3.40022, "alpha":0.16558, "fx":[-58.44127,-59.6247,-59.97129,-58.77282], "fy":[56.39557,55.14399,54.76305,56.04617]}, - {"t":0.35353, "x":3.74176, "y":5.46563, "heading":-1.03646, "vx":-1.28068, "vy":1.20247, "omega":0.0587, "ax":-3.62082, "ay":3.39971, "alpha":0.16538, "fx":[-58.43434,-59.6169,-59.96022,-58.76255], "fy":[56.38506,55.13443,54.75667,56.03877]}, - {"t":0.39772, "x":3.68163, "y":5.52209, "heading":-1.03387, "vx":-1.44069, "vy":1.35271, "omega":0.06601, "ax":-3.62015, "ay":3.39907, "alpha":0.16514, "fx":[-58.42578,-59.60724,-59.94679,-58.75013], "fy":[56.37231,55.12288,54.74877,56.02957]}, - {"t":0.44191, "x":3.61443, "y":5.58518, "heading":-1.03095, "vx":-1.60066, "vy":1.50292, "omega":0.0733, "ax":-3.6193, "ay":3.39828, "alpha":0.16484, "fx":[-58.4149,-59.59492,-59.93016,-58.73484], "fy":[56.35654,55.10869,54.73872,56.01783]}, - {"t":0.4861, "x":3.54016, "y":5.65492, "heading":-1.02771, "vx":-1.7606, "vy":1.65309, "omega":0.08059, "ax":-3.61822, "ay":3.39726, "alpha":0.16448, "fx":[-58.40064,-59.57872,-59.90903,-58.71556], "fy":[56.33655,55.09084,54.72551,56.00233]}, - {"t":0.53029, "x":3.45882, "y":5.73129, "heading":-1.02415, "vx":-1.9205, "vy":1.80322, "omega":0.08786, "ax":-3.61677, "ay":3.39591, "alpha":0.164, "fx":[-58.38122,-59.55659,-59.88125,-58.6904], "fy":[56.31032,55.0676,54.70746,55.9811]}, - {"t":0.57448, "x":3.37042, "y":5.81429, "heading":-1.02027, "vx":-2.08032, "vy":1.95328, "omega":0.09511, "ax":-3.61475, "ay":3.39401, "alpha":0.16337, "fx":[-58.35337,-59.52486,-59.84296,-58.65594], "fy":[56.27426,55.03588,54.68152,55.95057]}, - {"t":0.61867, "x":3.27496, "y":5.90392, "heading":-1.01607, "vx":-2.24006, "vy":2.10327, "omega":0.10232, "ax":-3.61171, "ay":3.39116, "alpha":0.16249, "fx":[-58.31057,-59.47622,-59.78653,-58.60532], "fy":[56.22125,54.9894,54.64152,55.90367]}, - {"t":0.66286, "x":3.17245, "y":6.00017, "heading":-1.01155, "vx":-2.39967, "vy":2.25313, "omega":0.1095, "ax":-3.60666, "ay":3.38641, "alpha":0.16118, "fx":[-58.23737,-59.39375,-59.69433,-58.52244], "fy":[56.13486,54.91347,54.57291,55.82403]}, - {"t":0.70705, "x":3.06288, "y":6.10305, "heading":-1.00671, "vx":-2.55905, "vy":2.40277, "omega":0.11663, "ax":-3.59655, "ay":3.37692, "alpha":0.15905, "fx":[-58.08695,-59.22719,-59.51421,-58.35869], "fy":[55.96658,54.76368,54.4314,55.66308]}, - {"t":0.75124, "x":2.94628, "y":6.21253, "heading":-1.00155, "vx":-2.71798, "vy":2.552, "omega":0.12366, "ax":-3.56636, "ay":3.34857, "alpha":0.15505, "fx":[-57.6225,-58.72906,-58.99117,-57.87], "fy":[55.47947,54.3166,53.99259,55.18229]}, - {"t":0.79543, "x":2.82269, "y":6.32857, "heading":-0.99609, "vx":-2.87558, "vy":2.69998, "omega":0.13051, "ax":-0.00001, "ay":-0.00002, "alpha":0.00349, "fx":[0.00277,-0.01359,-0.00322,0.01314], "fy":[0.01295,0.00235,-0.01366,-0.00307]}, - {"t":0.83962, "x":2.69562, "y":6.44788, "heading":-0.99032, "vx":-2.87558, "vy":2.69998, "omega":0.13066, "ax":3.56636, "ay":-3.34857, "alpha":-0.15494, "fx":[57.6279,58.73611,58.98597,57.8627], "fy":[-55.47412,-54.30879,-53.998,-55.19014]}, - {"t":0.88381, "x":2.57203, "y":6.56393, "heading":-0.98455, "vx":-2.71798, "vy":2.552, "omega":0.12382, "ax":3.59655, "ay":-3.37692, "alpha":-0.15905, "fx":[58.09698,59.24224,59.50468,58.34313], "fy":[-55.95645,-54.74723,-54.44153,-55.67957]}, - {"t":0.92801, "x":2.45543, "y":6.67341, "heading":-0.97908, "vx":-2.55905, "vy":2.40277, "omega":0.11679, "ax":3.60666, "ay":-3.38641, "alpha":-0.16123, "fx":[58.252,59.41636,59.68047,58.49907], "fy":[-56.11995,-54.88885,-54.58778,-55.84869]}, - {"t":0.9722, "x":2.34586, "y":6.77628, "heading":-0.97391, "vx":-2.39967, "vy":2.25312, "omega":0.10966, "ax":3.61172, "ay":-3.39116, "alpha":-0.16258, "fx":[58.3296,59.50593,59.76853,58.5746], "fy":[-56.20178,-54.95707,-54.66093,-55.93603]}, - {"t":1.01639, "x":2.24335, "y":6.87254, "heading":-0.96907, "vx":-2.24006, "vy":2.10327, "omega":0.10248, "ax":3.61475, "ay":-3.39401, "alpha":-0.1635, "fx":[58.37655,59.56123,59.82105,58.61833], "fy":[-56.25048,-54.99635,-54.70521,-55.99014]}, - {"t":1.06058, "x":2.14789, "y":6.96217, "heading":-0.96454, "vx":-2.08032, "vy":1.95328, "omega":0.09525, "ax":3.61677, "ay":-3.39591, "alpha":-0.16416, "fx":[58.40827,59.59916,59.85567,58.64638], "fy":[-56.28251,-55.02137,-54.73516,-56.02738]}, - {"t":1.10477, "x":2.05949, "y":7.04517, "heading":-0.96033, "vx":-1.9205, "vy":1.80322, "omega":0.088, "ax":3.61822, "ay":-3.39726, "alpha":-0.16466, "fx":[58.4313,59.62702,59.88006,58.66562], "fy":[-56.30499,-55.03841,-54.75694,-56.05481]}, - {"t":1.14896, "x":1.97815, "y":7.12154, "heading":-0.95644, "vx":-1.7606, "vy":1.65309, "omega":0.08072, "ax":3.6193, "ay":-3.39828, "alpha":-0.16505, "fx":[58.44888,59.64847,59.89806,58.67946], "fy":[-56.32154,-55.05058,-54.77358,-56.07599]}, - {"t":1.19315, "x":1.90388, "y":7.19127, "heading":-0.95288, "vx":-1.60066, "vy":1.50292, "omega":0.07343, "ax":3.62015, "ay":-3.39907, "alpha":-0.16537, "fx":[58.46278,59.66557,59.91184,58.68981], "fy":[-56.33416,-55.0596,-54.78677,-56.09291]}, - {"t":1.23734, "x":1.83668, "y":7.25436, "heading":-0.94963, "vx":-1.44069, "vy":1.35271, "omega":0.06612, "ax":3.62082, "ay":-3.39971, "alpha":-0.16563, "fx":[58.47409,59.67953,59.92269,58.69778], "fy":[-56.34406,-55.06649,-54.79751,-56.10676]}, - {"t":1.28153, "x":1.77655, "y":7.31082, "heading":-0.94671, "vx":-1.28068, "vy":1.20247, "omega":0.0588, "ax":3.62137, "ay":-3.40022, "alpha":-0.16584, "fx":[58.48346,59.69116,59.93144,58.70409], "fy":[-56.35203,-55.07191,-54.80643,-56.1183]}, - {"t":1.32572, "x":1.72349, "y":7.36064, "heading":-0.94411, "vx":-1.12065, "vy":1.05221, "omega":0.05147, "ax":3.62183, "ay":-3.40065, "alpha":-0.16602, "fx":[58.49135,59.70096,59.93866,58.70922], "fy":[-56.35859,-55.0763,-54.81394,-56.12805]}, - {"t":1.36991, "x":1.67751, "y":7.40382, "heading":-0.94184, "vx":-0.9606, "vy":0.90194, "omega":0.04414, "ax":3.62222, "ay":-3.40102, "alpha":-0.16618, "fx":[58.49806,59.70932,59.94474,58.71351], "fy":[-56.3641,-55.07995,-54.82034,-56.13637]}, - {"t":1.4141, "x":1.6386, "y":7.44035, "heading":-0.93989, "vx":-0.80053, "vy":0.75164, "omega":0.03679, "ax":3.62255, "ay":-3.40133, "alpha":-0.16631, "fx":[58.50381,59.71648,59.94994,58.71717], "fy":[-56.36882,-55.08307,-54.82582,-56.1435]}, - {"t":1.45829, "x":1.60676, "y":7.47025, "heading":-0.93826, "vx":-0.64045, "vy":0.60134, "omega":0.02944, "ax":3.62284, "ay":-3.4016, "alpha":-0.16642, "fx":[58.50876,59.72265,59.95448,58.7204], "fy":[-56.37295,-55.08582,-54.83054,-56.14963]}, - {"t":1.50249, "x":1.58199, "y":7.4935, "heading":-0.93696, "vx":-0.48035, "vy":0.45102, "omega":0.02209, "ax":3.6231, "ay":-3.40184, "alpha":-0.16653, "fx":[58.51304,59.72796,59.95851,58.7233], "fy":[-56.37661,-55.08832,-54.83461,-56.1549]}, - {"t":1.54668, "x":1.5643, "y":7.51011, "heading":-0.93598, "vx":-0.32024, "vy":0.30069, "omega":0.01473, "ax":3.62332, "ay":-3.40205, "alpha":-0.16661, "fx":[58.51672,59.73253,59.96214,58.72599], "fy":[-56.37994,-55.09066,-54.83811,-56.15942]}, - {"t":1.59087, "x":1.55369, "y":7.52008, "heading":-0.93533, "vx":-0.16013, "vy":0.15035, "omega":0.00737, "ax":3.62352, "ay":-3.40224, "alpha":-0.16669, "fx":[58.51989,59.73644,59.96548,58.72853], "fy":[-56.383,-55.09289,-54.84111,-56.16328]}, - {"t":1.63506, "x":1.55015, "y":7.5234, "heading":-0.93501, "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.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.8823, "ay":3.64522, "alpha":0.17839, "fx":[-62.64421,-63.91746,-64.30032,-63.01117], "fy":[60.46689,59.12013,58.70097,60.08189]}, + {"t":0.04386, "x":3.96442, "y":5.25656, "heading":-1.04555, "vx":-0.17029, "vy":0.15989, "omega":0.00782, "ax":-3.88208, "ay":3.64502, "alpha":0.17829, "fx":[-62.64111,-63.91364,-64.2962,-63.00778], "fy":[60.46303,59.11706,58.69808,60.07817]}, + {"t":0.08773, "x":3.95322, "y":5.26708, "heading":-1.04521, "vx":-0.34057, "vy":0.31978, "omega":0.01565, "ax":-3.88184, "ay":3.64478, "alpha":0.17819, "fx":[-62.63775,-63.90961,-64.29142,-63.00366], "fy":[60.45852,59.11329,58.69497,60.07427]}, + {"t":0.13159, "x":3.93454, "y":5.28461, "heading":-1.04452, "vx":-0.51084, "vy":0.47965, "omega":0.02346, "ax":-3.88155, "ay":3.64452, "alpha":0.17808, "fx":[-62.63406,-63.90527,-64.28585,-62.99873], "fy":[60.45325,59.10874,58.69156,60.07009]}, + {"t":0.17545, "x":3.9084, "y":5.30916, "heading":-1.04349, "vx":-0.6811, "vy":0.63951, "omega":0.03127, "ax":-3.88123, "ay":3.64421, "alpha":0.17795, "fx":[-62.62991,-63.90048,-64.27937,-62.99287], "fy":[60.44709,59.1033,58.68774,60.06551]}, + {"t":0.21932, "x":3.87479, "y":5.34072, "heading":-1.04212, "vx":-0.85135, "vy":0.79936, "omega":0.03908, "ax":-3.88085, "ay":3.64386, "alpha":0.1778, "fx":[-62.62518,-63.89506,-64.27178,-62.98591], "fy":[60.43988,59.09684,58.68339,60.06036]}, + {"t":0.26318, "x":3.83372, "y":5.37928, "heading":-1.04041, "vx":-1.02158, "vy":0.95919, "omega":0.04688, "ax":-3.88041, "ay":3.64344, "alpha":0.17763, "fx":[-62.61966,-63.88879,-64.26284,-62.97766], "fy":[60.43138,59.08915,58.67831,60.0544]}, + {"t":0.30704, "x":3.78517, "y":5.42486, "heading":-1.03835, "vx":-1.19178, "vy":1.119, "omega":0.05467, "ax":-3.87988, "ay":3.64294, "alpha":0.17743, "fx":[-62.61308,-63.88133,-64.2522,-62.96781], "fy":[60.42126,59.07999,58.67225,60.04732]}, + {"t":0.35091, "x":3.72917, "y":5.47745, "heading":-1.03595, "vx":-1.36197, "vy":1.2788, "omega":0.06245, "ax":-3.87924, "ay":3.64234, "alpha":0.1772, "fx":[-62.60506,-63.87224,-64.23938,-62.95597], "fy":[60.40907,59.06897,58.66486,60.03867]}, + {"t":0.39477, "x":3.66569, "y":5.53705, "heading":-1.03322, "vx":-1.53213, "vy":1.43856, "omega":0.07022, "ax":-3.87845, "ay":3.6416, "alpha":0.17691, "fx":[-62.59501,-63.86082,-64.22365,-62.94152], "fy":[60.39415,59.05555,58.65559,60.0278]}, + {"t":0.43864, "x":3.59476, "y":5.60365, "heading":-1.03013, "vx":-1.70225, "vy":1.5983, "omega":0.07798, "ax":-3.87745, "ay":3.64066, "alpha":0.17655, "fx":[-62.58207,-63.84607,-64.20394,-62.92352], "fy":[60.37547,59.03886,58.64363,60.0137]}, + {"t":0.4825, "x":3.51636, "y":5.67726, "heading":-1.02671, "vx":-1.87233, "vy":1.75799, "omega":0.08573, "ax":-3.87614, "ay":3.63943, "alpha":0.1761, "fx":[-62.5648,-63.8263,-64.17849,-62.90047], "fy":[60.3514,59.01754,58.62762,59.99476]}, + {"t":0.52636, "x":3.43051, "y":5.75787, "heading":-1.02295, "vx":-2.04235, "vy":1.91763, "omega":0.09345, "ax":-3.87436, "ay":3.63776, "alpha":0.1755, "fx":[-62.5407,-63.79866,-64.14428,-62.86972], "fy":[60.31912,58.98918,58.60523,59.96821]}, + {"t":0.57023, "x":3.33719, "y":5.84549, "heading":-1.01885, "vx":-2.21229, "vy":2.07719, "omega":0.10115, "ax":-3.87178, "ay":3.63534, "alpha":0.17468, "fx":[-62.50505,-63.75779,-64.09568,-62.82629], "fy":[60.27337,58.94924,58.572,59.92884]}, + {"t":0.61409, "x":3.23643, "y":5.9401, "heading":-1.01442, "vx":-2.38212, "vy":2.23665, "omega":0.10881, "ax":-3.86773, "ay":3.63154, "alpha":0.17348, "fx":[-62.44761,-63.69221,-64.02068,-62.75944], "fy":[60.20296,58.88794,58.5183,59.86553]}, + {"t":0.65795, "x":3.12822, "y":6.0417, "heading":-1.00965, "vx":-2.55177, "vy":2.39594, "omega":0.11642, "ax":-3.86044, "ay":3.62469, "alpha":0.1716, "fx":[-62.34148,-63.57234,-63.88844,-62.64109], "fy":[60.07914,58.77962,58.41876,59.74966]}, + {"t":0.70182, "x":3.01258, "y":6.15028, "heading":-1.00454, "vx":-2.72111, "vy":2.55494, "omega":0.12395, "ax":-3.84346, "ay":3.60875, "alpha":0.16812, "fx":[-62.087,-63.29081,-63.58754,-62.36774], "fy":[59.79817,58.52967,58.17923,59.47756]}, + {"t":0.74568, "x":2.88952, "y":6.26582, "heading":-0.9991, "vx":-2.88969, "vy":2.71323, "omega":0.13132, "ax":-3.75944, "ay":3.52985, "alpha":0.15825, "fx":[-60.7834,-61.90141,-62.14297,-61.01091], "fy":[58.45288,57.2869,56.94723,58.13834]}, + {"t":0.78954, "x":2.75915, "y":6.38823, "heading":-0.99334, "vx":-3.0546, "vy":2.86806, "omega":0.13826, "ax":3.75943, "ay":-3.52986, "alpha":-0.15655, "fx":[60.79304,61.90028,62.13298,61.01177], "fy":[-58.44267,-57.28729,-56.958,-58.13798]}, + {"t":0.83341, "x":2.62879, "y":6.51063, "heading":-0.98728, "vx":-2.88969, "vy":2.71323, "omega":0.1314, "ax":3.84346, "ay":-3.60875, "alpha":-0.16761, "fx":[62.09747,63.3017,63.57736,62.35643], "fy":[-59.78754,-58.51763,-58.19006,-59.48965]}, + {"t":0.87727, "x":2.50573, "y":6.62617, "heading":-0.98151, "vx":-2.72111, "vy":2.55493, "omega":0.12405, "ax":3.86044, "ay":-3.62469, "alpha":-0.17134, "fx":[62.35627,63.59231,63.87429,62.62043], "fy":[-60.06407,-58.7578,-58.43392,-59.77153]}, + {"t":0.92113, "x":2.39009, "y":6.73476, "heading":-0.97607, "vx":-2.55177, "vy":2.39594, "omega":0.11653, "ax":3.86773, "ay":-3.63154, "alpha":-0.17336, "fx":[62.467,63.72045,64.00224,62.73023], "fy":[-60.18313,-58.85718,-58.53816,-59.89633]}, + {"t":0.965, "x":2.28188, "y":6.83636, "heading":-0.97096, "vx":-2.38212, "vy":2.23665, "omega":0.10893, "ax":3.87178, "ay":-3.63534, "alpha":-0.17464, "fx":[62.52893,63.79369,64.07303,62.78916], "fy":[-60.24888,-58.9102,-58.59647,-59.96793]}, + {"t":1.00886, "x":2.18111, "y":6.93097, "heading":-0.96618, "vx":-2.21229, "vy":2.07719, "omega":0.10126, "ax":3.87436, "ay":-3.63776, "alpha":-0.17553, "fx":[62.56884,63.84166,64.11763,62.82525], "fy":[-60.29021,-58.94247,-58.63409,-60.01498]}, + {"t":1.05272, "x":2.0878, "y":7.01858, "heading":-0.96174, "vx":-2.04235, "vy":1.91763, "omega":0.09357, "ax":3.87614, "ay":-3.63943, "alpha":-0.17618, "fx":[62.59692,63.87583,64.14809,62.84924], "fy":[-60.31835,-58.96375,-58.66059,-60.0486]}, + {"t":1.09659, "x":2.00195, "y":7.09919, "heading":-0.95764, "vx":-1.87233, "vy":1.75799, "omega":0.08584, "ax":3.87745, "ay":-3.64066, "alpha":-0.17667, "fx":[62.61788,63.90159,64.17007,62.8661], "fy":[-60.33859,-58.9786,-58.68041,-60.07402]}, + {"t":1.14045, "x":1.92355, "y":7.1728, "heading":-0.95387, "vx":-1.70225, "vy":1.5983, "omega":0.07809, "ax":3.87845, "ay":-3.6416, "alpha":-0.17707, "fx":[62.6342,63.92179,64.1866,62.87847], "fy":[-60.35376,-58.9894,-58.69586,-60.09401]}, + {"t":1.18432, "x":1.85261, "y":7.23941, "heading":-0.95045, "vx":-1.53213, "vy":1.43856, "omega":0.07032, "ax":3.87924, "ay":-3.64234, "alpha":-0.17738, "fx":[62.6473,63.9381,64.19944,62.88785], "fy":[-60.3655,-58.99752,-58.7083,-60.11018]}, + {"t":1.22818, "x":1.78914, "y":7.299, "heading":-0.94736, "vx":-1.36197, "vy":1.2788, "omega":0.06254, "ax":3.87988, "ay":-3.64294, "alpha":-0.17764, "fx":[62.65806,63.95155,64.20969,62.89519], "fy":[-60.37484,-59.00383,-58.71852,-60.12354]}, + {"t":1.27204, "x":1.73313, "y":7.35159, "heading":-0.94462, "vx":-1.19178, "vy":1.119, "omega":0.05475, "ax":3.88041, "ay":-3.64344, "alpha":-0.17786, "fx":[62.66706,63.96282,64.21805,62.90109], "fy":[-60.38245,-59.00887,-58.72708,-60.13474]}, + {"t":1.31591, "x":1.68459, "y":7.39717, "heading":-0.94222, "vx":-1.02158, "vy":0.95919, "omega":0.04695, "ax":3.88085, "ay":-3.64386, "alpha":-0.17805, "fx":[62.67466,63.97236,64.22503,62.90596], "fy":[-60.38879,-59.01302,-58.73431,-60.14423]}, + {"t":1.35977, "x":1.64351, "y":7.43574, "heading":-0.94016, "vx":-0.85135, "vy":0.79936, "omega":0.03914, "ax":3.88123, "ay":-3.64421, "alpha":-0.17821, "fx":[62.68114,63.9805,64.23097,62.9101], "fy":[-60.39418,-59.01654,-58.74049,-60.15234]}, + {"t":1.40363, "x":1.6099, "y":7.46729, "heading":-0.93844, "vx":-0.6811, "vy":0.63951, "omega":0.03132, "ax":3.88155, "ay":-3.64452, "alpha":-0.17834, "fx":[62.6867,63.98747,64.23611,62.9137], "fy":[-60.39885,-59.01962,-58.74578,-60.15927]}, + {"t":1.4475, "x":1.58376, "y":7.49184, "heading":-0.93707, "vx":-0.51084, "vy":0.47965, "omega":0.0235, "ax":3.88184, "ay":-3.64478, "alpha":-0.17846, "fx":[62.69148,63.99346,64.24065,62.91694], "fy":[-60.40299,-59.02239,-58.75032,-60.16522]}, + {"t":1.49136, "x":1.56509, "y":7.50937, "heading":-0.93604, "vx":-0.34057, "vy":0.31978, "omega":0.01567, "ax":3.88209, "ay":-3.64502, "alpha":-0.17857, "fx":[62.69558,63.99859,64.24473,62.91991], "fy":[-60.40672,-59.02498,-58.75422,-60.1703]}, + {"t":1.53522, "x":1.55389, "y":7.51989, "heading":-0.93535, "vx":-0.17029, "vy":0.15989, "omega":0.00784, "ax":3.88231, "ay":-3.64522, "alpha":-0.17866, "fx":[62.6991,64.00296,64.24846,62.92273], "fy":[-60.41015,-59.02745,-58.75754,-60.17462]}, + {"t":1.57909, "x":1.55015, "y":7.5234, "heading":-0.93501, "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/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 3c2a93e5..3406c19a 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,8 +18,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":48, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -36,72 +36,69 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.94616,2.04867], + "waypoints":[0.0,0.89025,1.99276], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17853, "ay":-1.9575, "alpha":0.00521, "fx":[-68.30368,-68.32648,-68.31847,-68.29567], "fy":[-32.01702,-31.96839,-31.98566,-32.03425]}, - {"t":0.03379, "x":7.10893, "y":6.23445, "heading":-2.35619, "vx":-0.1412, "vy":-0.06615, "omega":0.00018, "ax":-4.17821, "ay":-1.95735, "alpha":0.00538, "fx":[-68.29818,-68.32175,-68.31347,-68.2899], "fy":[-32.01517,-31.96489,-31.98275,-32.033]}, - {"t":0.06758, "x":7.10177, "y":6.23109, "heading":-2.35619, "vx":-0.28239, "vy":-0.13229, "omega":0.00036, "ax":-4.17785, "ay":-1.95719, "alpha":0.00559, "fx":[-68.29194,-68.3164,-68.30781,-68.28335], "fy":[-32.01308,-31.96092,-31.97945,-32.03157]}, - {"t":0.10137, "x":7.08984, "y":6.22551, "heading":-2.35618, "vx":-0.42356, "vy":-0.19842, "omega":0.00055, "ax":-4.17743, "ay":-1.957, "alpha":0.00582, "fx":[-68.28481,-68.31028,-68.30134,-68.27588], "fy":[-32.01069,-31.95639,-31.97568,-32.02994]}, - {"t":0.13517, "x":7.07315, "y":6.21768, "heading":-2.35616, "vx":-0.56472, "vy":-0.26455, "omega":0.00074, "ax":-4.17695, "ay":-1.95678, "alpha":0.00608, "fx":[-68.27659,-68.30322,-68.29388,-68.26725], "fy":[-32.00793,-31.95115,-31.97134,-32.02807]}, - {"t":0.16896, "x":7.05168, "y":6.20763, "heading":-2.35613, "vx":-0.70587, "vy":-0.33068, "omega":0.00095, "ax":-4.17639, "ay":-1.95653, "alpha":0.00639, "fx":[-68.267,-68.29498,-68.28518,-68.2572], "fy":[-32.00471,-31.94505,-31.96627,-32.02588]}, - {"t":0.20275, "x":7.02544, "y":6.19534, "heading":-2.3561, "vx":-0.84699, "vy":-0.39679, "omega":0.00116, "ax":-4.17573, "ay":-1.95623, "alpha":0.00676, "fx":[-68.25567,-68.28526,-68.2749,-68.24531], "fy":[-32.00092,-31.93784,-31.96027,-32.02329]}, - {"t":0.23654, "x":6.99444, "y":6.18081, "heading":-2.35606, "vx":-0.9881, "vy":-0.46289, "omega":0.00139, "ax":-4.17494, "ay":-1.95587, "alpha":0.0072, "fx":[-68.24208,-68.27359,-68.26257,-68.23106], "fy":[-31.99636,-31.92918,-31.95309,-32.0202]}, - {"t":0.27033, "x":6.95866, "y":6.16405, "heading":-2.35601, "vx":-1.12917, "vy":-0.52898, "omega":0.00164, "ax":-4.17397, "ay":-1.95543, "alpha":0.00774, "fx":[-68.22547,-68.25935,-68.24752,-68.21365], "fy":[-31.99079,-31.91861,-31.94431,-32.01642]}, - {"t":0.30412, "x":6.91813, "y":6.14506, "heading":-2.35596, "vx":-1.27022, "vy":-0.59506, "omega":0.0019, "ax":-4.17276, "ay":-1.95488, "alpha":0.00841, "fx":[-68.20473,-68.24155,-68.22872,-68.19189], "fy":[-31.98385,-31.90539,-31.93334,-32.01171]}, - {"t":0.33791, "x":6.87282, "y":6.12384, "heading":-2.35589, "vx":-1.41122, "vy":-0.66112, "omega":0.00218, "ax":-4.17121, "ay":-1.95418, "alpha":0.00928, "fx":[-68.17807,-68.2187,-68.20457,-68.16395], "fy":[-31.97492,-31.8884,-31.91925,-32.00566]}, - {"t":0.37171, "x":6.82275, "y":6.10038, "heading":-2.35582, "vx":-1.55217, "vy":-0.72715, "omega":0.0025, "ax":-4.16914, "ay":-1.95324, "alpha":0.01044, "fx":[-68.14256,-68.18826,-68.17242,-68.12673], "fy":[-31.96304,-31.86576,-31.90049,-31.99763]}, - {"t":0.4055, "x":6.76792, "y":6.07469, "heading":-2.35574, "vx":-1.69305, "vy":-0.79316, "omega":0.00285, "ax":-4.16625, "ay":-1.95193, "alpha":0.01207, "fx":[-68.09291,-68.14573,-68.1275,-68.07469], "fy":[-31.94645,-31.83407,-31.87425,-31.98644]}, - {"t":0.43929, "x":6.70833, "y":6.04678, "heading":-2.35564, "vx":-1.83384, "vy":-0.85911, "omega":0.00326, "ax":-4.16192, "ay":-1.94997, "alpha":0.01453, "fx":[-68.01857,-68.08212,-68.06032,-67.99679], "fy":[-31.92166,-31.78656,-31.83495,-31.96978]}, - {"t":0.47308, "x":6.64399, "y":6.01663, "heading":-2.35553, "vx":-1.97447, "vy":-0.92501, "omega":0.00375, "ax":-4.15473, "ay":-1.94671, "alpha":0.01865, "fx":[-67.895,-67.97658,-67.94891,-67.86736], "fy":[-31.88059,-31.7074,-31.76964,-31.94238]}, - {"t":0.50687, "x":6.5749, "y":5.98427, "heading":-2.3554, "vx":-2.11487, "vy":-0.99079, "omega":0.00438, "ax":-4.14046, "ay":-1.94024, "alpha":0.02709, "fx":[-67.64906,-67.76748,-67.72818,-67.60982], "fy":[-31.79959,-31.54882,-31.63949,-31.88928]}, - {"t":0.54066, "x":6.50107, "y":5.94968, "heading":-2.35526, "vx":-2.25478, "vy":-1.05635, "omega":0.00529, "ax":-4.09842, "ay":-1.92123, "alpha":0.05556, "fx":[-66.91789,-67.1608,-67.08457,-66.84247], "fy":[-31.57129,-31.06019,-31.24828,-31.75425]}, - {"t":0.57445, "x":6.42254, "y":5.91288, "heading":-2.35508, "vx":-2.39327, "vy":-1.12127, "omega":0.00717, "ax":-1.55891, "ay":-0.75503, "alpha":1.69471, "fx":[-19.10513,-26.10422,-31.66581,-25.06558], "fy":[-12.8562,-5.34424,-11.94818,-19.22492]}, - {"t":0.60824, "x":6.34077, "y":5.87456, "heading":-2.35483, "vx":-2.44595, "vy":-1.14679, "omega":0.06444, "ax":4.09634, "ay":1.91743, "alpha":0.05401, "fx":[67.0479,66.81312,66.8864,67.12227], "fy":[31.19128,31.68256,31.50421,31.00736]}, - {"t":0.64204, "x":6.26046, "y":5.83691, "heading":-2.35266, "vx":-2.30753, "vy":-1.08199, "omega":0.06626, "ax":4.14018, "ay":1.93876, "alpha":0.02311, "fx":[67.7176,67.61678,67.6505,67.75137], "fy":[31.62739,31.8403,31.76307,31.54944]}, - {"t":0.67583, "x":6.18485, "y":5.80145, "heading":-2.35042, "vx":-2.16763, "vy":-1.01648, "omega":0.06704, "ax":4.15472, "ay":1.94581, "alpha":0.01441, "fx":[67.94239,67.87949,67.90117,67.96408], "fy":[31.76789,31.90119,31.85269,31.71912]}, - {"t":0.70962, "x":6.11397, "y":5.76822, "heading":-2.34815, "vx":-2.02723, "vy":-0.95073, "omega":0.06753, "ax":4.16198, "ay":1.94932, "alpha":0.0102, "fx":[68.05483,68.01035,68.02597,68.07046], "fy":[31.83774,31.93218,31.89765,31.80308]}, - {"t":0.74341, "x":6.04785, "y":5.7372, "heading":-2.34587, "vx":-1.88659, "vy":-0.88486, "omega":0.06787, "ax":4.16633, "ay":1.95143, "alpha":0.0077, "fx":[68.1223,68.08875,68.1007,68.13425], "fy":[31.87956,31.95087,31.92464,31.85327]}, - {"t":0.7772, "x":5.98648, "y":5.70842, "heading":-2.34358, "vx":-1.74581, "vy":-0.81892, "omega":0.06813, "ax":4.16922, "ay":1.95283, "alpha":0.00604, "fx":[68.16727,68.14097,68.15045,68.17675], "fy":[31.90741,31.96335,31.94265,31.88666]}, - {"t":0.81099, "x":5.92986, "y":5.68186, "heading":-2.34127, "vx":-1.60492, "vy":-0.75293, "omega":0.06834, "ax":4.17129, "ay":1.95383, "alpha":0.00485, "fx":[68.19939,68.17826,68.18595,68.20709], "fy":[31.92728,31.97226,31.95551,31.9105]}, - {"t":0.84478, "x":5.87801, "y":5.65753, "heading":-2.33897, "vx":-1.46397, "vy":-0.68691, "omega":0.0685, "ax":4.17284, "ay":1.95458, "alpha":0.00397, "fx":[68.22349,68.20622,68.21256,68.22983], "fy":[31.94217,31.97894,31.96516,31.92837]}, - {"t":0.87858, "x":5.83092, "y":5.63543, "heading":-2.33665, "vx":-1.32296, "vy":-0.62086, "omega":0.06864, "ax":4.17405, "ay":1.95516, "alpha":0.00328, "fx":[68.24222,68.22796,68.23325,68.24751], "fy":[31.95375,31.98413,31.97267,31.94227]}, - {"t":0.91237, "x":5.7886, "y":5.61557, "heading":-2.33433, "vx":-1.18192, "vy":-0.55479, "omega":0.06875, "ax":4.17501, "ay":1.95563, "alpha":0.00273, "fx":[68.25722,68.24535,68.24979,68.26165], "fy":[31.963,31.98828,31.97868,31.95339]}, - {"t":0.94616, "x":5.75105, "y":5.59794, "heading":-2.33201, "vx":-1.04084, "vy":-0.48871, "omega":0.06884, "ax":0.04202, "ay":-0.07239, "alpha":0.93694, "fx":[4.19677,0.45363,-2.79,0.88718], "fy":[-1.07029,2.63185,-1.29798,-4.9976]}, - {"t":0.97766, "x":5.71828, "y":5.58251, "heading":-2.32984, "vx":-1.03951, "vy":-0.49099, "omega":0.09835, "ax":0.02045, "ay":-0.04321, "alpha":0.86648, "fx":[3.56488,0.15184,-2.89367,0.51414], "fy":[-0.59039,2.82384,-0.82406,-4.2351]}, - {"t":1.00916, "x":5.68555, "y":5.56702, "heading":-2.32674, "vx":-1.03887, "vy":-0.49235, "omega":0.12565, "ax":0.00518, "ay":-0.01093, "alpha":0.7981, "fx":[3.05749,-0.09563,-2.88501,0.26217], "fy":[-0.05861,3.07465,-0.29951,-3.43157]}, - {"t":1.04066, "x":5.65283, "y":5.55151, "heading":-2.32278, "vx":-1.03871, "vy":-0.49269, "omega":0.15079, "ax":0.00079, "ay":-0.00167, "alpha":0.73168, "fx":[2.73474,-0.16382,-2.70671,0.18759], "fy":[0.0968,2.95724,-0.15163,-3.01164]}, - {"t":1.07216, "x":5.62011, "y":5.53599, "heading":-2.31803, "vx":-1.03868, "vy":-0.49275, "omega":0.17384, "ax":-0.00034, "ay":0.00071, "alpha":0.6671, "fx":[2.47224,-0.17794,-2.48218,0.16588], "fy":[0.13903,2.73466,-0.11591,-2.71141]}, - {"t":1.10366, "x":5.58739, "y":5.52047, "heading":-2.31256, "vx":-1.03869, "vy":-0.49272, "omega":0.19485, "ax":-0.00063, "ay":0.00133, "alpha":0.60419, "fx":[2.23004,-0.17849,-2.25005,0.15726], "fy":[0.1514,2.49,-0.10794,-2.44653]}, - {"t":1.13516, "x":5.55467, "y":5.50495, "heading":-2.30642, "vx":-1.03871, "vy":-0.49268, "omega":0.21388, "ax":-0.00093, "ay":0.00196, "alpha":0.54278, "fx":[1.99356,-0.17814,-2.02359,0.14734], "fy":[0.16243,2.25152,-0.09831,-2.18742]}, - {"t":1.16666, "x":5.52195, "y":5.48943, "heading":-2.29968, "vx":-1.03874, "vy":-0.49262, "omega":0.23098, "ax":-0.00123, "ay":0.0026, "alpha":0.48269, "fx":[1.76272,-0.178,-1.80161,0.13634], "fy":[0.17162,2.01847,-0.08671,-1.93357]}, - {"t":1.19816, "x":5.48923, "y":5.47391, "heading":-2.29241, "vx":-1.03878, "vy":-0.49254, "omega":0.24618, "ax":-0.00019, "ay":0.0004, "alpha":0.42377, "fx":[1.55897,-0.15619,-1.56129,0.14604], "fy":[0.13223,1.74382,-0.1192,-1.73073]}, - {"t":1.22966, "x":5.4565, "y":5.4584, "heading":-2.28465, "vx":-1.03879, "vy":-0.49253, "omega":0.25953, "ax":0.01067, "ay":-0.02223, "alpha":0.366, "fx":[1.5288,0.00027,-1.1546,0.32339], "fy":[-0.24376,1.13922,-0.48323,-1.86598]}, - {"t":1.26116, "x":5.42379, "y":5.44287, "heading":-2.27648, "vx":-1.03845, "vy":-0.49323, "omega":0.27106, "ax":0.4775, "ay":0.25378, "alpha":0.30245, "fx":[8.9563,7.48784,6.78051,8.00041], "fy":[4.24571,5.40027,4.05161,2.898]}, - {"t":1.29266, "x":5.39131, "y":5.42746, "heading":-2.26794, "vx":-1.02341, "vy":-0.48523, "omega":0.28059, "ax":1.34486, "ay":0.63723, "alpha":0.2277, "fx":[22.75287,21.97093,21.14287,22.07725], "fy":[10.4761,11.38538,10.36327,9.44511]}, - {"t":1.32416, "x":5.35974, "y":5.41249, "heading":-2.2591, "vx":-0.98104, "vy":-0.46516, "omega":0.28776, "ax":1.35012, "ay":0.64, "alpha":0.17795, "fx":[22.67537,22.02869,21.42819,22.155], "fy":[10.51629,11.22252,10.41201,9.70032]}, - {"t":1.35566, "x":5.32951, "y":5.39816, "heading":-2.25004, "vx":-0.93852, "vy":-0.445, "omega":0.29337, "ax":1.35186, "ay":0.64091, "alpha":0.12911, "fx":[22.53581,22.06655,21.63821,22.16083], "fy":[10.5247,11.03191,10.43238,9.92175]}, - {"t":1.38716, "x":5.30062, "y":5.38446, "heading":-2.24079, "vx":-0.89593, "vy":-0.42481, "omega":0.29743, "ax":1.35273, "ay":0.64136, "alpha":0.0804, "fx":[22.38055,22.10293,21.82875,22.14614], "fy":[10.52289,10.83395,10.44845,10.13501]}, - {"t":1.41866, "x":5.27307, "y":5.37139, "heading":-2.23142, "vx":-0.85332, "vy":-0.40461, "omega":0.29997, "ax":1.35326, "ay":0.64163, "alpha":0.03159, "fx":[22.21868,22.14084,22.01162,22.12142], "fy":[10.51488,10.63253,10.465,10.34544]}, - {"t":1.45016, "x":5.24686, "y":5.35897, "heading":-2.22198, "vx":-0.81069, "vy":-0.3844, "omega":0.30096, "ax":1.3536, "ay":0.64181, "alpha":-0.01746, "fx":[22.05308,22.18123,22.19096,22.09011], "fy":[10.50198,10.42878,10.48365,10.55504]}, - {"t":1.48166, "x":5.22199, "y":5.34718, "heading":-2.2125, "vx":-0.76805, "vy":-0.36418, "omega":0.30041, "ax":1.35385, "ay":0.64193, "alpha":-0.06686, "fx":[21.88479,22.22458,22.36869,22.05361], "fy":[10.48471,10.22302,10.50509,10.76483]}, - {"t":1.51316, "x":5.19847, "y":5.33602, "heading":-2.20303, "vx":-0.72541, "vy":-0.34396, "omega":0.29831, "ax":1.35404, "ay":0.64203, "alpha":-0.11671, "fx":[21.71426,22.27117,22.54584,22.01266], "fy":[10.46331,10.01517,10.52973,10.97551]}, - {"t":1.54466, "x":5.17629, "y":5.32551, "heading":-2.19364, "vx":-0.68275, "vy":-0.32373, "omega":0.29463, "ax":1.35419, "ay":0.6421, "alpha":-0.1671, "fx":[21.54157,22.32107,22.72336,21.96748], "fy":[10.43794,9.80516,10.55769,11.18762]}, - {"t":1.57616, "x":5.15546, "y":5.31563, "heading":-2.18435, "vx":-0.6401, "vy":-0.30351, "omega":0.28937, "ax":1.3543, "ay":0.64216, "alpha":-0.21814, "fx":[21.36645,22.37453,22.90166,21.91849], "fy":[10.40871,9.59268,10.58912,11.40161]}, - {"t":1.60766, "x":5.13597, "y":5.30639, "heading":-2.17524, "vx":-0.59744, "vy":-0.28328, "omega":0.2825, "ax":1.3544, "ay":0.6422, "alpha":-0.2699, "fx":[21.189,22.43151,23.08118,21.8657], "fy":[10.37565,9.37749,10.62407,11.61791]}, - {"t":1.63916, "x":5.11782, "y":5.29778, "heading":-2.16634, "vx":-0.55477, "vy":-0.26305, "omega":0.27399, "ax":1.35448, "ay":0.64224, "alpha":-0.32247, "fx":[21.00891,22.49211,23.26236,21.80925], "fy":[10.33888,9.15933,10.66249,11.8369]}, - {"t":1.67066, "x":5.10101, "y":5.28981, "heading":-2.15771, "vx":-0.51211, "vy":-0.24282, "omega":0.26384, "ax":1.35455, "ay":0.64227, "alpha":-0.37596, "fx":[20.82592,22.55633,23.44559,21.74923], "fy":[10.29852,8.93784,10.70431,12.059]}, - {"t":1.70216, "x":5.08556, "y":5.28248, "heading":-2.1494, "vx":-0.46944, "vy":-0.22259, "omega":0.25199, "ax":1.35461, "ay":0.6423, "alpha":-0.43045, "fx":[20.63971,22.62415,23.63125,21.68576], "fy":[10.25468,8.71266,10.74948,12.28463]}, - {"t":1.73366, "x":5.07144, "y":5.27579, "heading":-2.14146, "vx":-0.42677, "vy":-0.20235, "omega":0.23843, "ax":1.35466, "ay":0.64232, "alpha":-0.48604, "fx":[20.44996,22.69551,23.81975,21.61896], "fy":[10.20751,8.4834,10.79784,12.51421]}, - {"t":1.76516, "x":5.05867, "y":5.26973, "heading":-2.13395, "vx":-0.3841, "vy":-0.18212, "omega":0.22312, "ax":1.3547, "ay":0.64234, "alpha":-0.54283, "fx":[20.25629,22.77037,24.01145,21.54896], "fy":[10.15723,8.24968,10.8492,12.74817]}, - {"t":1.79666, "x":5.04724, "y":5.26432, "heading":-2.12692, "vx":-0.34142, "vy":-0.16189, "omega":0.20602, "ax":1.35474, "ay":0.64236, "alpha":-0.60092, "fx":[20.0583,22.84863,24.20677,21.47592], "fy":[10.10408,8.01105,10.90334,12.98698]}, - {"t":1.82816, "x":5.03716, "y":5.25954, "heading":-2.12043, "vx":-0.29875, "vy":-0.14165, "omega":0.18709, "ax":1.35477, "ay":0.64238, "alpha":-0.66042, "fx":[19.85555,22.93013,24.40617,21.40005], "fy":[10.04831,7.76701,10.96002,13.23113]}, - {"t":1.85967, "x":5.02842, "y":5.25539, "heading":-2.11454, "vx":-0.25607, "vy":-0.12142, "omega":0.16629, "ax":1.35481, "ay":0.64239, "alpha":-0.72143, "fx":[19.64755,23.01471,24.61009,21.32158], "fy":[9.99027,7.51707,11.01889,13.48114]}, - {"t":1.89117, "x":5.02103, "y":5.25189, "heading":-2.1093, "vx":-0.2134, "vy":-0.10118, "omega":0.14357, "ax":1.35483, "ay":0.6424, "alpha":-0.78406, "fx":[19.43377,23.10216,24.81904,21.24081], "fy":[9.93036,7.26067,11.07959,13.73758]}, - {"t":1.92267, "x":5.01498, "y":5.24902, "heading":-2.10478, "vx":-0.17072, "vy":-0.08095, "omega":0.11887, "ax":1.35486, "ay":0.64241, "alpha":-0.84843, "fx":[19.21364,23.19221,25.03353,21.15804], "fy":[9.86904,6.99721,11.14169,14.001]}, - {"t":1.95417, "x":5.01027, "y":5.24679, "heading":-2.10103, "vx":-0.12804, "vy":-0.06071, "omega":0.09214, "ax":1.35488, "ay":0.64242, "alpha":-0.91466, "fx":[18.98657,23.28456,25.25412,21.07369], "fy":[9.80683,6.72605,11.20468,14.27206]}, - {"t":1.98567, "x":5.00691, "y":5.24519, "heading":-2.09813, "vx":-0.08536, "vy":-0.04047, "omega":0.06333, "ax":1.3549, "ay":0.64243, "alpha":-0.98289, "fx":[18.75183,23.37884,25.48146,20.98818], "fy":[9.74439,6.44649,11.26796,14.55139]}, - {"t":2.01717, "x":5.00489, "y":5.24424, "heading":-2.09614, "vx":-0.04268, "vy":-0.02024, "omega":0.03237, "ax":1.35492, "ay":0.64244, "alpha":-1.02758, "fx":[18.5059,23.46284,25.71728,20.91556], "fy":[9.72826,6.35308,11.28328,14.64617]}, - {"t":2.04867, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.82157, "ay":-2.25873, "alpha":0.00548, "fx":[-78.81573,-78.83971,-78.83128,-78.8073], "fy":[-36.94249,-36.89133,-36.9095,-36.96062]}, + {"t":0.03424, "x":7.10849, "y":6.23424, "heading":-2.35619, "vx":-0.16509, "vy":-0.07734, "omega":0.00019, "ax":-4.82119, "ay":-2.25856, "alpha":0.00568, "fx":[-78.80914,-78.834,-78.82527,-78.80041], "fy":[-36.94026,-36.88724,-36.90607,-36.95906]}, + {"t":0.06848, "x":7.10001, "y":6.23027, "heading":-2.35619, "vx":-0.33017, "vy":-0.15467, "omega":0.00038, "ax":-4.82075, "ay":-2.25836, "alpha":0.00591, "fx":[-78.80161,-78.82747,-78.81839,-78.79253], "fy":[-36.93772,-36.88256,-36.90216,-36.95728]}, + {"t":0.10272, "x":7.08588, "y":6.22365, "heading":-2.35617, "vx":-0.49524, "vy":-0.232, "omega":0.00058, "ax":-4.82024, "ay":-2.25813, "alpha":0.00617, "fx":[-78.79291,-78.81994,-78.81045,-78.78343], "fy":[-36.93479,-36.87716,-36.89764,-36.95522]}, + {"t":0.13696, "x":7.06609, "y":6.21438, "heading":-2.35615, "vx":-0.66029, "vy":-0.30932, "omega":0.0008, "ax":-4.81964, "ay":-2.25787, "alpha":0.00648, "fx":[-78.78276,-78.81114,-78.80119,-78.77281], "fy":[-36.93136,-36.87085,-36.89236,-36.95282]}, + {"t":0.1712, "x":7.04066, "y":6.20246, "heading":-2.35613, "vx":-0.82531, "vy":-0.38663, "omega":0.00102, "ax":-4.81894, "ay":-2.25755, "alpha":0.00685, "fx":[-78.77076,-78.80074,-78.79023,-78.76026], "fy":[-36.92731,-36.86339,-36.88612,-36.94998]}, + {"t":0.20544, "x":7.00958, "y":6.1879, "heading":-2.35609, "vx":-0.99032, "vy":-0.46393, "omega":0.00125, "ax":-4.8181, "ay":-2.25718, "alpha":0.00729, "fx":[-78.75634,-78.78825,-78.77708,-78.74518], "fy":[-36.92244,-36.85444,-36.87863,-36.94658]}, + {"t":0.23968, "x":6.97284, "y":6.17069, "heading":-2.35605, "vx":-1.15529, "vy":-0.54122, "omega":0.0015, "ax":-4.81707, "ay":-2.25671, "alpha":0.00783, "fx":[-78.73871,-78.77297,-78.76099,-78.72673], "fy":[-36.91649,-36.84348,-36.86946,-36.94241]}, + {"t":0.27392, "x":6.93046, "y":6.15084, "heading":-2.356, "vx":-1.32023, "vy":-0.61849, "omega":0.00177, "ax":-4.81578, "ay":-2.25613, "alpha":0.0085, "fx":[-78.71664,-78.75385,-78.74086,-78.70366], "fy":[-36.90905,-36.82976,-36.858,-36.9372]}, + {"t":0.30817, "x":6.88243, "y":6.12834, "heading":-2.35594, "vx":-1.48512, "vy":-0.69574, "omega":0.00206, "ax":-4.81411, "ay":-2.25539, "alpha":0.00937, "fx":[-78.68822,-78.72923,-78.71495,-78.67395], "fy":[-36.89946,-36.81211,-36.84324,-36.93049]}, + {"t":0.34241, "x":6.82876, "y":6.1032, "heading":-2.35587, "vx":-1.64996, "vy":-0.77297, "omega":0.00238, "ax":-4.81189, "ay":-2.25439, "alpha":0.01053, "fx":[-78.65026,-78.69635,-78.68034,-78.63426], "fy":[-36.88665,-36.78852,-36.82352,-36.92153]}, + {"t":0.37665, "x":6.76944, "y":6.07541, "heading":-2.35579, "vx":-1.81472, "vy":-0.85016, "omega":0.00274, "ax":-4.80878, "ay":-2.253, "alpha":0.01216, "fx":[-78.59698,-78.6502,-78.6318,-78.57858], "fy":[-36.86867,-36.7554,-36.79586,-36.90896]}, + {"t":0.41089, "x":6.70449, "y":6.04498, "heading":-2.35569, "vx":-1.97938, "vy":-0.9273, "omega":0.00316, "ax":-4.80409, "ay":-2.2509, "alpha":0.01462, "fx":[-78.51675,-78.58074,-78.55875,-78.49478], "fy":[-36.84161,-36.70555,-36.75424,-36.89006]}, + {"t":0.44513, "x":6.6339, "y":6.01191, "heading":-2.35558, "vx":-2.14387, "vy":-1.00438, "omega":0.00366, "ax":-4.79624, "ay":-2.24737, "alpha":0.01876, "fx":[-78.38224,-78.46431,-78.43641,-78.35435], "fy":[-36.79624,-36.62195,-36.68451,-36.85841]}, + {"t":0.47937, "x":6.55768, "y":5.9762, "heading":-2.35546, "vx":-2.3081, "vy":-1.08133, "omega":0.0043, "ax":-4.78036, "ay":-2.24026, "alpha":0.02718, "fx":[-78.11014,-78.229,-78.18948,-78.07066], "fy":[-36.70453,-36.45283,-36.54371,-36.79459]}, + {"t":0.51361, "x":6.47584, "y":5.93786, "heading":-2.35531, "vx":-2.47178, "vy":-1.15803, "omega":0.00523, "ax":-4.7314, "ay":-2.2183, "alpha":0.05396, "fx":[-77.26808,-77.50373,-77.43066,-77.19529], "fy":[-36.42246,-35.92755,-36.10932,-36.60088]}, + {"t":0.54785, "x":6.38843, "y":5.89691, "heading":-2.35513, "vx":-2.63379, "vy":-1.23399, "omega":0.00708, "ax":3.32647, "ay":1.53345, "alpha":1.0632, "fx":[57.30949,53.11585,51.27974,55.82051], "fy":[23.82831,30.2232,26.51559,19.70882]}, + {"t":0.58209, "x":6.3002, "y":5.85555, "heading":-2.35489, "vx":-2.51989, "vy":-1.18148, "omega":0.04348, "ax":4.74177, "ay":2.21949, "alpha":0.04418, "fx":[77.58482,77.39231,77.45287,77.64552], "fy":[36.15653,36.56006,36.4133,36.00759]}, + {"t":0.61633, "x":6.2167, "y":5.8164, "heading":-2.3534, "vx":-2.35753, "vy":-1.10549, "omega":0.045, "ax":4.78333, "ay":2.2398, "alpha":0.02132, "fx":[78.22922,78.13614,78.16742,78.26052], "fy":[36.5538,36.75063,36.67945,36.48212]}, + {"t":0.65057, "x":6.13878, "y":5.77986, "heading":-2.35186, "vx":-2.19374, "vy":-1.0288, "omega":0.04573, "ax":4.79772, "ay":2.24684, "alpha":0.01356, "fx":[78.45295,78.39373,78.41414,78.47337], "fy":[36.69146,36.81708,36.77161,36.64578]}, + {"t":0.68481, "x":6.06648, "y":5.74595, "heading":-2.35029, "vx":-2.02947, "vy":-0.95186, "omega":0.04619, "ax":4.80502, "ay":2.2504, "alpha":0.00964, "fx":[78.56654,78.52444,78.53918,78.58128], "fy":[36.76135,36.8508,36.81832,36.72878]}, + {"t":0.71905, "x":5.9998, "y":5.71468, "heading":-2.34871, "vx":-1.86494, "vy":-0.87481, "omega":0.04652, "ax":4.80943, "ay":2.25256, "alpha":0.00728, "fx":[78.63526,78.60351,78.61475,78.6465], "fy":[36.80364,36.87117,36.84656,36.77896]}, + {"t":0.75329, "x":5.93877, "y":5.68604, "heading":-2.34712, "vx":-1.70026, "vy":-0.79768, "omega":0.04677, "ax":4.81239, "ay":2.25401, "alpha":0.00569, "fx":[78.68131,78.65649,78.66535,78.69018], "fy":[36.83197,36.8848,36.86546,36.8126]}, + {"t":0.78753, "x":5.88337, "y":5.66005, "heading":-2.34552, "vx":-1.53548, "vy":-0.7205, "omega":0.04697, "ax":4.8145, "ay":2.25504, "alpha":0.00455, "fx":[78.71432,78.69447,78.70161,78.72147], "fy":[36.85228,36.89455,36.87901,36.83671]}, + {"t":0.82177, "x":5.83362, "y":5.6367, "heading":-2.34391, "vx":-1.37063, "vy":-0.64329, "omega":0.04712, "ax":4.8161, "ay":2.25582, "alpha":0.0037, "fx":[78.73914,78.72302,78.72887,78.74499], "fy":[36.86754,36.90188,36.8892,36.85485]}, + {"t":0.85601, "x":5.78951, "y":5.616, "heading":-2.3423, "vx":-1.20573, "vy":-0.56605, "omega":0.04725, "ax":4.81734, "ay":2.25643, "alpha":0.00303, "fx":[78.75849,78.74528,78.7501,78.76331], "fy":[36.87944,36.90758,36.89714,36.869]}, + {"t":0.89025, "x":5.75105, "y":5.59794, "heading":-2.34068, "vx":-1.04078, "vy":-0.48878, "omega":0.04735, "ax":0.04227, "ay":-0.07579, "alpha":1.05708, "fx":[4.74064,0.47438,-3.33352,0.88287], "fy":[-1.14797,2.97283,-1.3317,-5.44936]}, + {"t":0.92268, "x":5.71732, "y":5.58205, "heading":-2.33914, "vx":-1.03941, "vy":-0.49124, "omega":0.08163, "ax":0.01701, "ay":-0.03593, "alpha":0.97427, "fx":[4.0009,0.09536,-3.43939,0.45523], "fy":[-0.49241,3.29507,-0.68356,-4.46852]}, + {"t":0.95511, "x":5.68363, "y":5.5661, "heading":-2.3365, "vx":-1.03886, "vy":-0.49241, "omega":0.11322, "ax":0.00371, "ay":-0.00782, "alpha":0.89474, "fx":[3.47802,-0.1192,-3.35193,0.2356], "fy":[-0.02735,3.43797,-0.22874,-3.6933]}, + {"t":0.98753, "x":5.64994, "y":5.55013, "heading":-2.33282, "vx":-1.03874, "vy":-0.49266, "omega":0.14224, "ax":0.00061, "ay":-0.00129, "alpha":0.81831, "fx":[3.13331,-0.16641,-3.11005,0.18313], "fy":[0.08534,3.24057,-0.1276,-3.28261]}, + {"t":1.01996, "x":5.61626, "y":5.53416, "heading":-2.32821, "vx":-1.03872, "vy":-0.4927, "omega":0.16877, "ax":-0.00004, "ay":0.00009, "alpha":0.74473, "fx":[2.83972,-0.17449,-2.83881,0.17093], "fy":[0.11345,2.97016,-0.11068,-2.96735]}, + {"t":1.05239, "x":5.58258, "y":5.51818, "heading":-2.32274, "vx":-1.03872, "vy":-0.4927, "omega":0.19292, "ax":-0.00015, "ay":0.00031, "alpha":0.67372, "fx":[2.56505,-0.17368,-2.56831,0.16717], "fy":[0.12196,2.69122,-0.11165,-2.68093]}, + {"t":1.08481, "x":5.54889, "y":5.5022, "heading":-2.31648, "vx":-1.03872, "vy":-0.49269, "omega":0.21477, "ax":-0.00023, "ay":0.00049, "alpha":0.60502, "fx":[2.29999,-0.17237,-2.30582,0.16298], "fy":[0.12814,2.42053,-0.11208,-2.40451]}, + {"t":1.11724, "x":5.51521, "y":5.48623, "heading":-2.30952, "vx":-1.03873, "vy":-0.49267, "omega":0.23439, "ax":-0.0006, "ay":0.00126, "alpha":0.53836, "fx":[2.03864,-0.17547,-2.05522,0.15301], "fy":[0.14209,2.16764,-0.10091,-2.12651]}, + {"t":1.14967, "x":5.48153, "y":5.47025, "heading":-2.30192, "vx":-1.03875, "vy":-0.49263, "omega":0.25184, "ax":-0.00061, "ay":0.00129, "alpha":0.47352, "fx":[1.79163,-0.17612,-1.80399,0.1484], "fy":[0.14167,1.90989,-0.09944,-1.86765]}, + {"t":1.18209, "x":5.44785, "y":5.45428, "heading":-2.29375, "vx":-1.03877, "vy":-0.49259, "omega":0.2672, "ax":0.00148, "ay":-0.00258, "alpha":0.41052, "fx":[1.60743,-0.20952,-1.50045,0.19903], "fy":[0.0745,1.59424,-0.15901,-1.67868]}, + {"t":1.21452, "x":5.41416, "y":5.4383, "heading":-2.28509, "vx":-1.03872, "vy":-0.49267, "omega":0.28051, "ax":0.89666, "ay":0.42914, "alpha":0.33358, "fx":[15.91888,14.45167,13.43655,14.82793], "fy":[7.09929,8.3646,6.93266,5.66569]}, + {"t":1.24695, "x":5.38095, "y":5.42255, "heading":-2.27599, "vx":-1.00965, "vy":-0.47876, "omega":0.29133, "ax":1.34557, "ay":0.63811, "alpha":0.26418, "fx":[22.94235,21.93879,20.99855,22.11014], "fy":[10.49491,11.51575,10.37189,9.34515]}, + {"t":1.27937, "x":5.34892, "y":5.40736, "heading":-2.26655, "vx":-0.96601, "vy":-0.45807, "omega":0.29989, "ax":1.35042, "ay":0.6404, "alpha":0.20878, "fx":[22.82629,22.01024,21.29521,22.17521], "fy":[10.52723,11.32679,10.41328,9.61008]}, + {"t":1.3118, "x":5.3183, "y":5.39285, "heading":-2.25682, "vx":-0.92222, "vy":-0.4373, "omega":0.30666, "ax":1.35205, "ay":0.64117, "alpha":0.15398, "fx":[22.65563,22.05146,21.52882,22.17788], "fy":[10.53215,11.11465,10.43282,9.84786]}, + {"t":1.34423, "x":5.28911, "y":5.379, "heading":-2.24688, "vx":-0.87838, "vy":-0.41651, "omega":0.31166, "ax":1.35287, "ay":0.64155, "alpha":0.09926, "fx":[22.47088,22.08917,21.74571,22.1617], "fy":[10.52727,10.89652,10.44982,10.0788]}, + {"t":1.37665, "x":5.26134, "y":5.36584, "heading":-2.23677, "vx":-0.83451, "vy":-0.39571, "omega":0.31487, "ax":1.35336, "ay":0.64178, "alpha":0.04437, "fx":[22.2799,22.12825,21.95606,22.13555], "fy":[10.51601,10.67534,10.46833,10.30757]}, + {"t":1.40908, "x":5.23499, "y":5.35334, "heading":-2.22656, "vx":-0.79063, "vy":-0.3749, "omega":0.31631, "ax":1.3537, "ay":0.64193, "alpha":-0.01088, "fx":[22.08505,22.17026,22.16366,22.10238], "fy":[10.4994,10.45176,10.4898,10.5361]}, + {"t":1.44151, "x":5.21006, "y":5.34152, "heading":-2.2163, "vx":-0.74673, "vy":-0.35408, "omega":0.31596, "ax":1.35393, "ay":0.64203, "alpha":-0.06666, "fx":[21.88707,22.21591,22.37046,22.06338], "fy":[10.47783,10.22572,10.51489,10.76555]}, + {"t":1.47393, "x":5.18656, "y":5.33038, "heading":-2.20606, "vx":-0.70283, "vy":-0.33326, "omega":0.3138, "ax":1.35411, "ay":0.64211, "alpha":-0.12311, "fx":[21.6861,22.26561,22.57766,22.01909], "fy":[10.45145,9.99694,10.54396,10.99677]}, + {"t":1.50636, "x":5.16448, "y":5.31991, "heading":-2.19588, "vx":-0.65892, "vy":-0.31244, "omega":0.30981, "ax":1.35425, "ay":0.64217, "alpha":-0.18039, "fx":[21.48196,22.31964,22.78614,21.96978], "fy":[10.42032,9.76502,10.57724,11.23049]}, + {"t":1.53879, "x":5.14383, "y":5.31012, "heading":-2.18584, "vx":-0.61501, "vy":-0.29162, "omega":0.30396, "ax":1.35436, "ay":0.64222, "alpha":-0.23854, "fx":[21.27492,22.37865,22.99517,21.91607], "fy":[10.38438,9.5294,10.61518,11.46724]}, + {"t":1.57121, "x":5.1246, "y":5.301, "heading":-2.17598, "vx":-0.57109, "vy":-0.27079, "omega":0.29622, "ax":1.35445, "ay":0.64226, "alpha":-0.29772, "fx":[21.06676,22.43547,23.2107,21.85783], "fy":[10.34328,9.29102,10.65669,11.70775]}, + {"t":1.60364, "x":5.10679, "y":5.29255, "heading":-2.16638, "vx":-0.52717, "vy":-0.24997, "omega":0.28657, "ax":1.35453, "ay":0.64229, "alpha":-0.35844, "fx":[20.8508,22.5039,23.42706,21.79398], "fy":[10.29817,9.0466,10.70317,11.95289]}, + {"t":1.63607, "x":5.09041, "y":5.28479, "heading":-2.15708, "vx":-0.48324, "vy":-0.22914, "omega":0.27495, "ax":1.35459, "ay":0.64232, "alpha":-0.42062, "fx":[20.63003,22.57716,23.64733,21.72544], "fy":[10.24848,8.79692,10.75405,12.20312]}, + {"t":1.66849, "x":5.07545, "y":5.27769, "heading":-2.14817, "vx":-0.43932, "vy":-0.20831, "omega":0.26131, "ax":1.35465, "ay":0.64234, "alpha":-0.48452, "fx":[20.4026,22.65569,23.87256,21.65275], "fy":[10.19455,8.5413,10.80917,12.45905]}, + {"t":1.70092, "x":5.06192, "y":5.27128, "heading":-2.13969, "vx":-0.39539, "vy":-0.18748, "omega":0.24559, "ax":1.3547, "ay":0.64236, "alpha":-0.55008, "fx":[20.17063,22.73852,24.10254,21.57506], "fy":[10.136,8.27921,10.86857,12.72155]}, + {"t":1.73335, "x":5.04981, "y":5.26553, "heading":-2.13173, "vx":-0.35146, "vy":-0.16665, "omega":0.22776, "ax":1.35474, "ay":0.64238, "alpha":-0.61761, "fx":[19.93199,22.82618,24.33837,21.49296], "fy":[10.0733,8.00985,10.93201,12.99129]}, + {"t":1.76578, "x":5.03912, "y":5.26047, "heading":-2.12434, "vx":-0.30754, "vy":-0.14582, "omega":0.20773, "ax":1.35477, "ay":0.64239, "alpha":-0.68728, "fx":[19.68595,22.91857,24.58078,21.40664], "fy":[10.00669,7.73242,10.99923,13.26908]}, + {"t":1.7982, "x":5.02986, "y":5.25608, "heading":-2.11761, "vx":-0.2636, "vy":-0.12499, "omega":0.18544, "ax":1.35481, "ay":0.6424, "alpha":-0.75929, "fx":[19.4317,23.01556,24.83051,21.31633], "fy":[9.93649,7.4461,11.06993,13.55576]}, + {"t":1.83063, "x":5.02203, "y":5.25236, "heading":-2.1116, "vx":-0.21967, "vy":-0.10416, "omega":0.16082, "ax":1.35484, "ay":0.64242, "alpha":-0.83382, "fx":[19.16841,23.11691,25.0884,21.22232], "fy":[9.8631,7.14997,11.14374,13.85225]}, + {"t":1.86306, "x":5.01562, "y":5.24932, "heading":-2.10638, "vx":-0.17574, "vy":-0.08333, "omega":0.13378, "ax":1.35486, "ay":0.64243, "alpha":-0.9111, "fx":[18.89509,23.22239,25.35532,21.12499], "fy":[9.78699,6.84305,11.22017,14.15952]}, + {"t":1.89548, "x":5.01063, "y":5.24696, "heading":-2.10204, "vx":-0.13181, "vy":-0.0625, "omega":0.10424, "ax":1.35489, "ay":0.64244, "alpha":-0.99134, "fx":[18.61073,23.33164,25.63221,21.02479], "fy":[9.70878,6.52428,11.29866,14.47864]}, + {"t":1.92791, "x":5.00707, "y":5.24527, "heading":-2.09866, "vx":-0.08787, "vy":-0.04166, "omega":0.07209, "ax":1.35491, "ay":0.64244, "alpha":-1.07477, "fx":[18.31417,23.44426,25.92009,20.92229], "fy":[9.62919,6.19249,11.37852,14.81073]}, + {"t":1.96034, "x":5.00493, "y":5.24426, "heading":-2.09632, "vx":-0.04394, "vy":-0.02083, "omega":0.03724, "ax":1.35493, "ay":0.64245, "alpha":-1.14853, "fx":[18.00309,23.55492,26.22061,20.8235], "fy":[9.57375,5.94578,11.43432,15.05761]}, + {"t":1.99276, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/LtoPLM.traj b/src/main/deploy/choreo/LtoPLM.traj index 6e5a55c3..81e8561f 100644 --- a/src/main/deploy/choreo/LtoPLM.traj +++ b/src/main/deploy/choreo/LtoPLM.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.0301326513290403, "y":6.979412078857422, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"PLM.x", "val":1.0301326513290403}, "y":{"exp":"PLM.y", "val":6.979412078857422}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,47 +26,45 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.68274], + "waypoints":[0.0,1.56657], "samples":[ - {"t":0.0, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.76305, "ay":2.67232, "alpha":0.15651, "fx":[-60.86418,-61.7697,-62.17457,-61.26628], "fy":[44.60357,43.34199,42.7567,44.04716]}, - {"t":0.04428, "x":3.68797, "y":5.09196, "heading":-1.04555, "vx":-0.16664, "vy":0.11834, "omega":0.00693, "ax":-3.76285, "ay":2.67218, "alpha":0.15647, "fx":[-60.86116,-61.7664,-62.17108,-61.26306], "fy":[44.60096,43.33982,42.75468,44.04468]}, - {"t":0.08857, "x":3.6769, "y":5.09982, "heading":-1.04525, "vx":-0.33327, "vy":0.23667, "omega":0.01386, "ax":-3.76263, "ay":2.67202, "alpha":0.15643, "fx":[-60.85784,-61.76291,-62.1671,-61.25924], "fy":[44.59795,43.33711,42.75251,44.0422]}, - {"t":0.13285, "x":3.65845, "y":5.11292, "heading":-1.04463, "vx":-0.49988, "vy":0.35499, "omega":0.02079, "ax":-3.76237, "ay":2.67184, "alpha":0.15638, "fx":[-60.85415,-61.75913,-62.16255,-61.25474], "fy":[44.59446,43.3338,42.75014,44.03964]}, - {"t":0.17713, "x":3.63263, "y":5.13126, "heading":-1.04371, "vx":-0.66649, "vy":0.47331, "omega":0.02771, "ax":-3.76209, "ay":2.67164, "alpha":0.15632, "fx":[-60.85,-61.75498,-62.15732,-61.24948], "fy":[44.59042,43.32983,42.7475,44.03694]}, - {"t":0.22141, "x":3.59942, "y":5.15484, "heading":-1.04248, "vx":-0.83309, "vy":0.59161, "omega":0.03463, "ax":-3.76176, "ay":2.6714, "alpha":0.15626, "fx":[-60.84528,-61.75032,-62.1513,-61.24332], "fy":[44.58574,43.32512,42.74452,44.034]}, - {"t":0.2657, "x":3.55885, "y":5.18365, "heading":-1.04095, "vx":-0.99967, "vy":0.70991, "omega":0.04155, "ax":-3.76137, "ay":2.67113, "alpha":0.1562, "fx":[-60.83982,-61.74498,-62.14429,-61.23611], "fy":[44.58027,43.31955,42.74109,44.03069]}, - {"t":0.30998, "x":3.51089, "y":5.21771, "heading":-1.03911, "vx":-1.16623, "vy":0.8282, "omega":0.04847, "ax":-3.76092, "ay":2.67081, "alpha":0.15612, "fx":[-60.83341,-61.73875,-62.13605,-61.22761], "fy":[44.57386,43.31297,42.73707,44.02685]}, - {"t":0.35426, "x":3.45556, "y":5.257, "heading":-1.03696, "vx":-1.33277, "vy":0.94647, "omega":0.05538, "ax":-3.76039, "ay":2.67043, "alpha":0.15603, "fx":[-60.82576,-61.73129,-62.12627,-61.21752], "fy":[44.56625,43.30518,42.73225,44.02223]}, - {"t":0.39854, "x":3.39285, "y":5.30153, "heading":-1.03451, "vx":-1.49929, "vy":1.06472, "omega":0.06229, "ax":-3.75974, "ay":2.66997, "alpha":0.15593, "fx":[-60.81646,-61.72218,-62.11448,-61.2054], "fy":[44.5571,43.29587,42.72634,44.01651]}, - {"t":0.44283, "x":3.32277, "y":5.3513, "heading":-1.03175, "vx":-1.66578, "vy":1.18295, "omega":0.0692, "ax":-3.75894, "ay":2.6694, "alpha":0.15581, "fx":[-60.80487,-61.71076,-62.09998,-61.1906], "fy":[44.54593,43.28462,42.71894,44.0092]}, - {"t":0.48711, "x":3.24532, "y":5.4063, "heading":-1.02869, "vx":-1.83224, "vy":1.30116, "omega":0.0761, "ax":-3.75793, "ay":2.66868, "alpha":0.15566, "fx":[-60.79006,-61.69602,-62.08175,-61.17214], "fy":[44.53196,43.27074,42.70937,43.99956]}, - {"t":0.53139, "x":3.1605, "y":5.46654, "heading":-1.02532, "vx":-1.99865, "vy":1.41934, "omega":0.08299, "ax":-3.7566, "ay":2.66774, "alpha":0.15547, "fx":[-60.77049,-61.67635,-62.0581,-61.1484], "fy":[44.51399,43.25315,42.69658,43.98638]}, - {"t":0.57567, "x":3.06831, "y":5.532, "heading":-1.02164, "vx":-2.165, "vy":1.53747, "omega":0.08987, "ax":-3.7548, "ay":2.66646, "alpha":0.15522, "fx":[-60.74348,-61.64898,-62.02614,-61.11662], "fy":[44.4899,43.22997,42.67873,43.96761]}, - {"t":0.61996, "x":2.96876, "y":5.6027, "heading":-1.01766, "vx":-2.33127, "vy":1.65555, "omega":0.09675, "ax":-3.75219, "ay":2.66461, "alpha":0.1549, "fx":[-60.70399,-61.60866,-61.98044,-61.07155], "fy":[44.4558,43.19761,42.65229,43.93936]}, - {"t":0.66424, "x":2.86185, "y":5.67862, "heading":-1.01338, "vx":-2.49743, "vy":1.77354, "omega":0.10361, "ax":-3.74809, "ay":2.6617, "alpha":0.15444, "fx":[-60.64115,-61.54419,-61.90941,-61.00195], "fy":[44.40333,43.1484,42.60967,43.89335]}, - {"t":0.70852, "x":2.74758, "y":5.75977, "heading":-1.00879, "vx":-2.6634, "vy":1.89141, "omega":0.11045, "ax":-3.74072, "ay":2.65646, "alpha":0.15378, "fx":[-60.52665,-61.42656,-61.78312,-60.87856], "fy":[44.31106,43.06232,42.53098,43.8082]}, - {"t":0.7528, "x":2.62597, "y":5.84613, "heading":-1.0039, "vx":-2.82905, "vy":2.00904, "omega":0.11726, "ax":-3.72358, "ay":2.64429, "alpha":0.15271, "fx":[-60.25651,-61.15002,-61.49271,-60.59432], "fy":[44.10133,42.86594,42.34282,43.6061]}, - {"t":0.79709, "x":2.49704, "y":5.93769, "heading":-0.99871, "vx":-2.99394, "vy":2.12614, "omega":0.12402, "ax":-3.63893, "ay":2.58417, "alpha":0.15047, "fx":[-58.90442,-59.77834,-60.0773,-59.19817], "fy":[43.09131,41.90523,41.38833,42.60012]}, - {"t":0.84137, "x":2.36089, "y":6.03437, "heading":-0.99322, "vx":-3.15508, "vy":2.24057, "omega":0.13068, "ax":3.63892, "ay":-2.58418, "alpha":-0.14814, "fx":[58.91499,59.77683,60.06655,59.19944], "fy":[-43.07652,-41.90605,-41.40371,-42.59922]}, - {"t":0.88565, "x":2.22475, "y":6.13106, "heading":-0.98743, "vx":-2.99394, "vy":2.12614, "omega":0.12412, "ax":3.72358, "ay":-2.64429, "alpha":-0.15199, "fx":[60.26428,61.1584,61.4853,60.58556], "fy":[-44.09097,-42.8537,-42.35324,-43.61851]}, - {"t":0.92993, "x":2.09582, "y":6.22262, "heading":-0.98193, "vx":-2.82905, "vy":2.00904, "omega":0.11739, "ax":3.74072, "ay":-2.65647, "alpha":-0.15343, "fx":[60.53611,61.44197,61.77431,60.8625], "fy":[-44.29844,-43.04014,-42.54342,-43.8307]}, - {"t":0.97422, "x":1.97421, "y":6.30898, "heading":-0.97674, "vx":-2.6634, "vy":1.89141, "omega":0.1106, "ax":3.74809, "ay":-2.6617, "alpha":-0.15427, "fx":[60.65285,61.56594,61.89864,60.97928], "fy":[-44.38766,-43.11719,-42.62497,-43.925]}, - {"t":1.0185, "x":1.85994, "y":6.39013, "heading":-0.97184, "vx":-2.49743, "vy":1.77354, "omega":0.10377, "ax":3.75219, "ay":-2.66461, "alpha":-0.15483, "fx":[60.718,61.63628,61.9676,61.04277], "fy":[-44.43696,-43.15804,-42.6706,-43.97949]}, - {"t":1.06278, "x":1.75303, "y":6.46605, "heading":-0.96724, "vx":-2.33127, "vy":1.65555, "omega":0.09691, "ax":3.7548, "ay":-2.66646, "alpha":-0.15524, "fx":[60.75976,61.68205,62.01127,61.08216], "fy":[-44.46798,-43.18262,-42.7,-44.01562]}, - {"t":1.10706, "x":1.65348, "y":6.53675, "heading":-0.96295, "vx":-2.165, "vy":1.53747, "omega":0.09003, "ax":3.75661, "ay":-2.66774, "alpha":-0.15556, "fx":[60.78892,61.71447,62.04129,61.10868], "fy":[-44.48911,-43.19861,-42.72068,-44.04169]}, - {"t":1.15135, "x":1.56129, "y":6.60221, "heading":-0.95896, "vx":-1.99865, "vy":1.41934, "omega":0.08315, "ax":3.75793, "ay":-2.66868, "alpha":-0.1558, "fx":[60.81052,61.73878,62.06312,61.12757], "fy":[-44.50431,-43.20957,-42.73613,-44.06159]}, - {"t":1.19563, "x":1.47647, "y":6.66245, "heading":-0.95528, "vx":-1.83224, "vy":1.30116, "omega":0.07625, "ax":3.75894, "ay":-2.6694, "alpha":-0.156, "fx":[60.82721,61.75778,62.07966,61.1416], "fy":[-44.51571,-43.21737,-42.74817,-44.0774]}, - {"t":1.23991, "x":1.39902, "y":6.71745, "heading":-0.95191, "vx":-1.66578, "vy":1.18295, "omega":0.06934, "ax":3.75974, "ay":-2.66997, "alpha":-0.15616, "fx":[60.84052,61.77307,62.09259,61.15236], "fy":[-44.52452,-43.22311,-42.75785,-44.0903]}, - {"t":1.28419, "x":1.32894, "y":6.76722, "heading":-0.94884, "vx":-1.49929, "vy":1.06472, "omega":0.06242, "ax":3.76039, "ay":-2.67043, "alpha":-0.15629, "fx":[60.85139,61.78566,62.10297,61.16085], "fy":[-44.53151,-43.22745,-42.76582,-44.10105]}, - {"t":1.32848, "x":1.26623, "y":6.81175, "heading":-0.94607, "vx":-1.33277, "vy":0.94646, "omega":0.0555, "ax":3.76093, "ay":-2.67081, "alpha":-0.15641, "fx":[60.86044,61.79621,62.11148,61.16772], "fy":[-44.5372,-43.23083,-42.7725,-44.11014]}, - {"t":1.37276, "x":1.2109, "y":6.85104, "heading":-0.94361, "vx":-1.16623, "vy":0.82819, "omega":0.04858, "ax":3.76137, "ay":-2.67113, "alpha":-0.15651, "fx":[60.86809,61.80515,62.11859,61.1734], "fy":[-44.54191,-43.23355,-42.77816,-44.11789]}, - {"t":1.41704, "x":1.16294, "y":6.8851, "heading":-0.94146, "vx":-0.99967, "vy":0.70991, "omega":0.04165, "ax":3.76176, "ay":-2.6714, "alpha":-0.15659, "fx":[60.87462,61.8128,62.12463,61.1782], "fy":[-44.5459,-43.23582,-42.78302,-44.12455]}, - {"t":1.46132, "x":1.12237, "y":6.91391, "heading":-0.93962, "vx":-0.83309, "vy":0.59161, "omega":0.03471, "ax":3.76209, "ay":-2.67164, "alpha":-0.15666, "fx":[60.88025,61.81939,62.12984,61.18234], "fy":[-44.54934,-43.23778,-42.7872,-44.13029]}, - {"t":1.50561, "x":1.08916, "y":6.93749, "heading":-0.93808, "vx":-0.66649, "vy":0.47331, "omega":0.02777, "ax":3.76237, "ay":-2.67184, "alpha":-0.15673, "fx":[60.88513,61.82509,62.13439,61.18599], "fy":[-44.55237,-43.23954,-42.79081,-44.13522]}, - {"t":1.54989, "x":1.06334, "y":6.95583, "heading":-0.93685, "vx":-0.49988, "vy":0.35499, "omega":0.02083, "ax":3.76263, "ay":-2.67202, "alpha":-0.15678, "fx":[60.88938,61.83003,62.13843,61.18928], "fy":[-44.55507,-43.2412,-42.79394,-44.13945]}, - {"t":1.59417, "x":1.04489, "y":6.96893, "heading":-0.93593, "vx":-0.33327, "vy":0.23667, "omega":0.01389, "ax":3.76285, "ay":-2.67218, "alpha":-0.15683, "fx":[60.8931,61.83429,62.14205,61.19229], "fy":[-44.55754,-43.24281,-42.79664,-44.14304]}, - {"t":1.63845, "x":1.03382, "y":6.97679, "heading":-0.93531, "vx":-0.16664, "vy":0.11834, "omega":0.00695, "ax":3.76305, "ay":-2.67232, "alpha":-0.15687, "fx":[60.89634,61.83798,62.14534,61.19511], "fy":[-44.55983,-43.24443,-42.79897,-44.14607]}, - {"t":1.68274, "x":1.03013, "y":6.97941, "heading":-0.93501, "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.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.3419, "ay":3.08339, "alpha":0.18101, "fx":[-70.22489,-71.27213,-71.74034,-70.68989], "fy":[51.46716,50.00819,49.33124,50.82369]}, + {"t":0.04352, "x":3.68755, "y":5.09226, "heading":-1.04555, "vx":-0.18894, "vy":0.13418, "omega":0.00788, "ax":-4.34165, "ay":3.08321, "alpha":0.18093, "fx":[-70.22124,-71.26798,-71.73586,-70.68592], "fy":[51.46375,50.00555,49.3289,50.82054]}, + {"t":0.08703, "x":3.67521, "y":5.10101, "heading":-1.04521, "vx":-0.37787, "vy":0.26834, "omega":0.01575, "ax":-4.34137, "ay":3.08302, "alpha":0.18084, "fx":[-70.21721,-71.26354,-71.73072,-70.68116], "fy":[51.45977,50.0022,49.32638,50.81735]}, + {"t":0.13055, "x":3.65466, "y":5.11561, "heading":-1.04452, "vx":-0.56679, "vy":0.4025, "omega":0.02362, "ax":-4.34105, "ay":3.08279, "alpha":0.18074, "fx":[-70.2127,-71.25871,-71.7248,-70.67553], "fy":[51.45513,49.99808,49.32361,50.81405]}, + {"t":0.17406, "x":3.62589, "y":5.13605, "heading":-1.0435, "vx":-0.75569, "vy":0.53665, "omega":0.03148, "ax":-4.34069, "ay":3.08253, "alpha":0.18063, "fx":[-70.20759,-71.25334,-71.71795,-70.66887], "fy":[51.44972,49.9931,49.3205,50.81051]}, + {"t":0.21758, "x":3.58889, "y":5.16232, "heading":-1.04213, "vx":-0.94458, "vy":0.67079, "omega":0.03934, "ax":-4.34026, "ay":3.08223, "alpha":0.1805, "fx":[-70.2017,-71.24725,-71.70996,-70.661], "fy":[51.44338,49.98712,49.31695,50.80659]}, + {"t":0.26109, "x":3.54368, "y":5.19442, "heading":-1.04041, "vx":-1.13345, "vy":0.80492, "omega":0.0472, "ax":-4.33976, "ay":3.08187, "alpha":0.18036, "fx":[-70.1948,-71.24017,-71.70057,-70.65169], "fy":[51.43592,49.98,49.31281,50.80211]}, + {"t":0.30461, "x":3.49025, "y":5.23237, "heading":-1.03836, "vx":-1.3223, "vy":0.93903, "omega":0.05505, "ax":-4.33916, "ay":3.08145, "alpha":0.18019, "fx":[-70.18659,-71.23175,-71.68939,-70.64059], "fy":[51.42704,49.9715,49.30787,50.79679]}, + {"t":0.34813, "x":3.4286, "y":5.27615, "heading":-1.03597, "vx":-1.51112, "vy":1.07312, "omega":0.06289, "ax":-4.33844, "ay":3.08094, "alpha":0.17999, "fx":[-70.1766,-71.22149,-71.67589,-70.62722], "fy":[51.41635,49.9613,49.30183,50.79025]}, + {"t":0.39164, "x":3.35873, "y":5.32576, "heading":-1.03323, "vx":-1.69991, "vy":1.20719, "omega":0.07072, "ax":-4.33755, "ay":3.0803, "alpha":0.17974, "fx":[-70.16417,-71.20866,-71.65931,-70.61087], "fy":[51.40327,49.94893,49.29425,50.78194]}, + {"t":0.43516, "x":3.28065, "y":5.38121, "heading":-1.03015, "vx":-1.88866, "vy":1.34123, "omega":0.07854, "ax":-4.33643, "ay":3.0795, "alpha":0.17944, "fx":[-70.14827,-71.19213,-71.63843,-70.59045], "fy":[51.38691,49.93366,49.28444,50.77102]}, + {"t":0.47867, "x":3.19436, "y":5.44249, "heading":-1.02673, "vx":-2.07737, "vy":1.47524, "omega":0.08635, "ax":-4.33495, "ay":3.07846, "alpha":0.17906, "fx":[-70.12724,-71.17007,-71.61134,-70.5642], "fy":[51.36584,49.9143,49.2713,50.7561]}, + {"t":0.52219, "x":3.09986, "y":5.5096, "heading":-1.02298, "vx":-2.26601, "vy":1.6092, "omega":0.09414, "ax":-4.33294, "ay":3.07703, "alpha":0.17855, "fx":[-70.09819,-71.13935,-71.57475,-70.52906], "fy":[51.33762,49.88879,49.25287,50.73485]}, + {"t":0.5657, "x":2.99715, "y":5.58254, "heading":-1.01888, "vx":-2.45456, "vy":1.7431, "omega":0.10191, "ax":-4.33004, "ay":3.07496, "alpha":0.17787, "fx":[-70.05562,-71.09407,-71.52245,-70.47925], "fy":[51.29767,49.85325,49.22545,50.70284]}, + {"t":0.60922, "x":2.88623, "y":5.66131, "heading":-1.01444, "vx":-2.64298, "vy":1.87691, "omega":0.10965, "ax":-4.32547, "ay":3.07172, "alpha":0.17688, "fx":[-69.98771,-71.02161,-71.44124,-70.40237], "fy":[51.23632,49.79927,49.18098,50.65067]}, + {"t":0.65274, "x":2.76713, "y":5.74589, "heading":-1.00967, "vx":-2.83121, "vy":2.01058, "omega":0.11735, "ax":-4.31726, "ay":3.06589, "alpha":0.17533, "fx":[-69.86348,-70.88931,-71.2971,-70.26609], "fy":[51.12875,49.7049,49.09815,50.55408]}, + {"t":0.69625, "x":2.63984, "y":5.83628, "heading":-1.00457, "vx":-3.01908, "vy":2.14399, "omega":0.12498, "ax":-4.29814, "ay":3.05231, "alpha":0.17256, "fx":[-69.56861,-70.57825,-70.96677,-69.95178], "fy":[50.88579,49.48919,48.89763,50.32511]}, + {"t":0.73977, "x":2.50439, "y":5.93247, "heading":-0.99913, "vx":-3.20611, "vy":2.27681, "omega":0.13249, "ax":-4.20356, "ay":2.98514, "alpha":0.16516, "fx":[-68.07659,-69.0363,-69.36653,-68.40142], "fy":[49.73011,48.42595,47.85915,49.19021]}, + {"t":0.78328, "x":2.36089, "y":6.03437, "heading":-0.99336, "vx":-3.38903, "vy":2.40671, "omega":0.13967, "ax":4.20355, "ay":-2.98515, "alpha":-0.1626, "fx":[68.08832,69.03481,69.35461,68.40263], "fy":[-49.71372,-48.42664,-47.87617,-49.18946]}, + {"t":0.8268, "x":2.2174, "y":6.13628, "heading":-0.98728, "vx":-3.20611, "vy":2.27681, "omega":0.1326, "ax":4.29814, "ay":-3.05231, "alpha":-0.17178, "fx":[69.57756,70.58832,70.95825,69.94125], "fy":[-50.87387,-49.47451,-48.9096,-50.34]}, + {"t":0.87032, "x":2.08195, "y":6.23247, "heading":-0.98151, "vx":-3.01908, "vy":2.14399, "omega":0.12512, "ax":4.31726, "ay":-3.06589, "alpha":-0.17495, "fx":[69.8746,70.90782,71.28676,70.2468], "fy":[-51.11391,-49.67827,-49.11276,-50.58109]}, + {"t":0.91383, "x":1.95466, "y":6.32286, "heading":-0.97607, "vx":-2.83121, "vy":2.01057, "omega":0.11751, "ax":4.32547, "ay":-3.07172, "alpha":-0.1767, "fx":[70.0016,71.04777,71.42845,70.37512], "fy":[-51.2177,-49.76176,-49.19914,-50.68872]}, + {"t":0.95735, "x":1.83555, "y":6.40744, "heading":-0.97096, "vx":-2.64298, "vy":1.87691, "omega":0.10982, "ax":4.33004, "ay":-3.07496, "alpha":-0.17782, "fx":[70.07234,71.1273,71.50714,70.44464], "fy":[-51.27519,-49.80566,-49.2473,-50.75111]}, + {"t":1.00086, "x":1.72464, "y":6.48621, "heading":-0.96618, "vx":-2.45456, "vy":1.7431, "omega":0.10208, "ax":4.33294, "ay":-3.07703, "alpha":-0.1786, "fx":[70.11765,71.17912,71.55698,70.48762], "fy":[-51.31139,-49.83187,-49.2783,-50.79258]}, + {"t":1.04438, "x":1.62193, "y":6.55915, "heading":-0.96173, "vx":-2.26601, "vy":1.6092, "omega":0.09431, "ax":4.33495, "ay":-3.07846, "alpha":-0.17918, "fx":[70.1493,71.21586,71.59124,70.51648], "fy":[-51.33607,-49.84878,-49.30013,-50.82255]}, + {"t":1.08789, "x":1.52743, "y":6.62626, "heading":-0.95763, "vx":-2.07737, "vy":1.47524, "omega":0.08652, "ax":4.33643, "ay":-3.0795, "alpha":-0.17963, "fx":[70.17274,71.24344,71.61615,70.53698], "fy":[-51.35383,-49.86027,-49.31645,-50.84544]}, + {"t":1.13141, "x":1.44114, "y":6.68754, "heading":-0.95387, "vx":-1.88866, "vy":1.34123, "omega":0.0787, "ax":4.33755, "ay":-3.0803, "alpha":-0.17998, "fx":[70.19086,71.26499,71.63502,70.55217], "fy":[-51.36714,-49.86838,-49.32919,-50.86363]}, + {"t":1.17493, "x":1.36306, "y":6.74299, "heading":-0.95044, "vx":-1.69991, "vy":1.20719, "omega":0.07087, "ax":4.33844, "ay":-3.08093, "alpha":-0.18027, "fx":[70.20532,71.28233,71.64978,70.56381], "fy":[-51.37745,-49.87432,-49.33944,-50.87846]}, + {"t":1.21844, "x":1.29319, "y":6.7926, "heading":-0.94736, "vx":-1.51112, "vy":1.07312, "omega":0.06302, "ax":4.33917, "ay":-3.08145, "alpha":-0.1805, "fx":[70.21712,71.2966,71.66163,70.573], "fy":[-51.38565,-49.87879,-49.34788,-50.8908]}, + {"t":1.26196, "x":1.23154, "y":6.83638, "heading":-0.94461, "vx":-1.3223, "vy":0.93903, "omega":0.05517, "ax":4.33976, "ay":-3.08187, "alpha":-0.1807, "fx":[70.22694,71.30853,71.67136,70.58043], "fy":[-51.39232,-49.88228,-49.35494,-50.9012]}, + {"t":1.30547, "x":1.17811, "y":6.87432, "heading":-0.94221, "vx":-1.13345, "vy":0.80492, "omega":0.0473, "ax":4.34026, "ay":-3.08222, "alpha":-0.18087, "fx":[70.23523,71.31862,71.6795,70.58661], "fy":[-51.39788,-49.88511,-49.36092,-50.91003]}, + {"t":1.34899, "x":1.1329, "y":6.90643, "heading":-0.94016, "vx":-0.94458, "vy":0.67079, "omega":0.03943, "ax":4.34069, "ay":-3.08253, "alpha":-0.18102, "fx":[70.24229,71.32722,71.68642,70.59185], "fy":[-51.4026,-49.8875,-49.36603,-50.91758]}, + {"t":1.3925, "x":1.0959, "y":6.9327, "heading":-0.93844, "vx":-0.75569, "vy":0.53665, "omega":0.03156, "ax":4.34105, "ay":-3.08279, "alpha":-0.18114, "fx":[70.24836,71.3346,71.69241,70.59641], "fy":[-51.40669,-49.88962,-49.3704,-50.92402]}, + {"t":1.43602, "x":1.06713, "y":6.95314, "heading":-0.93707, "vx":-0.56679, "vy":0.4025, "omega":0.02367, "ax":4.34137, "ay":-3.08301, "alpha":-0.18125, "fx":[70.2536,71.34095,71.69766,70.60047], "fy":[-51.41032,-49.89159,-49.37416,-50.92952]}, + {"t":1.47954, "x":1.04658, "y":6.96773, "heading":-0.93604, "vx":-0.37787, "vy":0.26834, "omega":0.01579, "ax":4.34165, "ay":-3.08321, "alpha":-0.18134, "fx":[70.25814,71.3464,71.70234,70.60417], "fy":[-51.41359,-49.89349,-49.37737,-50.93416]}, + {"t":1.52305, "x":1.03424, "y":6.97649, "heading":-0.93535, "vx":-0.18894, "vy":0.13418, "omega":0.00789, "ax":4.3419, "ay":-3.08339, "alpha":-0.18143, "fx":[70.26208,71.35106,71.70655,70.60761], "fy":[-51.4166,-49.8954,-49.38011,-50.93805]}, + {"t":1.56657, "x":1.03013, "y":6.97941, "heading":-0.93501, "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/PLMtoA.traj b/src/main/deploy/choreo/PLMtoA.traj index 1d78e8d1..a78e6cfa 100644 --- a/src/main/deploy/choreo/PLMtoA.traj +++ b/src/main/deploy/choreo/PLMtoA.traj @@ -36,94 +36,94 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.42603,2.20774], + "waypoints":[0.0,1.33923,2.12091], "samples":[ - {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.8986, "ay":-3.53745, "alpha":1.80025, "fx":[55.97527,39.29158,40.54674,53.73245], "fy":[-50.49105,-64.33028,-63.58517,-52.91606]}, - {"t":0.02377, "x":1.03095, "y":6.97841, "heading":-0.93501, "vx":0.06889, "vy":-0.08408, "omega":0.04279, "ax":2.89823, "ay":-3.53755, "alpha":1.79848, "fx":[55.95996,39.2934,40.54703,53.72196], "fy":[-50.50162,-64.32408,-63.58094,-52.9219]}, - {"t":0.04753, "x":1.03341, "y":6.97542, "heading":-0.93399, "vx":0.13777, "vy":-0.16815, "omega":0.08553, "ax":2.89785, "ay":-3.53764, "alpha":1.79665, "fx":[55.9382,39.28688,40.55493,53.7176], "fy":[-50.51892,-64.3227,-63.57162,-52.92119]}, - {"t":0.0713, "x":1.0375, "y":6.97042, "heading":-0.93196, "vx":0.20665, "vy":-0.25223, "omega":0.12823, "ax":2.89746, "ay":-3.53773, "alpha":1.79475, "fx":[55.90986,39.27208,40.57041,53.71929], "fy":[-50.54301,-64.32606,-63.55721,-52.91401]}, - {"t":0.09507, "x":1.04323, "y":6.96343, "heading":-0.92891, "vx":0.27551, "vy":-0.33631, "omega":0.17089, "ax":2.89704, "ay":-3.53781, "alpha":1.79276, "fx":[55.8748,39.24913,40.59349,53.72689], "fy":[-50.57398,-64.33406,-63.53766,-52.90045]}, - {"t":0.11884, "x":1.0506, "y":6.95443, "heading":-0.92485, "vx":0.34437, "vy":-0.4204, "omega":0.2135, "ax":2.8966, "ay":-3.53791, "alpha":1.79066, "fx":[55.83282,39.21821,40.62415,53.74025], "fy":[-50.61199,-64.34653,-63.51293,-52.88061]}, - {"t":0.1426, "x":1.0596, "y":6.94344, "heading":-0.91977, "vx":0.41321, "vy":-0.50448, "omega":0.25606, "ax":2.89613, "ay":-3.538, "alpha":1.78843, "fx":[55.78368,39.17954,40.66242,53.7592], "fy":[-50.65721,-64.36331,-63.48297,-52.85463]}, - {"t":0.16637, "x":1.07024, "y":6.93045, "heading":-0.91369, "vx":0.48204, "vy":-0.58857, "omega":0.29856, "ax":2.89563, "ay":-3.53809, "alpha":1.78604, "fx":[55.72707,39.13337,40.70832,53.78352], "fy":[-50.70986,-64.38416,-63.44769,-52.82266]}, - {"t":0.19014, "x":1.08251, "y":6.91547, "heading":-0.90659, "vx":0.55086, "vy":-0.67266, "omega":0.34101, "ax":2.8951, "ay":-3.53819, "alpha":1.78346, "fx":[55.66265,39.08001,40.76187,53.81296], "fy":[-50.77019,-64.40883,-63.40703,-52.78486]}, - {"t":0.2139, "x":1.09642, "y":6.89848, "heading":-0.89849, "vx":0.61967, "vy":-0.75675, "omega":0.3834, "ax":2.89453, "ay":-3.5383, "alpha":1.78065, "fx":[55.59003,39.01983,40.82311,53.84724], "fy":[-50.83851,-64.43703,-63.36086,-52.74146]}, - {"t":0.23767, "x":1.11197, "y":6.87949, "heading":-0.88937, "vx":0.68847, "vy":-0.84085, "omega":0.42572, "ax":2.89392, "ay":-3.53841, "alpha":1.77757, "fx":[55.50874,38.95322,40.89208,53.88603], "fy":[-50.91513,-64.46842,-63.30909,-52.69267]}, - {"t":0.26144, "x":1.12915, "y":6.85851, "heading":-0.87926, "vx":0.75725, "vy":-0.92495, "omega":0.46797, "ax":2.89325, "ay":-3.53854, "alpha":1.77418, "fx":[55.41827,38.88064,40.9688,53.92895], "fy":[-51.00043,-64.50262,-63.25158,-52.63879]}, - {"t":0.28521, "x":1.14796, "y":6.83553, "heading":-0.86813, "vx":0.82601, "vy":-1.00905, "omega":0.51014, "ax":2.89253, "ay":-3.53867, "alpha":1.77041, "fx":[55.31802,38.80262,41.05331,53.97556], "fy":[-51.09483,-64.5392,-63.18818,-52.58013]}, - {"t":0.30897, "x":1.16841, "y":6.81054, "heading":-0.85601, "vx":0.89476, "vy":-1.09315, "omega":0.55221, "ax":2.89174, "ay":-3.53883, "alpha":1.76621, "fx":[55.20733,38.71972,41.14561,54.02534], "fy":[-51.19877,-64.57769,-63.11875,-52.51705]}, - {"t":0.33274, "x":1.19049, "y":6.78356, "heading":-0.84288, "vx":0.96349, "vy":-1.17726, "omega":0.59419, "ax":2.89088, "ay":-3.539, "alpha":1.7615, "fx":[55.08545,38.63258,41.24571,54.07771], "fy":[-51.31273,-64.61755,-63.04312,-52.45]}, - {"t":0.35651, "x":1.21421, "y":6.75458, "heading":-0.82876, "vx":1.0322, "vy":-1.26137, "omega":0.63606, "ax":2.88992, "ay":-3.53919, "alpha":1.75621, "fx":[54.95154,38.5419,41.3536,54.13198], "fy":[-51.43725,-64.6582,-62.9611,-52.37947]}, - {"t":0.38027, "x":1.23956, "y":6.72361, "heading":-0.81364, "vx":1.10088, "vy":-1.34549, "omega":0.6778, "ax":2.88886, "ay":-3.53941, "alpha":1.75024, "fx":[54.80465,38.44846,41.46921,54.18735], "fy":[-51.57289,-64.69897,-62.87249,-52.30607]}, - {"t":0.40404, "x":1.26654, "y":6.69063, "heading":-0.79754, "vx":1.16954, "vy":-1.42961, "omega":0.71939, "ax":2.88768, "ay":-3.53966, "alpha":1.74348, "fx":[54.6437,38.35309,41.59245,54.24285], "fy":[-51.72026,-64.73911,-62.77709,-52.23049]}, - {"t":0.42781, "x":1.29515, "y":6.65565, "heading":-0.78044, "vx":1.23817, "vy":-1.51374, "omega":0.76083, "ax":2.88634, "ay":-3.53995, "alpha":1.7358, "fx":[54.46744,38.25673,41.72318,54.29735], "fy":[-51.88002,-64.77779,-62.67466,-52.15358]}, - {"t":0.45158, "x":1.3254, "y":6.61867, "heading":-0.76235, "vx":1.30677, "vy":-1.59787, "omega":0.80209, "ax":2.88482, "ay":-3.54029, "alpha":1.72707, "fx":[54.27444,38.16043,41.86116,54.34948], "fy":[-52.0529,-64.81405,-62.56495,-52.07636]}, - {"t":0.47534, "x":1.35727, "y":6.5797, "heading":-0.74329, "vx":1.37534, "vy":-1.68202, "omega":0.84313, "ax":2.88309, "ay":-3.54069, "alpha":1.7171, "fx":[54.06302,38.0653,42.00608,54.39753], "fy":[-52.23968,-64.84675,-62.44768,-52.00004]}, - {"t":0.49911, "x":1.39077, "y":6.53872, "heading":-0.72325, "vx":1.44386, "vy":-1.76617, "omega":0.88395, "ax":2.88108, "ay":-3.54115, "alpha":1.70568, "fx":[53.83115,37.97264,42.15746,54.43941], "fy":[-52.44124,-64.87459,-62.32256,-51.92617]}, - {"t":0.52288, "x":1.4259, "y":6.49574, "heading":-0.70224, "vx":1.51233, "vy":-1.85033, "omega":0.92448, "ax":2.87874, "ay":-3.5417, "alpha":1.69252, "fx":[53.57641,37.88388,42.31464,54.47244], "fy":[-52.65855,-64.89598,-62.18923,-51.85666]}, - {"t":0.54664, "x":1.46266, "y":6.45076, "heading":-0.68027, "vx":1.58075, "vy":-1.93451, "omega":0.96471, "ax":2.87597, "ay":-3.54235, "alpha":1.67728, "fx":[53.29571,37.80065,42.47671,54.49315], "fy":[-52.89277,-64.90897,-62.04729,-51.79397]}, - {"t":0.57041, "x":1.50104, "y":6.40379, "heading":-0.65734, "vx":1.64911, "vy":-2.0187, "omega":1.00457, "ax":2.87265, "ay":-3.54313, "alpha":1.65948, "fx":[52.98512,37.7249,42.64235,54.4969], "fy":[-53.14528,-64.91106,-61.89627,-51.74134]}, - {"t":0.59418, "x":1.54105, "y":6.35481, "heading":-0.63347, "vx":1.71738, "vy":-2.10291, "omega":1.04402, "ax":2.86861, "ay":-3.54407, "alpha":1.63849, "fx":[52.6394,37.65894,42.80966,54.47733], "fy":[-53.41781,-64.899,-61.73554,-51.70314]}, - {"t":0.61795, "x":1.58267, "y":6.30383, "heading":-0.60865, "vx":1.78556, "vy":-2.18714, "omega":1.08296, "ax":2.86361, "ay":-3.54523, "alpha":1.61339, "fx":[52.25125,37.60565,42.97583,54.42534], "fy":[-53.71266,-64.86826,-61.56432,-51.68548]}, - {"t":0.64171, "x":1.62592, "y":6.25084, "heading":-0.58291, "vx":1.85362, "vy":-2.2714, "omega":1.1213, "ax":2.85726, "ay":-3.54665, "alpha":1.58279, "fx":[51.81011,37.56884,43.13655,54.32743], "fy":[-54.03304,-64.81238,-61.38145,-51.69727]}, - {"t":0.66548, "x":1.67078, "y":6.19586, "heading":-0.55626, "vx":1.92153, "vy":-2.3557, "omega":1.15892, "ax":2.84897, "ay":-3.54846, "alpha":1.54454, "fx":[51.29976,37.55377,43.28496,54.16238], "fy":[-54.38369,-64.72148,-61.18524,-51.75213]}, - {"t":0.68925, "x":1.71726, "y":6.13887, "heading":-0.52872, "vx":1.98924, "vy":-2.44003, "omega":1.19563, "ax":2.83773, "ay":-3.55082, "alpha":1.495, "fx":[50.69359,37.56831,43.4093,53.89458], "fy":[-54.77225,-64.57941,-60.97284,-51.87228]}, - {"t":0.71301, "x":1.76534, "y":6.07987, "heading":-0.5003, "vx":2.05669, "vy":-2.52443, "omega":1.23116, "ax":2.82168, "ay":-3.55401, "alpha":1.42759, "fx":[49.9443,37.62537,43.48787,53.45897], "fy":[-55.21186,-64.35744,-60.73913,-52.09697]}, - {"t":0.73678, "x":1.81501, "y":6.01887, "heading":-0.47104, "vx":2.12375, "vy":-2.60889, "omega":1.26509, "ax":2.79702, "ay":-3.55856, "alpha":1.32885, "fx":[48.95797,37.74875,43.47551,52.7216], "fy":[-55.72764,-63.99813,-60.47345,-52.5034]}, - {"t":0.76055, "x":1.86628, "y":5.95586, "heading":-0.44097, "vx":2.19023, "vy":-2.69347, "omega":1.29668, "ax":2.75446, "ay":-3.56553, "alpha":1.16643, "fx":[47.51681,37.9901,43.26008,51.354], "fy":[-56.37434,-63.36582,-60.14917,-53.26888]}, - {"t":0.78432, "x":1.91911, "y":5.89083, "heading":-0.41016, "vx":2.25569, "vy":-2.77821, "omega":1.3244, "ax":2.66404, "ay":-3.57719, "alpha":0.83723, "fx":[44.97245,38.49435,42.46575,48.27533], "fy":[-57.29614,-62.0386,-59.67447,-54.91158]}, - {"t":0.80808, "x":1.97348, "y":5.82379, "heading":-0.37868, "vx":2.31901, "vy":-2.86323, "omega":1.3443, "ax":2.34638, "ay":-3.58924, "alpha":-0.26471, "fx":[38.15582,39.93996,38.58672,36.75277], "fy":[-58.98598,-57.65918,-58.37545,-59.68839]}, - {"t":0.83185, "x":2.02926, "y":5.75473, "heading":-0.34673, "vx":2.37478, "vy":-2.94854, "omega":1.33801, "ax":-2.38245, "ay":1.10304, "alpha":-9.27248, "fx":[-39.93023,-5.81514,-45.9627,-64.08617], "fy":[-28.46057,53.71773,45.32362,1.54961]}, - {"t":0.85562, "x":2.08502, "y":5.68496, "heading":-0.31493, "vx":2.31815, "vy":-2.92232, "omega":1.11763, "ax":-2.97197, "ay":3.08456, "alpha":-3.00658, "fx":[-54.89912,-31.10253,-46.37822,-61.96401], "fy":[44.04423,64.58846,55.94169,37.13237]}, - {"t":0.87938, "x":2.13928, "y":5.61638, "heading":-0.28837, "vx":2.24752, "vy":-2.84901, "omega":1.04617, "ax":-2.95385, "ay":3.29262, "alpha":-2.41945, "fx":[-52.74706,-33.65165,-46.52318,-60.23746], "fy":[50.25818,65.27966,57.40323,42.37127]}, - {"t":0.90315, "x":2.19186, "y":5.54959, "heading":-0.2635, "vx":2.17731, "vy":-2.77076, "omega":0.98866, "ax":-2.94378, "ay":3.3693, "alpha":-2.20459, "fx":[-51.71432,-34.59126,-46.69666,-59.49859], "fy":[52.54636,65.53258,57.89546,44.3524]}, - {"t":0.92692, "x":2.24278, "y":5.48469, "heading":-0.24, "vx":2.10735, "vy":-2.69068, "omega":0.93627, "ax":-2.93776, "ay":3.40919, "alpha":-2.09306, "fx":[-51.03854,-35.08104,-46.87698,-59.11031], "fy":[53.81416,65.66282,58.09082,45.36736]}, - {"t":0.95069, "x":2.29204, "y":5.42171, "heading":-0.21775, "vx":2.03752, "vy":-2.60965, "omega":0.88652, "ax":-2.93377, "ay":3.43365, "alpha":-2.02471, "fx":[-50.5236,-35.3861,-47.05593,-58.88047], "fy":[54.66323,65.73961,58.15945,45.97255]}, - {"t":0.97445, "x":2.33963, "y":5.36065, "heading":-0.19668, "vx":1.9678, "vy":-2.52804, "omega":0.8384, "ax":-2.93093, "ay":3.4502, "alpha":-1.9785, "fx":[-50.09804,-35.59893,-47.22991,-58.73353], "fy":[55.2962,65.78756,58.16448,46.36849]}, - {"t":0.99822, "x":2.38558, "y":5.30154, "heading":-0.17675, "vx":1.89814, "vy":-2.44604, "omega":0.79138, "ax":-2.9288, "ay":3.46214, "alpha":-1.94519, "fx":[-49.72994,-35.75972,-47.39711,-58.63421], "fy":[55.80026,65.81787,58.13477,46.64463]}, - {"t":1.02199, "x":2.42986, "y":5.24438, "heading":-0.15795, "vx":1.82853, "vy":-2.36376, "omega":0.74515, "ax":-2.92713, "ay":3.47117, "alpha":-1.92008, "fx":[-49.40299,-35.88846,-47.55654,-58.56404], "fy":[56.21914,65.83655,58.08541,46.84671]}, - {"t":1.04575, "x":2.47249, "y":5.18919, "heading":-0.14024, "vx":1.75896, "vy":-2.28126, "omega":0.69951, "ax":-2.92579, "ay":3.47823, "alpha":-1.90053, "fx":[-49.10806,-35.99601,-47.70762,-58.51254], "fy":[56.57716,65.84719,58.02508,47.00037]}, - {"t":1.06952, "x":2.51347, "y":5.13595, "heading":-0.12361, "vx":1.68942, "vy":-2.19859, "omega":0.65434, "ax":-2.92468, "ay":3.48391, "alpha":-1.88491, "fx":[-48.83968,-36.08871,-47.85001,-58.47337], "fy":[56.88898,65.85212,57.95914,47.12102]}, - {"t":1.09329, "x":2.5528, "y":5.08468, "heading":-0.10806, "vx":1.61991, "vy":-2.11579, "omega":0.60954, "ax":-2.92375, "ay":3.48858, "alpha":-1.8722, "fx":[-48.59437,-36.17036,-47.98349,-58.44255], "fy":[57.16395,65.85296,57.89107,47.21843]}, - {"t":1.11706, "x":2.59047, "y":5.03538, "heading":-0.09357, "vx":1.55042, "vy":-2.03287, "omega":0.56504, "ax":-2.92295, "ay":3.49248, "alpha":-1.86171, "fx":[-48.36982,-36.24339,-48.10793,-58.4175], "fy":[57.40837,65.85089,57.82325,47.29899]}, - {"t":1.14082, "x":2.6265, "y":4.98805, "heading":-0.08014, "vx":1.48095, "vy":-1.94987, "omega":0.5208, "ax":-2.92226, "ay":3.49579, "alpha":-1.85293, "fx":[-48.16446,-36.30931,-48.22326,-58.3965], "fy":[57.62662,65.84682,57.75735,47.36708]}, - {"t":1.16459, "x":2.66087, "y":4.94269, "heading":-0.06776, "vx":1.4115, "vy":-1.86678, "omega":0.47676, "ax":-2.92166, "ay":3.49863, "alpha":-1.84551, "fx":[-47.97716,-36.36911,-48.32942,-58.37837], "fy":[57.82188,65.84144,57.69459,47.42572]}, - {"t":1.18836, "x":2.69359, "y":4.89931, "heading":-0.05643, "vx":1.34206, "vy":-1.78363, "omega":0.4329, "ax":-2.92112, "ay":3.50109, "alpha":-1.83918, "fx":[-47.80708,-36.42346,-48.4264,-58.36232], "fy":[57.99651,65.83534,57.63588,47.47707]}, - {"t":1.21212, "x":2.72466, "y":4.85791, "heading":-0.04614, "vx":1.27263, "vy":-1.70042, "omega":0.38918, "ax":-2.92065, "ay":3.50325, "alpha":-1.83374, "fx":[-47.65362,-36.47276,-48.5142,-58.34781], "fy":[58.15233,65.82897,57.5819,47.52268]}, - {"t":1.23589, "x":2.75409, "y":4.81849, "heading":-0.03689, "vx":1.20321, "vy":-1.61716, "omega":0.3456, "ax":-2.92023, "ay":3.50515, "alpha":-1.82904, "fx":[-47.51628,-36.51729,-48.59282,-58.33445], "fy":[58.29072,65.82272,57.53319,47.5637]}, - {"t":1.25966, "x":2.78186, "y":4.78104, "heading":-0.02868, "vx":1.13381, "vy":-1.53385, "omega":0.30213, "ax":-2.91985, "ay":3.50684, "alpha":-1.82494, "fx":[-47.39469,-36.55722,-48.66228,-58.32198], "fy":[58.41281,65.81692,57.49017,47.60097]}, - {"t":1.28343, "x":2.80798, "y":4.74558, "heading":-0.0215, "vx":1.06441, "vy":-1.4505, "omega":0.25876, "ax":-2.91951, "ay":3.50835, "alpha":-1.82135, "fx":[-47.28853,-36.59267,-48.72258,-58.31021], "fy":[58.5195,65.81184,57.45316,47.63514]}, - {"t":1.30719, "x":2.83246, "y":4.71209, "heading":-0.01535, "vx":0.99502, "vy":-1.36712, "omega":0.21547, "ax":-2.91921, "ay":3.50971, "alpha":-1.81819, "fx":[-47.19756,-36.62369,-48.77375,-58.29901], "fy":[58.61151,65.80771,57.42245,47.6667]}, - {"t":1.33096, "x":2.85528, "y":4.68059, "heading":-0.01023, "vx":0.92564, "vy":-1.2837, "omega":0.17225, "ax":-2.91893, "ay":3.51094, "alpha":-1.81538, "fx":[-47.12158,-36.65035,-48.81581,-58.28829], "fy":[58.68944,65.80471,57.39823,47.69605]}, - {"t":1.35473, "x":2.87645, "y":4.65107, "heading":-0.00613, "vx":0.85627, "vy":-1.20026, "omega":0.12911, "ax":-2.91869, "ay":3.51205, "alpha":-1.81289, "fx":[-47.06039,-36.67267,-48.84875,-58.27796], "fy":[58.7538,65.80302,57.38069,47.7235]}, - {"t":1.37849, "x":2.89598, "y":4.62354, "heading":-0.00307, "vx":0.7869, "vy":-1.11678, "omega":0.08602, "ax":-2.91846, "ay":3.51305, "alpha":-1.81066, "fx":[-47.01386,-36.69069,-48.87261,-58.26796], "fy":[58.80497,65.80274,57.36998,47.74932]}, - {"t":1.40226, "x":2.91386, "y":4.59799, "heading":-0.00102, "vx":0.71754, "vy":-1.03329, "omega":0.04299, "ax":-2.91826, "ay":3.51398, "alpha":-1.80866, "fx":[-46.98186,-36.70446,-48.88737,-58.25822], "fy":[58.84331,65.804,57.36622,47.77374]}, - {"t":1.42603, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.64818, "vy":-0.94977, "omega":0.0, "ax":-1.45495, "ay":0.1374, "alpha":0.0, "fx":[-23.76062,-23.76537,-23.80728,-23.80917], "fy":[2.26938,2.11025,2.33807,2.2675]}, - {"t":1.4573, "x":2.94965, "y":4.54479, "heading":0.0, "vx":0.60268, "vy":-0.94548, "omega":0.0, "ax":-1.13183, "ay":0.97154, "alpha":0.0, "fx":[-18.50161,-18.50243,-18.50467,-18.5046], "fy":[15.88443,15.88021,15.88303,15.88394]}, - {"t":1.48857, "x":2.96794, "y":4.5157, "heading":0.0, "vx":0.56729, "vy":-0.9151, "omega":0.0, "ax":-0.97536, "ay":1.133, "alpha":0.0, "fx":[-15.94693,-15.94569,-15.94442,-15.94388], "fy":[18.5211,18.52937,18.51745,18.52151]}, - {"t":1.51983, "x":2.9852, "y":4.48764, "heading":0.0, "vx":0.53679, "vy":-0.87967, "omega":0.0, "ax":-0.9023, "ay":1.19383, "alpha":0.0, "fx":[-14.75263,-14.75142,-14.75,-14.74955], "fy":[19.51556,19.52317,19.51277,19.51599]}, - {"t":1.5511, "x":3.00154, "y":4.46072, "heading":0.0, "vx":0.50858, "vy":-0.84234, "omega":0.0, "ax":-0.8609, "ay":1.22501, "alpha":0.0, "fx":[-14.07551,-14.07454,-14.0734,-14.07304], "fy":[20.02546,20.03148,20.02334,20.02582]}, - {"t":1.58237, "x":3.01702, "y":4.43498, "heading":0.0, "vx":0.48166, "vy":-0.80404, "omega":0.0, "ax":-0.83448, "ay":1.24377, "alpha":0.0, "fx":[-13.6432,-13.64244,-13.64151,-13.64124], "fy":[20.33245,20.3371,20.33093,20.33274]}, - {"t":1.61364, "x":3.03168, "y":4.41045, "heading":0.0, "vx":0.45557, "vy":-0.76515, "omega":0.0, "ax":-0.81625, "ay":1.25623, "alpha":0.0, "fx":[-13.34496,-13.34435,-13.34359,-13.34338], "fy":[20.53626,20.53992,20.53512,20.53649]}, - {"t":1.64491, "x":3.04552, "y":4.38714, "heading":0.0, "vx":0.43005, "vy":-0.72587, "omega":0.0, "ax":-0.80297, "ay":1.26506, "alpha":0.0, "fx":[-13.12779,-13.12728,-13.12665,-13.12648], "fy":[20.68071,20.68373,20.67978,20.68091]}, - {"t":1.67618, "x":3.05858, "y":4.36506, "heading":0.0, "vx":0.40494, "vy":-0.68631, "omega":0.0, "ax":-0.79291, "ay":1.27162, "alpha":0.0, "fx":[-12.96324,-12.9628,-12.96226,-12.96211], "fy":[20.788,20.79059,20.78722,20.78817]}, - {"t":1.70744, "x":3.07085, "y":4.34422, "heading":0.0, "vx":0.38015, "vy":-0.64655, "omega":0.0, "ax":-0.78505, "ay":1.27666, "alpha":0.0, "fx":[-12.83471,-12.83431,-12.83381,-12.83368], "fy":[20.87054,20.87287,20.86985,20.87069]}, - {"t":1.73871, "x":3.08235, "y":4.32463, "heading":0.0, "vx":0.3556, "vy":-0.60663, "omega":0.0, "ax":-0.77876, "ay":1.28065, "alpha":0.0, "fx":[-12.73182,-12.73144,-12.73097,-12.73085], "fy":[20.93583,20.93803,20.93518,20.93597]}, - {"t":1.76998, "x":3.09309, "y":4.30629, "heading":0.0, "vx":0.33125, "vy":-0.56659, "omega":0.0, "ax":-0.77362, "ay":1.28388, "alpha":0.0, "fx":[-12.6478,-12.64744,-12.64698,-12.64686], "fy":[20.98863,20.99078,20.98798,20.98877]}, - {"t":1.80125, "x":3.10307, "y":4.2892, "heading":0.0, "vx":0.30706, "vy":-0.52644, "omega":0.0, "ax":-0.76936, "ay":1.28654, "alpha":0.0, "fx":[-12.57805,-12.57768,-12.57722,-12.5771], "fy":[21.03212,21.03429,21.03147,21.03226]}, - {"t":1.83252, "x":3.1123, "y":4.27337, "heading":0.0, "vx":0.283, "vy":-0.48621, "omega":0.0, "ax":-0.76576, "ay":1.28877, "alpha":0.0, "fx":[-12.51929,-12.51892,-12.51845,-12.51833], "fy":[21.06852,21.07075,21.06784,21.06866]}, - {"t":1.86379, "x":3.12077, "y":4.25879, "heading":0.0, "vx":0.25906, "vy":-0.44592, "omega":0.0, "ax":-0.7627, "ay":1.29066, "alpha":0.0, "fx":[-12.46919,-12.4688,-12.46831,-12.46818], "fy":[21.09939,21.10172,21.09868,21.09954]}, - {"t":1.89505, "x":3.1285, "y":4.24548, "heading":0.0, "vx":0.23521, "vy":-0.40556, "omega":0.0, "ax":-0.76005, "ay":1.29228, "alpha":0.0, "fx":[-12.42597,-12.42558,-12.42507,-12.42493], "fy":[21.12589,21.12832,21.12514,21.12604]}, - {"t":1.92632, "x":3.13548, "y":4.23343, "heading":0.0, "vx":0.21144, "vy":-0.36515, "omega":0.0, "ax":-0.75775, "ay":1.29369, "alpha":0.0, "fx":[-12.38833,-12.38794,-12.38739,-12.38727], "fy":[21.14889,21.15133,21.14816,21.14903]}, - {"t":1.95759, "x":3.14172, "y":4.22265, "heading":0.0, "vx":0.18775, "vy":-0.3247, "omega":0.0, "ax":-0.75572, "ay":1.29492, "alpha":0.0, "fx":[-12.35526,-12.35485,-12.35428,-12.35415], "fy":[21.16902,21.17158,21.16824,21.16916]}, - {"t":1.98886, "x":3.14722, "y":4.21313, "heading":0.0, "vx":0.16412, "vy":-0.28421, "omega":0.0, "ax":-0.75393, "ay":1.29601, "alpha":0.0, "fx":[-12.32596,-12.32554,-12.32495,-12.32481], "fy":[21.1868,21.18947,21.18597,21.18694]}, - {"t":2.02013, "x":3.15199, "y":4.20487, "heading":0.0, "vx":0.14055, "vy":-0.24369, "omega":0.0, "ax":-0.75233, "ay":1.29698, "alpha":0.0, "fx":[-12.29981,-12.29937,-12.29876,-12.29862], "fy":[21.20261,21.20541,21.20174,21.20277]}, - {"t":2.0514, "x":3.15601, "y":4.19789, "heading":0.0, "vx":0.11702, "vy":-0.20313, "omega":0.0, "ax":-0.75089, "ay":1.29785, "alpha":0.0, "fx":[-12.27631,-12.27586,-12.27523,-12.27508], "fy":[21.2168,21.2197,21.21588,21.21695]}, - {"t":2.08266, "x":3.15931, "y":4.19217, "heading":0.0, "vx":0.09354, "vy":-0.16255, "omega":0.0, "ax":-0.74959, "ay":1.29863, "alpha":0.0, "fx":[-12.25507,-12.2546,-12.25395,-12.25379], "fy":[21.22959,21.23259,21.22863,21.22975]}, - {"t":2.11393, "x":3.16186, "y":4.18772, "heading":0.0, "vx":0.07011, "vy":-0.12194, "omega":0.0, "ax":-0.74841, "ay":1.29934, "alpha":0.0, "fx":[-12.23575,-12.23527,-12.2346,-12.23444], "fy":[21.24119,21.24429,21.2402,21.24136]}, - {"t":2.1452, "x":3.16369, "y":4.18455, "heading":0.0, "vx":0.0467, "vy":-0.08132, "omega":0.0, "ax":-0.74733, "ay":1.29999, "alpha":0.0, "fx":[-12.2181,-12.21761,-12.21692,-12.21676], "fy":[21.25178,21.25496,21.25075,21.25195]}, - {"t":2.17647, "x":3.16479, "y":4.18264, "heading":0.0, "vx":0.02334, "vy":-0.04067, "omega":0.0, "ax":-0.74633, "ay":1.30058, "alpha":0.0, "fx":[-12.20051,-12.20099,-12.20304,-12.19999], "fy":[21.26472,21.26183,21.26156,21.26017]}, - {"t":2.20774, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.3389, "ay":-4.07969, "alpha":2.1798, "fx":[65.0028,44.72604,46.37971,62.23003], "fy":[-57.77547,-74.58153,-73.6142,-60.80957]}, + {"t":0.02232, "x":1.03096, "y":6.9784, "heading":-0.93501, "vx":0.07453, "vy":-0.09106, "omega":0.04865, "ax":3.33864, "ay":-4.07986, "alpha":2.17394, "fx":[64.96861,44.74997,46.3941,62.2087], "fy":[-57.80579,-74.56079,-73.60003,-60.82526]}, + {"t":0.04464, "x":1.03346, "y":6.97535, "heading":-0.93392, "vx":0.14905, "vy":-0.18213, "omega":0.09718, "ax":3.33836, "ay":-4.08003, "alpha":2.16779, "fx":[64.92527,44.76446,46.41919,62.19469], "fy":[-57.84579,-74.54534,-73.5788,-60.83306]}, + {"t":0.06696, "x":1.03762, "y":6.97027, "heading":-0.93175, "vx":0.22356, "vy":-0.27319, "omega":0.14556, "ax":3.33808, "ay":-4.0802, "alpha":2.16129, "fx":[64.87258,44.76984,46.45495,62.18776], "fy":[-57.89559,-74.53497,-73.55048,-60.83317]}, + {"t":0.08928, "x":1.04344, "y":6.96315, "heading":-0.9285, "vx":0.29807, "vy":-0.36427, "omega":0.1938, "ax":3.33779, "ay":-4.08038, "alpha":2.15441, "fx":[64.81027,44.76652,46.5014,62.18761], "fy":[-57.95539,-74.52941,-73.51501,-60.82585]}, + {"t":0.1116, "x":1.05092, "y":6.954, "heading":-0.92418, "vx":0.37257, "vy":-0.45534, "omega":0.24189, "ax":3.33748, "ay":-4.08056, "alpha":2.14709, "fx":[64.73802,44.75497,46.55857,62.19392], "fy":[-58.02543,-74.5283,-73.4723,-60.81138]}, + {"t":0.13392, "x":1.06007, "y":6.94282, "heading":-0.91878, "vx":0.44706, "vy":-0.54642, "omega":0.28982, "ax":3.33715, "ay":-4.08074, "alpha":2.13926, "fx":[64.65542,44.7358,46.6265,62.20629], "fy":[-58.10601,-74.53126,-73.42224,-60.7901]}, + {"t":0.15624, "x":1.07088, "y":6.92961, "heading":-0.91231, "vx":0.52155, "vy":-0.63751, "omega":0.33757, "ax":3.3368, "ay":-4.08094, "alpha":2.13086, "fx":[64.56198,44.70967,46.70525,62.22426], "fy":[-58.19749,-74.5378,-73.36469,-60.76239]}, + {"t":0.17856, "x":1.08335, "y":6.91437, "heading":-0.90477, "vx":0.59603, "vy":-0.7286, "omega":0.38513, "ax":3.33642, "ay":-4.08115, "alpha":2.12178, "fx":[64.45713,44.67738,46.79489,62.2473], "fy":[-58.3003,-74.54739,-73.2995,-60.72872]}, + {"t":0.20088, "x":1.09749, "y":6.89709, "heading":-0.89618, "vx":0.6705, "vy":-0.81969, "omega":0.43249, "ax":3.33602, "ay":-4.08137, "alpha":2.11195, "fx":[64.34023,44.63983,46.8955,62.27481], "fy":[-58.4149,-74.55938,-73.22649,-60.68959]}, + {"t":0.22321, "x":1.11329, "y":6.87777, "heading":-0.88652, "vx":0.74496, "vy":-0.91079, "omega":0.47963, "ax":3.33558, "ay":-4.08161, "alpha":2.10124, "fx":[64.21055,44.59804,47.00717,62.30607], "fy":[-58.54185,-74.57308,-73.14544,-60.64561]}, + {"t":0.24553, "x":1.13074, "y":6.85643, "heading":-0.87582, "vx":0.81941, "vy":-1.00189, "omega":0.52653, "ax":3.33511, "ay":-4.08187, "alpha":2.08953, "fx":[64.06724,44.55319,47.13002,62.34027], "fy":[-58.68173,-74.58767,-73.05612,-60.59748]}, + {"t":0.26785, "x":1.14986, "y":6.83305, "heading":-0.86407, "vx":0.89385, "vy":-1.093, "omega":0.57317, "ax":3.33459, "ay":-4.08215, "alpha":2.07666, "fx":[63.90936,44.50659,47.26413,62.37645], "fy":[-58.83519,-74.60223,-72.95826,-60.54602]}, + {"t":0.29017, "x":1.17065, "y":6.80763, "heading":-0.85127, "vx":0.96828, "vy":-1.18412, "omega":0.61952, "ax":3.33401, "ay":-4.08247, "alpha":2.06246, "fx":[63.73584,44.45975,47.40963,62.4135], "fy":[-59.00296,-74.61571,-72.85155,-60.49217]}, + {"t":0.31249, "x":1.19309, "y":6.78019, "heading":-0.83744, "vx":1.0427, "vy":-1.27524, "omega":0.66555, "ax":3.33336, "ay":-4.08282, "alpha":2.04673, "fx":[63.54549,44.41436,47.56662,62.45012], "fy":[-59.1858,-74.62694,-72.73565,-60.43708]}, + {"t":0.33481, "x":1.21719, "y":6.75071, "heading":-0.82259, "vx":1.1171, "vy":-1.36637, "omega":0.71124, "ax":3.33264, "ay":-4.08322, "alpha":2.02924, "fx":[63.33694,44.37237,47.73519,62.48476], "fy":[-59.38457,-74.63454,-72.61018,-60.38206]}, + {"t":0.35713, "x":1.24296, "y":6.71919, "heading":-0.80671, "vx":1.19149, "vy":-1.45751, "omega":0.75653, "ax":3.33182, "ay":-4.08366, "alpha":2.00971, "fx":[63.10866,44.33601,47.91546,62.51558], "fy":[-59.60017,-74.63693,-72.47469,-60.32873]}, + {"t":0.37945, "x":1.27038, "y":6.68564, "heading":-0.78983, "vx":1.26586, "vy":-1.54866, "omega":0.80139, "ax":3.33089, "ay":-4.08417, "alpha":1.98781, "fx":[62.85887,44.30786,48.1075,62.54034], "fy":[-59.83362,-74.63226,-72.32868,-60.27899]}, + {"t":0.40177, "x":1.29947, "y":6.65006, "heading":-0.77194, "vx":1.3402, "vy":-1.63982, "omega":0.84576, "ax":3.32981, "ay":-4.08474, "alpha":1.96314, "fx":[62.58555,44.29093,48.31139,62.55634], "fy":[-60.08599,-74.61836,-72.17155,-60.23519]}, + {"t":0.42409, "x":1.33021, "y":6.61244, "heading":-0.75306, "vx":1.41453, "vy":-1.73099, "omega":0.88958, "ax":3.32856, "ay":-4.0854, "alpha":1.9352, "fx":[62.28631,44.28881,48.52718,62.56019], "fy":[-60.35853,-74.59259,-72.00261,-60.20023]}, + {"t":0.44641, "x":1.36261, "y":6.57278, "heading":-0.73321, "vx":1.48882, "vy":-1.82218, "omega":0.93277, "ax":3.32709, "ay":-4.08615, "alpha":1.90337, "fx":[61.95832,44.30578,48.7549,62.54765], "fy":[-60.65258,-74.55174,-71.82099,-60.17771]}, + {"t":0.46873, "x":1.39667, "y":6.53109, "heading":-0.71239, "vx":1.56308, "vy":-1.91338, "omega":0.97526, "ax":3.32536, "ay":-4.08701, "alpha":1.86686, "fx":[61.59815,44.34711,48.99455,62.51327], "fy":[-60.96974,-74.49176,-71.62566,-60.17225]}, + {"t":0.49105, "x":1.43239, "y":6.48737, "heading":-0.69062, "vx":1.63731, "vy":-2.00461, "omega":1.01692, "ax":3.32328, "ay":-4.08801, "alpha":1.82463, "fx":[61.20153,44.41941,49.24612,62.44991], "fy":[-61.31187,-74.4075,-71.41524,-60.1898]}, + {"t":0.51337, "x":1.46976, "y":6.44161, "heading":-0.66792, "vx":1.71149, "vy":-2.09586, "omega":1.05765, "ax":3.32075, "ay":-4.08916, "alpha":1.77528, "fx":[60.76304,44.53118,49.50954,62.34791], "fy":[-61.68127,-74.29214,-71.18797,-60.23827]}, + {"t":0.53569, "x":1.50879, "y":6.39381, "heading":-0.64431, "vx":1.78561, "vy":-2.18713, "omega":1.09728, "ax":3.31763, "ay":-4.09049, "alpha":1.71687, "fx":[60.27553,44.69369,49.7847,62.19394], "fy":[-62.08085,-74.13639,-70.9414,-60.32844]}, + {"t":0.55801, "x":1.54947, "y":6.34397, "heading":-0.61982, "vx":1.85966, "vy":-2.27843, "omega":1.1356, "ax":3.31372, "ay":-4.09205, "alpha":1.64663, "fx":[59.72925,44.92241,50.07144,61.96881], "fy":[-62.51451,-73.92715,-70.67203,-60.47543]}, + {"t":0.58033, "x":1.59181, "y":6.2921, "heading":-0.59447, "vx":1.93362, "vy":-2.36977, "omega":1.17235, "ax":3.30869, "ay":-4.09388, "alpha":1.56039, "fx":[59.11033,45.23947,50.36956,61.64384], "fy":[-62.98768,-73.64509,-70.37464,-60.70129]}, + {"t":0.60265, "x":1.63579, "y":6.23818, "heading":-0.56831, "vx":2.00747, "vy":-2.46114, "omega":1.20718, "ax":3.30205, "ay":-4.09603, "alpha":1.45169, "fx":[58.39803,45.67802,50.67872,61.17384], "fy":[-63.50834,-73.26026,-70.04097,-61.03959]}, + {"t":0.62497, "x":1.68142, "y":6.18223, "heading":-0.54136, "vx":2.08118, "vy":-2.55257, "omega":1.23958, "ax":3.29292, "ay":-4.09854, "alpha":1.30979, "fx":[57.55961,46.29063,50.99842,60.48308], "fy":[-64.08886,-72.72333,-69.657,-61.54434]}, + {"t":0.64729, "x":1.72869, "y":6.12423, "heading":-0.51369, "vx":2.15468, "vy":-2.64405, "omega":1.26882, "ax":3.2797, "ay":-4.10142, "alpha":1.11555, "fx":[56.53961,47.16658,51.3277,59.43372], "fy":[-64.74968,-71.94642,-69.19695,-62.30856]}, + {"t":0.66962, "x":1.77761, "y":6.06419, "heading":-0.48537, "vx":2.22788, "vy":-2.7356, "omega":1.29372, "ax":3.25905, "ay":-4.10442, "alpha":0.83127, "fx":[55.23551,48.47152,51.66418,57.74575], "fy":[-65.52713,-70.7561,-68.60739,-63.50713]}, + {"t":0.69194, "x":1.82814, "y":6.00211, "heading":-0.4565, "vx":2.30062, "vy":-2.82721, "omega":1.31227, "ax":3.22266, "ay":-4.1062, "alpha":0.37128, "fx":[53.4339,50.55083,51.99975,54.75273], "fy":[-66.49175,-68.75678,-67.75728,-65.50855]}, + {"t":0.71426, "x":1.8803, "y":5.93798, "heading":-0.42721, "vx":2.37256, "vy":-2.91886, "omega":1.32056, "ax":3.14367, "ay":-4.09899, "alpha":-0.50725, "fx":[50.60616,54.24898,52.29217,48.42462], "fy":[-67.79681,-64.82399,-66.21945,-69.20266]}, + {"t":0.73658, "x":1.93404, "y":5.87181, "heading":-0.39773, "vx":2.44272, "vy":-3.01035, "omega":1.30924, "ax":2.88163, "ay":-4.00789, "alpha":-2.81174, "fx":[45.00901,62.02166,52.04961,29.35633], "fy":[-69.802,-54.34061,-61.42862,-76.51443]}, + {"t":0.7589, "x":1.98928, "y":5.80362, "heading":-0.36851, "vx":2.50704, "vy":-3.09981, "omega":1.24648, "ax":1.17843, "ay":-3.17375, "alpha":-7.925, "fx":[27.4124,58.84867,8.12413,-17.32501], "fy":[-69.35133,-37.14062,-32.55605,-68.49125]}, + {"t":0.78122, "x":2.04553, "y":5.73364, "heading":-0.34069, "vx":2.53335, "vy":-3.17065, "omega":1.06959, "ax":-3.54815, "ay":3.16833, "alpha":0.53555, "fx":[-57.36413,-60.35806,-58.75387,-55.54571], "fy":[53.12638,49.37092,50.40831,54.27902]}, + {"t":0.80354, "x":2.10119, "y":5.66366, "heading":-0.31681, "vx":2.45415, "vy":-3.09993, "omega":1.08154, "ax":-3.45507, "ay":3.75623, "alpha":-1.12511, "fx":[-58.26323,-50.23249,-55.22227,-62.21753], "fy":[59.52377,66.79876,63.23619,56.07024]}, + {"t":0.82586, "x":2.15511, "y":5.5954, "heading":-0.29267, "vx":2.37703, "vy":-3.01609, "omega":1.05643, "ax":-3.42, "ay":3.88316, "alpha":-1.55109, "fx":[-58.32508,-46.80845,-54.45946,-64.04876], "fy":[61.39874,70.90998,65.60283,56.0176]}, + {"t":0.84818, "x":2.20731, "y":5.52905, "heading":-0.26909, "vx":2.30069, "vy":-2.92942, "omega":1.02181, "ax":-3.40274, "ay":3.93773, "alpha":-1.74615, "fx":[-58.17711,-45.12543,-54.25684,-64.9538], "fy":[62.43272,72.7215,66.48403,55.85919]}, + {"t":0.8705, "x":2.25782, "y":5.46465, "heading":-0.24628, "vx":2.22474, "vy":-2.84152, "omega":0.98283, "ax":-3.39251, "ay":3.96801, "alpha":-1.8573, "fx":[-57.94648,-44.13333,-54.25075,-65.51375], "fy":[63.16105,73.73443,66.87975,55.70231]}, + {"t":0.89282, "x":2.30663, "y":5.40221, "heading":-0.22435, "vx":2.14902, "vy":-2.75296, "omega":0.94138, "ax":-3.38573, "ay":3.98727, "alpha":-1.92861, "fx":[-57.6784,-43.48678,-54.33187,-65.90406], "fy":[63.73972,74.37646,67.06033,55.56063]}, + {"t":0.91514, "x":2.35376, "y":5.34176, "heading":-0.20333, "vx":2.07345, "vy":-2.66396, "omega":0.89833, "ax":-3.3809, "ay":4.00062, "alpha":-1.97799, "fx":[-57.39368,-43.03876,-54.45589,-66.1965], "fy":[64.23012,74.81566,67.12929,55.43487]}, + {"t":0.93746, "x":2.39919, "y":5.28329, "heading":-0.18328, "vx":1.99799, "vy":-2.57466, "omega":0.85418, "ax":-3.37726, "ay":4.01043, "alpha":-2.01407, "fx":[-57.10358,-42.71572,-54.6014,-66.42617], "fy":[64.66109,75.13166,67.1349,55.32361]}, + {"t":0.95978, "x":2.44295, "y":5.22682, "heading":-0.16422, "vx":1.9226, "vy":-2.48515, "omega":0.80922, "ax":-3.37441, "ay":4.01795, "alpha":-2.04156, "fx":[-56.81486,-42.47654,-54.75673,-66.61241], "fy":[65.04788,75.36719,67.10275,55.22518]}, + {"t":0.9821, "x":2.48502, "y":5.17235, "heading":-0.14616, "vx":1.84729, "vy":-2.39547, "omega":0.76366, "ax":-3.37211, "ay":4.0239, "alpha":-2.06321, "fx":[-56.53196,-42.29625,-54.91502,-66.76687], "fy":[65.39928,75.54732,67.04771,55.13809]}, + {"t":1.00442, "x":2.52541, "y":5.11989, "heading":-0.12911, "vx":1.77202, "vy":-2.30565, "omega":0.7176, "ax":-3.3702, "ay":4.02874, "alpha":-2.08075, "fx":[-56.25792,-42.15869,-55.07192,-66.89709], "fy":[65.7207,75.68782,66.97907,55.06104]}, + {"t":1.02674, "x":2.56413, "y":5.06943, "heading":-0.11309, "vx":1.69679, "vy":-2.21573, "omega":0.67116, "ax":-3.3686, "ay":4.03274, "alpha":-2.09529, "fx":[-55.99496,-42.05283,-55.22458,-67.00818], "fy":[66.01568,75.79912,66.90292,54.99291]}, + {"t":1.04906, "x":2.60116, "y":5.02098, "heading":-0.09811, "vx":1.62161, "vy":-2.12571, "omega":0.62439, "ax":-3.36722, "ay":4.03612, "alpha":-2.1076, "fx":[-55.74475,-41.97086,-55.37104,-67.10382], "fy":[66.28666,75.88845,66.82345,54.93274]}, + {"t":1.07138, "x":2.63652, "y":4.97453, "heading":-0.08418, "vx":1.54645, "vy":-2.03563, "omega":0.57735, "ax":-3.36602, "ay":4.039, "alpha":-2.11821, "fx":[-55.50855,-41.90703,-55.50991,-67.18672], "fy":[66.53536,75.96102,66.74362,54.87965]}, + {"t":1.0937, "x":2.6702, "y":4.9301, "heading":-0.07129, "vx":1.47132, "vy":-1.94547, "omega":0.53007, "ax":-3.36497, "ay":4.04148, "alpha":-2.12748, "fx":[-55.28736,-41.85697,-55.6402,-67.25898], "fy":[66.76307,76.02067,66.66564,54.83289]}, + {"t":1.11603, "x":2.7022, "y":4.88769, "heading":-0.05946, "vx":1.39621, "vy":-1.85527, "omega":0.48258, "ax":-3.36404, "ay":4.04365, "alpha":-2.13569, "fx":[-55.08196,-41.81733,-55.76115,-67.32223], "fy":[66.9708,76.07032,66.59114,54.79176]}, + {"t":1.13835, "x":2.73252, "y":4.84728, "heading":-0.04869, "vx":1.32112, "vy":-1.76501, "omega":0.43491, "ax":-3.36321, "ay":4.04556, "alpha":-2.14305, "fx":[-54.893,-41.78541,-55.87222,-67.37781], "fy":[67.15931,76.11222,66.52139,54.75565]}, + {"t":1.16067, "x":2.76117, "y":4.8089, "heading":-0.03898, "vx":1.24605, "vy":-1.67471, "omega":0.38708, "ax":-3.36247, "ay":4.04724, "alpha":-2.14968, "fx":[-54.72098,-41.75909,-55.97297,-67.42678], "fy":[67.32926,76.14817,66.45736,54.72399]}, + {"t":1.18299, "x":2.78815, "y":4.77252, "heading":-0.03034, "vx":1.171, "vy":-1.58437, "omega":0.3391, "ax":-3.3618, "ay":4.04874, "alpha":-2.15572, "fx":[-54.56632,-41.73659,-56.06308,-67.47006], "fy":[67.48117,76.17961,66.39985,54.6963]}, + {"t":1.20531, "x":2.81345, "y":4.73817, "heading":-0.02277, "vx":1.09596, "vy":-1.494, "omega":0.29098, "ax":-3.36119, "ay":4.05009, "alpha":-2.16125, "fx":[-54.42935,-41.7165,-56.14228,-67.50839], "fy":[67.61547,76.2077,66.3495,54.67213]}, + {"t":1.22763, "x":2.83707, "y":4.70583, "heading":-0.01628, "vx":1.02094, "vy":-1.4036, "omega":0.24274, "ax":-3.36065, "ay":4.05129, "alpha":-2.16633, "fx":[-54.31035,-41.69761,-56.21037,-67.54241], "fy":[67.73254,76.23339,66.3068,54.65108]}, + {"t":1.24995, "x":2.85903, "y":4.67551, "heading":-0.01086, "vx":0.94593, "vy":-1.31318, "omega":0.19439, "ax":-3.36015, "ay":4.05239, "alpha":-2.17101, "fx":[-54.20956,-41.67893,-56.26717,-67.57263], "fy":[67.83269,76.25746,66.27219,54.63282]}, + {"t":1.27227, "x":2.8793, "y":4.64721, "heading":-0.00652, "vx":0.87093, "vy":-1.22273, "omega":0.14593, "ax":-3.3597, "ay":4.05337, "alpha":-2.17536, "fx":[-54.12717,-41.65965,-56.31254,-67.59952], "fy":[67.91616,76.28057,66.24602,54.61706]}, + {"t":1.29459, "x":2.8979, "y":4.62093, "heading":-0.00326, "vx":0.79594, "vy":-1.13225, "omega":0.09737, "ax":-3.35929, "ay":4.05427, "alpha":-2.17939, "fx":[-54.06334,-41.63907,-56.34636,-67.62342], "fy":[67.98319,76.30325,66.22858,54.60357]}, + {"t":1.31691, "x":2.91483, "y":4.59666, "heading":-0.00109, "vx":0.72096, "vy":-1.04176, "omega":0.04873, "ax":-3.35892, "ay":4.05509, "alpha":-2.18314, "fx":[-54.01821,-41.61664,-56.36852,-67.64464], "fy":[68.03393,76.32595,66.22011,54.59215]}, + {"t":1.33923, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.64598, "vy":-0.95125, "omega":0.0, "ax":-1.45143, "ay":0.12678, "alpha":0.0, "fx":[-23.69969,-23.70318,-23.75429,-23.75545], "fy":[2.0995,1.93388,2.15919,2.0982]}, + {"t":1.3705, "x":2.94958, "y":4.54474, "heading":0.0, "vx":0.6006, "vy":-0.94728, "omega":0.0, "ax":-1.10919, "ay":0.99685, "alpha":0.0, "fx":[-18.13301,-18.13311,-18.13337,-18.13328], "fy":[16.29675,16.29728,16.29566,16.29671]}, + {"t":1.40176, "x":2.96781, "y":4.51561, "heading":0.0, "vx":0.56592, "vy":-0.91611, "omega":0.0, "ax":-0.95827, "ay":1.14737, "alpha":0.0, "fx":[-15.66794,-15.66676,-15.66452,-15.66408], "fy":[18.75557,18.76549,18.75208,18.75602]}, + {"t":1.43303, "x":2.98504, "y":4.48753, "heading":0.0, "vx":0.53596, "vy":-0.88024, "omega":0.0, "ax":-0.89014, "ay":1.20287, "alpha":0.0, "fx":[-14.55405,-14.55298,-14.55088,-14.55052], "fy":[19.66311,19.67179,19.66036,19.66353]}, + {"t":1.4643, "x":3.00136, "y":4.46059, "heading":0.0, "vx":0.50813, "vy":-0.84263, "omega":0.0, "ax":-0.85208, "ay":1.23113, "alpha":0.0, "fx":[-13.93144,-13.93059,-13.92892,-13.92863], "fy":[20.1254,20.13215,20.12337,20.12573]}, + {"t":1.49557, "x":3.01683, "y":4.43485, "heading":0.0, "vx":0.48149, "vy":-0.80414, "omega":0.0, "ax":-0.82797, "ay":1.2481, "alpha":0.0, "fx":[-13.53697,-13.53629,-13.53496,-13.53474], "fy":[20.40299,20.40826,20.40146,20.40325]}, + {"t":1.52683, "x":3.03148, "y":4.41031, "heading":0.0, "vx":0.4556, "vy":-0.76511, "omega":0.0, "ax":-0.81142, "ay":1.25934, "alpha":0.0, "fx":[-13.26613,-13.26558,-13.2645,-13.26433], "fy":[20.58703,20.59125,20.58585,20.58725]}, + {"t":1.5581, "x":3.04533, "y":4.38701, "heading":0.0, "vx":0.43023, "vy":-0.72574, "omega":0.0, "ax":-0.7994, "ay":1.26731, "alpha":0.0, "fx":[-13.06957,-13.06911,-13.06821,-13.06807], "fy":[20.71738,20.72088,20.7164,20.71756]}, + {"t":1.58937, "x":3.05839, "y":4.36493, "heading":0.0, "vx":0.40523, "vy":-0.68611, "omega":0.0, "ax":-0.79032, "ay":1.27322, "alpha":0.0, "fx":[-12.92101,-12.9206,-12.91982,-12.91969], "fy":[20.81414,20.81719,20.81331,20.8143]}, + {"t":1.62063, "x":3.07068, "y":4.3441, "heading":0.0, "vx":0.38052, "vy":-0.6463, "omega":0.0, "ax":-0.78324, "ay":1.27777, "alpha":0.0, "fx":[-12.80516,-12.80479,-12.80409,-12.80397], "fy":[20.88857,20.89133,20.88782,20.88872]}, + {"t":1.6519, "x":3.08219, "y":4.32452, "heading":0.0, "vx":0.35603, "vy":-0.60635, "omega":0.0, "ax":-0.77758, "ay":1.28137, "alpha":0.0, "fx":[-12.71258,-12.71223,-12.71156,-12.71145], "fy":[20.94742,20.95002,20.94672,20.94756]}, + {"t":1.68317, "x":3.09295, "y":4.30619, "heading":0.0, "vx":0.33172, "vy":-0.56628, "omega":0.0, "ax":-0.77296, "ay":1.28428, "alpha":0.0, "fx":[-12.63689,-12.63681,-12.63606,-12.63612], "fy":[20.99514,20.99665,20.99511,20.99512]}, + {"t":1.71443, "x":3.10294, "y":4.28911, "heading":0.0, "vx":0.30755, "vy":-0.52613, "omega":0.0, "ax":-0.76913, "ay":1.28668, "alpha":0.0, "fx":[-12.57428,-12.57419,-12.57344,-12.57349], "fy":[21.03433,21.03589,21.03426,21.03431]}, + {"t":1.7457, "x":3.11218, "y":4.27329, "heading":0.0, "vx":0.2835, "vy":-0.4859, "omega":0.0, "ax":-0.76591, "ay":1.28868, "alpha":0.0, "fx":[-12.52159,-12.52148,-12.52073,-12.52077], "fy":[21.06712,21.06878,21.06699,21.06711]}, + {"t":1.77697, "x":3.12067, "y":4.25873, "heading":0.0, "vx":0.25955, "vy":-0.4456, "omega":0.0, "ax":-0.76316, "ay":1.29038, "alpha":0.0, "fx":[-12.47669,-12.47657,-12.4758,-12.47583], "fy":[21.09492,21.0967,21.09474,21.09492]}, + {"t":1.80824, "x":3.12841, "y":4.24542, "heading":0.0, "vx":0.23569, "vy":-0.40526, "omega":0.0, "ax":-0.76079, "ay":1.29184, "alpha":0.0, "fx":[-12.438,-12.43786,-12.43707,-12.43709], "fy":[21.11879,21.12069,21.11855,21.11879]}, + {"t":1.8395, "x":3.13541, "y":4.23338, "heading":0.0, "vx":0.2119, "vy":-0.36487, "omega":0.0, "ax":-0.75873, "ay":1.29311, "alpha":0.0, "fx":[-12.40433,-12.40416,-12.40336,-12.40337], "fy":[21.13948,21.14151,21.13919,21.1395]}, + {"t":1.87077, "x":3.14166, "y":4.22261, "heading":0.0, "vx":0.18818, "vy":-0.32443, "omega":0.0, "ax":-0.75692, "ay":1.29422, "alpha":0.0, "fx":[-12.37475,-12.37457,-12.37374,-12.37374], "fy":[21.1576,21.15977,21.15725,21.15762]}, + {"t":1.90204, "x":3.14718, "y":4.2131, "heading":0.0, "vx":0.16451, "vy":-0.28397, "omega":0.0, "ax":-0.75532, "ay":1.2952, "alpha":0.0, "fx":[-12.34856,-12.34836,-12.34751,-12.3475], "fy":[21.1736,21.17591,21.17319,21.17363]}, + {"t":1.9333, "x":3.15195, "y":4.20485, "heading":0.0, "vx":0.1409, "vy":-0.24347, "omega":0.0, "ax":-0.75389, "ay":1.29607, "alpha":0.0, "fx":[-12.3252,-12.32498,-12.32411,-12.3241], "fy":[21.18784,21.19028,21.18738,21.18788]}, + {"t":1.96457, "x":3.15599, "y":4.19787, "heading":0.0, "vx":0.11733, "vy":-0.20295, "omega":0.0, "ax":-0.7526, "ay":1.29686, "alpha":0.0, "fx":[-12.30421,-12.30398,-12.30308,-12.30306], "fy":[21.2006,21.20316,21.20009,21.20065]}, + {"t":1.99584, "x":3.15929, "y":4.19216, "heading":0.0, "vx":0.09379, "vy":-0.1624, "omega":0.0, "ax":-0.75144, "ay":1.29756, "alpha":0.0, "fx":[-12.28525,-12.285,-12.28408,-12.28405], "fy":[21.21211,21.21479,21.21155,21.21216]}, + {"t":2.02711, "x":3.16186, "y":4.18772, "heading":0.0, "vx":0.0703, "vy":-0.12183, "omega":0.0, "ax":-0.75038, "ay":1.2982, "alpha":0.0, "fx":[-12.26801,-12.26774,-12.2668,-12.26677], "fy":[21.22255,21.22534,21.22195,21.22261]}, + {"t":2.05837, "x":3.16369, "y":4.18454, "heading":0.0, "vx":0.04684, "vy":-0.08124, "omega":0.0, "ax":-0.74942, "ay":1.29878, "alpha":0.0, "fx":[-12.25226,-12.25198,-12.25102,-12.25098], "fy":[21.23207,21.23496,21.23144,21.23214]}, + {"t":2.08964, "x":3.16478, "y":4.18264, "heading":0.0, "vx":0.0234, "vy":-0.04063, "omega":0.0, "ax":-0.74853, "ay":1.29932, "alpha":0.0, "fx":[-12.23695,-12.23691,-12.23856,-12.23594], "fy":[21.24334,21.24129,21.2412,21.23976]}, + {"t":2.12091, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index aef7eecf..1acd9fb3 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,73 +30,71 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.2456,2.23311], + "waypoints":[0.0,1.17196,2.15946], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.48118, "ay":-3.02968, "alpha":-0.00331, "fx":[56.89659,56.91977,56.92479,56.90161], "fy":[-49.54565,-49.51904,-49.51321,-49.53984]}, - {"t":0.03775, "x":1.55263, "y":7.52124, "heading":-0.93501, "vx":0.1314, "vy":-0.11436, "omega":-0.00012, "ax":3.48069, "ay":-3.02983, "alpha":-0.00339, "fx":[56.88825,56.91203,56.91718,56.89339], "fy":[-49.54865,-49.52134,-49.51536,-49.54268]}, - {"t":0.07549, "x":1.56007, "y":7.51477, "heading":-0.93501, "vx":0.26278, "vy":-0.22872, "omega":-0.00025, "ax":3.48015, "ay":-3.03001, "alpha":-0.00349, "fx":[56.87898,56.90344,56.90873,56.88426], "fy":[-49.55197,-49.52389,-49.51774,-49.54584]}, - {"t":0.11324, "x":1.57247, "y":7.50397, "heading":-0.93502, "vx":0.39414, "vy":-0.34309, "omega":-0.00038, "ax":3.47955, "ay":-3.0302, "alpha":-0.0036, "fx":[56.86861,56.89384,56.89929,56.87406], "fy":[-49.55568,-49.52674,-49.52041,-49.54937]}, - {"t":0.15098, "x":1.58982, "y":7.48887, "heading":-0.93503, "vx":0.52548, "vy":-0.45747, "omega":-0.00052, "ax":3.47886, "ay":-3.03042, "alpha":-0.00372, "fx":[56.85696,56.88303,56.88867,56.86258], "fy":[-49.55986,-49.52995,-49.5234,-49.55333]}, - {"t":0.18873, "x":1.61214, "y":7.46944, "heading":-0.93505, "vx":0.65679, "vy":-0.57185, "omega":-0.00066, "ax":3.47809, "ay":-3.03067, "alpha":-0.00386, "fx":[56.84375,56.8708,56.87663,56.84957], "fy":[-49.56459,-49.53358,-49.52679,-49.55782]}, - {"t":0.22647, "x":1.63941, "y":7.4457, "heading":-0.93508, "vx":0.78807, "vy":-0.68624, "omega":-0.00081, "ax":3.47721, "ay":-3.03095, "alpha":-0.00401, "fx":[56.82866,56.85681,56.86288,56.83471], "fy":[-49.57,-49.53772,-49.53066,-49.56296]}, - {"t":0.26422, "x":1.67163, "y":7.41763, "heading":-0.93511, "vx":0.91932, "vy":-0.80065, "omega":-0.00096, "ax":3.47619, "ay":-3.03128, "alpha":-0.0042, "fx":[56.81125,56.84068,56.84702,56.81757], "fy":[-49.57623,-49.54251,-49.53513,-49.56887]}, - {"t":0.30196, "x":1.7088, "y":7.38525, "heading":-0.93515, "vx":1.05053, "vy":-0.91507, "omega":-0.00112, "ax":3.47501, "ay":-3.03166, "alpha":-0.00441, "fx":[56.79095,56.82188,56.82852,56.79758], "fy":[-49.58349,-49.54808,-49.54034,-49.57577]}, - {"t":0.33971, "x":1.75093, "y":7.34855, "heading":-0.93519, "vx":1.1817, "vy":-1.0295, "omega":-0.00128, "ax":3.4736, "ay":-3.03211, "alpha":-0.00466, "fx":[56.76697,56.79966,56.80666,56.77396], "fy":[-49.59206,-49.55466,-49.54648,-49.58391]}, - {"t":0.37746, "x":1.79801, "y":7.30753, "heading":-0.93524, "vx":1.31281, "vy":-1.14395, "omega":-0.00146, "ax":3.47192, "ay":-3.03265, "alpha":-0.00496, "fx":[56.73821,56.77301,56.78045,56.74564], "fy":[-49.60233,-49.56254,-49.55384,-49.59367]}, - {"t":0.4152, "x":1.85004, "y":7.2622, "heading":-0.93529, "vx":1.44386, "vy":-1.25842, "omega":-0.00165, "ax":3.46987, "ay":-3.03331, "alpha":-0.00532, "fx":[56.70308,56.74046,56.74844,56.71103], "fy":[-49.61487,-49.57216,-49.56283,-49.60558]}, - {"t":0.45295, "x":1.90701, "y":7.21254, "heading":-0.93535, "vx":1.57483, "vy":-1.37291, "omega":-0.00185, "ax":3.4673, "ay":-3.03413, "alpha":-0.00578, "fx":[56.6592,56.69982,56.70845,56.66781], "fy":[-49.63051,-49.58416,-49.57404,-49.62043]}, - {"t":0.49069, "x":1.96892, "y":7.15855, "heading":-0.93542, "vx":1.70571, "vy":-1.48744, "omega":-0.00206, "ax":3.46401, "ay":-3.03519, "alpha":-0.00637, "fx":[56.60285,56.64763,56.6571,56.61229], "fy":[-49.65056,-49.59954,-49.58841,-49.63948]}, - {"t":0.52844, "x":2.03577, "y":7.10025, "heading":-0.9355, "vx":1.83646, "vy":-1.602, "omega":-0.00231, "ax":3.45962, "ay":-3.03658, "alpha":-0.00716, "fx":[56.52782,56.57815,56.58872,56.53836], "fy":[-49.6772,-49.61996,-49.60749,-49.66479]}, - {"t":0.56618, "x":2.10755, "y":7.03761, "heading":-0.93559, "vx":1.96704, "vy":-1.71662, "omega":-0.00258, "ax":3.4535, "ay":-3.03853, "alpha":-0.00825, "fx":[56.423,56.4811,56.4932,56.43505], "fy":[-49.7143,-49.64841,-49.63408,-49.70006]}, - {"t":0.60393, "x":2.18426, "y":6.97066, "heading":-0.93569, "vx":2.0974, "vy":-1.83131, "omega":-0.00289, "ax":3.44433, "ay":-3.04143, "alpha":-0.00989, "fx":[56.26626,56.33602,56.35034,56.28051], "fy":[-49.76953,-49.69074,-49.67365,-49.75257]}, - {"t":0.64167, "x":2.26588, "y":6.89937, "heading":-0.93579, "vx":2.22741, "vy":-1.94611, "omega":-0.00326, "ax":3.42914, "ay":-3.04621, "alpha":-0.01262, "fx":[56.00633,56.09553,56.11342,56.02411], "fy":[-49.86046,-49.7604,-49.73879,-49.83906]}, - {"t":0.67942, "x":2.3524, "y":6.82374, "heading":-0.93592, "vx":2.35684, "vy":-2.06109, "omega":-0.00374, "ax":3.39903, "ay":-3.05554, "alpha":-0.01803, "fx":[55.49145,55.61952,55.64399,55.51569], "fy":[-50.03812,-49.89636,-49.86606,-50.00822]}, - {"t":0.71717, "x":2.44378, "y":6.74376, "heading":-0.93606, "vx":2.48514, "vy":-2.17642, "omega":-0.00442, "ax":3.31097, "ay":-3.08185, "alpha":-0.03392, "fx":[53.9863,54.23041,54.27039,54.0254], "fy":[-50.53895,-50.279,-50.22501,-50.48633]}, - {"t":0.75491, "x":2.53994, "y":6.65942, "heading":-0.93623, "vx":2.61011, "vy":-2.29275, "omega":-0.0057, "ax":0.22135, "ay":-3.01768, "alpha":-0.64591, "fx":[2.84089,6.99076,4.46698,0.17601], "fy":[-50.86345,-49.39344,-47.75772,-49.31855]}, - {"t":0.79266, "x":2.63862, "y":6.57073, "heading":-0.93644, "vx":2.61847, "vy":-2.40665, "omega":-0.03008, "ax":-3.52195, "ay":2.827, "alpha":-0.03365, "fx":[-57.71276,-57.49259,-57.44203,-57.66159], "fy":[46.04252,46.31378,46.3888,46.11893]}, - {"t":0.8304, "x":2.73495, "y":6.4819, "heading":-0.93758, "vx":2.48553, "vy":-2.29995, "omega":-0.03135, "ax":-3.50697, "ay":2.9282, "alpha":-0.0159, "fx":[-57.39841,-57.29076,-57.26634,-57.37383], "fy":[47.7903,47.91857,47.9506,47.82264]}, - {"t":0.86815, "x":2.82627, "y":6.39718, "heading":-0.93876, "vx":2.35316, "vy":-2.18942, "omega":-0.03195, "ax":-3.50169, "ay":2.96127, "alpha":-0.0101, "fx":[-57.28836,-57.21928,-57.20362,-57.27264], "fy":[48.36056,48.44198,48.46166,48.38037]}, - {"t":0.90589, "x":2.91259, "y":6.31664, "heading":-0.93996, "vx":2.22098, "vy":-2.07764, "omega":-0.03233, "ax":-3.49899, "ay":2.97769, "alpha":-0.0072, "fx":[-57.23227,-57.18272,-57.17147,-57.22098], "fy":[48.64352,48.7016,48.71545,48.65743]}, - {"t":0.94364, "x":2.99393, "y":6.24034, "heading":-0.94118, "vx":2.08891, "vy":-1.96525, "omega":-0.0326, "ax":-3.49735, "ay":2.9875, "alpha":-0.00547, "fx":[-57.19824,-57.16052,-57.15192,-57.18962], "fy":[48.81265,48.85672,48.86717,48.82313]}, - {"t":0.98138, "x":3.07029, "y":6.16829, "heading":-0.94242, "vx":1.9569, "vy":-1.85249, "omega":-0.03281, "ax":-3.49625, "ay":2.99403, "alpha":-0.00431, "fx":[-57.17536,-57.14559,-57.13877,-57.16854], "fy":[48.92516,48.95988,48.96809,48.9334]}, - {"t":1.01913, "x":3.14166, "y":6.1005, "heading":-0.94365, "vx":1.82494, "vy":-1.73947, "omega":-0.03297, "ax":-3.49546, "ay":2.99868, "alpha":-0.00348, "fx":[-57.15893,-57.13486,-57.12932,-57.15338], "fy":[49.00542,49.03344,49.04007,49.01206]}, - {"t":1.05688, "x":3.20806, "y":6.03698, "heading":-0.9449, "vx":1.693, "vy":-1.62629, "omega":-0.0331, "ax":-3.49486, "ay":3.00217, "alpha":-0.00285, "fx":[-57.14654,-57.12677,-57.1222,-57.14196], "fy":[49.06556,49.08855,49.09399,49.07102]}, - {"t":1.09462, "x":3.26947, "y":5.97773, "heading":-0.94615, "vx":1.56108, "vy":-1.51297, "omega":-0.03321, "ax":-3.4944, "ay":3.00488, "alpha":-0.00237, "fx":[-57.13686,-57.12045,-57.11664,-57.13304], "fy":[49.11232,49.13138,49.1359,49.11685]}, - {"t":1.13237, "x":3.3259, "y":5.92277, "heading":-0.9474, "vx":1.42918, "vy":-1.39955, "omega":-0.0333, "ax":-3.49402, "ay":3.00705, "alpha":-0.00198, "fx":[-57.12909,-57.11537,-57.11217,-57.12588], "fy":[49.1497,49.16562,49.16941,49.1535]}, - {"t":1.17011, "x":3.37736, "y":5.87208, "heading":-0.94866, "vx":1.2973, "vy":-1.28605, "omega":-0.03337, "ax":-3.49372, "ay":3.00882, "alpha":-0.00166, "fx":[-57.12271,-57.11121,-57.10851,-57.12001], "fy":[49.18029,49.19363,49.19681,49.18348]}, - {"t":1.20786, "x":3.42384, "y":5.82568, "heading":-0.94992, "vx":1.16543, "vy":-1.17248, "omega":-0.03344, "ax":-3.49346, "ay":3.0103, "alpha":-0.00139, "fx":[-57.11738,-57.10773,-57.10545,-57.1151], "fy":[49.20577,49.21696,49.21964,49.20845]}, - {"t":1.2456, "x":3.46534, "y":5.78357, "heading":-0.95118, "vx":1.03357, "vy":-1.05885, "omega":-0.03349, "ax":-1.12411, "ay":0.98635, "alpha":-0.42679, "fx":[-18.794,-16.80009,-17.95851,-19.95541], "fy":[14.41941,15.97602,17.82529,16.27887]}, - {"t":1.27746, "x":3.49769, "y":5.75034, "heading":-0.95225, "vx":0.99776, "vy":-1.02743, "omega":-0.04708, "ax":-1.11126, "ay":1.00453, "alpha":-0.39388, "fx":[-18.56966,-16.65637,-17.77713,-19.66488], "fy":[14.89258,16.30103,17.93894,16.55601]}, - {"t":1.30931, "x":3.52891, "y":5.71812, "heading":-0.95375, "vx":0.96236, "vy":-0.99543, "omega":-0.05963, "ax":-1.09943, "ay":1.01776, "alpha":-0.3607, "fx":[-18.33368,-16.63113,-17.61685,-19.31238], "fy":[15.20568,16.51089,18.06697,16.7704]}, - {"t":1.34117, "x":3.55901, "y":5.68693, "heading":-0.95565, "vx":0.92734, "vy":-0.96301, "omega":-0.07112, "ax":-1.08961, "ay":1.0285, "alpha":-0.32917, "fx":[-18.1538,-16.5481,-17.48123,-19.06894], "fy":[15.54129,16.70765,18.07708,16.93033]}, - {"t":1.37302, "x":3.588, "y":5.65678, "heading":-0.95791, "vx":0.89263, "vy":-0.93025, "omega":-0.08161, "ax":-1.08134, "ay":1.03739, "alpha":-0.29739, "fx":[-17.97869,-16.57007,-17.37846,-18.78402], "fy":[15.78195,16.84841,18.13404,17.07323]}, - {"t":1.40488, "x":3.61589, "y":5.62767, "heading":-0.96051, "vx":0.85818, "vy":-0.8972, "omega":-0.09108, "ax":-1.07428, "ay":1.04487, "alpha":-0.26705, "fx":[-17.84277,-16.53508,-17.28802,-18.58367], "fy":[16.05275,16.98958,18.10297,17.1812]}, - {"t":1.43673, "x":3.64268, "y":5.59962, "heading":-0.96341, "vx":0.82396, "vy":-0.86392, "omega":-0.09959, "ax":-1.06818, "ay":1.05124, "alpha":-0.23632, "fx":[-17.70491,-16.58292,-17.22043,-18.34258], "fy":[16.25241,17.09122,18.11761,17.28203]}, - {"t":1.46859, "x":3.66838, "y":5.57263, "heading":-0.96658, "vx":0.78993, "vy":-0.83043, "omega":-0.10712, "ax":-1.06286, "ay":1.05674, "alpha":-0.20684, "fx":[-17.59603,-16.57945,-17.15929,-18.16838], "fy":[16.48157,17.19903,18.06402,17.35816]}, - {"t":1.50044, "x":3.69301, "y":5.54671, "heading":-0.97, "vx":0.75607, "vy":-0.79677, "omega":-0.1137, "ax":-1.05819, "ay":1.06153, "alpha":-0.17685, "fx":[-17.48233,-16.64222,-17.11503,-17.95771], "fy":[16.65664,17.27691,18.05067,17.43179]}, - {"t":1.5323, "x":3.71655, "y":5.52187, "heading":-0.97362, "vx":0.72237, "vy":-0.76295, "omega":-0.11934, "ax":-1.05404, "ay":1.06574, "alpha":-0.14791, "fx":[-17.39072,-16.66124,-17.07445,-17.79976], "fy":[16.85716,17.36347,17.98398,17.48672]}, - {"t":1.56415, "x":3.73903, "y":5.49811, "heading":-0.97742, "vx":0.68879, "vy":-0.729, "omega":-0.12405, "ax":-1.05034, "ay":1.06947, "alpha":-0.11838, "fx":[-17.29332,-16.73298,-17.04669,-17.6112], "fy":[17.01717,17.42629,17.95037,17.54141]}, - {"t":1.59601, "x":3.76044, "y":5.47543, "heading":-0.98137, "vx":0.65533, "vy":-0.69493, "omega":-0.12782, "ax":-1.04702, "ay":1.0728, "alpha":-0.08971, "fx":[-17.21289,-16.76924,-17.02158,-17.46321], "fy":[17.19709,17.49874,17.87567,17.58129]}, - {"t":1.62786, "x":3.78078, "y":5.45384, "heading":-0.98544, "vx":0.62198, "vy":-0.66076, "omega":-0.13068, "ax":-1.04402, "ay":1.07578, "alpha":-0.06037, "fx":[-17.12679,-16.84661,-17.00614,-17.29124], "fy":[17.34809,17.55185,17.82619,17.62193]}, - {"t":1.65972, "x":3.80007, "y":5.43333, "heading":-0.98961, "vx":0.58872, "vy":-0.62649, "omega":-0.1326, "ax":-1.0413, "ay":1.07848, "alpha":-0.03171, "fx":[-17.05352,-16.89699,-16.9934,-17.14891], "fy":[17.51275,17.61464,17.74629,17.65056]}, - {"t":1.69157, "x":3.81829, "y":5.41392, "heading":-0.99383, "vx":0.55555, "vy":-0.59214, "omega":-0.13361, "ax":-1.03882, "ay":1.08092, "alpha":-0.00233, "fx":[-16.97536,-16.97809,-16.98757,-16.98959], "fy":[17.65886,17.66153,17.68346,17.6802]}, - {"t":1.72343, "x":3.83546, "y":5.39561, "heading":-0.99809, "vx":0.52246, "vy":-0.5577, "omega":-0.13369, "ax":-1.03655, "ay":1.08315, "alpha":0.02657, "fx":[-16.90644,-17.04074,-16.98523,-16.84977], "fy":[17.81206,17.71757,17.59991,17.70008]}, - {"t":1.75528, "x":3.85158, "y":5.37839, "heading":-1.00235, "vx":0.48944, "vy":-0.5232, "omega":-0.13284, "ax":-1.03446, "ay":1.08518, "alpha":0.05624, "fx":[-16.83393,-17.12455,-16.98718,-16.70019], "fy":[17.95632,17.76061,17.52514,17.72074]}, - {"t":1.78714, "x":3.86665, "y":5.36228, "heading":-1.00658, "vx":0.45649, "vy":-0.48863, "omega":-0.13105, "ax":-1.03254, "ay":1.08706, "alpha":0.08562, "fx":[-16.76735,-17.19845,-16.99399,-16.56039], "fy":[18.1009,17.81185,17.43876,17.7336]}, - {"t":1.81899, "x":3.88066, "y":5.34726, "heading":-1.01075, "vx":0.4236, "vy":-0.454, "omega":-0.12832, "ax":-1.03076, "ay":1.08878, "alpha":0.1158, "fx":[-16.69886,-17.28447,-17.00241,-16.41824], "fy":[18.24569,17.85273,17.35275,17.74666]}, - {"t":1.85085, "x":3.89363, "y":5.33335, "heading":-1.01484, "vx":0.39076, "vy":-0.41932, "omega":-0.12463, "ax":-1.02911, "ay":1.09037, "alpha":0.14589, "fx":[-16.63313,-17.36914,-17.01756,-16.2764], "fy":[18.38389,17.90045,17.26388,17.75378]}, - {"t":1.8827, "x":3.90556, "y":5.32055, "heading":-1.01881, "vx":0.35798, "vy":-0.38459, "omega":-0.11998, "ax":-1.02758, "ay":1.09185, "alpha":0.17681, "fx":[-16.56751,-17.45724,-17.03145,-16.13981], "fy":[18.53119,17.94036,17.16683,17.76023]}, - {"t":1.91456, "x":3.91644, "y":5.30885, "heading":-1.02263, "vx":0.32525, "vy":-0.34981, "omega":-0.11435, "ax":-1.02615, "ay":1.09322, "alpha":0.20784, "fx":[-16.50152,-17.55263,-17.05438,-15.99407], "fy":[18.66486,17.98542,17.0755,17.76263]}, - {"t":1.94641, "x":3.92628, "y":5.29826, "heading":-1.02627, "vx":0.29256, "vy":-0.31498, "omega":-0.10773, "ax":-1.02482, "ay":1.0945, "alpha":0.23975, "fx":[-16.43798,-17.64294,-17.07296,-15.86142], "fy":[18.81645,18.02521,16.9672,17.76326]}, - {"t":1.97827, "x":3.93508, "y":5.28879, "heading":-1.02971, "vx":0.25991, "vy":-0.28012, "omega":-0.10009, "ax":-1.02357, "ay":1.0957, "alpha":0.27197, "fx":[-16.37091,-17.7493,-17.10328,-15.71004], "fy":[18.94718,18.06814,16.87323,17.76179]}, - {"t":2.01012, "x":3.94284, "y":5.28042, "heading":-1.03289, "vx":0.22731, "vy":-0.24521, "omega":-0.09143, "ax":-1.02239, "ay":1.09682, "alpha":0.3051, "fx":[-16.3089,-17.84215,-17.12588,-15.57987], "fy":[19.10472,18.10839,16.7532,17.75727]}, - {"t":2.04198, "x":3.94956, "y":5.27316, "heading":-1.03581, "vx":0.19474, "vy":-0.21027, "omega":-0.08171, "ax":-1.02129, "ay":1.09787, "alpha":0.33872, "fx":[-16.24026,-17.96003,-17.16326,-15.42109], "fy":[19.23356,18.14955,16.65634,17.75285]}, - {"t":2.07383, "x":3.95525, "y":5.26702, "heading":-1.03841, "vx":0.1622, "vy":-0.1753, "omega":-0.07092, "ax":-1.02025, "ay":1.09886, "alpha":0.37336, "fx":[-16.1794,-18.05593,-17.18927,-15.29206], "fy":[19.39908,18.19047,16.52366,17.74371]}, - {"t":2.10569, "x":3.9599, "y":5.262, "heading":-1.04067, "vx":0.1297, "vy":-0.1403, "omega":-0.05903, "ax":-1.01927, "ay":1.09979, "alpha":0.40869, "fx":[-16.10875,-18.18624,-17.23343,-15.12409], "fy":[19.52786,18.22943,16.42339,17.7371]}, - {"t":2.13754, "x":3.96351, "y":5.25809, "heading":-1.04255, "vx":0.09724, "vy":-0.10526, "omega":-0.04601, "ax":-1.01834, "ay":1.10067, "alpha":0.44508, "fx":[-16.04902,-18.28574,-17.26224,-14.99489], "fy":[19.70254,18.2716,16.27701,17.72406]}, - {"t":2.1694, "x":3.96609, "y":5.25529, "heading":-1.04401, "vx":0.0648, "vy":-0.0702, "omega":-0.03183, "ax":-1.01747, "ay":1.1015, "alpha":0.48241, "fx":[-15.97628,-18.4295,-17.31286,-14.81583], "fy":[19.83248,18.30855,16.17254,17.71593]}, - {"t":2.20125, "x":3.96764, "y":5.25361, "heading":-1.04503, "vx":0.03238, "vy":-0.03511, "omega":-0.01646, "ax":-1.01663, "ay":1.10228, "alpha":0.51683, "fx":[-15.919,-18.53043,-17.34202,-14.68862], "fy":[19.99151,18.34685,16.04057,17.70193]}, - {"t":2.23311, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.01035, "ay":-3.50327, "alpha":-0.00347, "fx":[65.54678,65.57116,65.5764,65.55201], "fy":[-57.28882,-57.26093,-57.25487,-57.28278]}, + {"t":0.03781, "x":1.55302, "y":7.52089, "heading":-0.93501, "vx":0.15161, "vy":-0.13244, "omega":-0.00013, "ax":4.00973, "ay":-3.50349, "alpha":-0.00357, "fx":[65.5363,65.56136,65.56674,65.54167], "fy":[-57.29289,-57.26423,-57.25801,-57.28668]}, + {"t":0.07561, "x":1.56161, "y":7.51338, "heading":-0.93501, "vx":0.3032, "vy":-0.26489, "omega":-0.00027, "ax":4.00905, "ay":-3.50374, "alpha":-0.00367, "fx":[65.52459,65.55041,65.55594,65.53012], "fy":[-57.29744,-57.26791,-57.26151,-57.29105]}, + {"t":0.11342, "x":1.57594, "y":7.50087, "heading":-0.93502, "vx":0.45476, "vy":-0.39735, "omega":-0.0004, "ax":4.00827, "ay":-3.50402, "alpha":-0.0038, "fx":[65.5114,65.53808,65.54379,65.51711], "fy":[-57.30255,-57.27206,-57.26545,-57.29596]}, + {"t":0.15122, "x":1.596, "y":7.48334, "heading":-0.93504, "vx":0.6063, "vy":-0.52982, "omega":-0.00055, "ax":4.00739, "ay":-3.50433, "alpha":-0.00393, "fx":[65.49645,65.5241,65.53001,65.50236], "fy":[-57.30836,-57.27676,-57.26991,-57.30152]}, + {"t":0.18903, "x":1.62178, "y":7.46081, "heading":-0.93506, "vx":0.7578, "vy":-0.6623, "omega":-0.0007, "ax":4.00639, "ay":-3.50469, "alpha":-0.00409, "fx":[65.47935,65.50811,65.51425,65.48548], "fy":[-57.31499,-57.28214,-57.27502,-57.30789]}, + {"t":0.22683, "x":1.6533, "y":7.43326, "heading":-0.93508, "vx":0.90926, "vy":-0.7948, "omega":-0.00085, "ax":4.00523, "ay":-3.5051, "alpha":-0.00427, "fx":[65.4596,65.48964,65.49605,65.466], "fy":[-57.32265,-57.28835,-57.28091,-57.31523]}, + {"t":0.26464, "x":1.69053, "y":7.40071, "heading":-0.93512, "vx":1.06068, "vy":-0.92731, "omega":-0.00101, "ax":4.00387, "ay":-3.50559, "alpha":-0.00448, "fx":[65.43654,65.46807,65.47479,65.44324], "fy":[-57.33158,-57.29559,-57.28779,-57.32381]}, + {"t":0.30244, "x":1.73349, "y":7.36315, "heading":-0.93515, "vx":1.21205, "vy":-1.05984, "omega":-0.00118, "ax":4.00227, "ay":-3.50616, "alpha":-0.00474, "fx":[65.40925,65.44256,65.44965,65.41632], "fy":[-57.34215,-57.30416,-57.29593,-57.33395]}, + {"t":0.34025, "x":1.78217, "y":7.32057, "heading":-0.9352, "vx":1.36335, "vy":-1.19239, "omega":-0.00136, "ax":4.00034, "ay":-3.50685, "alpha":-0.00504, "fx":[65.37646,65.41191,65.41943,65.38397], "fy":[-57.35484,-57.31444,-57.3057,-57.34612]}, + {"t":0.37805, "x":1.83658, "y":7.27299, "heading":-0.93525, "vx":1.51459, "vy":-1.32497, "omega":-0.00155, "ax":3.99798, "ay":-3.50769, "alpha":-0.00541, "fx":[65.33633,65.37439,65.38244,65.34436], "fy":[-57.37035,-57.32702,-57.31764,-57.36101]}, + {"t":0.41586, "x":1.89669, "y":7.22039, "heading":-0.93531, "vx":1.66573, "vy":-1.45758, "omega":-0.00176, "ax":3.99503, "ay":-3.50874, "alpha":-0.00587, "fx":[65.28607,65.32741,65.33613,65.29476], "fy":[-57.38977,-57.34275,-57.33259,-57.37964]}, + {"t":0.45366, "x":1.96252, "y":7.16278, "heading":-0.93537, "vx":1.81676, "vy":-1.59023, "omega":-0.00198, "ax":3.99122, "ay":-3.51009, "alpha":-0.00647, "fx":[65.2213,65.26688,65.27643,65.23083], "fy":[-57.41475,-57.36299,-57.35182,-57.40361]}, + {"t":0.49147, "x":2.03406, "y":7.10015, "heading":-0.93545, "vx":1.96765, "vy":-1.72293, "omega":-0.00222, "ax":3.98613, "ay":-3.5119, "alpha":-0.00727, "fx":[65.13467,65.18593,65.19661,65.14532], "fy":[-57.44809,-57.39,-57.37748,-57.43562]}, + {"t":0.52927, "x":2.11129, "y":7.03251, "heading":-0.93553, "vx":2.11835, "vy":-1.8557, "omega":-0.0025, "ax":3.97897, "ay":-3.51443, "alpha":-0.00839, "fx":[65.0129,65.07217,65.0844,65.0251], "fy":[-57.49483,-57.42786,-57.41345,-57.48049]}, + {"t":0.56708, "x":2.19422, "y":6.95984, "heading":-0.93563, "vx":2.26878, "vy":-1.98856, "omega":-0.00282, "ax":3.96818, "ay":-3.51823, "alpha":-0.0101, "fx":[64.8292,64.90059,64.91512,64.84366], "fy":[-57.56505,-57.48472,-57.4675,-57.54794]}, + {"t":0.60488, "x":2.28283, "y":6.88215, "heading":-0.93573, "vx":2.41879, "vy":-2.12157, "omega":-0.0032, "ax":3.95002, "ay":-3.52458, "alpha":-0.01296, "fx":[64.52022,64.61215,64.6304,64.53837], "fy":[-57.68236,-57.57967,-57.55778,-57.66065]}, + {"t":0.64269, "x":2.37709, "y":6.79942, "heading":-0.93586, "vx":2.56813, "vy":-2.25481, "omega":-0.00369, "ax":3.91309, "ay":-3.53732, "alpha":-0.01882, "fx":[63.89181,64.02599,64.05127,63.91687], "fy":[-57.91784,-57.77011,-57.73902,-57.88712]}, + {"t":0.68049, "x":2.47698, "y":6.71165, "heading":-0.93599, "vx":2.71606, "vy":-2.38854, "omega":-0.0044, "ax":3.79716, "ay":-3.57591, "alpha":-0.03732, "fx":[61.92013,62.19055,62.2331,61.96173], "fy":[-58.63008,-58.34541,-58.28787,-58.57398]}, + {"t":0.7183, "x":2.58237, "y":6.6188, "heading":-0.93616, "vx":2.85961, "vy":-2.52373, "omega":-0.00581, "ax":-3.96381, "ay":0.86701, "alpha":-0.4768, "fx":[-65.40693,-63.98412,-64.18797,-65.62365], "fy":[11.28726,14.36507,17.03743,14.00622]}, + {"t":0.7561, "x":2.68765, "y":6.52401, "heading":-0.93638, "vx":2.70976, "vy":-2.49095, "omega":-0.02383, "ax":-4.06175, "ay":3.29442, "alpha":-0.03031, "fx":[-66.52506,-66.32492,-66.27895,-66.47865], "fy":[53.70214,53.94665,54.01241,53.76889]}, + {"t":0.79391, "x":2.78719, "y":6.43219, "heading":-0.93728, "vx":2.55621, "vy":-2.36641, "omega":-0.02498, "ax":-4.04317, "ay":3.39336, "alpha":-0.01499, "fx":[-66.1606,-66.0587,-66.03568,-66.13747], "fy":[55.39957,55.52045,55.55018,55.42954]}, + {"t":0.83172, "x":2.88094, "y":6.34515, "heading":-0.93823, "vx":2.40335, "vy":-2.23812, "omega":-0.02555, "ax":-4.03636, "ay":3.4278, "alpha":-0.00964, "fx":[-66.02731,-65.96111,-65.94621,-66.01236], "fy":[55.98979,56.06751,56.08606,56.00843]}, + {"t":0.86952, "x":2.96891, "y":6.26299, "heading":-0.93919, "vx":2.25076, "vy":-2.10853, "omega":-0.02591, "ax":-4.03282, "ay":3.44531, "alpha":-0.00691, "fx":[-65.9581,-65.91043,-65.8997,-65.94734], "fy":[56.28985,56.34552,56.35863,56.303]}, + {"t":0.90733, "x":3.05112, "y":6.18574, "heading":-0.94017, "vx":2.0983, "vy":-1.97828, "omega":-0.02617, "ax":-4.03065, "ay":3.45592, "alpha":-0.00525, "fx":[-65.91566,-65.87936,-65.87118,-65.90746], "fy":[56.47156,56.51382,56.52369,56.48147]}, + {"t":0.94513, "x":3.12757, "y":6.11342, "heading":-0.94116, "vx":1.94592, "vy":-1.84763, "omega":-0.02637, "ax":-4.02918, "ay":3.46303, "alpha":-0.00413, "fx":[-65.88696,-65.85836,-65.85189,-65.88048], "fy":[56.59342,56.62665,56.63439,56.60119]}, + {"t":0.98294, "x":3.19825, "y":6.04604, "heading":-0.94216, "vx":1.79359, "vy":-1.71671, "omega":-0.02653, "ax":-4.02812, "ay":3.46813, "alpha":-0.00332, "fx":[-65.86623,-65.8432,-65.83798,-65.861], "fy":[56.68084,56.70756,56.71378,56.68708]}, + {"t":1.02074, "x":3.26318, "y":5.98362, "heading":-0.94316, "vx":1.64131, "vy":-1.5856, "omega":-0.02665, "ax":-4.02732, "ay":3.47197, "alpha":-0.00271, "fx":[-65.85056,-65.83175,-65.82746,-65.84627], "fy":[56.74662,56.76842,56.7735,56.75171]}, + {"t":1.05855, "x":3.32235, "y":5.92616, "heading":-0.94417, "vx":1.48905, "vy":-1.45434, "omega":-0.02676, "ax":-4.02669, "ay":3.47496, "alpha":-0.00223, "fx":[-65.83829,-65.82278,-65.81923,-65.83474], "fy":[56.79791,56.81587,56.82006,56.80211]}, + {"t":1.09635, "x":3.37577, "y":5.87366, "heading":-0.94518, "vx":1.33682, "vy":-1.32296, "omega":-0.02684, "ax":-4.02619, "ay":3.47736, "alpha":-0.00185, "fx":[-65.82842,-65.81556,-65.81261,-65.82547], "fy":[56.83903,56.85391,56.85738,56.84251]}, + {"t":1.13416, "x":3.42343, "y":5.82613, "heading":-0.94619, "vx":1.18461, "vy":-1.1915, "omega":-0.02691, "ax":-4.02577, "ay":3.47933, "alpha":-0.00153, "fx":[-65.82031,-65.80964,-65.80718,-65.81785], "fy":[56.87273,56.88507,56.88796,56.87562]}, + {"t":1.17196, "x":3.46534, "y":5.78357, "heading":-0.94721, "vx":1.03242, "vy":-1.05997, "omega":-0.02697, "ax":-1.12119, "ay":0.98869, "alpha":-0.47799, "fx":[-18.74966,-16.56794,-17.89407,-20.10559], "fy":[14.25565,15.95412,18.06787,16.37541]}, + {"t":1.20382, "x":3.49766, "y":5.75031, "heading":-0.94807, "vx":0.9967, "vy":-1.02847, "omega":-0.04219, "ax":-1.10748, "ay":1.00834, "alpha":-0.44053, "fx":[-18.51306,-16.39309,-17.70841,-19.80595], "fy":[14.79348,16.31901,18.15702,16.66845]}, + {"t":1.23567, "x":3.52885, "y":5.71806, "heading":-0.94942, "vx":0.96142, "vy":-0.99635, "omega":-0.05623, "ax":-1.09517, "ay":1.02207, "alpha":-0.40256, "fx":[-18.26658,-16.41346,-17.53754,-19.39824], "fy":[15.10635,16.52858,18.30855,16.89197]}, + {"t":1.26753, "x":3.55892, "y":5.68684, "heading":-0.95121, "vx":0.92654, "vy":-0.96379, "omega":-0.06905, "ax":-1.08524, "ay":1.0329, "alpha":-0.36849, "fx":[-18.08772,-16.30832,-17.40361,-19.16707], "fy":[15.47552,16.74198,18.2814,17.04461]}, + {"t":1.29938, "x":3.58788, "y":5.65666, "heading":-0.95341, "vx":0.89197, "vy":-0.93089, "omega":-0.08079, "ax":-1.07707, "ay":1.04165, "alpha":-0.33249, "fx":[-17.9121,-16.37668,-17.29996,-18.84354], "fy":[15.70853,16.87373,18.34782,17.18612]}, + {"t":1.33124, "x":3.61575, "y":5.62754, "heading":-0.95598, "vx":0.85766, "vy":-0.89771, "omega":-0.09138, "ax":-1.07023, "ay":1.04888, "alpha":-0.30014, "fx":[-17.78277,-16.32836,-17.21505,-18.65853], "fy":[16.00129,17.0242,18.28116,17.28216]}, + {"t":1.36309, "x":3.64253, "y":5.59947, "heading":-0.95889, "vx":0.82356, "vy":-0.8643, "omega":-0.10094, "ax":-1.06441, "ay":1.05495, "alpha":-0.26564, "fx":[-17.64771,-16.41856,-17.15004,-18.38819], "fy":[16.19298,17.11522,18.29887,17.37835]}, + {"t":1.39495, "x":3.66822, "y":5.57247, "heading":-0.96211, "vx":0.78966, "vy":-0.83069, "omega":-0.1094, "ax":-1.05941, "ay":1.06011, "alpha":-0.23449, "fx":[-17.54732,-16.40684,-17.09501,-18.22838], "fy":[16.43759,17.22948,18.21412,17.44174]}, + {"t":1.4268, "x":3.69284, "y":5.54655, "heading":-0.96559, "vx":0.75591, "vy":-0.79692, "omega":-0.11687, "ax":-1.05507, "ay":1.06455, "alpha":-0.20191, "fx":[-17.43868,-16.50115,-17.05237,-18.00124], "fy":[16.60633,17.2982,18.20048,17.50861]}, + {"t":1.45866, "x":3.71638, "y":5.52171, "heading":-0.96931, "vx":0.7223, "vy":-0.76301, "omega":-0.1233, "ax":-1.05126, "ay":1.06842, "alpha":-0.17146, "fx":[-17.35555,-16.52209,-17.01833,-17.84829], "fy":[16.81335,17.3843,18.11413,17.55479]}, + {"t":1.49051, "x":3.73886, "y":5.49794, "heading":-0.97324, "vx":0.68881, "vy":-0.72898, "omega":-0.12877, "ax":-1.04789, "ay":1.07182, "alpha":-0.13874, "fx":[-17.26252,-16.61938,-16.99467,-17.64741], "fy":[16.97453,17.44389,18.07006,17.60021]}, + {"t":1.52237, "x":3.76027, "y":5.47526, "heading":-0.97734, "vx":0.65543, "vy":-0.69483, "omega":-0.13319, "ax":-1.04489, "ay":1.07482, "alpha":-0.10848, "fx":[-17.19008,-16.66033,-16.97506,-17.50239], "fy":[17.15849,17.51882,17.97744,17.6305]}, + {"t":1.55422, "x":3.78062, "y":5.45368, "heading":-0.98159, "vx":0.62215, "vy":-0.6606, "omega":-0.13664, "ax":-1.0422, "ay":1.0775, "alpha":-0.07617, "fx":[-17.10774,-16.76069,-16.96347,-17.32021], "fy":[17.313,17.5663,17.91771,17.66344]}, + {"t":1.58608, "x":3.7999, "y":5.43318, "heading":-0.98594, "vx":0.58895, "vy":-0.62627, "omega":-0.13907, "ax":-1.03978, "ay":1.07991, "alpha":-0.04585, "fx":[-17.04195,-16.81746,-16.95567,-17.17867], "fy":[17.48095,17.63182,17.82206,17.68274]}, + {"t":1.61793, "x":3.81814, "y":5.41378, "heading":-0.99037, "vx":0.55583, "vy":-0.59187, "omega":-0.14053, "ax":-1.03759, "ay":1.08207, "alpha":-0.0136, "fx":[-16.96706,-16.91891,-16.9535,-17.01084], "fy":[17.63207,17.67405,17.74808,17.70506]}, + {"t":1.64978, "x":3.83532, "y":5.39547, "heading":-0.99485, "vx":0.52278, "vy":-0.5574, "omega":-0.14096, "ax":-1.03559, "ay":1.08404, "alpha":0.01678, "fx":[-16.90558,-16.98962,-16.95551,-16.86907], "fy":[17.78448,17.73436,17.65172,17.71715]}, + {"t":1.68164, "x":3.85145, "y":5.37827, "heading":-0.99934, "vx":0.48979, "vy":-0.52287, "omega":-0.14043, "ax":-1.03377, "ay":1.08582, "alpha":0.04964, "fx":[-16.8351,-17.09191,-16.96114,-16.71233], "fy":[17.93932,17.77259,17.56336,17.72941]}, + {"t":1.71349, "x":3.86652, "y":5.36216, "heading":-1.00381, "vx":0.45686, "vy":-0.48828, "omega":-0.13885, "ax":-1.03209, "ay":1.08746, "alpha":0.08067, "fx":[-16.77396,-17.17929,-16.97106,-16.56673], "fy":[18.08835,17.81128,17.47135,17.74065]}, + {"t":1.74535, "x":3.88055, "y":5.34716, "heading":-1.00823, "vx":0.42398, "vy":-0.45364, "omega":-0.13628, "ax":-1.03055, "ay":1.08896, "alpha":0.11423, "fx":[-16.70802,-17.27898,-16.98411,-16.41917], "fy":[18.24062,17.86564,17.36424,17.73931]}, + {"t":1.7772, "x":3.89354, "y":5.33326, "heading":-1.01257, "vx":0.39115, "vy":-0.41895, "omega":-0.13264, "ax":-1.02913, "ay":1.09034, "alpha":0.14664, "fx":[-16.64615,-17.3789,-17.00306,-16.2691], "fy":[18.38563,17.90502,17.26762,17.74196]}, + {"t":1.80906, "x":3.90547, "y":5.32047, "heading":-1.0168, "vx":0.35837, "vy":-0.38422, "omega":-0.12797, "ax":-1.02781, "ay":1.09162, "alpha":0.17964, "fx":[-16.58789,-17.46555,-17.02568,-16.13186], "fy":[18.53985,17.95857,17.14954,17.73584]}, + {"t":1.84091, "x":3.91637, "y":5.30878, "heading":-1.02087, "vx":0.32563, "vy":-0.34945, "omega":-0.12224, "ax":-1.02659, "ay":1.09281, "alpha":0.21459, "fx":[-16.51961,-17.59358,-17.04809,-15.96959], "fy":[18.68145,17.99375,17.05145,17.73461]}, + {"t":1.87277, "x":3.92622, "y":5.29821, "heading":-1.02477, "vx":0.29293, "vy":-0.31463, "omega":-0.11541, "ax":-1.02544, "ay":1.09391, "alpha":0.24913, "fx":[-16.45951,-17.68398,-17.07604,-15.83672], "fy":[18.84401,18.0473,16.91974,17.72221]}, + {"t":1.90462, "x":3.93503, "y":5.28874, "heading":-1.02844, "vx":0.26026, "vy":-0.27979, "omega":-0.10747, "ax":-1.02438, "ay":1.09493, "alpha":0.28556, "fx":[-16.39207,-17.82412,-17.1058,-15.66458], "fy":[18.98242,18.0817,16.81901,17.71722]}, + {"t":1.93648, "x":3.9428, "y":5.28038, "heading":-1.03187, "vx":0.22763, "vy":-0.24491, "omega":-0.09838, "ax":-1.02338, "ay":1.09589, "alpha":0.32177, "fx":[-16.33392,-17.91888,-17.13423,-15.53431], "fy":[19.15467,18.1362,16.67328,17.69888]}, + {"t":1.96833, "x":3.94953, "y":5.27314, "heading":-1.035, "vx":0.19503, "vy":-0.21, "omega":-0.08813, "ax":-1.02245, "ay":1.09679, "alpha":0.36031, "fx":[-16.26226,-18.07235,-17.17554,-15.35002], "fy":[19.29246,18.17006,16.56831,17.69087]}, + {"t":2.00019, "x":3.95523, "y":5.267, "heading":-1.03781, "vx":0.16246, "vy":-0.17506, "omega":-0.07665, "ax":-1.02157, "ay":1.09763, "alpha":0.39872, "fx":[-16.2047,-18.17135,-17.20735,-15.2193], "fy":[19.47686,18.22614,16.40652,17.66722]}, + {"t":2.03204, "x":3.95988, "y":5.26198, "heading":-1.04025, "vx":0.12992, "vy":-0.1401, "omega":-0.06395, "ax":-1.02074, "ay":1.09842, "alpha":0.43963, "fx":[-16.12934,-18.3407,-17.25674,-15.0218], "fy":[19.61554,18.25928,16.29694,17.65673]}, + {"t":2.0639, "x":3.9635, "y":5.25808, "heading":-1.04229, "vx":0.0974, "vy":-0.10511, "omega":-0.04994, "ax":-1.01996, "ay":1.09917, "alpha":0.48067, "fx":[-16.07223,-18.44517,-17.29141,-14.88873], "fy":[19.81447,18.31712,16.11731,17.62832]}, + {"t":2.09575, "x":3.96609, "y":5.25529, "heading":-1.04388, "vx":0.06491, "vy":-0.07009, "omega":-0.03463, "ax":-1.01922, "ay":1.09987, "alpha":0.52439, "fx":[-15.99294,-18.63215,-17.34873,-14.6755], "fy":[19.95586,18.34926,16.00193,17.61615]}, + {"t":2.12761, "x":3.96764, "y":5.25361, "heading":-1.04498, "vx":0.03244, "vy":-0.03506, "omega":-0.01793, "ax":-1.01852, "ay":1.10054, "alpha":0.56277, "fx":[-15.9365,-18.74432,-17.38185,-14.54102], "fy":[20.12816,18.40158,15.84484,17.59206]}, + {"t":2.15946, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index 36d866ab..063b9572 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,74 +30,71 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.19635,2.22924], + "waypoints":[0.0,1.12595,2.15883], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.12866, "ay":-3.39248, "alpha":-0.00255, "fx":[51.13646,51.1568,51.15897,51.13862], "fy":[-55.47103,-55.45227,-55.45024,-55.469]}, - {"t":0.03739, "x":1.55234, "y":7.52103, "heading":-0.93501, "vx":0.11697, "vy":-0.12683, "omega":-0.0001, "ax":3.12833, "ay":-3.39243, "alpha":-0.00261, "fx":[51.13076,51.15161,51.15383,51.13297], "fy":[-55.47038,-55.45115,-55.44906,-55.4683]}, - {"t":0.07477, "x":1.5589, "y":7.51392, "heading":-0.93501, "vx":0.23392, "vy":-0.25366, "omega":-0.00019, "ax":3.12796, "ay":-3.39237, "alpha":-0.00268, "fx":[51.12442,51.14584,51.14812,51.12669], "fy":[-55.46965,-55.44991,-55.44776,-55.46751]}, - {"t":0.11216, "x":1.56983, "y":7.50206, "heading":-0.93502, "vx":0.35087, "vy":-0.38049, "omega":-0.00029, "ax":3.12755, "ay":-3.3923, "alpha":-0.00276, "fx":[51.11734,51.1394,51.14174,51.11968], "fy":[-55.46884,-55.44852,-55.44631,-55.46664]}, - {"t":0.14954, "x":1.58513, "y":7.48547, "heading":-0.93503, "vx":0.46779, "vy":-0.50731, "omega":-0.0004, "ax":3.12709, "ay":-3.39222, "alpha":-0.00285, "fx":[51.10938,51.13214,51.13456,51.11178], "fy":[-55.46793,-55.44695,-55.44467,-55.46566]}, - {"t":0.18693, "x":1.60481, "y":7.46413, "heading":-0.93504, "vx":0.5847, "vy":-0.63413, "omega":-0.0005, "ax":3.12656, "ay":-3.39213, "alpha":-0.00295, "fx":[51.10035,51.12392,51.12642,51.10284], "fy":[-55.4669,-55.44518,-55.44281,-55.46454]}, - {"t":0.22432, "x":1.62885, "y":7.43805, "heading":-0.93506, "vx":0.70159, "vy":-0.76095, "omega":-0.00061, "ax":3.12596, "ay":-3.39203, "alpha":-0.00307, "fx":[51.09003,51.11453,51.11712,51.09261], "fy":[-55.46572,-55.44315,-55.44069,-55.46327]}, - {"t":0.2617, "x":1.65726, "y":7.40723, "heading":-0.93508, "vx":0.81846, "vy":-0.88776, "omega":-0.00073, "ax":3.12527, "ay":-3.39191, "alpha":-0.0032, "fx":[51.07812,51.10368,51.10638,51.08081], "fy":[-55.46435,-55.44081,-55.43824,-55.4618]}, - {"t":0.29909, "x":1.69005, "y":7.37167, "heading":-0.93511, "vx":0.9353, "vy":-1.01457, "omega":-0.00085, "ax":3.12446, "ay":-3.39178, "alpha":-0.00336, "fx":[51.06423,51.09104,51.09386,51.06705], "fy":[-55.46276,-55.43808,-55.43539,-55.46008]}, - {"t":0.33647, "x":1.7272, "y":7.33137, "heading":-0.93514, "vx":1.05211, "vy":-1.14138, "omega":-0.00097, "ax":3.12351, "ay":-3.39162, "alpha":-0.00354, "fx":[51.04782,51.07609,51.07906,51.05078], "fy":[-55.46088,-55.43486,-55.43201,-55.45805]}, - {"t":0.37386, "x":1.76871, "y":7.28633, "heading":-0.93518, "vx":1.16889, "vy":-1.26818, "omega":-0.00111, "ax":3.12236, "ay":-3.39142, "alpha":-0.00377, "fx":[51.02812,51.05815,51.0613,51.03125], "fy":[-55.45863,-55.43098,-55.42796,-55.45562]}, - {"t":0.41125, "x":1.8146, "y":7.23655, "heading":-0.93522, "vx":1.28562, "vy":-1.39497, "omega":-0.00125, "ax":3.12096, "ay":-3.39119, "alpha":-0.00404, "fx":[51.00404,51.03623,51.0396,51.00739], "fy":[-55.45587,-55.42625,-55.42301,-55.45264]}, - {"t":0.44863, "x":1.86484, "y":7.18202, "heading":-0.93527, "vx":1.4023, "vy":-1.52175, "omega":-0.0014, "ax":3.11921, "ay":-3.39089, "alpha":-0.00437, "fx":[50.97394,51.00884,51.01247,50.97756], "fy":[-55.45242,-55.42034,-55.41682,-55.44892]}, - {"t":0.48602, "x":1.91945, "y":7.12276, "heading":-0.93532, "vx":1.51891, "vy":-1.64852, "omega":-0.00156, "ax":3.11696, "ay":-3.39051, "alpha":-0.00481, "fx":[50.93525,50.97361,50.97759,50.9392], "fy":[-55.44798,-55.41274,-55.40887,-55.44413]}, - {"t":0.5234, "x":1.97841, "y":7.05876, "heading":-0.93538, "vx":1.63544, "vy":-1.77528, "omega":-0.00174, "ax":3.11396, "ay":-3.39001, "alpha":-0.00539, "fx":[50.88367,50.92666,50.93108,50.88806], "fy":[-55.44205,-55.4026,-55.39825,-55.43774]}, - {"t":0.56079, "x":2.04173, "y":6.99002, "heading":-0.93544, "vx":1.75186, "vy":-1.90202, "omega":-0.00194, "ax":3.10976, "ay":-3.3893, "alpha":-0.0062, "fx":[50.81148,50.86095,50.86598,50.81647], "fy":[-55.43374,-55.3884,-55.38339,-55.42877]}, - {"t":0.59817, "x":2.1094, "y":6.91654, "heading":-0.93552, "vx":1.86812, "vy":-2.02873, "omega":-0.00218, "ax":3.10347, "ay":-3.38823, "alpha":-0.00742, "fx":[50.70324,50.76244,50.76836,50.70911], "fy":[-55.42125,-55.3671,-55.36109,-55.4153]}, - {"t":0.63556, "x":2.18141, "y":6.83833, "heading":-0.9356, "vx":1.98415, "vy":-2.1554, "omega":-0.00245, "ax":3.09298, "ay":-3.38646, "alpha":-0.00946, "fx":[50.52299,50.5984,50.60572,50.53022], "fy":[-55.40034,-55.33158,-55.3239,-55.39276]}, - {"t":0.67295, "x":2.25775, "y":6.75538, "heading":-0.93569, "vx":2.09978, "vy":-2.28201, "omega":-0.00281, "ax":3.07203, "ay":-3.3829, "alpha":-0.01354, "fx":[50.16305,50.2709,50.28076,50.17273], "fy":[-55.35824,-55.26054,-55.24945,-55.34736]}, - {"t":0.71033, "x":2.3384, "y":6.6677, "heading":-0.93579, "vx":2.21463, "vy":-2.40848, "omega":-0.00331, "ax":3.00938, "ay":-3.37215, "alpha":-0.02578, "fx":[49.08777,49.29279,49.30798,49.1023], "fy":[-55.22982,-55.04768,-55.02615,-55.209]}, - {"t":0.74772, "x":2.4233, "y":6.5753, "heading":-0.93592, "vx":2.32714, "vy":-2.53455, "omega":-0.00428, "ax":-1.20515, "ay":-0.89187, "alpha":-0.89784, "fx":[-20.09712,-16.24615,-19.34566,-23.11871], "fy":[-18.05042,-15.29237,-11.05849,-13.92032]}, - {"t":0.7851, "x":2.50946, "y":6.47992, "heading":-0.93608, "vx":2.28209, "vy":-2.5679, "omega":-0.03784, "ax":-3.12386, "ay":3.26686, "alpha":-0.02366, "fx":[-51.16996,-50.98763,-50.96863,-51.15044], "fy":[53.30733,53.48077,53.50631,53.33351]}, - {"t":0.82249, "x":2.59259, "y":6.3862, "heading":-0.93749, "vx":2.1653, "vy":-2.44576, "omega":-0.03873, "ax":-3.12956, "ay":3.32986, "alpha":-0.01141, "fx":[-51.21202,-51.12254,-51.11286,-51.20221], "fy":[54.38951,54.47339,54.48428,54.40054]}, - {"t":0.85988, "x":2.67136, "y":6.29709, "heading":-0.93894, "vx":2.0483, "vy":-2.32127, "omega":-0.03915, "ax":-3.13144, "ay":3.35098, "alpha":-0.00729, "fx":[-51.22498,-51.16749,-51.16114,-51.21858], "fy":[54.752,54.80564,54.81234,54.75876]}, - {"t":0.89726, "x":2.74575, "y":6.21265, "heading":-0.9404, "vx":1.93122, "vy":-2.19599, "omega":-0.03943, "ax":-3.13237, "ay":3.36157, "alpha":-0.00521, "fx":[-51.23115,-51.18992,-51.18529,-51.22649], "fy":[54.93367,54.97204,54.97677,54.93843]}, - {"t":0.93465, "x":2.81576, "y":6.1329, "heading":-0.94188, "vx":1.81412, "vy":-2.07032, "omega":-0.03962, "ax":-3.13292, "ay":3.36793, "alpha":-0.00396, "fx":[-51.2347,-51.20336,-51.19978,-51.23111], "fy":[55.04285,55.07198,55.07555,55.04644]}, - {"t":0.97203, "x":2.88139, "y":6.05785, "heading":-0.94336, "vx":1.69699, "vy":-1.9444, "omega":-0.03977, "ax":-3.13328, "ay":3.37217, "alpha":-0.00312, "fx":[-51.23699,-51.2123,-51.20944,-51.23413], "fy":[55.11571,55.13864,55.14146,55.11854]}, - {"t":1.00942, "x":2.94265, "y":5.98752, "heading":-0.94485, "vx":1.57985, "vy":-1.81833, "omega":-0.03989, "ax":-3.13354, "ay":3.37521, "alpha":-0.00251, "fx":[-51.23858,-51.21868,-51.21634,-51.23624], "fy":[55.16781,55.18628,55.18856,55.1701]}, - {"t":1.04681, "x":2.99952, "y":5.9219, "heading":-0.94634, "vx":1.4627, "vy":-1.69215, "omega":-0.03998, "ax":-3.13374, "ay":3.37748, "alpha":-0.00205, "fx":[-51.23974,-51.22345,-51.22152,-51.2378], "fy":[55.20692,55.22202,55.22389,55.2088]}, - {"t":1.08419, "x":3.05201, "y":5.86099, "heading":-0.94783, "vx":1.34554, "vy":-1.56588, "omega":-0.04006, "ax":-3.13389, "ay":3.37925, "alpha":-0.00169, "fx":[-51.24061,-51.22716,-51.22554,-51.23899], "fy":[55.23736,55.24982,55.25139,55.23892]}, - {"t":1.12158, "x":3.10013, "y":5.80481, "heading":-0.94933, "vx":1.22838, "vy":-1.43954, "omega":-0.04012, "ax":-3.13401, "ay":3.38067, "alpha":-0.00141, "fx":[-51.24129,-51.23012,-51.22876,-51.23993], "fy":[55.26172,55.27208,55.27338,55.26303]}, - {"t":1.15896, "x":3.14386, "y":5.75336, "heading":-0.95083, "vx":1.11121, "vy":-1.31315, "omega":-0.04017, "ax":-3.1341, "ay":3.38183, "alpha":-0.00117, "fx":[-51.24184,-51.23254,-51.23139,-51.24069], "fy":[55.28167,55.29029,55.29138,55.28277]}, - {"t":1.19635, "x":3.18322, "y":5.70663, "heading":-0.95233, "vx":0.99404, "vy":-1.18672, "omega":-0.04022, "ax":-1.01055, "ay":1.10185, "alpha":-0.35139, "fx":[-16.87622,-15.19648,-16.16097,-17.84881], "fy":[16.63621,17.90516,19.38537,18.12558]}, - {"t":1.22765, "x":3.21383, "y":5.67002, "heading":-0.95359, "vx":0.96241, "vy":-1.15223, "omega":-0.05121, "ax":-1.00296, "ay":1.11244, "alpha":-0.32555, "fx":[-16.7391,-15.13652,-16.0639,-17.64674], "fy":[16.93242,18.10031,19.427,18.28529]}, - {"t":1.25895, "x":3.24347, "y":5.63451, "heading":-0.95519, "vx":0.93102, "vy":-1.11741, "omega":-0.0614, "ax":-0.99553, "ay":1.11939, "alpha":-0.29851, "fx":[-16.58362,-15.14258,-15.96819,-17.40591], "fy":[17.13643,18.20619,19.45951,18.39743]}, - {"t":1.29025, "x":3.27212, "y":5.60008, "heading":-0.95712, "vx":0.89986, "vy":-1.08237, "omega":-0.07075, "ax":-0.98943, "ay":1.12503, "alpha":-0.27374, "fx":[-16.46648,-15.11528,-15.89127,-17.22829], "fy":[17.34078,18.31483,19.43315,18.4794]}, - {"t":1.32155, "x":3.2998, "y":5.56675, "heading":-0.95933, "vx":0.86889, "vy":-1.04716, "omega":-0.07932, "ax":-0.98433, "ay":1.12969, "alpha":-0.24803, "fx":[-16.35148,-15.15094,-15.83288,-17.03267], "fy":[17.50393,18.3855,19.43006,18.55353]}, - {"t":1.35285, "x":3.32651, "y":5.53453, "heading":-0.96181, "vx":0.83808, "vy":-1.0118, "omega":-0.08708, "ax":-0.98001, "ay":1.13361, "alpha":-0.22422, "fx":[-16.26289,-15.15315,-15.78463,-16.88459], "fy":[17.67322,18.46434,19.38354,18.60825]}, - {"t":1.38415, "x":3.35226, "y":5.50342, "heading":-0.96454, "vx":0.80741, "vy":-0.97632, "omega":-0.0941, "ax":-0.9763, "ay":1.13695, "alpha":-0.19951, "fx":[-16.17203,-15.20445,-15.74853,-16.71752], "fy":[17.81268,18.51535,19.35978,18.66006]}, - {"t":1.41545, "x":3.37706, "y":5.47341, "heading":-0.96748, "vx":0.77685, "vy":-0.94073, "omega":-0.10034, "ax":-0.97308, "ay":1.13983, "alpha":-0.1764, "fx":[-16.10062,-15.22525,-15.71848,-16.58748], "fy":[17.95955,18.57639,19.3024,18.698]}, - {"t":1.44675, "x":3.4009, "y":5.44453, "heading":-0.97062, "vx":0.74639, "vy":-0.90506, "omega":-0.10586, "ax":-0.97025, "ay":1.14234, "alpha":-0.15241, "fx":[-16.02509,-15.28542,-15.69698,-16.43975], "fy":[18.08437,18.61561,19.26525,18.73535]}, - {"t":1.47805, "x":3.42378, "y":5.41676, "heading":-0.97394, "vx":0.71602, "vy":-0.8693, "omega":-0.11063, "ax":-0.96776, "ay":1.14455, "alpha":-0.12974, "fx":[-15.96445,-15.31905,-15.67955,-16.32113], "fy":[18.21602,18.66542,19.20151,18.762]}, - {"t":1.50935, "x":3.44572, "y":5.39011, "heading":-0.9774, "vx":0.68573, "vy":-0.83348, "omega":-0.11469, "ax":-0.96554, "ay":1.14651, "alpha":-0.10623, "fx":[-15.89906,-15.38461,-15.66837,-16.18707], "fy":[18.33155,18.69732,19.15477,18.78924]}, - {"t":1.54065, "x":3.46671, "y":5.36459, "heading":-0.98099, "vx":0.65551, "vy":-0.79759, "omega":-0.11802, "ax":-0.96356, "ay":1.14825, "alpha":-0.08378, "fx":[-15.84527,-15.42784,-15.66038,-16.07571], "fy":[18.45252,18.7398,19.08695,18.80772]}, - {"t":1.57195, "x":3.48676, "y":5.34018, "heading":-0.98468, "vx":0.62535, "vy":-0.76165, "omega":-0.12064, "ax":-0.96177, "ay":1.14982, "alpha":-0.06052, "fx":[-15.7867,-15.49701,-15.65696,-15.95153], "fy":[18.56224,18.76711,19.03268,18.8274]}, - {"t":1.60325, "x":3.50586, "y":5.31691, "heading":-0.98846, "vx":0.59525, "vy":-0.72566, "omega":-0.12254, "ax":-0.96015, "ay":1.15123, "alpha":-0.03808, "fx":[-15.73717,-15.548,-15.65653,-15.84456], "fy":[18.67563,18.80473,18.96191,18.83962]}, - {"t":1.63454, "x":3.52402, "y":5.29476, "heading":-0.99229, "vx":0.5652, "vy":-0.68963, "omega":-0.12373, "ax":-0.95867, "ay":1.15252, "alpha":-0.01485, "fx":[-15.68323,-15.61985,-15.65929,-15.72755], "fy":[18.78196,18.82924,18.90121,18.85333]}, - {"t":1.66584, "x":3.54124, "y":5.27374, "heading":-0.99617, "vx":0.53519, "vy":-0.65356, "omega":-0.12419, "ax":-0.95733, "ay":1.15368, "alpha":0.00776, "fx":[-15.63618,-15.6776,-15.66524,-15.62286], "fy":[18.89004,18.86364,18.82799,18.8605]}, - {"t":1.69714, "x":3.55752, "y":5.25385, "heading":-1.00005, "vx":0.50523, "vy":-0.61745, "omega":-0.12395, "ax":-0.95609, "ay":1.15475, "alpha":0.03115, "fx":[-15.58533,-15.75172,-15.67314,-15.51098], "fy":[18.99485,18.88657,18.76139,18.86928]}, - {"t":1.72844, "x":3.57287, "y":5.23509, "heading":-1.00393, "vx":0.4753, "vy":-0.5813, "omega":-0.12297, "ax":-0.95496, "ay":1.15574, "alpha":0.05414, "fx":[-15.53946,-15.81574,-15.68477,-15.4069], "fy":[19.09935,18.91887,18.68585,18.87225]}, - {"t":1.75974, "x":3.58728, "y":5.21746, "heading":-1.00778, "vx":0.44541, "vy":-0.54513, "omega":-0.12128, "ax":-0.95391, "ay":1.15664, "alpha":0.07789, "fx":[-15.4906,-15.89205,-15.69709,-15.29853], "fy":[19.20412,18.94107,18.61349,18.87681]}, - {"t":1.79104, "x":3.60075, "y":5.20096, "heading":-1.01158, "vx":0.41555, "vy":-0.50893, "omega":-0.11884, "ax":-0.95294, "ay":1.15748, "alpha":0.10143, "fx":[-15.44494,-15.96218,-15.71396,-15.19363], "fy":[19.30646,18.97204,18.53555,18.87617]}, - {"t":1.82234, "x":3.61329, "y":5.1856, "heading":-1.0153, "vx":0.38573, "vy":-0.4727, "omega":-0.11567, "ax":-0.95203, "ay":1.15825, "alpha":0.12573, "fx":[-15.39726,-16.04081,-15.73021,-15.08741], "fy":[19.41248,18.99416,18.45731,18.87701]}, - {"t":1.85364, "x":3.6249, "y":5.17137, "heading":-1.01892, "vx":0.35593, "vy":-0.43645, "omega":-0.11173, "ax":-0.95119, "ay":1.15898, "alpha":0.14998, "fx":[-15.35118,-16.11712,-15.75201,-14.98039], "fy":[19.51342,19.02446,18.3769,18.87338]}, - {"t":1.88494, "x":3.63557, "y":5.15828, "heading":-1.02242, "vx":0.32616, "vy":-0.40017, "omega":-0.10704, "ax":-0.95041, "ay":1.15965, "alpha":0.17504, "fx":[-15.30401,-16.19838,-15.77183,-14.87517], "fy":[19.62231,19.0468,18.29228,18.87075]}, - {"t":1.91624, "x":3.64531, "y":5.14632, "heading":-1.02577, "vx":0.29641, "vy":-0.36387, "omega":-0.10156, "ax":-0.94967, "ay":1.16028, "alpha":0.20024, "fx":[-15.25681,-16.28132,-15.79841,-14.76482], "fy":[19.72351,19.07621,18.20902,18.86452]}, - {"t":1.94754, "x":3.65413, "y":5.1355, "heading":-1.02895, "vx":0.26669, "vy":-0.32756, "omega":-0.09529, "ax":-0.94898, "ay":1.16087, "alpha":0.22623, "fx":[-15.20985,-16.36546,-15.82149,-14.65953], "fy":[19.83579,19.09966,18.11756,18.85875]}, - {"t":1.97884, "x":3.66201, "y":5.12582, "heading":-1.03193, "vx":0.23698, "vy":-0.29122, "omega":-0.08821, "ax":-0.94834, "ay":1.16142, "alpha":0.25256, "fx":[-15.16113,-16.45544,-15.85278,-14.54466], "fy":[19.9381,19.12879,18.03085,18.85014]}, - {"t":2.01014, "x":3.66896, "y":5.11727, "heading":-1.03469, "vx":0.2073, "vy":-0.25487, "omega":-0.08031, "ax":-0.94773, "ay":1.16194, "alpha":0.27969, "fx":[-15.11409,-16.54304,-15.8788,-14.43826], "fy":[20.05504,19.1531,17.93203,18.84169]}, - {"t":2.04144, "x":3.67499, "y":5.10986, "heading":-1.0372, "vx":0.17764, "vy":-0.2185, "omega":-0.07155, "ax":-0.94715, "ay":1.16243, "alpha":0.30737, "fx":[-15.06354,-16.64071,-15.91469,-14.31769], "fy":[20.1595,19.18215,17.8415,18.83071]}, - {"t":2.07274, "x":3.68008, "y":5.10359, "heading":-1.03944, "vx":0.14799, "vy":-0.18212, "omega":-0.06193, "ax":-0.94661, "ay":1.16289, "alpha":0.33584, "fx":[-15.01626,-16.73237,-15.94341,-14.20909], "fy":[20.28214,19.20729,17.73434,18.8203]}, - {"t":2.10404, "x":3.68425, "y":5.09846, "heading":-1.04138, "vx":0.11836, "vy":-0.14572, "omega":-0.05142, "ax":-0.9461, "ay":1.16333, "alpha":0.3651, "fx":[-14.96373,-16.8386,-15.98385,-14.0814], "fy":[20.38959,19.23604,17.63924,18.80775]}, - {"t":2.13534, "x":3.68749, "y":5.09447, "heading":-1.04299, "vx":0.08875, "vy":-0.10931, "omega":-0.03999, "ax":-0.94561, "ay":1.16374, "alpha":0.39516, "fx":[-14.91616,-16.93498,-16.01494,-13.96968], "fy":[20.51928,19.26209,17.5229,18.79537]}, - {"t":2.16664, "x":3.68981, "y":5.09162, "heading":-1.04424, "vx":0.05915, "vy":-0.07289, "omega":-0.02762, "ax":-0.94515, "ay":1.16413, "alpha":0.42626, "fx":[-14.86159,-17.05077,-16.05985,-13.83337], "fy":[20.63078,19.29046,17.42217,18.78186]}, - {"t":2.19794, "x":3.69119, "y":5.08991, "heading":-1.04511, "vx":0.02957, "vy":-0.03645, "omega":-0.01428, "ax":-0.94471, "ay":1.1645, "alpha":0.45627, "fx":[-14.8142,-17.1495,-16.09199,-13.7212], "fy":[20.75853,19.31598,17.30837,18.76671]}, - {"t":2.22924, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.60902, "ay":-3.91522, "alpha":-0.00314, "fx":[58.98668,59.01173,59.01439,58.98933], "fy":[-64.01915,-63.99606,-63.99356,-64.01665]}, + {"t":0.03753, "x":1.55269, "y":7.52064, "heading":-0.93501, "vx":0.13545, "vy":-0.14694, "omega":-0.00012, "ax":3.60849, "ay":-3.91523, "alpha":-0.00323, "fx":[58.97763,59.00343,59.00617,58.98036], "fy":[-64.0198,-63.99603,-63.99345,-64.01724]}, + {"t":0.07506, "x":1.56032, "y":7.51237, "heading":-0.93501, "vx":0.27088, "vy":-0.29389, "omega":-0.00024, "ax":3.60789, "ay":-3.91525, "alpha":-0.00334, "fx":[58.96746,58.99411,58.99693,58.97027], "fy":[-64.02055,-63.996,-63.99334,-64.01789]}, + {"t":0.11259, "x":1.57303, "y":7.49858, "heading":-0.93502, "vx":0.40629, "vy":-0.44084, "omega":-0.00036, "ax":3.60722, "ay":-3.91528, "alpha":-0.00346, "fx":[58.95594,58.98354,58.98646,58.95885], "fy":[-64.02139,-63.99596,-63.9932,-64.01864]}, + {"t":0.15013, "x":1.59082, "y":7.47928, "heading":-0.93503, "vx":0.54168, "vy":-0.58778, "omega":-0.00049, "ax":3.60645, "ay":-3.9153, "alpha":-0.0036, "fx":[58.94278,58.97148,58.97451,58.9458], "fy":[-64.02234,-63.99592,-63.99305,-64.01949]}, + {"t":0.18766, "x":1.61369, "y":7.45446, "heading":-0.93505, "vx":0.67704, "vy":-0.73473, "omega":-0.00063, "ax":3.60557, "ay":-3.91533, "alpha":-0.00375, "fx":[58.9276,58.95756,58.96072,58.93075], "fy":[-64.02345,-63.99587,-63.99288,-64.02047]}, + {"t":0.22519, "x":1.64164, "y":7.42413, "heading":-0.93508, "vx":0.81236, "vy":-0.88168, "omega":-0.00077, "ax":3.60453, "ay":-3.91536, "alpha":-0.00394, "fx":[58.90991,58.94134,58.94465,58.91321], "fy":[-64.02473,-63.99581,-63.99267,-64.02161]}, + {"t":0.26272, "x":1.67466, "y":7.38828, "heading":-0.9351, "vx":0.94764, "vy":-1.02863, "omega":-0.00092, "ax":3.60332, "ay":-3.9154, "alpha":-0.00416, "fx":[58.88903,58.92219,58.92567,58.89249], "fy":[-64.02625,-63.99574,-63.99243,-64.02295]}, + {"t":0.30025, "x":1.71277, "y":7.34691, "heading":-0.93514, "vx":1.08288, "vy":-1.17558, "omega":-0.00107, "ax":3.60185, "ay":-3.91545, "alpha":-0.00442, "fx":[58.86399,58.89924,58.90293,58.86766], "fy":[-64.02807,-63.99566,-63.99214,-64.02456]}, + {"t":0.33778, "x":1.75595, "y":7.30004, "heading":-0.93518, "vx":1.21806, "vy":-1.32253, "omega":-0.00124, "ax":3.60007, "ay":-3.9155, "alpha":-0.00473, "fx":[58.83343,58.87123,58.87517,58.83735], "fy":[-64.03028,-63.99555,-63.99177,-64.02653]}, + {"t":0.37532, "x":1.8042, "y":7.24764, "heading":-0.93523, "vx":1.35318, "vy":-1.46949, "omega":-0.00142, "ax":3.59784, "ay":-3.91557, "alpha":-0.00513, "fx":[58.7953,58.83627,58.84052,58.79953], "fy":[-64.03304,-63.99541,-63.99132,-64.02897]}, + {"t":0.41285, "x":1.85752, "y":7.18973, "heading":-0.93528, "vx":1.48821, "vy":-1.61644, "omega":-0.00161, "ax":3.59499, "ay":-3.91566, "alpha":-0.00564, "fx":[58.74639,58.79144,58.79608,58.751], "fy":[-64.03656,-63.99523,-63.99073,-64.03209]}, + {"t":0.45038, "x":1.91591, "y":7.12631, "heading":-0.93534, "vx":1.62314, "vy":-1.76341, "omega":-0.00182, "ax":3.59119, "ay":-3.91578, "alpha":-0.00632, "fx":[58.68136,58.73184,58.737,58.68649], "fy":[-64.04123,-63.99497,-63.98993,-64.03623]}, + {"t":0.48791, "x":1.97935, "y":7.05736, "heading":-0.93541, "vx":1.75792, "vy":-1.91037, "omega":-0.00206, "ax":3.5859, "ay":-3.91595, "alpha":-0.00727, "fx":[58.5907,58.64876,58.65463,58.59652], "fy":[-64.0477,-63.99458,-63.98879,-64.04196]}, + {"t":0.52544, "x":2.04786, "y":6.98291, "heading":-0.93548, "vx":1.89251, "vy":-2.05734, "omega":-0.00233, "ax":3.57801, "ay":-3.91618, "alpha":-0.00869, "fx":[58.45554,58.52492,58.5318,58.46236], "fy":[-64.05726,-63.99394,-63.98703,-64.05042]}, + {"t":0.56297, "x":2.12141, "y":6.90293, "heading":-0.93557, "vx":2.02679, "vy":-2.20432, "omega":-0.00266, "ax":3.56499, "ay":-3.91657, "alpha":-0.01103, "fx":[58.23248,58.3206,58.32908,58.24085], "fy":[-64.07282,-63.99273,-63.98397,-64.06418]}, + {"t":0.60051, "x":2.19999, "y":6.81744, "heading":-0.93567, "vx":2.16059, "vy":-2.35132, "omega":-0.00307, "ax":3.53942, "ay":-3.91728, "alpha":-0.01564, "fx":[57.79457,57.91966,57.93094,57.80564], "fy":[-64.10253,-63.98975,-63.9774,-64.09042]}, + {"t":0.63804, "x":2.28357, "y":6.72644, "heading":-0.93579, "vx":2.29343, "vy":-2.49834, "omega":-0.00366, "ax":3.46629, "ay":-3.91902, "alpha":-0.02895, "fx":[56.543,56.77486,56.79175,56.55914], "fy":[-64.18178,-63.97745,-63.95494,-64.16005]}, + {"t":0.67557, "x":2.37209, "y":6.62991, "heading":-0.93592, "vx":2.42353, "vy":-2.64543, "omega":-0.00475, "ax":1.76513, "ay":-3.68998, "alpha":-0.42347, "fx":[27.69958,30.89174,30.08457,26.75012], "fy":[-61.47158,-59.72798,-59.14226,-60.95471]}, + {"t":0.7131, "x":2.46429, "y":6.52802, "heading":-0.9361, "vx":2.48978, "vy":-2.78392, "omega":-0.02064, "ax":-3.63676, "ay":3.74027, "alpha":-0.031, "fx":[-59.58607,-59.34915,-59.3225,-59.55867], "fy":[61.01377,61.24289,61.27832,61.05015]}, + {"t":0.75063, "x":2.55517, "y":6.42617, "heading":-0.93688, "vx":2.35328, "vy":-2.64354, "omega":-0.0218, "ax":-3.62752, "ay":3.83057, "alpha":-0.01432, "fx":[-59.36536,-59.25343,-59.24092,-59.35268], "fy":[62.56256,62.66831,62.68228,62.57674]}, + {"t":0.78816, "x":2.64094, "y":6.32965, "heading":-0.9377, "vx":2.21714, "vy":-2.49977, "omega":-0.02234, "ax":-3.62444, "ay":3.85928, "alpha":-0.00902, "fx":[-59.29213,-59.22117,-59.21321,-59.28411], "fy":[63.05434,63.12089,63.12924,63.06278]}, + {"t":0.8257, "x":2.7216, "y":6.23855, "heading":-0.93853, "vx":2.08111, "vy":-2.35493, "omega":-0.02268, "ax":-3.62289, "ay":3.87339, "alpha":-0.0064, "fx":[-59.25541,-59.20491,-59.19922,-59.24969], "fy":[63.29602,63.34321,63.349,63.30185]}, + {"t":0.86323, "x":2.79716, "y":6.1529, "heading":-0.93938, "vx":1.94513, "vy":-2.20955, "omega":-0.02292, "ax":-3.62195, "ay":3.88178, "alpha":-0.00483, "fx":[-59.2333,-59.19509,-59.19076,-59.22895], "fy":[63.43974,63.47536,63.47969,63.44409]}, + {"t":0.90076, "x":2.86761, "y":6.0727, "heading":-0.94025, "vx":1.8092, "vy":-2.06386, "omega":-0.0231, "ax":-3.62133, "ay":3.88735, "alpha":-0.00379, "fx":[-59.21851,-59.18851,-59.18509,-59.21507], "fy":[63.53504,63.56296,63.56633,63.53843]}, + {"t":0.93829, "x":2.93296, "y":5.99798, "heading":-0.94111, "vx":1.67328, "vy":-1.91797, "omega":-0.02324, "ax":-3.62088, "ay":3.89131, "alpha":-0.00304, "fx":[-59.2079,-59.18379,-59.18102,-59.20512], "fy":[63.60288,63.62529,63.62799,63.60559]}, + {"t":0.97582, "x":2.99321, "y":5.92874, "heading":-0.94198, "vx":1.53738, "vy":-1.77192, "omega":-0.02336, "ax":-3.62054, "ay":3.89427, "alpha":-0.00248, "fx":[-59.19991,-59.18024,-59.17796,-59.19763], "fy":[63.65362,63.6719,63.67411,63.65584]}, + {"t":1.01335, "x":3.04836, "y":5.86498, "heading":-0.94286, "vx":1.4015, "vy":-1.62576, "omega":-0.02345, "ax":-3.62028, "ay":3.89657, "alpha":-0.00204, "fx":[-59.19368,-59.17746,-59.17557,-59.19179], "fy":[63.69302,63.70808,63.70991,63.69485]}, + {"t":1.05088, "x":3.09841, "y":5.8067, "heading":-0.94374, "vx":1.26562, "vy":-1.47952, "omega":-0.02353, "ax":-3.62006, "ay":3.8984, "alpha":-0.00169, "fx":[-59.18868,-59.17523,-59.17365,-59.1871], "fy":[63.72449,63.73698,63.7385,63.72601]}, + {"t":1.08842, "x":3.14336, "y":5.75392, "heading":-0.94462, "vx":1.12976, "vy":-1.3332, "omega":-0.02359, "ax":-3.61989, "ay":3.89991, "alpha":-0.00141, "fx":[-59.18458,-59.1734,-59.17208,-59.18326], "fy":[63.75022,63.76059,63.76186,63.75148]}, + {"t":1.12595, "x":3.18322, "y":5.70663, "heading":-0.94551, "vx":0.9939, "vy":-1.18683, "omega":-0.02364, "ax":-1.01074, "ay":1.1015, "alpha":-0.45447, "fx":[-16.93054,-14.84186,-16.10011,-18.2223], "fy":[16.20231,17.81442,19.8088,18.20427]}, + {"t":1.15823, "x":3.21477, "y":5.66889, "heading":-0.94627, "vx":0.96127, "vy":-1.15128, "omega":-0.03831, "ax":-1.00248, "ay":1.11283, "alpha":-0.42176, "fx":[-16.78285,-14.73242,-16.00498,-18.0345], "fy":[16.59019,18.03919,19.77574,18.36566]}, + {"t":1.1905, "x":3.24528, "y":5.63231, "heading":-0.94751, "vx":0.92892, "vy":-1.11536, "omega":-0.05193, "ax":-0.99464, "ay":1.12017, "alpha":-0.38534, "fx":[-16.61457,-14.82445,-15.90189,-17.70117], "fy":[16.7885,18.14629,19.83346,18.48233]}, + {"t":1.22278, "x":3.27474, "y":5.5969, "heading":-0.94919, "vx":0.89681, "vy":-1.0792, "omega":-0.06436, "ax":-0.98831, "ay":1.12602, "alpha":-0.35524, "fx":[-16.49318,-14.76331,-15.82844,-17.54323], "fy":[17.06003,18.27302,19.74095,18.55896]}, + {"t":1.25506, "x":3.30317, "y":5.56265, "heading":-0.95126, "vx":0.86491, "vy":-1.04286, "omega":-0.07583, "ax":-0.9831, "ay":1.13079, "alpha":-0.32008, "fx":[-16.36959,-14.88097,-15.76918,-17.2673], "fy":[17.22079,18.34106,19.74951,18.63347]}, + {"t":1.28734, "x":3.33058, "y":5.52958, "heading":-0.95371, "vx":0.83318, "vy":-1.00636, "omega":-0.08616, "ax":-0.97872, "ay":1.13475, "alpha":-0.29239, "fx":[-16.2814,-14.85464,-15.7244,-17.14062], "fy":[17.44223,18.43399,19.64731,18.68046]}, + {"t":1.31961, "x":3.35696, "y":5.49768, "heading":-0.95649, "vx":0.80159, "vy":-0.96973, "omega":-0.0956, "ax":-0.975, "ay":1.1381, "alpha":-0.25831, "fx":[-16.18301,-14.98149,-15.69094,-16.90245], "fy":[17.58431,18.48126,19.62594,18.73126]}, + {"t":1.35189, "x":3.38233, "y":5.46698, "heading":-0.95958, "vx":0.77012, "vy":-0.933, "omega":-0.10394, "ax":-0.9718, "ay":1.14096, "alpha":-0.23222, "fx":[-16.11449,-14.97856,-15.66334,-16.7922], "fy":[17.77252,18.55469,19.52223,18.76046]}, + {"t":1.38417, "x":3.40668, "y":5.43746, "heading":-0.96293, "vx":0.73875, "vy":-0.89617, "omega":-0.11143, "ax":-0.96902, "ay":1.14343, "alpha":-0.1991, "fx":[-16.03148,-15.10671,-15.64659,-16.58175], "fy":[17.90491,18.59005,19.48059,18.79626]}, + {"t":1.41645, "x":3.43002, "y":5.40913, "heading":-0.96653, "vx":0.70747, "vy":-0.85926, "omega":-0.11786, "ax":-0.96658, "ay":1.1456, "alpha":-0.17385, "fx":[-15.97493,-15.12218,-15.63068,-16.47896], "fy":[18.07005,18.65133,19.37814,18.81371]}, + {"t":1.44872, "x":3.45235, "y":5.38199, "heading":-0.97033, "vx":0.67627, "vy":-0.82229, "omega":-0.12347, "ax":-0.96441, "ay":1.1475, "alpha":-0.14162, "fx":[-15.90195,-15.24751,-15.62549,-16.29043], "fy":[18.19744,18.67985,19.32159,18.83896]}, + {"t":1.481, "x":3.47368, "y":5.35604, "heading":-0.97432, "vx":0.64514, "vy":-0.78525, "omega":-0.12804, "ax":-0.96249, "ay":1.14919, "alpha":-0.11655, "fx":[-15.85246,-15.27922,-15.61859,-16.18916], "fy":[18.34615,18.73324,19.22099,18.84808]}, + {"t":1.51328, "x":3.494, "y":5.3313, "heading":-0.97845, "vx":0.61408, "vy":-0.74815, "omega":-0.1318, "ax":-0.96076, "ay":1.1507, "alpha":-0.08508, "fx":[-15.78636,-15.39971,-15.62176,-16.01866], "fy":[18.47149,18.75804,19.15258,18.86521]}, + {"t":1.54556, "x":3.51332, "y":5.30775, "heading":-0.98271, "vx":0.58307, "vy":-0.71101, "omega":-0.13455, "ax":-0.9592, "ay":1.15206, "alpha":-0.05966, "fx":[-15.74056,-15.44673,-15.62259,-15.91477], "fy":[18.60836,18.8061,19.05363,18.86811]}, + {"t":1.57783, "x":3.53164, "y":5.2854, "heading":-0.98705, "vx":0.55211, "vy":-0.67383, "omega":-0.13647, "ax":-0.95779, "ay":1.15329, "alpha":-0.02875, "fx":[-15.67933,-15.5617,-15.63204,-15.7593], "fy":[18.73387,18.82928,18.97491,18.87848]}, + {"t":1.61011, "x":3.54896, "y":5.26425, "heading":-0.99145, "vx":0.52119, "vy":-0.6366, "omega":-0.1374, "ax":-0.95651, "ay":1.15441, "alpha":-0.00256, "fx":[-15.63481,-15.62362,-15.64004,-15.64987], "fy":[18.8621,18.87366,18.8772,18.87655]}, + {"t":1.64239, "x":3.56528, "y":5.2443, "heading":-0.99589, "vx":0.49032, "vy":-0.59934, "omega":-0.13748, "ax":-0.95533, "ay":1.15543, "alpha":0.02806, "fx":[-15.5771,-15.73321,-15.65438,-15.50682], "fy":[18.98978,18.89672,18.78866,18.88091]}, + {"t":1.67467, "x":3.58061, "y":5.22556, "heading":-1.00033, "vx":0.45948, "vy":-0.56204, "omega":-0.13658, "ax":-0.95425, "ay":1.15636, "alpha":0.05534, "fx":[-15.53209,-15.80981,-15.66936,-15.38975], "fy":[19.11164,18.93848,18.69178,18.87516]}, + {"t":1.70694, "x":3.59495, "y":5.20802, "heading":-1.00473, "vx":0.42868, "vy":-0.52472, "omega":-0.13479, "ax":-0.95326, "ay":1.15722, "alpha":0.08604, "fx":[-15.47692,-15.91476,-15.68769,-15.25668], "fy":[19.24349,18.96257,18.59316,18.8739]}, + {"t":1.73922, "x":3.60829, "y":5.19169, "heading":-1.00908, "vx":0.39791, "vy":-0.48737, "omega":-0.13202, "ax":-0.95234, "ay":1.15801, "alpha":0.11464, "fx":[-15.43004,-16.00599,-15.70961,-15.1304], "fy":[19.36064,19.00235,18.49675,18.8651]}, + {"t":1.7715, "x":3.62063, "y":5.17656, "heading":-1.01335, "vx":0.36717, "vy":-0.44999, "omega":-0.12831, "ax":-0.95149, "ay":1.15874, "alpha":0.14583, "fx":[-15.37665,-16.10745,-15.73134,-15.00498], "fy":[19.4987,19.02839,18.38722,18.85839]}, + {"t":1.80378, "x":3.63199, "y":5.16264, "heading":-1.01749, "vx":0.33646, "vy":-0.41259, "omega":-0.12361, "ax":-0.9507, "ay":1.15942, "alpha":0.17597, "fx":[-15.3269,-16.21333,-15.76026,-14.86822], "fy":[19.61246,19.06661,18.29086,18.84718]}, + {"t":1.83605, "x":3.64236, "y":5.14992, "heading":-1.02148, "vx":0.30577, "vy":-0.37517, "omega":-0.11793, "ax":-0.94997, "ay":1.16005, "alpha":0.20809, "fx":[-15.2747,-16.31263,-15.785,-14.74822], "fy":[19.7588,19.09532,18.16922,18.83509]}, + {"t":1.86833, "x":3.65173, "y":5.13842, "heading":-1.02528, "vx":0.27511, "vy":-0.33772, "omega":-0.11121, "ax":-0.94928, "ay":1.16064, "alpha":0.23956, "fx":[-15.22104,-16.43606,-15.8198,-14.59866], "fy":[19.87138,19.11869,18.07877,18.82815]}, + {"t":1.90061, "x":3.66012, "y":5.12812, "heading":-1.02887, "vx":0.24447, "vy":-0.30026, "omega":-0.10348, "ax":-0.94863, "ay":1.16119, "alpha":0.27347, "fx":[-15.16984,-16.53199,-15.84849,-14.48311], "fy":[20.02706,19.16416,17.93724,18.80458]}, + {"t":1.93289, "x":3.66751, "y":5.11904, "heading":-1.03221, "vx":0.21385, "vy":-0.26278, "omega":-0.09465, "ax":-0.94803, "ay":1.16171, "alpha":0.30694, "fx":[-15.11195,-16.67085,-15.89055,-14.32055], "fy":[20.13844,19.18724,17.84526,18.79589]}, + {"t":1.96517, "x":3.67392, "y":5.11116, "heading":-1.03527, "vx":0.18325, "vy":-0.22528, "omega":-0.08474, "ax":-0.94746, "ay":1.16219, "alpha":0.34266, "fx":[-15.06118,-16.76756,-15.92168,-14.20635], "fy":[20.30675,19.23538,17.68894,18.76748]}, + {"t":1.99744, "x":3.67934, "y":5.10449, "heading":-1.038, "vx":0.15267, "vy":-0.18777, "omega":-0.07368, "ax":-0.94693, "ay":1.16265, "alpha":0.37844, "fx":[-14.99863,-16.92295,-15.97106,-14.02914], "fy":[20.41827,19.25788,17.59461,18.75763]}, + {"t":2.02972, "x":3.68378, "y":5.09904, "heading":-1.04038, "vx":0.1221, "vy":-0.15024, "omega":-0.06147, "ax":-0.94642, "ay":1.16308, "alpha":0.41643, "fx":[-14.94813,-17.02178,-16.00442,-13.91445], "fy":[20.60134,19.30911,17.4216,18.72447]}, + {"t":2.062, "x":3.68723, "y":5.09479, "heading":-1.04237, "vx":0.09156, "vy":-0.1127, "omega":-0.04803, "ax":-0.94594, "ay":1.16349, "alpha":0.45488, "fx":[-14.88067,-17.19519,-16.06115,-13.72059], "fy":[20.7145,19.33086,17.32371,18.714]}, + {"t":2.09428, "x":3.68969, "y":5.09176, "heading":-1.04392, "vx":0.06102, "vy":-0.07515, "omega":-0.03335, "ax":-0.94549, "ay":1.16387, "alpha":0.49559, "fx":[-14.83038,-17.29762,-16.09649,-13.60358], "fy":[20.91454,19.38516,17.13199,18.67651]}, + {"t":2.12655, "x":3.69116, "y":5.08994, "heading":-1.04499, "vx":0.0305, "vy":-0.03758, "omega":-0.01735, "ax":-0.94507, "ay":1.16423, "alpha":0.53749, "fx":[-14.76976,-17.44474,-16.14592,-13.43969], "fy":[21.07939,19.42386,16.97768,18.65104]}, + {"t":2.15883, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index fc2f4d8f..28c1259a 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -526,8 +526,8 @@ "val":523.5987755982989 }, "tmax":{ - "exp":"0.65 N * m", - "val":0.65 + "exp":"0.75 N * m", + "val":0.75 }, "cof":{ "exp":"1.5", diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 82181798..be439ac8 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -465,14 +465,14 @@ public Command intakeCoralInAuto(Supplier> pose) { ? Commands.runOnce(() -> manipulator.setSecondBeambreak(true)) : Commands.none(), Commands.print("intake - 2nd bb" + manipulator.getSecondBeambreak()), - // AutoAim.translateToPose( - // swerve, - // () -> pose.get().get(), - // () -> - // ChassisSpeeds.fromRobotRelativeSpeeds( - // new ChassisSpeeds(-0.5, 0.0, 0.0), swerve.getRotation())) - swerve - .driveVoltage(() -> new ChassisSpeeds(-0.0, 0.0, 0.0)) + AutoAim.translateToPose( + swerve, + () -> pose.get().get(), + () -> + ChassisSpeeds.fromRobotRelativeSpeeds( + new ChassisSpeeds(-0.5, 0.0, 0.0), swerve.getRotation())) + // swerve + // .driveVoltage(() -> new ChassisSpeeds(-0.0, 0.0, 0.0)) .until( () -> manipulator.getSecondBeambreak() From d43e0f5feaed6b5f3a1d1df85c2fabd4224e68fa Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 9 Apr 2025 13:53:39 -0700 Subject: [PATCH 011/154] turn off current limits in auto --- src/main/deploy/choreo/JtoPLO.traj | 166 +++++++++++++-------------- src/main/deploy/choreo/KtoPLO.traj | 77 ++++++------- src/main/deploy/choreo/LOtoJ.traj | 130 +++++++++++---------- src/main/deploy/choreo/LtoPLM.traj | 76 ++++++------- src/main/deploy/choreo/PLMtoA.traj | 174 ++++++++++++++--------------- src/main/deploy/choreo/PLOtoK.traj | 133 +++++++++++----------- src/main/deploy/choreo/PLOtoL.traj | 134 +++++++++++----------- src/main/deploy/choreo/choreo.chor | 4 +- src/main/java/frc/robot/Robot.java | 7 +- 9 files changed, 443 insertions(+), 458 deletions(-) diff --git a/src/main/deploy/choreo/JtoPLO.traj b/src/main/deploy/choreo/JtoPLO.traj index 2ddd5bd3..11b842e4 100644 --- a/src/main/deploy/choreo/JtoPLO.traj +++ b/src/main/deploy/choreo/JtoPLO.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.807075500488281, "y":5.671792030334473, "heading":-1.7084131860016765, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.906598448753357, "y":7.005744457244873, "heading":-1.3217094862895618, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.807075500488281, "y":5.671792030334473, "heading":-1.7084131860016765, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.906598448753357, "y":7.005744457244873, "heading":-1.3217094862895618, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,9 +15,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.807075500488281 m", "val":4.807075500488281}, "y":{"exp":"5.671792030334473 m", "val":5.671792030334473}, "heading":{"exp":"-1.7084131860016765 rad", "val":-1.7084131860016765}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"1.906598448753357 m", "val":1.906598448753357}, "y":{"exp":"7.005744457244873 m", "val":7.005744457244873}, "heading":{"exp":"-1.3217094862895618 rad", "val":-1.3217094862895618}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.807075500488281 m", "val":4.807075500488281}, "y":{"exp":"5.671792030334473 m", "val":5.671792030334473}, "heading":{"exp":"-1.7084131860016765 rad", "val":-1.7084131860016765}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.906598448753357 m", "val":1.906598448753357}, "y":{"exp":"7.005744457244873 m", "val":7.005744457244873}, "heading":{"exp":"-1.3217094862895618 rad", "val":-1.3217094862895618}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,87 +30,81 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.43931,1.44395,1.94358], + "waypoints":[0.0,0.40052,1.32283,1.7786], "samples":[ - {"t":0.0, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.16523, "ay":5.29814, "alpha":1.39064, "fx":[7.45874,-5.01736,-13.43147,0.18531], "fy":[86.67692,86.86406,85.94588,86.97084]}, - {"t":0.02312, "x":5.00418, "y":5.24533, "heading":-2.09512, "vx":-0.00382, "vy":0.1225, "omega":0.03215, "ax":-0.38503, "ay":5.28492, "alpha":1.4295, "fx":[4.29601,-8.28362,-17.35529,-3.83495], "fy":[86.88142,86.60582,85.23127,86.87468]}, - {"t":0.04624, "x":5.00399, "y":5.24958, "heading":-2.09437, "vx":-0.01272, "vy":0.2447, "omega":0.06521, "ax":-0.63178, "ay":5.25917, "alpha":1.47017, "fx":[0.69455,-11.92542,-21.68473,-8.39769], "fy":[86.97407,86.17136,84.22423,86.53988]}, - {"t":0.06936, "x":5.00352, "y":5.25664, "heading":-2.09287, "vx":-0.02733, "vy":0.3663, "omega":0.0992, "ax":-0.9088, "ay":5.21631, "alpha":1.51228, "fx":[-3.41903,-15.98637,-26.44684,-13.57657], "fy":[86.89774,85.50186,82.84195,85.86559]}, - {"t":0.09249, "x":5.00265, "y":5.26651, "heading":-2.09057, "vx":-0.04834, "vy":0.48691, "omega":0.13417, "ax":-1.21923, "ay":5.15032, "alpha":1.55519, "fx":[-8.12616,-20.50846,-31.65513,-19.43893], "fy":[86.57134,84.52118,80.9838,84.71548]}, - {"t":0.11561, "x":5.0012, "y":5.27914, "heading":-2.08747, "vx":-0.07653, "vy":0.60599, "omega":0.17012, "ax":-1.56543, "ay":5.05345, "alpha":1.59796, "fx":[-13.51154,-25.52639,-37.29972,-26.02959], "fy":[85.8812,83.13252,78.53222,82.91113]}, - {"t":0.13873, "x":4.99902, "y":5.2945, "heading":-2.08354, "vx":-0.11273, "vy":0.72284, "omega":0.20707, "ax":-1.94809, "ay":4.91613, "alpha":1.63914, "fx":[-19.65231,-31.05855,-43.33462,-33.3446], "fy":[84.67136,81.21625,75.35817,80.23137]}, - {"t":0.16185, "x":4.99589, "y":5.31253, "heading":-2.07875, "vx":-0.15777, "vy":0.8365, "omega":0.24497, "ax":-2.36495, "ay":4.72725, "alpha":1.67653, "fx":[-26.59775,-37.09375,-49.66396,-41.29432], "fy":[82.73555,78.63092,71.3336,76.4259]}, - {"t":0.18497, "x":4.99161, "y":5.33314, "heading":-2.07308, "vx":-0.21245, "vy":0.94581, "omega":0.28373, "ax":-2.80925, "ay":4.47522, "alpha":1.70672, "fx":[-34.33549,-43.57397,-56.13108,-49.66303], "fy":[79.8172,75.22068,66.353,71.25439]}, - {"t":0.20809, "x":4.98595, "y":5.3562, "heading":-2.06652, "vx":-0.27741, "vy":1.04928, "omega":0.3232, "ax":-3.26831, "ay":4.15008, "alpha":1.72454, "fx":[-42.7447,-50.37564,-62.51731,-58.08519], "fy":[75.62941,70.83307,60.36322,64.55764]}, - {"t":0.23122, "x":4.97866, "y":5.38157, "heading":-2.05905, "vx":-0.35298, "vy":1.14524, "omega":0.36307, "ax":-3.72323, "ay":3.74665, "alpha":1.72315, "fx":[-51.54738,-57.29618,-68.55797,-66.06962], "fy":[69.9094,65.34975,53.39519,56.34819]}, - {"t":0.25434, "x":4.9695, "y":5.40905, "heading":-2.05066, "vx":-0.43906, "vy":1.23187, "omega":0.40291, "ax":-4.15058, "ay":3.26802, "alpha":1.69555, "fx":[-60.28818,-64.05639,-73.97854,-73.09297], "fy":[62.51356,58.72813,45.58517,46.87655]}, - {"t":0.27746, "x":4.95824, "y":5.43841, "heading":-2.04134, "vx":-0.53503, "vy":1.30743, "omega":0.44212, "ax":-4.52648, "ay":2.7275, "alpha":1.6377, "fx":[-68.38471,-70.32913,-78.54434,-78.7392], "fy":[53.5273,51.04196,37.17169,36.61704]}, - {"t":0.30058, "x":4.94466, "y":5.46937, "heading":-2.03112, "vx":-0.63969, "vy":1.37049, "omega":0.47998, "ax":-4.83206, "ay":2.14771, "alpha":1.5513, "fx":[-75.26486,-75.79561,-82.1064,-82.81329], "fy":[43.32332,42.5007,28.46252,26.15712]}, - {"t":0.3237, "x":4.92858, "y":5.50163, "heading":-2.02002, "vx":-0.75142, "vy":1.42015, "omega":0.51585, "ax":-5.05778, "ay":1.55612, "alpha":1.44356, "fx":[-80.5381,-80.21309,-84.62485,-85.36409], "fy":[32.50252,33.42972,19.78103,16.04515]}, - {"t":0.34682, "x":4.90985, "y":5.53488, "heading":-2.00809, "vx":-0.86836, "vy":1.45613, "omega":0.54923, "ax":-5.20463, "ay":0.97917, "alpha":1.32398, "fx":[-84.09881,-83.46522,-86.16172,-86.61715], "fy":[21.72853,24.21123,11.41235,6.67838]}, - {"t":0.36995, "x":4.88838, "y":5.56881, "heading":-1.99539, "vx":-0.9887, "vy":1.47877, "omega":0.57984, "ax":-5.28196, "ay":0.43736, "alpha":1.20101, "fx":[-86.10345,-85.57263,-86.85058,-86.87335], "fy":[11.55202,15.2094,3.56797,-1.72935]}, - {"t":0.39307, "x":4.86411, "y":5.60312, "heading":-1.98199, "vx":-1.11083, "vy":1.48888, "omega":0.60761, "ax":-5.30353, "ay":-0.0568, "alpha":1.08055, "fx":[-86.86022,-86.664,-86.85863,-86.42731], "fy":[2.31739,6.71079,-3.62467,-9.11799]}, - {"t":0.41619, "x":4.83701, "y":5.63753, "heading":-1.96794, "vx":-1.23345, "vy":1.48757, "omega":0.6326, "ax":-5.28377, "ay":-0.49818, "alpha":0.96612, "fx":[-86.71175,-86.92717,-86.35475,-85.52444], "fy":[-5.83385,-1.10174,-10.11293,-15.52836]}, - {"t":0.43931, "x":4.80708, "y":5.67179, "heading":-1.95331, "vx":-1.35562, "vy":1.47605, "omega":0.65493, "ax":-5.26396, "ay":-0.69983, "alpha":0.90056, "fx":[-86.42101,-86.82689,-85.97304,-85.00198], "fy":[-9.48599,-4.83007,-13.13303,-18.3142]}, - {"t":0.46575, "x":4.7694, "y":5.71057, "heading":-1.936, "vx":-1.49479, "vy":1.45755, "omega":0.67874, "ax":-5.26347, "ay":-0.70113, "alpha":0.88439, "fx":[-86.41724,-86.80698,-85.94792,-85.01841], "fy":[-9.42218,-5.00749,-13.23225,-18.18689]}, - {"t":0.49219, "x":4.72804, "y":5.74886, "heading":-1.91805, "vx":-1.63394, "vy":1.43901, "omega":0.70212, "ax":-5.26291, "ay":-0.70258, "alpha":0.86645, "fx":[-86.41136,-86.7846,-85.92179,-85.03638], "fy":[-9.36653,-5.20146,-13.32977,-18.04566]}, - {"t":0.51862, "x":4.683, "y":5.78666, "heading":-1.89949, "vx":-1.77308, "vy":1.42044, "omega":0.72503, "ax":-5.26228, "ay":-0.70419, "alpha":0.8464, "fx":[-86.40292,-86.75926,-85.89465,-85.05601], "fy":[-9.32151,-5.41424,-13.42425,-17.88858]}, - {"t":0.54506, "x":4.63429, "y":5.82397, "heading":-1.88032, "vx":-1.91221, "vy":1.40182, "omega":0.74741, "ax":-5.26156, "ay":-0.70599, "alpha":0.82386, "fx":[-86.39136,-86.7304,-85.86649,-85.07742], "fy":[-9.29015,-5.64863,-13.51408,-17.71334]}, - {"t":0.5715, "x":4.58189, "y":5.86078, "heading":-1.86056, "vx":-2.05131, "vy":1.38316, "omega":0.76919, "ax":-5.26072, "ay":-0.70801, "alpha":0.79833, "fx":[-86.37597,-86.69725,-85.83731,-85.10076], "fy":[-9.27614,-5.90815,-13.59722,-17.51711]}, - {"t":0.59794, "x":4.52582, "y":5.8971, "heading":-1.84023, "vx":-2.19039, "vy":1.36444, "omega":0.79029, "ax":-5.25975, "ay":-0.71031, "alpha":0.76914, "fx":[-86.35583,-86.65883,-85.80709,-85.12615], "fy":[-9.28415,-6.19727,-13.67109,-17.29634]}, - {"t":0.62438, "x":4.46607, "y":5.93292, "heading":-1.81933, "vx":-2.32945, "vy":1.34566, "omega":0.81063, "ax":-5.25861, "ay":-0.71294, "alpha":0.73547, "fx":[-86.32976,-86.61383,-85.7758,-85.15375], "fy":[-9.32011,-6.52183,-13.73233,-17.0465]}, - {"t":0.65081, "x":4.40265, "y":5.96825, "heading":-1.7979, "vx":-2.46847, "vy":1.32681, "omega":0.83007, "ax":-5.25724, "ay":-0.71598, "alpha":0.69617, "fx":[-86.29616,-86.56044,-85.74339,-85.18372], "fy":[-9.39173,-6.8896,-13.77646,-16.76168]}, - {"t":0.67725, "x":4.33555, "y":6.00308, "heading":-1.77596, "vx":-2.60746, "vy":1.30788, "omega":0.84848, "ax":-5.25558, "ay":-0.71953, "alpha":0.6497, "fx":[-86.25281,-86.49612,-85.70976,-85.2162], "fy":[-9.50928,-7.31123,-13.79739,-16.43394]}, - {"t":0.70369, "x":4.26478, "y":6.03741, "heading":-1.75352, "vx":-2.74641, "vy":1.28886, "omega":0.86565, "ax":-5.25351, "ay":-0.72374, "alpha":0.59389, "fx":[-86.19659,-86.41717,-85.67472,-85.25134], "fy":[-9.68675,-7.80174,-13.78656,-16.05236]}, - {"t":0.73013, "x":4.19034, "y":6.07123, "heading":-1.73064, "vx":-2.8853, "vy":1.26972, "omega":0.88136, "ax":-5.25089, "ay":-0.72883, "alpha":0.52558, "fx":[-86.12285,-86.318,-85.63793,-85.2892], "fy":[-9.94387,-8.38304,-13.73147,-15.60136]}, - {"t":0.75656, "x":4.11222, "y":6.10454, "heading":-1.70734, "vx":-3.02412, "vy":1.25046, "omega":0.89525, "ax":-5.24744, "ay":-0.73508, "alpha":0.44005, "fx":[-86.02446,-86.18971,-85.59872,-85.3297], "fy":[-10.30943,-9.0885,-13.61307,-15.05782]}, - {"t":0.783, "x":4.03044, "y":6.13734, "heading":-1.68367, "vx":-3.16285, "vy":1.23102, "omega":0.90689, "ax":-5.24274, "ay":-0.74298, "alpha":0.32976, "fx":[-85.88987,-86.01729,-85.55578,-85.37235], "fy":[-10.82732,-9.97159,-13.4007,-14.38586]}, - {"t":0.80944, "x":3.94498, "y":6.16963, "heading":-1.65969, "vx":-3.30146, "vy":1.21138, "omega":0.9156, "ax":-5.236, "ay":-0.7533, "alpha":0.18208, "fx":[-85.69908,-85.7734,-85.50627,-85.41559], "fy":[-11.56821,-11.1237,-13.04144,-13.52644]}, - {"t":0.83588, "x":3.85587, "y":6.20139, "heading":-1.63549, "vx":-3.43989, "vy":1.19146, "omega":0.92042, "ax":-5.22562, "ay":-0.76735, "alpha":-0.02615, "fx":[-85.41459,-85.40284,-85.44345,-85.45488], "fy":[-12.65391,-12.71489,-12.43519,-12.37507]}, - {"t":0.86231, "x":3.7631, "y":6.23262, "heading":-1.61115, "vx":-3.57804, "vy":1.17118, "omega":0.91973, "ax":-5.20794, "ay":-0.7877, "alpha":-0.34249, "fx":[-84.95764,-84.77676,-85.34866,-85.4766], "fy":[-14.31405,-15.10094,-11.36667,-10.72797]}, - {"t":0.88875, "x":3.66669, "y":6.26331, "heading":-1.58684, "vx":-3.71573, "vy":1.15035, "omega":0.91067, "ax":-5.17253, "ay":-0.81976, "alpha":-0.88337, "fx":[-84.13459,-83.52007,-85.15571,-85.43389], "fy":[-17.03784,-19.16549,-9.27336,-8.12929]}, - {"t":0.91519, "x":3.56664, "y":6.29344, "heading":-1.56276, "vx":-3.85248, "vy":1.12868, "omega":0.88732, "ax":-5.07694, "ay":-0.87592, "alpha":-2.03507, "fx":[-82.33995,-80.03839,-84.49641,-85.11846], "fy":[-22.0632,-27.7924,-4.09812,-3.3247]}, - {"t":0.94163, "x":3.46302, "y":6.32297, "heading":-1.5393, "vx":-3.9867, "vy":1.10552, "omega":0.83351, "ax":-4.53984, "ay":-0.91054, "alpha":-6.24542, "fx":[-76.83341,-59.96197,-77.06635,-83.00894], "fy":[-33.09589,-54.55347,19.68172,8.425]}, - {"t":0.96807, "x":3.35603, "y":6.35188, "heading":-1.51727, "vx":-4.10672, "vy":1.08145, "omega":0.6684, "ax":-0.46443, "ay":2.63676, "alpha":-3.22757, "fx":[-19.92837,4.48023,2.58014,-17.50187], "fy":[35.23941,37.73549,50.74799,48.70139]}, - {"t":0.9945, "x":3.2473, "y":6.38139, "heading":-1.49959, "vx":-4.119, "vy":1.15116, "omega":0.58307, "ax":4.17725, "ay":1.74527, "alpha":6.86193, "fx":[70.24556,42.04698,77.59264,83.27536], "fy":[45.63929,69.836,-3.49805,2.15002]}, - {"t":1.02094, "x":3.13986, "y":6.41244, "heading":-1.48418, "vx":-4.00856, "vy":1.1973, "omega":0.76448, "ax":4.91902, "ay":1.37684, "alpha":2.78348, "fx":[78.98037,74.05017,83.87533,84.76066], "fy":[32.11366,40.83872,8.71514,8.36756]}, - {"t":1.04738, "x":3.0356, "y":6.44457, "heading":-1.46397, "vx":-3.87852, "vy":1.2337, "omega":0.83807, "ax":5.09652, "ay":1.14968, "alpha":1.30255, "fx":[82.30458,81.24616,84.70761,85.01556], "fy":[24.37327,26.97418,12.13474,11.69824]}, - {"t":1.07382, "x":2.93484, "y":6.47759, "heading":-1.44181, "vx":-3.74378, "vy":1.2641, "omega":0.87251, "ax":5.16088, "ay":1.03116, "alpha":0.61517, "fx":[83.83776,83.58494,84.97266,85.08669], "fy":[19.81963,20.5182,13.63854,13.4535]}, - {"t":1.10025, "x":2.83767, "y":6.51137, "heading":-1.41874, "vx":-3.60733, "vy":1.29136, "omega":0.88877, "ax":5.19226, "ay":0.95983, "alpha":0.2185, "fx":[84.68669,84.64241,85.08757,85.11743], "fy":[16.82605,16.92956,14.52331,14.48645]}, - {"t":1.12669, "x":2.74411, "y":6.54585, "heading":-1.39525, "vx":-3.47006, "vy":1.31673, "omega":0.89455, "ax":5.21035, "ay":0.91232, "alpha":-0.04032, "fx":[85.21619,85.2193,85.14251,85.139], "fy":[14.69342,14.69617,15.13468,15.13423]}, - {"t":1.15313, "x":2.6542, "y":6.58098, "heading":-1.3716, "vx":-3.33231, "vy":1.34085, "omega":0.89348, "ax":5.22194, "ay":0.8784, "alpha":-0.22281, "fx":[85.57445,85.57288,85.1678,85.15981], "fy":[13.084,13.20304,15.60116,15.55279]}, - {"t":1.17957, "x":2.56792, "y":6.61673, "heading":-1.34798, "vx":-3.19426, "vy":1.36408, "omega":0.88759, "ax":5.22992, "ay":0.85298, "alpha":-0.35855, "fx":[85.83139,85.8071,85.1765,85.18222], "fy":[11.81668,12.15679,15.98083,15.82379]}, - {"t":1.20601, "x":2.4853, "y":6.65309, "heading":-1.32451, "vx":-3.05599, "vy":1.38663, "omega":0.87811, "ax":5.23573, "ay":0.83319, "alpha":-0.46351, "fx":[86.02383,85.97092,85.17511,85.20676], "fy":[10.78616,11.40036,16.30354,15.99417]}, - {"t":1.23244, "x":2.40634, "y":6.69004, "heading":-1.30129, "vx":-2.91757, "vy":1.40865, "omega":0.86586, "ax":5.24011, "ay":0.81735, "alpha":-0.54715, "fx":[86.17283,86.09011,85.16732,85.23332], "fy":[9.92713,10.8422,16.58611,16.09291]}, - {"t":1.25888, "x":2.33103, "y":6.72757, "heading":-1.2784, "vx":-2.77903, "vy":1.43026, "omega":0.85139, "ax":5.24354, "ay":0.80437, "alpha":-0.61536, "fx":[86.29126,86.17937,85.15532,85.2616], "fy":[9.197,10.42531,16.83874,16.13892]}, - {"t":1.28532, "x":2.25939, "y":6.76567, "heading":-1.25589, "vx":-2.6404, "vy":1.45153, "omega":0.83512, "ax":5.24628, "ay":0.79355, "alpha":-0.67207, "fx":[86.38737,86.24768,85.14055,85.29126], "fy":[8.56677,10.11231,17.06789,16.14518]}, - {"t":1.31176, "x":2.19142, "y":6.80432, "heading":-1.23382, "vx":-2.5017, "vy":1.47251, "omega":0.81736, "ax":5.24852, "ay":0.78438, "alpha":-0.71996, "fx":[86.46671,86.30079,85.12397,85.32198], "fy":[8.01601,9.87762,17.27793,16.12092]}, - {"t":1.33819, "x":2.12712, "y":6.84352, "heading":-1.21221, "vx":-2.36294, "vy":1.49325, "omega":0.79832, "ax":5.25039, "ay":0.77651, "alpha":-0.76094, "fx":[86.53314,86.34255,85.10625,85.35345], "fy":[7.52986,9.70314,17.47188,16.07295]}, - {"t":1.36463, "x":2.06648, "y":6.88327, "heading":-1.1911, "vx":-2.22414, "vy":1.51377, "omega":0.77821, "ax":5.25196, "ay":0.76968, "alpha":-0.7964, "fx":[86.58941,86.37564,85.08788,85.3854], "fy":[7.09723,9.57564,17.65192,16.00641]}, - {"t":1.39107, "x":2.00951, "y":6.92356, "heading":-1.17053, "vx":-2.08529, "vy":1.53412, "omega":0.75715, "ax":5.25331, "ay":0.7637, "alpha":-0.82738, "fx":[86.63757,86.40197,85.06923,85.4176], "fy":[6.70965,9.48525,17.81967,15.92532]}, - {"t":1.41751, "x":1.95622, "y":6.96439, "heading":-1.15051, "vx":-1.9464, "vy":1.55431, "omega":0.73528, "ax":5.25447, "ay":0.75841, "alpha":-0.85469, "fx":[86.67913,86.42295,85.05055,85.44983], "fy":[6.36051,9.42439,17.9764,15.83289]}, - {"t":1.44395, "x":1.9066, "y":7.00574, "heading":-1.13107, "vx":-1.80748, "vy":1.57436, "omega":0.71268, "ax":5.27843, "ay":0.54435, "alpha":-0.91419, "fx":[86.86122,86.70287,85.58754,86.01762], "fy":[2.22333,6.08214,15.09134,12.19922]}, - {"t":1.46774, "x":1.86509, "y":7.04336, "heading":-1.11411, "vx":-1.6819, "vy":1.58731, "omega":0.69093, "ax":5.30292, "ay":0.09702, "alpha":-1.01632, "fx":[86.66542,86.90686,86.44859,86.74989], "fy":[-6.14146,-0.84067,8.83778,4.48866]}, - {"t":1.49153, "x":1.82657, "y":7.08115, "heading":-1.09768, "vx":-1.55573, "vy":1.58962, "omega":0.66675, "ax":5.28768, "ay":-0.37719, "alpha":-1.11681, "fx":[85.60486,86.5382,86.86579,86.76513], "fy":[-14.81987,-8.00152,2.10504,-3.94907]}, - {"t":1.51532, "x":1.79106, "y":7.11886, "heading":-1.08181, "vx":-1.42993, "vy":1.58065, "omega":0.64018, "ax":5.22678, "ay":-0.86859, "alpha":-1.21288, "fx":[83.6231,85.55451,86.73864,85.87523], "fy":[-23.55173,-15.26311,-5.03399,-12.95059]}, - {"t":1.53911, "x":1.75851, "y":7.15622, "heading":-1.06658, "vx":-1.30557, "vy":1.55998, "omega":0.61132, "ax":5.11708, "ay":-1.36481, "alpha":-1.30218, "fx":[80.74802,83.94917,85.98047,83.94043], "fy":[-32.05397,-22.47281,-12.46584,-22.25537]}, - {"t":1.56291, "x":1.7289, "y":7.19295, "heading":-1.05204, "vx":-1.18383, "vy":1.52751, "omega":0.58034, "ax":4.95919, "ay":-1.85206, "alpha":-1.38283, "fx":[77.09247,81.75451,84.53329,80.913], "fy":[-40.06411,-29.47783,-20.03982,-31.52854]}, - {"t":1.5867, "x":1.70214, "y":7.22877, "heading":-1.03823, "vx":-1.06584, "vy":1.48345, "omega":0.54744, "ax":4.7577, "ay":-2.31697, "alpha":-1.45316, "fx":[72.8335,79.03867,82.38109,76.86435], "fy":[-47.37901,-36.14095,-27.57943,-40.41258]}, - {"t":1.61049, "x":1.67813, "y":7.26341, "heading":-1.0252, "vx":-0.95264, "vy":1.42832, "omega":0.51287, "ax":4.52059, "ay":-2.74837, "alpha":-1.51152, "fx":[68.17835,75.89649,79.55738,71.97964], "fy":[-53.87617,-42.35318,-34.90089,-48.59241]}, - {"t":1.63428, "x":1.65674, "y":7.29661, "heading":-1.013, "vx":-0.84509, "vy":1.36293, "omega":0.4769, "ax":4.25783, "ay":-3.13856, "alpha":-1.55649, "fx":[63.32998,72.43716,76.14363,66.51901], "fy":[-59.51402,-48.04084,-41.83475,-55.84836]}, - {"t":1.65807, "x":1.63784, "y":7.32815, "heading":-1.00166, "vx":-0.74379, "vy":1.28826, "omega":0.43987, "ax":3.9799, "ay":-3.48367, "alpha":-1.58732, "fx":[58.46211,68.77175,72.2586,60.76274], "fy":[-64.31641,-53.16641,-48.24498,-62.07765]}, - {"t":1.68187, "x":1.62127, "y":7.35782, "heading":-0.99119, "vx":-0.6491, "vy":1.20538, "omega":0.40211, "ax":3.69637, "ay":-3.7833, "alpha":-1.60437, "fx":[53.70743,65.00297,68.04148,54.96277], "fy":[-68.35059,-57.72437,-54.04076,-67.28315]}, - {"t":1.70566, "x":1.60687, "y":7.38542, "heading":-0.98162, "vx":-0.56115, "vy":1.11536, "omega":0.36394, "ax":3.41513, "ay":-4.03964, "alpha":-1.60906, "fx":[49.15672,61.21866,63.63387,49.31452], "fy":[-71.70617,-61.73411,-59.179,-71.5424]}, - {"t":1.72945, "x":1.59449, "y":7.41082, "heading":-0.97296, "vx":-0.4799, "vy":1.01925, "omega":0.32565, "ax":3.14208, "ay":-4.25651, "alpha":-1.60355, "fx":[44.86454,57.48891,59.16491,43.94989], "fy":[-74.4792,-65.23203,-63.65896,-74.97315]}, - {"t":1.75324, "x":1.58396, "y":7.43386, "heading":-0.96522, "vx":-0.40514, "vy":0.91798, "omega":0.2875, "ax":2.88122, "ay":-4.4385, "alpha":-1.59026, "fx":[40.85743,53.86596,54.74193,38.94442], "fy":[-76.76192,-68.26435,-67.5121,-77.70571]}, - {"t":1.77703, "x":1.57514, "y":7.45445, "heading":-0.95838, "vx":-0.33659, "vy":0.81238, "omega":0.24967, "ax":2.63495, "ay":-4.59036, "alpha":-1.57146, "fx":[37.14207,50.38599,50.44685,34.33053], "fy":[-78.6374,-70.88133,-70.79073,-79.86514]}, - {"t":1.80083, "x":1.56787, "y":7.47248, "heading":-0.95244, "vx":-0.2739, "vy":0.70317, "omega":0.21228, "ax":2.40444, "ay":-4.71662, "alpha":-1.54912, "fx":[33.71208,47.07178,46.33689,30.11108], "fy":[-80.17744,-73.13324,-73.55781,-81.56227]}, - {"t":1.82462, "x":1.56204, "y":7.48787, "heading":-0.94739, "vx":-0.2167, "vy":0.59095, "omega":0.17542, "ax":2.18997, "ay":-4.82136, "alpha":-1.52476, "fx":[30.55321,43.9355,42.44786,26.27063], "fy":[-81.44247,-75.06769,-75.87919,-82.89062]}, - {"t":1.84841, "x":1.5575, "y":7.50057, "heading":-0.94321, "vx":-0.16459, "vy":0.47624, "omega":0.13914, "ax":1.99122, "ay":-4.90815, "alpha":-1.49952, "fx":[27.64698,40.98146,38.7985,22.78366], "fy":[-82.48254,-76.72819,-77.81843,-83.92666]}, - {"t":1.8722, "x":1.55415, "y":7.51051, "heading":-0.9399, "vx":-0.11722, "vy":0.35947, "omega":0.10347, "ax":1.8075, "ay":-4.98006, "alpha":-1.47421, "fx":[24.97315,38.20825,35.39485,19.62016], "fy":[-83.33859,-78.15354,-79.4338,-84.73177]}, - {"t":1.89599, "x":1.55187, "y":7.51765, "heading":-0.93744, "vx":-0.07421, "vy":0.24098, "omega":0.06839, "ax":1.63788, "ay":-5.03963, "alpha":-1.44934, "fx":[22.51124,35.61063,32.23403,16.74904], "fy":[-84.04396,-79.3777,-80.7769,-85.3546]}, - {"t":1.91979, "x":1.55057, "y":7.52196, "heading":-0.93581, "vx":-0.03524, "vy":0.12108, "omega":0.03391, "ax":1.48136, "ay":-5.089, "alpha":-1.42527, "fx":[20.24149,33.18088,29.30731,14.14021], "fy":[-84.62567,-80.43007,-81.89237,-85.83349]}, - {"t":1.94358, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.29969, "ay":6.33494, "alpha":2.20137, "fx":[11.11608,-8.25263,-22.01005,-0.45099], "fy":[103.79752,104.0828,102.03172,104.34491]}, + {"t":0.02225, "x":5.00415, "y":5.24549, "heading":-2.09512, "vx":-0.00667, "vy":0.14096, "omega":0.04898, "ax":-0.56869, "ay":6.3138, "alpha":2.24687, "fx":[7.30676,-12.11508,-26.75759,-5.62204], "fy":[104.12235,103.69445,100.87947,104.17818]}, + {"t":0.0445, "x":5.00386, "y":5.25019, "heading":-2.09403, "vx":-0.01932, "vy":0.28145, "omega":0.09898, "ax":-0.87161, "ay":6.27627, "alpha":2.29374, "fx":[2.93207,-16.4412,-31.97744,-11.51008], "fy":[104.32312,103.08634,99.33602,103.67506]}, + {"t":0.06675, "x":5.00321, "y":5.258, "heading":-2.09182, "vx":-0.03872, "vy":0.42111, "omega":0.15002, "ax":-1.21276, "ay":6.2163, "alpha":2.34137, "fx":[-2.11055,-21.28738,-37.69492,-18.21237], "fy":[104.32725,102.18393,97.29709,102.6902]}, + {"t":0.08901, "x":5.00205, "y":5.26891, "heading":-2.08849, "vx":-0.0657, "vy":0.55943, "omega":0.20212, "ax":-1.59614, "ay":6.12583, "alpha":2.38884, "fx":[-7.93768,-26.70885,-43.91617,-25.81247], "fy":[104.02872,100.8898,94.63783,101.02648]}, + {"t":0.11126, "x":5.00019, "y":5.28288, "heading":-2.08399, "vx":-0.10122, "vy":0.69574, "omega":0.25527, "ax":-2.02463, "ay":5.99452, "alpha":2.4348, "fx":[-14.67413,-32.75243,-50.61576,-34.35328], "fy":[103.27453,99.07884,91.21547,98.42734]}, + {"t":0.13351, "x":4.99744, "y":5.29984, "heading":-2.07831, "vx":-0.14627, "vy":0.82912, "omega":0.30945, "ax":-2.49874, "ay":5.80958, "alpha":2.47712, "fx":[-22.43797,-39.44465,-57.72138,-43.7943], "fy":[101.84849,96.59466,86.87783,94.58097]}, + {"t":0.15576, "x":4.99357, "y":5.31973, "heading":-2.07142, "vx":-0.20187, "vy":0.95839, "omega":0.36457, "ax":-3.01468, "ay":5.55629, "alpha":2.51212, "fx":[-31.30978,-46.77374,-65.09831,-53.9553], "fy":[99.45661,93.25024,81.4811,89.1514]}, + {"t":0.17801, "x":4.98833, "y":5.34243, "heading":-2.06331, "vx":-0.26895, "vy":1.08203, "omega":0.42047, "ax":-3.5623, "ay":5.21976, "alpha":2.53338, "fx":[-41.27884,-54.66539,-72.53929,-64.46393], "fy":[95.72468,88.83736,74.91827,81.85237]}, + {"t":0.20026, "x":4.98146, "y":5.3678, "heading":-2.05396, "vx":-0.34822, "vy":1.19817, "omega":0.47684, "ax":-4.12335, "ay":4.78805, "alpha":2.53069, "fx":[-52.16542,-62.95583,-79.76813,-74.74624], "fy":[90.2295,83.15103,67.15604,72.56536]}, + {"t":0.22251, "x":4.97269, "y":5.39564, "heading":-2.04334, "vx":-0.43997, "vy":1.30471, "omega":0.53315, "ax":-4.67155, "ay":4.25673, "alpha":2.49095, "fx":[-63.53975,-71.37184,-86.46522,-84.10682], "fy":[82.59161,76.03351,58.27057,61.46177]}, + {"t":0.24476, "x":4.96175, "y":5.42573, "heading":-2.03148, "vx":-0.54391, "vy":1.39943, "omega":0.58858, "ax":-5.17568, "ay":3.63325, "alpha":2.40288, "fx":[-74.69631,-79.53401,-92.31603,-91.90377], "fy":[72.63875,67.43481,48.46627,49.04725]}, + {"t":0.26702, "x":4.94836, "y":5.45777, "heading":-2.01839, "vx":-0.65908, "vy":1.48028, "omega":0.64204, "ax":-5.60575, "ay":2.9389, "alpha":2.26395, "fx":[-84.76096,-86.99987,-97.07113,-97.74128], "fy":[60.5838,57.47165,38.06235,36.06374]}, + {"t":0.28927, "x":4.93231, "y":5.49143, "heading":-2.0041, "vx":-0.78382, "vy":1.54567, "omega":0.69242, "ax":-5.9403, "ay":2.20616, "alpha":2.08357, "fx":[-92.94394,-93.34821,-100.59529,-101.5632], "fy":[47.08482,46.45269,27.44447,23.28425]}, + {"t":0.31152, "x":4.9134, "y":5.52637, "heading":-1.98869, "vx":-0.916, "vy":1.59476, "omega":0.73878, "ax":-6.17156, "ay":1.47183, "alpha":1.87883, "fx":[-98.80916,-98.27743,-102.88544,-103.60127], "fy":[33.08701,34.84252,16.99715,11.31989]}, + {"t":0.33377, "x":4.89149, "y":5.56222, "heading":-1.97225, "vx":-1.05332, "vy":1.62751, "omega":0.78059, "ax":-6.3056, "ay":0.76866, "alpha":1.66664, "fx":[-102.37512,-101.67225,-104.05245,-104.23828], "fy":[19.52074,23.16833,7.04267,0.53251]}, + {"t":0.35602, "x":4.86649, "y":5.59863, "heading":-1.95488, "vx":-1.19363, "vy":1.64462, "omega":0.81767, "ax":-6.35802, "ay":0.11965, "alpha":1.45907, "fx":[-104.00207,-103.60696,-104.27953,-103.87727], "fy":[7.05456,11.90992,-2.19384,-8.94613]}, + {"t":0.37827, "x":4.83835, "y":5.63525, "heading":-1.93669, "vx":-1.3351, "vy":1.64728, "omega":0.85014, "ax":-6.34816, "ay":-0.4632, "alpha":1.263, "fx":[-104.18793,-104.29241,-103.77666,-102.86447], "fy":[-3.97783,1.42114,-10.58919,-17.14357]}, + {"t":0.40052, "x":4.80708, "y":5.67179, "heading":-1.91777, "vx":-1.47636, "vy":1.63697, "omega":0.87824, "ax":-6.32821, "ay":-0.73448, "alpha":1.12799, "fx":[-103.92107,-104.27143,-103.33738,-102.28704], "fy":[-9.06191,-3.88444,-14.53516,-20.54774]}, + {"t":0.42688, "x":4.76597, "y":5.71467, "heading":-1.89463, "vx":-1.64312, "vy":1.61762, "omega":0.90797, "ax":-6.32727, "ay":-0.74654, "alpha":1.05725, "fx":[-103.887,-104.22538,-103.29287,-102.34972], "fy":[-9.27907,-4.64621,-14.74041,-20.15229]}, + {"t":0.45323, "x":4.72048, "y":5.75704, "heading":-1.8707, "vx":-1.80985, "vy":1.59794, "omega":0.93583, "ax":-6.32606, "ay":-0.76019, "alpha":0.97703, "fx":[-103.84361,-104.16703,-103.24783,-102.41747], "fy":[-9.57276,-5.49543,-14.93078,-19.71194]}, + {"t":0.47958, "x":4.67059, "y":5.79889, "heading":-1.84604, "vx":-1.97655, "vy":1.57791, "omega":0.96157, "ax":-6.3245, "ay":-0.77578, "alpha":0.88532, "fx":[-103.78821,-104.09269,-103.20273,-102.4905], "fy":[-9.96035,-6.4492,-15.10011,-19.22034]}, + {"t":0.50593, "x":4.61631, "y":5.8402, "heading":-1.8207, "vx":-2.14322, "vy":1.55747, "omega":0.9849, "ax":-6.32247, "ay":-0.79373, "alpha":0.7795, "fx":[-103.71707,-103.99714,-103.15814,-102.56897], "fy":[-10.46358,-7.53014,-15.24047,-18.66968]}, + {"t":0.53228, "x":4.55763, "y":5.88096, "heading":-1.79475, "vx":-2.30982, "vy":1.53655, "omega":1.00544, "ax":-6.31978, "ay":-0.81463, "alpha":0.65607, "fx":[-103.62489,-103.87291,-103.11479,-102.65299], "fy":[-11.11017,-8.76882,-15.34141,-18.05004]}, + {"t":0.55863, "x":4.49457, "y":5.92117, "heading":-1.76825, "vx":-2.47636, "vy":1.51509, "omega":1.02273, "ax":-6.31616, "ay":-0.83926, "alpha":0.51028, "fx":[-103.50396,-103.70881,-103.07358,-102.7426], "fy":[-11.93614,-10.20773,-15.38873,-17.34857]}, + {"t":0.58499, "x":4.42712, "y":5.96081, "heading":-1.7413, "vx":-2.6428, "vy":1.49297, "omega":1.03618, "ax":-6.31119, "ay":-0.86872, "alpha":0.33546, "fx":[-103.34278,-103.48758,-103.03562,-102.83768], "fy":[-12.98939,-11.90759,-15.36251,-16.54803]}, + {"t":0.61134, "x":4.35529, "y":5.99985, "heading":-1.714, "vx":-2.80911, "vy":1.47008, "omega":1.04502, "ax":-6.30417, "ay":-0.90456, "alpha":0.12194, "fx":[-103.12362,-103.18118,-103.00228,-102.93786], "fy":[-14.3351,-13.95832,-15.23361,-15.62454]}, + {"t":0.63769, "x":4.27908, "y":6.03827, "heading":-1.68646, "vx":-2.97524, "vy":1.44624, "omega":1.04823, "ax":-6.29396, "ay":-0.9491, "alpha":-0.14491, "fx":[-102.81823,-102.74162,-102.97513,-103.04228], "fy":[-16.06454,-16.49842,-14.95722,-14.54383]}, + {"t":0.66404, "x":4.19849, "y":6.07605, "heading":-1.65884, "vx":-3.14109, "vy":1.42123, "omega":1.04441, "ax":-6.27849, "ay":-1.00587, "alpha":-0.48836, "fx":[-102.37973,-102.08095,-102.95571,-103.14898], "fy":[-18.30932,-19.7518,-14.4601,-13.25469]}, + {"t":0.69039, "x":4.11354, "y":6.11316, "heading":-1.63132, "vx":-3.30654, "vy":1.39472, "omega":1.03155, "ax":-6.25376, "ay":-1.08051, "alpha":-0.94799, "fx":[-101.72657,-101.02379,-102.94439,-103.25367], "fy":[-21.26572,-24.1017,-13.61286,-11.67713]}, + {"t":0.71674, "x":4.02423, "y":6.14953, "heading":-1.60413, "vx":-3.47134, "vy":1.36625, "omega":1.00656, "ax":-6.2113, "ay":-1.18252, "alpha":-1.59712, "fx":[-100.70874,-99.18068,-102.93567,-103.34635], "fy":[-25.23648,-30.24974,-12.16198,-9.67951]}, + {"t":0.7431, "x":3.9306, "y":6.18513, "heading":-1.57761, "vx":-3.63502, "vy":1.33509, "omega":0.96448, "ax":-6.13053, "ay":-1.328, "alpha":-2.5883, "fx":[-99.03251,-95.55885,-102.89655,-103.40224], "fy":[-30.70159,-39.57279,-9.53397,-7.03291]}, + {"t":0.76945, "x":3.83268, "y":6.21985, "heading":-1.55219, "vx":-3.79657, "vy":1.30009, "omega":0.89627, "ax":-5.95268, "ay":-1.53961, "alpha":-4.28735, "fx":[-96.08847,-87.18175,-102.63589,-103.35369], "fy":[-38.41337,-54.81699,-4.12548,-3.32261]}, + {"t":0.7958, "x":3.73057, "y":6.25357, "heading":-1.52857, "vx":-3.95343, "vy":1.25952, "omega":0.78329, "ax":-5.49022, "ay":-1.77679, "alpha":-7.62979, "fx":[-90.67957,-64.76021,-100.57716,-103.00139], "fy":[-49.2199,-79.07663,10.04714,2.06094]}, + {"t":0.82215, "x":3.62448, "y":6.28615, "heading":-1.50793, "vx":-4.09811, "vy":1.2127, "omega":0.58223, "ax":-4.91702, "ay":-1.83551, "alpha":-10.58866, "fx":[-85.14228,-40.45991,-93.81133,-102.12222], "fy":[-57.08834,-92.49357,25.55536,3.9981]}, + {"t":0.8485, "x":3.51478, "y":6.31746, "heading":-1.49259, "vx":-4.22768, "vy":1.16433, "omega":0.30321, "ax":-4.53977, "ay":-2.32111, "alpha":-9.87916, "fx":[-79.02616,-31.25428,-87.30294,-99.28327], "fy":[-61.59473,-92.22531,5.73456,-3.69727]}, + {"t":0.87485, "x":3.4018, "y":6.34734, "heading":-1.4846, "vx":-4.34731, "vy":1.10317, "omega":0.04287, "ax":-1.22501, "ay":-2.8467, "alpha":-1.44496, "fx":[-23.45379,-14.48962,-16.37846,-25.78475], "fy":[-49.48862,-50.27737,-43.57994,-42.80684]}, + {"t":0.90121, "x":3.28682, "y":6.37542, "heading":-1.48347, "vx":-4.37959, "vy":1.02815, "omega":0.0048, "ax":2.08735, "ay":5.07766, "alpha":2.17878, "fx":[38.81559,21.91159,28.55706,47.21256], "fy":[83.63943,88.48794,83.34382,76.56917]}, + {"t":0.92756, "x":3.17213, "y":6.40428, "heading":-1.48334, "vx":-4.32459, "vy":1.16196, "omega":0.06221, "ax":4.2817, "ay":3.35673, "alpha":8.16806, "fx":[73.33535,24.0559,83.72103,98.87846], "fy":[69.89193,96.71518,35.33473,17.56246]}, + {"t":0.95391, "x":3.05966, "y":6.43606, "heading":-1.4817, "vx":-4.21176, "vy":1.25041, "omega":0.27745, "ax":4.8578, "ay":2.46495, "alpha":9.38208, "fx":[81.31606,36.13974,97.90862,102.2986], "fy":[62.86162,94.95166,-2.18475,5.56038]}, + {"t":0.98026, "x":2.95036, "y":6.46987, "heading":-1.47439, "vx":-4.08375, "vy":1.31537, "omega":0.52469, "ax":5.42621, "ay":2.19039, "alpha":7.04953, "fx":[87.88259,62.51192,101.47908,102.95941], "fy":[54.26711,81.17024,1.66323,6.13454]}, + {"t":1.00661, "x":2.84463, "y":6.50529, "heading":-1.46057, "vx":-3.94076, "vy":1.37309, "omega":0.71045, "ax":5.87332, "ay":1.85502, "alpha":4.21979, "fx":[93.70552,84.87485,102.47609,103.01387], "fy":[44.04929,58.47141,8.92689,9.8563]}, + {"t":1.03296, "x":2.74282, "y":6.54212, "heading":-1.44184, "vx":-3.78598, "vy":1.42197, "omega":0.82165, "ax":6.07083, "ay":1.586, "alpha":2.61316, "fx":[97.18474,94.16625,102.69259,102.9422], "fy":[36.2276,42.85827,12.15573,12.47052]}, + {"t":1.05932, "x":2.64516, "y":6.58014, "heading":-1.42019, "vx":-3.62601, "vy":1.46377, "omega":0.89051, "ax":6.16666, "ay":1.39921, "alpha":1.64046, "fx":[99.32923,98.31978,102.74868,102.85499], "fy":[30.28958,32.99035,13.96951,14.24791]}, + {"t":1.08567, "x":2.55175, "y":6.6192, "heading":-1.39673, "vx":-3.46351, "vy":1.50064, "omega":0.93374, "ax":6.21916, "ay":1.26665, "alpha":0.99376, "fx":[100.72264,100.42981,102.75235,102.78091], "fy":[25.66578,26.50902,15.17789,15.47682]}, + {"t":1.11202, "x":2.46264, "y":6.65919, "heading":-1.37212, "vx":-3.29962, "vy":1.53401, "omega":0.95993, "ax":6.25071, "ay":1.16873, "alpha":0.53209, "fx":[101.67056,101.61947,102.73286,102.72613], "fy":[21.96626,22.04473,16.07947,16.33544]}, + {"t":1.13837, "x":2.37786, "y":6.70002, "heading":-1.34682, "vx":-3.1349, "vy":1.56481, "omega":0.97395, "ax":6.27101, "ay":1.09371, "alpha":0.18509, "fx":[102.33972,102.34552,102.70107,102.69028], "fy":[18.93447,18.84708,16.80527,16.93343]}, + {"t":1.16472, "x":2.29743, "y":6.74163, "heading":-1.32116, "vx":-2.96965, "vy":1.59363, "omega":0.97883, "ax":6.28476, "ay":1.0345, "alpha":-0.08584, "fx":[102.82614,102.81628,102.662,102.67119], "fy":[16.39944,16.48751,17.42031,17.34099]}, + {"t":1.19107, "x":2.22136, "y":6.78398, "heading":-1.29537, "vx":-2.80404, "vy":1.62089, "omega":0.97657, "ax":6.29445, "ay":0.98661, "alpha":-0.30358, "fx":[103.18798,103.13627,102.61841,102.66635], "fy":[14.24431,14.70758,17.95985,17.60511]}, + {"t":1.21743, "x":2.14965, "y":6.82704, "heading":-1.26963, "vx":-2.63817, "vy":1.64689, "omega":0.96857, "ax":6.30149, "ay":0.9471, "alpha":-0.48255, "fx":[103.46205,103.36205,102.57199,102.6734], "fy":[12.38707,13.34337,18.44438,17.75852]}, + {"t":1.24378, "x":2.08232, "y":6.87077, "heading":-1.24411, "vx":-2.47211, "vy":1.67185, "omega":0.95585, "ax":6.30674, "ay":0.91396, "alpha":-0.63235, "fx":[103.67258,103.52618,102.5239,102.69027], "fy":[10.76865,12.2862,18.8864,17.82491]}, + {"t":1.27013, "x":2.01936, "y":6.91514, "heading":-1.21892, "vx":-2.30592, "vy":1.69594, "omega":0.93919, "ax":6.31074, "ay":0.88577, "alpha":-0.75958, "fx":[103.83606,103.64841,102.47497,102.71512], "fy":[9.34544,11.46123,19.29394,17.82199]}, + {"t":1.29648, "x":1.96079, "y":6.96014, "heading":-1.19417, "vx":-2.13962, "vy":1.71928, "omega":0.91917, "ax":6.31385, "ay":0.8615, "alpha":-0.86897, "fx":[103.96405,103.74122,102.42584,102.74641], "fy":[8.08451,10.81523,19.67234,17.76341]}, + {"t":1.32283, "x":1.9066, "y":7.00574, "heading":-1.16995, "vx":-1.97324, "vy":1.74198, "omega":0.89627, "ax":6.34436, "ay":0.55609, "alpha":-1.00221, "fx":[104.23743,104.12229,103.06775,103.44515], "fy":[1.85659,5.81564,15.82672,12.86479]}, + {"t":1.34682, "x":1.86109, "y":7.04769, "heading":-1.14845, "vx":-1.82105, "vy":1.75532, "omega":0.87223, "ax":6.3641, "ay":-0.06596, "alpha":-1.18717, "fx":[103.75693,104.20424,104.00392,104.19855], "fy":[-10.07908,-3.97158,7.38327,2.35437]}, + {"t":1.37081, "x":1.81924, "y":7.08978, "heading":-1.12753, "vx":-1.66839, "vy":1.75374, "omega":0.84375, "ax":6.31891, "ay":-0.72035, "alpha":-1.37008, "fx":[101.81502,103.34622,104.24434,103.80268], "fy":[-22.35867,-13.90777,-1.62525,-9.21353]}, + {"t":1.3948, "x":1.78103, "y":7.13164, "heading":-1.10729, "vx":-1.51682, "vy":1.73646, "omega":0.81089, "ax":6.20174, "ay":-1.38767, "alpha":-1.54478, "fx":[98.38576,101.54382,103.66272,101.95436], "fy":[-34.44712,-23.7247,-11.05343,-21.51804]}, + {"t":1.41878, "x":1.74643, "y":7.17289, "heading":-1.08784, "vx":-1.36805, "vy":1.70317, "omega":0.77383, "ax":6.01212, "ay":-2.04529, "alpha":-1.70672, "fx":[93.63776,98.8678,102.17014,98.47083], "fy":[-45.82022,-33.16419,-20.70098,-34.06096]}, + {"t":1.44277, "x":1.71535, "y":7.21316, "heading":-1.06927, "vx":-1.22383, "vy":1.65411, "omega":0.73289, "ax":5.75687, "ay":-2.67082, "alpha":-1.85302, "fx":[87.89436,95.45161,99.73563,93.37385], "fy":[-56.07252,-42.01054,-30.32794,-46.24037]}, + {"t":1.46676, "x":1.68765, "y":7.25207, "heading":-1.05169, "vx":-1.08574, "vy":1.59004, "omega":0.68844, "ax":5.44909, "ay":-3.24582, "alpha":-1.98122, "fx":[81.54793,91.46751,96.39869,86.91506], "fy":[-64.97802,-50.11174,-39.68114,-57.48092]}, + {"t":1.49075, "x":1.66317, "y":7.28928, "heading":-1.03518, "vx":-0.95502, "vy":1.51218, "omega":0.64092, "ax":5.10544, "ay":-3.75827, "alpha":-2.08854, "fx":[74.97201,87.09933,92.26817,79.51746], "fy":[-72.48771,-57.38553,-48.52618,-67.36283]}, + {"t":1.51474, "x":1.64173, "y":7.32447, "heading":-1.0198, "vx":-0.83256, "vy":1.42203, "omega":0.59082, "ax":4.74296, "ay":-4.20323, "alpha":-2.17253, "fx":[68.46467,82.51957,87.50698,71.66223], "fy":[-78.68356,-63.81237,-56.67598,-75.68728]}, + {"t":1.53872, "x":1.62312, "y":7.35737, "heading":-1.00563, "vx":-0.71878, "vy":1.3212, "omega":0.5387, "ax":4.37655, "ay":-4.58173, "alpha":-2.2324, "fx":[62.23037,77.87474,82.30709,63.78064], "fy":[-83.72073,-69.42076,-64.00815,-82.46085]}, + {"t":1.56271, "x":1.60714, "y":7.38775, "heading":-0.99271, "vx":-0.6138, "vy":1.2113, "omega":0.48515, "ax":4.01765, "ay":-4.89886, "alpha":-2.26959, "fx":[56.38845,73.2793,76.8629,56.19275], "fy":[-87.7798,-74.27059,-70.46827,-87.82964]}, + {"t":1.5867, "x":1.59357, "y":7.41539, "heading":-0.98107, "vx":-0.51743, "vy":1.09378, "omega":0.43071, "ax":3.67406, "ay":-5.16179, "alpha":-2.28736, "fx":[50.99335,68.81597,71.34966,49.09669], "fy":[-91.03648,-78.43841,-76.06119,-92.00587]}, + {"t":1.61069, "x":1.58222, "y":7.44015, "heading":-0.97074, "vx":-0.42929, "vy":0.96996, "omega":0.37584, "ax":3.35046, "ay":-5.37832, "alpha":-2.28986, "fx":[46.05568,64.53973,65.91061,42.58884], "fy":[-93.64643,-82.00633,-80.83552,-95.21294]}, + {"t":1.63467, "x":1.57288, "y":7.46187, "heading":-0.96172, "vx":-0.34892, "vy":0.84095, "omega":0.32091, "ax":3.04912, "ay":-5.55594, "alpha":-2.28121, "fx":[41.55945,60.4833,60.65262,36.694], "fy":[-95.74028,-85.05465,-84.86694,-97.65444]}, + {"t":1.65866, "x":1.56539, "y":7.48044, "heading":-0.95403, "vx":-0.27578, "vy":0.70767, "omega":0.26619, "ax":2.77063, "ay":-5.70139, "alpha":-2.26502, "fx":[37.47436,56.66269,55.64811,31.39295], "fy":[-97.42414,-87.65771,-88.24367,-99.5017]}, + {"t":1.68265, "x":1.55957, "y":7.49578, "heading":-0.94764, "vx":-0.20932, "vy":0.57091, "omega":0.21186, "ax":2.5145, "ay":-5.82044, "alpha":-2.24412, "fx":[33.7638,53.08198,50.94045,26.64299], "fy":[-98.78258,-89.88184,-91.05591,-100.8923]}, + {"t":1.70664, "x":1.55528, "y":7.5078, "heading":-0.94256, "vx":-0.149, "vy":0.43129, "omega":0.15803, "ax":2.2796, "ay":-5.91797, "alpha":-2.22062, "fx":[30.38972,49.73718,46.55045,22.39141], "fy":[-99.88231,-91.78481,-93.38905,-101.93366]}, + {"t":1.73063, "x":1.55236, "y":7.51644, "heading":-0.93877, "vx":-0.09432, "vy":0.28933, "omega":0.10476, "ax":2.06447, "ay":-5.99795, "alpha":-2.19598, "fx":[27.31536,46.61914,42.48261,18.5835], "fy":[-100.77565,-93.41619,-95.3201,-102.7083]}, + {"t":1.75461, "x":1.55069, "y":7.52165, "heading":-0.93626, "vx":-0.0448, "vy":0.14545, "omega":0.05208, "ax":1.86749, "ay":-6.06366, "alpha":-2.17121, "fx":[24.50658,43.71565,38.73037,15.16697], "fy":[-101.50361,-94.81801,-96.91626,-103.27911]}, + {"t":1.7786, "x":1.55015, "y":7.5234, "heading":-0.93501, "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/KtoPLO.traj b/src/main/deploy/choreo/KtoPLO.traj index ba40ea86..5c0e6f9e 100644 --- a/src/main/deploy/choreo/KtoPLO.traj +++ b/src/main/deploy/choreo/KtoPLO.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,45 +26,42 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.57909], + "waypoints":[0.0,1.44217], "samples":[ - {"t":0.0, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.8823, "ay":3.64522, "alpha":0.17839, "fx":[-62.64421,-63.91746,-64.30032,-63.01117], "fy":[60.46689,59.12013,58.70097,60.08189]}, - {"t":0.04386, "x":3.96442, "y":5.25656, "heading":-1.04555, "vx":-0.17029, "vy":0.15989, "omega":0.00782, "ax":-3.88208, "ay":3.64502, "alpha":0.17829, "fx":[-62.64111,-63.91364,-64.2962,-63.00778], "fy":[60.46303,59.11706,58.69808,60.07817]}, - {"t":0.08773, "x":3.95322, "y":5.26708, "heading":-1.04521, "vx":-0.34057, "vy":0.31978, "omega":0.01565, "ax":-3.88184, "ay":3.64478, "alpha":0.17819, "fx":[-62.63775,-63.90961,-64.29142,-63.00366], "fy":[60.45852,59.11329,58.69497,60.07427]}, - {"t":0.13159, "x":3.93454, "y":5.28461, "heading":-1.04452, "vx":-0.51084, "vy":0.47965, "omega":0.02346, "ax":-3.88155, "ay":3.64452, "alpha":0.17808, "fx":[-62.63406,-63.90527,-64.28585,-62.99873], "fy":[60.45325,59.10874,58.69156,60.07009]}, - {"t":0.17545, "x":3.9084, "y":5.30916, "heading":-1.04349, "vx":-0.6811, "vy":0.63951, "omega":0.03127, "ax":-3.88123, "ay":3.64421, "alpha":0.17795, "fx":[-62.62991,-63.90048,-64.27937,-62.99287], "fy":[60.44709,59.1033,58.68774,60.06551]}, - {"t":0.21932, "x":3.87479, "y":5.34072, "heading":-1.04212, "vx":-0.85135, "vy":0.79936, "omega":0.03908, "ax":-3.88085, "ay":3.64386, "alpha":0.1778, "fx":[-62.62518,-63.89506,-64.27178,-62.98591], "fy":[60.43988,59.09684,58.68339,60.06036]}, - {"t":0.26318, "x":3.83372, "y":5.37928, "heading":-1.04041, "vx":-1.02158, "vy":0.95919, "omega":0.04688, "ax":-3.88041, "ay":3.64344, "alpha":0.17763, "fx":[-62.61966,-63.88879,-64.26284,-62.97766], "fy":[60.43138,59.08915,58.67831,60.0544]}, - {"t":0.30704, "x":3.78517, "y":5.42486, "heading":-1.03835, "vx":-1.19178, "vy":1.119, "omega":0.05467, "ax":-3.87988, "ay":3.64294, "alpha":0.17743, "fx":[-62.61308,-63.88133,-64.2522,-62.96781], "fy":[60.42126,59.07999,58.67225,60.04732]}, - {"t":0.35091, "x":3.72917, "y":5.47745, "heading":-1.03595, "vx":-1.36197, "vy":1.2788, "omega":0.06245, "ax":-3.87924, "ay":3.64234, "alpha":0.1772, "fx":[-62.60506,-63.87224,-64.23938,-62.95597], "fy":[60.40907,59.06897,58.66486,60.03867]}, - {"t":0.39477, "x":3.66569, "y":5.53705, "heading":-1.03322, "vx":-1.53213, "vy":1.43856, "omega":0.07022, "ax":-3.87845, "ay":3.6416, "alpha":0.17691, "fx":[-62.59501,-63.86082,-64.22365,-62.94152], "fy":[60.39415,59.05555,58.65559,60.0278]}, - {"t":0.43864, "x":3.59476, "y":5.60365, "heading":-1.03013, "vx":-1.70225, "vy":1.5983, "omega":0.07798, "ax":-3.87745, "ay":3.64066, "alpha":0.17655, "fx":[-62.58207,-63.84607,-64.20394,-62.92352], "fy":[60.37547,59.03886,58.64363,60.0137]}, - {"t":0.4825, "x":3.51636, "y":5.67726, "heading":-1.02671, "vx":-1.87233, "vy":1.75799, "omega":0.08573, "ax":-3.87614, "ay":3.63943, "alpha":0.1761, "fx":[-62.5648,-63.8263,-64.17849,-62.90047], "fy":[60.3514,59.01754,58.62762,59.99476]}, - {"t":0.52636, "x":3.43051, "y":5.75787, "heading":-1.02295, "vx":-2.04235, "vy":1.91763, "omega":0.09345, "ax":-3.87436, "ay":3.63776, "alpha":0.1755, "fx":[-62.5407,-63.79866,-64.14428,-62.86972], "fy":[60.31912,58.98918,58.60523,59.96821]}, - {"t":0.57023, "x":3.33719, "y":5.84549, "heading":-1.01885, "vx":-2.21229, "vy":2.07719, "omega":0.10115, "ax":-3.87178, "ay":3.63534, "alpha":0.17468, "fx":[-62.50505,-63.75779,-64.09568,-62.82629], "fy":[60.27337,58.94924,58.572,59.92884]}, - {"t":0.61409, "x":3.23643, "y":5.9401, "heading":-1.01442, "vx":-2.38212, "vy":2.23665, "omega":0.10881, "ax":-3.86773, "ay":3.63154, "alpha":0.17348, "fx":[-62.44761,-63.69221,-64.02068,-62.75944], "fy":[60.20296,58.88794,58.5183,59.86553]}, - {"t":0.65795, "x":3.12822, "y":6.0417, "heading":-1.00965, "vx":-2.55177, "vy":2.39594, "omega":0.11642, "ax":-3.86044, "ay":3.62469, "alpha":0.1716, "fx":[-62.34148,-63.57234,-63.88844,-62.64109], "fy":[60.07914,58.77962,58.41876,59.74966]}, - {"t":0.70182, "x":3.01258, "y":6.15028, "heading":-1.00454, "vx":-2.72111, "vy":2.55494, "omega":0.12395, "ax":-3.84346, "ay":3.60875, "alpha":0.16812, "fx":[-62.087,-63.29081,-63.58754,-62.36774], "fy":[59.79817,58.52967,58.17923,59.47756]}, - {"t":0.74568, "x":2.88952, "y":6.26582, "heading":-0.9991, "vx":-2.88969, "vy":2.71323, "omega":0.13132, "ax":-3.75944, "ay":3.52985, "alpha":0.15825, "fx":[-60.7834,-61.90141,-62.14297,-61.01091], "fy":[58.45288,57.2869,56.94723,58.13834]}, - {"t":0.78954, "x":2.75915, "y":6.38823, "heading":-0.99334, "vx":-3.0546, "vy":2.86806, "omega":0.13826, "ax":3.75943, "ay":-3.52986, "alpha":-0.15655, "fx":[60.79304,61.90028,62.13298,61.01177], "fy":[-58.44267,-57.28729,-56.958,-58.13798]}, - {"t":0.83341, "x":2.62879, "y":6.51063, "heading":-0.98728, "vx":-2.88969, "vy":2.71323, "omega":0.1314, "ax":3.84346, "ay":-3.60875, "alpha":-0.16761, "fx":[62.09747,63.3017,63.57736,62.35643], "fy":[-59.78754,-58.51763,-58.19006,-59.48965]}, - {"t":0.87727, "x":2.50573, "y":6.62617, "heading":-0.98151, "vx":-2.72111, "vy":2.55493, "omega":0.12405, "ax":3.86044, "ay":-3.62469, "alpha":-0.17134, "fx":[62.35627,63.59231,63.87429,62.62043], "fy":[-60.06407,-58.7578,-58.43392,-59.77153]}, - {"t":0.92113, "x":2.39009, "y":6.73476, "heading":-0.97607, "vx":-2.55177, "vy":2.39594, "omega":0.11653, "ax":3.86773, "ay":-3.63154, "alpha":-0.17336, "fx":[62.467,63.72045,64.00224,62.73023], "fy":[-60.18313,-58.85718,-58.53816,-59.89633]}, - {"t":0.965, "x":2.28188, "y":6.83636, "heading":-0.97096, "vx":-2.38212, "vy":2.23665, "omega":0.10893, "ax":3.87178, "ay":-3.63534, "alpha":-0.17464, "fx":[62.52893,63.79369,64.07303,62.78916], "fy":[-60.24888,-58.9102,-58.59647,-59.96793]}, - {"t":1.00886, "x":2.18111, "y":6.93097, "heading":-0.96618, "vx":-2.21229, "vy":2.07719, "omega":0.10126, "ax":3.87436, "ay":-3.63776, "alpha":-0.17553, "fx":[62.56884,63.84166,64.11763,62.82525], "fy":[-60.29021,-58.94247,-58.63409,-60.01498]}, - {"t":1.05272, "x":2.0878, "y":7.01858, "heading":-0.96174, "vx":-2.04235, "vy":1.91763, "omega":0.09357, "ax":3.87614, "ay":-3.63943, "alpha":-0.17618, "fx":[62.59692,63.87583,64.14809,62.84924], "fy":[-60.31835,-58.96375,-58.66059,-60.0486]}, - {"t":1.09659, "x":2.00195, "y":7.09919, "heading":-0.95764, "vx":-1.87233, "vy":1.75799, "omega":0.08584, "ax":3.87745, "ay":-3.64066, "alpha":-0.17667, "fx":[62.61788,63.90159,64.17007,62.8661], "fy":[-60.33859,-58.9786,-58.68041,-60.07402]}, - {"t":1.14045, "x":1.92355, "y":7.1728, "heading":-0.95387, "vx":-1.70225, "vy":1.5983, "omega":0.07809, "ax":3.87845, "ay":-3.6416, "alpha":-0.17707, "fx":[62.6342,63.92179,64.1866,62.87847], "fy":[-60.35376,-58.9894,-58.69586,-60.09401]}, - {"t":1.18432, "x":1.85261, "y":7.23941, "heading":-0.95045, "vx":-1.53213, "vy":1.43856, "omega":0.07032, "ax":3.87924, "ay":-3.64234, "alpha":-0.17738, "fx":[62.6473,63.9381,64.19944,62.88785], "fy":[-60.3655,-58.99752,-58.7083,-60.11018]}, - {"t":1.22818, "x":1.78914, "y":7.299, "heading":-0.94736, "vx":-1.36197, "vy":1.2788, "omega":0.06254, "ax":3.87988, "ay":-3.64294, "alpha":-0.17764, "fx":[62.65806,63.95155,64.20969,62.89519], "fy":[-60.37484,-59.00383,-58.71852,-60.12354]}, - {"t":1.27204, "x":1.73313, "y":7.35159, "heading":-0.94462, "vx":-1.19178, "vy":1.119, "omega":0.05475, "ax":3.88041, "ay":-3.64344, "alpha":-0.17786, "fx":[62.66706,63.96282,64.21805,62.90109], "fy":[-60.38245,-59.00887,-58.72708,-60.13474]}, - {"t":1.31591, "x":1.68459, "y":7.39717, "heading":-0.94222, "vx":-1.02158, "vy":0.95919, "omega":0.04695, "ax":3.88085, "ay":-3.64386, "alpha":-0.17805, "fx":[62.67466,63.97236,64.22503,62.90596], "fy":[-60.38879,-59.01302,-58.73431,-60.14423]}, - {"t":1.35977, "x":1.64351, "y":7.43574, "heading":-0.94016, "vx":-0.85135, "vy":0.79936, "omega":0.03914, "ax":3.88123, "ay":-3.64421, "alpha":-0.17821, "fx":[62.68114,63.9805,64.23097,62.9101], "fy":[-60.39418,-59.01654,-58.74049,-60.15234]}, - {"t":1.40363, "x":1.6099, "y":7.46729, "heading":-0.93844, "vx":-0.6811, "vy":0.63951, "omega":0.03132, "ax":3.88155, "ay":-3.64452, "alpha":-0.17834, "fx":[62.6867,63.98747,64.23611,62.9137], "fy":[-60.39885,-59.01962,-58.74578,-60.15927]}, - {"t":1.4475, "x":1.58376, "y":7.49184, "heading":-0.93707, "vx":-0.51084, "vy":0.47965, "omega":0.0235, "ax":3.88184, "ay":-3.64478, "alpha":-0.17846, "fx":[62.69148,63.99346,64.24065,62.91694], "fy":[-60.40299,-59.02239,-58.75032,-60.16522]}, - {"t":1.49136, "x":1.56509, "y":7.50937, "heading":-0.93604, "vx":-0.34057, "vy":0.31978, "omega":0.01567, "ax":3.88209, "ay":-3.64502, "alpha":-0.17857, "fx":[62.69558,63.99859,64.24473,62.91991], "fy":[-60.40672,-59.02498,-58.75422,-60.1703]}, - {"t":1.53522, "x":1.55389, "y":7.51989, "heading":-0.93535, "vx":-0.17029, "vy":0.15989, "omega":0.00784, "ax":3.88231, "ay":-3.64522, "alpha":-0.17866, "fx":[62.6991,64.00296,64.24846,62.92273], "fy":[-60.41015,-59.02745,-58.75754,-60.17462]}, - {"t":1.57909, "x":1.55015, "y":7.5234, "heading":-0.93501, "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.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.65857, "ay":4.37407, "alpha":0.26986, "fx":[-74.91392,-76.83681,-77.41886,-75.46565], "fy":[72.82721,70.79664,70.15582,72.25151]}, + {"t":0.0437, "x":3.96371, "y":5.25723, "heading":-1.04555, "vx":-0.20359, "vy":0.19116, "omega":0.01179, "ax":-4.65829, "ay":4.37381, "alpha":0.26565, "fx":[-74.92875,-76.82177,-77.39436,-75.47196], "fy":[72.8025,70.8033,70.17292,72.23521]}, + {"t":0.0874, "x":3.95036, "y":5.26976, "heading":-1.04504, "vx":-0.40717, "vy":0.3823, "omega":0.0234, "ax":-4.65797, "ay":4.37351, "alpha":0.26083, "fx":[-74.94604,-76.80516,-77.36602,-75.47852], "fy":[72.77389,70.81027,70.1928,72.21724]}, + {"t":0.13111, "x":3.92812, "y":5.29065, "heading":-1.04401, "vx":-0.61073, "vy":0.57343, "omega":0.0348, "ax":-4.6576, "ay":4.37316, "alpha":0.25527, "fx":[-74.96627,-76.78642,-77.33305,-75.48562], "fy":[72.74055,70.81781,70.216,72.197]}, + {"t":0.17481, "x":3.89698, "y":5.31988, "heading":-1.04249, "vx":-0.81427, "vy":0.76455, "omega":0.04596, "ax":-4.65716, "ay":4.37275, "alpha":0.24877, "fx":[-74.99005,-76.76481,-77.29438,-75.49357], "fy":[72.70141,70.82629,70.24324,72.1737]}, + {"t":0.21851, "x":3.85695, "y":5.35747, "heading":-1.04049, "vx":-1.0178, "vy":0.95565, "omega":0.05683, "ax":-4.65664, "ay":4.37227, "alpha":0.24109, "fx":[-75.01825,-76.73932,-77.24857,-75.50286], "fy":[72.65499,70.83621,70.2755,72.14627]}, + {"t":0.26221, "x":3.80802, "y":5.40341, "heading":-1.038, "vx":-1.22131, "vy":1.14672, "omega":0.06737, "ax":-4.65602, "ay":4.37168, "alpha":0.23185, "fx":[-75.05207,-76.70853,-77.19355,-75.51411], "fy":[72.5992,70.84826,70.31417,72.1132]}, + {"t":0.30591, "x":3.7502, "y":5.4577, "heading":-1.03506, "vx":-1.42478, "vy":1.33777, "omega":0.0775, "ax":-4.65526, "ay":4.37097, "alpha":0.22054, "fx":[-75.09325,-76.67037,-77.12637,-75.52824], "fy":[72.53101,70.86343,70.36123,72.07233]}, + {"t":0.34962, "x":3.68349, "y":5.52033, "heading":-1.03167, "vx":-1.62823, "vy":1.52879, "omega":0.08714, "ax":-4.65429, "ay":4.37007, "alpha":0.20635, "fx":[-75.14441,-76.62176,-77.04257,-75.54662], "fy":[72.44586,70.88321,70.41966,72.02043]}, + {"t":0.39332, "x":3.60789, "y":5.59132, "heading":-1.02786, "vx":-1.83163, "vy":1.71977, "omega":0.09615, "ax":-4.65305, "ay":4.3689, "alpha":0.18805, "fx":[-75.20969,-76.55779,-76.93511,-75.57136], "fy":[72.3365,70.90992,70.49416,71.9524]}, + {"t":0.43702, "x":3.5234, "y":5.67065, "heading":-1.02366, "vx":-2.03498, "vy":1.9107, "omega":0.10437, "ax":-4.65137, "ay":4.36734, "alpha":0.16355, "fx":[-75.296,-76.47026,-76.7922,-75.60595], "fy":[72.19075,70.94744,70.59254,71.85979]}, + {"t":0.48072, "x":3.43003, "y":5.75832, "heading":-1.0191, "vx":-2.23825, "vy":2.10157, "omega":0.11152, "ax":-4.649, "ay":4.36512, "alpha":0.12906, "fx":[-75.41586,-76.34418,-76.59251,-75.65662], "fy":[71.9864,71.00277,70.72881,71.72734]}, + {"t":0.52442, "x":3.32777, "y":5.85433, "heading":-1.01423, "vx":-2.44142, "vy":2.29233, "omega":0.11716, "ax":-4.64538, "ay":4.36173, "alpha":0.07694, "fx":[-75.59442,-76.14908,-76.29302,-75.73563], "fy":[71.67826,71.08994,70.93095,71.52457]}, + {"t":0.56813, "x":3.21664, "y":5.95868, "heading":-1.00911, "vx":-2.64443, "vy":2.48295, "omega":0.12052, "ax":-4.63917, "ay":4.35592, "alpha":-0.01089, "fx":[-75.89073,-75.81195,-75.79232,-75.87105], "fy":[71.15832,71.24202,71.26376,71.18016]}, + {"t":0.61183, "x":3.09664, "y":6.07135, "heading":-1.00384, "vx":-2.84718, "vy":2.67331, "omega":0.12005, "ax":-4.62611, "ay":4.34373, "alpha":-0.18983, "fx":[-76.48315,-75.10328,-74.78148,-76.14426], "fy":[70.08904,71.5596,71.91849,70.47986]}, + {"t":0.65553, "x":2.9678, "y":6.19232, "heading":-0.99859, "vx":-3.04935, "vy":2.86314, "omega":0.11175, "ax":-4.5817, "ay":4.30237, "alpha":-0.74955, "fx":[-78.26165,-72.74567,-71.67613,-76.92491], "fy":[66.628,72.55815,73.79998,68.35586]}, + {"t":0.69923, "x":2.83016, "y":6.32156, "heading":-0.99371, "vx":-3.24957, "vy":3.05116, "omega":0.07899, "ax":-0.00007, "ay":0.00001, "alpha":-0.00021, "fx":[-0.00136,-0.00037,-0.001,-0.002], "fy":[-0.00065,-0.00001,0.00097,0.00032]}, + {"t":0.74293, "x":2.68815, "y":6.4549, "heading":-0.99026, "vx":-3.24958, "vy":3.05116, "omega":0.07898, "ax":4.58173, "ay":-4.30239, "alpha":0.74757, "fx":[78.23727,72.72385,71.70382,76.94522], "fy":[-66.65589,-72.58123,-73.77391,-68.3323]}, + {"t":0.78664, "x":2.55051, "y":6.58413, "heading":-0.98681, "vx":-3.04935, "vy":2.86314, "omega":0.11165, "ax":4.62611, "ay":-4.34373, "alpha":0.18935, "fx":[76.47232,75.09041,74.79277,76.15684], "fy":[-70.10066,-71.57334,-71.907,-70.46612]}, + {"t":0.83034, "x":2.42167, "y":6.70511, "heading":-0.98193, "vx":-2.84718, "vy":2.67331, "omega":0.11993, "ax":4.63917, "ay":-4.35592, "alpha":0.01086, "fx":[75.88976,75.81077,75.79332,75.87224], "fy":[-71.15936,-71.2433,-71.26273,-71.1789]}, + {"t":0.87404, "x":2.30167, "y":6.81778, "heading":-0.97668, "vx":-2.64444, "vy":2.48295, "omega":0.1204, "ax":4.64538, "ay":-4.36173, "alpha":-0.07679, "fx":[75.60311,76.16126,76.2845,75.72332], "fy":[-71.66922,-71.07684,-70.94002,-71.53768]}, + {"t":0.91774, "x":2.19054, "y":6.92212, "heading":-0.97142, "vx":-2.44142, "vy":2.29233, "omega":0.11705, "ax":4.649, "ay":-4.36512, "alpha":-0.12884, "fx":[75.43411,76.37026,76.57484,75.63003], "fy":[-71.96746,-70.9746,-70.74777,-71.75551]}, + {"t":0.96144, "x":2.08828, "y":7.01813, "heading":-0.96631, "vx":-2.23825, "vy":2.10156, "omega":0.11142, "ax":4.65138, "ay":-4.36734, "alpha":-0.1633, "fx":[75.32355,76.51016,76.76576,75.56504], "fy":[-72.16223,-70.90426,-70.62106,-71.90297]}, + {"t":1.00515, "x":1.99491, "y":7.1058, "heading":-0.96144, "vx":-2.03498, "vy":1.9107, "omega":0.10428, "ax":4.65305, "ay":-4.3689, "alpha":-0.1878, "fx":[75.24615,76.61106,76.90034,75.51653], "fy":[-72.29882,-70.8522,-70.53183,-72.01012]}, + {"t":1.04885, "x":1.91042, "y":7.18513, "heading":-0.95688, "vx":-1.83163, "vy":1.71977, "omega":0.09607, "ax":4.6543, "ay":-4.37007, "alpha":-0.2061, "fx":[75.18927,76.68767,77.0,75.47858], "fy":[-72.39956,-70.81172,-70.46592,-72.09192]}, + {"t":1.09255, "x":1.83482, "y":7.25612, "heading":-0.95268, "vx":-1.62823, "vy":1.52879, "omega":0.08707, "ax":4.65526, "ay":-4.37097, "alpha":-0.2203, "fx":[75.14592,76.74803,77.07659,75.4479], "fy":[-72.47672,-70.77914,-70.41546,-72.15663]}, + {"t":1.13625, "x":1.7681, "y":7.31876, "heading":-0.94888, "vx":-1.42479, "vy":1.33777, "omega":0.07744, "ax":4.65602, "ay":-4.37168, "alpha":-0.23163, "fx":[75.11188,76.7969,77.13719,75.4225], "fy":[-72.5376,-70.75227,-70.37569,-72.2092]}, + {"t":1.17995, "x":1.71028, "y":7.37304, "heading":-0.94549, "vx":-1.22131, "vy":1.14672, "omega":0.06732, "ax":4.65665, "ay":-4.37227, "alpha":-0.24088, "fx":[75.08448,76.83729,77.18632,75.40116], "fy":[-72.58683,-70.72974,-70.34356,-72.25275]}, + {"t":1.22366, "x":1.66136, "y":7.41898, "heading":-0.94255, "vx":-1.0178, "vy":0.95564, "omega":0.05679, "ax":4.65716, "ay":-4.37275, "alpha":-0.24858, "fx":[75.06193,76.87118,77.22696,75.38303], "fy":[-72.62748,-70.71065,-70.31705,-72.28936]}, + {"t":1.26736, "x":1.62133, "y":7.45657, "heading":-0.94007, "vx":-0.81428, "vy":0.76455, "omega":0.04592, "ax":4.6576, "ay":-4.37316, "alpha":-0.25508, "fx":[75.04299,76.89992,77.26121,75.36754], "fy":[-72.66168,-70.69436,-70.29473,-72.32045]}, + {"t":1.31106, "x":1.59019, "y":7.48581, "heading":-0.93806, "vx":-0.61073, "vy":0.57343, "omega":0.03478, "ax":4.65797, "ay":-4.37351, "alpha":-0.26065, "fx":[75.02677,76.92447,77.29053,75.35429], "fy":[-72.69094,-70.68046,-70.27561,-72.34705]}, + {"t":1.35476, "x":1.56795, "y":7.50669, "heading":-0.93654, "vx":-0.40717, "vy":0.3823, "omega":0.02339, "ax":4.6583, "ay":-4.37381, "alpha":-0.26546, "fx":[75.01261,76.94553,77.31603,75.34299], "fy":[-72.71636,-70.66861,-70.25892,-72.3699]}, + {"t":1.39846, "x":1.5546, "y":7.51922, "heading":-0.93552, "vx":-0.20359, "vy":0.19116, "omega":0.01179, "ax":4.65858, "ay":-4.37407, "alpha":-0.26967, "fx":[75.00002,76.96363,77.33852,75.33341], "fy":[-72.73879,-70.65858,-70.2441,-72.38956]}, + {"t":1.44217, "x":1.55015, "y":7.5234, "heading":-0.93501, "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/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 3406c19a..7b8a08e1 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,8 +18,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -36,69 +36,67 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.89025,1.99276], + "waypoints":[0.0,0.82372,1.92623], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.82157, "ay":-2.25873, "alpha":0.00548, "fx":[-78.81573,-78.83971,-78.83128,-78.8073], "fy":[-36.94249,-36.89133,-36.9095,-36.96062]}, - {"t":0.03424, "x":7.10849, "y":6.23424, "heading":-2.35619, "vx":-0.16509, "vy":-0.07734, "omega":0.00019, "ax":-4.82119, "ay":-2.25856, "alpha":0.00568, "fx":[-78.80914,-78.834,-78.82527,-78.80041], "fy":[-36.94026,-36.88724,-36.90607,-36.95906]}, - {"t":0.06848, "x":7.10001, "y":6.23027, "heading":-2.35619, "vx":-0.33017, "vy":-0.15467, "omega":0.00038, "ax":-4.82075, "ay":-2.25836, "alpha":0.00591, "fx":[-78.80161,-78.82747,-78.81839,-78.79253], "fy":[-36.93772,-36.88256,-36.90216,-36.95728]}, - {"t":0.10272, "x":7.08588, "y":6.22365, "heading":-2.35617, "vx":-0.49524, "vy":-0.232, "omega":0.00058, "ax":-4.82024, "ay":-2.25813, "alpha":0.00617, "fx":[-78.79291,-78.81994,-78.81045,-78.78343], "fy":[-36.93479,-36.87716,-36.89764,-36.95522]}, - {"t":0.13696, "x":7.06609, "y":6.21438, "heading":-2.35615, "vx":-0.66029, "vy":-0.30932, "omega":0.0008, "ax":-4.81964, "ay":-2.25787, "alpha":0.00648, "fx":[-78.78276,-78.81114,-78.80119,-78.77281], "fy":[-36.93136,-36.87085,-36.89236,-36.95282]}, - {"t":0.1712, "x":7.04066, "y":6.20246, "heading":-2.35613, "vx":-0.82531, "vy":-0.38663, "omega":0.00102, "ax":-4.81894, "ay":-2.25755, "alpha":0.00685, "fx":[-78.77076,-78.80074,-78.79023,-78.76026], "fy":[-36.92731,-36.86339,-36.88612,-36.94998]}, - {"t":0.20544, "x":7.00958, "y":6.1879, "heading":-2.35609, "vx":-0.99032, "vy":-0.46393, "omega":0.00125, "ax":-4.8181, "ay":-2.25718, "alpha":0.00729, "fx":[-78.75634,-78.78825,-78.77708,-78.74518], "fy":[-36.92244,-36.85444,-36.87863,-36.94658]}, - {"t":0.23968, "x":6.97284, "y":6.17069, "heading":-2.35605, "vx":-1.15529, "vy":-0.54122, "omega":0.0015, "ax":-4.81707, "ay":-2.25671, "alpha":0.00783, "fx":[-78.73871,-78.77297,-78.76099,-78.72673], "fy":[-36.91649,-36.84348,-36.86946,-36.94241]}, - {"t":0.27392, "x":6.93046, "y":6.15084, "heading":-2.356, "vx":-1.32023, "vy":-0.61849, "omega":0.00177, "ax":-4.81578, "ay":-2.25613, "alpha":0.0085, "fx":[-78.71664,-78.75385,-78.74086,-78.70366], "fy":[-36.90905,-36.82976,-36.858,-36.9372]}, - {"t":0.30817, "x":6.88243, "y":6.12834, "heading":-2.35594, "vx":-1.48512, "vy":-0.69574, "omega":0.00206, "ax":-4.81411, "ay":-2.25539, "alpha":0.00937, "fx":[-78.68822,-78.72923,-78.71495,-78.67395], "fy":[-36.89946,-36.81211,-36.84324,-36.93049]}, - {"t":0.34241, "x":6.82876, "y":6.1032, "heading":-2.35587, "vx":-1.64996, "vy":-0.77297, "omega":0.00238, "ax":-4.81189, "ay":-2.25439, "alpha":0.01053, "fx":[-78.65026,-78.69635,-78.68034,-78.63426], "fy":[-36.88665,-36.78852,-36.82352,-36.92153]}, - {"t":0.37665, "x":6.76944, "y":6.07541, "heading":-2.35579, "vx":-1.81472, "vy":-0.85016, "omega":0.00274, "ax":-4.80878, "ay":-2.253, "alpha":0.01216, "fx":[-78.59698,-78.6502,-78.6318,-78.57858], "fy":[-36.86867,-36.7554,-36.79586,-36.90896]}, - {"t":0.41089, "x":6.70449, "y":6.04498, "heading":-2.35569, "vx":-1.97938, "vy":-0.9273, "omega":0.00316, "ax":-4.80409, "ay":-2.2509, "alpha":0.01462, "fx":[-78.51675,-78.58074,-78.55875,-78.49478], "fy":[-36.84161,-36.70555,-36.75424,-36.89006]}, - {"t":0.44513, "x":6.6339, "y":6.01191, "heading":-2.35558, "vx":-2.14387, "vy":-1.00438, "omega":0.00366, "ax":-4.79624, "ay":-2.24737, "alpha":0.01876, "fx":[-78.38224,-78.46431,-78.43641,-78.35435], "fy":[-36.79624,-36.62195,-36.68451,-36.85841]}, - {"t":0.47937, "x":6.55768, "y":5.9762, "heading":-2.35546, "vx":-2.3081, "vy":-1.08133, "omega":0.0043, "ax":-4.78036, "ay":-2.24026, "alpha":0.02718, "fx":[-78.11014,-78.229,-78.18948,-78.07066], "fy":[-36.70453,-36.45283,-36.54371,-36.79459]}, - {"t":0.51361, "x":6.47584, "y":5.93786, "heading":-2.35531, "vx":-2.47178, "vy":-1.15803, "omega":0.00523, "ax":-4.7314, "ay":-2.2183, "alpha":0.05396, "fx":[-77.26808,-77.50373,-77.43066,-77.19529], "fy":[-36.42246,-35.92755,-36.10932,-36.60088]}, - {"t":0.54785, "x":6.38843, "y":5.89691, "heading":-2.35513, "vx":-2.63379, "vy":-1.23399, "omega":0.00708, "ax":3.32647, "ay":1.53345, "alpha":1.0632, "fx":[57.30949,53.11585,51.27974,55.82051], "fy":[23.82831,30.2232,26.51559,19.70882]}, - {"t":0.58209, "x":6.3002, "y":5.85555, "heading":-2.35489, "vx":-2.51989, "vy":-1.18148, "omega":0.04348, "ax":4.74177, "ay":2.21949, "alpha":0.04418, "fx":[77.58482,77.39231,77.45287,77.64552], "fy":[36.15653,36.56006,36.4133,36.00759]}, - {"t":0.61633, "x":6.2167, "y":5.8164, "heading":-2.3534, "vx":-2.35753, "vy":-1.10549, "omega":0.045, "ax":4.78333, "ay":2.2398, "alpha":0.02132, "fx":[78.22922,78.13614,78.16742,78.26052], "fy":[36.5538,36.75063,36.67945,36.48212]}, - {"t":0.65057, "x":6.13878, "y":5.77986, "heading":-2.35186, "vx":-2.19374, "vy":-1.0288, "omega":0.04573, "ax":4.79772, "ay":2.24684, "alpha":0.01356, "fx":[78.45295,78.39373,78.41414,78.47337], "fy":[36.69146,36.81708,36.77161,36.64578]}, - {"t":0.68481, "x":6.06648, "y":5.74595, "heading":-2.35029, "vx":-2.02947, "vy":-0.95186, "omega":0.04619, "ax":4.80502, "ay":2.2504, "alpha":0.00964, "fx":[78.56654,78.52444,78.53918,78.58128], "fy":[36.76135,36.8508,36.81832,36.72878]}, - {"t":0.71905, "x":5.9998, "y":5.71468, "heading":-2.34871, "vx":-1.86494, "vy":-0.87481, "omega":0.04652, "ax":4.80943, "ay":2.25256, "alpha":0.00728, "fx":[78.63526,78.60351,78.61475,78.6465], "fy":[36.80364,36.87117,36.84656,36.77896]}, - {"t":0.75329, "x":5.93877, "y":5.68604, "heading":-2.34712, "vx":-1.70026, "vy":-0.79768, "omega":0.04677, "ax":4.81239, "ay":2.25401, "alpha":0.00569, "fx":[78.68131,78.65649,78.66535,78.69018], "fy":[36.83197,36.8848,36.86546,36.8126]}, - {"t":0.78753, "x":5.88337, "y":5.66005, "heading":-2.34552, "vx":-1.53548, "vy":-0.7205, "omega":0.04697, "ax":4.8145, "ay":2.25504, "alpha":0.00455, "fx":[78.71432,78.69447,78.70161,78.72147], "fy":[36.85228,36.89455,36.87901,36.83671]}, - {"t":0.82177, "x":5.83362, "y":5.6367, "heading":-2.34391, "vx":-1.37063, "vy":-0.64329, "omega":0.04712, "ax":4.8161, "ay":2.25582, "alpha":0.0037, "fx":[78.73914,78.72302,78.72887,78.74499], "fy":[36.86754,36.90188,36.8892,36.85485]}, - {"t":0.85601, "x":5.78951, "y":5.616, "heading":-2.3423, "vx":-1.20573, "vy":-0.56605, "omega":0.04725, "ax":4.81734, "ay":2.25643, "alpha":0.00303, "fx":[78.75849,78.74528,78.7501,78.76331], "fy":[36.87944,36.90758,36.89714,36.869]}, - {"t":0.89025, "x":5.75105, "y":5.59794, "heading":-2.34068, "vx":-1.04078, "vy":-0.48878, "omega":0.04735, "ax":0.04227, "ay":-0.07579, "alpha":1.05708, "fx":[4.74064,0.47438,-3.33352,0.88287], "fy":[-1.14797,2.97283,-1.3317,-5.44936]}, - {"t":0.92268, "x":5.71732, "y":5.58205, "heading":-2.33914, "vx":-1.03941, "vy":-0.49124, "omega":0.08163, "ax":0.01701, "ay":-0.03593, "alpha":0.97427, "fx":[4.0009,0.09536,-3.43939,0.45523], "fy":[-0.49241,3.29507,-0.68356,-4.46852]}, - {"t":0.95511, "x":5.68363, "y":5.5661, "heading":-2.3365, "vx":-1.03886, "vy":-0.49241, "omega":0.11322, "ax":0.00371, "ay":-0.00782, "alpha":0.89474, "fx":[3.47802,-0.1192,-3.35193,0.2356], "fy":[-0.02735,3.43797,-0.22874,-3.6933]}, - {"t":0.98753, "x":5.64994, "y":5.55013, "heading":-2.33282, "vx":-1.03874, "vy":-0.49266, "omega":0.14224, "ax":0.00061, "ay":-0.00129, "alpha":0.81831, "fx":[3.13331,-0.16641,-3.11005,0.18313], "fy":[0.08534,3.24057,-0.1276,-3.28261]}, - {"t":1.01996, "x":5.61626, "y":5.53416, "heading":-2.32821, "vx":-1.03872, "vy":-0.4927, "omega":0.16877, "ax":-0.00004, "ay":0.00009, "alpha":0.74473, "fx":[2.83972,-0.17449,-2.83881,0.17093], "fy":[0.11345,2.97016,-0.11068,-2.96735]}, - {"t":1.05239, "x":5.58258, "y":5.51818, "heading":-2.32274, "vx":-1.03872, "vy":-0.4927, "omega":0.19292, "ax":-0.00015, "ay":0.00031, "alpha":0.67372, "fx":[2.56505,-0.17368,-2.56831,0.16717], "fy":[0.12196,2.69122,-0.11165,-2.68093]}, - {"t":1.08481, "x":5.54889, "y":5.5022, "heading":-2.31648, "vx":-1.03872, "vy":-0.49269, "omega":0.21477, "ax":-0.00023, "ay":0.00049, "alpha":0.60502, "fx":[2.29999,-0.17237,-2.30582,0.16298], "fy":[0.12814,2.42053,-0.11208,-2.40451]}, - {"t":1.11724, "x":5.51521, "y":5.48623, "heading":-2.30952, "vx":-1.03873, "vy":-0.49267, "omega":0.23439, "ax":-0.0006, "ay":0.00126, "alpha":0.53836, "fx":[2.03864,-0.17547,-2.05522,0.15301], "fy":[0.14209,2.16764,-0.10091,-2.12651]}, - {"t":1.14967, "x":5.48153, "y":5.47025, "heading":-2.30192, "vx":-1.03875, "vy":-0.49263, "omega":0.25184, "ax":-0.00061, "ay":0.00129, "alpha":0.47352, "fx":[1.79163,-0.17612,-1.80399,0.1484], "fy":[0.14167,1.90989,-0.09944,-1.86765]}, - {"t":1.18209, "x":5.44785, "y":5.45428, "heading":-2.29375, "vx":-1.03877, "vy":-0.49259, "omega":0.2672, "ax":0.00148, "ay":-0.00258, "alpha":0.41052, "fx":[1.60743,-0.20952,-1.50045,0.19903], "fy":[0.0745,1.59424,-0.15901,-1.67868]}, - {"t":1.21452, "x":5.41416, "y":5.4383, "heading":-2.28509, "vx":-1.03872, "vy":-0.49267, "omega":0.28051, "ax":0.89666, "ay":0.42914, "alpha":0.33358, "fx":[15.91888,14.45167,13.43655,14.82793], "fy":[7.09929,8.3646,6.93266,5.66569]}, - {"t":1.24695, "x":5.38095, "y":5.42255, "heading":-2.27599, "vx":-1.00965, "vy":-0.47876, "omega":0.29133, "ax":1.34557, "ay":0.63811, "alpha":0.26418, "fx":[22.94235,21.93879,20.99855,22.11014], "fy":[10.49491,11.51575,10.37189,9.34515]}, - {"t":1.27937, "x":5.34892, "y":5.40736, "heading":-2.26655, "vx":-0.96601, "vy":-0.45807, "omega":0.29989, "ax":1.35042, "ay":0.6404, "alpha":0.20878, "fx":[22.82629,22.01024,21.29521,22.17521], "fy":[10.52723,11.32679,10.41328,9.61008]}, - {"t":1.3118, "x":5.3183, "y":5.39285, "heading":-2.25682, "vx":-0.92222, "vy":-0.4373, "omega":0.30666, "ax":1.35205, "ay":0.64117, "alpha":0.15398, "fx":[22.65563,22.05146,21.52882,22.17788], "fy":[10.53215,11.11465,10.43282,9.84786]}, - {"t":1.34423, "x":5.28911, "y":5.379, "heading":-2.24688, "vx":-0.87838, "vy":-0.41651, "omega":0.31166, "ax":1.35287, "ay":0.64155, "alpha":0.09926, "fx":[22.47088,22.08917,21.74571,22.1617], "fy":[10.52727,10.89652,10.44982,10.0788]}, - {"t":1.37665, "x":5.26134, "y":5.36584, "heading":-2.23677, "vx":-0.83451, "vy":-0.39571, "omega":0.31487, "ax":1.35336, "ay":0.64178, "alpha":0.04437, "fx":[22.2799,22.12825,21.95606,22.13555], "fy":[10.51601,10.67534,10.46833,10.30757]}, - {"t":1.40908, "x":5.23499, "y":5.35334, "heading":-2.22656, "vx":-0.79063, "vy":-0.3749, "omega":0.31631, "ax":1.3537, "ay":0.64193, "alpha":-0.01088, "fx":[22.08505,22.17026,22.16366,22.10238], "fy":[10.4994,10.45176,10.4898,10.5361]}, - {"t":1.44151, "x":5.21006, "y":5.34152, "heading":-2.2163, "vx":-0.74673, "vy":-0.35408, "omega":0.31596, "ax":1.35393, "ay":0.64203, "alpha":-0.06666, "fx":[21.88707,22.21591,22.37046,22.06338], "fy":[10.47783,10.22572,10.51489,10.76555]}, - {"t":1.47393, "x":5.18656, "y":5.33038, "heading":-2.20606, "vx":-0.70283, "vy":-0.33326, "omega":0.3138, "ax":1.35411, "ay":0.64211, "alpha":-0.12311, "fx":[21.6861,22.26561,22.57766,22.01909], "fy":[10.45145,9.99694,10.54396,10.99677]}, - {"t":1.50636, "x":5.16448, "y":5.31991, "heading":-2.19588, "vx":-0.65892, "vy":-0.31244, "omega":0.30981, "ax":1.35425, "ay":0.64217, "alpha":-0.18039, "fx":[21.48196,22.31964,22.78614,21.96978], "fy":[10.42032,9.76502,10.57724,11.23049]}, - {"t":1.53879, "x":5.14383, "y":5.31012, "heading":-2.18584, "vx":-0.61501, "vy":-0.29162, "omega":0.30396, "ax":1.35436, "ay":0.64222, "alpha":-0.23854, "fx":[21.27492,22.37865,22.99517,21.91607], "fy":[10.38438,9.5294,10.61518,11.46724]}, - {"t":1.57121, "x":5.1246, "y":5.301, "heading":-2.17598, "vx":-0.57109, "vy":-0.27079, "omega":0.29622, "ax":1.35445, "ay":0.64226, "alpha":-0.29772, "fx":[21.06676,22.43547,23.2107,21.85783], "fy":[10.34328,9.29102,10.65669,11.70775]}, - {"t":1.60364, "x":5.10679, "y":5.29255, "heading":-2.16638, "vx":-0.52717, "vy":-0.24997, "omega":0.28657, "ax":1.35453, "ay":0.64229, "alpha":-0.35844, "fx":[20.8508,22.5039,23.42706,21.79398], "fy":[10.29817,9.0466,10.70317,11.95289]}, - {"t":1.63607, "x":5.09041, "y":5.28479, "heading":-2.15708, "vx":-0.48324, "vy":-0.22914, "omega":0.27495, "ax":1.35459, "ay":0.64232, "alpha":-0.42062, "fx":[20.63003,22.57716,23.64733,21.72544], "fy":[10.24848,8.79692,10.75405,12.20312]}, - {"t":1.66849, "x":5.07545, "y":5.27769, "heading":-2.14817, "vx":-0.43932, "vy":-0.20831, "omega":0.26131, "ax":1.35465, "ay":0.64234, "alpha":-0.48452, "fx":[20.4026,22.65569,23.87256,21.65275], "fy":[10.19455,8.5413,10.80917,12.45905]}, - {"t":1.70092, "x":5.06192, "y":5.27128, "heading":-2.13969, "vx":-0.39539, "vy":-0.18748, "omega":0.24559, "ax":1.3547, "ay":0.64236, "alpha":-0.55008, "fx":[20.17063,22.73852,24.10254,21.57506], "fy":[10.136,8.27921,10.86857,12.72155]}, - {"t":1.73335, "x":5.04981, "y":5.26553, "heading":-2.13173, "vx":-0.35146, "vy":-0.16665, "omega":0.22776, "ax":1.35474, "ay":0.64238, "alpha":-0.61761, "fx":[19.93199,22.82618,24.33837,21.49296], "fy":[10.0733,8.00985,10.93201,12.99129]}, - {"t":1.76578, "x":5.03912, "y":5.26047, "heading":-2.12434, "vx":-0.30754, "vy":-0.14582, "omega":0.20773, "ax":1.35477, "ay":0.64239, "alpha":-0.68728, "fx":[19.68595,22.91857,24.58078,21.40664], "fy":[10.00669,7.73242,10.99923,13.26908]}, - {"t":1.7982, "x":5.02986, "y":5.25608, "heading":-2.11761, "vx":-0.2636, "vy":-0.12499, "omega":0.18544, "ax":1.35481, "ay":0.6424, "alpha":-0.75929, "fx":[19.4317,23.01556,24.83051,21.31633], "fy":[9.93649,7.4461,11.06993,13.55576]}, - {"t":1.83063, "x":5.02203, "y":5.25236, "heading":-2.1116, "vx":-0.21967, "vy":-0.10416, "omega":0.16082, "ax":1.35484, "ay":0.64242, "alpha":-0.83382, "fx":[19.16841,23.11691,25.0884,21.22232], "fy":[9.8631,7.14997,11.14374,13.85225]}, - {"t":1.86306, "x":5.01562, "y":5.24932, "heading":-2.10638, "vx":-0.17574, "vy":-0.08333, "omega":0.13378, "ax":1.35486, "ay":0.64243, "alpha":-0.9111, "fx":[18.89509,23.22239,25.35532,21.12499], "fy":[9.78699,6.84305,11.22017,14.15952]}, - {"t":1.89548, "x":5.01063, "y":5.24696, "heading":-2.10204, "vx":-0.13181, "vy":-0.0625, "omega":0.10424, "ax":1.35489, "ay":0.64244, "alpha":-0.99134, "fx":[18.61073,23.33164,25.63221,21.02479], "fy":[9.70878,6.52428,11.29866,14.47864]}, - {"t":1.92791, "x":5.00707, "y":5.24527, "heading":-2.09866, "vx":-0.08787, "vy":-0.04166, "omega":0.07209, "ax":1.35491, "ay":0.64244, "alpha":-1.07477, "fx":[18.31417,23.44426,25.92009,20.92229], "fy":[9.62919,6.19249,11.37852,14.81073]}, - {"t":1.96034, "x":5.00493, "y":5.24426, "heading":-2.09632, "vx":-0.04394, "vy":-0.02083, "omega":0.03724, "ax":1.35493, "ay":0.64245, "alpha":-1.14853, "fx":[18.00309,23.55492,26.22061,20.8235], "fy":[9.57375,5.94578,11.43432,15.05761]}, - {"t":1.99276, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.78579, "ay":-2.7103, "alpha":0.00605, "fx":[-94.57808,-94.60455,-94.59524,-94.56877], "fy":[-44.32651,-44.27006,-44.29011,-44.34653]}, + {"t":0.03432, "x":7.10791, "y":6.23397, "heading":-2.35619, "vx":-0.19858, "vy":-0.09302, "omega":0.00021, "ax":-5.78525, "ay":-2.71006, "alpha":0.00631, "fx":[-94.56883,-94.59646,-94.58676,-94.55913], "fy":[-44.32331,-44.26438,-44.28531,-44.3442]}, + {"t":0.06864, "x":7.09768, "y":6.22918, "heading":-2.35619, "vx":-0.39714, "vy":-0.18604, "omega":0.00042, "ax":-5.78462, "ay":-2.70977, "alpha":0.00662, "fx":[-94.55805,-94.58704,-94.57686,-94.54787], "fy":[-44.31957,-44.25775,-44.27971,-44.34149]}, + {"t":0.10297, "x":7.08065, "y":6.2212, "heading":-2.35617, "vx":-0.59568, "vy":-0.27904, "omega":0.00065, "ax":-5.78387, "ay":-2.70944, "alpha":0.00699, "fx":[-94.54531,-94.5759,-94.56517,-94.53458], "fy":[-44.31515,-44.24991,-44.2731,-44.33829]}, + {"t":0.13729, "x":7.05679, "y":6.21002, "heading":-2.35615, "vx":-0.79419, "vy":-0.37203, "omega":0.00089, "ax":-5.78297, "ay":-2.70903, "alpha":0.00743, "fx":[-94.53002,-94.56255,-94.55115,-94.51862], "fy":[-44.30986,-44.2405,-44.26516,-44.33447]}, + {"t":0.17161, "x":7.02613, "y":6.19566, "heading":-2.35612, "vx":-0.99267, "vy":-0.46501, "omega":0.00115, "ax":-5.78188, "ay":-2.70854, "alpha":0.00798, "fx":[-94.51133,-94.54624,-94.53402,-94.49911], "fy":[-44.3034,-44.22898,-44.25545,-44.32981]}, + {"t":0.20593, "x":6.98865, "y":6.1781, "heading":-2.35608, "vx":-1.19112, "vy":-0.55797, "omega":0.00142, "ax":-5.78051, "ay":-2.70792, "alpha":0.00866, "fx":[-94.48797,-94.52587,-94.51262,-94.47472], "fy":[-44.29534,-44.21455,-44.2433,-44.32402]}, + {"t":0.24025, "x":6.94437, "y":6.15736, "heading":-2.35603, "vx":-1.38952, "vy":-0.65092, "omega":0.00172, "ax":-5.77875, "ay":-2.70712, "alpha":0.00955, "fx":[-94.45794,-94.49972,-94.48514,-94.44336], "fy":[-44.285,-44.19598,-44.22768,-44.31661]}, + {"t":0.27457, "x":6.89327, "y":6.13342, "heading":-2.35597, "vx":-1.58785, "vy":-0.74383, "omega":0.00204, "ax":-5.77641, "ay":-2.70607, "alpha":0.01074, "fx":[-94.4179,-94.4649,-94.44854,-94.40154], "fy":[-44.27126,-44.17115,-44.20683,-44.30683]}, + {"t":0.3089, "x":6.83537, "y":6.1063, "heading":-2.3559, "vx":-1.78611, "vy":-0.83671, "omega":0.00241, "ax":-5.77313, "ay":-2.70459, "alpha":0.01244, "fx":[-94.36186,-94.41628,-94.39739,-94.34298], "fy":[-44.25212,-44.13625,-44.17759,-44.29331]}, + {"t":0.34322, "x":6.77067, "y":6.07599, "heading":-2.35582, "vx":-1.98425, "vy":-0.92953, "omega":0.00284, "ax":-5.76822, "ay":-2.70237, "alpha":0.01504, "fx":[-94.27779,-94.34361,-94.32088,-94.25507], "fy":[-44.22363,-44.08357,-44.13362,-44.27346]}, + {"t":0.37754, "x":6.69917, "y":6.04249, "heading":-2.35572, "vx":-2.18223, "vy":-1.02228, "omega":0.00336, "ax":-5.76005, "ay":-2.69868, "alpha":0.01959, "fx":[-94.13765,-94.22335,-94.19398,-94.10831], "fy":[-44.17685,-43.99466,-44.05994,-44.24173]}, + {"t":0.41186, "x":6.62088, "y":6.00582, "heading":-2.35561, "vx":-2.37993, "vy":-1.11491, "omega":0.00403, "ax":-5.74378, "ay":-2.69134, "alpha":0.02973, "fx":[-93.85682,-93.98688,-93.9429,-93.81299], "fy":[-44.08684,-43.81083,-43.91022,-44.18519]}, + {"t":0.44618, "x":6.53581, "y":5.96597, "heading":-2.35547, "vx":-2.57706, "vy":-1.20728, "omega":0.00505, "ax":-5.69545, "ay":-2.66969, "alpha":0.07272, "fx":[-93.00263,-93.32152,-93.21582,-92.89921], "fy":[-43.86077,-43.18546,-43.43262,-44.09868]}, + {"t":0.48051, "x":6.44401, "y":5.92296, "heading":-2.3553, "vx":-2.77254, "vy":-1.29891, "omega":0.00754, "ax":-1.40173, "ay":-0.68458, "alpha":1.70125, "fx":[-16.34781,-23.1995,-29.40696,-22.70812], "fy":[-11.38741,-4.42357,-11.03549,-17.91978]}, + {"t":0.51483, "x":6.34802, "y":5.87798, "heading":-2.35504, "vx":-2.82065, "vy":-1.3224, "omega":0.06594, "ax":5.69532, "ay":2.66549, "alpha":0.07183, "fx":[93.21133,92.89947,93.00244,93.31704], "fy":[43.3677,44.02505,43.78875,43.12122]}, + {"t":0.54915, "x":6.25457, "y":5.83416, "heading":-2.35277, "vx":-2.62518, "vy":-1.23092, "omega":0.0684, "ax":5.74401, "ay":2.68971, "alpha":0.02516, "fx":[93.93982,93.83002,93.86748,93.97738], "fy":[43.89765,44.13022,44.04612,43.81278]}, + {"t":0.58347, "x":6.16785, "y":5.79349, "heading":-2.35043, "vx":-2.42803, "vy":-1.1386, "omega":0.06926, "ax":5.76029, "ay":2.69765, "alpha":0.01493, "fx":[94.19103,94.12587,94.14859,94.21377], "fy":[44.05752,44.19588,44.14561,44.00703]}, + {"t":0.61779, "x":6.08791, "y":5.756, "heading":-2.34805, "vx":-2.23033, "vy":-1.04601, "omega":0.06978, "ax":5.76843, "ay":2.70162, "alpha":0.01035, "fx":[94.31749,94.27234,94.28833,94.33348], "fy":[44.13586,44.23184,44.19677,44.10069]}, + {"t":0.65211, "x":6.01476, "y":5.72169, "heading":-2.34565, "vx":-2.03234, "vy":-0.95329, "omega":0.07013, "ax":5.77332, "ay":2.70399, "alpha":0.00771, "fx":[94.39354,94.35993,94.37198,94.40559], "fy":[44.18255,44.25405,44.22775,44.1562]}, + {"t":0.68644, "x":5.9484, "y":5.69057, "heading":-2.34325, "vx":-1.83419, "vy":-0.86048, "omega":0.0704, "ax":5.77657, "ay":2.70558, "alpha":0.00598, "fx":[94.4443,94.41824,94.42769,94.45375], "fy":[44.21356,44.26904,44.2485,44.19299]}, + {"t":0.72076, "x":5.88885, "y":5.66263, "heading":-2.34083, "vx":-1.63593, "vy":-0.76762, "omega":0.0706, "ax":5.7789, "ay":2.70671, "alpha":0.00476, "fx":[94.48058,94.45985,94.46744,94.48817], "fy":[44.23567,44.27982,44.26336,44.2192]}, + {"t":0.75508, "x":5.83611, "y":5.63788, "heading":-2.33841, "vx":-1.43759, "vy":-0.67472, "omega":0.07077, "ax":5.78064, "ay":2.70756, "alpha":0.00385, "fx":[94.50781,94.49105,94.49724,94.514], "fy":[44.25223,44.28793,44.27453,44.23882]}, + {"t":0.7894, "x":5.79017, "y":5.61631, "heading":-2.33598, "vx":-1.23919, "vy":-0.5818, "omega":0.0709, "ax":5.782, "ay":2.70822, "alpha":0.00315, "fx":[94.52899,94.51531,94.52041,94.53409], "fy":[44.26509,44.29425,44.28323,44.25407]}, + {"t":0.82372, "x":5.75105, "y":5.59794, "heading":-2.33355, "vx":-1.04074, "vy":-0.48884, "omega":0.07101, "ax":0.03944, "ay":-0.07066, "alpha":0.91123, "fx":[4.16704,0.41204,-2.83885,0.83899], "fy":[-1.05537,2.44987,-1.25513,-4.75979]}, + {"t":0.85615, "x":5.71732, "y":5.58205, "heading":-2.33124, "vx":-1.03946, "vy":-0.49114, "omega":0.10055, "ax":0.01906, "ay":-0.04027, "alpha":0.83517, "fx":[3.53209,0.11315,-2.8904,0.49151], "fy":[-0.55566,2.64496,-0.76155,-3.96108]}, + {"t":0.88858, "x":5.68362, "y":5.56611, "heading":-2.32798, "vx":-1.03884, "vy":-0.49244, "omega":0.12764, "ax":0.00406, "ay":-0.00856, "alpha":0.76316, "fx":[3.0077,-0.12283,-2.86099,0.24144], "fy":[-0.0335,2.87755,-0.24646,-3.15711]}, + {"t":0.921, "x":5.64994, "y":5.55013, "heading":-2.32384, "vx":-1.03871, "vy":-0.49272, "omega":0.15238, "ax":0.00032, "ay":-0.00066, "alpha":0.69483, "fx":[2.68216,-0.17753,-2.66059,0.17658], "fy":[0.09915,2.73537,-0.12095,-2.75705]}, + {"t":0.95343, "x":5.61626, "y":5.53415, "heading":-2.3189, "vx":-1.0387, "vy":-0.49274, "omega":0.17491, "ax":-0.00038, "ay":0.0008, "alpha":0.62983, "fx":[2.41982,-0.18433,-2.42231,0.16197], "fy":[0.12616,2.50148,-0.09997,-2.47526]}, + {"t":0.98586, "x":5.58258, "y":5.51818, "heading":-2.31323, "vx":-1.03871, "vy":-0.49271, "omega":0.19534, "ax":-0.00054, "ay":0.00113, "alpha":0.56784, "fx":[2.17851,-0.18424,-2.18569,0.15633], "fy":[0.1336,2.26093,-0.0966,-2.22396]}, + {"t":1.01828, "x":5.5489, "y":5.5022, "heading":-2.3069, "vx":-1.03873, "vy":-0.49268, "omega":0.21375, "ax":-0.00095, "ay":0.00201, "alpha":0.50851, "fx":[1.9443,-0.19108,-1.96215,0.14658], "fy":[0.14868,2.03998,-0.08292,-1.97428]}, + {"t":1.05071, "x":5.51521, "y":5.48623, "heading":-2.29997, "vx":-1.03876, "vy":-0.49261, "omega":0.23024, "ax":-0.00154, "ay":0.00326, "alpha":0.45154, "fx":[1.71737,-0.203,-1.74913,0.13377], "fy":[0.1681,1.83442,-0.06159,-1.72797]}, + {"t":1.08314, "x":5.48153, "y":5.47025, "heading":-2.2925, "vx":-1.03881, "vy":-0.49251, "omega":0.24488, "ax":0.00055, "ay":-0.00117, "alpha":0.39666, "fx":[1.54523,-0.17857,-1.49678,0.16632], "fy":[0.09287,1.54432,-0.13105,-1.58249]}, + {"t":1.11556, "x":5.44784, "y":5.45428, "heading":-2.28456, "vx":-1.03879, "vy":-0.49255, "omega":0.25774, "ax":0.00724, "ay":-0.01477, "alpha":0.34401, "fx":[1.48206,-0.16099,-1.1474,0.29995], "fy":[-0.1346,1.11107,-0.34823,-1.59381]}, + {"t":1.14799, "x":5.41416, "y":5.4383, "heading":-2.2762, "vx":-1.03856, "vy":-0.49302, "omega":0.2689, "ax":0.89112, "ay":0.44082, "alpha":0.28291, "fx":[15.65678,14.38922,13.51167,14.71449], "fy":[7.29089,8.32844,7.12406,6.08297]}, + {"t":1.18042, "x":5.38095, "y":5.42255, "heading":-2.26748, "vx":-1.00966, "vy":-0.47873, "omega":0.27807, "ax":1.34569, "ay":0.63787, "alpha":0.2274, "fx":[22.81385,22.02542,21.08969,22.06892], "fy":[10.49872,11.33899,10.36059,9.51346]}, + {"t":1.21284, "x":5.34892, "y":5.40736, "heading":-2.25846, "vx":-0.96602, "vy":-0.45805, "omega":0.28545, "ax":1.35048, "ay":0.64028, "alpha":0.17944, "fx":[22.72515,22.07603,21.36693,22.14278], "fy":[10.52949,11.18554,10.40809,9.74623]}, + {"t":1.24527, "x":5.31831, "y":5.39284, "heading":-2.24921, "vx":-0.92223, "vy":-0.43728, "omega":0.29126, "ax":1.35209, "ay":0.64108, "alpha":0.1319, "fx":[22.57958,22.10305,21.58064,22.15316], "fy":[10.53194,11.00771,10.43155,9.95088]}, + {"t":1.2777, "x":5.28911, "y":5.379, "heading":-2.23976, "vx":-0.87839, "vy":-0.4165, "omega":0.29554, "ax":1.3529, "ay":0.64149, "alpha":0.08427, "fx":[22.41837,22.1294,21.77752,22.14414], "fy":[10.52486,10.82315,10.45139,10.1489]}, + {"t":1.31012, "x":5.26134, "y":5.36583, "heading":-2.23018, "vx":-0.83452, "vy":-0.39569, "omega":0.29827, "ax":1.35339, "ay":0.64173, "alpha":0.03628, "fx":[22.24975,22.15827,21.96828,22.12505], "fy":[10.51191,10.63483,10.47191,10.34531]}, + {"t":1.34255, "x":5.23499, "y":5.35334, "heading":-2.22051, "vx":-0.79063, "vy":-0.37489, "omega":0.29945, "ax":1.35372, "ay":0.64188, "alpha":-0.01228, "fx":[22.07606,22.19065,22.15701,22.09896], "fy":[10.49417,10.44321,10.49464,10.54227]}, + {"t":1.37498, "x":5.21007, "y":5.34152, "heading":-2.2108, "vx":-0.74674, "vy":-0.35407, "omega":0.29905, "ax":1.35395, "ay":0.642, "alpha":-0.06162, "fx":[21.89802,22.22688,22.34592,22.06714], "fy":[10.47201,10.24812,10.52034,10.74114]}, + {"t":1.4074, "x":5.18656, "y":5.33038, "heading":-2.2011, "vx":-0.70283, "vy":-0.33325, "omega":0.29705, "ax":1.35412, "ay":0.64208, "alpha":-0.11195, "fx":[21.71559,22.26738,22.53638,22.03009], "fy":[10.44555,10.04911,10.54945,10.94295]}, + {"t":1.43983, "x":5.16448, "y":5.31991, "heading":-2.19147, "vx":-0.65892, "vy":-0.31243, "omega":0.29342, "ax":1.35426, "ay":0.64214, "alpha":-0.16346, "fx":[21.52847,22.31231,22.72958,21.98804], "fy":[10.4148,9.84556,10.58226,11.14862]}, + {"t":1.47226, "x":5.14383, "y":5.31011, "heading":-2.18195, "vx":-0.61501, "vy":-0.29161, "omega":0.28812, "ax":1.35437, "ay":0.64219, "alpha":-0.21636, "fx":[21.33623,22.36186,22.92641,21.94108], "fy":[10.3797,9.63683,10.61901,11.35902]}, + {"t":1.50468, "x":5.1246, "y":5.301, "heading":-2.17261, "vx":-0.57109, "vy":-0.27079, "omega":0.28111, "ax":1.35446, "ay":0.64223, "alpha":-0.27084, "fx":[21.13827,22.41624,23.12776,21.8892], "fy":[10.34019,9.4222,10.65989,11.57498]}, + {"t":1.53711, "x":5.10679, "y":5.29255, "heading":-2.1635, "vx":-0.52717, "vy":-0.24996, "omega":0.27232, "ax":1.35454, "ay":0.64227, "alpha":-0.32713, "fx":[20.93395,22.47559,23.33449,21.83236], "fy":[10.29618,9.2009,10.70503,11.79738]}, + {"t":1.56954, "x":5.09041, "y":5.28478, "heading":-2.15466, "vx":-0.48325, "vy":-0.22913, "omega":0.26172, "ax":1.3546, "ay":0.6423, "alpha":-0.38545, "fx":[20.72254,22.54006,23.54746,21.77049], "fy":[10.24761,8.97209,10.75455,12.0271]}, + {"t":1.60196, "x":5.07545, "y":5.27769, "heading":-2.14618, "vx":-0.43932, "vy":-0.20831, "omega":0.24922, "ax":1.35466, "ay":0.64232, "alpha":-0.44603, "fx":[20.50324,22.60978,23.76755,21.70355], "fy":[10.19442,8.7349,10.80853,12.26508]}, + {"t":1.63439, "x":5.06192, "y":5.27128, "heading":-2.1381, "vx":-0.39539, "vy":-0.18748, "omega":0.23475, "ax":1.3547, "ay":0.64234, "alpha":-0.50911, "fx":[20.27518,22.68493,23.99567,21.63146], "fy":[10.13659,8.48837,10.86702,12.51231]}, + {"t":1.66682, "x":5.04981, "y":5.26553, "heading":-2.13048, "vx":-0.35147, "vy":-0.16665, "omega":0.21825, "ax":1.35474, "ay":0.64236, "alpha":-0.57496, "fx":[20.03738,22.76559,24.23279,21.5542], "fy":[10.07413,8.23147,10.93003,12.76986]}, + {"t":1.69924, "x":5.03913, "y":5.26047, "heading":-2.12341, "vx":-0.30754, "vy":-0.14582, "omega":0.1996, "ax":1.35478, "ay":0.64238, "alpha":-0.64385, "fx":[19.78882,22.85182,24.47996,21.47175], "fy":[10.00707,7.96307,10.99751,13.03887]}, + {"t":1.73167, "x":5.02986, "y":5.25608, "heading":-2.11694, "vx":-0.26361, "vy":-0.12499, "omega":0.17872, "ax":1.35481, "ay":0.64239, "alpha":-0.71607, "fx":[19.52832,22.94371,24.73829,21.38418], "fy":[9.93554,7.68199,11.06938,13.32055]}, + {"t":1.7641, "x":5.02203, "y":5.25236, "heading":-2.11114, "vx":-0.21967, "vy":-0.10416, "omega":0.1555, "ax":1.35484, "ay":0.6424, "alpha":-0.79193, "fx":[19.25465,23.04122,25.00897,21.29157], "fy":[9.8597,7.38688,11.14545,13.61624]}, + {"t":1.79652, "x":5.01562, "y":5.24932, "heading":-2.1061, "vx":-0.17574, "vy":-0.08333, "omega":0.12982, "ax":1.35487, "ay":0.64241, "alpha":-0.87177, "fx":[18.96642,23.14428,25.29334,21.19411], "fy":[9.77982,7.07632,11.22548,13.9274]}, + {"t":1.82895, "x":5.01063, "y":5.24696, "heading":-2.10189, "vx":-0.13181, "vy":-0.0625, "omega":0.10156, "ax":1.35489, "ay":0.64242, "alpha":-0.95594, "fx":[18.6621,23.25273,25.5928,21.09205], "fy":[9.69626,6.74873,11.3091,14.25559]}, + {"t":1.86138, "x":5.00707, "y":5.24527, "heading":-2.09859, "vx":-0.08787, "vy":-0.04166, "omega":0.07056, "ax":1.35491, "ay":0.64243, "alpha":-1.04484, "fx":[18.34002,23.36637,25.90892,20.98579], "fy":[9.60955,6.40237,11.39582,14.60254]}, + {"t":1.8938, "x":5.00493, "y":5.24426, "heading":-2.09631, "vx":-0.04394, "vy":-0.02083, "omega":0.03668, "ax":1.35493, "ay":0.64244, "alpha":-1.1311, "fx":[17.99789,23.46871,26.24264,20.89317], "fy":[9.53735,6.08781,11.47165,14.91404]}, + {"t":1.92623, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/LtoPLM.traj b/src/main/deploy/choreo/LtoPLM.traj index 81e8561f..0a56c1d0 100644 --- a/src/main/deploy/choreo/LtoPLM.traj +++ b/src/main/deploy/choreo/LtoPLM.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.0301326513290403, "y":6.979412078857422, "heading":-0.9350057865774468, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"PLM.x", "val":1.0301326513290403}, "y":{"exp":"PLM.y", "val":6.979412078857422}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,45 +26,41 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.56657], + "waypoints":[0.0,1.43064], "samples":[ - {"t":0.0, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.3419, "ay":3.08339, "alpha":0.18101, "fx":[-70.22489,-71.27213,-71.74034,-70.68989], "fy":[51.46716,50.00819,49.33124,50.82369]}, - {"t":0.04352, "x":3.68755, "y":5.09226, "heading":-1.04555, "vx":-0.18894, "vy":0.13418, "omega":0.00788, "ax":-4.34165, "ay":3.08321, "alpha":0.18093, "fx":[-70.22124,-71.26798,-71.73586,-70.68592], "fy":[51.46375,50.00555,49.3289,50.82054]}, - {"t":0.08703, "x":3.67521, "y":5.10101, "heading":-1.04521, "vx":-0.37787, "vy":0.26834, "omega":0.01575, "ax":-4.34137, "ay":3.08302, "alpha":0.18084, "fx":[-70.21721,-71.26354,-71.73072,-70.68116], "fy":[51.45977,50.0022,49.32638,50.81735]}, - {"t":0.13055, "x":3.65466, "y":5.11561, "heading":-1.04452, "vx":-0.56679, "vy":0.4025, "omega":0.02362, "ax":-4.34105, "ay":3.08279, "alpha":0.18074, "fx":[-70.2127,-71.25871,-71.7248,-70.67553], "fy":[51.45513,49.99808,49.32361,50.81405]}, - {"t":0.17406, "x":3.62589, "y":5.13605, "heading":-1.0435, "vx":-0.75569, "vy":0.53665, "omega":0.03148, "ax":-4.34069, "ay":3.08253, "alpha":0.18063, "fx":[-70.20759,-71.25334,-71.71795,-70.66887], "fy":[51.44972,49.9931,49.3205,50.81051]}, - {"t":0.21758, "x":3.58889, "y":5.16232, "heading":-1.04213, "vx":-0.94458, "vy":0.67079, "omega":0.03934, "ax":-4.34026, "ay":3.08223, "alpha":0.1805, "fx":[-70.2017,-71.24725,-71.70996,-70.661], "fy":[51.44338,49.98712,49.31695,50.80659]}, - {"t":0.26109, "x":3.54368, "y":5.19442, "heading":-1.04041, "vx":-1.13345, "vy":0.80492, "omega":0.0472, "ax":-4.33976, "ay":3.08187, "alpha":0.18036, "fx":[-70.1948,-71.24017,-71.70057,-70.65169], "fy":[51.43592,49.98,49.31281,50.80211]}, - {"t":0.30461, "x":3.49025, "y":5.23237, "heading":-1.03836, "vx":-1.3223, "vy":0.93903, "omega":0.05505, "ax":-4.33916, "ay":3.08145, "alpha":0.18019, "fx":[-70.18659,-71.23175,-71.68939,-70.64059], "fy":[51.42704,49.9715,49.30787,50.79679]}, - {"t":0.34813, "x":3.4286, "y":5.27615, "heading":-1.03597, "vx":-1.51112, "vy":1.07312, "omega":0.06289, "ax":-4.33844, "ay":3.08094, "alpha":0.17999, "fx":[-70.1766,-71.22149,-71.67589,-70.62722], "fy":[51.41635,49.9613,49.30183,50.79025]}, - {"t":0.39164, "x":3.35873, "y":5.32576, "heading":-1.03323, "vx":-1.69991, "vy":1.20719, "omega":0.07072, "ax":-4.33755, "ay":3.0803, "alpha":0.17974, "fx":[-70.16417,-71.20866,-71.65931,-70.61087], "fy":[51.40327,49.94893,49.29425,50.78194]}, - {"t":0.43516, "x":3.28065, "y":5.38121, "heading":-1.03015, "vx":-1.88866, "vy":1.34123, "omega":0.07854, "ax":-4.33643, "ay":3.0795, "alpha":0.17944, "fx":[-70.14827,-71.19213,-71.63843,-70.59045], "fy":[51.38691,49.93366,49.28444,50.77102]}, - {"t":0.47867, "x":3.19436, "y":5.44249, "heading":-1.02673, "vx":-2.07737, "vy":1.47524, "omega":0.08635, "ax":-4.33495, "ay":3.07846, "alpha":0.17906, "fx":[-70.12724,-71.17007,-71.61134,-70.5642], "fy":[51.36584,49.9143,49.2713,50.7561]}, - {"t":0.52219, "x":3.09986, "y":5.5096, "heading":-1.02298, "vx":-2.26601, "vy":1.6092, "omega":0.09414, "ax":-4.33294, "ay":3.07703, "alpha":0.17855, "fx":[-70.09819,-71.13935,-71.57475,-70.52906], "fy":[51.33762,49.88879,49.25287,50.73485]}, - {"t":0.5657, "x":2.99715, "y":5.58254, "heading":-1.01888, "vx":-2.45456, "vy":1.7431, "omega":0.10191, "ax":-4.33004, "ay":3.07496, "alpha":0.17787, "fx":[-70.05562,-71.09407,-71.52245,-70.47925], "fy":[51.29767,49.85325,49.22545,50.70284]}, - {"t":0.60922, "x":2.88623, "y":5.66131, "heading":-1.01444, "vx":-2.64298, "vy":1.87691, "omega":0.10965, "ax":-4.32547, "ay":3.07172, "alpha":0.17688, "fx":[-69.98771,-71.02161,-71.44124,-70.40237], "fy":[51.23632,49.79927,49.18098,50.65067]}, - {"t":0.65274, "x":2.76713, "y":5.74589, "heading":-1.00967, "vx":-2.83121, "vy":2.01058, "omega":0.11735, "ax":-4.31726, "ay":3.06589, "alpha":0.17533, "fx":[-69.86348,-70.88931,-71.2971,-70.26609], "fy":[51.12875,49.7049,49.09815,50.55408]}, - {"t":0.69625, "x":2.63984, "y":5.83628, "heading":-1.00457, "vx":-3.01908, "vy":2.14399, "omega":0.12498, "ax":-4.29814, "ay":3.05231, "alpha":0.17256, "fx":[-69.56861,-70.57825,-70.96677,-69.95178], "fy":[50.88579,49.48919,48.89763,50.32511]}, - {"t":0.73977, "x":2.50439, "y":5.93247, "heading":-0.99913, "vx":-3.20611, "vy":2.27681, "omega":0.13249, "ax":-4.20356, "ay":2.98514, "alpha":0.16516, "fx":[-68.07659,-69.0363,-69.36653,-68.40142], "fy":[49.73011,48.42595,47.85915,49.19021]}, - {"t":0.78328, "x":2.36089, "y":6.03437, "heading":-0.99336, "vx":-3.38903, "vy":2.40671, "omega":0.13967, "ax":4.20355, "ay":-2.98515, "alpha":-0.1626, "fx":[68.08832,69.03481,69.35461,68.40263], "fy":[-49.71372,-48.42664,-47.87617,-49.18946]}, - {"t":0.8268, "x":2.2174, "y":6.13628, "heading":-0.98728, "vx":-3.20611, "vy":2.27681, "omega":0.1326, "ax":4.29814, "ay":-3.05231, "alpha":-0.17178, "fx":[69.57756,70.58832,70.95825,69.94125], "fy":[-50.87387,-49.47451,-48.9096,-50.34]}, - {"t":0.87032, "x":2.08195, "y":6.23247, "heading":-0.98151, "vx":-3.01908, "vy":2.14399, "omega":0.12512, "ax":4.31726, "ay":-3.06589, "alpha":-0.17495, "fx":[69.8746,70.90782,71.28676,70.2468], "fy":[-51.11391,-49.67827,-49.11276,-50.58109]}, - {"t":0.91383, "x":1.95466, "y":6.32286, "heading":-0.97607, "vx":-2.83121, "vy":2.01057, "omega":0.11751, "ax":4.32547, "ay":-3.07172, "alpha":-0.1767, "fx":[70.0016,71.04777,71.42845,70.37512], "fy":[-51.2177,-49.76176,-49.19914,-50.68872]}, - {"t":0.95735, "x":1.83555, "y":6.40744, "heading":-0.97096, "vx":-2.64298, "vy":1.87691, "omega":0.10982, "ax":4.33004, "ay":-3.07496, "alpha":-0.17782, "fx":[70.07234,71.1273,71.50714,70.44464], "fy":[-51.27519,-49.80566,-49.2473,-50.75111]}, - {"t":1.00086, "x":1.72464, "y":6.48621, "heading":-0.96618, "vx":-2.45456, "vy":1.7431, "omega":0.10208, "ax":4.33294, "ay":-3.07703, "alpha":-0.1786, "fx":[70.11765,71.17912,71.55698,70.48762], "fy":[-51.31139,-49.83187,-49.2783,-50.79258]}, - {"t":1.04438, "x":1.62193, "y":6.55915, "heading":-0.96173, "vx":-2.26601, "vy":1.6092, "omega":0.09431, "ax":4.33495, "ay":-3.07846, "alpha":-0.17918, "fx":[70.1493,71.21586,71.59124,70.51648], "fy":[-51.33607,-49.84878,-49.30013,-50.82255]}, - {"t":1.08789, "x":1.52743, "y":6.62626, "heading":-0.95763, "vx":-2.07737, "vy":1.47524, "omega":0.08652, "ax":4.33643, "ay":-3.0795, "alpha":-0.17963, "fx":[70.17274,71.24344,71.61615,70.53698], "fy":[-51.35383,-49.86027,-49.31645,-50.84544]}, - {"t":1.13141, "x":1.44114, "y":6.68754, "heading":-0.95387, "vx":-1.88866, "vy":1.34123, "omega":0.0787, "ax":4.33755, "ay":-3.0803, "alpha":-0.17998, "fx":[70.19086,71.26499,71.63502,70.55217], "fy":[-51.36714,-49.86838,-49.32919,-50.86363]}, - {"t":1.17493, "x":1.36306, "y":6.74299, "heading":-0.95044, "vx":-1.69991, "vy":1.20719, "omega":0.07087, "ax":4.33844, "ay":-3.08093, "alpha":-0.18027, "fx":[70.20532,71.28233,71.64978,70.56381], "fy":[-51.37745,-49.87432,-49.33944,-50.87846]}, - {"t":1.21844, "x":1.29319, "y":6.7926, "heading":-0.94736, "vx":-1.51112, "vy":1.07312, "omega":0.06302, "ax":4.33917, "ay":-3.08145, "alpha":-0.1805, "fx":[70.21712,71.2966,71.66163,70.573], "fy":[-51.38565,-49.87879,-49.34788,-50.8908]}, - {"t":1.26196, "x":1.23154, "y":6.83638, "heading":-0.94461, "vx":-1.3223, "vy":0.93903, "omega":0.05517, "ax":4.33976, "ay":-3.08187, "alpha":-0.1807, "fx":[70.22694,71.30853,71.67136,70.58043], "fy":[-51.39232,-49.88228,-49.35494,-50.9012]}, - {"t":1.30547, "x":1.17811, "y":6.87432, "heading":-0.94221, "vx":-1.13345, "vy":0.80492, "omega":0.0473, "ax":4.34026, "ay":-3.08222, "alpha":-0.18087, "fx":[70.23523,71.31862,71.6795,70.58661], "fy":[-51.39788,-49.88511,-49.36092,-50.91003]}, - {"t":1.34899, "x":1.1329, "y":6.90643, "heading":-0.94016, "vx":-0.94458, "vy":0.67079, "omega":0.03943, "ax":4.34069, "ay":-3.08253, "alpha":-0.18102, "fx":[70.24229,71.32722,71.68642,70.59185], "fy":[-51.4026,-49.8875,-49.36603,-50.91758]}, - {"t":1.3925, "x":1.0959, "y":6.9327, "heading":-0.93844, "vx":-0.75569, "vy":0.53665, "omega":0.03156, "ax":4.34105, "ay":-3.08279, "alpha":-0.18114, "fx":[70.24836,71.3346,71.69241,70.59641], "fy":[-51.40669,-49.88962,-49.3704,-50.92402]}, - {"t":1.43602, "x":1.06713, "y":6.95314, "heading":-0.93707, "vx":-0.56679, "vy":0.4025, "omega":0.02367, "ax":4.34137, "ay":-3.08301, "alpha":-0.18125, "fx":[70.2536,71.34095,71.69766,70.60047], "fy":[-51.41032,-49.89159,-49.37416,-50.92952]}, - {"t":1.47954, "x":1.04658, "y":6.96773, "heading":-0.93604, "vx":-0.37787, "vy":0.26834, "omega":0.01579, "ax":4.34165, "ay":-3.08321, "alpha":-0.18134, "fx":[70.25814,71.3464,71.70234,70.60417], "fy":[-51.41359,-49.89349,-49.37737,-50.93416]}, - {"t":1.52305, "x":1.03424, "y":6.97649, "heading":-0.93535, "vx":-0.18894, "vy":0.13418, "omega":0.00789, "ax":4.3419, "ay":-3.08339, "alpha":-0.18143, "fx":[70.26208,71.35106,71.70655,70.60761], "fy":[-51.4166,-49.8954,-49.38011,-50.93805]}, - {"t":1.56657, "x":1.03013, "y":6.97941, "heading":-0.93501, "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.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.2103, "ay":3.70008, "alpha":0.24591, "fx":[-84.15036,-85.57273,-86.20905,-84.78174], "fy":[61.92717,59.94809,59.02549,61.05606]}, + {"t":0.04471, "x":3.68645, "y":5.09304, "heading":-1.04555, "vx":-0.23294, "vy":0.16542, "omega":0.01099, "ax":-5.20996, "ay":3.69984, "alpha":0.24457, "fx":[-84.15051,-85.56514,-86.19782,-84.7783], "fy":[61.91543,59.94713,59.02965,61.04887]}, + {"t":0.08942, "x":3.67083, "y":5.10413, "heading":-1.04506, "vx":-0.46586, "vy":0.33083, "omega":0.02193, "ax":-5.20957, "ay":3.69956, "alpha":0.24304, "fx":[-84.15086,-85.55687,-86.18471,-84.77383], "fy":[61.90164,59.94531,59.03467,61.04129]}, + {"t":0.13412, "x":3.6448, "y":5.12262, "heading":-1.04408, "vx":-0.69877, "vy":0.49623, "omega":0.03279, "ax":-5.20912, "ay":3.69923, "alpha":0.24125, "fx":[-84.15141,-85.54761,-86.1693,-84.76821], "fy":[61.88536,59.94264,59.0407,61.03303]}, + {"t":0.17883, "x":3.60835, "y":5.1485, "heading":-1.04261, "vx":-0.93166, "vy":0.66161, "omega":0.04358, "ax":-5.20858, "ay":3.69885, "alpha":0.23915, "fx":[-84.15216,-85.53697,-86.15102,-84.76126], "fy":[61.86601,59.93906,59.04795,61.0237]}, + {"t":0.22354, "x":3.56149, "y":5.18177, "heading":-1.04067, "vx":-1.16452, "vy":0.82698, "omega":0.05427, "ax":-5.20794, "ay":3.69839, "alpha":0.23664, "fx":[-84.15309,-85.52435,-86.12905,-84.75278], "fy":[61.84274,59.93456,59.05668,61.01273]}, + {"t":0.26825, "x":3.50422, "y":5.22244, "heading":-1.03824, "vx":-1.39736, "vy":0.99233, "omega":0.06485, "ax":-5.20715, "ay":3.69783, "alpha":0.23358, "fx":[-84.1542,-85.50894,-86.10226,-84.74242], "fy":[61.81435,59.92905,59.06731,60.99935]}, + {"t":0.31295, "x":3.43655, "y":5.2705, "heading":-1.03534, "vx":-1.63016, "vy":1.15765, "omega":0.07529, "ax":-5.20617, "ay":3.69713, "alpha":0.22976, "fx":[-84.15547,-85.48949,-86.0689,-84.72969], "fy":[61.77906,59.92245,59.08041,60.98236]}, + {"t":0.35766, "x":3.35846, "y":5.32595, "heading":-1.03197, "vx":-1.86291, "vy":1.32294, "omega":0.08557, "ax":-5.2049, "ay":3.69623, "alpha":0.22487, "fx":[-84.15684,-85.46406,-86.02629,-84.71382], "fy":[61.73407,59.9146,59.09691,60.95993]}, + {"t":0.40237, "x":3.26998, "y":5.38879, "heading":-1.02815, "vx":-2.09561, "vy":1.48819, "omega":0.09562, "ax":-5.20322, "ay":3.69504, "alpha":0.21838, "fx":[-84.15822,-85.42945,-85.96996,-84.69349], "fy":[61.67474,59.90521,59.11829,60.92902]}, + {"t":0.44708, "x":3.17109, "y":5.45902, "heading":-1.02387, "vx":-2.32823, "vy":1.65338, "omega":0.10538, "ax":-5.20088, "ay":3.69337, "alpha":0.20935, "fx":[-84.15941,-85.37991,-85.89196,-84.66634], "fy":[61.59283,59.89374,59.14717,60.88423]}, + {"t":0.49178, "x":3.0618, "y":5.53663, "heading":-1.01916, "vx":-2.56075, "vy":1.81851, "omega":0.11474, "ax":-5.19737, "ay":3.69087, "alpha":0.19594, "fx":[-84.15988,-85.30406,-85.7766,-84.62765], "fy":[61.47205,59.87905,59.18858,60.81493]}, + {"t":0.53649, "x":2.94212, "y":5.62162, "heading":-1.01403, "vx":-2.79311, "vy":1.98352, "omega":0.1235, "ax":-5.19155, "ay":3.68673, "alpha":0.17398, "fx":[-84.15806,-85.17569,-85.58798,-84.56633], "fy":[61.2752,59.85829,59.25362,60.69689]}, + {"t":0.5812, "x":2.81206, "y":5.71398, "heading":-1.00851, "vx":-3.02521, "vy":2.14834, "omega":0.13128, "ax":-5.18007, "ay":3.67855, "alpha":0.13149, "fx":[-84.1476,-84.91795,-85.22206,-84.44925], "fy":[60.89455,59.82196,59.37257,60.46021]}, + {"t":0.62591, "x":2.67163, "y":5.8137, "heading":-1.00264, "vx":-3.2568, "vy":2.3128, "omega":0.13716, "ax":-5.14679, "ay":3.65487, "alpha":0.01422, "fx":[-84.08287,-84.16625,-84.19759,-84.11419], "fy":[59.83189,59.71588,59.66846,59.78465]}, + {"t":0.67061, "x":2.52088, "y":5.92076, "heading":-0.99651, "vx":-3.4869, "vy":2.4762, "omega":0.13779, "ax":-4.10051, "ay":2.91556, "alpha":-3.06715, "fx":[-76.21073,-58.55042,-58.85072,-74.53004], "fy":[31.08926,52.96699,61.90266,44.69638]}, + {"t":0.71532, "x":2.36089, "y":6.03437, "heading":-0.99035, "vx":-3.67023, "vy":2.60655, "omega":0.00067, "ax":4.1005, "ay":-2.91558, "alpha":3.07196, "fx":[76.18648,58.47335,58.89936,74.58212], "fy":[-31.07929,-53.07923,-61.88899,-44.60923]}, + {"t":0.76003, "x":2.20091, "y":6.14799, "heading":-0.99032, "vx":-3.4869, "vy":2.4762, "omega":0.13801, "ax":5.14679, "ay":-3.65488, "alpha":-0.01388, "fx":[84.08452,84.1663,84.19592,84.11411], "fy":[-59.8296,-59.71576,-59.67084,-59.78485]}, + {"t":0.80474, "x":2.05016, "y":6.25505, "heading":-0.98415, "vx":-3.2568, "vy":2.3128, "omega":0.13739, "ax":5.18007, "ay":-3.67855, "alpha":-0.13136, "fx":[84.15423,84.93028,85.21575,84.4366], "fy":[-60.88565,-59.80432,-59.38137,-60.47802]}, + {"t":0.84944, "x":1.90973, "y":6.35477, "heading":-0.97801, "vx":-3.02521, "vy":2.14834, "omega":0.13152, "ax":5.19155, "ay":-3.68673, "alpha":-0.17398, "fx":[84.17008,85.20028,85.5768,84.54091], "fy":[-61.25903,-59.82313,-59.2694,-60.73246]}, + {"t":0.89415, "x":1.77967, "y":6.44713, "heading":-0.97213, "vx":-2.79311, "vy":1.98351, "omega":0.12374, "ax":5.19737, "ay":-3.69087, "alpha":-0.19605, "fx":[84.17719,85.34039,85.7607,84.58993], "fy":[-61.44872,-59.82709,-59.21122,-60.86757]}, + {"t":0.93886, "x":1.65999, "y":6.53212, "heading":-0.9666, "vx":-2.56075, "vy":1.8185, "omega":0.11497, "ax":5.20088, "ay":-3.69336, "alpha":-0.20956, "fx":[84.18175,85.42728,85.87158,84.61705], "fy":[-61.56268,-59.82599,-59.17634,-60.95292]}, + {"t":0.98357, "x":1.5507, "y":6.60973, "heading":-0.96146, "vx":-2.32823, "vy":1.65338, "omega":0.1056, "ax":5.20322, "ay":-3.69504, "alpha":-0.21867, "fx":[84.18526,85.48703,85.94541,84.63345], "fy":[-61.63822,-59.82283,-59.15356,-61.01258]}, + {"t":1.02827, "x":1.45181, "y":6.67996, "heading":-0.95673, "vx":-2.09561, "vy":1.48819, "omega":0.09583, "ax":5.2049, "ay":-3.69623, "alpha":-0.22523, "fx":[84.18821,85.53098,85.9979,84.64396], "fy":[-61.69167,-59.81886,-59.13779,-61.05708]}, + {"t":1.07298, "x":1.36333, "y":6.7428, "heading":-0.95245, "vx":-1.86291, "vy":1.32294, "omega":0.08576, "ax":5.20617, "ay":-3.69713, "alpha":-0.23018, "fx":[84.19077,85.56482,86.03703,84.65097], "fy":[-61.73133,-59.81465,-59.1264,-61.09178]}, + {"t":1.11769, "x":1.28524, "y":6.79825, "heading":-0.94862, "vx":-1.63016, "vy":1.15765, "omega":0.07547, "ax":5.20715, "ay":-3.69783, "alpha":-0.23405, "fx":[84.19303,85.59175,86.06727,84.65582], "fy":[-61.76184,-59.81055,-59.11786,-61.11965]}, + {"t":1.1624, "x":1.21757, "y":6.84631, "heading":-0.94524, "vx":-1.39736, "vy":0.99233, "omega":0.065, "ax":5.20794, "ay":-3.69839, "alpha":-0.23716, "fx":[84.19501,85.61366,86.09133,84.65933], "fy":[-61.78603,-59.80674,-59.11126,-61.14251]}, + {"t":1.2071, "x":1.1603, "y":6.88698, "heading":-0.94234, "vx":-1.16452, "vy":0.82698, "omega":0.0544, "ax":5.20858, "ay":-3.69885, "alpha":-0.2397, "fx":[84.19673,85.6318,86.11095,84.66199], "fy":[-61.80569,-59.80335,-59.10597,-61.16152]}, + {"t":1.25181, "x":1.11344, "y":6.92025, "heading":-0.9399, "vx":-0.93166, "vy":0.66161, "omega":0.04368, "ax":5.20912, "ay":-3.69923, "alpha":-0.24183, "fx":[84.19819,85.64697,86.12729,84.66416], "fy":[-61.82205,-59.80044,-59.1016,-61.17745]}, + {"t":1.29652, "x":1.07699, "y":6.94613, "heading":-0.93795, "vx":-0.69877, "vy":0.49623, "omega":0.03287, "ax":5.20957, "ay":-3.69955, "alpha":-0.24362, "fx":[84.19939,85.65974,86.14116,84.66607], "fy":[-61.83594,-59.79807,-59.09785,-61.19084]}, + {"t":1.34123, "x":1.05096, "y":6.96462, "heading":-0.93648, "vx":-0.46586, "vy":0.33083, "omega":0.02198, "ax":5.20996, "ay":-3.69983, "alpha":-0.24516, "fx":[84.20033,85.67051,86.15313,84.66789], "fy":[-61.84797,-59.7963,-59.09452,-61.20207]}, + {"t":1.38593, "x":1.03534, "y":6.97571, "heading":-0.9355, "vx":-0.23294, "vy":0.16542, "omega":0.01102, "ax":5.2103, "ay":-3.70007, "alpha":-0.2465, "fx":[84.201,85.67959,86.16363,84.66974], "fy":[-61.85858,-59.79514,-59.09146,-61.21142]}, + {"t":1.43064, "x":1.03013, "y":6.97941, "heading":-0.93501, "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/PLMtoA.traj b/src/main/deploy/choreo/PLMtoA.traj index a78e6cfa..21f2f69e 100644 --- a/src/main/deploy/choreo/PLMtoA.traj +++ b/src/main/deploy/choreo/PLMtoA.traj @@ -36,94 +36,94 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.33923,2.12091], + "waypoints":[0.0,1.23783,2.01946], "samples":[ - {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.3389, "ay":-4.07969, "alpha":2.1798, "fx":[65.0028,44.72604,46.37971,62.23003], "fy":[-57.77547,-74.58153,-73.6142,-60.80957]}, - {"t":0.02232, "x":1.03096, "y":6.9784, "heading":-0.93501, "vx":0.07453, "vy":-0.09106, "omega":0.04865, "ax":3.33864, "ay":-4.07986, "alpha":2.17394, "fx":[64.96861,44.74997,46.3941,62.2087], "fy":[-57.80579,-74.56079,-73.60003,-60.82526]}, - {"t":0.04464, "x":1.03346, "y":6.97535, "heading":-0.93392, "vx":0.14905, "vy":-0.18213, "omega":0.09718, "ax":3.33836, "ay":-4.08003, "alpha":2.16779, "fx":[64.92527,44.76446,46.41919,62.19469], "fy":[-57.84579,-74.54534,-73.5788,-60.83306]}, - {"t":0.06696, "x":1.03762, "y":6.97027, "heading":-0.93175, "vx":0.22356, "vy":-0.27319, "omega":0.14556, "ax":3.33808, "ay":-4.0802, "alpha":2.16129, "fx":[64.87258,44.76984,46.45495,62.18776], "fy":[-57.89559,-74.53497,-73.55048,-60.83317]}, - {"t":0.08928, "x":1.04344, "y":6.96315, "heading":-0.9285, "vx":0.29807, "vy":-0.36427, "omega":0.1938, "ax":3.33779, "ay":-4.08038, "alpha":2.15441, "fx":[64.81027,44.76652,46.5014,62.18761], "fy":[-57.95539,-74.52941,-73.51501,-60.82585]}, - {"t":0.1116, "x":1.05092, "y":6.954, "heading":-0.92418, "vx":0.37257, "vy":-0.45534, "omega":0.24189, "ax":3.33748, "ay":-4.08056, "alpha":2.14709, "fx":[64.73802,44.75497,46.55857,62.19392], "fy":[-58.02543,-74.5283,-73.4723,-60.81138]}, - {"t":0.13392, "x":1.06007, "y":6.94282, "heading":-0.91878, "vx":0.44706, "vy":-0.54642, "omega":0.28982, "ax":3.33715, "ay":-4.08074, "alpha":2.13926, "fx":[64.65542,44.7358,46.6265,62.20629], "fy":[-58.10601,-74.53126,-73.42224,-60.7901]}, - {"t":0.15624, "x":1.07088, "y":6.92961, "heading":-0.91231, "vx":0.52155, "vy":-0.63751, "omega":0.33757, "ax":3.3368, "ay":-4.08094, "alpha":2.13086, "fx":[64.56198,44.70967,46.70525,62.22426], "fy":[-58.19749,-74.5378,-73.36469,-60.76239]}, - {"t":0.17856, "x":1.08335, "y":6.91437, "heading":-0.90477, "vx":0.59603, "vy":-0.7286, "omega":0.38513, "ax":3.33642, "ay":-4.08115, "alpha":2.12178, "fx":[64.45713,44.67738,46.79489,62.2473], "fy":[-58.3003,-74.54739,-73.2995,-60.72872]}, - {"t":0.20088, "x":1.09749, "y":6.89709, "heading":-0.89618, "vx":0.6705, "vy":-0.81969, "omega":0.43249, "ax":3.33602, "ay":-4.08137, "alpha":2.11195, "fx":[64.34023,44.63983,46.8955,62.27481], "fy":[-58.4149,-74.55938,-73.22649,-60.68959]}, - {"t":0.22321, "x":1.11329, "y":6.87777, "heading":-0.88652, "vx":0.74496, "vy":-0.91079, "omega":0.47963, "ax":3.33558, "ay":-4.08161, "alpha":2.10124, "fx":[64.21055,44.59804,47.00717,62.30607], "fy":[-58.54185,-74.57308,-73.14544,-60.64561]}, - {"t":0.24553, "x":1.13074, "y":6.85643, "heading":-0.87582, "vx":0.81941, "vy":-1.00189, "omega":0.52653, "ax":3.33511, "ay":-4.08187, "alpha":2.08953, "fx":[64.06724,44.55319,47.13002,62.34027], "fy":[-58.68173,-74.58767,-73.05612,-60.59748]}, - {"t":0.26785, "x":1.14986, "y":6.83305, "heading":-0.86407, "vx":0.89385, "vy":-1.093, "omega":0.57317, "ax":3.33459, "ay":-4.08215, "alpha":2.07666, "fx":[63.90936,44.50659,47.26413,62.37645], "fy":[-58.83519,-74.60223,-72.95826,-60.54602]}, - {"t":0.29017, "x":1.17065, "y":6.80763, "heading":-0.85127, "vx":0.96828, "vy":-1.18412, "omega":0.61952, "ax":3.33401, "ay":-4.08247, "alpha":2.06246, "fx":[63.73584,44.45975,47.40963,62.4135], "fy":[-59.00296,-74.61571,-72.85155,-60.49217]}, - {"t":0.31249, "x":1.19309, "y":6.78019, "heading":-0.83744, "vx":1.0427, "vy":-1.27524, "omega":0.66555, "ax":3.33336, "ay":-4.08282, "alpha":2.04673, "fx":[63.54549,44.41436,47.56662,62.45012], "fy":[-59.1858,-74.62694,-72.73565,-60.43708]}, - {"t":0.33481, "x":1.21719, "y":6.75071, "heading":-0.82259, "vx":1.1171, "vy":-1.36637, "omega":0.71124, "ax":3.33264, "ay":-4.08322, "alpha":2.02924, "fx":[63.33694,44.37237,47.73519,62.48476], "fy":[-59.38457,-74.63454,-72.61018,-60.38206]}, - {"t":0.35713, "x":1.24296, "y":6.71919, "heading":-0.80671, "vx":1.19149, "vy":-1.45751, "omega":0.75653, "ax":3.33182, "ay":-4.08366, "alpha":2.00971, "fx":[63.10866,44.33601,47.91546,62.51558], "fy":[-59.60017,-74.63693,-72.47469,-60.32873]}, - {"t":0.37945, "x":1.27038, "y":6.68564, "heading":-0.78983, "vx":1.26586, "vy":-1.54866, "omega":0.80139, "ax":3.33089, "ay":-4.08417, "alpha":1.98781, "fx":[62.85887,44.30786,48.1075,62.54034], "fy":[-59.83362,-74.63226,-72.32868,-60.27899]}, - {"t":0.40177, "x":1.29947, "y":6.65006, "heading":-0.77194, "vx":1.3402, "vy":-1.63982, "omega":0.84576, "ax":3.32981, "ay":-4.08474, "alpha":1.96314, "fx":[62.58555,44.29093,48.31139,62.55634], "fy":[-60.08599,-74.61836,-72.17155,-60.23519]}, - {"t":0.42409, "x":1.33021, "y":6.61244, "heading":-0.75306, "vx":1.41453, "vy":-1.73099, "omega":0.88958, "ax":3.32856, "ay":-4.0854, "alpha":1.9352, "fx":[62.28631,44.28881,48.52718,62.56019], "fy":[-60.35853,-74.59259,-72.00261,-60.20023]}, - {"t":0.44641, "x":1.36261, "y":6.57278, "heading":-0.73321, "vx":1.48882, "vy":-1.82218, "omega":0.93277, "ax":3.32709, "ay":-4.08615, "alpha":1.90337, "fx":[61.95832,44.30578,48.7549,62.54765], "fy":[-60.65258,-74.55174,-71.82099,-60.17771]}, - {"t":0.46873, "x":1.39667, "y":6.53109, "heading":-0.71239, "vx":1.56308, "vy":-1.91338, "omega":0.97526, "ax":3.32536, "ay":-4.08701, "alpha":1.86686, "fx":[61.59815,44.34711,48.99455,62.51327], "fy":[-60.96974,-74.49176,-71.62566,-60.17225]}, - {"t":0.49105, "x":1.43239, "y":6.48737, "heading":-0.69062, "vx":1.63731, "vy":-2.00461, "omega":1.01692, "ax":3.32328, "ay":-4.08801, "alpha":1.82463, "fx":[61.20153,44.41941,49.24612,62.44991], "fy":[-61.31187,-74.4075,-71.41524,-60.1898]}, - {"t":0.51337, "x":1.46976, "y":6.44161, "heading":-0.66792, "vx":1.71149, "vy":-2.09586, "omega":1.05765, "ax":3.32075, "ay":-4.08916, "alpha":1.77528, "fx":[60.76304,44.53118,49.50954,62.34791], "fy":[-61.68127,-74.29214,-71.18797,-60.23827]}, - {"t":0.53569, "x":1.50879, "y":6.39381, "heading":-0.64431, "vx":1.78561, "vy":-2.18713, "omega":1.09728, "ax":3.31763, "ay":-4.09049, "alpha":1.71687, "fx":[60.27553,44.69369,49.7847,62.19394], "fy":[-62.08085,-74.13639,-70.9414,-60.32844]}, - {"t":0.55801, "x":1.54947, "y":6.34397, "heading":-0.61982, "vx":1.85966, "vy":-2.27843, "omega":1.1356, "ax":3.31372, "ay":-4.09205, "alpha":1.64663, "fx":[59.72925,44.92241,50.07144,61.96881], "fy":[-62.51451,-73.92715,-70.67203,-60.47543]}, - {"t":0.58033, "x":1.59181, "y":6.2921, "heading":-0.59447, "vx":1.93362, "vy":-2.36977, "omega":1.17235, "ax":3.30869, "ay":-4.09388, "alpha":1.56039, "fx":[59.11033,45.23947,50.36956,61.64384], "fy":[-62.98768,-73.64509,-70.37464,-60.70129]}, - {"t":0.60265, "x":1.63579, "y":6.23818, "heading":-0.56831, "vx":2.00747, "vy":-2.46114, "omega":1.20718, "ax":3.30205, "ay":-4.09603, "alpha":1.45169, "fx":[58.39803,45.67802,50.67872,61.17384], "fy":[-63.50834,-73.26026,-70.04097,-61.03959]}, - {"t":0.62497, "x":1.68142, "y":6.18223, "heading":-0.54136, "vx":2.08118, "vy":-2.55257, "omega":1.23958, "ax":3.29292, "ay":-4.09854, "alpha":1.30979, "fx":[57.55961,46.29063,50.99842,60.48308], "fy":[-64.08886,-72.72333,-69.657,-61.54434]}, - {"t":0.64729, "x":1.72869, "y":6.12423, "heading":-0.51369, "vx":2.15468, "vy":-2.64405, "omega":1.26882, "ax":3.2797, "ay":-4.10142, "alpha":1.11555, "fx":[56.53961,47.16658,51.3277,59.43372], "fy":[-64.74968,-71.94642,-69.19695,-62.30856]}, - {"t":0.66962, "x":1.77761, "y":6.06419, "heading":-0.48537, "vx":2.22788, "vy":-2.7356, "omega":1.29372, "ax":3.25905, "ay":-4.10442, "alpha":0.83127, "fx":[55.23551,48.47152,51.66418,57.74575], "fy":[-65.52713,-70.7561,-68.60739,-63.50713]}, - {"t":0.69194, "x":1.82814, "y":6.00211, "heading":-0.4565, "vx":2.30062, "vy":-2.82721, "omega":1.31227, "ax":3.22266, "ay":-4.1062, "alpha":0.37128, "fx":[53.4339,50.55083,51.99975,54.75273], "fy":[-66.49175,-68.75678,-67.75728,-65.50855]}, - {"t":0.71426, "x":1.8803, "y":5.93798, "heading":-0.42721, "vx":2.37256, "vy":-2.91886, "omega":1.32056, "ax":3.14367, "ay":-4.09899, "alpha":-0.50725, "fx":[50.60616,54.24898,52.29217,48.42462], "fy":[-67.79681,-64.82399,-66.21945,-69.20266]}, - {"t":0.73658, "x":1.93404, "y":5.87181, "heading":-0.39773, "vx":2.44272, "vy":-3.01035, "omega":1.30924, "ax":2.88163, "ay":-4.00789, "alpha":-2.81174, "fx":[45.00901,62.02166,52.04961,29.35633], "fy":[-69.802,-54.34061,-61.42862,-76.51443]}, - {"t":0.7589, "x":1.98928, "y":5.80362, "heading":-0.36851, "vx":2.50704, "vy":-3.09981, "omega":1.24648, "ax":1.17843, "ay":-3.17375, "alpha":-7.925, "fx":[27.4124,58.84867,8.12413,-17.32501], "fy":[-69.35133,-37.14062,-32.55605,-68.49125]}, - {"t":0.78122, "x":2.04553, "y":5.73364, "heading":-0.34069, "vx":2.53335, "vy":-3.17065, "omega":1.06959, "ax":-3.54815, "ay":3.16833, "alpha":0.53555, "fx":[-57.36413,-60.35806,-58.75387,-55.54571], "fy":[53.12638,49.37092,50.40831,54.27902]}, - {"t":0.80354, "x":2.10119, "y":5.66366, "heading":-0.31681, "vx":2.45415, "vy":-3.09993, "omega":1.08154, "ax":-3.45507, "ay":3.75623, "alpha":-1.12511, "fx":[-58.26323,-50.23249,-55.22227,-62.21753], "fy":[59.52377,66.79876,63.23619,56.07024]}, - {"t":0.82586, "x":2.15511, "y":5.5954, "heading":-0.29267, "vx":2.37703, "vy":-3.01609, "omega":1.05643, "ax":-3.42, "ay":3.88316, "alpha":-1.55109, "fx":[-58.32508,-46.80845,-54.45946,-64.04876], "fy":[61.39874,70.90998,65.60283,56.0176]}, - {"t":0.84818, "x":2.20731, "y":5.52905, "heading":-0.26909, "vx":2.30069, "vy":-2.92942, "omega":1.02181, "ax":-3.40274, "ay":3.93773, "alpha":-1.74615, "fx":[-58.17711,-45.12543,-54.25684,-64.9538], "fy":[62.43272,72.7215,66.48403,55.85919]}, - {"t":0.8705, "x":2.25782, "y":5.46465, "heading":-0.24628, "vx":2.22474, "vy":-2.84152, "omega":0.98283, "ax":-3.39251, "ay":3.96801, "alpha":-1.8573, "fx":[-57.94648,-44.13333,-54.25075,-65.51375], "fy":[63.16105,73.73443,66.87975,55.70231]}, - {"t":0.89282, "x":2.30663, "y":5.40221, "heading":-0.22435, "vx":2.14902, "vy":-2.75296, "omega":0.94138, "ax":-3.38573, "ay":3.98727, "alpha":-1.92861, "fx":[-57.6784,-43.48678,-54.33187,-65.90406], "fy":[63.73972,74.37646,67.06033,55.56063]}, - {"t":0.91514, "x":2.35376, "y":5.34176, "heading":-0.20333, "vx":2.07345, "vy":-2.66396, "omega":0.89833, "ax":-3.3809, "ay":4.00062, "alpha":-1.97799, "fx":[-57.39368,-43.03876,-54.45589,-66.1965], "fy":[64.23012,74.81566,67.12929,55.43487]}, - {"t":0.93746, "x":2.39919, "y":5.28329, "heading":-0.18328, "vx":1.99799, "vy":-2.57466, "omega":0.85418, "ax":-3.37726, "ay":4.01043, "alpha":-2.01407, "fx":[-57.10358,-42.71572,-54.6014,-66.42617], "fy":[64.66109,75.13166,67.1349,55.32361]}, - {"t":0.95978, "x":2.44295, "y":5.22682, "heading":-0.16422, "vx":1.9226, "vy":-2.48515, "omega":0.80922, "ax":-3.37441, "ay":4.01795, "alpha":-2.04156, "fx":[-56.81486,-42.47654,-54.75673,-66.61241], "fy":[65.04788,75.36719,67.10275,55.22518]}, - {"t":0.9821, "x":2.48502, "y":5.17235, "heading":-0.14616, "vx":1.84729, "vy":-2.39547, "omega":0.76366, "ax":-3.37211, "ay":4.0239, "alpha":-2.06321, "fx":[-56.53196,-42.29625,-54.91502,-66.76687], "fy":[65.39928,75.54732,67.04771,55.13809]}, - {"t":1.00442, "x":2.52541, "y":5.11989, "heading":-0.12911, "vx":1.77202, "vy":-2.30565, "omega":0.7176, "ax":-3.3702, "ay":4.02874, "alpha":-2.08075, "fx":[-56.25792,-42.15869,-55.07192,-66.89709], "fy":[65.7207,75.68782,66.97907,55.06104]}, - {"t":1.02674, "x":2.56413, "y":5.06943, "heading":-0.11309, "vx":1.69679, "vy":-2.21573, "omega":0.67116, "ax":-3.3686, "ay":4.03274, "alpha":-2.09529, "fx":[-55.99496,-42.05283,-55.22458,-67.00818], "fy":[66.01568,75.79912,66.90292,54.99291]}, - {"t":1.04906, "x":2.60116, "y":5.02098, "heading":-0.09811, "vx":1.62161, "vy":-2.12571, "omega":0.62439, "ax":-3.36722, "ay":4.03612, "alpha":-2.1076, "fx":[-55.74475,-41.97086,-55.37104,-67.10382], "fy":[66.28666,75.88845,66.82345,54.93274]}, - {"t":1.07138, "x":2.63652, "y":4.97453, "heading":-0.08418, "vx":1.54645, "vy":-2.03563, "omega":0.57735, "ax":-3.36602, "ay":4.039, "alpha":-2.11821, "fx":[-55.50855,-41.90703,-55.50991,-67.18672], "fy":[66.53536,75.96102,66.74362,54.87965]}, - {"t":1.0937, "x":2.6702, "y":4.9301, "heading":-0.07129, "vx":1.47132, "vy":-1.94547, "omega":0.53007, "ax":-3.36497, "ay":4.04148, "alpha":-2.12748, "fx":[-55.28736,-41.85697,-55.6402,-67.25898], "fy":[66.76307,76.02067,66.66564,54.83289]}, - {"t":1.11603, "x":2.7022, "y":4.88769, "heading":-0.05946, "vx":1.39621, "vy":-1.85527, "omega":0.48258, "ax":-3.36404, "ay":4.04365, "alpha":-2.13569, "fx":[-55.08196,-41.81733,-55.76115,-67.32223], "fy":[66.9708,76.07032,66.59114,54.79176]}, - {"t":1.13835, "x":2.73252, "y":4.84728, "heading":-0.04869, "vx":1.32112, "vy":-1.76501, "omega":0.43491, "ax":-3.36321, "ay":4.04556, "alpha":-2.14305, "fx":[-54.893,-41.78541,-55.87222,-67.37781], "fy":[67.15931,76.11222,66.52139,54.75565]}, - {"t":1.16067, "x":2.76117, "y":4.8089, "heading":-0.03898, "vx":1.24605, "vy":-1.67471, "omega":0.38708, "ax":-3.36247, "ay":4.04724, "alpha":-2.14968, "fx":[-54.72098,-41.75909,-55.97297,-67.42678], "fy":[67.32926,76.14817,66.45736,54.72399]}, - {"t":1.18299, "x":2.78815, "y":4.77252, "heading":-0.03034, "vx":1.171, "vy":-1.58437, "omega":0.3391, "ax":-3.3618, "ay":4.04874, "alpha":-2.15572, "fx":[-54.56632,-41.73659,-56.06308,-67.47006], "fy":[67.48117,76.17961,66.39985,54.6963]}, - {"t":1.20531, "x":2.81345, "y":4.73817, "heading":-0.02277, "vx":1.09596, "vy":-1.494, "omega":0.29098, "ax":-3.36119, "ay":4.05009, "alpha":-2.16125, "fx":[-54.42935,-41.7165,-56.14228,-67.50839], "fy":[67.61547,76.2077,66.3495,54.67213]}, - {"t":1.22763, "x":2.83707, "y":4.70583, "heading":-0.01628, "vx":1.02094, "vy":-1.4036, "omega":0.24274, "ax":-3.36065, "ay":4.05129, "alpha":-2.16633, "fx":[-54.31035,-41.69761,-56.21037,-67.54241], "fy":[67.73254,76.23339,66.3068,54.65108]}, - {"t":1.24995, "x":2.85903, "y":4.67551, "heading":-0.01086, "vx":0.94593, "vy":-1.31318, "omega":0.19439, "ax":-3.36015, "ay":4.05239, "alpha":-2.17101, "fx":[-54.20956,-41.67893,-56.26717,-67.57263], "fy":[67.83269,76.25746,66.27219,54.63282]}, - {"t":1.27227, "x":2.8793, "y":4.64721, "heading":-0.00652, "vx":0.87093, "vy":-1.22273, "omega":0.14593, "ax":-3.3597, "ay":4.05337, "alpha":-2.17536, "fx":[-54.12717,-41.65965,-56.31254,-67.59952], "fy":[67.91616,76.28057,66.24602,54.61706]}, - {"t":1.29459, "x":2.8979, "y":4.62093, "heading":-0.00326, "vx":0.79594, "vy":-1.13225, "omega":0.09737, "ax":-3.35929, "ay":4.05427, "alpha":-2.17939, "fx":[-54.06334,-41.63907,-56.34636,-67.62342], "fy":[67.98319,76.30325,66.22858,54.60357]}, - {"t":1.31691, "x":2.91483, "y":4.59666, "heading":-0.00109, "vx":0.72096, "vy":-1.04176, "omega":0.04873, "ax":-3.35892, "ay":4.05509, "alpha":-2.18314, "fx":[-54.01821,-41.61664,-56.36852,-67.64464], "fy":[68.03393,76.32595,66.22011,54.59215]}, - {"t":1.33923, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.64598, "vy":-0.95125, "omega":0.0, "ax":-1.45143, "ay":0.12678, "alpha":0.0, "fx":[-23.69969,-23.70318,-23.75429,-23.75545], "fy":[2.0995,1.93388,2.15919,2.0982]}, - {"t":1.3705, "x":2.94958, "y":4.54474, "heading":0.0, "vx":0.6006, "vy":-0.94728, "omega":0.0, "ax":-1.10919, "ay":0.99685, "alpha":0.0, "fx":[-18.13301,-18.13311,-18.13337,-18.13328], "fy":[16.29675,16.29728,16.29566,16.29671]}, - {"t":1.40176, "x":2.96781, "y":4.51561, "heading":0.0, "vx":0.56592, "vy":-0.91611, "omega":0.0, "ax":-0.95827, "ay":1.14737, "alpha":0.0, "fx":[-15.66794,-15.66676,-15.66452,-15.66408], "fy":[18.75557,18.76549,18.75208,18.75602]}, - {"t":1.43303, "x":2.98504, "y":4.48753, "heading":0.0, "vx":0.53596, "vy":-0.88024, "omega":0.0, "ax":-0.89014, "ay":1.20287, "alpha":0.0, "fx":[-14.55405,-14.55298,-14.55088,-14.55052], "fy":[19.66311,19.67179,19.66036,19.66353]}, - {"t":1.4643, "x":3.00136, "y":4.46059, "heading":0.0, "vx":0.50813, "vy":-0.84263, "omega":0.0, "ax":-0.85208, "ay":1.23113, "alpha":0.0, "fx":[-13.93144,-13.93059,-13.92892,-13.92863], "fy":[20.1254,20.13215,20.12337,20.12573]}, - {"t":1.49557, "x":3.01683, "y":4.43485, "heading":0.0, "vx":0.48149, "vy":-0.80414, "omega":0.0, "ax":-0.82797, "ay":1.2481, "alpha":0.0, "fx":[-13.53697,-13.53629,-13.53496,-13.53474], "fy":[20.40299,20.40826,20.40146,20.40325]}, - {"t":1.52683, "x":3.03148, "y":4.41031, "heading":0.0, "vx":0.4556, "vy":-0.76511, "omega":0.0, "ax":-0.81142, "ay":1.25934, "alpha":0.0, "fx":[-13.26613,-13.26558,-13.2645,-13.26433], "fy":[20.58703,20.59125,20.58585,20.58725]}, - {"t":1.5581, "x":3.04533, "y":4.38701, "heading":0.0, "vx":0.43023, "vy":-0.72574, "omega":0.0, "ax":-0.7994, "ay":1.26731, "alpha":0.0, "fx":[-13.06957,-13.06911,-13.06821,-13.06807], "fy":[20.71738,20.72088,20.7164,20.71756]}, - {"t":1.58937, "x":3.05839, "y":4.36493, "heading":0.0, "vx":0.40523, "vy":-0.68611, "omega":0.0, "ax":-0.79032, "ay":1.27322, "alpha":0.0, "fx":[-12.92101,-12.9206,-12.91982,-12.91969], "fy":[20.81414,20.81719,20.81331,20.8143]}, - {"t":1.62063, "x":3.07068, "y":4.3441, "heading":0.0, "vx":0.38052, "vy":-0.6463, "omega":0.0, "ax":-0.78324, "ay":1.27777, "alpha":0.0, "fx":[-12.80516,-12.80479,-12.80409,-12.80397], "fy":[20.88857,20.89133,20.88782,20.88872]}, - {"t":1.6519, "x":3.08219, "y":4.32452, "heading":0.0, "vx":0.35603, "vy":-0.60635, "omega":0.0, "ax":-0.77758, "ay":1.28137, "alpha":0.0, "fx":[-12.71258,-12.71223,-12.71156,-12.71145], "fy":[20.94742,20.95002,20.94672,20.94756]}, - {"t":1.68317, "x":3.09295, "y":4.30619, "heading":0.0, "vx":0.33172, "vy":-0.56628, "omega":0.0, "ax":-0.77296, "ay":1.28428, "alpha":0.0, "fx":[-12.63689,-12.63681,-12.63606,-12.63612], "fy":[20.99514,20.99665,20.99511,20.99512]}, - {"t":1.71443, "x":3.10294, "y":4.28911, "heading":0.0, "vx":0.30755, "vy":-0.52613, "omega":0.0, "ax":-0.76913, "ay":1.28668, "alpha":0.0, "fx":[-12.57428,-12.57419,-12.57344,-12.57349], "fy":[21.03433,21.03589,21.03426,21.03431]}, - {"t":1.7457, "x":3.11218, "y":4.27329, "heading":0.0, "vx":0.2835, "vy":-0.4859, "omega":0.0, "ax":-0.76591, "ay":1.28868, "alpha":0.0, "fx":[-12.52159,-12.52148,-12.52073,-12.52077], "fy":[21.06712,21.06878,21.06699,21.06711]}, - {"t":1.77697, "x":3.12067, "y":4.25873, "heading":0.0, "vx":0.25955, "vy":-0.4456, "omega":0.0, "ax":-0.76316, "ay":1.29038, "alpha":0.0, "fx":[-12.47669,-12.47657,-12.4758,-12.47583], "fy":[21.09492,21.0967,21.09474,21.09492]}, - {"t":1.80824, "x":3.12841, "y":4.24542, "heading":0.0, "vx":0.23569, "vy":-0.40526, "omega":0.0, "ax":-0.76079, "ay":1.29184, "alpha":0.0, "fx":[-12.438,-12.43786,-12.43707,-12.43709], "fy":[21.11879,21.12069,21.11855,21.11879]}, - {"t":1.8395, "x":3.13541, "y":4.23338, "heading":0.0, "vx":0.2119, "vy":-0.36487, "omega":0.0, "ax":-0.75873, "ay":1.29311, "alpha":0.0, "fx":[-12.40433,-12.40416,-12.40336,-12.40337], "fy":[21.13948,21.14151,21.13919,21.1395]}, - {"t":1.87077, "x":3.14166, "y":4.22261, "heading":0.0, "vx":0.18818, "vy":-0.32443, "omega":0.0, "ax":-0.75692, "ay":1.29422, "alpha":0.0, "fx":[-12.37475,-12.37457,-12.37374,-12.37374], "fy":[21.1576,21.15977,21.15725,21.15762]}, - {"t":1.90204, "x":3.14718, "y":4.2131, "heading":0.0, "vx":0.16451, "vy":-0.28397, "omega":0.0, "ax":-0.75532, "ay":1.2952, "alpha":0.0, "fx":[-12.34856,-12.34836,-12.34751,-12.3475], "fy":[21.1736,21.17591,21.17319,21.17363]}, - {"t":1.9333, "x":3.15195, "y":4.20485, "heading":0.0, "vx":0.1409, "vy":-0.24347, "omega":0.0, "ax":-0.75389, "ay":1.29607, "alpha":0.0, "fx":[-12.3252,-12.32498,-12.32411,-12.3241], "fy":[21.18784,21.19028,21.18738,21.18788]}, - {"t":1.96457, "x":3.15599, "y":4.19787, "heading":0.0, "vx":0.11733, "vy":-0.20295, "omega":0.0, "ax":-0.7526, "ay":1.29686, "alpha":0.0, "fx":[-12.30421,-12.30398,-12.30308,-12.30306], "fy":[21.2006,21.20316,21.20009,21.20065]}, - {"t":1.99584, "x":3.15929, "y":4.19216, "heading":0.0, "vx":0.09379, "vy":-0.1624, "omega":0.0, "ax":-0.75144, "ay":1.29756, "alpha":0.0, "fx":[-12.28525,-12.285,-12.28408,-12.28405], "fy":[21.21211,21.21479,21.21155,21.21216]}, - {"t":2.02711, "x":3.16186, "y":4.18772, "heading":0.0, "vx":0.0703, "vy":-0.12183, "omega":0.0, "ax":-0.75038, "ay":1.2982, "alpha":0.0, "fx":[-12.26801,-12.26774,-12.2668,-12.26677], "fy":[21.22255,21.22534,21.22195,21.22261]}, - {"t":2.05837, "x":3.16369, "y":4.18454, "heading":0.0, "vx":0.04684, "vy":-0.08124, "omega":0.0, "ax":-0.74942, "ay":1.29878, "alpha":0.0, "fx":[-12.25226,-12.25198,-12.25102,-12.25098], "fy":[21.23207,21.23496,21.23144,21.23214]}, - {"t":2.08964, "x":3.16478, "y":4.18264, "heading":0.0, "vx":0.0234, "vy":-0.04063, "omega":0.0, "ax":-0.74853, "ay":1.29932, "alpha":0.0, "fx":[-12.23695,-12.23691,-12.23856,-12.23594], "fy":[21.24334,21.24129,21.2412,21.23976]}, - {"t":2.12091, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.03013, "y":6.97941, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.9764, "ay":-4.87648, "alpha":3.27674, "fx":[80.78593,49.78035,53.28415,76.1759], "fy":[-66.02777,-91.68882,-89.77661,-71.39119]}, + {"t":0.02063, "x":1.03098, "y":6.97837, "heading":-0.93501, "vx":0.08204, "vy":-0.1006, "omega":0.0676, "ax":3.977, "ay":-4.87745, "alpha":3.24881, "fx":[80.65613,49.9339,53.36978,76.10549], "fy":[-66.17464,-91.59677,-89.71889,-71.45773]}, + {"t":0.04126, "x":1.03352, "y":6.97526, "heading":-0.93361, "vx":0.16408, "vy":-0.20123, "omega":0.13463, "ax":3.97765, "ay":-4.87845, "alpha":3.21924, "fx":[80.50569,50.07629,53.47978,76.046], "fy":[-66.34514,-91.51004,-89.64608,-71.51191]}, + {"t":0.06189, "x":1.03775, "y":6.97007, "heading":-0.93083, "vx":0.24614, "vy":-0.30187, "omega":0.20104, "ax":3.97835, "ay":-4.87947, "alpha":3.18786, "fx":[80.33388,50.2093,53.6141,75.99637], "fy":[-66.53972,-91.42766,-89.55805,-71.55484]}, + {"t":0.08252, "x":1.04367, "y":6.96281, "heading":-0.92669, "vx":0.32822, "vy":-0.40254, "omega":0.26681, "ax":3.9791, "ay":-4.88054, "alpha":3.15441, "fx":[80.1398,50.33503,53.77275,75.95536], "fy":[-66.75903,-91.34848,-89.45459,-71.58779]}, + {"t":0.10315, "x":1.05129, "y":6.95346, "heading":-0.92118, "vx":0.41031, "vy":-0.50323, "omega":0.33188, "ax":3.97991, "ay":-4.88165, "alpha":3.11865, "fx":[79.92234,50.45595,53.95578,75.92157], "fy":[-67.00384,-91.27112,-89.33541,-71.61221]}, + {"t":0.12378, "x":1.0606, "y":6.94204, "heading":-0.91433, "vx":0.49242, "vy":-0.60394, "omega":0.39622, "ax":3.98076, "ay":-4.88282, "alpha":3.08026, "fx":[79.6802,50.57487,54.16328,75.89335], "fy":[-67.27508,-91.19396,-89.20018,-71.62977]}, + {"t":0.14441, "x":1.07161, "y":6.92854, "heading":-0.90616, "vx":0.57454, "vy":-0.70467, "omega":0.45977, "ax":3.98167, "ay":-4.88405, "alpha":3.03889, "fx":[79.41188,50.69501,54.3954,75.86877], "fy":[-67.57381,-91.11515,-89.04849,-71.64239]}, + {"t":0.16504, "x":1.08431, "y":6.91297, "heading":-0.89668, "vx":0.65669, "vy":-0.80543, "omega":0.52246, "ax":3.98263, "ay":-4.88537, "alpha":2.99415, "fx":[79.11566,50.82003,54.65233,75.84564], "fy":[-67.90121,-91.03253,-88.87981,-71.65227]}, + {"t":0.18567, "x":1.09871, "y":6.89531, "heading":-0.8859, "vx":0.73885, "vy":-0.90622, "omega":0.58423, "ax":3.98364, "ay":-4.88677, "alpha":2.94557, "fx":[78.78963,50.95407,54.93434,75.82139], "fy":[-68.25858,-90.94366,-88.69358,-71.66198]}, + {"t":0.2063, "x":1.1148, "y":6.87557, "heading":-0.87384, "vx":0.82103, "vy":-1.00704, "omega":0.645, "ax":3.98469, "ay":-4.88828, "alpha":2.89264, "fx":[78.43163,51.10177,55.24173,75.793], "fy":[-68.64727,-90.84571,-88.4891,-71.6745]}, + {"t":0.22694, "x":1.13258, "y":6.85376, "heading":-0.86054, "vx":0.90324, "vy":-1.10788, "omega":0.70468, "ax":3.98578, "ay":-4.88991, "alpha":2.83475, "fx":[78.0393,51.26843,55.5749,75.75696], "fy":[-69.06873,-90.73546,-88.26557,-71.69334]}, + {"t":0.24757, "x":1.15206, "y":6.82986, "heading":-0.846, "vx":0.98547, "vy":-1.20877, "omega":0.76316, "ax":3.98691, "ay":-4.89167, "alpha":2.77122, "fx":[77.61004,51.46004,55.93431,75.70909], "fy":[-69.52445,-90.60917,-88.02203,-71.7226]}, + {"t":0.2682, "x":1.17324, "y":6.80388, "heading":-0.83025, "vx":1.06772, "vy":-1.30968, "omega":0.82033, "ax":3.98807, "ay":-4.89358, "alpha":2.70123, "fx":[77.14097,51.68344,56.32053,75.64443], "fy":[-70.01595,-90.46248,-87.7574,-71.76717]}, + {"t":0.28883, "x":1.19612, "y":6.77582, "heading":-0.81333, "vx":1.15, "vy":-1.41064, "omega":0.87606, "ax":3.98925, "ay":-4.89565, "alpha":2.62384, "fx":[76.62898,51.94646,56.73429,75.55697], "fy":[-70.54476,-90.29028,-87.47033,-71.83289]}, + {"t":0.30946, "x":1.22069, "y":6.74568, "heading":-0.79526, "vx":1.2323, "vy":-1.51164, "omega":0.93019, "ax":3.99045, "ay":-4.89789, "alpha":2.5379, "fx":[76.07062,52.2582,57.17648,75.43942], "fy":[-71.11238,-90.08646,-87.15924,-71.92678]}, + {"t":0.33009, "x":1.24697, "y":6.71345, "heading":-0.77607, "vx":1.31462, "vy":-1.61269, "omega":0.98255, "ax":3.99163, "ay":-4.90032, "alpha":2.44203, "fx":[75.46214,52.62931,57.64827,75.28276], "fy":[-71.72032,-89.84366,-86.82216,-72.0574]}, + {"t":0.35072, "x":1.27494, "y":6.67914, "heading":-0.7558, "vx":1.39697, "vy":-1.71378, "omega":1.03293, "ax":3.9928, "ay":-4.90294, "alpha":2.33452, "fx":[74.79938,53.07242,58.15118,75.07568], "fy":[-72.37001,-89.55281,-86.45664,-72.23528]}, + {"t":0.37135, "x":1.30461, "y":6.64274, "heading":-0.73449, "vx":1.47934, "vy":-1.81493, "omega":1.08109, "ax":3.99391, "ay":-4.90575, "alpha":2.2132, "fx":[74.0777,53.60276,58.68725,74.80374], "fy":[-73.0629,-89.20257,-86.05955,-72.47356]}, + {"t":0.39198, "x":1.33598, "y":6.60425, "heading":-0.71218, "vx":1.56174, "vy":-1.91614, "omega":1.12675, "ax":3.99494, "ay":-4.90874, "alpha":2.07529, "fx":[73.29187,54.23898,59.25928,74.44819], "fy":[-73.8004,-88.77842,-85.62677,-72.78885]}, + {"t":0.41261, "x":1.36905, "y":6.56367, "heading":-0.68894, "vx":1.64416, "vy":-2.01741, "omega":1.16957, "ax":3.99581, "ay":-4.9119, "alpha":1.91713, "fx":[72.43582,55.00438,59.87122,73.98406], "fy":[-74.58399,-88.26137,-85.15275,-73.20248]}, + {"t":0.43324, "x":1.40382, "y":6.52101, "heading":-0.66481, "vx":1.72659, "vy":-2.11874, "omega":1.20912, "ax":3.99645, "ay":-4.91515, "alpha":1.73373, "fx":[71.50242,55.92868,60.52873,73.37731], "fy":[-75.41531,-87.62596,-84.62976,-73.74239]}, + {"t":0.45387, "x":1.44029, "y":6.47625, "heading":-0.63986, "vx":1.80904, "vy":-2.22015, "omega":1.24489, "ax":3.9967, "ay":-4.9184, "alpha":1.51819, "fx":[70.48293,57.05062,61.24017,72.58013], "fy":[-76.29634,-86.83702,-84.04665,-74.44587]}, + {"t":0.4745, "x":1.47846, "y":6.4294, "heading":-0.61418, "vx":1.89149, "vy":-2.32161, "omega":1.27621, "ax":3.99633, "ay":-4.92144, "alpha":1.26065, "fx":[69.36629,58.42197,62.01833,71.52309], "fy":[-77.22969,-85.84451,-83.38673,-75.36389]}, + {"t":0.49513, "x":1.51833, "y":6.38046, "heading":-0.58785, "vx":1.97394, "vy":-2.42315, "omega":1.30221, "ax":3.9949, "ay":-4.9239, "alpha":0.9465, "fx":[68.1379,60.11364,62.88343,70.10132], "fy":[-78.2191,-84.57467,-82.62373,-76.56776]}, + {"t":0.51576, "x":1.5599, "y":6.32942, "heading":-0.56099, "vx":2.05636, "vy":-2.52473, "omega":1.32174, "ax":3.99161, "ay":-4.92501, "alpha":0.55331, "fx":[66.77767,62.22529,63.86899,68.14894], "fy":[-79.27018,-82.91431,-81.71402,-78.15981]}, + {"t":0.53639, "x":1.60318, "y":6.27629, "heading":-0.53372, "vx":2.13871, "vy":-2.62633, "omega":1.33316, "ax":3.98486, "ay":-4.92325, "alpha":0.04498, "fx":[65.25663,64.90046,65.03411,65.38851], "fy":[-80.39164,-80.68163,-80.57964,-80.29015]}, + {"t":0.55702, "x":1.64815, "y":6.22106, "heading":-0.50622, "vx":2.22092, "vy":-2.7279, "omega":1.33408, "ax":3.97136, "ay":-4.91516, "alpha":-0.6398, "fx":[63.5312,68.34962,66.49153,61.32434], "fy":[-81.59722,-77.56867,-79.06741,-83.18053]}, + {"t":0.57765, "x":1.69481, "y":6.16373, "heading":-0.47869, "vx":2.30285, "vy":-2.8293, "omega":1.32088, "ax":3.94379, "ay":-4.89211, "alpha":-1.61257, "fx":[61.53293,72.88012,68.48158,54.99912], "fy":[-82.90862,-73.02199,-76.83324,-87.14316]}, + {"t":0.59828, "x":1.74316, "y":6.10432, "heading":-0.45144, "vx":2.38421, "vy":-2.93023, "omega":1.28762, "ax":3.88533, "ay":-4.82883, "alpha":-3.0934, "fx":[59.15085,78.90464,71.60471,44.41105], "fy":[-84.35878,-65.98846,-72.92945,-92.49205]}, + {"t":0.61891, "x":1.79317, "y":6.04284, "heading":-0.42488, "vx":2.46437, "vy":-3.02985, "omega":1.2238, "ax":3.76165, "ay":-4.63081, "alpha":-5.56417, "fx":[56.21279,86.73331,77.72683,25.31012], "fy":[-85.98438,-54.45234,-63.67319,-98.70977]}, + {"t":0.63954, "x":1.84482, "y":5.97935, "heading":-0.39963, "vx":2.54197, "vy":-3.12539, "omega":1.10901, "ax":3.5465, "ay":-3.84455, "alpha":-10.30838, "fx":[52.75004,94.85372,90.25765,-5.94722], "fy":[-87.60614,-36.7444,-26.33554,-100.71825]}, + {"t":0.66018, "x":1.89801, "y":5.91405, "heading":-0.37675, "vx":2.61514, "vy":-3.2047, "omega":0.89634, "ax":3.31241, "ay":-3.48325, "alpha":-11.81459, "fx":[51.38825,94.96467,83.89611,-13.64261], "fy":[-87.13812,-32.20046,-10.15448,-98.28476]}, + {"t":0.68081, "x":1.95267, "y":5.8472, "heading":-0.35826, "vx":2.68347, "vy":-3.27656, "omega":0.6526, "ax":2.77661, "ay":-3.57297, "alpha":-11.15597, "fx":[47.93868,89.98944,58.87425,-15.23342], "fy":[-85.27882,-32.99416,-22.11544,-93.2567]}, + {"t":0.70144, "x":2.00862, "y":5.77884, "heading":-0.3448, "vx":2.74076, "vy":-3.35028, "omega":0.42245, "ax":-1.78663, "ay":-1.27167, "alpha":0.21729, "fx":[-29.6168,-29.92231,-28.80339,-28.48967], "fy":[-19.98252,-21.1283,-21.59765,-20.44896]}, + {"t":0.72207, "x":2.06478, "y":5.70945, "heading":-0.33608, "vx":2.7039, "vy":-3.37651, "omega":0.42693, "ax":-3.56623, "ay":3.02473, "alpha":11.0432, "fx":[-58.61137,-93.39356,-78.03287,-3.16641], "fy":[78.26084,24.14778,1.96333,93.42242]}, + {"t":0.7427, "x":2.11981, "y":5.64044, "heading":-0.32727, "vx":2.63032, "vy":-3.31411, "omega":0.65476, "ax":-3.75446, "ay":3.45417, "alpha":10.41256, "fx":[-58.94673,-96.19777,-88.06653,-2.30177], "fy":[82.17013,28.58068,16.08428,99.04143]}, + {"t":0.76333, "x":2.17327, "y":5.5728, "heading":-0.31377, "vx":2.55287, "vy":-3.24285, "omega":0.86957, "ax":-3.99273, "ay":4.46875, "alpha":4.74026, "fx":[-62.19993,-87.31459,-75.79074,-35.78855], "fy":[80.95938,52.04019,64.59376,94.6292]}, + {"t":0.78396, "x":2.22509, "y":5.50685, "heading":-0.29583, "vx":2.4705, "vy":-3.15066, "omega":0.96737, "ax":-4.06592, "ay":4.71143, "alpha":2.00529, "fx":[-64.6463,-77.08742,-69.63187,-54.51446], "fy":[79.67859,67.41932,74.45909,86.53477]}, + {"t":0.80459, "x":2.27519, "y":5.44285, "heading":-0.27587, "vx":2.38661, "vy":-3.05346, "omega":1.00874, "ax":-4.07753, "ay":4.79006, "alpha":0.52194, "fx":[-66.12365,-69.6011,-67.28384,-63.63087], "fy":[78.89332,75.77737,77.73731,80.82518]}, + {"t":0.82522, "x":2.32356, "y":5.38088, "heading":-0.25506, "vx":2.30249, "vy":-2.95464, "omega":1.0195, "ax":-4.07266, "ay":4.82265, "alpha":-0.39213, "fx":[-66.9935,-64.28909,-66.21459,-68.82394], "fy":[78.46722,80.7414,79.22653,76.92928]}, + {"t":0.84585, "x":2.3702, "y":5.32095, "heading":-0.23402, "vx":2.21847, "vy":-2.85514, "omega":1.01141, "ax":-4.06422, "ay":4.83793, "alpha":-1.00488, "fx":[-67.47244,-60.43376,-65.71009,-72.15275], "fy":[78.28868,83.94184,79.99046,74.14289]}, + {"t":0.86648, "x":2.4151, "y":5.26307, "heading":-0.21316, "vx":2.13462, "vy":-2.75533, "omega":0.99068, "ax":-4.05571, "ay":4.84561, "alpha":-1.44086, "fx":[-67.68998,-57.55397,-65.49889,-74.46993], "fy":[78.28134,86.13946,80.39288,72.05218]}, + {"t":0.88711, "x":2.45828, "y":5.20726, "heading":-0.19272, "vx":2.05095, "vy":-2.65537, "omega":0.96096, "ax":-4.04803, "ay":4.84961, "alpha":-1.76514, "fx":[-67.72768,-55.3466,-65.45522,-76.18082], "fy":[78.39282,87.72202,80.59197,70.4205]}, + {"t":0.90774, "x":2.49973, "y":5.15351, "heading":-0.1729, "vx":1.96744, "vy":-2.55532, "omega":0.92454, "ax":-4.0413, "ay":4.85171, "alpha":-2.01479, "fx":[-67.63965,-53.61773,-65.51248,-77.5001], "fy":[78.58631,88.90394,80.66791,69.10688]}, + {"t":0.92837, "x":2.53946, "y":5.10183, "heading":-0.15382, "vx":1.88407, "vy":-2.45522, "omega":0.88297, "ax":-4.03542, "ay":4.85281, "alpha":-2.21243, "fx":[-67.46347,-52.23924,-65.6319,-78.55134], "fy":[78.83524,89.81215,80.66596,68.02319]}, + {"t":0.949, "x":2.57747, "y":5.05221, "heading":-0.13561, "vx":1.80081, "vy":-2.35511, "omega":0.83733, "ax":-4.03028, "ay":4.85334, "alpha":-2.37255, "fx":[-67.22625,-51.12363,-65.78928,-79.41049], "fy":[79.11998,90.52612,80.61379,67.11178]}, + {"t":0.96963, "x":2.61376, "y":5.00465, "heading":-0.11833, "vx":1.71767, "vy":-2.25498, "omega":0.78838, "ax":-4.02574, "ay":4.85357, "alpha":-2.50486, "fx":[-66.94827,-50.2092,-65.96862,-80.12677], "fy":[79.42572,91.09802,80.52937,66.33339]}, + {"t":0.99026, "x":2.64834, "y":4.95917, "heading":-0.10207, "vx":1.63461, "vy":-2.15485, "omega":0.73671, "ax":-4.0217, "ay":4.85362, "alpha":-2.61607, "fx":[-66.64507,-49.45119,-66.15895,-80.73351], "fy":[79.74113,91.56346,80.42499,65.66039]}, + {"t":1.01089, "x":2.68121, "y":4.91574, "heading":-0.08687, "vx":1.55164, "vy":-2.05472, "omega":0.68274, "ax":-4.01808, "ay":4.85358, "alpha":-2.71093, "fx":[-66.32888,-48.81635,-66.35241,-81.25411], "fy":[80.05742,91.94757,80.30938,65.07267]}, + {"t":1.03152, "x":2.71236, "y":4.87439, "heading":-0.07278, "vx":1.46875, "vy":-1.95459, "omega":0.62681, "ax":-4.0148, "ay":4.85348, "alpha":-2.79291, "fx":[-66.00948,-48.27938,-66.54326,-81.70552], "fy":[80.3677,92.2686,80.189,64.55522]}, + {"t":1.05215, "x":2.74181, "y":4.83509, "heading":-0.05985, "vx":1.38592, "vy":-1.85446, "omega":0.56919, "ax":-4.01183, "ay":4.85335, "alpha":-2.86457, "fx":[-65.69482,-47.82068,-66.72717,-82.10037], "fy":[80.66653,92.54014,80.0687,64.09657]}, + {"t":1.07278, "x":2.76955, "y":4.79787, "heading":-0.04811, "vx":1.30316, "vy":-1.75433, "omega":0.51009, "ax":-4.00911, "ay":4.8532, "alpha":-2.92782, "fx":[-65.39147,-47.42479,-66.90082,-82.44827], "fy":[80.9496,92.77253,79.95227,63.68778]}, + {"t":1.09342, "x":2.79558, "y":4.76271, "heading":-0.03759, "vx":1.22045, "vy":-1.6542, "omega":0.44969, "ax":-4.00662, "ay":4.85304, "alpha":-2.98412, "fx":[-65.10489,-47.07938,-67.06161,-82.75668], "fy":[81.21346,92.97379,79.84271,63.32173]}, + {"t":1.11405, "x":2.81991, "y":4.72961, "heading":-0.02831, "vx":1.13779, "vy":-1.55408, "omega":0.38813, "ax":-4.00434, "ay":4.85287, "alpha":-3.0346, "fx":[-64.83966,-46.77444,-67.20747,-83.0315], "fy":[81.45534,93.15021,79.74243,62.99269]}, + {"t":1.13468, "x":2.84253, "y":4.69859, "heading":-0.0203, "vx":1.05518, "vy":-1.45397, "omega":0.32552, "ax":-4.00223, "ay":4.85269, "alpha":-3.08014, "fx":[-64.59965,-46.50179,-67.33674,-83.27744], "fy":[81.67298,93.30684,79.65342,62.69601]}, + {"t":1.15531, "x":2.86344, "y":4.66962, "heading":-0.01359, "vx":0.97261, "vy":-1.35385, "omega":0.26198, "ax":-4.0003, "ay":4.85251, "alpha":-3.12142, "fx":[-64.3882,-46.25468,-67.44804,-83.49832], "fy":[81.86453,93.44773,79.57731,62.42783]}, + {"t":1.17594, "x":2.88266, "y":4.64272, "heading":-0.00818, "vx":0.89008, "vy":-1.25374, "omega":0.19758, "ax":-3.99853, "ay":4.85233, "alpha":-3.159, "fx":[-64.20812,-46.02749,-67.54024,-83.69726], "fy":[82.02848,93.5762,79.51547,62.18497]}, + {"t":1.19657, "x":2.90017, "y":4.61789, "heading":-0.0041, "vx":0.80759, "vy":-1.15364, "omega":0.13241, "ax":-3.9969, "ay":4.85213, "alpha":-3.19333, "fx":[-64.06189,-45.81554,-67.61236,-83.8768], "fy":[82.1635,93.69496,79.46906,61.96483]}, + {"t":1.2172, "x":2.91598, "y":4.59512, "heading":-0.00137, "vx":0.72513, "vy":-1.05354, "omega":0.06653, "ax":-3.99541, "ay":4.85193, "alpha":-3.22478, "fx":[-63.95163,-45.6149,-67.66358,-84.03902], "fy":[82.26849,93.80625,79.43908,61.76521]}, + {"t":1.23783, "x":2.93009, "y":4.57442, "heading":0.0, "vx":0.6427, "vy":-0.95344, "omega":0.0, "ax":-1.44438, "ay":0.11427, "alpha":0.0, "fx":[-23.59394,-23.59551,-23.63078,-23.6314], "fy":[1.88618,1.76338,1.93725,1.88564]}, + {"t":1.26909, "x":2.94948, "y":4.54467, "heading":0.0, "vx":0.59754, "vy":-0.94987, "omega":0.0, "ax":-1.07355, "ay":1.03449, "alpha":0.0, "fx":[-17.55015,-17.54978,-17.55137,-17.55084], "fy":[16.9125,16.91566,16.90712,16.91251]}, + {"t":1.30036, "x":2.96764, "y":4.51548, "heading":0.0, "vx":0.56398, "vy":-0.91752, "omega":0.0, "ax":-0.93319, "ay":1.16769, "alpha":0.0, "fx":[-15.25774,-15.25667,-15.25467,-15.2542], "fy":[19.08802,19.09803,19.08383,19.08841]}, + {"t":1.33162, "x":2.98481, "y":4.48736, "heading":0.0, "vx":0.5348, "vy":-0.88101, "omega":0.0, "ax":-0.87276, "ay":1.21547, "alpha":0.0, "fx":[-14.26964,-14.26872,-14.26693,-14.26655], "fy":[19.86932,19.87768,19.8661,19.86967]}, + {"t":1.36289, "x":3.00111, "y":4.46041, "heading":0.0, "vx":0.50752, "vy":-0.84301, "omega":0.0, "ax":-0.83964, "ay":1.23961, "alpha":0.0, "fx":[-13.72798,-13.72721,-13.72574,-13.72544], "fy":[20.26413,20.2708,20.26168,20.26442]}, + {"t":1.39416, "x":3.01656, "y":4.43466, "heading":0.0, "vx":0.48126, "vy":-0.80426, "omega":0.0, "ax":-0.81888, "ay":1.25405, "alpha":0.0, "fx":[-13.38833,-13.38768,-13.3865,-13.38625], "fy":[20.50047,20.50586,20.49852,20.50072]}, + {"t":1.42542, "x":3.03121, "y":4.41012, "heading":0.0, "vx":0.45566, "vy":-0.76505, "omega":0.0, "ax":-0.80472, "ay":1.26362, "alpha":0.0, "fx":[-13.15664,-13.15609,-13.15511,-13.15489], "fy":[20.65694,20.66143,20.65533,20.65716]}, + {"t":1.45669, "x":3.04506, "y":4.38682, "heading":0.0, "vx":0.4305, "vy":-0.72554, "omega":0.0, "ax":-0.79449, "ay":1.27038, "alpha":0.0, "fx":[-12.98923,-12.98875,-12.98792,-12.98773], "fy":[20.76768,20.77152,20.76631,20.76787]}, + {"t":1.48795, "x":3.05814, "y":4.36476, "heading":0.0, "vx":0.40566, "vy":-0.68582, "omega":0.0, "ax":-0.78678, "ay":1.27541, "alpha":0.0, "fx":[-12.86311,-12.86268,-12.86195,-12.86178], "fy":[20.84986,20.85324,20.84866,20.85003]}, + {"t":1.51922, "x":3.07043, "y":4.34394, "heading":0.0, "vx":0.38106, "vy":-0.64594, "omega":0.0, "ax":-0.78078, "ay":1.27927, "alpha":0.0, "fx":[-12.76501,-12.76462,-12.76395,-12.7638], "fy":[20.91305,20.91611,20.91198,20.9132]}, + {"t":1.55048, "x":3.08197, "y":4.32437, "heading":0.0, "vx":0.35665, "vy":-0.60595, "omega":0.0, "ax":-0.776, "ay":1.28232, "alpha":0.0, "fx":[-12.68676,-12.6864,-12.68578,-12.68564], "fy":[20.96301,20.96585,20.96203,20.96315]}, + {"t":1.58175, "x":3.09274, "y":4.30605, "heading":0.0, "vx":0.33239, "vy":-0.56585, "omega":0.0, "ax":-0.77211, "ay":1.28479, "alpha":0.0, "fx":[-12.62306,-12.62271,-12.62212,-12.62198], "fy":[21.0034,21.00612,21.00245,21.00354]}, + {"t":1.61301, "x":3.10275, "y":4.28899, "heading":0.0, "vx":0.30825, "vy":-0.52568, "omega":0.0, "ax":-0.76888, "ay":1.28682, "alpha":0.0, "fx":[-12.57029,-12.56996,-12.56938,-12.56926], "fy":[21.03667,21.03928,21.03576,21.0368]}, + {"t":1.64428, "x":3.11201, "y":4.27318, "heading":0.0, "vx":0.28421, "vy":-0.48545, "omega":0.0, "ax":-0.76617, "ay":1.28853, "alpha":0.0, "fx":[-12.52594,-12.52563,-12.52505,-12.52493], "fy":[21.0645,21.06705,21.06361,21.06462]}, + {"t":1.67554, "x":3.12053, "y":4.25863, "heading":0.0, "vx":0.26025, "vy":-0.44517, "omega":0.0, "ax":-0.76386, "ay":1.28997, "alpha":0.0, "fx":[-12.48818,-12.48788,-12.48731,-12.48719], "fy":[21.0881,21.09061,21.08723,21.08822]}, + {"t":1.70681, "x":3.12829, "y":4.24534, "heading":0.0, "vx":0.23637, "vy":-0.40483, "omega":0.0, "ax":-0.76187, "ay":1.29121, "alpha":0.0, "fx":[-12.45568,-12.45539,-12.45481,-12.45469], "fy":[21.10835,21.11085,21.10749,21.10846]}, + {"t":1.73808, "x":3.13531, "y":4.23332, "heading":0.0, "vx":0.21255, "vy":-0.36446, "omega":0.0, "ax":-0.76014, "ay":1.29228, "alpha":0.0, "fx":[-12.42741,-12.42712,-12.42654,-12.42643], "fy":[21.12591,21.12841,21.12506,21.12602]}, + {"t":1.76934, "x":3.14158, "y":4.22256, "heading":0.0, "vx":0.18879, "vy":-0.32406, "omega":0.0, "ax":-0.75862, "ay":1.29322, "alpha":0.0, "fx":[-12.4026,-12.40232,-12.40173,-12.40162], "fy":[21.14129,21.14379,21.14043,21.14139]}, + {"t":1.80061, "x":3.14711, "y":4.21306, "heading":0.0, "vx":0.16507, "vy":-0.28363, "omega":0.0, "ax":-0.75728, "ay":1.29405, "alpha":0.0, "fx":[-12.38065,-12.38037,-12.37977,-12.37966], "fy":[21.15486,21.15739,21.154,21.15497]}, + {"t":1.83187, "x":3.1519, "y":4.20482, "heading":0.0, "vx":0.14139, "vy":-0.24317, "omega":0.0, "ax":-0.75608, "ay":1.29479, "alpha":0.0, "fx":[-12.36108,-12.3608,-12.36019,-12.36008], "fy":[21.16694,21.16949,21.16608,21.16705]}, + {"t":1.86314, "x":3.15596, "y":4.19785, "heading":0.0, "vx":0.11775, "vy":-0.20269, "omega":0.0, "ax":-0.75501, "ay":1.29545, "alpha":0.0, "fx":[-12.34351,-12.34323,-12.34261,-12.34251], "fy":[21.17777,21.18033,21.1769,21.17787]}, + {"t":1.8944, "x":3.15927, "y":4.19215, "heading":0.0, "vx":0.09415, "vy":-0.16218, "omega":0.0, "ax":-0.75404, "ay":1.29605, "alpha":0.0, "fx":[-12.32763,-12.32736,-12.32672,-12.32662], "fy":[21.18753,21.19014,21.18664,21.18763]}, + {"t":1.92567, "x":3.16184, "y":4.18771, "heading":0.0, "vx":0.07057, "vy":-0.12166, "omega":0.0, "ax":-0.75316, "ay":1.29659, "alpha":0.0, "fx":[-12.31321,-12.31293,-12.31229,-12.31219], "fy":[21.19639,21.19903,21.19549,21.19649]}, + {"t":1.95693, "x":3.16368, "y":4.18454, "heading":0.0, "vx":0.04702, "vy":-0.08112, "omega":0.0, "ax":-0.75235, "ay":1.29709, "alpha":0.0, "fx":[-12.30004,-12.29976,-12.29911,-12.29901], "fy":[21.20447,21.20713,21.20356,21.20457]}, + {"t":1.9882, "x":3.16478, "y":4.18264, "heading":0.0, "vx":0.0235, "vy":-0.04057, "omega":0.0, "ax":-0.75161, "ay":1.29754, "alpha":0.0, "fx":[-12.28685,-12.28716,-12.28838,-12.28716], "fy":[21.21459,21.21201,21.21191,21.21083]}, + {"t":2.01946, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index 1acd9fb3..6a7a3efc 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,71 +30,68 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.17196,2.15946], + "waypoints":[0.0,1.0836,2.0711], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.01035, "ay":-3.50327, "alpha":-0.00347, "fx":[65.54678,65.57116,65.5764,65.55201], "fy":[-57.28882,-57.26093,-57.25487,-57.28278]}, - {"t":0.03781, "x":1.55302, "y":7.52089, "heading":-0.93501, "vx":0.15161, "vy":-0.13244, "omega":-0.00013, "ax":4.00973, "ay":-3.50349, "alpha":-0.00357, "fx":[65.5363,65.56136,65.56674,65.54167], "fy":[-57.29289,-57.26423,-57.25801,-57.28668]}, - {"t":0.07561, "x":1.56161, "y":7.51338, "heading":-0.93501, "vx":0.3032, "vy":-0.26489, "omega":-0.00027, "ax":4.00905, "ay":-3.50374, "alpha":-0.00367, "fx":[65.52459,65.55041,65.55594,65.53012], "fy":[-57.29744,-57.26791,-57.26151,-57.29105]}, - {"t":0.11342, "x":1.57594, "y":7.50087, "heading":-0.93502, "vx":0.45476, "vy":-0.39735, "omega":-0.0004, "ax":4.00827, "ay":-3.50402, "alpha":-0.0038, "fx":[65.5114,65.53808,65.54379,65.51711], "fy":[-57.30255,-57.27206,-57.26545,-57.29596]}, - {"t":0.15122, "x":1.596, "y":7.48334, "heading":-0.93504, "vx":0.6063, "vy":-0.52982, "omega":-0.00055, "ax":4.00739, "ay":-3.50433, "alpha":-0.00393, "fx":[65.49645,65.5241,65.53001,65.50236], "fy":[-57.30836,-57.27676,-57.26991,-57.30152]}, - {"t":0.18903, "x":1.62178, "y":7.46081, "heading":-0.93506, "vx":0.7578, "vy":-0.6623, "omega":-0.0007, "ax":4.00639, "ay":-3.50469, "alpha":-0.00409, "fx":[65.47935,65.50811,65.51425,65.48548], "fy":[-57.31499,-57.28214,-57.27502,-57.30789]}, - {"t":0.22683, "x":1.6533, "y":7.43326, "heading":-0.93508, "vx":0.90926, "vy":-0.7948, "omega":-0.00085, "ax":4.00523, "ay":-3.5051, "alpha":-0.00427, "fx":[65.4596,65.48964,65.49605,65.466], "fy":[-57.32265,-57.28835,-57.28091,-57.31523]}, - {"t":0.26464, "x":1.69053, "y":7.40071, "heading":-0.93512, "vx":1.06068, "vy":-0.92731, "omega":-0.00101, "ax":4.00387, "ay":-3.50559, "alpha":-0.00448, "fx":[65.43654,65.46807,65.47479,65.44324], "fy":[-57.33158,-57.29559,-57.28779,-57.32381]}, - {"t":0.30244, "x":1.73349, "y":7.36315, "heading":-0.93515, "vx":1.21205, "vy":-1.05984, "omega":-0.00118, "ax":4.00227, "ay":-3.50616, "alpha":-0.00474, "fx":[65.40925,65.44256,65.44965,65.41632], "fy":[-57.34215,-57.30416,-57.29593,-57.33395]}, - {"t":0.34025, "x":1.78217, "y":7.32057, "heading":-0.9352, "vx":1.36335, "vy":-1.19239, "omega":-0.00136, "ax":4.00034, "ay":-3.50685, "alpha":-0.00504, "fx":[65.37646,65.41191,65.41943,65.38397], "fy":[-57.35484,-57.31444,-57.3057,-57.34612]}, - {"t":0.37805, "x":1.83658, "y":7.27299, "heading":-0.93525, "vx":1.51459, "vy":-1.32497, "omega":-0.00155, "ax":3.99798, "ay":-3.50769, "alpha":-0.00541, "fx":[65.33633,65.37439,65.38244,65.34436], "fy":[-57.37035,-57.32702,-57.31764,-57.36101]}, - {"t":0.41586, "x":1.89669, "y":7.22039, "heading":-0.93531, "vx":1.66573, "vy":-1.45758, "omega":-0.00176, "ax":3.99503, "ay":-3.50874, "alpha":-0.00587, "fx":[65.28607,65.32741,65.33613,65.29476], "fy":[-57.38977,-57.34275,-57.33259,-57.37964]}, - {"t":0.45366, "x":1.96252, "y":7.16278, "heading":-0.93537, "vx":1.81676, "vy":-1.59023, "omega":-0.00198, "ax":3.99122, "ay":-3.51009, "alpha":-0.00647, "fx":[65.2213,65.26688,65.27643,65.23083], "fy":[-57.41475,-57.36299,-57.35182,-57.40361]}, - {"t":0.49147, "x":2.03406, "y":7.10015, "heading":-0.93545, "vx":1.96765, "vy":-1.72293, "omega":-0.00222, "ax":3.98613, "ay":-3.5119, "alpha":-0.00727, "fx":[65.13467,65.18593,65.19661,65.14532], "fy":[-57.44809,-57.39,-57.37748,-57.43562]}, - {"t":0.52927, "x":2.11129, "y":7.03251, "heading":-0.93553, "vx":2.11835, "vy":-1.8557, "omega":-0.0025, "ax":3.97897, "ay":-3.51443, "alpha":-0.00839, "fx":[65.0129,65.07217,65.0844,65.0251], "fy":[-57.49483,-57.42786,-57.41345,-57.48049]}, - {"t":0.56708, "x":2.19422, "y":6.95984, "heading":-0.93563, "vx":2.26878, "vy":-1.98856, "omega":-0.00282, "ax":3.96818, "ay":-3.51823, "alpha":-0.0101, "fx":[64.8292,64.90059,64.91512,64.84366], "fy":[-57.56505,-57.48472,-57.4675,-57.54794]}, - {"t":0.60488, "x":2.28283, "y":6.88215, "heading":-0.93573, "vx":2.41879, "vy":-2.12157, "omega":-0.0032, "ax":3.95002, "ay":-3.52458, "alpha":-0.01296, "fx":[64.52022,64.61215,64.6304,64.53837], "fy":[-57.68236,-57.57967,-57.55778,-57.66065]}, - {"t":0.64269, "x":2.37709, "y":6.79942, "heading":-0.93586, "vx":2.56813, "vy":-2.25481, "omega":-0.00369, "ax":3.91309, "ay":-3.53732, "alpha":-0.01882, "fx":[63.89181,64.02599,64.05127,63.91687], "fy":[-57.91784,-57.77011,-57.73902,-57.88712]}, - {"t":0.68049, "x":2.47698, "y":6.71165, "heading":-0.93599, "vx":2.71606, "vy":-2.38854, "omega":-0.0044, "ax":3.79716, "ay":-3.57591, "alpha":-0.03732, "fx":[61.92013,62.19055,62.2331,61.96173], "fy":[-58.63008,-58.34541,-58.28787,-58.57398]}, - {"t":0.7183, "x":2.58237, "y":6.6188, "heading":-0.93616, "vx":2.85961, "vy":-2.52373, "omega":-0.00581, "ax":-3.96381, "ay":0.86701, "alpha":-0.4768, "fx":[-65.40693,-63.98412,-64.18797,-65.62365], "fy":[11.28726,14.36507,17.03743,14.00622]}, - {"t":0.7561, "x":2.68765, "y":6.52401, "heading":-0.93638, "vx":2.70976, "vy":-2.49095, "omega":-0.02383, "ax":-4.06175, "ay":3.29442, "alpha":-0.03031, "fx":[-66.52506,-66.32492,-66.27895,-66.47865], "fy":[53.70214,53.94665,54.01241,53.76889]}, - {"t":0.79391, "x":2.78719, "y":6.43219, "heading":-0.93728, "vx":2.55621, "vy":-2.36641, "omega":-0.02498, "ax":-4.04317, "ay":3.39336, "alpha":-0.01499, "fx":[-66.1606,-66.0587,-66.03568,-66.13747], "fy":[55.39957,55.52045,55.55018,55.42954]}, - {"t":0.83172, "x":2.88094, "y":6.34515, "heading":-0.93823, "vx":2.40335, "vy":-2.23812, "omega":-0.02555, "ax":-4.03636, "ay":3.4278, "alpha":-0.00964, "fx":[-66.02731,-65.96111,-65.94621,-66.01236], "fy":[55.98979,56.06751,56.08606,56.00843]}, - {"t":0.86952, "x":2.96891, "y":6.26299, "heading":-0.93919, "vx":2.25076, "vy":-2.10853, "omega":-0.02591, "ax":-4.03282, "ay":3.44531, "alpha":-0.00691, "fx":[-65.9581,-65.91043,-65.8997,-65.94734], "fy":[56.28985,56.34552,56.35863,56.303]}, - {"t":0.90733, "x":3.05112, "y":6.18574, "heading":-0.94017, "vx":2.0983, "vy":-1.97828, "omega":-0.02617, "ax":-4.03065, "ay":3.45592, "alpha":-0.00525, "fx":[-65.91566,-65.87936,-65.87118,-65.90746], "fy":[56.47156,56.51382,56.52369,56.48147]}, - {"t":0.94513, "x":3.12757, "y":6.11342, "heading":-0.94116, "vx":1.94592, "vy":-1.84763, "omega":-0.02637, "ax":-4.02918, "ay":3.46303, "alpha":-0.00413, "fx":[-65.88696,-65.85836,-65.85189,-65.88048], "fy":[56.59342,56.62665,56.63439,56.60119]}, - {"t":0.98294, "x":3.19825, "y":6.04604, "heading":-0.94216, "vx":1.79359, "vy":-1.71671, "omega":-0.02653, "ax":-4.02812, "ay":3.46813, "alpha":-0.00332, "fx":[-65.86623,-65.8432,-65.83798,-65.861], "fy":[56.68084,56.70756,56.71378,56.68708]}, - {"t":1.02074, "x":3.26318, "y":5.98362, "heading":-0.94316, "vx":1.64131, "vy":-1.5856, "omega":-0.02665, "ax":-4.02732, "ay":3.47197, "alpha":-0.00271, "fx":[-65.85056,-65.83175,-65.82746,-65.84627], "fy":[56.74662,56.76842,56.7735,56.75171]}, - {"t":1.05855, "x":3.32235, "y":5.92616, "heading":-0.94417, "vx":1.48905, "vy":-1.45434, "omega":-0.02676, "ax":-4.02669, "ay":3.47496, "alpha":-0.00223, "fx":[-65.83829,-65.82278,-65.81923,-65.83474], "fy":[56.79791,56.81587,56.82006,56.80211]}, - {"t":1.09635, "x":3.37577, "y":5.87366, "heading":-0.94518, "vx":1.33682, "vy":-1.32296, "omega":-0.02684, "ax":-4.02619, "ay":3.47736, "alpha":-0.00185, "fx":[-65.82842,-65.81556,-65.81261,-65.82547], "fy":[56.83903,56.85391,56.85738,56.84251]}, - {"t":1.13416, "x":3.42343, "y":5.82613, "heading":-0.94619, "vx":1.18461, "vy":-1.1915, "omega":-0.02691, "ax":-4.02577, "ay":3.47933, "alpha":-0.00153, "fx":[-65.82031,-65.80964,-65.80718,-65.81785], "fy":[56.87273,56.88507,56.88796,56.87562]}, - {"t":1.17196, "x":3.46534, "y":5.78357, "heading":-0.94721, "vx":1.03242, "vy":-1.05997, "omega":-0.02697, "ax":-1.12119, "ay":0.98869, "alpha":-0.47799, "fx":[-18.74966,-16.56794,-17.89407,-20.10559], "fy":[14.25565,15.95412,18.06787,16.37541]}, - {"t":1.20382, "x":3.49766, "y":5.75031, "heading":-0.94807, "vx":0.9967, "vy":-1.02847, "omega":-0.04219, "ax":-1.10748, "ay":1.00834, "alpha":-0.44053, "fx":[-18.51306,-16.39309,-17.70841,-19.80595], "fy":[14.79348,16.31901,18.15702,16.66845]}, - {"t":1.23567, "x":3.52885, "y":5.71806, "heading":-0.94942, "vx":0.96142, "vy":-0.99635, "omega":-0.05623, "ax":-1.09517, "ay":1.02207, "alpha":-0.40256, "fx":[-18.26658,-16.41346,-17.53754,-19.39824], "fy":[15.10635,16.52858,18.30855,16.89197]}, - {"t":1.26753, "x":3.55892, "y":5.68684, "heading":-0.95121, "vx":0.92654, "vy":-0.96379, "omega":-0.06905, "ax":-1.08524, "ay":1.0329, "alpha":-0.36849, "fx":[-18.08772,-16.30832,-17.40361,-19.16707], "fy":[15.47552,16.74198,18.2814,17.04461]}, - {"t":1.29938, "x":3.58788, "y":5.65666, "heading":-0.95341, "vx":0.89197, "vy":-0.93089, "omega":-0.08079, "ax":-1.07707, "ay":1.04165, "alpha":-0.33249, "fx":[-17.9121,-16.37668,-17.29996,-18.84354], "fy":[15.70853,16.87373,18.34782,17.18612]}, - {"t":1.33124, "x":3.61575, "y":5.62754, "heading":-0.95598, "vx":0.85766, "vy":-0.89771, "omega":-0.09138, "ax":-1.07023, "ay":1.04888, "alpha":-0.30014, "fx":[-17.78277,-16.32836,-17.21505,-18.65853], "fy":[16.00129,17.0242,18.28116,17.28216]}, - {"t":1.36309, "x":3.64253, "y":5.59947, "heading":-0.95889, "vx":0.82356, "vy":-0.8643, "omega":-0.10094, "ax":-1.06441, "ay":1.05495, "alpha":-0.26564, "fx":[-17.64771,-16.41856,-17.15004,-18.38819], "fy":[16.19298,17.11522,18.29887,17.37835]}, - {"t":1.39495, "x":3.66822, "y":5.57247, "heading":-0.96211, "vx":0.78966, "vy":-0.83069, "omega":-0.1094, "ax":-1.05941, "ay":1.06011, "alpha":-0.23449, "fx":[-17.54732,-16.40684,-17.09501,-18.22838], "fy":[16.43759,17.22948,18.21412,17.44174]}, - {"t":1.4268, "x":3.69284, "y":5.54655, "heading":-0.96559, "vx":0.75591, "vy":-0.79692, "omega":-0.11687, "ax":-1.05507, "ay":1.06455, "alpha":-0.20191, "fx":[-17.43868,-16.50115,-17.05237,-18.00124], "fy":[16.60633,17.2982,18.20048,17.50861]}, - {"t":1.45866, "x":3.71638, "y":5.52171, "heading":-0.96931, "vx":0.7223, "vy":-0.76301, "omega":-0.1233, "ax":-1.05126, "ay":1.06842, "alpha":-0.17146, "fx":[-17.35555,-16.52209,-17.01833,-17.84829], "fy":[16.81335,17.3843,18.11413,17.55479]}, - {"t":1.49051, "x":3.73886, "y":5.49794, "heading":-0.97324, "vx":0.68881, "vy":-0.72898, "omega":-0.12877, "ax":-1.04789, "ay":1.07182, "alpha":-0.13874, "fx":[-17.26252,-16.61938,-16.99467,-17.64741], "fy":[16.97453,17.44389,18.07006,17.60021]}, - {"t":1.52237, "x":3.76027, "y":5.47526, "heading":-0.97734, "vx":0.65543, "vy":-0.69483, "omega":-0.13319, "ax":-1.04489, "ay":1.07482, "alpha":-0.10848, "fx":[-17.19008,-16.66033,-16.97506,-17.50239], "fy":[17.15849,17.51882,17.97744,17.6305]}, - {"t":1.55422, "x":3.78062, "y":5.45368, "heading":-0.98159, "vx":0.62215, "vy":-0.6606, "omega":-0.13664, "ax":-1.0422, "ay":1.0775, "alpha":-0.07617, "fx":[-17.10774,-16.76069,-16.96347,-17.32021], "fy":[17.313,17.5663,17.91771,17.66344]}, - {"t":1.58608, "x":3.7999, "y":5.43318, "heading":-0.98594, "vx":0.58895, "vy":-0.62627, "omega":-0.13907, "ax":-1.03978, "ay":1.07991, "alpha":-0.04585, "fx":[-17.04195,-16.81746,-16.95567,-17.17867], "fy":[17.48095,17.63182,17.82206,17.68274]}, - {"t":1.61793, "x":3.81814, "y":5.41378, "heading":-0.99037, "vx":0.55583, "vy":-0.59187, "omega":-0.14053, "ax":-1.03759, "ay":1.08207, "alpha":-0.0136, "fx":[-16.96706,-16.91891,-16.9535,-17.01084], "fy":[17.63207,17.67405,17.74808,17.70506]}, - {"t":1.64978, "x":3.83532, "y":5.39547, "heading":-0.99485, "vx":0.52278, "vy":-0.5574, "omega":-0.14096, "ax":-1.03559, "ay":1.08404, "alpha":0.01678, "fx":[-16.90558,-16.98962,-16.95551,-16.86907], "fy":[17.78448,17.73436,17.65172,17.71715]}, - {"t":1.68164, "x":3.85145, "y":5.37827, "heading":-0.99934, "vx":0.48979, "vy":-0.52287, "omega":-0.14043, "ax":-1.03377, "ay":1.08582, "alpha":0.04964, "fx":[-16.8351,-17.09191,-16.96114,-16.71233], "fy":[17.93932,17.77259,17.56336,17.72941]}, - {"t":1.71349, "x":3.86652, "y":5.36216, "heading":-1.00381, "vx":0.45686, "vy":-0.48828, "omega":-0.13885, "ax":-1.03209, "ay":1.08746, "alpha":0.08067, "fx":[-16.77396,-17.17929,-16.97106,-16.56673], "fy":[18.08835,17.81128,17.47135,17.74065]}, - {"t":1.74535, "x":3.88055, "y":5.34716, "heading":-1.00823, "vx":0.42398, "vy":-0.45364, "omega":-0.13628, "ax":-1.03055, "ay":1.08896, "alpha":0.11423, "fx":[-16.70802,-17.27898,-16.98411,-16.41917], "fy":[18.24062,17.86564,17.36424,17.73931]}, - {"t":1.7772, "x":3.89354, "y":5.33326, "heading":-1.01257, "vx":0.39115, "vy":-0.41895, "omega":-0.13264, "ax":-1.02913, "ay":1.09034, "alpha":0.14664, "fx":[-16.64615,-17.3789,-17.00306,-16.2691], "fy":[18.38563,17.90502,17.26762,17.74196]}, - {"t":1.80906, "x":3.90547, "y":5.32047, "heading":-1.0168, "vx":0.35837, "vy":-0.38422, "omega":-0.12797, "ax":-1.02781, "ay":1.09162, "alpha":0.17964, "fx":[-16.58789,-17.46555,-17.02568,-16.13186], "fy":[18.53985,17.95857,17.14954,17.73584]}, - {"t":1.84091, "x":3.91637, "y":5.30878, "heading":-1.02087, "vx":0.32563, "vy":-0.34945, "omega":-0.12224, "ax":-1.02659, "ay":1.09281, "alpha":0.21459, "fx":[-16.51961,-17.59358,-17.04809,-15.96959], "fy":[18.68145,17.99375,17.05145,17.73461]}, - {"t":1.87277, "x":3.92622, "y":5.29821, "heading":-1.02477, "vx":0.29293, "vy":-0.31463, "omega":-0.11541, "ax":-1.02544, "ay":1.09391, "alpha":0.24913, "fx":[-16.45951,-17.68398,-17.07604,-15.83672], "fy":[18.84401,18.0473,16.91974,17.72221]}, - {"t":1.90462, "x":3.93503, "y":5.28874, "heading":-1.02844, "vx":0.26026, "vy":-0.27979, "omega":-0.10747, "ax":-1.02438, "ay":1.09493, "alpha":0.28556, "fx":[-16.39207,-17.82412,-17.1058,-15.66458], "fy":[18.98242,18.0817,16.81901,17.71722]}, - {"t":1.93648, "x":3.9428, "y":5.28038, "heading":-1.03187, "vx":0.22763, "vy":-0.24491, "omega":-0.09838, "ax":-1.02338, "ay":1.09589, "alpha":0.32177, "fx":[-16.33392,-17.91888,-17.13423,-15.53431], "fy":[19.15467,18.1362,16.67328,17.69888]}, - {"t":1.96833, "x":3.94953, "y":5.27314, "heading":-1.035, "vx":0.19503, "vy":-0.21, "omega":-0.08813, "ax":-1.02245, "ay":1.09679, "alpha":0.36031, "fx":[-16.26226,-18.07235,-17.17554,-15.35002], "fy":[19.29246,18.17006,16.56831,17.69087]}, - {"t":2.00019, "x":3.95523, "y":5.267, "heading":-1.03781, "vx":0.16246, "vy":-0.17506, "omega":-0.07665, "ax":-1.02157, "ay":1.09763, "alpha":0.39872, "fx":[-16.2047,-18.17135,-17.20735,-15.2193], "fy":[19.47686,18.22614,16.40652,17.66722]}, - {"t":2.03204, "x":3.95988, "y":5.26198, "heading":-1.04025, "vx":0.12992, "vy":-0.1401, "omega":-0.06395, "ax":-1.02074, "ay":1.09842, "alpha":0.43963, "fx":[-16.12934,-18.3407,-17.25674,-15.0218], "fy":[19.61554,18.25928,16.29694,17.65673]}, - {"t":2.0639, "x":3.9635, "y":5.25808, "heading":-1.04229, "vx":0.0974, "vy":-0.10511, "omega":-0.04994, "ax":-1.01996, "ay":1.09917, "alpha":0.48067, "fx":[-16.07223,-18.44517,-17.29141,-14.88873], "fy":[19.81447,18.31712,16.11731,17.62832]}, - {"t":2.09575, "x":3.96609, "y":5.25529, "heading":-1.04388, "vx":0.06491, "vy":-0.07009, "omega":-0.03463, "ax":-1.01922, "ay":1.09987, "alpha":0.52439, "fx":[-15.99294,-18.63215,-17.34873,-14.6755], "fy":[19.95586,18.34926,16.00193,17.61615]}, - {"t":2.12761, "x":3.96764, "y":5.25361, "heading":-1.04498, "vx":0.03244, "vy":-0.03506, "omega":-0.01793, "ax":-1.01852, "ay":1.10054, "alpha":0.56277, "fx":[-15.9365,-18.74432,-17.38185,-14.54102], "fy":[20.12816,18.40158,15.84484,17.59206]}, - {"t":2.15946, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.80798, "ay":-4.20903, "alpha":-0.00468, "fx":[78.58125,78.61421,78.62125,78.58828], "fy":[-68.83246,-68.79484,-68.78671,-68.82435]}, + {"t":0.0387, "x":1.55375, "y":7.52025, "heading":-0.93501, "vx":0.18607, "vy":-0.16289, "omega":-0.00018, "ax":4.80676, "ay":-4.20974, "alpha":-0.00484, "fx":[78.56069,78.59478,78.60205,78.56796], "fy":[-68.84485,-68.80595,-68.79755,-68.83647]}, + {"t":0.0774, "x":1.56455, "y":7.51079, "heading":-0.93501, "vx":0.37209, "vy":-0.32581, "omega":-0.00037, "ax":4.80537, "ay":-4.21055, "alpha":-0.00503, "fx":[78.53726,78.57264,78.58018,78.54479], "fy":[-68.85896,-68.81861,-68.8099,-68.85028]}, + {"t":0.1161, "x":1.58255, "y":7.49503, "heading":-0.93503, "vx":0.55806, "vy":-0.48876, "omega":-0.00056, "ax":4.80378, "ay":-4.21148, "alpha":-0.00524, "fx":[78.51031,78.54718,78.55502,78.51814], "fy":[-68.87518,-68.83316,-68.8241,-68.86615]}, + {"t":0.1548, "x":1.60774, "y":7.47296, "heading":-0.93505, "vx":0.74397, "vy":-0.65174, "omega":-0.00077, "ax":4.80193, "ay":-4.21256, "alpha":-0.00548, "fx":[78.47898,78.51757,78.52577,78.48716], "fy":[-68.89403,-68.85007,-68.8406,-68.88459]}, + {"t":0.1935, "x":1.64013, "y":7.44458, "heading":-0.93508, "vx":0.9298, "vy":-0.81477, "omega":-0.00098, "ax":4.79975, "ay":-4.21383, "alpha":-0.00577, "fx":[78.4421,78.48274,78.49135,78.4507], "fy":[-68.9162,-68.86994,-68.85999,-68.90628]}, + {"t":0.2322, "x":1.67971, "y":7.4099, "heading":-0.93512, "vx":1.11555, "vy":-0.97784, "omega":-0.0012, "ax":4.79714, "ay":-4.21535, "alpha":-0.00611, "fx":[78.39808,78.44116,78.45026,78.40716], "fy":[-68.94264,-68.89366,-68.88313,-68.93215]}, + {"t":0.2709, "x":1.72647, "y":7.3689, "heading":-0.93516, "vx":1.3012, "vy":-1.14098, "omega":-0.00144, "ax":4.79398, "ay":-4.21719, "alpha":-0.00653, "fx":[78.3446,78.39065,78.40035,78.35427], "fy":[-68.97473,-68.92243,-68.91121,-68.96355]}, + {"t":0.3096, "x":1.78042, "y":7.32158, "heading":-0.93522, "vx":1.48673, "vy":-1.30418, "omega":-0.00169, "ax":4.79006, "ay":-4.21947, "alpha":-0.00705, "fx":[78.27825,78.328,78.33843,78.28866], "fy":[-69.01449,-68.95807,-68.94599,-69.00245]}, + {"t":0.3483, "x":1.84154, "y":7.26795, "heading":-0.93528, "vx":1.67211, "vy":-1.46748, "omega":-0.00196, "ax":4.78506, "ay":-4.22237, "alpha":-0.00771, "fx":[78.19376,78.24825,78.25961,78.2051], "fy":[-69.06504,-69.00337,-68.9902,-69.05192]}, + {"t":0.387, "x":1.90984, "y":7.208, "heading":-0.93536, "vx":1.85729, "vy":-1.63088, "omega":-0.00226, "ax":4.77849, "ay":-4.22617, "alpha":-0.00859, "fx":[78.08253,78.14327,78.15586,78.09507], "fy":[-69.13146,-69.06287,-69.04826,-69.11692]}, + {"t":0.4257, "x":1.98529, "y":7.14172, "heading":-0.93545, "vx":2.04222, "vy":-1.79444, "omega":-0.00259, "ax":4.76944, "ay":-4.2314, "alpha":-0.0098, "fx":[77.92948,77.99889,78.01313,77.94367], "fy":[-69.22259,-69.14447,-69.1279,-69.20611]}, + {"t":0.4644, "x":2.0679, "y":7.0691, "heading":-0.93555, "vx":2.22679, "vy":-1.95819, "omega":-0.00297, "ax":4.75621, "ay":-4.239, "alpha":-0.01158, "fx":[77.70559,77.78782,77.80445,77.72214], "fy":[-69.35534,-69.26326,-69.24386,-69.33607]}, + {"t":0.5031, "x":2.15764, "y":6.99015, "heading":-0.93566, "vx":2.41086, "vy":-2.12224, "omega":-0.00342, "ax":4.73503, "ay":-4.25111, "alpha":-0.01446, "fx":[77.34699,77.45007,77.47042,77.36723], "fy":[-69.56663,-69.4521,-69.42824,-69.54297]}, + {"t":0.5418, "x":2.25449, "y":6.90483, "heading":-0.9358, "vx":2.59411, "vy":-2.28676, "omega":-0.00398, "ax":4.69565, "ay":-4.27334, "alpha":-0.01991, "fx":[76.68011,76.82304,76.85001,76.70686], "fy":[-69.95517,-69.79863,-69.7667,-69.92361]}, + {"t":0.5805, "x":2.35839, "y":6.81313, "heading":-0.93595, "vx":2.77583, "vy":-2.45214, "omega":-0.00475, "ax":4.59718, "ay":-4.32751, "alpha":-0.03423, "fx":[75.00956,75.25933,75.30115,75.05065], "fy":[-70.90378,-70.63981,-70.58882,-70.8539]}, + {"t":0.6192, "x":2.46926, "y":6.715, "heading":-0.93613, "vx":2.95374, "vy":-2.61961, "omega":-0.00608, "ax":3.93103, "ay":-4.63649, "alpha":-0.16217, "fx":[63.592,64.88207,64.95117,63.63437], "fy":[-76.40625,-75.31934,-75.17671,-76.28894]}, + {"t":0.6579, "x":2.58652, "y":6.61014, "heading":-0.93637, "vx":3.10587, "vy":-2.79905, "omega":-0.01235, "ax":-5.01597, "ay":3.691, "alpha":-0.07671, "fx":[-82.30256,-81.83141,-81.70217,-82.17021], "fy":[59.92125,60.55389,60.75686,60.13146]}, + {"t":0.6966, "x":2.70296, "y":6.50458, "heading":-0.93685, "vx":2.91175, "vy":-2.6562, "omega":-0.01532, "ax":-4.90654, "ay":3.99817, "alpha":-0.02586, "fx":[-80.31946,-80.14722,-80.10599,-80.27793], "fy":[65.22976,65.44015,65.49485,65.28511]}, + {"t":0.7353, "x":2.81197, "y":6.40478, "heading":-0.93744, "vx":2.72187, "vy":-2.50147, "omega":-0.01632, "ax":-4.87743, "ay":4.07341, "alpha":-0.01516, "fx":[-79.80016,-79.69722,-79.67326,-79.77609], "fy":[66.51588,66.63878,66.66899,66.54631]}, + {"t":0.774, "x":2.91365, "y":6.31103, "heading":-0.93807, "vx":2.53311, "vy":-2.34383, "omega":-0.01691, "ax":-4.86389, "ay":4.10748, "alpha":-0.0104, "fx":[-79.55914,-79.48792,-79.47153,-79.5427], "fy":[67.09727,67.18143,67.20161,67.11755]}, + {"t":0.8127, "x":3.00804, "y":6.2234, "heading":-0.93873, "vx":2.34488, "vy":-2.18487, "omega":-0.01731, "ax":-4.85606, "ay":4.12691, "alpha":-0.0077, "fx":[-79.41995,-79.367,-79.35487,-79.4078], "fy":[67.42861,67.49083,67.50554,67.44338]}, + {"t":0.8514, "x":3.09515, "y":6.14193, "heading":-0.9394, "vx":2.15695, "vy":-2.02516, "omega":-0.01761, "ax":-4.85097, "ay":4.13946, "alpha":-0.00595, "fx":[-79.32929,-79.28822,-79.27884,-79.31989], "fy":[67.64268,67.69074,67.70202,67.65399]}, + {"t":0.8901, "x":3.17499, "y":6.06666, "heading":-0.94008, "vx":1.96922, "vy":-1.86496, "omega":-0.01784, "ax":-4.84738, "ay":4.14825, "alpha":-0.00473, "fx":[-79.26553,-79.23283,-79.22536,-79.25806], "fy":[67.79238,67.83055,67.83947,67.80131]}, + {"t":0.9288, "x":3.24757, "y":5.99759, "heading":-0.94077, "vx":1.78162, "vy":-1.70443, "omega":-0.01802, "ax":-4.84472, "ay":4.15473, "alpha":-0.00383, "fx":[-79.21824,-79.19174,-79.18569,-79.21218], "fy":[67.90295,67.93382,67.94101,67.91016]}, + {"t":0.9675, "x":3.31289, "y":5.93474, "heading":-0.94146, "vx":1.59413, "vy":-1.54364, "omega":-0.01817, "ax":-4.84267, "ay":4.15972, "alpha":-0.00313, "fx":[-79.18177,-79.16005,-79.15509,-79.1768], "fy":[67.98797,68.01322,68.0191,67.99385]}, + {"t":1.0062, "x":3.37096, "y":5.87812, "heading":-0.94217, "vx":1.40672, "vy":-1.38266, "omega":-0.01829, "ax":-4.84104, "ay":4.16368, "alpha":-0.00258, "fx":[-79.15278,-79.13487,-79.13077,-79.14867], "fy":[68.05537,68.07618,68.08101,68.06021]}, + {"t":1.0449, "x":3.42177, "y":5.82772, "heading":-0.94288, "vx":1.21937, "vy":-1.22152, "omega":-0.01839, "ax":-4.83971, "ay":4.16689, "alpha":-0.00213, "fx":[-79.12918,-79.11437,-79.11097,-79.12578], "fy":[68.11012,68.12731,68.13131,68.11412]}, + {"t":1.0836, "x":3.46534, "y":5.78357, "heading":-0.94359, "vx":1.03207, "vy":-1.06026, "omega":-0.01847, "ax":-1.12167, "ay":0.98746, "alpha":-0.53156, "fx":[-18.74599,-16.39292,-17.90776,-20.30203], "fy":[14.00666,15.86246,18.27649,16.42699]}, + {"t":1.11546, "x":3.49765, "y":5.7503, "heading":-0.94418, "vx":0.99634, "vy":-1.02881, "omega":-0.03541, "ax":-1.10699, "ay":1.00863, "alpha":-0.49749, "fx":[-18.52062,-16.14768,-17.6897,-20.03084], "fy":[14.53657,16.3682,18.32478,16.72719]}, + {"t":1.14731, "x":3.52882, "y":5.71804, "heading":-0.9453, "vx":0.96108, "vy":-0.99668, "omega":-0.05125, "ax":-1.09409, "ay":1.02304, "alpha":-0.44921, "fx":[-18.24272,-16.23081,-17.52394,-19.54793], "fy":[14.92772,16.48408,18.51949,16.96768]}, + {"t":1.17917, "x":3.55888, "y":5.68681, "heading":-0.94694, "vx":0.92623, "vy":-0.96409, "omega":-0.06556, "ax":-1.08388, "ay":1.03419, "alpha":-0.40791, "fx":[-18.06525,-16.09917,-17.3722,-19.34079], "fy":[15.41584,16.66253,18.43862,17.1112]}, + {"t":1.21102, "x":3.58784, "y":5.65662, "heading":-0.94902, "vx":0.8917, "vy":-0.93114, "omega":-0.07856, "ax":-1.07559, "ay":1.04307, "alpha":-0.37224, "fx":[-17.88394,-16.21279,-17.27826,-18.96042], "fy":[15.56492,16.8454,18.53815,17.26066]}, + {"t":1.24288, "x":3.6157, "y":5.62749, "heading":-0.95153, "vx":0.85744, "vy":-0.89792, "omega":-0.09041, "ax":-1.06873, "ay":1.05032, "alpha":-0.33636, "fx":[-17.76449,-16.14091,-17.18156,-18.8001], "fy":[15.91194,17.00545,18.42079,17.34449]}, + {"t":1.27473, "x":3.64247, "y":5.59942, "heading":-0.95441, "vx":0.82339, "vy":-0.86446, "omega":-0.10113, "ax":-1.06297, "ay":1.05633, "alpha":-0.29972, "fx":[-17.62361,-16.27499,-17.12677,-18.48467], "fy":[16.07238,17.09475,18.46467,17.44424]}, + {"t":1.30659, "x":3.66816, "y":5.57242, "heading":-0.95763, "vx":0.78953, "vy":-0.83081, "omega":-0.11068, "ax":-1.05805, "ay":1.06141, "alpha":-0.26536, "fx":[-17.53293,-16.24645,-17.06284,-18.34643], "fy":[16.36216,17.21514,18.3351,17.49553]}, + {"t":1.33844, "x":3.69277, "y":5.54649, "heading":-0.96115, "vx":0.75583, "vy":-0.797, "omega":-0.11913, "ax":-1.05381, "ay":1.06575, "alpha":-0.23025, "fx":[-17.42015,-16.38319,-17.03145,-18.07663], "fy":[16.50365,17.28169,18.34163,17.56474]}, + {"t":1.3703, "x":3.71631, "y":5.52164, "heading":-0.96495, "vx":0.72226, "vy":-0.76305, "omega":-0.12646, "ax":-1.05012, "ay":1.0695, "alpha":-0.19686, "fx":[-17.34595,-16.38676,-16.98967,-17.9475], "fy":[16.75295,17.3775,18.21031,17.59634]}, + {"t":1.40215, "x":3.73879, "y":5.49788, "heading":-0.96898, "vx":0.68881, "vy":-0.72898, "omega":-0.13274, "ax":-1.04687, "ay":1.07278, "alpha":-0.1627, "fx":[-17.25157,-16.52057,-16.97352,-17.71191], "fy":[16.88725,17.43145,18.18815,17.64456]}, + {"t":1.43401, "x":3.7602, "y":5.4752, "heading":-0.97321, "vx":0.65546, "vy":-0.69481, "omega":-0.13792, "ax":-1.044, "ay":1.07566, "alpha":-0.12996, "fx":[-17.18677,-16.55055,-16.94799,-17.58416], "fy":[17.10517,17.51071,18.06181,17.66246]}, + {"t":1.46586, "x":3.78055, "y":5.45361, "heading":-0.9776, "vx":0.6222, "vy":-0.66054, "omega":-0.14206, "ax":-1.04143, "ay":1.07822, "alpha":-0.096, "fx":[-17.10475,-16.67917,-16.94261,-17.37515], "fy":[17.24046,17.55825,18.01324,17.69571]}, + {"t":1.49772, "x":3.79984, "y":5.43312, "heading":-0.98212, "vx":0.58903, "vy":-0.6262, "omega":-0.14512, "ax":-1.03913, "ay":1.08051, "alpha":-0.06308, "fx":[-17.04456,-16.73228,-16.93097,-17.24328], "fy":[17.43518,17.62708,17.88974,17.70533]}, + {"t":1.52957, "x":3.81808, "y":5.41372, "heading":-0.98675, "vx":0.55593, "vy":-0.59178, "omega":-0.14713, "ax":-1.03705, "ay":1.08257, "alpha":-0.02923, "fx":[-16.97148,-16.85473,-16.93305,-17.0559], "fy":[17.57488,17.67096,17.82094,17.72506]}, + {"t":1.56143, "x":3.83526, "y":5.39542, "heading":-0.99143, "vx":0.52289, "vy":-0.55729, "omega":-0.14806, "ax":-1.03516, "ay":1.08443, "alpha":0.00417, "fx":[-16.91271,-16.92995,-16.93325,-16.91599], "fy":[17.75041,17.73185,17.70301,17.72812]}, + {"t":1.59328, "x":3.85139, "y":5.37822, "heading":-0.99615, "vx":0.48992, "vy":-0.52275, "omega":-0.14792, "ax":-1.03345, "ay":1.08612, "alpha":0.03857, "fx":[-16.84595,-17.04619,-16.94132,-16.74611], "fy":[17.89908,17.7753,17.61262,17.73677]}, + {"t":1.62514, "x":3.86647, "y":5.36212, "heading":-1.00086, "vx":0.457, "vy":-0.48815, "omega":-0.14669, "ax":-1.03188, "ay":1.08766, "alpha":0.0729, "fx":[-16.78621,-17.14317,-16.95249,-16.59493], "fy":[18.0591,17.83034,17.50014,17.73486]}, + {"t":1.65699, "x":3.88051, "y":5.34712, "heading":-1.00553, "vx":0.42413, "vy":-0.4535, "omega":-0.14437, "ax":-1.03043, "ay":1.08907, "alpha":0.1083, "fx":[-16.72404,-17.25366,-16.96537,-16.43935], "fy":[18.21991,17.8753,17.38803,17.73342]}, + {"t":1.68884, "x":3.8935, "y":5.33322, "heading":-1.01013, "vx":0.3913, "vy":-0.41881, "omega":-0.14092, "ax":-1.0291, "ay":1.09036, "alpha":0.14403, "fx":[-16.66148,-17.37278,-16.98716,-16.27402], "fy":[18.36744,17.92591,17.28038,17.72766]}, + {"t":1.7207, "x":3.90544, "y":5.32044, "heading":-1.01462, "vx":0.35852, "vy":-0.38408, "omega":-0.13633, "ax":-1.02787, "ay":1.09156, "alpha":0.18094, "fx":[-16.6027,-17.47847,-17.0041,-16.12978], "fy":[18.54325,17.97381,17.14578,17.71672]}, + {"t":1.75255, "x":3.91634, "y":5.30876, "heading":-1.01897, "vx":0.32578, "vy":-0.34931, "omega":-0.13057, "ax":-1.02673, "ay":1.09266, "alpha":0.21853, "fx":[-16.53576,-17.6206,-17.0364,-15.94773], "fy":[18.68096,18.02094,17.04206,17.70792]}, + {"t":1.78441, "x":3.92619, "y":5.29818, "heading":-1.02312, "vx":0.29307, "vy":-0.3145, "omega":-0.12361, "ax":-1.02567, "ay":1.09369, "alpha":0.25743, "fx":[-16.47938,-17.72253,-17.05673,-15.81252], "fy":[18.87457,18.07296,16.88368,17.68778]}, + {"t":1.81626, "x":3.93501, "y":5.28872, "heading":-1.02706, "vx":0.2604, "vy":-0.27966, "omega":-0.11541, "ax":-1.02468, "ay":1.09464, "alpha":0.29739, "fx":[-16.40692,-17.88915,-17.09975,-15.61071], "fy":[19.00466,18.11731,16.78274,17.67671]}, + {"t":1.84812, "x":3.94278, "y":5.28037, "heading":-1.03074, "vx":0.22776, "vy":-0.24479, "omega":-0.10594, "ax":-1.02376, "ay":1.09554, "alpha":0.33884, "fx":[-16.35227,-17.98867,-17.12304,-15.48217], "fy":[19.21919,18.17428,16.59869,17.64751]}, + {"t":1.87997, "x":3.94952, "y":5.27312, "heading":-1.03411, "vx":0.19515, "vy":-0.20989, "omega":-0.09514, "ax":-1.0229, "ay":1.09637, "alpha":0.38176, "fx":[-16.27324,-18.18178,-17.17701,-15.25757], "fy":[19.34435,18.21593,16.49909,17.63475]}, + {"t":1.91183, "x":3.95522, "y":5.26699, "heading":-1.03714, "vx":0.16256, "vy":-0.17497, "omega":-0.08298, "ax":-1.02208, "ay":1.09715, "alpha":0.42634, "fx":[-16.2199,-18.28046,-17.20287,-15.1333], "fy":[19.5826,18.27879,16.28705,17.59669]}, + {"t":1.94368, "x":3.95988, "y":5.26198, "heading":-1.03979, "vx":0.13, "vy":-0.14002, "omega":-0.0694, "ax":-1.02132, "ay":1.09788, "alpha":0.47285, "fx":[-16.13353,-18.50248,-17.26807,-14.88254], "fy":[19.70521,18.31807,16.187,17.58275]}, + {"t":1.97554, "x":3.9635, "y":5.25807, "heading":-1.042, "vx":0.09747, "vy":-0.10505, "omega":-0.05434, "ax":-1.0206, "ay":1.09857, "alpha":0.52121, "fx":[-16.08128,-18.60208,-17.29618,-14.76008], "fy":[19.97072,18.38707,15.94411,17.53619]}, + {"t":2.00739, "x":3.96609, "y":5.25529, "heading":-1.04373, "vx":0.06496, "vy":-0.07005, "omega":-0.03774, "ax":-1.01992, "ay":1.09922, "alpha":0.57188, "fx":[-15.9872,-18.85635,-17.37256,-14.47915], "fy":[20.09215,18.42297,15.84269,17.52275]}, + {"t":2.03925, "x":3.96764, "y":5.25361, "heading":-1.04493, "vx":0.03247, "vy":-0.03503, "omega":-0.01952, "ax":-1.01928, "ay":1.09983, "alpha":0.61272, "fx":[-15.93795,-18.95629,-17.40063,-14.35846], "fy":[20.30103,18.47601,15.65564,17.48797]}, + {"t":2.0711, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index 063b9572..07abd56e 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -30,71 +30,69 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.12595,2.15883], + "waypoints":[0.0,1.04206,2.07495], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.60902, "ay":-3.91522, "alpha":-0.00314, "fx":[58.98668,59.01173,59.01439,58.98933], "fy":[-64.01915,-63.99606,-63.99356,-64.01665]}, - {"t":0.03753, "x":1.55269, "y":7.52064, "heading":-0.93501, "vx":0.13545, "vy":-0.14694, "omega":-0.00012, "ax":3.60849, "ay":-3.91523, "alpha":-0.00323, "fx":[58.97763,59.00343,59.00617,58.98036], "fy":[-64.0198,-63.99603,-63.99345,-64.01724]}, - {"t":0.07506, "x":1.56032, "y":7.51237, "heading":-0.93501, "vx":0.27088, "vy":-0.29389, "omega":-0.00024, "ax":3.60789, "ay":-3.91525, "alpha":-0.00334, "fx":[58.96746,58.99411,58.99693,58.97027], "fy":[-64.02055,-63.996,-63.99334,-64.01789]}, - {"t":0.11259, "x":1.57303, "y":7.49858, "heading":-0.93502, "vx":0.40629, "vy":-0.44084, "omega":-0.00036, "ax":3.60722, "ay":-3.91528, "alpha":-0.00346, "fx":[58.95594,58.98354,58.98646,58.95885], "fy":[-64.02139,-63.99596,-63.9932,-64.01864]}, - {"t":0.15013, "x":1.59082, "y":7.47928, "heading":-0.93503, "vx":0.54168, "vy":-0.58778, "omega":-0.00049, "ax":3.60645, "ay":-3.9153, "alpha":-0.0036, "fx":[58.94278,58.97148,58.97451,58.9458], "fy":[-64.02234,-63.99592,-63.99305,-64.01949]}, - {"t":0.18766, "x":1.61369, "y":7.45446, "heading":-0.93505, "vx":0.67704, "vy":-0.73473, "omega":-0.00063, "ax":3.60557, "ay":-3.91533, "alpha":-0.00375, "fx":[58.9276,58.95756,58.96072,58.93075], "fy":[-64.02345,-63.99587,-63.99288,-64.02047]}, - {"t":0.22519, "x":1.64164, "y":7.42413, "heading":-0.93508, "vx":0.81236, "vy":-0.88168, "omega":-0.00077, "ax":3.60453, "ay":-3.91536, "alpha":-0.00394, "fx":[58.90991,58.94134,58.94465,58.91321], "fy":[-64.02473,-63.99581,-63.99267,-64.02161]}, - {"t":0.26272, "x":1.67466, "y":7.38828, "heading":-0.9351, "vx":0.94764, "vy":-1.02863, "omega":-0.00092, "ax":3.60332, "ay":-3.9154, "alpha":-0.00416, "fx":[58.88903,58.92219,58.92567,58.89249], "fy":[-64.02625,-63.99574,-63.99243,-64.02295]}, - {"t":0.30025, "x":1.71277, "y":7.34691, "heading":-0.93514, "vx":1.08288, "vy":-1.17558, "omega":-0.00107, "ax":3.60185, "ay":-3.91545, "alpha":-0.00442, "fx":[58.86399,58.89924,58.90293,58.86766], "fy":[-64.02807,-63.99566,-63.99214,-64.02456]}, - {"t":0.33778, "x":1.75595, "y":7.30004, "heading":-0.93518, "vx":1.21806, "vy":-1.32253, "omega":-0.00124, "ax":3.60007, "ay":-3.9155, "alpha":-0.00473, "fx":[58.83343,58.87123,58.87517,58.83735], "fy":[-64.03028,-63.99555,-63.99177,-64.02653]}, - {"t":0.37532, "x":1.8042, "y":7.24764, "heading":-0.93523, "vx":1.35318, "vy":-1.46949, "omega":-0.00142, "ax":3.59784, "ay":-3.91557, "alpha":-0.00513, "fx":[58.7953,58.83627,58.84052,58.79953], "fy":[-64.03304,-63.99541,-63.99132,-64.02897]}, - {"t":0.41285, "x":1.85752, "y":7.18973, "heading":-0.93528, "vx":1.48821, "vy":-1.61644, "omega":-0.00161, "ax":3.59499, "ay":-3.91566, "alpha":-0.00564, "fx":[58.74639,58.79144,58.79608,58.751], "fy":[-64.03656,-63.99523,-63.99073,-64.03209]}, - {"t":0.45038, "x":1.91591, "y":7.12631, "heading":-0.93534, "vx":1.62314, "vy":-1.76341, "omega":-0.00182, "ax":3.59119, "ay":-3.91578, "alpha":-0.00632, "fx":[58.68136,58.73184,58.737,58.68649], "fy":[-64.04123,-63.99497,-63.98993,-64.03623]}, - {"t":0.48791, "x":1.97935, "y":7.05736, "heading":-0.93541, "vx":1.75792, "vy":-1.91037, "omega":-0.00206, "ax":3.5859, "ay":-3.91595, "alpha":-0.00727, "fx":[58.5907,58.64876,58.65463,58.59652], "fy":[-64.0477,-63.99458,-63.98879,-64.04196]}, - {"t":0.52544, "x":2.04786, "y":6.98291, "heading":-0.93548, "vx":1.89251, "vy":-2.05734, "omega":-0.00233, "ax":3.57801, "ay":-3.91618, "alpha":-0.00869, "fx":[58.45554,58.52492,58.5318,58.46236], "fy":[-64.05726,-63.99394,-63.98703,-64.05042]}, - {"t":0.56297, "x":2.12141, "y":6.90293, "heading":-0.93557, "vx":2.02679, "vy":-2.20432, "omega":-0.00266, "ax":3.56499, "ay":-3.91657, "alpha":-0.01103, "fx":[58.23248,58.3206,58.32908,58.24085], "fy":[-64.07282,-63.99273,-63.98397,-64.06418]}, - {"t":0.60051, "x":2.19999, "y":6.81744, "heading":-0.93567, "vx":2.16059, "vy":-2.35132, "omega":-0.00307, "ax":3.53942, "ay":-3.91728, "alpha":-0.01564, "fx":[57.79457,57.91966,57.93094,57.80564], "fy":[-64.10253,-63.98975,-63.9774,-64.09042]}, - {"t":0.63804, "x":2.28357, "y":6.72644, "heading":-0.93579, "vx":2.29343, "vy":-2.49834, "omega":-0.00366, "ax":3.46629, "ay":-3.91902, "alpha":-0.02895, "fx":[56.543,56.77486,56.79175,56.55914], "fy":[-64.18178,-63.97745,-63.95494,-64.16005]}, - {"t":0.67557, "x":2.37209, "y":6.62991, "heading":-0.93592, "vx":2.42353, "vy":-2.64543, "omega":-0.00475, "ax":1.76513, "ay":-3.68998, "alpha":-0.42347, "fx":[27.69958,30.89174,30.08457,26.75012], "fy":[-61.47158,-59.72798,-59.14226,-60.95471]}, - {"t":0.7131, "x":2.46429, "y":6.52802, "heading":-0.9361, "vx":2.48978, "vy":-2.78392, "omega":-0.02064, "ax":-3.63676, "ay":3.74027, "alpha":-0.031, "fx":[-59.58607,-59.34915,-59.3225,-59.55867], "fy":[61.01377,61.24289,61.27832,61.05015]}, - {"t":0.75063, "x":2.55517, "y":6.42617, "heading":-0.93688, "vx":2.35328, "vy":-2.64354, "omega":-0.0218, "ax":-3.62752, "ay":3.83057, "alpha":-0.01432, "fx":[-59.36536,-59.25343,-59.24092,-59.35268], "fy":[62.56256,62.66831,62.68228,62.57674]}, - {"t":0.78816, "x":2.64094, "y":6.32965, "heading":-0.9377, "vx":2.21714, "vy":-2.49977, "omega":-0.02234, "ax":-3.62444, "ay":3.85928, "alpha":-0.00902, "fx":[-59.29213,-59.22117,-59.21321,-59.28411], "fy":[63.05434,63.12089,63.12924,63.06278]}, - {"t":0.8257, "x":2.7216, "y":6.23855, "heading":-0.93853, "vx":2.08111, "vy":-2.35493, "omega":-0.02268, "ax":-3.62289, "ay":3.87339, "alpha":-0.0064, "fx":[-59.25541,-59.20491,-59.19922,-59.24969], "fy":[63.29602,63.34321,63.349,63.30185]}, - {"t":0.86323, "x":2.79716, "y":6.1529, "heading":-0.93938, "vx":1.94513, "vy":-2.20955, "omega":-0.02292, "ax":-3.62195, "ay":3.88178, "alpha":-0.00483, "fx":[-59.2333,-59.19509,-59.19076,-59.22895], "fy":[63.43974,63.47536,63.47969,63.44409]}, - {"t":0.90076, "x":2.86761, "y":6.0727, "heading":-0.94025, "vx":1.8092, "vy":-2.06386, "omega":-0.0231, "ax":-3.62133, "ay":3.88735, "alpha":-0.00379, "fx":[-59.21851,-59.18851,-59.18509,-59.21507], "fy":[63.53504,63.56296,63.56633,63.53843]}, - {"t":0.93829, "x":2.93296, "y":5.99798, "heading":-0.94111, "vx":1.67328, "vy":-1.91797, "omega":-0.02324, "ax":-3.62088, "ay":3.89131, "alpha":-0.00304, "fx":[-59.2079,-59.18379,-59.18102,-59.20512], "fy":[63.60288,63.62529,63.62799,63.60559]}, - {"t":0.97582, "x":2.99321, "y":5.92874, "heading":-0.94198, "vx":1.53738, "vy":-1.77192, "omega":-0.02336, "ax":-3.62054, "ay":3.89427, "alpha":-0.00248, "fx":[-59.19991,-59.18024,-59.17796,-59.19763], "fy":[63.65362,63.6719,63.67411,63.65584]}, - {"t":1.01335, "x":3.04836, "y":5.86498, "heading":-0.94286, "vx":1.4015, "vy":-1.62576, "omega":-0.02345, "ax":-3.62028, "ay":3.89657, "alpha":-0.00204, "fx":[-59.19368,-59.17746,-59.17557,-59.19179], "fy":[63.69302,63.70808,63.70991,63.69485]}, - {"t":1.05088, "x":3.09841, "y":5.8067, "heading":-0.94374, "vx":1.26562, "vy":-1.47952, "omega":-0.02353, "ax":-3.62006, "ay":3.8984, "alpha":-0.00169, "fx":[-59.18868,-59.17523,-59.17365,-59.1871], "fy":[63.72449,63.73698,63.7385,63.72601]}, - {"t":1.08842, "x":3.14336, "y":5.75392, "heading":-0.94462, "vx":1.12976, "vy":-1.3332, "omega":-0.02359, "ax":-3.61989, "ay":3.89991, "alpha":-0.00141, "fx":[-59.18458,-59.1734,-59.17208,-59.18326], "fy":[63.75022,63.76059,63.76186,63.75148]}, - {"t":1.12595, "x":3.18322, "y":5.70663, "heading":-0.94551, "vx":0.9939, "vy":-1.18683, "omega":-0.02364, "ax":-1.01074, "ay":1.1015, "alpha":-0.45447, "fx":[-16.93054,-14.84186,-16.10011,-18.2223], "fy":[16.20231,17.81442,19.8088,18.20427]}, - {"t":1.15823, "x":3.21477, "y":5.66889, "heading":-0.94627, "vx":0.96127, "vy":-1.15128, "omega":-0.03831, "ax":-1.00248, "ay":1.11283, "alpha":-0.42176, "fx":[-16.78285,-14.73242,-16.00498,-18.0345], "fy":[16.59019,18.03919,19.77574,18.36566]}, - {"t":1.1905, "x":3.24528, "y":5.63231, "heading":-0.94751, "vx":0.92892, "vy":-1.11536, "omega":-0.05193, "ax":-0.99464, "ay":1.12017, "alpha":-0.38534, "fx":[-16.61457,-14.82445,-15.90189,-17.70117], "fy":[16.7885,18.14629,19.83346,18.48233]}, - {"t":1.22278, "x":3.27474, "y":5.5969, "heading":-0.94919, "vx":0.89681, "vy":-1.0792, "omega":-0.06436, "ax":-0.98831, "ay":1.12602, "alpha":-0.35524, "fx":[-16.49318,-14.76331,-15.82844,-17.54323], "fy":[17.06003,18.27302,19.74095,18.55896]}, - {"t":1.25506, "x":3.30317, "y":5.56265, "heading":-0.95126, "vx":0.86491, "vy":-1.04286, "omega":-0.07583, "ax":-0.9831, "ay":1.13079, "alpha":-0.32008, "fx":[-16.36959,-14.88097,-15.76918,-17.2673], "fy":[17.22079,18.34106,19.74951,18.63347]}, - {"t":1.28734, "x":3.33058, "y":5.52958, "heading":-0.95371, "vx":0.83318, "vy":-1.00636, "omega":-0.08616, "ax":-0.97872, "ay":1.13475, "alpha":-0.29239, "fx":[-16.2814,-14.85464,-15.7244,-17.14062], "fy":[17.44223,18.43399,19.64731,18.68046]}, - {"t":1.31961, "x":3.35696, "y":5.49768, "heading":-0.95649, "vx":0.80159, "vy":-0.96973, "omega":-0.0956, "ax":-0.975, "ay":1.1381, "alpha":-0.25831, "fx":[-16.18301,-14.98149,-15.69094,-16.90245], "fy":[17.58431,18.48126,19.62594,18.73126]}, - {"t":1.35189, "x":3.38233, "y":5.46698, "heading":-0.95958, "vx":0.77012, "vy":-0.933, "omega":-0.10394, "ax":-0.9718, "ay":1.14096, "alpha":-0.23222, "fx":[-16.11449,-14.97856,-15.66334,-16.7922], "fy":[17.77252,18.55469,19.52223,18.76046]}, - {"t":1.38417, "x":3.40668, "y":5.43746, "heading":-0.96293, "vx":0.73875, "vy":-0.89617, "omega":-0.11143, "ax":-0.96902, "ay":1.14343, "alpha":-0.1991, "fx":[-16.03148,-15.10671,-15.64659,-16.58175], "fy":[17.90491,18.59005,19.48059,18.79626]}, - {"t":1.41645, "x":3.43002, "y":5.40913, "heading":-0.96653, "vx":0.70747, "vy":-0.85926, "omega":-0.11786, "ax":-0.96658, "ay":1.1456, "alpha":-0.17385, "fx":[-15.97493,-15.12218,-15.63068,-16.47896], "fy":[18.07005,18.65133,19.37814,18.81371]}, - {"t":1.44872, "x":3.45235, "y":5.38199, "heading":-0.97033, "vx":0.67627, "vy":-0.82229, "omega":-0.12347, "ax":-0.96441, "ay":1.1475, "alpha":-0.14162, "fx":[-15.90195,-15.24751,-15.62549,-16.29043], "fy":[18.19744,18.67985,19.32159,18.83896]}, - {"t":1.481, "x":3.47368, "y":5.35604, "heading":-0.97432, "vx":0.64514, "vy":-0.78525, "omega":-0.12804, "ax":-0.96249, "ay":1.14919, "alpha":-0.11655, "fx":[-15.85246,-15.27922,-15.61859,-16.18916], "fy":[18.34615,18.73324,19.22099,18.84808]}, - {"t":1.51328, "x":3.494, "y":5.3313, "heading":-0.97845, "vx":0.61408, "vy":-0.74815, "omega":-0.1318, "ax":-0.96076, "ay":1.1507, "alpha":-0.08508, "fx":[-15.78636,-15.39971,-15.62176,-16.01866], "fy":[18.47149,18.75804,19.15258,18.86521]}, - {"t":1.54556, "x":3.51332, "y":5.30775, "heading":-0.98271, "vx":0.58307, "vy":-0.71101, "omega":-0.13455, "ax":-0.9592, "ay":1.15206, "alpha":-0.05966, "fx":[-15.74056,-15.44673,-15.62259,-15.91477], "fy":[18.60836,18.8061,19.05363,18.86811]}, - {"t":1.57783, "x":3.53164, "y":5.2854, "heading":-0.98705, "vx":0.55211, "vy":-0.67383, "omega":-0.13647, "ax":-0.95779, "ay":1.15329, "alpha":-0.02875, "fx":[-15.67933,-15.5617,-15.63204,-15.7593], "fy":[18.73387,18.82928,18.97491,18.87848]}, - {"t":1.61011, "x":3.54896, "y":5.26425, "heading":-0.99145, "vx":0.52119, "vy":-0.6366, "omega":-0.1374, "ax":-0.95651, "ay":1.15441, "alpha":-0.00256, "fx":[-15.63481,-15.62362,-15.64004,-15.64987], "fy":[18.8621,18.87366,18.8772,18.87655]}, - {"t":1.64239, "x":3.56528, "y":5.2443, "heading":-0.99589, "vx":0.49032, "vy":-0.59934, "omega":-0.13748, "ax":-0.95533, "ay":1.15543, "alpha":0.02806, "fx":[-15.5771,-15.73321,-15.65438,-15.50682], "fy":[18.98978,18.89672,18.78866,18.88091]}, - {"t":1.67467, "x":3.58061, "y":5.22556, "heading":-1.00033, "vx":0.45948, "vy":-0.56204, "omega":-0.13658, "ax":-0.95425, "ay":1.15636, "alpha":0.05534, "fx":[-15.53209,-15.80981,-15.66936,-15.38975], "fy":[19.11164,18.93848,18.69178,18.87516]}, - {"t":1.70694, "x":3.59495, "y":5.20802, "heading":-1.00473, "vx":0.42868, "vy":-0.52472, "omega":-0.13479, "ax":-0.95326, "ay":1.15722, "alpha":0.08604, "fx":[-15.47692,-15.91476,-15.68769,-15.25668], "fy":[19.24349,18.96257,18.59316,18.8739]}, - {"t":1.73922, "x":3.60829, "y":5.19169, "heading":-1.00908, "vx":0.39791, "vy":-0.48737, "omega":-0.13202, "ax":-0.95234, "ay":1.15801, "alpha":0.11464, "fx":[-15.43004,-16.00599,-15.70961,-15.1304], "fy":[19.36064,19.00235,18.49675,18.8651]}, - {"t":1.7715, "x":3.62063, "y":5.17656, "heading":-1.01335, "vx":0.36717, "vy":-0.44999, "omega":-0.12831, "ax":-0.95149, "ay":1.15874, "alpha":0.14583, "fx":[-15.37665,-16.10745,-15.73134,-15.00498], "fy":[19.4987,19.02839,18.38722,18.85839]}, - {"t":1.80378, "x":3.63199, "y":5.16264, "heading":-1.01749, "vx":0.33646, "vy":-0.41259, "omega":-0.12361, "ax":-0.9507, "ay":1.15942, "alpha":0.17597, "fx":[-15.3269,-16.21333,-15.76026,-14.86822], "fy":[19.61246,19.06661,18.29086,18.84718]}, - {"t":1.83605, "x":3.64236, "y":5.14992, "heading":-1.02148, "vx":0.30577, "vy":-0.37517, "omega":-0.11793, "ax":-0.94997, "ay":1.16005, "alpha":0.20809, "fx":[-15.2747,-16.31263,-15.785,-14.74822], "fy":[19.7588,19.09532,18.16922,18.83509]}, - {"t":1.86833, "x":3.65173, "y":5.13842, "heading":-1.02528, "vx":0.27511, "vy":-0.33772, "omega":-0.11121, "ax":-0.94928, "ay":1.16064, "alpha":0.23956, "fx":[-15.22104,-16.43606,-15.8198,-14.59866], "fy":[19.87138,19.11869,18.07877,18.82815]}, - {"t":1.90061, "x":3.66012, "y":5.12812, "heading":-1.02887, "vx":0.24447, "vy":-0.30026, "omega":-0.10348, "ax":-0.94863, "ay":1.16119, "alpha":0.27347, "fx":[-15.16984,-16.53199,-15.84849,-14.48311], "fy":[20.02706,19.16416,17.93724,18.80458]}, - {"t":1.93289, "x":3.66751, "y":5.11904, "heading":-1.03221, "vx":0.21385, "vy":-0.26278, "omega":-0.09465, "ax":-0.94803, "ay":1.16171, "alpha":0.30694, "fx":[-15.11195,-16.67085,-15.89055,-14.32055], "fy":[20.13844,19.18724,17.84526,18.79589]}, - {"t":1.96517, "x":3.67392, "y":5.11116, "heading":-1.03527, "vx":0.18325, "vy":-0.22528, "omega":-0.08474, "ax":-0.94746, "ay":1.16219, "alpha":0.34266, "fx":[-15.06118,-16.76756,-15.92168,-14.20635], "fy":[20.30675,19.23538,17.68894,18.76748]}, - {"t":1.99744, "x":3.67934, "y":5.10449, "heading":-1.038, "vx":0.15267, "vy":-0.18777, "omega":-0.07368, "ax":-0.94693, "ay":1.16265, "alpha":0.37844, "fx":[-14.99863,-16.92295,-15.97106,-14.02914], "fy":[20.41827,19.25788,17.59461,18.75763]}, - {"t":2.02972, "x":3.68378, "y":5.09904, "heading":-1.04038, "vx":0.1221, "vy":-0.15024, "omega":-0.06147, "ax":-0.94642, "ay":1.16308, "alpha":0.41643, "fx":[-14.94813,-17.02178,-16.00442,-13.91445], "fy":[20.60134,19.30911,17.4216,18.72447]}, - {"t":2.062, "x":3.68723, "y":5.09479, "heading":-1.04237, "vx":0.09156, "vy":-0.1127, "omega":-0.04803, "ax":-0.94594, "ay":1.16349, "alpha":0.45488, "fx":[-14.88067,-17.19519,-16.06115,-13.72059], "fy":[20.7145,19.33086,17.32371,18.714]}, - {"t":2.09428, "x":3.68969, "y":5.09176, "heading":-1.04392, "vx":0.06102, "vy":-0.07515, "omega":-0.03335, "ax":-0.94549, "ay":1.16387, "alpha":0.49559, "fx":[-14.83038,-17.29762,-16.09649,-13.60358], "fy":[20.91454,19.38516,17.13199,18.67651]}, - {"t":2.12655, "x":3.69116, "y":5.08994, "heading":-1.04499, "vx":0.0305, "vy":-0.03758, "omega":-0.01735, "ax":-0.94507, "ay":1.16423, "alpha":0.53749, "fx":[-14.76976,-17.44474,-16.14592,-13.43969], "fy":[21.07939,19.42386,16.97768,18.65104]}, - {"t":2.15883, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.32594, "ay":-4.70286, "alpha":-0.00267, "fx":[70.70912,70.73045,70.73269,70.71136], "fy":[-76.89371,-76.8741,-76.87199,-76.89161]}, + {"t":0.03722, "x":1.55315, "y":7.52014, "heading":-0.93501, "vx":0.161, "vy":-0.17502, "omega":-0.0001, "ax":4.32541, "ay":-4.70276, "alpha":-0.00275, "fx":[70.7001,70.72203,70.72433,70.7024], "fy":[-76.89228,-76.87211,-76.86994,-76.89012]}, + {"t":0.07443, "x":1.56213, "y":7.51037, "heading":-0.93501, "vx":0.32197, "vy":-0.35004, "omega":-0.0002, "ax":4.32481, "ay":-4.70264, "alpha":-0.00283, "fx":[70.68987,70.71249,70.71486,70.69224], "fy":[-76.89065,-76.86986,-76.86762,-76.88842]}, + {"t":0.11165, "x":1.57711, "y":7.49409, "heading":-0.93502, "vx":0.48293, "vy":-0.52506, "omega":-0.00031, "ax":4.32412, "ay":-4.7025, "alpha":-0.00293, "fx":[70.67817,70.70158,70.70403,70.68062], "fy":[-76.8888,-76.86728,-76.86497,-76.88649]}, + {"t":0.14887, "x":1.59808, "y":7.47129, "heading":-0.93503, "vx":0.64386, "vy":-0.70007, "omega":-0.00042, "ax":4.32332, "ay":-4.70234, "alpha":-0.00304, "fx":[70.66467,70.68898,70.69153,70.66721], "fy":[-76.88665,-76.86431,-76.86191,-76.88426]}, + {"t":0.18608, "x":1.62504, "y":7.44198, "heading":-0.93504, "vx":0.80476, "vy":-0.87508, "omega":-0.00053, "ax":4.32239, "ay":-4.70215, "alpha":-0.00318, "fx":[70.64892,70.67428,70.67693,70.65156], "fy":[-76.88415,-76.86085,-76.85834,-76.88165]}, + {"t":0.2233, "x":1.65798, "y":7.40615, "heading":-0.93506, "vx":0.96562, "vy":-1.05007, "omega":-0.00065, "ax":4.3213, "ay":-4.70193, "alpha":-0.00333, "fx":[70.63028,70.6569,70.65967,70.63305], "fy":[-76.8812,-76.85675,-76.85412,-76.87858]}, + {"t":0.26052, "x":1.69691, "y":7.36382, "heading":-0.93509, "vx":1.12644, "vy":-1.22506, "omega":-0.00077, "ax":4.31998, "ay":-4.70167, "alpha":-0.00352, "fx":[70.6079,70.63602,70.63895,70.61082], "fy":[-76.87766,-76.85183,-76.84905,-76.87488]}, + {"t":0.29773, "x":1.74182, "y":7.31497, "heading":-0.93512, "vx":1.28722, "vy":-1.40004, "omega":-0.0009, "ax":4.31837, "ay":-4.70135, "alpha":-0.00375, "fx":[70.58053,70.61048,70.61359,70.58363], "fy":[-76.87332,-76.84582,-76.84285,-76.87037]}, + {"t":0.33495, "x":1.79272, "y":7.25961, "heading":-0.93515, "vx":1.44793, "vy":-1.57501, "omega":-0.00104, "ax":4.31635, "ay":-4.70095, "alpha":-0.00404, "fx":[70.54628,70.57853,70.58187,70.5496], "fy":[-76.86791,-76.8383,-76.83511,-76.86472]}, + {"t":0.37217, "x":1.8496, "y":7.19774, "heading":-0.93519, "vx":1.60857, "vy":-1.74997, "omega":-0.00119, "ax":4.31375, "ay":-4.70043, "alpha":-0.00441, "fx":[70.50219,70.53741,70.54103,70.5058], "fy":[-76.86094,-76.82864,-76.82514,-76.85746]}, + {"t":0.40938, "x":1.91245, "y":7.12935, "heading":-0.93523, "vx":1.76912, "vy":-1.9249, "omega":-0.00136, "ax":4.31028, "ay":-4.69974, "alpha":-0.00491, "fx":[70.44331,70.4825,70.4865,70.4473], "fy":[-76.85165,-76.81574,-76.81184,-76.84778]}, + {"t":0.4466, "x":1.98127, "y":7.05446, "heading":-0.93528, "vx":1.92953, "vy":-2.09981, "omega":-0.00154, "ax":4.30542, "ay":-4.69877, "alpha":-0.0056, "fx":[70.36072,70.40547,70.41,70.36523], "fy":[-76.83864,-76.79767,-76.79321,-76.83421]}, + {"t":0.48382, "x":2.05607, "y":6.97306, "heading":-0.93534, "vx":2.08976, "vy":-2.27468, "omega":-0.00175, "ax":4.2981, "ay":-4.69732, "alpha":-0.00665, "fx":[70.2365,70.28964,70.29494,70.24177], "fy":[-76.8191,-76.77054,-76.76523,-76.81384]}, + {"t":0.52103, "x":2.13682, "y":6.88515, "heading":-0.93541, "vx":2.24972, "vy":-2.4495, "omega":-0.002, "ax":4.28585, "ay":-4.69489, "alpha":-0.00842, "fx":[70.02862,70.09582,70.10236,70.03511], "fy":[-76.78652,-76.72527,-76.71853,-76.77984]}, + {"t":0.55825, "x":2.22351, "y":6.79074, "heading":-0.93548, "vx":2.40923, "vy":-2.62422, "omega":-0.00231, "ax":4.26117, "ay":-4.69003, "alpha":-0.01199, "fx":[69.60985,69.70547,69.71431,69.61859], "fy":[-76.72127,-76.63457,-76.62491,-76.71173]}, + {"t":0.59546, "x":2.31613, "y":6.68982, "heading":-0.93557, "vx":2.56781, "vy":-2.79877, "omega":-0.00275, "ax":4.1857, "ay":-4.67546, "alpha":-0.02296, "fx":[68.32986,68.51274,68.52684,68.34357], "fy":[-76.52571,-76.36262,-76.34379,-76.50729]}, + {"t":0.63268, "x":2.41459, "y":6.58243, "heading":-0.93567, "vx":2.72359, "vy":-2.97278, "omega":-0.00361, "ax":-3.20601, "ay":1.34523, "alpha":-0.63594, "fx":[-53.19356,-50.30785,-51.67468,-54.47261], "fy":[19.16074,22.04613,24.80859,21.95205]}, + {"t":0.6699, "x":2.51373, "y":6.47272, "heading":-0.9358, "vx":2.60428, "vy":-2.92271, "omega":-0.02728, "ax":-4.31912, "ay":4.56126, "alpha":-0.02121, "fx":[-70.70039,-70.5357,-70.51867,-70.68305], "fy":[74.47943,74.63472,74.65635,74.50142]}, + {"t":0.70711, "x":2.60766, "y":6.36711, "heading":-0.93682, "vx":2.44353, "vy":-2.75296, "omega":-0.02807, "ax":-4.32674, "ay":4.63184, "alpha":-0.01055, "fx":[-70.77998,-70.69686,-70.68801,-70.77105], "fy":[75.67821,75.75569,75.76535,75.68795]}, + {"t":0.74433, "x":2.69561, "y":6.26786, "heading":-0.93786, "vx":2.28251, "vy":-2.58058, "omega":-0.02846, "ax":-4.32937, "ay":4.65579, "alpha":-0.00682, "fx":[-70.80687,-70.75294,-70.7471,-70.80099], "fy":[76.08521,76.13528,76.14131,76.09128]}, + {"t":0.78155, "x":2.77756, "y":6.17504, "heading":-0.93892, "vx":2.12138, "vy":-2.4073, "omega":-0.02871, "ax":-4.33069, "ay":4.66785, "alpha":-0.00488, "fx":[-70.82006,-70.78133,-70.77708,-70.81579], "fy":[76.29035,76.32624,76.33051,76.29463]}, + {"t":0.81876, "x":2.85351, "y":6.08868, "heading":-0.93999, "vx":1.96021, "vy":-2.23358, "omega":-0.02889, "ax":-4.33148, "ay":4.67511, "alpha":-0.0037, "fx":[-70.8278,-70.79844,-70.79518,-70.82453], "fy":[76.41402,76.4412,76.44442,76.41725]}, + {"t":0.85598, "x":2.92346, "y":6.0088, "heading":-0.94107, "vx":1.79901, "vy":-2.05959, "omega":-0.02903, "ax":-4.33201, "ay":4.67997, "alpha":-0.00289, "fx":[-70.83285,-70.80987,-70.80729,-70.83027], "fy":[76.49674,76.518,76.52052,76.49926]}, + {"t":0.8932, "x":2.98741, "y":5.93539, "heading":-0.94215, "vx":1.63778, "vy":-1.88542, "omega":-0.02914, "ax":-4.33238, "ay":4.68345, "alpha":-0.00231, "fx":[-70.83639,-70.81803,-70.81595,-70.83431], "fy":[76.55597,76.57294,76.57496,76.55799]}, + {"t":0.93041, "x":3.04537, "y":5.86846, "heading":-0.94323, "vx":1.47655, "vy":-1.71112, "omega":-0.02923, "ax":-4.33266, "ay":4.68606, "alpha":-0.00187, "fx":[-70.839,-70.82415,-70.82245,-70.83729], "fy":[76.60048,76.6142,76.61583,76.60211]}, + {"t":0.96763, "x":3.09732, "y":5.80802, "heading":-0.94432, "vx":1.3153, "vy":-1.53672, "omega":-0.02929, "ax":-4.33287, "ay":4.68809, "alpha":-0.00152, "fx":[-70.84099,-70.8289,-70.8275,-70.83959], "fy":[76.63515,76.64631,76.64765,76.63648]}, + {"t":1.00485, "x":3.14327, "y":5.75408, "heading":-0.94541, "vx":1.15405, "vy":-1.36224, "omega":-0.02935, "ax":-4.33305, "ay":4.68972, "alpha":-0.00124, "fx":[-70.84256,-70.83269,-70.83154,-70.8414], "fy":[76.66292,76.67203,76.67312,76.66401]}, + {"t":1.04206, "x":3.18322, "y":5.70663, "heading":-0.9465, "vx":0.99278, "vy":-1.18771, "omega":-0.0294, "ax":-1.00692, "ay":1.10378, "alpha":-0.42916, "fx":[-16.81059,-14.86406,-16.09998,-18.06996], "fy":[16.34338,17.84343,19.74219,18.24989]}, + {"t":1.07434, "x":3.21474, "y":5.66887, "heading":-0.94745, "vx":0.96028, "vy":-1.15208, "omega":-0.04325, "ax":-0.99831, "ay":1.11614, "alpha":-0.39429, "fx":[-16.66382,-14.76706,-15.9831,-17.86759], "fy":[16.75669,18.08584,19.72479,18.41994]}, + {"t":1.10662, "x":3.24521, "y":5.63226, "heading":-0.94885, "vx":0.92806, "vy":-1.11605, "omega":-0.05598, "ax":-0.99034, "ay":1.12365, "alpha":-0.35897, "fx":[-16.49101,-14.84497,-15.88759,-17.53683], "fy":[16.95301,18.19865,19.78322,18.54345]}, + {"t":1.1389, "x":3.27465, "y":5.59682, "heading":-0.95065, "vx":0.89609, "vy":-1.07978, "omega":-0.06756, "ax":-0.98413, "ay":1.12942, "alpha":-0.32705, "fx":[-16.37785,-14.79969,-15.80385,-17.37328], "fy":[17.23008,18.32675,19.68731,18.61153]}, + {"t":1.17117, "x":3.30306, "y":5.56256, "heading":-0.95283, "vx":0.86433, "vy":-1.04333, "omega":-0.07812, "ax":-0.97916, "ay":1.134, "alpha":-0.29499, "fx":[-16.25888,-14.90308,-15.75398,-17.11396], "fy":[17.37596,18.39259,19.69945,18.68668]}, + {"t":1.20345, "x":3.33045, "y":5.52947, "heading":-0.95535, "vx":0.83272, "vy":-1.00673, "omega":-0.08764, "ax":-0.9751, "ay":1.13771, "alpha":-0.26569, "fx":[-16.18044,-14.89399,-15.70445,-16.98516], "fy":[17.59918,18.48244,19.59153,18.72428]}, + {"t":1.23573, "x":3.35682, "y":5.49757, "heading":-0.95818, "vx":0.80125, "vy":-0.97, "omega":-0.09622, "ax":-0.97171, "ay":1.14078, "alpha":-0.23611, "fx":[-16.09086,-15.00286,-15.67835,-16.77037], "fy":[17.71977,18.52704,19.57821,18.7734]}, + {"t":1.26801, "x":3.38218, "y":5.46686, "heading":-0.96129, "vx":0.76989, "vy":-0.93318, "omega":-0.10384, "ax":-0.96884, "ay":1.14337, "alpha":-0.20864, "fx":[-16.03088,-15.01662,-15.64832,-16.65908], "fy":[17.90834,18.59522,19.46929,18.79471]}, + {"t":1.30029, "x":3.40652, "y":5.43733, "heading":-0.96464, "vx":0.73861, "vy":-0.89628, "omega":-0.11057, "ax":-0.96638, "ay":1.14557, "alpha":-0.18082, "fx":[-15.95885,-15.12437,-15.63614,-16.47476], "fy":[18.01597,18.6288,19.43925,18.82784]}, + {"t":1.33256, "x":3.42986, "y":5.409, "heading":-0.96821, "vx":0.70742, "vy":-0.8593, "omega":-0.11641, "ax":-0.96425, "ay":1.14748, "alpha":-0.15458, "fx":[-15.90932,-15.15432,-15.619,-16.37212], "fy":[18.1803,18.68378,19.33311,18.83924]}, + {"t":1.36484, "x":3.45219, "y":5.38186, "heading":-0.97197, "vx":0.6763, "vy":-0.82226, "omega":-0.1214, "ax":-0.96239, "ay":1.14914, "alpha":-0.12789, "fx":[-15.84841,-15.25871,-15.6159,-16.20981], "fy":[18.282,18.7115,19.29017,18.86136]}, + {"t":1.39712, "x":3.47352, "y":5.35592, "heading":-0.97589, "vx":0.64523, "vy":-0.78517, "omega":-0.12553, "ax":-0.96074, "ay":1.1506, "alpha":-0.10233, "fx":[-15.80469,-15.30188,-15.60833,-16.11031], "fy":[18.42857,18.7582,19.18775,18.86605]}, + {"t":1.4294, "x":3.49385, "y":5.33117, "heading":-0.97994, "vx":0.61422, "vy":-0.74803, "omega":-0.12883, "ax":-0.95928, "ay":1.1519, "alpha":-0.07623, "fx":[-15.75099,-15.40252,-15.61172,-15.96433], "fy":[18.529,18.78291,19.1335,18.87985]}, + {"t":1.46167, "x":3.51317, "y":5.30763, "heading":-0.9841, "vx":0.58326, "vy":-0.71085, "omega":-0.13129, "ax":-0.95797, "ay":1.15305, "alpha":-0.05086, "fx":[-15.7101,-15.45774,-15.61199,-15.86415], "fy":[18.66249,18.82397,19.03468,18.87969]}, + {"t":1.49395, "x":3.5315, "y":5.28529, "heading":-0.98833, "vx":0.55234, "vy":-0.67363, "omega":-0.13293, "ax":-0.95679, "ay":1.15409, "alpha":-0.02484, "fx":[-15.66105,-15.55521,-15.62037,-15.73033], "fy":[18.76491,18.84783,18.96943,18.88655]}, + {"t":1.52623, "x":3.54883, "y":5.26414, "heading":-0.99262, "vx":0.52146, "vy":-0.63638, "omega":-0.13373, "ax":-0.95573, "ay":1.15503, "alpha":0.00081, "fx":[-15.62094,-15.62209,-15.62772,-15.62651], "fy":[18.88881,18.88521,18.87343,18.88255]}, + {"t":1.55851, "x":3.56516, "y":5.2442, "heading":-0.99694, "vx":0.49061, "vy":-0.5991, "omega":-0.13371, "ax":-0.95476, "ay":1.15588, "alpha":0.02722, "fx":[-15.57484,-15.7171,-15.64033,-15.50164], "fy":[18.99592,18.90963,18.79683,18.88325]}, + {"t":1.59078, "x":3.5805, "y":5.22547, "heading":-1.00126, "vx":0.45979, "vy":-0.56179, "omega":-0.13283, "ax":-0.95387, "ay":1.15665, "alpha":0.05362, "fx":[-15.53387,-15.79623,-15.65436,-15.3916], "fy":[19.11307,18.94459,18.70262,18.87604]}, + {"t":1.62306, "x":3.59484, "y":5.20794, "heading":-1.00554, "vx":0.429, "vy":-0.52446, "omega":-0.1311, "ax":-0.95306, "ay":1.15736, "alpha":0.08088, "fx":[-15.48944,-15.88987,-15.67075,-15.27298], "fy":[19.22728,18.97064,18.61385,18.87095]}, + {"t":1.65534, "x":3.6082, "y":5.19161, "heading":-1.00978, "vx":0.39824, "vy":-0.4871, "omega":-0.12849, "ax":-0.95232, "ay":1.15801, "alpha":0.10848, "fx":[-15.4463,-15.98214,-15.69148,-15.15433], "fy":[19.34008,19.00414,18.52022,18.86091]}, + {"t":1.68762, "x":3.62055, "y":5.17649, "heading":-1.01392, "vx":0.3675, "vy":-0.44972, "omega":-0.12499, "ax":-0.95163, "ay":1.15862, "alpha":0.13706, "fx":[-15.40256,-16.07565,-15.7114,-15.03961], "fy":[19.46372,19.03269,18.41806,18.85018]}, + {"t":1.71989, "x":3.63192, "y":5.16258, "heading":-1.01796, "vx":0.33678, "vy":-0.41232, "omega":-0.12056, "ax":-0.95099, "ay":1.15917, "alpha":0.16635, "fx":[-15.35616,-16.18234,-15.73905,-14.90997], "fy":[19.57436,19.06539,18.32369,18.83755]}, + {"t":1.75217, "x":3.6423, "y":5.14988, "heading":-1.02185, "vx":0.30609, "vy":-0.37491, "omega":-0.11519, "ax":-0.9504, "ay":1.15969, "alpha":0.19675, "fx":[-15.31234,-16.27708,-15.7624,-14.79699], "fy":[19.70977,19.09722,18.20655,18.82116]}, + {"t":1.78445, "x":3.65168, "y":5.13838, "heading":-1.02557, "vx":0.27541, "vy":-0.33748, "omega":-0.10884, "ax":-0.94985, "ay":1.16017, "alpha":0.22823, "fx":[-15.26168,-16.39986,-15.79736,-14.65386], "fy":[19.82032,19.12961,18.11001,18.80612]}, + {"t":1.81673, "x":3.66007, "y":5.12809, "heading":-1.02908, "vx":0.24475, "vy":-0.30003, "omega":-0.10148, "ax":-0.94933, "ay":1.16061, "alpha":0.26097, "fx":[-15.21716,-16.49733,-15.8241,-14.54052], "fy":[19.97001,19.16538,17.97592,18.78397]}, + {"t":1.84901, "x":3.66748, "y":5.11901, "heading":-1.03236, "vx":0.21411, "vy":-0.26257, "omega":-0.09305, "ax":-0.94885, "ay":1.16103, "alpha":0.2952, "fx":[-15.16137,-16.63827,-15.86684,-14.38117], "fy":[20.08253,19.19779,17.8756,18.76669]}, + {"t":1.88128, "x":3.6739, "y":5.11114, "heading":-1.03536, "vx":0.18348, "vy":-0.22509, "omega":-0.08352, "ax":-0.9484, "ay":1.16142, "alpha":0.33084, "fx":[-15.11568,-16.74012,-15.89698,-14.26537], "fy":[20.24926,19.23807,17.7222,18.73868]}, + {"t":1.91356, "x":3.67933, "y":5.10448, "heading":-1.03806, "vx":0.15287, "vy":-0.1876, "omega":-0.07285, "ax":-0.94798, "ay":1.16179, "alpha":0.36831, "fx":[-15.05414,-16.90182,-15.94788,-14.08658], "fy":[20.36499,19.27,17.61714,18.7201]}, + {"t":1.94584, "x":3.68377, "y":5.09903, "heading":-1.04041, "vx":0.12227, "vy":-0.1501, "omega":-0.06096, "ax":-0.94758, "ay":1.16214, "alpha":0.40762, "fx":[-15.00676,-17.00983,-15.98155,-13.96621], "fy":[20.55277,19.31594,17.44074,18.68539]}, + {"t":1.97812, "x":3.68722, "y":5.09479, "heading":-1.04237, "vx":0.09169, "vy":-0.11259, "omega":-0.0478, "ax":-0.9472, "ay":1.16246, "alpha":0.44926, "fx":[-14.93856,-17.19512,-16.0414,-13.76467], "fy":[20.67566,19.34886,17.32738,18.66422]}, + {"t":2.01039, "x":3.68968, "y":5.09176, "heading":-1.04392, "vx":0.06111, "vy":-0.07507, "omega":-0.0333, "ax":-0.94685, "ay":1.16277, "alpha":0.49273, "fx":[-14.88956,-17.31164,-16.07826,-13.63706], "fy":[20.8864,19.39929,17.12609,18.62443]}, + {"t":2.04267, "x":3.69116, "y":5.08994, "heading":-1.04499, "vx":0.03055, "vy":-0.03754, "omega":-0.01739, "ax":-0.94651, "ay":1.16306, "alpha":0.5389, "fx":[-14.8277,-17.47646,-16.13127,-13.45913], "fy":[21.06657,19.443,16.95435,18.59129]}, + {"t":2.07495, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index 28c1259a..f05e602d 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -526,8 +526,8 @@ "val":523.5987755982989 }, "tmax":{ - "exp":"0.75 N * m", - "val":0.75 + "exp":"0.9 N * m", + "val":0.9 }, "cof":{ "exp":"1.5", diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a3ec268d..3bd07f34 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -734,7 +734,12 @@ public Robot() { new Trigger(() -> DriverStation.isAutonomousEnabled()) .onTrue( - Commands.runOnce(() -> swerve.setCurrentLimits(new CurrentLimitsConfigs())) + Commands.runOnce( + () -> + swerve.setCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimitEnable(false) + .withSupplyCurrentLimitEnable(false))) .ignoringDisable(true)) .onFalse( Commands.runOnce( From f374f1ebb361fc51ad31d61279add38287454bfa Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 9 Apr 2025 14:01:42 -0700 Subject: [PATCH 012/154] adjust hp shoulder pos --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index d4b5755c..539ad4bc 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(51.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); From 3d420de42191b8e91851b4d0355a47af10ae1a99 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 17:36:08 -0700 Subject: [PATCH 013/154] speed up ground intake --- .../frc/robot/subsystems/Superstructure.java | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5d5dfde8..f1d11875 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -304,19 +304,21 @@ private void configureStateTransitionCommands() { .whileTrue( extendWithClearance( ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - WristSubsystem.WRIST_CORAL_GROUND) - .until( - () -> - shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_CORAL_GROUND)) + Rotation2d.fromDegrees(35.0), + WristSubsystem.WRIST_CLEARANCE_POS) + .until(() -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)) .andThen( Commands.parallel( - shoulder.setVoltage(-1.0), + shoulder + .setVoltage(-10.0) + .until(() -> shoulder.getAngle().getDegrees() < 10.0) + .andThen(shoulder.setVoltage(-1.0)), elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), wrist .setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND) - .until(wrist::isNearTarget) + .until( + () -> + wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) .andThen(wrist.setVoltage(-1.0))))) .whileTrue( Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) From 4a881b3425da26a6d7106bb99d7dd95805969be2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 17:39:13 -0700 Subject: [PATCH 014/154] slow down barge shot --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 ++++- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 6 +++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f1d11875..d7c5b400 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -807,7 +807,10 @@ private void configureStateTransitionCommands() { .whileTrue( manipulator .hold() - .until(() -> shoulder.getVelocity() > 0.4) + .until( + () -> + shoulder.getVelocity() + > ShoulderSubsystem.TOSS_CONFIGS.MotionMagicCruiseVelocity - 0.1) .andThen(manipulator.setVoltage(-13.0))) .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 539ad4bc..9979cb10 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -51,10 +51,10 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); - private static final MotionMagicConfigs DEFAULT_CONFIGS = + public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); - private static final MotionMagicConfigs TOSS_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.5).withMotionMagicAcceleration(4.0); + public static final MotionMagicConfigs TOSS_CONFIGS = + new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.3).withMotionMagicAcceleration(4.0); private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); From 2172d625910b2723b4c1689b9f2a57573f110bf7 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 17:55:40 -0700 Subject: [PATCH 015/154] adjust shoulder pose for ground algae --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 9979cb10..c453e7c4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,10 +29,10 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(51.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(5.0)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = From cfde337eb5dbde3238e81c650bb4c21f4b066868 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 18:11:13 -0700 Subject: [PATCH 016/154] more barge tuning --- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index c453e7c4..b942aa43 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -32,7 +32,8 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(5.0)); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = + Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(2.0)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = @@ -54,7 +55,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); public static final MotionMagicConfigs TOSS_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.3).withMotionMagicAcceleration(4.0); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.35).withMotionMagicAcceleration(4.0); private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); From 2e14b80997b141fdce9540371185384383567473 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 18:41:09 -0700 Subject: [PATCH 017/154] add algae ground to graph --- src/main/java/frc/robot/subsystems/ExtensionPathing.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index 345dc6ce..fb9c6af6 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -111,6 +111,15 @@ public class ExtensionPathing { graph.addNode(l1); graph.putEdge(l1, betweenTucked); + final var algaeGround = + new ExtensionState( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS); + graph.addNode(algaeGround); + graph.putEdge(algaeGround, betweenTucked); + graph.putEdge(algaeGround, untucked); + final var algaeLow = new ExtensionState( ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, From ac01ffdce5a5f7a06d5352dde5082f97d44bac93 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 18:49:18 -0700 Subject: [PATCH 018/154] adjust coral hold pose, add more edges for l4 to algae intake cancelling --- src/main/java/frc/robot/subsystems/ExtensionPathing.java | 4 ++++ src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index fb9c6af6..a73e0848 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -135,6 +135,10 @@ public class ExtensionPathing { ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeHigh); graph.putEdge(untucked, algaeHigh); + graph.putEdge(algaeLow, algaeHigh); + + graph.putEdge(l4TuckedOut, algaeHigh); + graph.putEdge(l4TuckedOut, algaeLow); } private ExtensionPathing() {} diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index a99c15ca..46b50a9b 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -34,7 +34,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double ALGAE_CURRENT_THRESHOLD = 6.0; public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; - public static final double CORAL_HOLD_POS = 0.5; + public static final double CORAL_HOLD_POS = 0.45; public static final double GEAR_RATIO = (58.0 / 10.0) * (24.0 / 18.0); From 7439d016880ffff8724f05feef5b1254b03ae02f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 18:59:54 -0700 Subject: [PATCH 019/154] adjust graph --- .../java/frc/robot/subsystems/ExtensionPathing.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index a73e0848..b69d7435 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -126,7 +126,7 @@ public class ExtensionPathing { ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeLow); - graph.putEdge(untucked, algaeLow); + graph.putEdge(betweenTucked, algaeLow); final var algaeHigh = new ExtensionState( @@ -134,11 +134,11 @@ public class ExtensionPathing { ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeHigh); - graph.putEdge(untucked, algaeHigh); + graph.putEdge(betweenTucked, algaeHigh); graph.putEdge(algaeLow, algaeHigh); - graph.putEdge(l4TuckedOut, algaeHigh); - graph.putEdge(l4TuckedOut, algaeLow); + graph.putEdge(l4Tucked, algaeHigh); + graph.putEdge(l4Tucked, algaeLow); } private ExtensionPathing() {} @@ -193,6 +193,8 @@ private static Pair, TotalMotion> search( result.stream() .min( Comparator.comparing( + (Pair, TotalMotion> s) -> s.getFirst().size()) + .thenComparing( (Pair, TotalMotion> s) -> s.getSecond().elevator) .thenComparing((s) -> s.getSecond().wristRotations) .thenComparing((s) -> s.getSecond().shoulderRotations)) From 68e57911c7b32ff28cf04d84525a56986888388d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 19:02:49 -0700 Subject: [PATCH 020/154] make net autoalign use angle --- src/main/java/frc/robot/Robot.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3bd07f34..137c82a9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1002,10 +1002,11 @@ public Robot() { modifyJoystick(driver.getLeftX()) * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), () -> - Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) - > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) - ? Rotation2d.kZero - : Rotation2d.k180deg), + (Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) + > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) + ? Rotation2d.kZero + : Rotation2d.k180deg) + .plus(Rotation2d.fromDegrees(20.0))), Commands.waitUntil( () -> { final var diff = From 4620e9c10896198efe717a6d17ece75990f7c16a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 19:15:33 -0700 Subject: [PATCH 021/154] reduce manipulator stator limit --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 137c82a9..5e21db44 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -311,7 +311,7 @@ public static enum AlgaeScoreTarget { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(120.0) + .withStatorCurrentLimit(80.0) .withSupplyCurrentLimit(30.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( From db7a81b3a672d072f710405ebe77a7d4c390cfb3 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 19:16:12 -0700 Subject: [PATCH 022/154] increase autoain kd --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 5b625042..f1644467 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -113,7 +113,7 @@ public static Command translateToPose( new ProfiledPIDController( 10.0, 0.01, - 0.02, + 0.04, new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION)); return Commands.runOnce( () -> { From 58cb824dc9c56e0538b17a3c9d3f5a1acfc0efca Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 19:39:01 -0700 Subject: [PATCH 023/154] adjust ground algae --- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 3cfb6e75..17e63b96 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -51,7 +51,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double L4_EXTENSION_METERS = ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.135; + public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14; public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index b942aa43..1024916f 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -33,7 +33,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(2.0)); + Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(-1.0)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = From 2b99dc138e89fa6bb01692a03b0f29268072df73 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 19:51:12 -0700 Subject: [PATCH 024/154] tune coral hold pos and pid --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5e21db44..4de9443d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -320,7 +320,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(7.5).withKD(0.5).withKS(0.39))) + .withSlot1(new Slot1Configs().withKP(7.5).withKD(0.5).withKS(0.5))) : new RollerIOSim( 0.01, 5.8677, diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 46b50a9b..db07b981 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -34,7 +34,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double ALGAE_CURRENT_THRESHOLD = 6.0; public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; - public static final double CORAL_HOLD_POS = 0.45; + public static final double CORAL_HOLD_POS = 0.6; public static final double GEAR_RATIO = (58.0 / 10.0) * (24.0 / 18.0); From edb46fba9231f862a9d058afa9e72186aa3d8598 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 9 Apr 2025 20:52:28 -0700 Subject: [PATCH 025/154] jog coral back during scoring --- .../java/frc/robot/subsystems/Superstructure.java | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d7c5b400..99440c3b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -433,7 +433,8 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L1_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())); + .whileTrue( + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())); stateTriggers .get(SuperState.PRE_L1) @@ -447,7 +448,8 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L1_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) + .whileTrue( + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -465,7 +467,8 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L2_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) + .whileTrue( + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -483,7 +486,8 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L3_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) + .whileTrue( + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -501,7 +505,8 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L4_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) + .whileTrue( + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From 4e00c50a1595f21c121dbbbe6e3f52b9b3283892 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 12:22:39 -0700 Subject: [PATCH 026/154] make auto algae logic actually make sense :sob: --- src/main/java/frc/robot/Autos.java | 170 +++++++++++------- .../subsystems/ManipulatorSubsystem.java | 5 +- .../frc/robot/subsystems/Superstructure.java | 8 +- 3 files changed, 114 insertions(+), 69 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index be439ac8..6989a976 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -108,7 +108,7 @@ public Command LMtoH() { final var routine = factory.newRoutine("LM to H"); final var traj = routine.trajectory("LMtoH"); routine.active().whileTrue(Commands.sequence(traj.resetOdometry(), traj.cmd())); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); routine.observe(traj.done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } @@ -117,7 +117,7 @@ public Command RMtoG() { final var routine = factory.newRoutine("RM to G"); final var traj = routine.trajectory("RMtoG"); routine.active().whileTrue(Commands.sequence(traj.resetOdometry(), traj.cmd())); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); routine.observe(traj.done()).onTrue(scoreCoralInAuto(swerve::getPose)); return routine.cmd(); } @@ -160,7 +160,7 @@ public void runCoralPath( public Command LOtoJ() { final var routine = factory.newRoutine("LO to J"); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); routine.active().onTrue(Commands.print("Auto!")); HashMap steps = new HashMap(); // key - name of path, value - traj @@ -208,7 +208,7 @@ public Command LOtoJ() { public Command ROtoE() { final var routine = factory.newRoutine("RO to E"); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -242,7 +242,7 @@ public Command ROtoE() { public Command LItoK() { final var routine = factory.newRoutine("LI to K"); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -272,7 +272,7 @@ public Command LItoK() { public Command RItoD() { final var routine = factory.newRoutine("RI to D"); - bindElevatorExtension(routine); + bindCoralElevatorExtension(routine); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -302,7 +302,7 @@ public Command RItoD() { public Command PMtoPL() { final var routine = factory.newRoutine("PM to PL"); - bindElevatorExtension(routine, 2.0); + bindCoralElevatorExtension(routine, 2.0); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -331,9 +331,10 @@ public Command PMtoPL() { return routine.cmd(); } - public Command CMtoGH() { + public Command CMtoGH() { // algae path final var routine = factory.newRoutine("CM to GH"); - bindElevatorExtension(routine, 2.0); + bindCoralElevatorExtension(routine, 2.0); + bindAlgaeElevatorExtension(routine); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -397,7 +398,11 @@ public Command CMtoGH() { autoAlgaeIntake = false; }), steps.get("GHtoNI").cmd())); - routine.observe(steps.get("GHtoNI").done()).onTrue(scoreAlgaeInAuto()); + routine.observe(steps.get("GHtoNI").done()).onTrue( + // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + scoreAlgaeInAuto()); + // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign + // for (int i = 0; i < stops.length - 2; i++) { // String startPos = stops[i]; // String endPos = stops[i + 1]; @@ -481,11 +486,11 @@ public Command intakeCoralInAuto(Supplier> pose) { } } - public void bindElevatorExtension(AutoRoutine routine) { - bindElevatorExtension(routine, 3.75); // TODO tune + public void bindCoralElevatorExtension(AutoRoutine routine) { + bindCoralElevatorExtension(routine, 3.75); // TODO tune } - public void bindElevatorExtension(AutoRoutine routine, double toleranceMeters) { + public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMeters) { routine .observe( () -> @@ -503,63 +508,100 @@ public void bindElevatorExtension(AutoRoutine routine, double toleranceMeters) { .whileFalse(Commands.run(() -> autoPreScore = false)); } - public void runAlgaePath( - AutoRoutine routine, - String startPos, - String endPos, - String nextPos, - HashMap steps) { + public void bindAlgaeElevatorExtension(AutoRoutine routine, double toleranceMeters) { routine - .observe(steps.get(startPos + "to" + endPos).done()) - .onTrue( - Commands.sequence( - endPos.equals("NI") - ? scoreAlgaeInAuto() - : intakeAlgaeInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()), - steps.get(endPos + "to" + nextPos).cmd())); + .observe( + () -> + MathUtil.isNear( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + toleranceMeters)) + .whileTrue(Commands.run(() -> autoPreScore = true)) + .whileFalse(Commands.run(() -> autoPreScore = false)); } - public Command intakeAlgaeInAuto(Supplier> pose) { - if (!pose.get().isPresent()) { - return Commands.none(); - } else { - return Commands.sequence( - Commands.runOnce( - () -> { - autoAlgaeIntake = true; - Robot.setCurrentAlgaeIntakeTarget( - AlgaeIntakeTargets.getClosestTarget(pose.get().get()).height); // are you serios - }), - Commands.waitUntil(() -> manipulator.hasAlgae()) - .alongWith( - Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setHasAlgae(true)) - : Commands.none()), - Commands.runOnce( - () -> { - autoAlgaeIntake = false; - })); - } + public void bindAlgaeElevatorExtension(AutoRoutine routine) { + bindAlgaeElevatorExtension(routine, 0.2); // TODO tune - is the coral one still requiring atp?? } - public Command scoreAlgaeInAuto() { - return Commands.runOnce( + public Command scoreAlgaeInAuto() { // oh good lord + return Commands.sequence( + Commands.waitUntil( + new Trigger( + () -> + MathUtil.isNear( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + Units.inchesToMeters(1.0)) + && MathUtil.isNear( + 0, + Math.hypot( + swerve.getVelocityRobotRelative().vxMetersPerSecond, + swerve.getVelocityRobotRelative().vyMetersPerSecond), + AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) + && MathUtil.isNear( + 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) + .debounce(0.06)), // TODO + Commands.runOnce( () -> { - autoScore = true; Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); - }) - .andThen( - Commands.waitUntil( - () -> !manipulator.hasAlgae()) // TODO maybe check state directly instead - .alongWith( - Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) - : Commands.none()) - .andThen( - Commands.runOnce( - () -> { - autoScore = false; - autoPreScore = false; - }))); + autoScore = true; + }), + Commands.waitUntil(() -> manipulator.hasAlgae()) + .alongWith( + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) + : Commands.none()) + .andThen( + Commands.runOnce( + () -> { + autoScore = false; + autoPreScore = false; + }))); } + // public void runAlgaePath( + // AutoRoutine routine, + // String startPos, + // String endPos, + // String nextPos, + // HashMap steps) { + // routine + // .observe(steps.get(startPos + "to" + endPos).done()) + // .onTrue( + // Commands.sequence( + // endPos.equals("NI") + // ? scoreAlgaeInAuto() + // : intakeAlgaeInAuto(() -> steps.get(startPos + "to" + + // endPos).getFinalPose()), + // steps.get(endPos + "to" + nextPos).cmd())); + // } + + // public Command intakeAlgaeInAuto(Supplier> pose) { + // if (!pose.get().isPresent()) { + // return Commands.none(); + // } else { + // return Commands.sequence( + // Commands.runOnce( + // () -> { + // autoAlgaeIntake = true; + // Robot.setCurrentAlgaeIntakeTarget( + // AlgaeIntakeTargets.getClosestTarget(pose.get().get()).height); // are you + // serios + // }), + // Commands.waitUntil(() -> manipulator.hasAlgae()) + // .alongWith( + // Robot.isSimulation() + // ? Commands.runOnce(() -> manipulator.setHasAlgae(true)) + // : Commands.none()), + // Commands.runOnce( + // () -> { + // autoAlgaeIntake = false; + // })); + // } + // } + } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index db07b981..c5eaa3cf 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.beambreak.BeambreakIO; import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; @@ -206,7 +205,7 @@ public void setHasAlgae(boolean state) { hasAlgae = state; } - public boolean hasAlgae() { - return hasAlgae || Robot.state.get() == SuperState.READY_ALGAE; + public boolean hasAlgae() { // TODO icky + return hasAlgae; } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 99440c3b..eaf962b0 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -736,7 +736,9 @@ private void configureStateTransitionCommands() { > 0.3 || (algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND || algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK)) - .onTrue(this.forceState(SuperState.READY_ALGAE)); + .onTrue( + this.forceState(SuperState.READY_ALGAE) + .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(true)))); // gahahaha // READY_ALGAE logic stateTriggers @@ -823,7 +825,9 @@ private void configureStateTransitionCommands() { .and(() -> stateTimer.hasElapsed(0.5)) .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS)) .and(() -> stateTimer.hasElapsed(1.0)) - .onTrue(forceState(SuperState.IDLE)); + .onTrue( + forceState(SuperState.IDLE) + .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(false)))); stateTriggers .get(SuperState.SCORE_ALGAE_PROCESSOR) From bbf418f3c03fd0459989e135372ba195aad34b28 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 13:06:19 -0700 Subject: [PATCH 027/154] add barge intermediate + change angle --- src/main/java/frc/robot/Autos.java | 8 +++++--- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 4 +++- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 3 ++- 4 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 6989a976..1fd75dd3 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -398,9 +398,11 @@ public Command CMtoGH() { // algae path autoAlgaeIntake = false; }), steps.get("GHtoNI").cmd())); - routine.observe(steps.get("GHtoNI").done()).onTrue( - // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - scoreAlgaeInAuto()); + routine + .observe(steps.get("GHtoNI").done()) + .onTrue( + // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + scoreAlgaeInAuto()); // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index eaf962b0..91ba5bba 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -801,8 +801,8 @@ private void configureStateTransitionCommands() { elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), // Make it initially extend to the full 90 degrees shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), - wrist.setSlowTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS))) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) + wrist.setSlowTargetAngle(WristSubsystem.WRIST_PRE_NET_POS))) + .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_PRE_NET_POS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .and(scoreReq) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 1024916f..58bde40a 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -55,7 +55,9 @@ public class ShoulderSubsystem extends SubsystemBase { public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); public static final MotionMagicConfigs TOSS_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.35).withMotionMagicAcceleration(4.0); + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(0.325) + .withMotionMagicAcceleration(4.0); private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 853f00b2..e4ad1269 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -40,7 +40,8 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(100); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(110); + public static final Rotation2d WRIST_PRE_NET_POS = Rotation2d.fromDegrees(100); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = From 3c8fb1d79f17d4adc0ddda2aefdb6d6fa619a88f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 14:40:39 -0700 Subject: [PATCH 028/154] algae auto stuff that doesn't really work yet --- src/main/deploy/choreo/GHtoNI.traj | 74 +++++++-------- src/main/deploy/choreo/IJtoNI.traj | 93 +++++++++---------- src/main/deploy/choreo/NItoEF.traj | 144 ++++++++++++++--------------- src/main/deploy/choreo/NItoIJ.traj | 89 ++++++++---------- src/main/deploy/choreo/choreo.chor | 14 +++ src/main/java/frc/robot/Autos.java | 84 ++++++++++++----- 6 files changed, 263 insertions(+), 235 deletions(-) diff --git a/src/main/deploy/choreo/GHtoNI.traj b/src/main/deploy/choreo/GHtoNI.traj index 90b88d1f..ee8da55c 100644 --- a/src/main/deploy/choreo/GHtoNI.traj +++ b/src/main/deploy/choreo/GHtoNI.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.929035186767578, "y":5.583443641662598, "heading":3.490658503988659, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"NI.x", "val":7.929035186767578}, "y":{"exp":"NI.y", "val":5.583443641662598}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "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}, @@ -26,39 +26,41 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.46565], + "waypoints":[0.0,1.28627], "samples":[ - {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.23714, "ay":1.83157, "alpha":0.0, "fx":[69.26922,69.26922,69.26922,69.26922], "fy":[29.94265,29.94265,29.94265,29.94265]}, - {"t":0.04885, "x":5.81138, "y":4.01935, "heading":3.14159, "vx":0.20701, "vy":0.08948, "omega":0.0, "ax":4.23688, "ay":1.83145, "alpha":0.0, "fx":[69.26488,69.26488,69.26488,69.26488], "fy":[29.94077,29.94077,29.94077,29.94077]}, - {"t":0.09771, "x":5.82655, "y":4.02591, "heading":3.14159, "vx":0.414, "vy":0.17896, "omega":0.0, "ax":4.23657, "ay":1.83132, "alpha":0.0, "fx":[69.25984,69.25984,69.25984,69.25984], "fy":[29.93859,29.93859,29.93859,29.93859]}, - {"t":0.14656, "x":5.85183, "y":4.03684, "heading":3.14159, "vx":0.62097, "vy":0.26843, "omega":0.0, "ax":4.23621, "ay":1.83116, "alpha":0.0, "fx":[69.25393,69.25393,69.25393,69.25393], "fy":[29.93604,29.93604,29.93604,29.93604]}, - {"t":0.19542, "x":5.88722, "y":4.05214, "heading":3.14159, "vx":0.82793, "vy":0.35789, "omega":0.0, "ax":4.23578, "ay":1.83098, "alpha":0.0, "fx":[69.24689,69.24689,69.24689,69.24689], "fy":[29.93299,29.93299,29.93299,29.93299]}, - {"t":0.24427, "x":5.93272, "y":4.07181, "heading":3.14159, "vx":1.03487, "vy":0.44734, "omega":0.0, "ax":4.23526, "ay":1.83075, "alpha":0.0, "fx":[69.23837,69.23837,69.23837,69.23837], "fy":[29.92931,29.92931,29.92931,29.92931]}, - {"t":0.29313, "x":5.98834, "y":4.09585, "heading":3.14159, "vx":1.24179, "vy":0.53678, "omega":0.0, "ax":4.23461, "ay":1.83047, "alpha":0.0, "fx":[69.22785,69.22785,69.22785,69.22785], "fy":[29.92476,29.92476,29.92476,29.92476]}, - {"t":0.34198, "x":6.05406, "y":4.12426, "heading":3.14159, "vx":1.44867, "vy":0.62621, "omega":0.0, "ax":4.2338, "ay":1.83012, "alpha":0.0, "fx":[69.21452,69.21452,69.21452,69.21452], "fy":[29.919,29.919,29.919,29.919]}, - {"t":0.39084, "x":6.12989, "y":4.15703, "heading":3.14159, "vx":1.65551, "vy":0.71562, "omega":0.0, "ax":4.23273, "ay":1.82966, "alpha":0.0, "fx":[69.19709,69.19709,69.19709,69.19709], "fy":[29.91147,29.91147,29.91147,29.91147]}, - {"t":0.43969, "x":6.21582, "y":4.19418, "heading":3.14159, "vx":1.8623, "vy":0.80501, "omega":0.0, "ax":4.23128, "ay":1.82903, "alpha":0.0, "fx":[69.17333,69.17333,69.17333,69.17333], "fy":[29.9012,29.9012,29.9012,29.9012]}, - {"t":0.48855, "x":6.31185, "y":4.23569, "heading":3.14159, "vx":2.06902, "vy":0.89436, "omega":0.0, "ax":4.22918, "ay":1.82813, "alpha":0.0, "fx":[69.13902,69.13902,69.13902,69.13902], "fy":[29.88637,29.88637,29.88637,29.88637]}, - {"t":0.5374, "x":6.41798, "y":4.28157, "heading":3.14159, "vx":2.27563, "vy":0.98368, "omega":0.0, "ax":4.22588, "ay":1.8267, "alpha":0.0, "fx":[69.08514,69.08514,69.08514,69.08514], "fy":[29.86308,29.86308,29.86308,29.86308]}, - {"t":0.58626, "x":6.5342, "y":4.3318, "heading":3.14159, "vx":2.48209, "vy":1.07292, "omega":0.0, "ax":4.21996, "ay":1.82414, "alpha":0.0, "fx":[68.98826,68.98826,68.98826,68.98826], "fy":[29.8212,29.8212,29.8212,29.8212]}, - {"t":0.63511, "x":6.66049, "y":4.3864, "heading":3.14159, "vx":2.68825, "vy":1.16204, "omega":0.0, "ax":4.20616, "ay":1.81817, "alpha":0.0, "fx":[68.76271,68.76271,68.76271,68.76271], "fy":[29.7237,29.7237,29.7237,29.7237]}, - {"t":0.68397, "x":6.79685, "y":4.44534, "heading":3.14159, "vx":2.89375, "vy":1.25086, "omega":0.0, "ax":4.13785, "ay":1.78865, "alpha":0.0, "fx":[67.64593,67.64593,67.64593,67.64593], "fy":[29.24095,29.24095,29.24095,29.24095]}, - {"t":0.73282, "x":6.94316, "y":4.50858, "heading":3.14159, "vx":3.0959, "vy":1.33825, "omega":0.0, "ax":-4.13785, "ay":-1.78865, "alpha":0.0, "fx":[-67.64593,-67.64593,-67.64593,-67.64593], "fy":[-29.24095,-29.24095,-29.24095,-29.24095]}, - {"t":0.78168, "x":7.08947, "y":4.57183, "heading":3.14159, "vx":2.89375, "vy":1.25086, "omega":0.0, "ax":-4.20616, "ay":-1.81817, "alpha":0.0, "fx":[-68.76271,-68.76271,-68.76271,-68.76271], "fy":[-29.7237,-29.7237,-29.7237,-29.7237]}, - {"t":0.83053, "x":7.22583, "y":4.63077, "heading":3.14159, "vx":2.68825, "vy":1.16204, "omega":0.0, "ax":-4.21996, "ay":-1.82414, "alpha":0.0, "fx":[-68.98826,-68.98826,-68.98826,-68.98826], "fy":[-29.8212,-29.8212,-29.8212,-29.8212]}, - {"t":0.87939, "x":7.35212, "y":4.68536, "heading":3.14159, "vx":2.48209, "vy":1.07292, "omega":0.0, "ax":-4.22588, "ay":-1.8267, "alpha":0.0, "fx":[-69.08514,-69.08514,-69.08514,-69.08514], "fy":[-29.86308,-29.86308,-29.86308,-29.86308]}, - {"t":0.92824, "x":7.46834, "y":4.7356, "heading":3.14159, "vx":2.27563, "vy":0.98368, "omega":0.0, "ax":-4.22918, "ay":-1.82813, "alpha":0.0, "fx":[-69.13902,-69.13902,-69.13902,-69.13902], "fy":[-29.88637,-29.88637,-29.88637,-29.88637]}, - {"t":0.9771, "x":7.57447, "y":4.78148, "heading":3.14159, "vx":2.06902, "vy":0.89436, "omega":0.0, "ax":-4.23128, "ay":-1.82903, "alpha":0.0, "fx":[-69.17333,-69.17333,-69.17333,-69.17333], "fy":[-29.9012,-29.9012,-29.9012,-29.9012]}, - {"t":1.02595, "x":7.6705, "y":4.82299, "heading":3.14159, "vx":1.8623, "vy":0.80501, "omega":0.0, "ax":-4.23273, "ay":-1.82966, "alpha":0.0, "fx":[-69.19709,-69.19709,-69.19709,-69.19709], "fy":[-29.91147,-29.91147,-29.91147,-29.91147]}, - {"t":1.07481, "x":7.75643, "y":4.86013, "heading":3.14159, "vx":1.65551, "vy":0.71562, "omega":0.0, "ax":-4.2338, "ay":-1.83012, "alpha":0.0, "fx":[-69.21452,-69.21452,-69.21452,-69.21452], "fy":[-29.919,-29.919,-29.919,-29.919]}, - {"t":1.12366, "x":7.83226, "y":4.89291, "heading":3.14159, "vx":1.44867, "vy":0.62621, "omega":0.0, "ax":-4.23461, "ay":-1.83047, "alpha":0.0, "fx":[-69.22785,-69.22785,-69.22785,-69.22785], "fy":[-29.92476,-29.92476,-29.92476,-29.92476]}, - {"t":1.17252, "x":7.89798, "y":4.92132, "heading":3.14159, "vx":1.24179, "vy":0.53678, "omega":0.0, "ax":-4.23526, "ay":-1.83075, "alpha":0.0, "fx":[-69.23837,-69.23837,-69.23837,-69.23837], "fy":[-29.92931,-29.92931,-29.92931,-29.92931]}, - {"t":1.22137, "x":7.9536, "y":4.94536, "heading":3.14159, "vx":1.03487, "vy":0.44734, "omega":0.0, "ax":-4.23578, "ay":-1.83098, "alpha":0.0, "fx":[-69.24689,-69.24689,-69.24689,-69.24689], "fy":[-29.93299,-29.93299,-29.93299,-29.93299]}, - {"t":1.27023, "x":7.9991, "y":4.96503, "heading":3.14159, "vx":0.82793, "vy":0.35789, "omega":0.0, "ax":-4.23621, "ay":-1.83116, "alpha":0.0, "fx":[-69.25393,-69.25393,-69.25393,-69.25393], "fy":[-29.93604,-29.93604,-29.93604,-29.93604]}, - {"t":1.31908, "x":8.03449, "y":4.98033, "heading":3.14159, "vx":0.62097, "vy":0.26843, "omega":0.0, "ax":-4.23657, "ay":-1.83132, "alpha":0.0, "fx":[-69.25984,-69.25984,-69.25984,-69.25984], "fy":[-29.93859,-29.93859,-29.93859,-29.93859]}, - {"t":1.36794, "x":8.05977, "y":4.99126, "heading":3.14159, "vx":0.414, "vy":0.17896, "omega":0.0, "ax":-4.23688, "ay":-1.83145, "alpha":0.0, "fx":[-69.26488,-69.26488,-69.26488,-69.26488], "fy":[-29.94077,-29.94077,-29.94077,-29.94077]}, - {"t":1.41679, "x":8.07494, "y":4.99781, "heading":3.14159, "vx":0.20701, "vy":0.08948, "omega":0.0, "ax":-4.23714, "ay":-1.83157, "alpha":0.0, "fx":[-69.26922,-69.26922,-69.26922,-69.26922], "fy":[-29.94265,-29.94265,-29.94265,-29.94265]}, - {"t":1.46565, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.13698, "ay":3.79035, "alpha":0.84439, "fx":[87.71603,83.47671,80.01486,84.71206], "fy":[56.74507,62.82369,67.1665,61.12493]}, + {"t":0.0402, "x":5.81047, "y":4.02023, "heading":3.14159, "vx":0.20649, "vy":0.15236, "omega":0.03394, "ax":5.13663, "ay":3.79009, "alpha":0.84392, "fx":[87.70793,83.47144,80.01152,84.70527], "fy":[56.74409,62.81908,67.15887,61.12086]}, + {"t":0.08039, "x":5.82292, "y":4.02942, "heading":-3.14023, "vx":0.41296, "vy":0.3047, "omega":0.06786, "ax":5.13621, "ay":3.78978, "alpha":0.84345, "fx":[87.69779,83.46037,80.00798,84.70298], "fy":[56.74427,62.82045,67.14975,61.10855]}, + {"t":0.12059, "x":5.84367, "y":4.04473, "heading":-3.1375, "vx":0.61941, "vy":0.45704, "omega":0.10177, "ax":5.13573, "ay":3.78943, "alpha":0.84299, "fx":[87.68534,83.44333,80.00412,84.70493], "fy":[56.74557,62.82762,67.13886,61.08787]}, + {"t":0.16078, "x":5.87271, "y":4.06616, "heading":-3.13341, "vx":0.82584, "vy":0.60935, "omega":0.13565, "ax":5.13517, "ay":3.78902, "alpha":0.84251, "fx":[87.67019,83.42007,79.99977,84.71078], "fy":[56.74794,62.84034,67.12582,61.05866]}, + {"t":0.20098, "x":5.91006, "y":4.09371, "heading":-3.12796, "vx":1.03226, "vy":0.76166, "omega":0.16952, "ax":5.13449, "ay":3.78852, "alpha":0.84199, "fx":[87.6518,83.39026,79.99473,84.72005], "fy":[56.75133,62.85828,67.11012,61.02069]}, + {"t":0.24117, "x":5.9557, "y":4.12739, "heading":-3.12114, "vx":1.23864, "vy":0.91394, "omega":0.20336, "ax":5.13368, "ay":3.78792, "alpha":0.84142, "fx":[87.62942,83.35342,79.98868,84.73203], "fy":[56.75564,62.88096,67.09103,60.97363]}, + {"t":0.28137, "x":6.00963, "y":4.16718, "heading":-3.11297, "vx":1.44499, "vy":1.0662, "omega":0.23718, "ax":5.13267, "ay":3.78718, "alpha":0.84073, "fx":[87.60195,83.30886,79.98114,84.74576], "fy":[56.76069,62.90769,67.0675,60.91696]}, + {"t":0.32157, "x":6.07186, "y":4.2131, "heading":-3.10344, "vx":1.65131, "vy":1.21843, "omega":0.27098, "ax":5.1314, "ay":3.78624, "alpha":0.83989, "fx":[87.56778,83.25547,79.97129,84.75971], "fy":[56.76614,62.93741,67.03801,60.84991]}, + {"t":0.36176, "x":6.14238, "y":4.26514, "heading":-3.09254, "vx":1.85757, "vy":1.37062, "omega":0.30474, "ax":5.12973, "ay":3.78502, "alpha":0.8388, "fx":[87.52437,83.1915,79.95773,84.77148], "fy":[56.77135,62.96848,67.0002,60.77117]}, + {"t":0.40196, "x":6.22119, "y":4.32329, "heading":-3.08029, "vx":2.06376, "vy":1.52276, "omega":0.33845, "ax":5.12745, "ay":3.78334, "alpha":0.83732, "fx":[87.46742,83.11386,79.9379,84.77698], "fy":[56.77497,62.99805,66.9502,60.67846]}, + {"t":0.44215, "x":6.30829, "y":4.38755, "heading":-3.06669, "vx":2.26986, "vy":1.67483, "omega":0.37211, "ax":5.12416, "ay":3.78092, "alpha":0.83523, "fx":[87.38907,83.01668,79.90668,84.76856], "fy":[56.77419,63.02088,66.88102,60.56732]}, + {"t":0.48235, "x":6.40367, "y":4.45793, "heading":-3.05173, "vx":2.47583, "vy":1.82681, "omega":0.40568, "ax":5.11899, "ay":3.77711, "alpha":0.83208, "fx":[87.27294,82.88725,79.85249,84.73007], "fy":[56.76224,63.02591,66.77836,60.42801]}, + {"t":0.52255, "x":6.50732, "y":4.53441, "heading":-3.03543, "vx":2.68159, "vy":1.97864, "omega":0.43913, "ax":5.10967, "ay":3.77026, "alpha":0.82698, "fx":[87.07758,82.69219,79.74389,84.62011], "fy":[56.71984,62.98494,66.60691,60.23458]}, + {"t":0.56274, "x":6.61924, "y":4.61699, "heading":-3.01778, "vx":2.88698, "vy":2.13018, "omega":0.47237, "ax":5.08795, "ay":3.75427, "alpha":0.81769, "fx":[86.65599,82.3078,79.46018,84.28952], "fy":[56.56961,62.79699,66.24443,59.88944]}, + {"t":0.60294, "x":6.73939, "y":4.70564, "heading":-2.99879, "vx":3.0915, "vy":2.28109, "omega":0.50524, "ax":4.98029, "ay":3.67497, "alpha":0.7952, "fx":[84.7467,80.61709,77.87302,82.43598], "fy":[55.55521,61.58544,64.67437,58.49982]}, + {"t":0.64313, "x":6.86768, "y":4.8003, "heading":-2.97848, "vx":3.29168, "vy":2.42881, "omega":0.5372, "ax":-4.98056, "ay":-3.67465, "alpha":-0.74561, "fx":[-84.52902,-80.60303,-78.12269,-82.43565], "fy":[-55.86594,-61.56843,-64.35422,-58.50523]}, + {"t":0.68333, "x":6.99597, "y":4.89496, "heading":-2.95689, "vx":3.09149, "vy":2.2811, "omega":0.50723, "ax":-5.08815, "ay":-3.75425, "alpha":-0.80296, "fx":[-86.52491,-82.12093,-79.59601,-84.48453], "fy":[-56.77356,-63.03564,-66.07345,-59.61691]}, + {"t":0.72352, "x":7.11613, "y":4.98362, "heading":-2.9365, "vx":2.88696, "vy":2.1302, "omega":0.47495, "ax":-5.10979, "ay":-3.77027, "alpha":-0.82025, "fx":[-86.92619,-82.3615,-79.88568,-84.96773], "fy":[-56.95711,-63.41366,-66.43002,-59.74592]}, + {"t":0.76372, "x":7.22804, "y":5.0662, "heading":-2.91741, "vx":2.68157, "vy":1.97865, "omega":0.44198, "ax":-5.11905, "ay":-3.77713, "alpha":-0.82944, "fx":[-87.08879,-82.42583,-80.01652,-85.21565], "fy":[-57.05027,-63.62579,-66.57534,-59.74391]}, + {"t":0.80392, "x":7.33169, "y":5.14268, "heading":-2.89964, "vx":2.47581, "vy":1.82682, "omega":0.40864, "ax":-5.12419, "ay":-3.78094, "alpha":-0.83529, "fx":[-87.17002,-82.43548,-80.09697,-85.38028], "fy":[-57.11586,-63.77718,-66.64688,-59.70443]}, + {"t":0.84411, "x":7.42707, "y":5.21306, "heading":-2.88321, "vx":2.26984, "vy":1.67485, "omega":0.37507, "ax":-5.12745, "ay":-3.78336, "alpha":-0.83941, "fx":[-87.21417,-82.42328,-80.15506,-85.50372], "fy":[-57.16906,-63.89704,-66.68418,-59.65248]}, + {"t":0.88431, "x":7.51417, "y":5.27732, "heading":-2.86814, "vx":2.06373, "vy":1.52277, "omega":0.34133, "ax":-5.12971, "ay":-3.78503, "alpha":-0.84247, "fx":[-87.23873,-82.40174,-80.20096,-85.60236], "fy":[-57.21497,-63.99676,-66.70321,-59.59747]}, + {"t":0.9245, "x":7.59298, "y":5.33547, "heading":-2.85442, "vx":1.85754, "vy":1.37063, "omega":0.30746, "ax":-5.13136, "ay":-3.78626, "alpha":-0.84484, "fx":[-87.25213,-82.37662,-80.23913,-85.68399], "fy":[-57.25563,-64.08176,-66.71176,-59.54366]}, + {"t":0.9647, "x":7.6635, "y":5.38751, "heading":-2.84206, "vx":1.65128, "vy":1.21844, "omega":0.27351, "ax":-5.13262, "ay":-3.7872, "alpha":-0.84673, "fx":[-87.25892,-82.35096,-80.27175,-85.7528], "fy":[-57.29192,-64.15498,-66.71419,-59.49322]}, + {"t":1.00489, "x":7.72572, "y":5.43343, "heading":-2.83107, "vx":1.44497, "vy":1.06621, "omega":0.23947, "ax":-5.13362, "ay":-3.78795, "alpha":-0.84825, "fx":[-87.26185,-82.32646,-80.29998,-85.81124], "fy":[-57.32424,-64.21815,-66.71315,-59.44734]}, + {"t":1.04509, "x":7.77966, "y":5.47322, "heading":-2.82144, "vx":1.23862, "vy":0.91395, "omega":0.20537, "ax":-5.13442, "ay":-3.78855, "alpha":-0.8495, "fx":[-87.26267,-82.30417,-80.32445,-85.86091], "fy":[-57.35272,-64.27239,-66.71037,-59.40673]}, + {"t":1.08529, "x":7.8253, "y":5.5069, "heading":-2.81319, "vx":1.03224, "vy":0.76166, "omega":0.17123, "ax":-5.13509, "ay":-3.78905, "alpha":-0.85054, "fx":[-87.26256,-82.28477,-80.34554,-85.90285], "fy":[-57.37743,-64.31843,-66.70704,-59.37181]}, + {"t":1.12548, "x":7.86264, "y":5.53445, "heading":-2.8063, "vx":0.82583, "vy":0.60936, "omega":0.13704, "ax":-5.13565, "ay":-3.78946, "alpha":-0.85139, "fx":[-87.26234,-82.26868,-80.36344,-85.9378], "fy":[-57.39835,-64.3568,-66.70402,-59.34284]}, + {"t":1.16568, "x":7.89169, "y":5.55589, "heading":-2.80079, "vx":0.6194, "vy":0.45704, "omega":0.10282, "ax":-5.13612, "ay":-3.78982, "alpha":-0.8521, "fx":[-87.26261,-82.25622,-80.37829,-85.96629], "fy":[-57.41545,-64.38788,-66.70195,-59.31999]}, + {"t":1.20587, "x":7.91244, "y":5.5712, "heading":-2.79666, "vx":0.41295, "vy":0.3047, "omega":0.06857, "ax":-5.13654, "ay":-3.79013, "alpha":-0.85268, "fx":[-87.26381,-82.2476,-80.39017,-85.9887], "fy":[-57.42871,-64.41196,-66.70128,-59.30337]}, + {"t":1.24607, "x":7.92489, "y":5.58038, "heading":-2.79391, "vx":0.20648, "vy":0.15236, "omega":0.03429, "ax":-5.13689, "ay":-3.79039, "alpha":-0.85314, "fx":[-87.26627,-82.24298,-80.39913,-86.00534], "fy":[-57.43808,-64.42924,-66.70239,-59.29306]}, + {"t":1.28627, "x":7.92904, "y":5.58344, "heading":-2.79253, "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/IJtoNI.traj b/src/main/deploy/choreo/IJtoNI.traj index f6f12391..038a22da 100644 --- a/src/main/deploy/choreo/IJtoNI.traj +++ b/src/main/deploy/choreo/IJtoNI.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.188053131103516, "y":5.171266078948975, "heading":-2.073639537722758, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.188053131103516, "y":5.171266078948975, "heading":-2.073639537722758, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.929035186767578, "y":5.583443641662598, "heading":3.490658503988659, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"IJ.x", "val":5.188053131103516}, "y":{"exp":"IJ.y", "val":5.171266078948975}, "heading":{"exp":"IJ.heading", "val":-2.073639537722758}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"IJ.x", "val":5.188053131103516}, "y":{"exp":"IJ.y", "val":5.171266078948975}, "heading":{"exp":"IJ.heading", "val":-2.073639537722758}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"NI.x", "val":7.929035186767578}, "y":{"exp":"NI.y", "val":5.583443641662598}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "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}, @@ -26,54 +26,45 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.59193], + "waypoints":[0.0,1.32063], "samples":[ - {"t":0.0, "x":5.18805, "y":5.17127, "heading":-2.07364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.5723, "ay":-0.27152, "alpha":-1.65476, "fx":[74.81943,73.76345,75.44788,74.96278], "fy":[-9.5225,-15.84386,-0.82241,8.43375]}, - {"t":0.03538, "x":5.19091, "y":5.1711, "heading":-2.07364, "vx":0.16175, "vy":-0.00961, "omega":-0.05854, "ax":4.57205, "ay":-0.2715, "alpha":-1.6546, "fx":[74.81456,73.75986,75.44429,74.95857], "fy":[-9.52125,-15.842,-0.82265,8.43199]}, - {"t":0.07075, "x":5.1995, "y":5.17059, "heading":-2.07571, "vx":0.32349, "vy":-0.01921, "omega":-0.11707, "ax":4.57178, "ay":-0.27147, "alpha":-1.65429, "fx":[74.81294,73.75332,75.4401,74.95341], "fy":[-9.49032,-15.85185,-0.84523,8.43509]}, - {"t":0.10613, "x":5.2138, "y":5.16974, "heading":-2.07985, "vx":0.48522, "vy":-0.02881, "omega":-0.17559, "ax":4.57149, "ay":-0.27144, "alpha":-1.65382, "fx":[74.81447,73.74381,75.43523,74.94725], "fy":[-9.42961,-15.87326,-0.89017,8.44284]}, - {"t":0.1415, "x":5.23383, "y":5.16855, "heading":-2.08606, "vx":0.64695, "vy":-0.03842, "omega":-0.2341, "ax":4.57117, "ay":-0.2714, "alpha":-1.6532, "fx":[74.81899,73.73128,75.42958,74.94008], "fy":[-9.3389,-15.90596,-0.95752,8.45485]}, - {"t":0.17688, "x":5.25957, "y":5.16702, "heading":-2.09435, "vx":0.80866, "vy":-0.04802, "omega":-0.29258, "ax":4.57082, "ay":-0.27135, "alpha":-1.65244, "fx":[74.82632,73.71569,75.423,74.93184], "fy":[-9.2179,-15.94955,-1.04732,8.47054]}, - {"t":0.21226, "x":5.29104, "y":5.16515, "heading":-2.1047, "vx":0.97035, "vy":-0.05762, "omega":-0.35104, "ax":4.57043, "ay":-0.27129, "alpha":-1.65155, "fx":[74.8362,73.69703,75.4153,74.9225], "fy":[-9.06621,-16.00351,-1.15967,8.48916]}, - {"t":0.24763, "x":5.32823, "y":5.16294, "heading":-2.11711, "vx":1.13204, "vy":-0.06721, "omega":-0.40947, "ax":4.56998, "ay":-0.27122, "alpha":-1.65053, "fx":[74.8483,73.67523,75.40625,74.91202], "fy":[-8.88336,-16.06718,-1.29465,8.50977]}, - {"t":0.28301, "x":5.37114, "y":5.1604, "heading":-2.1316, "vx":1.29371, "vy":-0.07681, "omega":-0.46786, "ax":4.56947, "ay":-0.27113, "alpha":-1.64942, "fx":[74.86221,73.65024,75.39555,74.90032], "fy":[-8.66883,-16.13976,-1.45235,8.53126]}, - {"t":0.31839, "x":5.41976, "y":5.15751, "heading":-2.14815, "vx":1.45536, "vy":-0.0864, "omega":-0.52621, "ax":4.56887, "ay":-0.27102, "alpha":-1.64825, "fx":[74.87739,73.62198,75.38283,74.88729], "fy":[-8.42206,-16.22028,-1.63287,8.55234]}, - {"t":0.35376, "x":5.47411, "y":5.15428, "heading":-2.16677, "vx":1.61699, "vy":-0.09599, "omega":-0.58452, "ax":4.56817, "ay":-0.2709, "alpha":-1.64704, "fx":[74.89317,73.59029,75.36761,74.87276], "fy":[-8.14247,-16.30761,-1.8363,8.57157]}, - {"t":0.38914, "x":5.53417, "y":5.15072, "heading":-2.18744, "vx":1.77859, "vy":-0.10557, "omega":-0.64278, "ax":4.56734, "ay":-0.27075, "alpha":-1.64584, "fx":[74.90865,73.55495,75.34931,74.85644], "fy":[-7.82947,-16.40046,-2.0627,8.58734]}, - {"t":0.42451, "x":5.59994, "y":5.14681, "heading":-2.21018, "vx":1.94017, "vy":-0.11515, "omega":-0.70101, "ax":4.56633, "ay":-0.27058, "alpha":-1.64472, "fx":[74.92264,73.51557,75.32714,74.83789], "fy":[-7.48255,-16.49731,-2.31211,8.59786]}, - {"t":0.45989, "x":5.67144, "y":5.14257, "heading":-2.23498, "vx":2.10171, "vy":-0.12472, "omega":-0.75919, "ax":4.56508, "ay":-0.27038, "alpha":-1.64373, "fx":[74.93354,73.47152,75.30003,74.81637], "fy":[-7.10124,-16.5964,-2.58451,8.60123]}, - {"t":0.49527, "x":5.74864, "y":5.13799, "heading":-2.26184, "vx":2.2632, "vy":-0.13429, "omega":-0.81734, "ax":4.5635, "ay":-0.27014, "alpha":-1.64296, "fx":[74.93911,73.42175,75.26648,74.79072], "fy":[-6.68519,-16.69569,-2.87981,8.59536]}, - {"t":0.53064, "x":5.83156, "y":5.13307, "heading":-2.29075, "vx":2.42464, "vy":-0.14384, "omega":-0.87546, "ax":4.56144, "ay":-0.26986, "alpha":-1.6425, "fx":[74.93601,73.36443,75.22426,74.75896], "fy":[-6.23418,-16.79277,-3.19781,8.57802]}, - {"t":0.56602, "x":5.92019, "y":5.12781, "heading":-2.32172, "vx":2.58601, "vy":-0.15339, "omega":-0.93356, "ax":4.55868, "ay":-0.26951, "alpha":-1.64246, "fx":[74.91903,73.29628,75.16984,74.71765], "fy":[-5.74814,-16.88474,-3.53814,8.54681]}, - {"t":0.60139, "x":6.01453, "y":5.12222, "heading":-2.35475, "vx":2.74727, "vy":-0.16292, "omega":-0.99167, "ax":4.55478, "ay":-0.26908, "alpha":-1.643, "fx":[74.8792,73.21107,75.09705,74.66038], "fy":[-5.22716,-16.96792,-3.90013,8.49917]}, - {"t":0.63677, "x":6.11457, "y":5.11628, "heading":-2.38983, "vx":2.9084, "vy":-0.17244, "omega":-1.04979, "ax":4.5489, "ay":-0.26851, "alpha":-1.64433, "fx":[74.7993,73.09581,74.99386,74.57412], "fy":[-4.67126,-17.03732,-4.28254,8.43233]}, - {"t":0.67215, "x":6.2203, "y":5.11002, "heading":-2.42697, "vx":3.06933, "vy":-0.18194, "omega":-1.10796, "ax":4.53907, "ay":-0.26768, "alpha":-1.64689, "fx":[74.64014,72.91933,74.83274,74.42811], "fy":[-4.07983,-17.08503,-4.68282,8.34338]}, - {"t":0.70752, "x":6.33172, "y":5.10341, "heading":-2.46616, "vx":3.2299, "vy":-0.19141, "omega":-1.16622, "ax":4.51941, "ay":-0.2662, "alpha":-1.65205, "fx":[74.28619,72.58687,74.53202,74.12989], "fy":[-3.44873,-17.09458,-5.09415,8.22982]}, - {"t":0.7429, "x":6.44881, "y":5.09647, "heading":-2.50742, "vx":3.38978, "vy":-0.20083, "omega":-1.22467, "ax":4.4608, "ay":-0.26217, "alpha":-1.66716, "fx":[73.16851,71.62449,73.68889,73.22038], "fy":[-2.74894,-17.00454,-5.48612,8.09537]}, - {"t":0.77828, "x":6.57152, "y":5.08921, "heading":-2.55074, "vx":3.54759, "vy":-0.2101, "omega":-1.28364, "ax":-0.02415, "ay":0.01691, "alpha":-1.24584, "fx":[-5.15387,-1.2863,4.36804,0.49268], "fy":[1.26754,-4.50516,-0.71378,5.05716]}, - {"t":0.81365, "x":6.697, "y":5.08178, "heading":-2.59616, "vx":3.54673, "vy":-0.20951, "omega":-1.32772, "ax":-4.46317, "ay":0.26378, "alpha":1.61226, "fx":[-73.22989,-71.69651,-73.60396,-73.32708], "fy":[1.57796,16.57219,6.39697,-7.2981]}, - {"t":0.84903, "x":6.81968, "y":5.07454, "heading":-2.64312, "vx":3.38884, "vy":-0.20017, "omega":-1.27068, "ax":-4.52022, "ay":0.26657, "alpha":1.64057, "fx":[-74.37365,-72.59391,-74.37363,-74.24704], "fy":[0.86467,16.93661,6.98509,-7.35467]}, - {"t":0.8844, "x":6.93674, "y":5.06762, "heading":-2.68808, "vx":3.22893, "vy":-0.19074, "omega":-1.21264, "ax":-4.5392, "ay":0.26747, "alpha":1.6567, "fx":[-74.76107,-72.90019,-74.59937,-74.56869], "fy":[0.19245,17.03976,7.4992,-7.24102]}, - {"t":0.91978, "x":7.04812, "y":5.06104, "heading":-2.73098, "vx":3.06835, "vy":-0.18128, "omega":-1.15404, "ax":-4.54859, "ay":0.26792, "alpha":1.66912, "fx":[-74.95252,-73.06246,-74.68816,-74.74012], "fy":[-0.43599,17.05497,7.97489,-7.07375]}, - {"t":0.95516, "x":7.15382, "y":5.0548, "heading":-2.7718, "vx":2.90744, "vy":-0.1718, "omega":-1.09499, "ax":-4.55414, "ay":0.26822, "alpha":1.67959, "fx":[-75.06276,-73.16994,-74.72231,-74.85124], "fy":[-1.02016,17.02178,8.41922,-6.88103]}, - {"t":0.99053, "x":7.25383, "y":5.04889, "heading":-2.81054, "vx":2.74634, "vy":-0.16231, "omega":-1.03557, "ax":-4.55778, "ay":0.26846, "alpha":1.68868, "fx":[-75.13094,-73.25179,-74.72966,-74.93188], "fy":[-1.56037,16.95625,8.8346,-6.67492]}, - {"t":1.02591, "x":7.34813, "y":5.04331, "heading":-2.84717, "vx":2.5851, "vy":-0.15282, "omega":-0.97583, "ax":-4.56034, "ay":0.26868, "alpha":1.69664, "fx":[-75.17441,-73.32002,-74.72234,-74.99467], "fy":[-2.05746,16.8674,9.22219,-6.46239]}, - {"t":1.06128, "x":7.43673, "y":5.03807, "heading":-2.88169, "vx":2.42377, "vy":-0.14331, "omega":-0.91581, "ax":-4.56223, "ay":0.26889, "alpha":1.70359, "fx":[-75.2022,-73.38019,-74.7066,-75.04584], "fy":[-2.51268,16.76143,9.58272,-6.24818]}, - {"t":1.09666, "x":7.51962, "y":5.03317, "heading":-2.91409, "vx":2.26238, "vy":-0.1338, "omega":-0.85555, "ax":-4.56367, "ay":0.26909, "alpha":1.7096, "fx":[-75.21959,-73.43509,-74.68606,-75.08881], "fy":[-2.92754,16.64319,9.91683,-6.03589]}, - {"t":1.13204, "x":7.5968, "y":5.02861, "heading":-2.94436, "vx":2.10093, "vy":-0.12428, "omega":-0.79507, "ax":-4.56482, "ay":0.26929, "alpha":1.71474, "fx":[-75.22992,-73.48609,-74.66294,-75.12561], "fy":[-3.30374,16.51675,10.2251,-5.8284]}, - {"t":1.16741, "x":7.66826, "y":5.02438, "heading":-2.97248, "vx":1.93945, "vy":-0.11475, "omega":-0.73441, "ax":-4.56576, "ay":0.26949, "alpha":1.71908, "fx":[-75.23544,-73.53388,-74.63876,-75.15752], "fy":[-3.64312,16.38566,10.50809,-5.62808]}, - {"t":1.20279, "x":7.73402, "y":5.02049, "heading":-2.99846, "vx":1.77793, "vy":-0.10522, "omega":-0.67359, "ax":-4.56653, "ay":0.26968, "alpha":1.72269, "fx":[-75.23772,-73.57873,-74.61456,-75.1854], "fy":[-3.94754,16.25312,10.76639,-5.43694]}, - {"t":1.23817, "x":7.79405, "y":5.01694, "heading":-3.02229, "vx":1.61638, "vy":-0.09568, "omega":-0.61265, "ax":-4.56719, "ay":0.26986, "alpha":1.72565, "fx":[-75.23789,-73.6207,-74.59107,-75.20986], "fy":[-4.2189,16.12197,11.0006,-5.2567]}, - {"t":1.27354, "x":7.84838, "y":5.01372, "heading":-3.04397, "vx":1.45481, "vy":-0.08613, "omega":-0.5516, "ax":-4.56776, "ay":0.27004, "alpha":1.72804, "fx":[-75.23678,-73.65974,-74.56888,-75.23134], "fy":[-4.45902,15.9948,11.21128,-5.08883]}, - {"t":1.30892, "x":7.89699, "y":5.01084, "heading":-3.06348, "vx":1.29322, "vy":-0.07658, "omega":-0.49047, "ax":-4.56826, "ay":0.2702, "alpha":1.72993, "fx":[-75.23501,-73.69573,-74.54838,-75.25018], "fy":[-4.66965,15.87393,11.39901,-4.93457]}, - {"t":1.34429, "x":7.93988, "y":5.0083, "heading":-3.08083, "vx":1.13161, "vy":-0.06702, "omega":-0.42927, "ax":-4.5687, "ay":0.27034, "alpha":1.73139, "fx":[-75.23305,-73.72854,-74.5299,-75.26668], "fy":[-4.85245,15.7614,11.56434,-4.79501]}, - {"t":1.37967, "x":7.97705, "y":5.0061, "heading":-3.09602, "vx":0.96999, "vy":-0.05746, "omega":-0.36802, "ax":-4.56909, "ay":0.27047, "alpha":1.7325, "fx":[-75.23123,-73.75804,-74.51368,-75.28103], "fy":[-5.00891,15.65905,11.70778,-4.67104]}, - {"t":1.41505, "x":8.00851, "y":5.00424, "heading":-3.10904, "vx":0.80835, "vy":-0.04789, "omega":-0.30673, "ax":-4.56945, "ay":0.27059, "alpha":1.73332, "fx":[-75.22983,-73.7841,-74.49992,-75.29344], "fy":[-5.14039,15.56843,11.8298,-4.56343]}, - {"t":1.45042, "x":8.03424, "y":5.00271, "heading":-3.11989, "vx":0.6467, "vy":-0.03832, "omega":-0.24542, "ax":-4.56977, "ay":0.27069, "alpha":1.73389, "fx":[-75.22904,-73.80661,-74.48876,-75.30405], "fy":[-5.24805,15.49091,11.93082,-4.4728]}, - {"t":1.4858, "x":8.05426, "y":5.00153, "heading":-3.12857, "vx":0.48504, "vy":-0.02874, "omega":-0.18408, "ax":-4.57007, "ay":0.27077, "alpha":1.73428, "fx":[-75.22902,-73.82549,-74.48031,-75.313], "fy":[-5.3329,15.42759,12.01121,-4.39967]}, - {"t":1.52117, "x":8.06856, "y":5.00068, "heading":-3.13508, "vx":0.32337, "vy":-0.01916, "omega":-0.12272, "ax":-4.57034, "ay":0.27083, "alpha":1.73451, "fx":[-75.22989,-73.84068,-74.47466,-75.32038], "fy":[-5.39574,15.37934,12.07125,-4.34442]}, - {"t":1.55655, "x":8.07714, "y":5.00017, "heading":-3.13942, "vx":0.16169, "vy":-0.00958, "omega":-0.06136, "ax":-4.57059, "ay":0.27088, "alpha":1.73462, "fx":[-75.23174,-73.85212,-74.47188,-75.32628], "fy":[-5.43719,15.34685,12.11118,-4.30735]}, - {"t":1.59193, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.18805, "y":5.17127, "heading":-2.07364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.29052, "ay":0.94526, "alpha":-1.78116, "fx":[103.56036,104.44358,103.07836,100.26989], "fy":[13.53481,1.98256,17.01388,29.28165]}, + {"t":0.03668, "x":5.19229, "y":5.1719, "heading":-2.07364, "vx":0.23076, "vy":0.03468, "omega":-0.06534, "ax":6.29037, "ay":0.94527, "alpha":-1.77199, "fx":[103.5506,104.43522,103.07308,100.28353], "fy":[13.54637,2.05189,17.00647,29.20892]}, + {"t":0.07337, "x":5.20498, "y":5.17381, "heading":-2.07604, "vx":0.46152, "vy":0.06935, "omega":-0.13034, "ax":6.2902, "ay":0.94529, "alpha":-1.76177, "fx":[103.53464,104.42586,103.07187,100.29869], "fy":[13.59704,2.12104,16.96924,29.12755]}, + {"t":0.11005, "x":5.22615, "y":5.17699, "heading":-2.08082, "vx":0.69227, "vy":0.10403, "omega":-0.19497, "ax":6.28999, "ay":0.94532, "alpha":-1.75027, "fx":[103.5123,104.41526,103.07443,100.31574], "fy":[13.68607,2.19234,16.90264,29.03551]}, + {"t":0.14674, "x":5.25577, "y":5.18144, "heading":-2.08797, "vx":0.92302, "vy":0.13871, "omega":-0.25918, "ax":6.28975, "ay":0.94535, "alpha":-1.7372, "fx":[103.48332,104.40309,103.0804,100.33519], "fy":[13.81257,2.26889,16.80721,28.92999]}, + {"t":0.18342, "x":5.29387, "y":5.18717, "heading":-2.09748, "vx":1.15375, "vy":0.17339, "omega":-0.32291, "ax":6.28947, "ay":0.94539, "alpha":-1.72215, "fx":[103.44734,104.38889,103.08925,100.35775], "fy":[13.97542,2.35486,16.68362,28.80724]}, + {"t":0.22011, "x":5.34042, "y":5.19416, "heading":-2.10932, "vx":1.38448, "vy":0.20807, "omega":-0.38609, "ax":6.28912, "ay":0.94543, "alpha":-1.70459, "fx":[103.40387,104.37205,103.10034,100.38432], "fy":[14.17317,2.45581,16.53274,28.66223]}, + {"t":0.25679, "x":5.39544, "y":5.20243, "heading":-2.12349, "vx":1.61519, "vy":0.24275, "omega":-0.44862, "ax":6.2887, "ay":0.94548, "alpha":-1.68378, "fx":[103.3523,104.35169,103.11277,100.41609], "fy":[14.40383,2.57926,16.35572,28.48819]}, + {"t":0.29347, "x":5.45893, "y":5.21197, "heading":-2.13994, "vx":1.84588, "vy":0.27744, "omega":-0.51039, "ax":6.28817, "ay":0.94553, "alpha":-1.65865, "fx":[103.29182,104.32656,103.12531,100.45463], "fy":[14.66462,2.73557,16.1542,28.27575]}, + {"t":0.33016, "x":5.53087, "y":5.22279, "heading":-2.15867, "vx":2.07656, "vy":0.31212, "omega":-0.57123, "ax":6.2875, "ay":0.94557, "alpha":-1.6276, "fx":[103.22135,104.29473,103.13619,100.50205], "fy":[14.95153,2.93957,15.93051,28.01162]}, + {"t":0.36684, "x":5.61128, "y":5.23487, "heading":-2.17962, "vx":2.30721, "vy":0.34681, "omega":-0.63094, "ax":6.28661, "ay":0.94561, "alpha":-1.58814, "fx":[103.13935,104.25318,103.14278,100.5613], "fy":[15.25855,3.21326,15.68822,27.67599]}, + {"t":0.40353, "x":5.70015, "y":5.24823, "heading":-2.20277, "vx":2.53783, "vy":0.3815, "omega":-0.6892, "ax":6.28541, "ay":0.94565, "alpha":-1.53622, "fx":[103.04356,104.19676,103.14092,100.63673], "fy":[15.57628,3.59127,15.43296,27.23758]}, + {"t":0.44021, "x":5.79748, "y":5.26286, "heading":-2.22805, "vx":2.76841, "vy":0.41619, "omega":-0.74555, "ax":6.28368, "ay":0.94566, "alpha":-1.46477, "fx":[102.93027,104.11617,103.12342,100.73514], "fy":[15.88911,4.13206,15.1744,26.64321]}, + {"t":0.4769, "x":5.90326, "y":5.27877, "heading":-2.2554, "vx":2.99892, "vy":0.45088, "omega":-0.79929, "ax":6.281, "ay":0.94563, "alpha":-1.36022, "fx":[102.7927,103.99249,103.07665,100.86797], "fy":[16.16893,4.94427,14.9307,25.79293]}, + {"t":0.51358, "x":6.0175, "y":5.29594, "heading":-2.28472, "vx":3.22934, "vy":0.48557, "omega":-0.84919, "ax":6.27634, "ay":0.94551, "alpha":-1.19317, "fx":[102.61649,103.78126,102.97049,101.0563], "fy":[16.35869,6.25835,14.74105,24.47101]}, + {"t":0.55026, "x":6.14019, "y":5.31439, "heading":-2.31587, "vx":3.45958, "vy":0.52025, "omega":-0.89296, "ax":6.26644, "ay":0.94514, "alpha":-0.88554, "fx":[102.36421,103.34997,102.72143,101.34166], "fy":[16.3193,8.6712,14.71089,22.10356]}, + {"t":0.58695, "x":6.27132, "y":5.33411, "heading":-2.34863, "vx":3.68946, "vy":0.55493, "omega":-0.92544, "ax":6.23522, "ay":0.9435, "alpha":-0.13951, "fx":[101.89703,102.08808,101.97555,101.77538], "fy":[15.58523,14.37576,15.26739,16.46927]}, + {"t":0.62363, "x":6.41086, "y":5.35511, "heading":-2.38258, "vx":3.91819, "vy":0.58954, "omega":-0.93056, "ax":5.85299, "ay":0.92071, "alpha":4.1313, "fx":[99.98939,90.3298,94.38873,98.03295], "fy":[10.83112,42.47498,23.01198,-16.11084]}, + {"t":0.66032, "x":6.55854, "y":5.37735, "heading":-2.41672, "vx":4.13291, "vy":0.62331, "omega":-0.77901, "ax":-5.84873, "ay":-0.92095, "alpha":-4.23576, "fx":[-100.13593,-90.40453,-94.01989,-97.90195], "fy":[-9.92441,-42.54551,-24.64441,16.89117]}, + {"t":0.697, "x":6.70621, "y":5.3996, "heading":-2.4453, "vx":3.91835, "vy":0.58953, "omega":-0.93439, "ax":-6.23564, "ay":-0.94317, "alpha":0.08376, "fx":[-101.90927,-102.03118,-101.97389,-101.8492], "fy":[-15.5748,-14.80262,-15.26515,-16.03329]}, + {"t":0.73369, "x":6.84576, "y":5.42059, "heading":-2.47957, "vx":3.6896, "vy":0.55493, "omega":-0.93132, "ax":-6.26722, "ay":-0.94499, "alpha":0.84504, "fx":[-102.20087,-103.30093,-102.8485,-101.47824], "fy":[-17.35461,-9.15473,-13.77383,-21.51217]}, + {"t":0.77037, "x":6.97689, "y":5.44031, "heading":-2.51374, "vx":3.45969, "vy":0.52026, "omega":-0.90032, "ax":-6.27713, "ay":-0.94542, "alpha":1.16414, "fx":[-102.26507,-103.73669,-103.21091,-101.2641], "fy":[-18.46315,-6.78561,-12.92801,-23.64665]}, + {"t":0.80705, "x":7.09958, "y":5.45876, "heading":-2.54677, "vx":3.22942, "vy":0.48558, "omega":-0.85761, "ax":-6.28172, "ay":-0.94558, "alpha":1.34113, "fx":[-102.25439,-103.95045,-103.42213,-101.1498], "fy":[-19.31411,-5.51447,-12.28783,-24.71737]}, + {"t":0.84374, "x":7.21383, "y":5.47594, "heading":-2.57823, "vx":2.99898, "vy":0.45089, "omega":-0.80842, "ax":-6.28429, "ay":-0.94564, "alpha":1.45452, "fx":[-102.21214,-104.07559,-103.5666,-101.09031], "fy":[-20.02438,-4.74697,-11.75054,-25.31604]}, + {"t":0.88042, "x":7.31961, "y":5.49184, "heading":-2.60788, "vx":2.76844, "vy":0.4162, "omega":-0.75506, "ax":-6.2859, "ay":-0.94566, "alpha":1.53384, "fx":[-102.15565,-104.15701,-103.67452,-101.06267], "fy":[-20.63955,-4.25332,-11.28011,-25.66605]}, + {"t":0.91711, "x":7.41694, "y":5.50647, "heading":-2.63558, "vx":2.53785, "vy":0.38151, "omega":-0.69879, "ax":-6.28699, "ay":-0.94565, "alpha":1.59268, "fx":[-102.09335,-104.21381,-103.75948,-101.05436], "fy":[-21.18129,-3.92485,-10.8608,-25.87135]}, + {"t":0.95379, "x":7.50581, "y":5.51983, "heading":-2.66122, "vx":2.30722, "vy":0.34682, "omega":-0.64036, "ax":-6.28776, "ay":-0.94562, "alpha":1.63817, "fx":[-102.02991,-104.25546,-103.82856,-101.05796], "fy":[-21.66154,-3.70267,-10.48467,-25.98753]}, + {"t":0.99048, "x":7.58622, "y":5.53192, "heading":-2.68471, "vx":2.07655, "vy":0.31213, "omega":-0.58027, "ax":-6.28835, "ay":-0.94558, "alpha":1.67441, "fx":[-101.9681,-104.28717,-103.88588,-101.06875], "fy":[-22.08767,-3.55154,-10.14721,-26.04735]}, + {"t":1.02716, "x":7.65816, "y":5.54273, "heading":-2.70599, "vx":1.84587, "vy":0.27744, "omega":-0.51884, "ax":-6.2888, "ay":-0.94553, "alpha":1.70394, "fx":[-101.90971,-104.31204,-103.93404,-101.08355], "fy":[-22.46473,-3.44869,-9.84565,-26.07164]}, + {"t":1.06384, "x":7.72165, "y":5.55228, "heading":-2.72503, "vx":1.61517, "vy":0.24276, "omega":-0.45634, "ax":-6.28916, "ay":-0.94548, "alpha":1.72842, "fx":[-101.85588,-104.33206,-103.97477,-101.10012], "fy":[-22.79643,-3.37846,-9.5781,-26.07443]}, + {"t":1.10053, "x":7.77667, "y":5.56055, "heading":-2.74177, "vx":1.38446, "vy":0.20807, "omega":-0.39293, "ax":-6.28945, "ay":-0.94543, "alpha":1.74897, "fx":[-101.8074,-104.34854,-104.00931,-101.11683], "fy":[-23.08568,-3.32961,-9.34323,-26.06562]}, + {"t":1.13721, "x":7.82322, "y":5.56754, "heading":-2.75618, "vx":1.15373, "vy":0.17339, "omega":-0.32877, "ax":-6.2897, "ay":-0.94539, "alpha":1.76641, "fx":[-101.76479,-104.36242,-104.03854,-101.13247], "fy":[-23.33483,-3.29376,-9.14004,-26.05241]}, + {"t":1.1739, "x":7.86131, "y":5.57327, "heading":-2.76824, "vx":0.923, "vy":0.13871, "omega":-0.26397, "ax":-6.28991, "ay":-0.94534, "alpha":1.78134, "fx":[-101.72842,-104.37436,-104.06316,-101.14609], "fy":[-23.54579,-3.26448,-8.96774,-26.04021]}, + {"t":1.21058, "x":7.89094, "y":5.57772, "heading":-2.77793, "vx":0.69226, "vy":0.10403, "omega":-0.19862, "ax":-6.29009, "ay":-0.94531, "alpha":1.79418, "fx":[-101.69855,-104.38485,-104.08368,-101.157], "fy":[-23.72015,-3.2368,-8.82572,-26.03313]}, + {"t":1.24727, "x":7.9121, "y":5.5809, "heading":-2.78521, "vx":0.46151, "vy":0.06935, "omega":-0.13281, "ax":-6.29025, "ay":-0.94528, "alpha":1.80529, "fx":[-101.67535,-104.39428,-104.10048,-101.16462], "fy":[-23.85918,-3.20684,-8.71352,-26.03436]}, + {"t":1.28395, "x":7.9248, "y":5.58281, "heading":-2.79008, "vx":0.23076, "vy":0.03468, "omega":-0.06658, "ax":-6.2904, "ay":-0.94526, "alpha":1.81493, "fx":[-101.65897,-104.40292,-104.11389,-101.16853], "fy":[-23.96386,-3.17158,-8.63079,-26.04633]}, + {"t":1.32063, "x":7.92904, "y":5.58344, "heading":-2.79253, "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/NItoEF.traj b/src/main/deploy/choreo/NItoEF.traj index 93a20cb3..06648701 100644 --- a/src/main/deploy/choreo/NItoEF.traj +++ b/src/main/deploy/choreo/NItoEF.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.383368492126465, "y":3.130985975265503, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.929035186767578, "y":5.583443641662598, "heading":3.490658503988659, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.383368492126465, "y":3.130985975265503, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.188053131103516, "y":2.863070487976074, "heading":2.0928880900706415, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,8 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.383368492126465 m", "val":6.383368492126465}, "y":{"exp":"3.130985975265503 m", "val":3.130985975265503}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"NI.x", "val":7.929035186767578}, "y":{"exp":"NI.y", "val":5.583443641662598}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.383368492126465 m", "val":6.383368492126465}, "y":{"exp":"3.130985975265503 m", "val":3.130985975265503}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"EF.x", "val":5.188053131103516}, "y":{"exp":"EF.y", "val":2.863070487976074}, "heading":{"exp":"EF.heading", "val":2.0928880900706415}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,78 +28,72 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.12124,1.86587], + "waypoints":[0.0,1.06273,1.70547], "samples":[ - {"t":0.0, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.28056, "ay":-3.99197, "alpha":-1.1299, "fx":[-44.80091,-39.1584,-30.00595,-35.16625], "fy":[-60.66906,-64.47141,-69.19916,-66.70489]}, - {"t":0.02875, "x":8.07906, "y":4.99835, "heading":3.14159, "vx":-0.06557, "vy":-0.11477, "omega":-0.03248, "ax":-2.29702, "ay":-3.98215, "alpha":-1.13297, "fx":[-45.07531,-39.40122,-30.26033,-35.47044], "fy":[-60.46054,-64.31924,-69.08417,-66.53858]}, - {"t":0.0575, "x":8.07622, "y":4.9934, "heading":3.14066, "vx":-0.1316, "vy":-0.22925, "omega":-0.06506, "ax":-2.31466, "ay":-3.97152, "alpha":-1.13624, "fx":[-45.36803,-39.66778,-30.53646,-35.78915], "fy":[-60.23587,-64.15082,-68.95811,-66.36224]}, - {"t":0.08625, "x":8.07148, "y":4.98517, "heading":3.13879, "vx":-0.19815, "vy":-0.34343, "omega":-0.09772, "ax":-2.33365, "ay":-3.95996, "alpha":-1.13973, "fx":[-45.68125,-39.96007,-30.8365,-36.12486], "fy":[-59.9929,-63.96443,-68.81966,-66.1742]}, - {"t":0.115, "x":8.06482, "y":4.97366, "heading":3.13598, "vx":-0.26524, "vy":-0.45728, "omega":-0.13049, "ax":-2.35412, "ay":-3.94735, "alpha":-1.14345, "fx":[-46.01749,-40.28034,-31.16293,-36.48047], "fy":[-59.72909,-63.7581,-68.66728,-65.97243]}, - {"t":0.14375, "x":8.05622, "y":4.95888, "heading":3.13223, "vx":-0.33292, "vy":-0.57077, "omega":-0.16336, "ax":-2.37625, "ay":-3.93356, "alpha":-1.14744, "fx":[-46.37959,-40.63123,-31.51864,-36.85935], "fy":[-59.4415,-63.52949,-68.49911,-65.75455]}, - {"t":0.1725, "x":8.04567, "y":4.94085, "heading":3.12753, "vx":-0.40124, "vy":-0.68386, "omega":-0.19635, "ax":-2.40027, "ay":-3.91839, "alpha":-1.15173, "fx":[-46.77088,-41.01577,-31.90705,-37.26544], "fy":[-59.1266,-63.27582,-68.31292,-65.51766]}, - {"t":0.20125, "x":8.03314, "y":4.91957, "heading":3.12189, "vx":-0.47025, "vy":-0.79651, "omega":-0.22947, "ax":-2.4264, "ay":-3.90165, "alpha":-1.15636, "fx":[-47.19521,-41.43752,-32.33218,-37.70337], "fy":[-58.7802,-62.99376,-68.10604,-65.25829]}, - {"t":0.23, "x":8.01862, "y":4.89506, "heading":3.11529, "vx":-0.54001, "vy":-0.90868, "omega":-0.26271, "ax":-2.45495, "ay":-3.88308, "alpha":-1.16136, "fx":[-47.65709,-41.90065,-32.79876,-38.17866], "fy":[-58.39723,-62.67932,-67.8752,-64.9722]}, - {"t":0.25875, "x":8.00208, "y":4.86733, "heading":3.10774, "vx":-0.61059, "vy":-1.02032, "omega":-0.2961, "ax":-2.48626, "ay":-3.86237, "alpha":-1.16679, "fx":[-48.16182,-42.4101,-33.31249,-38.69784], "fy":[-57.97156,-62.32762,-67.61642,-64.65416]}, - {"t":0.2875, "x":7.9835, "y":4.8364, "heading":3.09922, "vx":-0.68206, "vy":-1.13136, "omega":-0.32964, "ax":-2.52073, "ay":-3.83914, "alpha":-1.1727, "fx":[-48.71569,-42.97174,-33.88016,-39.26878], "fy":[-57.49563,-61.9327,-67.32474,-64.29768]}, - {"t":0.31625, "x":7.96285, "y":4.80228, "heading":3.08975, "vx":-0.75453, "vy":-1.24174, "omega":-0.36336, "ax":-2.55886, "ay":-3.81292, "alpha":-1.17917, "fx":[-49.3262,-43.59262,-34.51002,-39.90096], "fy":[-56.96008,-61.48716,-66.99399,-63.89463]}, - {"t":0.345, "x":7.9401, "y":4.76501, "heading":3.0793, "vx":-0.8281, "vy":-1.35136, "omega":-0.39726, "ax":-2.60125, "ay":-3.7831, "alpha":-1.18626, "fx":[-50.00233,-44.28126,-35.21212,-40.60597], "fy":[-56.35319,-60.98173,-66.61631,-63.43466]}, - {"t":0.37375, "x":7.91521, "y":4.72459, "heading":3.06788, "vx":-0.90289, "vy":-1.46012, "omega":-0.43137, "ax":-2.64862, "ay":-3.74892, "alpha":-1.19408, "fx":[-50.75495,-45.04799,-35.99886,-41.39801], "fy":[-55.66006,-60.40466,-66.18166,-62.90443]}, - {"t":0.4025, "x":7.88816, "y":4.68107, "heading":3.05548, "vx":-0.97903, "vy":-1.5679, "omega":-0.46569, "ax":-2.70189, "ay":-3.70939, "alpha":-1.20272, "fx":[-51.59722,-45.90554,-36.88565,-42.29468], "fy":[-54.86156,-59.74083,-65.6769,-62.28651]}, - {"t":0.43125, "x":7.8589, "y":4.63446, "heading":3.04209, "vx":-1.05671, "vy":-1.67455, "omega":-0.50027, "ax":-2.76217, "ay":-3.6632, "alpha":-1.21232, "fx":[-52.54521,-46.86962,-37.89187,-43.31799], "fy":[-53.93272,-58.97051,-65.08469,-61.55777]}, - {"t":0.46, "x":7.82738, "y":4.5848, "heading":3.0277, "vx":-1.13612, "vy":-1.77986, "omega":-0.53513, "ax":-2.83085, "ay":-3.60862, "alpha":-1.22301, "fx":[-53.61855,-47.95983,-39.04217,-44.49567], "fy":[-52.84052,-58.06753,-64.38159,-60.68686]}, - {"t":0.48875, "x":7.79354, "y":4.53214, "heading":3.01232, "vx":-1.21751, "vy":-1.88361, "omega":-0.57029, "ax":-2.90972, "ay":-3.54327, "alpha":-1.23495, "fx":[-54.84126,-49.20079,-40.36831,-45.86296], "fy":[-51.54049,-56.99652,-63.5353,-59.63055]}, - {"t":0.5175, "x":7.75734, "y":4.47652, "heading":2.99592, "vx":-1.30117, "vy":-1.98548, "omega":-0.60579, "ax":-3.001, "ay":-3.46383, "alpha":-1.24831, "fx":[-56.24255,-50.62357,-41.91161,-47.46485], "fy":[-49.97163,-55.7087,-62.50027,-58.32777]}, - {"t":0.54625, "x":7.71869, "y":4.41801, "heading":2.97851, "vx":-1.38744, "vy":-2.08506, "omega":-0.64168, "ax":-3.10755, "ay":-3.3656, "alpha":-1.26323, "fx":[-57.85731,-52.26748,-43.72649,-49.35883], "fy":[-48.04873,-54.1353,-61.21051,-56.69021]}, - {"t":0.575, "x":7.67752, "y":4.35667, "heading":2.96006, "vx":-1.47678, "vy":-2.18182, "omega":-0.678, "ax":-3.23295, "ay":-3.24168, "alpha":-1.27981, "fx":[-59.72568,-54.18191,-45.88506,-51.61764], "fy":[-45.65026,-52.17687,-59.56753,-54.58669]}, - {"t":0.60375, "x":7.63372, "y":4.2926, "heading":2.94057, "vx":-1.56973, "vy":-2.27502, "omega":-0.71479, "ax":-3.38159, "ay":-3.08173, "alpha":-1.29796, "fx":[-61.88954,-56.42752,-48.48285,-54.33019], "fy":[-42.59958,-49.68583,-57.41947,-51.81709]}, - {"t":0.6325, "x":7.5872, "y":4.22592, "heading":2.92002, "vx":-1.66695, "vy":-2.36362, "omega":-0.75211, "ax":-3.55843, "ay":-2.86983, "alpha":-1.31717, "fx":[-64.38137,-59.07407,-51.64363,-57.5951], "fy":[-38.63577,-46.43713,-54.5237,-48.06829]}, - {"t":0.66125, "x":7.5378, "y":4.15678, "heading":2.89839, "vx":-1.76926, "vy":-2.44613, "omega":-0.78998, "ax":-3.76785, "ay":-2.5809, "alpha":-1.33595, "fx":[-67.19446,-62.18757,-55.51605,-61.49054], "fy":[-33.3711,-42.079,-50.47877,-42.84219]}, - {"t":0.69, "x":7.48538, "y":4.08539, "heading":2.87568, "vx":-1.87758, "vy":-2.52033, "omega":-0.82839, "ax":-4.00973, "ay":-2.17532, "alpha":-1.35043, "fx":[-70.20939,-65.78559,-60.23491,-65.97601], "fy":[-26.2397,-36.0538,-44.60226,-35.35332]}, - {"t":0.71875, "x":7.42974, "y":4.01203, "heading":2.85187, "vx":-1.99286, "vy":-2.58287, "omega":-0.86721, "ax":-4.26846, "ay":-1.59293, "alpha":-1.35107, "fx":[-73.03144,-69.70621,-65.7603,-70.62684], "fy":[-16.47844,-27.49195,-35.73812,-24.45667]}, - {"t":0.7475, "x":7.37068, "y":3.93712, "heading":2.82693, "vx":-2.11558, "vy":-2.62866, "omega":-0.90605, "ax":-4.48732, "ay":-0.75701, "alpha":-1.31414, "fx":[-74.70254,-73.2751,-71.36715,-74.09159], "fy":[-3.29436,-15.17279,-22.11724,-8.91816]}, - {"t":0.77624, "x":7.308, "y":3.86123, "heading":2.80089, "vx":-2.24459, "vy":-2.65043, "omega":-0.94384, "ax":-4.53295, "ay":0.37671, "alpha":-1.18964, "fx":[-73.47309,-74.69638,-74.56135,-73.68943], "fy":[13.44664,2.02139,-2.06868,11.23445]}, - {"t":0.80499, "x":7.2416, "y":3.78519, "heading":2.77375, "vx":-2.37491, "vy":-2.6396, "omega":-0.97804, "ax":-4.22556, "ay":1.69385, "alpha":-0.92774, "fx":[-67.4481,-70.91208,-70.95757,-67.00153], "fy":[31.99529,23.32846,22.79592,32.64547]}, - {"t":0.83374, "x":7.17158, "y":3.71, "heading":2.74563, "vx":-2.49639, "vy":-2.5909, "omega":-1.00471, "ax":-3.53657, "ay":2.8802, "alpha":-0.57237, "fx":[-56.60953,-60.16518,-59.20563,-55.28457], "fy":[48.69612,44.19624,45.35521,50.09533]}, - {"t":0.86249, "x":7.09834, "y":3.6367, "heading":2.71675, "vx":-2.59807, "vy":-2.50809, "omega":-1.02116, "ax":-2.6886, "ay":3.69566, "alpha":-0.2311, "fx":[-43.58431,-45.3238,-44.35007,-42.556], "fy":[60.71894,59.41569,60.11675,61.41677]}, - {"t":0.89124, "x":7.02254, "y":3.56612, "heading":2.68739, "vx":-2.67536, "vy":-2.40185, "omega":-1.02781, "ax":-1.91137, "ay":4.15938, "alpha":0.03878, "fx":[-31.27624,-30.97656,-31.21881,-31.51759], "fy":[67.98188,68.12171,68.0143,67.87363]}, - {"t":0.91999, "x":6.94483, "y":3.49879, "heading":2.65784, "vx":-2.73032, "vy":-2.28226, "omega":-1.02669, "ax":-1.28681, "ay":4.39853, "alpha":0.23564, "fx":[-21.02389,-19.28233,-21.05447,-22.78672], "fy":[71.90768,72.41004,71.92885,71.38395]}, - {"t":0.94874, "x":6.8658, "y":3.43499, "heading":2.62832, "vx":-2.76731, "vy":-2.15581, "omega":-1.01992, "ax":-0.80606, "ay":4.51541, "alpha":0.37737, "fx":[-12.92254,-10.30817,-13.42086,-16.05852], "fy":[73.87158,74.30503,73.82335,73.27335]}, - {"t":0.97749, "x":6.78591, "y":3.37488, "heading":2.599, "vx":-2.79049, "vy":-2.02599, "omega":-1.00907, "ax":-0.43711, "ay":4.56881, "alpha":0.48118, "fx":[-6.60912,-3.47646,-7.63702,-10.86111], "fy":[74.76447,75.00285,74.71118,74.28657]}, - {"t":1.00624, "x":6.7055, "y":3.31852, "heading":2.56999, "vx":-2.80305, "vy":-1.89464, "omega":-0.99524, "ax":-0.15038, "ay":4.58939, "alpha":0.55924, "fx":[-1.66963,1.79002,-3.16115,-6.79283], "fy":[75.08906,75.11531,75.08664,74.82017]}, - {"t":1.03499, "x":6.62486, "y":3.26595, "heading":2.54138, "vx":-2.80738, "vy":-1.76269, "omega":-0.97916, "ax":0.07638, "ay":4.59292, "alpha":0.61952, "fx":[2.23797,5.92798,0.38443,-3.55563], "fy":[75.11684,74.94538,75.19414,75.08576]}, - {"t":1.06374, "x":6.54418, "y":3.21717, "heading":2.51323, "vx":-2.80518, "vy":-1.63065, "omega":-0.96135, "ax":0.25896, "ay":4.58761, "alpha":0.66721, "fx":[5.3704,9.24447,3.25589,-0.93667], "fy":[74.99349,74.64408,75.15867,75.19873]}, - {"t":1.09249, "x":6.46363, "y":3.17218, "heading":2.48559, "vx":-2.79773, "vy":-1.49875, "omega":-0.94216, "ax":0.40847, "ay":4.57784, "alpha":0.70574, "fx":[7.91483,11.95238,5.62801,1.2154], "fy":[74.79764,74.28777,75.04645,75.22372]}, - {"t":1.12124, "x":6.38337, "y":3.13099, "heading":2.4585, "vx":-2.78599, "vy":-1.36714, "omega":-0.92188, "ax":0.67511, "ay":4.54288, "alpha":0.76556, "fx":[12.58059,16.6488,9.7503,5.1671], "fy":[74.11506,73.34409,74.5914,75.01896]}, - {"t":1.14606, "x":6.31443, "y":3.09845, "heading":2.43562, "vx":-2.76923, "vy":-1.25438, "omega":-0.90287, "ax":1.08771, "ay":4.45963, "alpha":0.86375, "fx":[19.99711,23.8179,15.97767,11.33535], "fy":[72.45628,71.33548,73.50606,74.32814]}, - {"t":1.17088, "x":6.24602, "y":3.06869, "heading":2.41321, "vx":-2.74224, "vy":-1.14369, "omega":-0.88143, "ax":1.50305, "ay":4.33479, "alpha":0.95807, "fx":[27.51974,30.83788,22.21126,17.71896], "fy":[69.939,68.59325,71.86721,73.06262]}, - {"t":1.19571, "x":6.17842, "y":3.04164, "heading":2.39133, "vx":-2.70493, "vy":-1.0361, "omega":-0.85765, "ax":1.90969, "ay":4.16917, "alpha":1.04589, "fx":[34.89277,37.50417,28.30927,24.17304], "fy":[66.56504,65.19108,69.692,71.18364]}, - {"t":1.22053, "x":6.11187, "y":3.0172, "heading":2.37004, "vx":-2.65753, "vy":-0.93261, "omega":-0.83169, "ax":2.29651, "ay":3.96693, "alpha":1.12478, "fx":[41.85798,43.64569,34.13739,30.53352], "fy":[62.42252,61.25647,67.0337,68.69447]}, - {"t":1.24535, "x":6.04662, "y":2.99528, "heading":2.3494, "vx":-2.60053, "vy":-0.83415, "omega":-0.80378, "ax":2.65417, "ay":3.73508, "alpha":1.19279, "fx":[48.1977,49.14529,39.58318,36.63623], "fy":[57.67588,56.9497,63.97536,65.64484]}, - {"t":1.27017, "x":5.98289, "y":2.97572, "heading":2.32945, "vx":-2.53465, "vy":-0.74144, "omega":-0.77417, "ax":2.97614, "ay":3.48235, "alpha":1.24872, "fx":[53.76712,53.94675,44.56592,42.33731], "fy":[52.53582,52.43839,60.61858,62.1263]}, - {"t":1.29499, "x":5.92089, "y":2.95839, "heading":2.31023, "vx":-2.46078, "vy":-0.65501, "omega":-0.74317, "ax":3.25916, "ay":3.2179, "alpha":1.29233, "fx":[58.50555,58.04883,49.04017,47.52941], "fy":[47.2209,47.87557,57.07065,58.25917]}, - {"t":1.31981, "x":5.86082, "y":2.94313, "heading":2.29179, "vx":-2.37988, "vy":-0.57513, "omega":-0.7111, "ax":3.50288, "ay":2.95018, "alpha":1.32428, "fx":[62.42676,61.49147,52.99351,52.14966], "fy":[41.92476,43.38601,53.43313,54.17523]}, - {"t":1.34463, "x":5.80282, "y":2.92976, "heading":2.27414, "vx":-2.29294, "vy":-0.50191, "omega":-0.67823, "ax":3.70924, "ay":2.68614, "alpha":1.34589, "fx":[65.59708,64.33987,56.44018,56.17861], "fy":[36.79723,39.06124,49.79381,50.00107]}, - {"t":1.36945, "x":5.74705, "y":2.91813, "heading":2.2573, "vx":-2.20087, "vy":-0.43524, "omega":-0.64482, "ax":3.88161, "ay":2.43101, "alpha":1.35884, "fx":[68.11133,66.67048,59.41317,59.63257], "fy":[31.93971,34.96124,46.22279,45.84602]}, - {"t":1.39427, "x":5.69362, "y":2.90808, "heading":2.2413, "vx":-2.10452, "vy":-0.37489, "omega":-0.61109, "ax":4.02407, "ay":2.18829, "alpha":1.36488, "fx":[70.07309,68.56094,61.95651,62.55282], "fy":[27.4103,31.11963,42.77176,41.79583]}, - {"t":1.4191, "x":5.64262, "y":2.89944, "heading":2.22613, "vx":-2.00464, "vy":-0.32058, "omega":-0.57722, "ax":4.14084, "ay":1.96003, "alpha":1.36561, "fx":[71.58186,70.08385,64.11898,64.99485], "fy":[23.23349,27.5501,39.47567,37.91164]}, - {"t":1.44392, "x":5.59414, "y":2.89209, "heading":2.2118, "vx":-1.90186, "vy":-0.27193, "omega":-0.54332, "ax":4.23595, "ay":1.74715, "alpha":1.36241, "fx":[72.72617,71.30365,65.94959,67.01963], "fy":[19.41022,24.25234,36.35547,34.23234]}, - {"t":1.46874, "x":5.54824, "y":2.88588, "heading":2.19831, "vx":-1.79672, "vy":-0.22856, "omega":-0.5095, "ax":4.31302, "ay":1.54977, "alpha":1.35638, "fx":[73.58106,72.27561,67.49466,68.68749], "fy":[15.92656,21.21698,33.42123,30.77869]}, - {"t":1.49356, "x":5.50497, "y":2.88068, "heading":2.18567, "vx":-1.68967, "vy":-0.1901, "omega":-0.47584, "ax":4.3752, "ay":1.36747, "alpha":1.34839, "fx":[74.20818,73.04605,68.79627,70.05438], "fy":[12.76018,18.42931,30.67496,27.55777]}, - {"t":1.51838, "x":5.46438, "y":2.87639, "heading":2.17386, "vx":-1.58107, "vy":-0.15615, "omega":-0.44237, "ax":4.42516, "ay":1.19948, "alpha":1.33909, "fx":[74.65715,73.65321,69.89159,71.17004], "fy":[9.88485,15.87189,28.11311,24.56705]}, - {"t":1.5432, "x":5.4265, "y":2.87288, "heading":2.16288, "vx":-1.47123, "vy":-0.12638, "omega":-0.40913, "ax":4.46514, "ay":1.04486, "alpha":1.32899, "fx":[74.96732,74.1283,70.81284,72.07742], "fy":[7.27333,13.52629,25.72845,21.79766]}, - {"t":1.56802, "x":5.39136, "y":2.87007, "heading":2.15272, "vx":-1.3604, "vy":-0.10045, "omega":-0.37614, "ax":4.49696, "ay":0.90258, "alpha":1.31844, "fx":[75.1697,74.49671,71.58753,72.81291], "fy":[4.8991,11.37425,23.51156,19.23701]}, - {"t":1.59284, "x":5.35898, "y":2.86785, "heading":2.14339, "vx":-1.24878, "vy":-0.07804, "omega":-0.34342, "ax":4.52214, "ay":0.77162, "alpha":1.3077, "fx":[75.28856,74.77896,72.23899,73.40686], "fy":[2.7373,9.3983,21.45186,16.87049]}, - {"t":1.61766, "x":5.32937, "y":2.86615, "heading":2.13486, "vx":-1.13654, "vy":-0.05889, "omega":-0.31096, "ax":4.5419, "ay":0.65097, "alpha":1.29696, "fx":[75.34289,74.99162,72.78686,73.88442], "fy":[0.7652,7.58215,19.53835,14.6828]}, - {"t":1.64248, "x":5.30256, "y":2.86489, "heading":2.12714, "vx":-1.0238, "vy":-0.04273, "omega":-0.27877, "ax":4.55725, "ay":0.5397, "alpha":1.28636, "fx":[75.34748,75.1481,73.2476,74.26625], "fy":[-1.03768,5.91085,17.76007,12.65875]}, - {"t":1.66731, "x":5.27855, "y":2.864, "heading":2.12022, "vx":-0.91069, "vy":-0.02934, "omega":-0.24684, "ax":4.56899, "ay":0.43692, "alpha":1.27598, "fx":[75.3139,75.25921,73.63498,74.5693], "fy":[-2.68958,4.37079,16.10643,10.7837]}, - {"t":1.69213, "x":5.25736, "y":2.8634, "heading":2.1141, "vx":-0.79728, "vy":-0.01849, "omega":-0.21517, "ax":4.57779, "ay":0.34185, "alpha":1.26589, "fx":[75.2512,75.33373,73.96052,74.80743], "fy":[-4.20669,2.94973,14.56743,9.04394]}, - {"t":1.71695, "x":5.23898, "y":2.86305, "heading":2.10876, "vx":-0.68366, "vy":-0.01001, "omega":-0.18375, "ax":4.58419, "ay":0.25376, "alpha":1.25612, "fx":[75.16643,75.37877,74.23384,74.99196], "fy":[-5.60328,1.63666,13.13369,7.42677]}, - {"t":1.74177, "x":5.22342, "y":2.86288, "heading":2.1042, "vx":-0.56987, "vy":-0.00371, "omega":-0.15257, "ax":4.58861, "ay":0.17199, "alpha":1.24671, "fx":[75.06515,75.4001,74.463,75.13216], "fy":[-6.89192,0.42174,11.79652,5.92054]}, - {"t":1.76659, "x":5.21069, "y":2.86284, "heading":2.10041, "vx":-0.45598, "vy":0.00056, "omega":-0.12162, "ax":4.59143, "ay":0.09596, "alpha":1.23766, "fx":[74.9517,75.40244,74.65475,75.2356], "fy":[-8.08368,-0.70384,10.54795,4.51468]}, - {"t":1.79141, "x":5.20079, "y":2.86288, "heading":2.09739, "vx":-0.34201, "vy":0.00294, "omega":-0.0909, "ax":4.59293, "ay":0.02514, "alpha":1.22898, "fx":[74.82949,75.38964,74.81475,75.30849], "fy":[-9.18828,-1.74794,9.38067,3.19961]}, - {"t":1.81623, "x":5.19371, "y":2.86296, "heading":2.09513, "vx":-0.22801, "vy":0.00357, "omega":-0.0604, "ax":4.59334, "ay":-0.04094, "alpha":1.22067, "fx":[74.70121,75.36483,74.94776,75.35593], "fy":[-10.21429,-2.71757,8.28801,1.9667]}, - {"t":1.84105, "x":5.18947, "y":2.86304, "heading":2.09364, "vx":-0.114, "vy":0.00255, "omega":-0.0301, "ax":4.59288, "ay":-0.10271, "alpha":1.21272, "fx":[74.56897,75.33061,75.05779,75.38211], "fy":[-11.16926,-3.619,7.2639,0.80819]}, - {"t":1.86587, "x":5.18805, "y":2.86307, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.92904, "y":5.58344, "heading":-2.79253, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.82756, "ay":-6.05685, "alpha":-2.6067, "fx":[-49.40678,-32.35104,-10.8985,-26.85221], "fy":[-92.00246,-99.3189,-103.86752,-100.88274]}, + {"t":0.02952, "x":7.92824, "y":5.5808, "heading":-2.79253, "vx":-0.05395, "vy":-0.1788, "omega":-0.07695, "ax":-1.85299, "ay":-6.04996, "alpha":-2.58106, "fx":[-49.59558,-32.67552,-11.48923,-27.41103], "fy":[-91.89332,-99.20657,-103.79727,-100.72423]}, + {"t":0.05904, "x":7.92584, "y":5.57289, "heading":-2.7948, "vx":-0.10865, "vy":-0.35739, "omega":-0.15314, "ax":-1.88078, "ay":-6.04229, "alpha":-2.55294, "fx":[-49.8035,-33.06817,-12.15221,-27.96482], "fy":[-91.77252,-99.06972,-103.71459,-100.56305]}, + {"t":0.08856, "x":7.92181, "y":5.55971, "heading":-2.79932, "vx":-0.16417, "vy":-0.53576, "omega":-0.22851, "ax":-1.91129, "ay":-6.0337, "alpha":-2.52194, "fx":[-50.03297,-33.53219,-12.89502,-28.52356], "fy":[-91.63842,-98.90625,-103.61703,-100.39635]}, + {"t":0.11808, "x":7.91613, "y":5.54126, "heading":-2.80606, "vx":-0.22059, "vy":-0.71388, "omega":-0.30295, "ax":-1.94494, "ay":-6.02402, "alpha":-2.48755, "fx":[-50.28668,-34.07153,-13.72706,-29.09908], "fy":[-91.4892,-98.71356,-103.50146,-100.22047]}, + {"t":0.1476, "x":7.90877, "y":5.51756, "heading":-2.81501, "vx":-0.27801, "vy":-0.89171, "omega":-0.37639, "ax":-1.98226, "ay":-6.01302, "alpha":-2.44919, "fx":[-50.56782,-34.69103,-14.65999,-29.70553], "fy":[-91.32266,-98.48839,-103.3639,-100.03072]}, + {"t":0.17712, "x":7.8997, "y":5.48862, "heading":-2.82612, "vx":-0.33652, "vy":-1.06921, "omega":-0.44869, "ax":-2.02386, "ay":-6.00044, "alpha":-2.40611, "fx":[-50.88027,-35.39678,-15.70832,-30.35993], "fy":[-91.13608,-98.22662,-103.19913,-99.8211]}, + {"t":0.20664, "x":7.88889, "y":5.45444, "heading":-2.83936, "vx":-0.39627, "vy":-1.24635, "omega":-0.51972, "ax":-2.07055, "ay":-5.98592, "alpha":-2.35741, "fx":[-51.22887,-36.19652,-16.8902,-31.08292], "fy":[-90.92605,-97.92303,-103.00031,-99.58391]}, + {"t":0.23616, "x":7.87629, "y":5.41504, "heading":-2.85471, "vx":-0.45739, "vy":-1.42305, "omega":-0.58931, "ax":-2.12331, "ay":-5.96899, "alpha":-2.30192, "fx":[-51.61987,-37.10015,-18.22856,-31.89974], "fy":[-90.68812,-97.57085,-102.75831,-99.30907]}, + {"t":0.26568, "x":7.86186, "y":5.37043, "heading":-2.8721, "vx":-0.52007, "vy":-1.59926, "omega":-0.65726, "ax":-2.18338, "ay":-5.94904, "alpha":-2.23817, "fx":[-52.06147,-38.12066,-19.7527,-32.84159], "fy":[-90.41642,-97.16122,-102.46069,-98.98329]}, + {"t":0.2952, "x":7.84556, "y":5.32063, "heading":-2.89151, "vx":-0.58453, "vy":-1.77488, "omega":-0.72333, "ax":-2.25237, "ay":-5.92522, "alpha":-2.16421, "fx":[-52.56462,-39.27521,-21.50059,-33.94743], "fy":[-90.10303,-96.68224,-102.09018,-98.58864]}, + {"t":0.32472, "x":7.82732, "y":5.26565, "heading":-2.91286, "vx":-0.65102, "vy":-1.94979, "omega":-0.78722, "ax":-2.33238, "ay":-5.89637, "alpha":-2.07744, "fx":[-53.14409,-40.58702,-23.52235,-35.26648], "fy":[-89.73706,-96.11759,-101.62226,-98.10053]}, + {"t":0.35424, "x":7.80708, "y":5.20553, "heading":-2.9361, "vx":-0.71987, "vy":-2.12385, "omega":-0.84855, "ax":-2.42621, "ay":-5.86082, "alpha":-1.97435, "fx":[-53.8202,-42.08811,-25.88533,-36.86177], "fy":[-89.3033,-95.44426,-101.02106,-97.4844]}, + {"t":0.38376, "x":7.78478, "y":5.14028, "heading":-2.96115, "vx":-0.79149, "vy":-2.29686, "omega":-0.90683, "ax":-2.53764, "ay":-5.81615, "alpha":-1.8501, "fx":[-54.62123,-43.82369,-28.68207,-38.81503], "fy":[-88.77998,-94.62872,-100.23243,-96.69045]}, + {"t":0.41328, "x":7.76031, "y":5.06994, "heading":-2.98792, "vx":-0.8664, "vy":-2.46856, "omega":-0.96144, "ax":-2.67192, "ay":-5.75865, "alpha":-1.69779, "fx":[-55.58722,-45.85937,-32.04269,-41.23364], "fy":[-88.13525,-93.62025,-99.17163,-95.64485]}, + {"t":0.4428, "x":7.73357, "y":4.99456, "heading":-3.0163, "vx":-0.94528, "vy":-2.63855, "omega":-1.01156, "ax":-2.83648, "ay":-5.68253, "alpha":-1.50728, "fx":[-56.77605,-48.29352,-36.15451,-44.26023], "fy":[-87.32103,-92.33846,-97.70007,-94.23489]}, + {"t":0.47232, "x":7.70442, "y":4.91419, "heading":-3.04616, "vx":-1.02901, "vy":-2.8063, "omega":-1.05606, "ax":-3.04211, "ay":-5.57822, "alpha":-1.26316, "fx":[-58.27331,-51.27912,-41.29318,-48.08524], "fy":[-86.26211,-90.64878,-95.57967,-92.28284]}, + {"t":0.50184, "x":7.67272, "y":4.82892, "heading":-3.07733, "vx":-1.11881, "vy":-2.97097, "omega":-1.09335, "ax":-3.30472, "ay":-5.42911, "alpha":-0.94094, "fx":[-60.20927,-55.06472,-47.86888,-52.96082], "fy":[-84.8354,-88.30984,-92.37791,-89.49928]}, + {"t":0.53136, "x":7.63826, "y":4.73885, "heading":-3.10961, "vx":-1.21637, "vy":-3.13124, "omega":-1.12112, "ax":-3.64795, "ay":-5.20454, "alpha":-0.49987, "fx":[-62.78893,-60.07332,-56.47894,-59.20688], "fy":[-82.82758,-84.84963,-87.26106,-85.39888]}, + {"t":0.56088, "x":7.60076, "y":4.64415, "heading":3.14048, "vx":-1.32406, "vy":-3.28488, "omega":-1.13588, "ax":-4.10529, "ay":-4.8442, "alpha":0.12969, "fx":[-66.34713,-67.05374,-67.87833,-67.17536], "fy":[-79.84084,-79.23756,-78.54135,-79.15393]}, + {"t":0.5904, "x":7.55988, "y":4.54507, "heading":3.10695, "vx":-1.44525, "vy":-3.42788, "omega":-1.13205, "ax":-4.71338, "ay":-4.22331, "alpha":1.0621, "fx":[-71.44924,-77.29481,-82.40746,-77.0679], "fy":[-75.05949,-68.89894,-62.83489,-69.37885]}, + {"t":0.61992, "x":7.51517, "y":4.44203, "heading":3.07353, "vx":-1.58439, "vy":-3.55255, "omega":-1.1007, "ax":-5.45583, "ay":-3.09038, "alpha":2.44948, "fx":[-79.04381,-91.98409,-97.37958,-88.36212], "fy":[-66.59324,-46.61336,-34.87308,-54.00779]}, + {"t":0.64944, "x":7.46602, "y":4.33582, "heading":3.04104, "vx":-1.74544, "vy":-3.64378, "omega":-1.02839, "ax":-6.03716, "ay":-1.10585, "alpha":4.44215, "fx":[-90.35222,-102.57268,-103.18875,-98.67032], "fy":[-49.33084,2.55687,5.48564,-31.02578]}, + {"t":0.67896, "x":7.41186, "y":4.22777, "heading":3.01068, "vx":-1.92366, "vy":-3.67643, "omega":-0.89726, "ax":-6.01766, "ay":1.15619, "alpha":4.55267, "fx":[-102.34262,-91.11873,-96.72794,-103.31978], "fy":[-7.86977,47.42265,36.37633,-0.32315]}, + {"t":0.70848, "x":7.35245, "y":4.11974, "heading":2.98419, "vx":-2.1013, "vy":-3.64229, "omega":-0.76286, "ax":-5.64051, "ay":2.78216, "alpha":1.57679, "fx":[-94.98729,-87.0159,-90.25689,-96.58605], "fy":[39.88042,55.30723,50.25302,36.49164]}, + {"t":0.738, "x":7.28796, "y":4.01344, "heading":2.96167, "vx":-2.26781, "vy":-3.56016, "omega":-0.71631, "ax":-4.99191, "ay":3.86851, "alpha":-0.89519, "fx":[-80.51178,-85.47975,-83.00155,-77.43955], "fy":[64.93108,58.153,61.46092,68.42597]}, + {"t":0.76752, "x":7.21884, "y":3.91002, "heading":2.94053, "vx":-2.41517, "vy":-3.44597, "omega":-0.74274, "ax":-4.31886, "ay":4.55128, "alpha":-2.57482, "fx":[-69.05758,-83.73314,-74.2717,-55.35833], "fy":[77.36564,60.98232,71.79125,87.48]}, + {"t":0.79704, "x":7.14566, "y":3.81028, "heading":2.9186, "vx":-2.54267, "vy":-3.31161, "omega":-0.81875, "ax":-3.73366, "ay":4.98437, "alpha":-3.56652, "fx":[-60.54903,-81.50757,-64.62036,-37.47572], "fy":[84.40772,64.16764,80.66698,96.69726]}, + {"t":0.82656, "x":7.06898, "y":3.71469, "heading":2.89443, "vx":-2.65288, "vy":-3.16447, "omega":-0.92403, "ax":-3.18056, "ay":5.34541, "alpha":-3.78436, "fx":[-52.92541,-76.13123,-53.14947,-25.77848], "fy":[89.51293,70.62402,88.81251,100.59946]}, + {"t":0.85608, "x":6.98928, "y":3.62361, "heading":2.86715, "vx":-2.74678, "vy":-3.00667, "omega":-1.03575, "ax":-2.58228, "ay":5.71024, "alpha":-3.15347, "fx":[-44.24883,-64.26795,-40.41115,-19.9337], "fy":[94.17556,81.67704,95.52409,102.02931]}, + {"t":0.8856, "x":6.90707, "y":3.53734, "heading":2.83658, "vx":-2.823, "vy":-2.83811, "omega":-1.12884, "ax":-2.05804, "ay":5.96566, "alpha":-2.39845, "fx":[-36.01438,-51.26872,-30.85108,-16.44565], "fy":[97.67387,90.49561,99.20709,102.73211]}, + {"t":0.91512, "x":6.82284, "y":3.45616, "heading":2.80325, "vx":-2.88376, "vy":-2.662, "omega":-1.19964, "ax":-1.63201, "ay":6.12597, "alpha":-1.74183, "fx":[-28.85013,-39.77424,-24.09,-14.00684], "fy":[100.06804,96.17938,101.18713,103.15715]}, + {"t":0.94464, "x":6.737, "y":3.38024, "heading":2.76784, "vx":-2.93194, "vy":-2.48116, "omega":-1.25106, "ax":-1.28929, "ay":6.22532, "alpha":-1.20316, "fx":[-22.78184,-30.21096,-19.1144,-12.20242], "fy":[101.65862,99.65943,102.33287,103.43739]}, + {"t":0.97416, "x":6.64988, "y":3.30971, "heading":2.73091, "vx":-2.97, "vy":-2.29739, "omega":-1.28658, "ax":-1.01189, "ay":6.2871, "alpha":-0.76488, "fx":[-17.69592,-22.37246,-15.27186,-10.82994], "fy":[102.69802,101.75602,103.04226,103.63199]}, + {"t":1.00368, "x":6.56177, "y":3.24463, "heading":2.69293, "vx":-2.99987, "vy":-2.11179, "omega":-1.30916, "ax":-0.78483, "ay":6.3256, "alpha":-0.40574, "fx":[-13.45143,-15.9286,-12.17505,-9.76675], "fy":[103.36716,103.00204,103.50428,103.77251]}, + {"t":1.0332, "x":6.47287, "y":3.18505, "heading":2.65428, "vx":-3.02304, "vy":-1.92506, "omega":-1.32113, "ax":-0.5966, "ay":6.34943, "alpha":-0.10794, "fx":[-9.91466,-10.57842,-9.58932,-8.9304], "fy":[103.79,103.7214,103.8155,103.87737]}, + {"t":1.06273, "x":6.38337, "y":3.13099, "heading":2.61528, "vx":-3.04065, "vy":-1.73762, "omega":-1.32432, "ax":-0.26239, "ay":6.36782, "alpha":0.10086, "fx":[-4.12554,-3.52058,-4.4513,-5.0611], "fy":[104.1069,104.13267,104.0992,104.06784]}, + {"t":1.08653, "x":6.31091, "y":3.09143, "heading":2.58376, "vx":-3.04689, "vy":-1.58604, "omega":-1.32192, "ax":0.27992, "ay":6.36508, "alpha":0.32273, "fx":[5.24639,6.9942,3.93872,2.12506], "fy":[104.03094,103.94062,104.10765,104.14854]}, + {"t":1.11034, "x":6.23846, "y":3.05547, "heading":2.55229, "vx":-3.04023, "vy":-1.43451, "omega":-1.31424, "ax":0.86993, "ay":6.30858, "alpha":0.56236, "fx":[15.67489,18.29019,12.89337,10.02841], "fy":[102.95179,102.54372,103.37119,103.66616]}, + {"t":1.13414, "x":6.16633, "y":3.02311, "heading":2.521, "vx":-3.01952, "vy":-1.28433, "omega":-1.30085, "ax":1.49665, "ay":6.18576, "alpha":0.81528, "fx":[27.0081,30.0384,22.23515,18.58763], "fy":[100.55147,99.72711,101.76436,102.45895]}, + {"t":1.15795, "x":6.09487, "y":2.99429, "heading":2.49004, "vx":-2.98389, "vy":-1.13708, "omega":-1.28144, "ax":2.14259, "ay":5.98734, "alpha":1.07514, "fx":[38.92231,41.79032,31.72658,27.67009], "fy":[96.54642,95.39671,99.21238,100.37081]}, + {"t":1.18175, "x":6.02445, "y":2.96892, "heading":2.45953, "vx":-2.93289, "vy":-0.99455, "omega":-1.25585, "ax":2.78507, "ay":5.71019, "alpha":1.33405, "fx":[50.91898,53.04518,41.09337,37.06498], "fy":[90.77896,89.6301,95.71368,97.28023]}, + {"t":1.20556, "x":5.95542, "y":2.94686, "heading":2.42963, "vx":-2.86659, "vy":-0.85861, "omega":-1.22409, "ax":3.39934, "ay":5.3595, "alpha":1.58298, "fx":[62.38998,63.34715,50.06026,46.49364], "fy":[83.307,82.68047,91.34918,93.13351]}, + {"t":1.22936, "x":5.88814, "y":2.92794, "heading":2.40049, "vx":-2.78566, "vy":-0.73103, "omega":-1.18641, "ax":3.96302, "ay":4.9487, "alpha":1.8126, "fx":[72.74755,72.3729,58.38965,55.64105], "fy":[74.43841,74.92454,86.27307,87.97106]}, + {"t":1.25317, "x":5.82295, "y":2.91194, "heading":2.37225, "vx":-2.69132, "vy":-0.61322, "omega":-1.14326, "ax":4.46003, "ay":4.49706, "alpha":2.01472, "fx":[81.56502,79.97269,65.91113,64.20316], "fy":[64.67409,66.77771,80.68737,81.93431]}, + {"t":1.27697, "x":5.76014, "y":2.89861, "heading":2.34504, "vx":-2.58515, "vy":-0.50617, "omega":-1.0953, "ax":4.88258, "ay":4.02573, "alpha":2.1841, "fx":[88.65689,86.15718,72.53387,71.93539], "fy":[54.58131,58.61479,74.80906,75.24677]}, + {"t":1.30078, "x":5.69999, "y":2.88771, "heading":2.31896, "vx":-2.46892, "vy":-0.41033, "omega":-1.0433, "ax":5.23064, "ay":3.55396, "alpha":2.31944, "fx":[94.06751,91.04969,78.24149,78.68529], "fy":[44.66424,50.72283,68.84031,68.17485]}, + {"t":1.32458, "x":5.6427, "y":2.87894, "heading":2.29413, "vx":-2.3444, "vy":-0.32573, "omega":-0.98809, "ax":5.50986, "ay":3.09682, "alpha":2.42297, "fx":[97.99633,94.83136,83.07536,84.40007], "fy":[35.28733,43.28966,62.94878,60.98296]}, + {"t":1.34839, "x":5.58845, "y":2.87207, "heading":2.2706, "vx":-2.21324, "vy":-0.25201, "omega":-0.93041, "ax":5.72902, "ay":2.66447, "alpha":2.49912, "fx":[100.71071,97.69779,87.1138,89.11174], "fy":[26.66193,36.41639,57.25952,53.89791]}, + {"t":1.3722, "x":5.53738, "y":2.86682, "heading":2.24845, "vx":-2.07685, "vy":-0.18858, "omega":-0.87091, "ax":5.89787, "ay":2.26263, "alpha":2.55314, "fx":[102.48044,99.83218,90.45326,92.91008], "fy":[18.87348,30.13941,51.85601,47.09006]}, + {"t":1.396, "x":5.48962, "y":2.86298, "heading":2.22772, "vx":-1.93645, "vy":-0.13472, "omega":-0.81014, "ax":6.02584, "ay":1.89366, "alpha":2.59001, "fx":[103.54248,101.3929,93.19409,95.91474], "fy":[11.92165,24.45254,46.78638,40.67063]}, + {"t":1.41981, "x":5.44522, "y":2.86031, "heading":2.20844, "vx":-1.793, "vy":-0.08964, "omega":-0.74848, "ax":6.12126, "ay":1.55755, "alpha":2.61398, "fx":[104.08872,102.51022,95.43165,98.25305], "fy":[5.7564,19.32463,42.07167,34.69927]}, + {"t":1.44361, "x":5.40428, "y":2.85861, "heading":2.19062, "vx":-1.64729, "vy":-0.05256, "omega":-0.68625, "ax":6.19112, "ay":1.25284, "alpha":2.6284, "fx":[104.26699,103.28806,97.25151,100.04568], "fy":[0.30406,14.71201,37.71365,29.19625]}, + {"t":1.46742, "x":5.36682, "y":2.85772, "heading":2.17428, "vx":-1.4999, "vy":-0.02274, "omega":-0.62368, "ax":6.24113, "ay":0.97728, "alpha":2.63587, "fx":[104.18767,103.80773,98.72767,101.39925], "fy":[-4.51628,10.56653,33.7015,24.1546]}, + {"t":1.49122, "x":5.33288, "y":2.85745, "heading":2.15944, "vx":-1.35133, "vy":0.00053, "omega":-0.56093, "ax":6.27581, "ay":0.72828, "alpha":2.63831, "fx":[103.93157,104.13215,99.92256,102.40376], "fy":[-8.78357,6.84027,30.0168,19.55046]}, + {"t":1.51503, "x":5.30249, "y":2.85767, "heading":2.14608, "vx":-1.20193, "vy":0.01786, "omega":-0.49813, "ax":6.29871, "ay":0.5032, "alpha":2.63714, "fx":[103.55709,104.30966,100.88801,103.13288], "fy":[-12.57029,3.48807,26.63715,15.35071]}, + {"t":1.53883, "x":5.27566, "y":2.85824, "heading":2.13422, "vx":-1.05199, "vy":0.02984, "omega":-0.43535, "ax":6.3126, "ay":0.2995, "alpha":2.63341, "fx":[103.10609,104.37736,101.66657,103.64572], "fy":[-15.94075,0.46879,23.53858,11.5183]}, + {"t":1.56264, "x":5.25241, "y":2.85903, "heading":2.12386, "vx":-0.90171, "vy":0.03697, "omega":-0.37266, "ax":6.31961, "ay":0.11481, "alpha":2.62789, "fx":[102.60841,104.36366,102.29296,103.98902], "fy":[-18.95083,-2.25435,20.69711,8.01555]}, + {"t":1.58644, "x":5.23273, "y":2.85995, "heading":2.11499, "vx":-0.75127, "vy":0.03971, "omega":-0.3101, "ax":6.32138, "ay":-0.05301, "alpha":2.62114, "fx":[102.08519,104.29034,102.79541,104.19945], "fy":[-21.64855,-4.71383,18.08969,4.80623]}, + {"t":1.61025, "x":5.21664, "y":2.86088, "heading":2.10761, "vx":-0.60079, "vy":0.03844, "omega":-0.2477, "ax":6.3192, "ay":-0.20586, "alpha":2.61359, "fx":[101.55135,104.17414,103.19681,104.30555], "fy":[-24.07485,-6.9381,15.6947,1.85663]}, + {"t":1.63406, "x":5.20413, "y":2.86173, "heading":2.10171, "vx":-0.45036, "vy":0.03354, "omega":-0.18549, "ax":6.31405, "ay":-0.34543, "alpha":2.60556, "fx":[101.01732,104.02794,103.51573,104.32949], "fy":[-26.26456,-8.95198,13.49221,-0.86399]}, + {"t":1.65786, "x":5.19519, "y":2.86243, "heading":2.09729, "vx":-0.30005, "vy":0.02532, "omega":-0.12346, "ax":6.30666, "ay":-0.4732, "alpha":2.59728, "fx":[100.49036,103.86168,103.76726,104.28842], "fy":[-28.24725,-10.77708,11.464,-3.38323]}, + {"t":1.68167, "x":5.18984, "y":2.8629, "heading":2.09436, "vx":-0.14992, "vy":0.01406, "omega":-0.06163, "ax":6.29764, "ay":-0.59047, "alpha":2.58894, "fx":[99.97543,103.68303,103.96366,104.19558], "fy":[-30.04804,-12.43221,9.59357,-5.72569]}, + {"t":1.70547, "x":5.18805, "y":2.86307, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/NItoIJ.traj b/src/main/deploy/choreo/NItoIJ.traj index fd1c23df..8b317490 100644 --- a/src/main/deploy/choreo/NItoIJ.traj +++ b/src/main/deploy/choreo/NItoIJ.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":8.08, "y":5.0, "heading":3.141592653589793, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.929035186767578, "y":5.583443641662598, "heading":3.490658503988659, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.188053131103516, "y":5.171266078948975, "heading":-2.073639537722758, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"NI.x", "val":8.08}, "y":{"exp":"NI.y", "val":5.0}, "heading":{"exp":"NI.heading", "val":3.141592653589793}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.929035186767578 m", "val":7.929035186767578}, "y":{"exp":"5.583443641662598 m", "val":5.583443641662598}, "heading":{"exp":"200 deg", "val":3.490658503988659}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"IJ.x", "val":5.188053131103516}, "y":{"exp":"IJ.y", "val":5.171266078948975}, "heading":{"exp":"IJ.heading", "val":-2.073639537722758}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -26,54 +26,45 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.59192], + "waypoints":[0.0,1.32063], "samples":[ - {"t":0.0, "x":8.08, "y":5.0, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.57063, "ay":0.27087, "alpha":1.73375, "fx":[-75.23066,-73.85697,-74.4693,-75.32756], "fy":[-5.45257,15.32339,12.12694,-4.2852]}, - {"t":0.03538, "x":8.07714, "y":5.00017, "heading":3.14159, "vx":-0.16169, "vy":0.00958, "omega":0.06133, "ax":-4.57038, "ay":0.27085, "alpha":1.73375, "fx":[-75.22592,-73.85239,-74.46565,-75.32383], "fy":[-5.45224,15.32248,12.12638,-4.285]}, - {"t":0.07075, "x":8.06856, "y":5.00068, "heading":-3.13942, "vx":-0.32337, "vy":0.01916, "omega":0.12267, "ax":-4.5701, "ay":0.27082, "alpha":1.73364, "fx":[-75.22218,-73.84399,-74.46488,-75.31866], "fy":[-5.4311,15.3379,12.10592,-4.3032]}, - {"t":0.10613, "x":8.05426, "y":5.00153, "heading":-3.13508, "vx":-0.48504, "vy":0.02874, "omega":0.184, "ax":-4.5698, "ay":0.27077, "alpha":1.73342, "fx":[-75.21935,-73.83174,-74.46694,-75.31198], "fy":[-5.38894,15.3694,12.06548,-4.3397]}, - {"t":0.1415, "x":8.03424, "y":5.00271, "heading":-3.12857, "vx":-0.64671, "vy":0.03832, "omega":0.24532, "ax":-4.56947, "ay":0.2707, "alpha":1.73306, "fx":[-75.21732,-73.81562,-74.47174,-75.3037], "fy":[-5.32532,15.4165,12.00489,-4.39428]}, - {"t":0.17688, "x":8.00851, "y":5.00424, "heading":-3.1199, "vx":-0.80836, "vy":0.0479, "omega":0.30663, "ax":-4.5691, "ay":0.27062, "alpha":1.73253, "fx":[-75.21598,-73.79564,-74.47919,-75.29369], "fy":[-5.23961,15.47852,11.9239,-4.46664]}, - {"t":0.21226, "x":7.97705, "y":5.0061, "heading":-3.10905, "vx":-0.96999, "vy":0.05747, "omega":0.36792, "ax":-4.5687, "ay":0.27051, "alpha":1.73178, "fx":[-75.21514,-73.77184,-74.48916,-75.28182], "fy":[-5.131,15.55453,11.82222,-4.55636]}, - {"t":0.24763, "x":7.93988, "y":5.0083, "heading":-3.09603, "vx":-1.13161, "vy":0.06704, "omega":0.42918, "ax":-4.56824, "ay":0.27039, "alpha":1.73078, "fx":[-75.21458,-73.74424,-74.5015,-75.26791], "fy":[-4.99847,15.64338,11.69945,-4.66291]}, - {"t":0.28301, "x":7.89699, "y":5.01084, "heading":-3.08085, "vx":-1.29322, "vy":0.07661, "omega":0.49041, "ax":-4.56773, "ay":0.27025, "alpha":1.72946, "fx":[-75.21402,-73.71288,-74.51599,-75.25173], "fy":[-4.84081,15.74366,11.55516,-4.78561]}, - {"t":0.31838, "x":7.84838, "y":5.01372, "heading":-3.0635, "vx":-1.45481, "vy":0.08617, "omega":0.55159, "ax":-4.56714, "ay":0.2701, "alpha":1.72774, "fx":[-75.21305,-73.67778,-74.53234,-75.23301], "fy":[-4.65665,15.85374,11.38885,-4.92366]}, - {"t":0.35376, "x":7.79406, "y":5.01694, "heading":-3.04399, "vx":-1.61638, "vy":0.09572, "omega":0.61271, "ax":-4.56646, "ay":0.26993, "alpha":1.72556, "fx":[-75.21115,-73.63896,-74.5502,-75.21138], "fy":[-4.44445,15.97169,11.19998,-5.07609]}, - {"t":0.38914, "x":7.73402, "y":5.0205, "heading":-3.02231, "vx":-1.77792, "vy":0.10527, "omega":0.67375, "ax":-4.56566, "ay":0.26974, "alpha":1.72283, "fx":[-75.20761,-73.59633,-74.56905,-75.18638], "fy":[-4.20254,16.09534,10.98796,-5.24176]}, - {"t":0.42451, "x":7.66826, "y":5.02439, "heading":-2.99848, "vx":-1.93943, "vy":0.11481, "omega":0.7347, "ax":-4.5647, "ay":0.26954, "alpha":1.71946, "fx":[-75.20141,-73.5497,-74.5882,-75.15737], "fy":[-3.92911,16.22218,10.75218,-5.41928]}, - {"t":0.45989, "x":7.5968, "y":5.02862, "heading":-2.97249, "vx":-2.10092, "vy":0.12435, "omega":0.79553, "ax":-4.56353, "ay":0.26933, "alpha":1.71535, "fx":[-75.19114,-73.49868,-74.60668,-75.12346], "fy":[-3.62225,16.34938,10.49196,-5.60701]}, - {"t":0.49526, "x":7.51962, "y":5.03319, "heading":-2.94435, "vx":-2.26236, "vy":0.13388, "omega":0.85621, "ax":-4.56205, "ay":0.2691, "alpha":1.7104, "fx":[-75.17473,-73.44245,-74.62306,-75.08332], "fy":[-3.28001,16.47367,10.2066,-5.80299]}, - {"t":0.53064, "x":7.43673, "y":5.03809, "heading":-2.91406, "vx":-2.42374, "vy":0.1434, "omega":0.91672, "ax":-4.56014, "ay":0.26886, "alpha":1.70449, "fx":[-75.14903,-73.3795,-74.63518,-75.03489], "fy":[-2.90038,16.59123,9.89535,-6.00477]}, - {"t":0.56602, "x":7.34814, "y":5.04333, "heading":-2.88163, "vx":-2.58506, "vy":0.15291, "omega":0.97701, "ax":-4.55757, "ay":0.2686, "alpha":1.69749, "fx":[-75.109,-73.3069,-74.63953,-74.9747], "fy":[-2.48131,16.69738,9.55731,-6.20927]}, - {"t":0.60139, "x":7.25384, "y":5.04891, "heading":-2.84706, "vx":-2.74629, "vy":0.16241, "omega":1.03707, "ax":-4.55391, "ay":0.26829, "alpha":1.6892, "fx":[-75.04585,-73.21892,-74.62994,-74.89652], "fy":[-2.02076,16.78596,9.19134,-6.41226]}, - {"t":0.63677, "x":7.15383, "y":5.05482, "heading":-2.81038, "vx":-2.90739, "vy":0.1719, "omega":1.09682, "ax":-4.54836, "ay":0.26791, "alpha":1.67928, "fx":[-74.94252,-73.10334,-74.59435,-74.78776], "fy":[-1.51657,16.84784,8.79564,-6.60744]}, - {"t":0.67214, "x":7.04814, "y":5.06107, "heading":-2.77158, "vx":-3.06829, "vy":0.18138, "omega":1.15623, "ax":-4.53898, "ay":0.26736, "alpha":1.66702, "fx":[-74.76024,-72.93057,-74.50507,-74.61887], "fy":[-0.96608,16.86643,8.36663,-6.78355]}, - {"t":0.70752, "x":6.93675, "y":5.06766, "heading":-2.73067, "vx":-3.22886, "vy":0.19084, "omega":1.2152, "ax":-4.52004, "ay":0.26634, "alpha":1.65038, "fx":[-74.38494,-72.61024,-74.27985,-74.30095], "fy":[-0.36449,16.80048,7.89432,-6.91368]}, - {"t":0.7429, "x":6.8197, "y":5.07457, "heading":-2.68768, "vx":-3.38877, "vy":0.20026, "omega":1.27359, "ax":-4.46309, "ay":0.26334, "alpha":1.6199, "fx":[-73.25905,-71.6897,-73.51189,-73.39138], "fy":[0.30637,16.46839,7.33039,-6.88467]}, - {"t":0.77827, "x":6.69703, "y":5.08182, "heading":-2.64263, "vx":-3.54665, "vy":0.20958, "omega":1.33089, "ax":-0.02749, "ay":0.01122, "alpha":-1.3406, "fx":[-5.45065,-1.87675,4.55646,0.97341], "fy":[1.71464,-4.85157,-1.34872,5.2192]}, - {"t":0.81365, "x":6.57154, "y":5.08924, "heading":-2.59555, "vx":-3.54762, "vy":0.20997, "omega":1.28347, "ax":4.46053, "ay":-0.26213, "alpha":-1.67818, "fx":[73.21363,71.58193,73.60859,73.28022], "fy":[-1.42247,-17.05682,-6.44465,7.78251]}, - {"t":0.84902, "x":6.44883, "y":5.09651, "heading":-2.55014, "vx":-3.38983, "vy":0.2007, "omega":1.2241, "ax":4.51925, "ay":-0.26598, "alpha":-1.66015, "fx":[74.33742,72.55253,74.46251,74.17183], "fy":[-2.16249,-17.17994,-6.0133,7.96237]}, - {"t":0.8844, "x":6.33174, "y":5.10344, "heading":-2.50684, "vx":-3.22996, "vy":0.19129, "omega":1.16537, "ax":4.53897, "ay":-0.2674, "alpha":-1.65284, "fx":[74.69855,72.88273,74.77219,74.46083], "fy":[-2.84389,-17.20304,-5.56114,8.12247]}, - {"t":0.91978, "x":6.22032, "y":5.11004, "heading":-2.46561, "vx":-3.06938, "vy":0.18183, "omega":1.1069, "ax":4.54886, "ay":-0.2682, "alpha":-1.64841, "fx":[74.86401,73.05551,74.94129,74.59998], "fy":[-3.49162,-17.18295,-5.11862,8.25504]}, - {"t":0.95515, "x":6.11458, "y":5.1163, "heading":-2.42646, "vx":-2.90846, "vy":0.17234, "omega":1.04858, "ax":4.55479, "ay":-0.26876, "alpha":-1.64546, "fx":[74.94891,73.1674,75.05167,74.68057], "fy":[-4.10806,-17.13559,-4.69283,8.36164]}, - {"t":0.99053, "x":6.01454, "y":5.12223, "heading":-2.38936, "vx":-2.74733, "vy":0.16283, "omega":0.99037, "ax":4.55873, "ay":-0.2692, "alpha":-1.64352, "fx":[74.9923,73.25002,75.13095,74.73297], "fy":[-4.69258,-17.06903,-4.28662,8.44486]}, - {"t":1.02591, "x":5.9202, "y":5.12783, "heading":-2.35433, "vx":-2.58606, "vy":0.15331, "omega":0.93223, "ax":4.56153, "ay":-0.26956, "alpha":-1.64238, "fx":[75.01143,73.31651,75.19117,74.77008], "fy":[-5.24418,-16.9886,-3.90145,8.50736]}, - {"t":1.06128, "x":5.83157, "y":5.13308, "heading":-2.32135, "vx":-2.4247, "vy":0.14378, "omega":0.87413, "ax":4.56361, "ay":-0.26986, "alpha":-1.64189, "fx":[75.01531,73.37309,75.23855,74.79826], "fy":[-5.76202,-16.89837,-3.53817,8.55172]}, - {"t":1.09666, "x":5.74865, "y":5.138, "heading":-2.29042, "vx":-2.26325, "vy":0.13423, "omega":0.81605, "ax":4.56521, "ay":-0.27012, "alpha":-1.6419, "fx":[75.00926,73.42301,75.27665,74.82088], "fy":[-6.24552,-16.80164,-3.1973,8.58042]}, - {"t":1.13203, "x":5.67145, "y":5.14258, "heading":-2.26156, "vx":-2.10175, "vy":0.12467, "omega":0.75796, "ax":4.56647, "ay":-0.27035, "alpha":-1.64233, "fx":[74.99671,73.468,75.30775,74.83991], "fy":[-6.69439,-16.70121,-2.87917,8.5958]}, - {"t":1.16741, "x":5.59995, "y":5.14682, "heading":-2.23474, "vx":-1.94021, "vy":0.11511, "omega":0.69986, "ax":4.56749, "ay":-0.27055, "alpha":-1.64305, "fx":[74.98005,73.50904,75.33339,74.85648], "fy":[-7.10857,-16.59948,-2.58398,8.60011]}, - {"t":1.20279, "x":5.53417, "y":5.15072, "heading":-2.20998, "vx":-1.77863, "vy":0.10554, "omega":0.64174, "ax":4.56832, "ay":-0.27072, "alpha":-1.64399, "fx":[74.961,73.54669,75.35467,74.87129], "fy":[-7.48824,-16.49856,-2.31182,8.59549]}, - {"t":1.23816, "x":5.47411, "y":5.15429, "heading":-2.18728, "vx":-1.61702, "vy":0.09596, "omega":0.58358, "ax":4.56902, "ay":-0.27087, "alpha":-1.64506, "fx":[74.94086,73.58123,75.37242,74.88475], "fy":[-7.83371,-16.40032,-2.06271,8.58393]}, - {"t":1.27354, "x":5.41976, "y":5.15751, "heading":-2.16664, "vx":-1.45539, "vy":0.08638, "omega":0.52539, "ax":4.56961, "ay":-0.271, "alpha":-1.64621, "fx":[74.92063,73.61284,75.38728,74.8971], "fy":[-8.14542,-16.30637,-1.83664,8.5673]}, - {"t":1.30891, "x":5.37114, "y":5.1604, "heading":-2.14805, "vx":-1.29373, "vy":0.07679, "omega":0.46715, "ax":4.57012, "ay":-0.27111, "alpha":-1.64737, "fx":[74.90111,73.64157,75.39977,74.90849], "fy":[-8.42388,-16.21815,-1.63354,8.54733]}, - {"t":1.34429, "x":5.32823, "y":5.16294, "heading":-2.13152, "vx":-1.13206, "vy":0.0672, "omega":0.40887, "ax":4.57056, "ay":-0.2712, "alpha":-1.6485, "fx":[74.88293,73.66747,75.4103,74.91897], "fy":[-8.66965,-16.13691,-1.45334,8.52561]}, - {"t":1.37967, "x":5.29104, "y":5.16515, "heading":-2.11706, "vx":-0.97037, "vy":0.05761, "omega":0.35056, "ax":4.57094, "ay":-0.27128, "alpha":-1.64956, "fx":[74.86658,73.69054,75.41921,74.92858], "fy":[-8.8833,-16.06372,-1.29593,8.50356]}, - {"t":1.41504, "x":5.25958, "y":5.16702, "heading":-2.10466, "vx":-0.80867, "vy":0.04801, "omega":0.2922, "ax":4.57129, "ay":-0.27134, "alpha":-1.65053, "fx":[74.85247,73.71075,75.42677,74.93733], "fy":[-9.06539,-15.99953,-1.16121,8.48245]}, - {"t":1.45042, "x":5.23383, "y":5.16855, "heading":-2.09432, "vx":-0.64696, "vy":0.03841, "omega":0.23381, "ax":4.57159, "ay":-0.2714, "alpha":-1.65139, "fx":[74.84092,73.7281,75.43323,74.94522], "fy":[-9.21643,-15.9451,-1.04908,8.46339]}, - {"t":1.48579, "x":5.2138, "y":5.16974, "heading":-2.08605, "vx":-0.48523, "vy":0.02881, "omega":0.17539, "ax":4.57187, "ay":-0.27144, "alpha":-1.65211, "fx":[74.83218,73.74258,75.43875,74.95222], "fy":[-9.33688,-15.90109,-0.95946,8.44729]}, - {"t":1.52117, "x":5.1995, "y":5.17059, "heading":-2.07985, "vx":-0.3235, "vy":0.01921, "omega":0.11695, "ax":4.57213, "ay":-0.27148, "alpha":-1.65269, "fx":[74.82645,73.75418,75.44351,74.95835], "fy":[-9.42712,-15.868,-0.89226,8.43491]}, - {"t":1.55655, "x":5.19091, "y":5.1711, "heading":-2.07571, "vx":-0.16175, "vy":0.0096, "omega":0.05848, "ax":4.57237, "ay":-0.2715, "alpha":-1.65313, "fx":[74.82388,73.76291,75.4476,74.9636], "fy":[-9.48746,-15.84623,-0.84743,8.42682]}, - {"t":1.59192, "x":5.18805, "y":5.17127, "heading":-2.07364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.92904, "y":5.58344, "heading":-2.79253, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.29058, "ay":-0.94521, "alpha":1.80957, "fx":[-101.65923,-104.40123,-104.11451,-101.18107], "fy":[-23.96309,-3.22608,-8.62268,-25.99758]}, + {"t":0.03668, "x":7.9248, "y":5.58281, "heading":-2.79253, "vx":-0.23076, "vy":-0.03467, "omega":0.06638, "ax":-6.29042, "ay":-0.94522, "alpha":1.80045, "fx":[-101.66222,-104.39153,-104.1054,-101.18662], "fy":[-23.91569,-3.29168,-8.65355,-25.94894]}, + {"t":0.07337, "x":7.9121, "y":5.5809, "heading":-2.79009, "vx":-0.46152, "vy":-0.06935, "omega":0.13243, "ax":-6.29025, "ay":-0.94523, "alpha":1.78988, "fx":[-101.67217,-104.38106,-104.09294,-101.18816], "fy":[-23.83387,-3.35064,-8.71427,-25.91219]}, + {"t":0.11005, "x":7.89094, "y":5.57772, "heading":-2.78523, "vx":-0.69227, "vy":-0.10402, "omega":0.19809, "ax":-6.29005, "ay":-0.94526, "alpha":1.77762, "fx":[-101.68898,-104.36956,-104.07687,-101.18602], "fy":[-23.71682,-3.40534,-8.80523,-25.88535]}, + {"t":0.14674, "x":7.86131, "y":5.57327, "heading":-2.77797, "vx":-0.92302, "vy":-0.1387, "omega":0.2633, "ax":-6.28982, "ay":-0.94529, "alpha":1.76334, "fx":[-101.71249,-104.35665,-104.05683,-101.18065], "fy":[-23.56337,-3.45905,-8.92694,-25.86572]}, + {"t":0.18342, "x":7.82322, "y":5.56754, "heading":-2.76831, "vx":-1.15376, "vy":-0.17338, "omega":0.32799, "ax":-6.28956, "ay":-0.94534, "alpha":1.74661, "fx":[-101.74254,-104.34187,-104.03234,-101.1726], "fy":[-23.37194,-3.5161,-9.08017,-25.84975]}, + {"t":0.2201, "x":7.77666, "y":5.56055, "heading":-2.75628, "vx":-1.38448, "vy":-0.20806, "omega":0.39206, "ax":-6.28925, "ay":-0.94539, "alpha":1.72684, "fx":[-101.77886,-104.32455,-104.00274,-101.16265], "fy":[-23.14041,-3.58228,-9.26595,-25.83265]}, + {"t":0.25679, "x":7.72164, "y":5.55228, "heading":-2.74189, "vx":-1.6152, "vy":-0.24274, "omega":0.45541, "ax":-6.28886, "ay":-0.94545, "alpha":1.70323, "fx":[-101.8211,-104.30376,-103.96714,-101.15182], "fy":[-22.86601,-3.6653,-9.48569,-25.80794]}, + {"t":0.29347, "x":7.65816, "y":5.54274, "heading":-2.72519, "vx":-1.8459, "vy":-0.27742, "omega":0.51789, "ax":-6.28839, "ay":-0.9455, "alpha":1.67466, "fx":[-101.86874,-104.27819,-103.92429,-101.14147], "fy":[-22.5451,-3.77575,-9.74136,-25.76662]}, + {"t":0.33016, "x":7.58621, "y":5.53193, "heading":-2.70619, "vx":-2.07658, "vy":-0.3121, "omega":0.57932, "ax":-6.28778, "ay":-0.94557, "alpha":1.63948, "fx":[-101.92096,-104.24585,-103.87241,-101.13353], "fy":[-22.17272,-3.92851,-10.03577,-25.69576]}, + {"t":0.36684, "x":7.50581, "y":5.51984, "heading":-2.68494, "vx":-2.30724, "vy":-0.34679, "omega":0.63946, "ax":-6.28697, "ay":-0.94562, "alpha":1.59518, "fx":[-101.97649,-104.20362,-103.80879,-101.13076], "fy":[-21.74185,-4.14554,-10.37315,-25.57599]}, + {"t":0.40353, "x":7.41694, "y":5.50648, "heading":-2.66148, "vx":-2.53788, "vy":-0.38148, "omega":0.69798, "ax":-6.28584, "ay":-0.94567, "alpha":1.53768, "fx":[-102.03325,-104.14632,-103.72911,-101.13728], "fy":[-21.24192,-4.46101,-10.76034,-25.37665]}, + {"t":0.44021, "x":7.31961, "y":5.49185, "heading":-2.63587, "vx":-2.76847, "vy":-0.41617, "omega":0.75439, "ax":-6.28418, "ay":-0.94571, "alpha":1.45996, "fx":[-102.0877,-104.06454,-103.62588,-101.15965], "fy":[-20.65566,-4.93222,-11.20921,-25.0454]}, + {"t":0.47689, "x":7.21382, "y":5.47595, "heading":-2.6082, "vx":-2.999, "vy":-0.45086, "omega":0.80795, "ax":-6.28156, "ay":-0.94573, "alpha":1.34863, "fx":[-102.13317,-103.93939,-103.4847,-101.20903], "fy":[-19.95183,-5.66507,-11.74269,-24.48389]}, + {"t":0.51358, "x":7.09958, "y":5.45877, "heading":-2.57856, "vx":-3.22943, "vy":-0.48556, "omega":0.85742, "ax":-6.27691, "ay":-0.94569, "alpha":1.17471, "fx":[-102.15558,-103.72672,-103.27355,-101.30597], "fy":[-19.06582,-6.8832,-12.41193,-23.47994]}, + {"t":0.55026, "x":6.97689, "y":5.44032, "heading":-2.54711, "vx":-3.45969, "vy":-0.52025, "omega":0.90051, "ax":-6.26692, "ay":-0.94547, "alpha":0.86128, "fx":[-102.11838,-103.29627,-102.90302,-101.49102], "fy":[-17.83593,-9.16314,-13.35849,-21.4689]}, + {"t":0.58695, "x":6.84575, "y":5.4206, "heading":-2.51407, "vx":-3.68959, "vy":-0.55493, "omega":0.93211, "ax":-6.23544, "ay":-0.94415, "alpha":0.11472, "fx":[-101.886,-102.05767,-101.9912,-101.81508], "fy":[-15.70608,-14.60737,-15.16873,-16.25826]}, + {"t":0.62363, "x":6.70621, "y":5.39961, "heading":-2.47988, "vx":-3.91833, "vy":-0.58957, "omega":0.93632, "ax":-5.85447, "ay":-0.92556, "alpha":-4.16862, "fx":[-100.21799,-91.09997,-93.58964,-97.9298], "fy":[-8.60666,-41.16074,-26.81321,16.05575]}, + {"t":0.66031, "x":6.55853, "y":5.37736, "heading":-2.44553, "vx":-4.1331, "vy":-0.62352, "omega":0.7834, "ax":5.85913, "ay":0.92488, "alpha":4.07218, "fx":[100.10481,91.00127,93.98758,98.04907], "fy":[9.49182,41.19496,25.25017,-15.45687]}, + {"t":0.697, "x":6.41085, "y":5.35511, "heading":-2.41679, "vx":-3.91816, "vy":-0.58959, "omega":0.93278, "ax":6.23515, "ay":0.94417, "alpha":-0.16405, "fx":[101.8772,102.11013,101.994,101.74978], "fy":[15.70828,14.218,15.16939,16.64554]}, + {"t":0.73368, "x":6.27131, "y":5.33411, "heading":-2.38257, "vx":-3.68943, "vy":-0.55496, "omega":0.92676, "ax":6.26622, "ay":0.94545, "alpha":-0.89925, "fx":[102.28396,103.35072,102.78248,101.34606], "fy":[16.81766,8.62157,14.28246,22.10337]}, + {"t":0.77037, "x":6.14019, "y":5.31439, "heading":-2.34858, "vx":-3.45956, "vy":-0.52027, "omega":0.89377, "ax":6.27615, "ay":0.94569, "alpha":-1.20275, "fx":[102.51191,103.78023,103.04619,101.0743], "fy":[17.00477,6.22154,14.20304,24.41183]}, + {"t":0.80705, "x":6.0175, "y":5.29594, "heading":-2.31579, "vx":-3.22932, "vy":-0.48558, "omega":0.84965, "ax":6.28086, "ay":0.94576, "alpha":-1.36742, "fx":[102.67968,103.99186,103.15826,100.89073], "fy":[16.87409,4.89963,14.35617,25.71556]}, + {"t":0.84373, "x":5.90326, "y":5.27877, "heading":-2.28462, "vx":-2.99891, "vy":-0.45089, "omega":0.79949, "ax":6.28358, "ay":0.94576, "alpha":-1.47024, "fx":[102.81695,104.11615,103.20668,100.75831], "fy":[16.60818,4.07527,14.59745,26.56481]}, + {"t":0.88042, "x":5.79748, "y":5.26286, "heading":-2.25529, "vx":-2.76841, "vy":-0.41619, "omega":0.74556, "ax":6.28534, "ay":0.94574, "alpha":-1.54031, "fx":[102.93448,104.19724,103.22323,100.65819], "fy":[16.28253,3.52321,14.87255,27.16569]}, + {"t":0.9171, "x":5.70015, "y":5.24823, "heading":-2.22794, "vx":-2.53784, "vy":-0.3815, "omega":0.68905, "ax":6.28657, "ay":0.94569, "alpha":-1.59106, "fx":[103.03714,104.25399,103.22234,100.58013], "fy":[15.93466,3.13675,15.15612,27.61359]}, + {"t":0.95379, "x":5.61128, "y":5.23487, "heading":-2.20267, "vx":-2.30722, "vy":-0.34681, "omega":0.63068, "ax":6.28747, "ay":0.94564, "alpha":-1.62951, "fx":[103.12754,104.29576,103.21161,100.51799], "fy":[15.58557,2.85814,15.43457,27.95933]}, + {"t":0.99047, "x":5.53087, "y":5.22279, "heading":-2.17953, "vx":-2.07657, "vy":-0.31212, "omega":0.57091, "ax":6.28817, "ay":0.94558, "alpha":-1.65972, "fx":[103.20731,104.32769,103.19548,100.46782], "fy":[15.24805,2.65291,15.6999,28.2329]}, + {"t":1.02715, "x":5.45893, "y":5.21197, "heading":-2.15859, "vx":-1.84589, "vy":-0.27743, "omega":0.51002, "ax":6.28871, "ay":0.94552, "alpha":-1.68412, "fx":[103.27753,104.35283,103.1768,100.42688], "fy":[14.93037,2.49892,15.94707,28.45342]}, + {"t":1.06384, "x":5.39544, "y":5.20243, "heading":-2.13988, "vx":-1.6152, "vy":-0.24275, "omega":0.44824, "ax":6.28915, "ay":0.94546, "alpha":-1.70432, "fx":[103.33906,104.37313,103.15746,100.39316], "fy":[14.63809,2.38112,16.17274,28.63391]}, + {"t":1.10052, "x":5.34042, "y":5.19416, "heading":-2.12343, "vx":-1.38448, "vy":-0.20806, "omega":0.38572, "ax":6.28951, "ay":0.9454, "alpha":-1.72136, "fx":[103.39253,104.38985,103.13882,100.36513], "fy":[14.37509,2.28879,16.3746,28.78363]}, + {"t":1.13721, "x":5.29387, "y":5.18717, "heading":-2.10928, "vx":-1.15376, "vy":-0.17338, "omega":0.32257, "ax":6.28981, "ay":0.94535, "alpha":-1.73598, "fx":[103.43851,104.40388,103.12186,100.34158], "fy":[14.14415,2.21405,16.551,28.90944]}, + {"t":1.17389, "x":5.25577, "y":5.18144, "heading":-2.09745, "vx":-0.92302, "vy":-0.1387, "omega":0.25889, "ax":6.29006, "ay":0.9453, "alpha":-1.7487, "fx":[103.47742,104.41584,103.10735,100.32156], "fy":[13.94727,2.15093,16.70073,29.01657]}, + {"t":1.21058, "x":5.22615, "y":5.17699, "heading":-2.08795, "vx":-0.69228, "vy":-0.10402, "omega":0.19474, "ax":6.29027, "ay":0.94526, "alpha":-1.75991, "fx":[103.50965,104.4262,103.09586,100.30429], "fy":[13.78596,2.09486,16.82289,29.10905]}, + {"t":1.24726, "x":5.20498, "y":5.17381, "heading":-2.08081, "vx":-0.46152, "vy":-0.06935, "omega":0.13018, "ax":6.29045, "ay":0.94522, "alpha":-1.76988, "fx":[103.5355,104.4353,103.08784,100.28917], "fy":[13.66139,2.04229,16.91676,29.19002]}, + {"t":1.28394, "x":5.19229, "y":5.1719, "heading":-2.07603, "vx":-0.23077, "vy":-0.03467, "omega":0.06525, "ax":6.29061, "ay":0.9452, "alpha":-1.77883, "fx":[103.5552,104.44339,103.08364,100.2757], "fy":[13.57447,1.99048,16.98177,29.26191]}, + {"t":1.32063, "x":5.18805, "y":5.17127, "heading":-2.07364, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index f05e602d..02ef768b 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -286,6 +286,20 @@ "val":3.141592653589793 } }, + "NI":{ + "x":{ + "exp":"7.929035186767578 m", + "val":7.929035186767578 + }, + "y":{ + "exp":"5.583443641662598 m", + "val":5.583443641662598 + }, + "heading":{ + "exp":"200 deg", + "val":3.490658503988659 + } + }, "PL":{ "x":{ "exp":"7.814075469970703 m", diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1fd75dd3..d516e45d 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -9,6 +9,7 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; @@ -357,7 +358,7 @@ public Command CMtoGH() { // algae path .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .onTrue( Commands.sequence( - swerve.driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0)).withTimeout(0.2), + swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), Commands.runOnce( () -> { autoAlgaeIntake = true; @@ -391,7 +392,7 @@ public Command CMtoGH() { // algae path swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) - .withTimeout(2)) + .withTimeout(0.5)) .andThen( Commands.runOnce( () -> { @@ -402,7 +403,40 @@ public Command CMtoGH() { // algae path .observe(steps.get("GHtoNI").done()) .onTrue( // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - scoreAlgaeInAuto()); + Commands.sequence( + AutoAim.translateToXCoord( + swerve, + () -> + Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) + > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) + ? AutoAim.RED_NET_X + : AutoAim.BLUE_NET_X, + () -> 0, + () -> + (Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) + > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) + ? Rotation2d.kZero + : Rotation2d.k180deg) + .plus(Rotation2d.fromDegrees(20.0))) + .until( + () -> + (MathUtil.isNear( + DriverStation.getAlliance().get() == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + Units.inchesToMeters(1)) + && MathUtil.isNear( + DriverStation.getAlliance().get() == Alliance.Blue + ? Rotation2d.k180deg + .plus(Rotation2d.fromDegrees(20.0)) + .getRadians() + : Rotation2d.kZero + .plus(Rotation2d.fromDegrees(20.0)) + .getRadians(), + swerve.getPose().getRotation().getRadians(), + AutoAim.ROTATION_TOLERANCE_RADIANS))), + scoreAlgaeInAuto())); // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { @@ -515,11 +549,12 @@ public void bindAlgaeElevatorExtension(AutoRoutine routine, double toleranceMete .observe( () -> MathUtil.isNear( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_NET_X - : AutoAim.RED_NET_X, - swerve.getPose().getX(), - toleranceMeters)) + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + toleranceMeters) + && (manipulator.hasAlgae())) .whileTrue(Commands.run(() -> autoPreScore = true)) .whileFalse(Commands.run(() -> autoPreScore = false)); } @@ -532,22 +567,23 @@ public Command scoreAlgaeInAuto() { // oh good lord return Commands.sequence( Commands.waitUntil( new Trigger( - () -> - MathUtil.isNear( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_NET_X - : AutoAim.RED_NET_X, - swerve.getPose().getX(), - Units.inchesToMeters(1.0)) - && MathUtil.isNear( - 0, - Math.hypot( - swerve.getVelocityRobotRelative().vxMetersPerSecond, - swerve.getVelocityRobotRelative().vyMetersPerSecond), - AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - && MathUtil.isNear( - 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) - .debounce(0.06)), // TODO + () -> + MathUtil.isNear( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + Units.inchesToMeters(3.0)) + && MathUtil.isNear( + 0, + Math.hypot( + swerve.getVelocityRobotRelative().vxMetersPerSecond, + swerve.getVelocityRobotRelative().vyMetersPerSecond), + AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) + && MathUtil.isNear( + 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) + // .debounce(0.06)), // TODO + ), Commands.runOnce( () -> { Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); From 56c6b524a0a0bace62ba561be63aba9195263014 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 15:17:36 -0700 Subject: [PATCH 029/154] more algae auto tuning, BARGE SHOT IS FUCKED --- src/main/java/frc/robot/Autos.java | 44 ++++++++++++++++-------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index d516e45d..a13b418c 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -381,7 +381,7 @@ public Command CMtoGH() { // algae path AlgaeIntakeTargets.getClosestTargetPose( swerve.getPose())), swerve.getVelocityFieldRelative(), - Units.inchesToMeters(1.0), + Units.inchesToMeters(0.5), Units.degreesToRadians(1.0)) && elevator.isNearTarget() && shoulder.isNearAngle( @@ -404,20 +404,22 @@ public Command CMtoGH() { // algae path .onTrue( // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); Commands.sequence( - AutoAim.translateToXCoord( - swerve, - () -> - Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) - > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) - ? AutoAim.RED_NET_X - : AutoAim.BLUE_NET_X, - () -> 0, - () -> - (Math.abs(swerve.getPose().getX() - AutoAim.BLUE_NET_X) - > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) - ? Rotation2d.kZero - : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(20.0))) + // AutoAim.translateToXCoord( + // swerve, + // () -> + // DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // () -> 0, + // () -> + // DriverStation.getAlliance().get() == Alliance.Blue + // ? Rotation2d.k180deg + // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) + AutoAim.translateToPose(swerve, + () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + )) .until( () -> (MathUtil.isNear( @@ -425,17 +427,17 @@ public Command CMtoGH() { // algae path ? AutoAim.BLUE_NET_X : AutoAim.RED_NET_X, swerve.getPose().getX(), - Units.inchesToMeters(1)) + Units.inchesToMeters(5)) && MathUtil.isNear( DriverStation.getAlliance().get() == Alliance.Blue ? Rotation2d.k180deg .plus(Rotation2d.fromDegrees(20.0)) - .getRadians() + .getDegrees() : Rotation2d.kZero .plus(Rotation2d.fromDegrees(20.0)) - .getRadians(), - swerve.getPose().getRotation().getRadians(), - AutoAim.ROTATION_TOLERANCE_RADIANS))), + .getDegrees(), + swerve.getPose().getRotation().getDegrees(), + 5.0))), scoreAlgaeInAuto())); // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign @@ -589,7 +591,7 @@ public Command scoreAlgaeInAuto() { // oh good lord Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); autoScore = true; }), - Commands.waitUntil(() -> manipulator.hasAlgae()) + Commands.waitUntil(() -> !manipulator.hasAlgae()) .alongWith( Robot.isSimulation() ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) From 0d83f63486a3db4e373df10ca3d6a236830d9906 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 15:54:41 -0700 Subject: [PATCH 030/154] yet more barge tuning, auto was overcooked --- src/main/java/frc/robot/Autos.java | 81 ++++++++++--------- src/main/java/frc/robot/Robot.java | 2 +- .../shoulder/ShoulderSubsystem.java | 2 +- 3 files changed, 44 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index a13b418c..76f98e7c 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -144,7 +144,7 @@ public void runCoralPath( endPos.length() == 1 ? scoreCoralInAuto( () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) - : AutoAim.translateToPose( + : AutoAim.translateToPose( //TODO does this get called? swerve, () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) .until( @@ -400,45 +400,47 @@ public Command CMtoGH() { // algae path }), steps.get("GHtoNI").cmd())); routine - .observe(steps.get("GHtoNI").done()) + .observe(steps.get("GHtoNI").atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) //TODO tune .onTrue( - // Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - Commands.sequence( - // AutoAim.translateToXCoord( - // swerve, - // () -> - // DriverStation.getAlliance().get() == Alliance.Blue - // ? AutoAim.BLUE_NET_X - // : AutoAim.RED_NET_X, - // () -> 0, - // () -> - // DriverStation.getAlliance().get() == Alliance.Blue - // ? Rotation2d.k180deg - // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) - AutoAim.translateToPose(swerve, - () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue - ? AutoAim.BLUE_NET_X - : AutoAim.RED_NET_X, - )) - .until( - () -> - (MathUtil.isNear( - DriverStation.getAlliance().get() == Alliance.Blue - ? AutoAim.BLUE_NET_X - : AutoAim.RED_NET_X, - swerve.getPose().getX(), - Units.inchesToMeters(5)) - && MathUtil.isNear( - DriverStation.getAlliance().get() == Alliance.Blue - ? Rotation2d.k180deg - .plus(Rotation2d.fromDegrees(20.0)) - .getDegrees() - : Rotation2d.kZero - .plus(Rotation2d.fromDegrees(20.0)) - .getDegrees(), - swerve.getPose().getRotation().getDegrees(), - 5.0))), - scoreAlgaeInAuto())); + Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + // Commands.sequence( + // // AutoAim.translateToXCoord( + // // swerve, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? AutoAim.BLUE_NET_X + // // : AutoAim.RED_NET_X, + // // () -> 0, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? Rotation2d.k180deg + // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) + // AutoAim.translateToPose(swerve, + // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // steps.get("GhtoNI").)) + // .until( + // () -> + // (MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // swerve.getPose().getX(), + // Units.inchesToMeters(5)) + // && MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? Rotation2d.k180deg + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees() + // : Rotation2d.kZero + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees(), + // swerve.getPose().getRotation().getDegrees(), + // 5.0))), + // scoreAlgaeInAuto()) + //) + ; // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { @@ -586,6 +588,7 @@ public Command scoreAlgaeInAuto() { // oh good lord 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) // .debounce(0.06)), // TODO ), + Commands.print("Scoring algae"), Commands.runOnce( () -> { Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4de9443d..9a9b9710 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1006,7 +1006,7 @@ public Robot() { > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) ? Rotation2d.kZero : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(20.0))), + .plus(Rotation2d.fromDegrees(30.0))), Commands.waitUntil( () -> { final var diff = diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 58bde40a..62361233 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -56,7 +56,7 @@ public class ShoulderSubsystem extends SubsystemBase { new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); public static final MotionMagicConfigs TOSS_CONFIGS = new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(0.325) + .withMotionMagicCruiseVelocity(0.275) .withMotionMagicAcceleration(4.0); private final ShoulderIO io; From 63cca92459655e97bcd5c02ed2af931ce438e407 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 10 Apr 2025 16:30:08 -0700 Subject: [PATCH 031/154] Add rezero button binding --- src/main/java/frc/robot/Robot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a9b9710..96e43c01 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1044,6 +1044,9 @@ public Robot() { swerveDriveSimulation.get().setSimulationWorldPose(swerve.getPose()); } })); + driver + .x() + .onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); operator .a() From 4231eb78f4f18d0d32584ed159dc7d65e2b97cfc Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 10 Apr 2025 16:43:22 -0700 Subject: [PATCH 032/154] zero shoulder correctly --- src/main/java/frc/robot/Autos.java | 87 ++++++++++--------- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/subsystems/Superstructure.java | 3 - 3 files changed, 45 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 76f98e7c..b3ebcb8e 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -9,7 +9,6 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; @@ -144,7 +143,7 @@ public void runCoralPath( endPos.length() == 1 ? scoreCoralInAuto( () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) - : AutoAim.translateToPose( //TODO does this get called? + : AutoAim.translateToPose( // TODO does this get called? swerve, () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) .until( @@ -400,47 +399,49 @@ public Command CMtoGH() { // algae path }), steps.get("GHtoNI").cmd())); routine - .observe(steps.get("GHtoNI").atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) //TODO tune - .onTrue( - Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - // Commands.sequence( - // // AutoAim.translateToXCoord( - // // swerve, - // // () -> - // // DriverStation.getAlliance().get() == Alliance.Blue - // // ? AutoAim.BLUE_NET_X - // // : AutoAim.RED_NET_X, - // // () -> 0, - // // () -> - // // DriverStation.getAlliance().get() == Alliance.Blue - // // ? Rotation2d.k180deg - // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) - // AutoAim.translateToPose(swerve, - // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue - // ? AutoAim.BLUE_NET_X - // : AutoAim.RED_NET_X, - // steps.get("GhtoNI").)) - // .until( - // () -> - // (MathUtil.isNear( - // DriverStation.getAlliance().get() == Alliance.Blue - // ? AutoAim.BLUE_NET_X - // : AutoAim.RED_NET_X, - // swerve.getPose().getX(), - // Units.inchesToMeters(5)) - // && MathUtil.isNear( - // DriverStation.getAlliance().get() == Alliance.Blue - // ? Rotation2d.k180deg - // .plus(Rotation2d.fromDegrees(20.0)) - // .getDegrees() - // : Rotation2d.kZero - // .plus(Rotation2d.fromDegrees(20.0)) - // .getDegrees(), - // swerve.getPose().getRotation().getDegrees(), - // 5.0))), - // scoreAlgaeInAuto()) - //) - ; + .observe( + steps + .get("GHtoNI") + .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + // Commands.sequence( + // // AutoAim.translateToXCoord( + // // swerve, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? AutoAim.BLUE_NET_X + // // : AutoAim.RED_NET_X, + // // () -> 0, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? Rotation2d.k180deg + // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) + // AutoAim.translateToPose(swerve, + // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // steps.get("GhtoNI").)) + // .until( + // () -> + // (MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // swerve.getPose().getX(), + // Units.inchesToMeters(5)) + // && MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? Rotation2d.k180deg + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees() + // : Rotation2d.kZero + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees(), + // swerve.getPose().getRotation().getDegrees(), + // 5.0))), + // scoreAlgaeInAuto()) + // ) + ; // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 96e43c01..8e51bb35 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1044,9 +1044,7 @@ public Robot() { swerveDriveSimulation.get().setSimulationWorldPose(swerve.getPose()); } })); - driver - .x() - .onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); + driver.x().onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); operator .a() diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 91ba5bba..a9c7f314 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -320,9 +320,6 @@ private void configureStateTransitionCommands() { () -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) .andThen(wrist.setVoltage(-1.0))))) - .whileTrue( - Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) - .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); stateTriggers From bd06530625fd3d4e46eba1b5aa55296a854b1c91 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 11 Apr 2025 09:15:03 -0700 Subject: [PATCH 033/154] Reduce jog distance on l4 --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 91ba5bba..259bd364 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -506,7 +506,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.125 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From fac858f9146bd11a68143622a8c57402a7e0e667 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 11 Apr 2025 09:15:47 -0700 Subject: [PATCH 034/154] Spotless --- src/main/java/frc/robot/Autos.java | 87 +++++++++++++++--------------- src/main/java/frc/robot/Robot.java | 4 +- 2 files changed, 45 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 76f98e7c..b3ebcb8e 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -9,7 +9,6 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; @@ -144,7 +143,7 @@ public void runCoralPath( endPos.length() == 1 ? scoreCoralInAuto( () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) - : AutoAim.translateToPose( //TODO does this get called? + : AutoAim.translateToPose( // TODO does this get called? swerve, () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) .until( @@ -400,47 +399,49 @@ public Command CMtoGH() { // algae path }), steps.get("GHtoNI").cmd())); routine - .observe(steps.get("GHtoNI").atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) //TODO tune - .onTrue( - Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - // Commands.sequence( - // // AutoAim.translateToXCoord( - // // swerve, - // // () -> - // // DriverStation.getAlliance().get() == Alliance.Blue - // // ? AutoAim.BLUE_NET_X - // // : AutoAim.RED_NET_X, - // // () -> 0, - // // () -> - // // DriverStation.getAlliance().get() == Alliance.Blue - // // ? Rotation2d.k180deg - // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) - // AutoAim.translateToPose(swerve, - // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue - // ? AutoAim.BLUE_NET_X - // : AutoAim.RED_NET_X, - // steps.get("GhtoNI").)) - // .until( - // () -> - // (MathUtil.isNear( - // DriverStation.getAlliance().get() == Alliance.Blue - // ? AutoAim.BLUE_NET_X - // : AutoAim.RED_NET_X, - // swerve.getPose().getX(), - // Units.inchesToMeters(5)) - // && MathUtil.isNear( - // DriverStation.getAlliance().get() == Alliance.Blue - // ? Rotation2d.k180deg - // .plus(Rotation2d.fromDegrees(20.0)) - // .getDegrees() - // : Rotation2d.kZero - // .plus(Rotation2d.fromDegrees(20.0)) - // .getDegrees(), - // swerve.getPose().getRotation().getDegrees(), - // 5.0))), - // scoreAlgaeInAuto()) - //) - ; + .observe( + steps + .get("GHtoNI") + .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + // Commands.sequence( + // // AutoAim.translateToXCoord( + // // swerve, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? AutoAim.BLUE_NET_X + // // : AutoAim.RED_NET_X, + // // () -> 0, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? Rotation2d.k180deg + // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) + // AutoAim.translateToPose(swerve, + // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // steps.get("GhtoNI").)) + // .until( + // () -> + // (MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // swerve.getPose().getX(), + // Units.inchesToMeters(5)) + // && MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? Rotation2d.k180deg + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees() + // : Rotation2d.kZero + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees(), + // swerve.getPose().getRotation().getDegrees(), + // 5.0))), + // scoreAlgaeInAuto()) + // ) + ; // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 96e43c01..8e51bb35 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1044,9 +1044,7 @@ public Robot() { swerveDriveSimulation.get().setSimulationWorldPose(swerve.getPose()); } })); - driver - .x() - .onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); + driver.x().onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); operator .a() From 3ec3f27314989db986b8057732e181ecc916427c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 11 Apr 2025 14:29:18 -0700 Subject: [PATCH 035/154] Add a dumb anti-coral jam binding --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/subsystems/Superstructure.java | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8e51bb35..c70d4b81 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -495,6 +495,7 @@ public static enum AlgaeScoreTarget { driver.start(), operator.rightBumper(), operator.leftBumper(), + operator.povDown(), new Trigger(() -> killVisionIK) .or(() -> currentTarget == ReefTarget.L1) .or(() -> DriverStation.isAutonomous()), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index e7333741..473a824b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -103,6 +103,9 @@ public static enum SuperState { @AutoLogOutput(key = "Superstructure/Force Funnel Req") private final Trigger forceFunnelReq; + @AutoLogOutput(key = "Superstructure/Force Funnel Req") + private final Trigger forceIndexReq; + @AutoLogOutput(key = "Superstructure/Kill Vision and IK") private final Trigger killVisionIK; @@ -146,6 +149,7 @@ public Superstructure( Trigger homeReq, Trigger revFunnelReq, Trigger forceFunnelReq, + Trigger forceIndexReq, Trigger killVisionIK, DoubleSupplier coralAdjust) { this.elevator = elevator; @@ -178,6 +182,8 @@ public Superstructure( this.revFunnelReq = revFunnelReq; this.forceFunnelReq = forceFunnelReq; + this.forceIndexReq = forceIndexReq; + this.killVisionIK = killVisionIK; this.coralAdjust = coralAdjust; @@ -197,6 +203,10 @@ public void periodic() { } private void configureStateTransitionCommands() { + // Prob a better way to impl this + // Vaughn says he wants this available anytime + forceIndexReq.whileTrue(manipulator.setVelocity(1.0)); + stateTriggers .get(SuperState.IDLE) .whileTrue( From 1660228d7041fed00f5b7bd49d339ae5d8f21a1d Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 11 Apr 2025 15:07:57 -0700 Subject: [PATCH 036/154] Tune ground algae intake --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 62361233..5425f074 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -33,7 +33,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(-1.0)); + Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(-5.0)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index e4ad1269..5d52fc45 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -25,7 +25,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); + public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); From c3bd8fc2a1458b87a4ff5be12b529edad021fec3 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 11 Apr 2025 17:16:49 -0700 Subject: [PATCH 037/154] move algae intake into its own method, add preliminary rest of path --- src/main/java/frc/robot/Autos.java | 140 +++++++++++++---------------- 1 file changed, 60 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b3ebcb8e..0a2ee218 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -358,45 +358,7 @@ public Command CMtoGH() { // algae path .onTrue( Commands.sequence( swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), - Commands.runOnce( - () -> { - autoAlgaeIntake = true; - Robot.setCurrentAlgaeIntakeTarget( - AlgaeIntakeTargets.getClosestTarget( - steps.get("CMtoG").getFinalPose().get()) - .height); - }), - AutoAim.translateToPose( - swerve, - () -> - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose( - steps.get("CMtoG").getFinalPose().get()))) - .until( - () -> - AutoAim.isInTolerance( - swerve.getPose(), - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose( - swerve.getPose())), - swerve.getVelocityFieldRelative(), - Units.inchesToMeters(0.5), - Units.degreesToRadians(1.0)) - && elevator.isNearTarget() - && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) - && wrist.isNearAngle( - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), - AutoAim.approachAlgae( - swerve, - () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), - 1) - .withTimeout(0.5)) - .andThen( - Commands.runOnce( - () -> { - autoAlgaeIntake = false; - }), + intakeAlgaeInAuto(() -> steps.get("CMtoG").getFinalPose()), steps.get("GHtoNI").cmd())); routine .observe( @@ -404,6 +366,21 @@ public Command CMtoGH() { // algae path .get("GHtoNI") .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + // ------------------sketchy-------- + routine.observe(steps.get("NItoIJ").done()).onTrue( + Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), + steps.get("IJtoNI").cmd())); + + routine + .observe( + steps + .get("IJtoNI") + .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); + routine.observe(steps.get("NItoEF").done()).onTrue( + Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); + + // --------------------------- // Commands.sequence( // // AutoAim.translateToXCoord( // // swerve, @@ -441,7 +418,7 @@ public Command CMtoGH() { // algae path // 5.0))), // scoreAlgaeInAuto()) // ) - ; + // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { @@ -607,45 +584,48 @@ public Command scoreAlgaeInAuto() { // oh good lord autoPreScore = false; }))); } - // public void runAlgaePath( - // AutoRoutine routine, - // String startPos, - // String endPos, - // String nextPos, - // HashMap steps) { - // routine - // .observe(steps.get(startPos + "to" + endPos).done()) - // .onTrue( - // Commands.sequence( - // endPos.equals("NI") - // ? scoreAlgaeInAuto() - // : intakeAlgaeInAuto(() -> steps.get(startPos + "to" + - // endPos).getFinalPose()), - // steps.get(endPos + "to" + nextPos).cmd())); - // } - - // public Command intakeAlgaeInAuto(Supplier> pose) { - // if (!pose.get().isPresent()) { - // return Commands.none(); - // } else { - // return Commands.sequence( - // Commands.runOnce( - // () -> { - // autoAlgaeIntake = true; - // Robot.setCurrentAlgaeIntakeTarget( - // AlgaeIntakeTargets.getClosestTarget(pose.get().get()).height); // are you - // serios - // }), - // Commands.waitUntil(() -> manipulator.hasAlgae()) - // .alongWith( - // Robot.isSimulation() - // ? Commands.runOnce(() -> manipulator.setHasAlgae(true)) - // : Commands.none()), - // Commands.runOnce( - // () -> { - // autoAlgaeIntake = false; - // })); - // } - // } + public Command intakeAlgaeInAuto(Supplier> pose) { + return Commands.sequence( + Commands.runOnce( + () -> { + autoAlgaeIntake = true; + Robot.setCurrentAlgaeIntakeTarget( + AlgaeIntakeTargets.getClosestTarget( + pose.get().get()) //wow + .height); + }), + AutoAim.translateToPose( + swerve, + () -> + AlgaeIntakeTargets.getOffsetLocation( + AlgaeIntakeTargets.getClosestTargetPose( + pose.get().get()))) + .until( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + AlgaeIntakeTargets.getOffsetLocation( + AlgaeIntakeTargets.getClosestTargetPose( + swerve.getPose())), + swerve.getVelocityFieldRelative(), + Units.inchesToMeters(0.5), + Units.degreesToRadians(1.0)) + && elevator.isNearTarget() + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + && wrist.isNearAngle( + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + AutoAim.approachAlgae( + swerve, + () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), + 1) + .withTimeout(0.5)) +.andThen( + Commands.runOnce( + () -> { + autoAlgaeIntake = false; + }) + ); + } } From 94daef1bccd9c2afde45085d0f63e25fd1ea62ff Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 11 Apr 2025 21:47:47 -0700 Subject: [PATCH 038/154] cancoder retuning --- src/main/java/frc/robot/Autos.java | 105 +++++++++--------- .../subsystems/shoulder/ShoulderIOReal.java | 2 +- .../shoulder/ShoulderSubsystem.java | 2 + 3 files changed, 54 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 0a2ee218..4af8dce4 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -357,9 +357,9 @@ public Command CMtoGH() { // algae path .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .onTrue( Commands.sequence( - swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), - intakeAlgaeInAuto(() -> steps.get("CMtoG").getFinalPose()), - steps.get("GHtoNI").cmd())); + swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), + intakeAlgaeInAuto(() -> steps.get("CMtoG").getFinalPose()), + steps.get("GHtoNI").cmd())); routine .observe( steps @@ -367,18 +367,22 @@ public Command CMtoGH() { // algae path .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); // ------------------sketchy-------- - routine.observe(steps.get("NItoIJ").done()).onTrue( - Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), - steps.get("IJtoNI").cmd())); - routine - .observe( - steps - .get("IJtoNI") - .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune - .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); - routine.observe(steps.get("NItoEF").done()).onTrue( - Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); + .observe(steps.get("NItoIJ").done()) + .onTrue( + Commands.sequence( + intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), + steps.get("IJtoNI").cmd())); + + routine + .observe( + steps + .get("IJtoNI") + .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); + routine + .observe(steps.get("NItoEF").done()) + .onTrue(Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); // --------------------------- // Commands.sequence( @@ -418,7 +422,7 @@ public Command CMtoGH() { // algae path // 5.0))), // scoreAlgaeInAuto()) // ) - + // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign // for (int i = 0; i < stops.length - 2; i++) { @@ -587,45 +591,38 @@ public Command scoreAlgaeInAuto() { // oh good lord public Command intakeAlgaeInAuto(Supplier> pose) { return Commands.sequence( - Commands.runOnce( - () -> { - autoAlgaeIntake = true; - Robot.setCurrentAlgaeIntakeTarget( - AlgaeIntakeTargets.getClosestTarget( - pose.get().get()) //wow - .height); - }), - AutoAim.translateToPose( - swerve, - () -> - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose( - pose.get().get()))) - .until( - () -> - AutoAim.isInTolerance( - swerve.getPose(), + Commands.runOnce( + () -> { + autoAlgaeIntake = true; + Robot.setCurrentAlgaeIntakeTarget( + AlgaeIntakeTargets.getClosestTarget(pose.get().get()) // wow + .height); + }), + AutoAim.translateToPose( + swerve, + () -> AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose( - swerve.getPose())), - swerve.getVelocityFieldRelative(), - Units.inchesToMeters(0.5), - Units.degreesToRadians(1.0)) - && elevator.isNearTarget() - && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) - && wrist.isNearAngle( - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), - AutoAim.approachAlgae( - swerve, - () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), - 1) - .withTimeout(0.5)) -.andThen( - Commands.runOnce( - () -> { - autoAlgaeIntake = false; - }) - ); + AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) + .until( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + AlgaeIntakeTargets.getOffsetLocation( + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())), + swerve.getVelocityFieldRelative(), + Units.inchesToMeters(0.5), + Units.degreesToRadians(1.0)) + && elevator.isNearTarget() + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + && wrist.isNearAngle(WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + AutoAim.approachAlgae( + swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) + .withTimeout(0.5)) + .andThen( + Commands.runOnce( + () -> { + autoAlgaeIntake = false; + })); } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index fae583d4..03e8a337 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -80,7 +80,7 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = 0.6323; + cancoderConfig.MagnetSensor.MagnetOffset = -0.378174;// 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 5425f074..a9d7b936 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -13,6 +13,7 @@ import frc.robot.subsystems.ExtensionKinematics; import frc.robot.utils.Tracer; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; @@ -90,6 +91,7 @@ public void periodic() { Logger.recordOutput("Carriage/Shoulder/Cancoder Pos", getZeroingAngle()); } + @AutoLogOutput(key = "Shoulder/Zeroing Angle") public Rotation2d getZeroingAngle() { return Rotation2d.fromRotations(inputs.cancoderPosition).div(SHOULDER_FINAL_STAGE_RATIO); } From fee140eaf45e50dea0cdbf209fb61497fc93c1d5 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 11 Apr 2025 22:31:15 -0700 Subject: [PATCH 039/154] tuning again --- src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 03e8a337..a73261d5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -80,7 +80,7 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = -0.378174;// 0.2307 + 0.25; // 0.6323; // 0.779; + cancoderConfig.MagnetSensor.MagnetOffset = -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); From fe4280e362e4c4673a376e9f9a884175c3da9730 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 11 Apr 2025 23:14:55 -0700 Subject: [PATCH 040/154] redo wrist zero at start of auto --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c70d4b81..f08e6e77 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -707,7 +707,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - wrist.resetPosition(Rotation2d.fromRadians(3.059)); + wrist.resetPosition(Rotation2d.fromRadians(3.094)); elevator.resetExtension(0.0); })); From 7cd4d9a09dba90e83b3e3522998d734ceb95ab59 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 11 Apr 2025 23:58:57 -0700 Subject: [PATCH 041/154] yayyy --- src/main/deploy/choreo/GHtoNI.traj | 73 +++++++------- src/main/java/frc/robot/Autos.java | 149 +++++++++++++++++------------ 2 files changed, 122 insertions(+), 100 deletions(-) diff --git a/src/main/deploy/choreo/GHtoNI.traj b/src/main/deploy/choreo/GHtoNI.traj index ee8da55c..712dee5a 100644 --- a/src/main/deploy/choreo/GHtoNI.traj +++ b/src/main/deploy/choreo/GHtoNI.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.929035186767578, "y":5.583443641662598, "heading":3.490658503988659, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.11117696762085, "y":5.479686737060547, "heading":3.490658503988659, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"NI.x", "val":7.929035186767578}, "y":{"exp":"NI.y", "val":5.583443641662598}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.11117696762085 m", "val":7.11117696762085}, "y":{"exp":"5.479686737060547 m", "val":5.479686737060547}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "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}, @@ -26,41 +26,38 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.28627], + "waypoints":[0.0,1.10984], "samples":[ - {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.13698, "ay":3.79035, "alpha":0.84439, "fx":[87.71603,83.47671,80.01486,84.71206], "fy":[56.74507,62.82369,67.1665,61.12493]}, - {"t":0.0402, "x":5.81047, "y":4.02023, "heading":3.14159, "vx":0.20649, "vy":0.15236, "omega":0.03394, "ax":5.13663, "ay":3.79009, "alpha":0.84392, "fx":[87.70793,83.47144,80.01152,84.70527], "fy":[56.74409,62.81908,67.15887,61.12086]}, - {"t":0.08039, "x":5.82292, "y":4.02942, "heading":-3.14023, "vx":0.41296, "vy":0.3047, "omega":0.06786, "ax":5.13621, "ay":3.78978, "alpha":0.84345, "fx":[87.69779,83.46037,80.00798,84.70298], "fy":[56.74427,62.82045,67.14975,61.10855]}, - {"t":0.12059, "x":5.84367, "y":4.04473, "heading":-3.1375, "vx":0.61941, "vy":0.45704, "omega":0.10177, "ax":5.13573, "ay":3.78943, "alpha":0.84299, "fx":[87.68534,83.44333,80.00412,84.70493], "fy":[56.74557,62.82762,67.13886,61.08787]}, - {"t":0.16078, "x":5.87271, "y":4.06616, "heading":-3.13341, "vx":0.82584, "vy":0.60935, "omega":0.13565, "ax":5.13517, "ay":3.78902, "alpha":0.84251, "fx":[87.67019,83.42007,79.99977,84.71078], "fy":[56.74794,62.84034,67.12582,61.05866]}, - {"t":0.20098, "x":5.91006, "y":4.09371, "heading":-3.12796, "vx":1.03226, "vy":0.76166, "omega":0.16952, "ax":5.13449, "ay":3.78852, "alpha":0.84199, "fx":[87.6518,83.39026,79.99473,84.72005], "fy":[56.75133,62.85828,67.11012,61.02069]}, - {"t":0.24117, "x":5.9557, "y":4.12739, "heading":-3.12114, "vx":1.23864, "vy":0.91394, "omega":0.20336, "ax":5.13368, "ay":3.78792, "alpha":0.84142, "fx":[87.62942,83.35342,79.98868,84.73203], "fy":[56.75564,62.88096,67.09103,60.97363]}, - {"t":0.28137, "x":6.00963, "y":4.16718, "heading":-3.11297, "vx":1.44499, "vy":1.0662, "omega":0.23718, "ax":5.13267, "ay":3.78718, "alpha":0.84073, "fx":[87.60195,83.30886,79.98114,84.74576], "fy":[56.76069,62.90769,67.0675,60.91696]}, - {"t":0.32157, "x":6.07186, "y":4.2131, "heading":-3.10344, "vx":1.65131, "vy":1.21843, "omega":0.27098, "ax":5.1314, "ay":3.78624, "alpha":0.83989, "fx":[87.56778,83.25547,79.97129,84.75971], "fy":[56.76614,62.93741,67.03801,60.84991]}, - {"t":0.36176, "x":6.14238, "y":4.26514, "heading":-3.09254, "vx":1.85757, "vy":1.37062, "omega":0.30474, "ax":5.12973, "ay":3.78502, "alpha":0.8388, "fx":[87.52437,83.1915,79.95773,84.77148], "fy":[56.77135,62.96848,67.0002,60.77117]}, - {"t":0.40196, "x":6.22119, "y":4.32329, "heading":-3.08029, "vx":2.06376, "vy":1.52276, "omega":0.33845, "ax":5.12745, "ay":3.78334, "alpha":0.83732, "fx":[87.46742,83.11386,79.9379,84.77698], "fy":[56.77497,62.99805,66.9502,60.67846]}, - {"t":0.44215, "x":6.30829, "y":4.38755, "heading":-3.06669, "vx":2.26986, "vy":1.67483, "omega":0.37211, "ax":5.12416, "ay":3.78092, "alpha":0.83523, "fx":[87.38907,83.01668,79.90668,84.76856], "fy":[56.77419,63.02088,66.88102,60.56732]}, - {"t":0.48235, "x":6.40367, "y":4.45793, "heading":-3.05173, "vx":2.47583, "vy":1.82681, "omega":0.40568, "ax":5.11899, "ay":3.77711, "alpha":0.83208, "fx":[87.27294,82.88725,79.85249,84.73007], "fy":[56.76224,63.02591,66.77836,60.42801]}, - {"t":0.52255, "x":6.50732, "y":4.53441, "heading":-3.03543, "vx":2.68159, "vy":1.97864, "omega":0.43913, "ax":5.10967, "ay":3.77026, "alpha":0.82698, "fx":[87.07758,82.69219,79.74389,84.62011], "fy":[56.71984,62.98494,66.60691,60.23458]}, - {"t":0.56274, "x":6.61924, "y":4.61699, "heading":-3.01778, "vx":2.88698, "vy":2.13018, "omega":0.47237, "ax":5.08795, "ay":3.75427, "alpha":0.81769, "fx":[86.65599,82.3078,79.46018,84.28952], "fy":[56.56961,62.79699,66.24443,59.88944]}, - {"t":0.60294, "x":6.73939, "y":4.70564, "heading":-2.99879, "vx":3.0915, "vy":2.28109, "omega":0.50524, "ax":4.98029, "ay":3.67497, "alpha":0.7952, "fx":[84.7467,80.61709,77.87302,82.43598], "fy":[55.55521,61.58544,64.67437,58.49982]}, - {"t":0.64313, "x":6.86768, "y":4.8003, "heading":-2.97848, "vx":3.29168, "vy":2.42881, "omega":0.5372, "ax":-4.98056, "ay":-3.67465, "alpha":-0.74561, "fx":[-84.52902,-80.60303,-78.12269,-82.43565], "fy":[-55.86594,-61.56843,-64.35422,-58.50523]}, - {"t":0.68333, "x":6.99597, "y":4.89496, "heading":-2.95689, "vx":3.09149, "vy":2.2811, "omega":0.50723, "ax":-5.08815, "ay":-3.75425, "alpha":-0.80296, "fx":[-86.52491,-82.12093,-79.59601,-84.48453], "fy":[-56.77356,-63.03564,-66.07345,-59.61691]}, - {"t":0.72352, "x":7.11613, "y":4.98362, "heading":-2.9365, "vx":2.88696, "vy":2.1302, "omega":0.47495, "ax":-5.10979, "ay":-3.77027, "alpha":-0.82025, "fx":[-86.92619,-82.3615,-79.88568,-84.96773], "fy":[-56.95711,-63.41366,-66.43002,-59.74592]}, - {"t":0.76372, "x":7.22804, "y":5.0662, "heading":-2.91741, "vx":2.68157, "vy":1.97865, "omega":0.44198, "ax":-5.11905, "ay":-3.77713, "alpha":-0.82944, "fx":[-87.08879,-82.42583,-80.01652,-85.21565], "fy":[-57.05027,-63.62579,-66.57534,-59.74391]}, - {"t":0.80392, "x":7.33169, "y":5.14268, "heading":-2.89964, "vx":2.47581, "vy":1.82682, "omega":0.40864, "ax":-5.12419, "ay":-3.78094, "alpha":-0.83529, "fx":[-87.17002,-82.43548,-80.09697,-85.38028], "fy":[-57.11586,-63.77718,-66.64688,-59.70443]}, - {"t":0.84411, "x":7.42707, "y":5.21306, "heading":-2.88321, "vx":2.26984, "vy":1.67485, "omega":0.37507, "ax":-5.12745, "ay":-3.78336, "alpha":-0.83941, "fx":[-87.21417,-82.42328,-80.15506,-85.50372], "fy":[-57.16906,-63.89704,-66.68418,-59.65248]}, - {"t":0.88431, "x":7.51417, "y":5.27732, "heading":-2.86814, "vx":2.06373, "vy":1.52277, "omega":0.34133, "ax":-5.12971, "ay":-3.78503, "alpha":-0.84247, "fx":[-87.23873,-82.40174,-80.20096,-85.60236], "fy":[-57.21497,-63.99676,-66.70321,-59.59747]}, - {"t":0.9245, "x":7.59298, "y":5.33547, "heading":-2.85442, "vx":1.85754, "vy":1.37063, "omega":0.30746, "ax":-5.13136, "ay":-3.78626, "alpha":-0.84484, "fx":[-87.25213,-82.37662,-80.23913,-85.68399], "fy":[-57.25563,-64.08176,-66.71176,-59.54366]}, - {"t":0.9647, "x":7.6635, "y":5.38751, "heading":-2.84206, "vx":1.65128, "vy":1.21844, "omega":0.27351, "ax":-5.13262, "ay":-3.7872, "alpha":-0.84673, "fx":[-87.25892,-82.35096,-80.27175,-85.7528], "fy":[-57.29192,-64.15498,-66.71419,-59.49322]}, - {"t":1.00489, "x":7.72572, "y":5.43343, "heading":-2.83107, "vx":1.44497, "vy":1.06621, "omega":0.23947, "ax":-5.13362, "ay":-3.78795, "alpha":-0.84825, "fx":[-87.26185,-82.32646,-80.29998,-85.81124], "fy":[-57.32424,-64.21815,-66.71315,-59.44734]}, - {"t":1.04509, "x":7.77966, "y":5.47322, "heading":-2.82144, "vx":1.23862, "vy":0.91395, "omega":0.20537, "ax":-5.13442, "ay":-3.78855, "alpha":-0.8495, "fx":[-87.26267,-82.30417,-80.32445,-85.86091], "fy":[-57.35272,-64.27239,-66.71037,-59.40673]}, - {"t":1.08529, "x":7.8253, "y":5.5069, "heading":-2.81319, "vx":1.03224, "vy":0.76166, "omega":0.17123, "ax":-5.13509, "ay":-3.78905, "alpha":-0.85054, "fx":[-87.26256,-82.28477,-80.34554,-85.90285], "fy":[-57.37743,-64.31843,-66.70704,-59.37181]}, - {"t":1.12548, "x":7.86264, "y":5.53445, "heading":-2.8063, "vx":0.82583, "vy":0.60936, "omega":0.13704, "ax":-5.13565, "ay":-3.78946, "alpha":-0.85139, "fx":[-87.26234,-82.26868,-80.36344,-85.9378], "fy":[-57.39835,-64.3568,-66.70402,-59.34284]}, - {"t":1.16568, "x":7.89169, "y":5.55589, "heading":-2.80079, "vx":0.6194, "vy":0.45704, "omega":0.10282, "ax":-5.13612, "ay":-3.78982, "alpha":-0.8521, "fx":[-87.26261,-82.25622,-80.37829,-85.96629], "fy":[-57.41545,-64.38788,-66.70195,-59.31999]}, - {"t":1.20587, "x":7.91244, "y":5.5712, "heading":-2.79666, "vx":0.41295, "vy":0.3047, "omega":0.06857, "ax":-5.13654, "ay":-3.79013, "alpha":-0.85268, "fx":[-87.26381,-82.2476,-80.39017,-85.9887], "fy":[-57.42871,-64.41196,-66.70128,-59.30337]}, - {"t":1.24607, "x":7.92489, "y":5.58038, "heading":-2.79391, "vx":0.20648, "vy":0.15236, "omega":0.03429, "ax":-5.13689, "ay":-3.79039, "alpha":-0.85314, "fx":[-87.26627,-82.24298,-80.39913,-86.00534], "fy":[-57.43808,-64.42924,-66.70239,-59.29306]}, - {"t":1.28627, "x":7.92904, "y":5.58344, "heading":-2.79253, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24646, "ay":4.75959, "alpha":1.12862, "fx":[75.89529,69.82763,62.76526,69.19804], "fy":[71.78112,77.70855,83.50697,78.24401]}, + {"t":0.03827, "x":5.80943, "y":4.02065, "heading":3.14159, "vx":0.16251, "vy":0.18215, "omega":0.04319, "ax":4.24611, "ay":4.75919, "alpha":1.12834, "fx":[75.88743,69.82221,62.76182,69.19184], "fy":[71.77674,77.70264,83.49878,78.23681]}, + {"t":0.07654, "x":5.81876, "y":4.03111, "heading":-3.13994, "vx":0.32501, "vy":0.36429, "omega":0.08637, "ax":4.2457, "ay":4.75873, "alpha":1.12813, "fx":[75.87833,69.80587,62.75579,69.19654], "fy":[71.7716,77.70474,83.4907,78.21785]}, + {"t":0.11481, "x":5.83431, "y":4.04854, "heading":-3.13663, "vx":0.4875, "vy":0.54641, "omega":0.12955, "ax":4.24522, "ay":4.75819, "alpha":1.12798, "fx":[75.86762,69.77837,62.74702,69.21184], "fy":[71.76549,77.71457,83.48238,78.18682]}, + {"t":0.15308, "x":5.85607, "y":4.07293, "heading":-3.13168, "vx":0.64996, "vy":0.7285, "omega":0.17272, "ax":4.24464, "ay":4.75753, "alpha":1.1279, "fx":[75.8548,69.73939,62.73532,69.23732], "fy":[71.75817,77.73173,83.47325,78.14326]}, + {"t":0.19135, "x":5.88405, "y":4.1043, "heading":-3.12507, "vx":0.81241, "vy":0.91058, "omega":0.21588, "ax":4.24392, "ay":4.75673, "alpha":1.12787, "fx":[75.8391,69.68842,62.72042,69.27237], "fy":[71.74929,77.75565,83.46251,78.08652]}, + {"t":0.22962, "x":5.91825, "y":4.14263, "heading":-3.1168, "vx":0.97482, "vy":1.09262, "omega":0.25905, "ax":4.24303, "ay":4.75573, "alpha":1.12789, "fx":[75.81942,69.62474,62.70186,69.31609], "fy":[71.73828,77.78548,83.44901,78.01561]}, + {"t":0.26789, "x":5.95867, "y":4.18792, "heading":-3.10689, "vx":1.13721, "vy":1.27462, "omega":0.30221, "ax":4.24189, "ay":4.75444, "alpha":1.12796, "fx":[75.79411,69.54718,62.67887,69.36705], "fy":[71.72418,77.81988,83.43096,77.92899]}, + {"t":0.30616, "x":6.0053, "y":4.24019, "heading":-3.09532, "vx":1.29955, "vy":1.45658, "omega":0.34538, "ax":4.24036, "ay":4.75272, "alpha":1.12806, "fx":[75.76056,69.45384,62.64999,69.42291], "fy":[71.70525,77.85665,83.40552,77.82412]}, + {"t":0.34443, "x":6.05813, "y":4.29941, "heading":-3.08211, "vx":1.46183, "vy":1.63846, "omega":0.38855, "ax":4.23822, "ay":4.75031, "alpha":1.12817, "fx":[75.71427,69.34129,62.61234,69.47946], "fy":[71.67811,77.89181,83.36778,77.69645]}, + {"t":0.3827, "x":6.11718, "y":4.36559, "heading":-3.06724, "vx":1.62402, "vy":1.82026, "omega":0.43173, "ax":4.23501, "ay":4.7467, "alpha":1.12829, "fx":[75.64662,69.20267,62.55977,69.5283], "fy":[71.63564,77.91749,83.30822,77.53691]}, + {"t":0.42097, "x":6.18244, "y":4.43873, "heading":-3.05071, "vx":1.7861, "vy":2.00192, "omega":0.47491, "ax":4.22966, "ay":4.7407, "alpha":1.12838, "fx":[75.53819,69.02204,62.47716,69.55005], "fy":[71.56056,77.91536,83.20524,77.32434]}, + {"t":0.45924, "x":6.25389, "y":4.51882, "heading":-3.03254, "vx":1.94797, "vy":2.18335, "omega":0.51809, "ax":4.21897, "ay":4.72871, "alpha":1.12843, "fx":[75.33208,68.7516,62.3178,69.48686], "fy":[71.39985,77.83077,82.99532,76.99544]}, + {"t":0.49751, "x":6.33153, "y":4.60584, "heading":-3.01271, "vx":2.10943, "vy":2.36432, "omega":0.56127, "ax":4.18703, "ay":4.6929, "alpha":1.12875, "fx":[74.74931,68.15337,61.84491,69.05245], "fy":[70.88599,77.39257,82.36628,76.23525]}, + {"t":0.53579, "x":6.41532, "y":4.69976, "heading":-2.99123, "vx":2.26967, "vy":2.54391, "omega":0.60447, "ax":-0.00061, "ay":-0.00125, "alpha":0.15067, "fx":[0.45973,0.33382,-0.47976,-0.35385], "fy":[-0.37433,0.45675,0.33349,-0.49758]}, + {"t":0.57406, "x":6.50218, "y":4.79711, "heading":-2.9681, "vx":2.26965, "vy":2.54387, "omega":0.61024, "ax":-4.18707, "ay":-4.69297, "alpha":-1.12637, "fx":[-74.68492,-67.88782,-61.87018,-69.35932], "fy":[-70.96195,-77.62436,-82.33994,-75.95815]}, + {"t":0.61233, "x":6.58598, "y":4.89103, "heading":-2.94475, "vx":2.10941, "vy":2.36426, "omega":0.56713, "ax":-4.21897, "ay":-4.72871, "alpha":-1.13065, "fx":[-75.23551,-68.22235,-62.33018,-70.10043], "fy":[-71.50972,-78.29429,-82.97869,-76.43858]}, + {"t":0.6506, "x":6.66362, "y":4.97805, "heading":-2.92304, "vx":1.94795, "vy":2.1833, "omega":0.52386, "ax":-4.22964, "ay":-4.74066, "alpha":-1.13348, "fx":[-75.40372,-68.24941,-62.48719,-70.44614], "fy":[-71.71015,-78.59231,-83.19058,-76.51005]}, + {"t":0.68887, "x":6.73507, "y":5.05813, "heading":-2.90299, "vx":1.78608, "vy":2.00187, "omega":0.48048, "ax":-4.23498, "ay":-4.74664, "alpha":-1.13569, "fx":[-75.47469,-68.20697,-62.57093,-70.68298], "fy":[-71.82441,-78.78989,-83.29295,-76.48691]}, + {"t":0.72714, "x":6.80032, "y":5.13127, "heading":-2.8846, "vx":1.624, "vy":1.82021, "omega":0.43702, "ax":-4.23818, "ay":-4.75023, "alpha":-1.13749, "fx":[-75.50666,-68.14288,-62.6266,-70.86874], "fy":[-71.90412,-78.94171,-83.35043,-76.43247]}, + {"t":0.76541, "x":6.85937, "y":5.19745, "heading":-2.86788, "vx":1.4618, "vy":1.63842, "omega":0.39349, "ax":-4.24031, "ay":-4.75262, "alpha":-1.139, "fx":[-75.51961,-68.07312,-62.66868,-71.02287], "fy":[-71.96602,-79.06608,-83.3851,-76.36785]}, + {"t":0.80368, "x":6.9122, "y":5.25667, "heading":-2.85282, "vx":1.29953, "vy":1.45654, "omega":0.3499, "ax":-4.24184, "ay":-4.75433, "alpha":-1.14027, "fx":[-75.52242,-68.00454,-62.70299,-71.15378], "fy":[-72.01695,-79.1709,-83.40673,-76.30211]}, + {"t":0.84195, "x":6.95883, "y":5.30893, "heading":-2.83943, "vx":1.13719, "vy":1.27459, "omega":0.30626, "ax":-4.24297, "ay":-4.75561, "alpha":-1.14134, "fx":[-75.51975,-67.94054,-62.73215,-71.26579], "fy":[-72.0601,-79.26017,-83.42041,-76.23972]}, + {"t":0.88022, "x":6.99925, "y":5.35423, "heading":-2.82771, "vx":0.97481, "vy":1.09259, "omega":0.26258, "ax":-4.24386, "ay":-4.7566, "alpha":-1.14222, "fx":[-75.51438,-67.883,-62.75739,-71.36135], "fy":[-72.09708,-79.33616,-83.42916,-76.18314]}, + {"t":0.91849, "x":7.03344, "y":5.39256, "heading":-2.81766, "vx":0.81239, "vy":0.91055, "omega":0.21887, "ax":-4.24457, "ay":-4.7574, "alpha":-1.14294, "fx":[-75.50807,-67.83302,-62.77934,-71.442], "fy":[-72.12874,-79.40025,-83.43487,-76.13382]}, + {"t":0.95676, "x":7.06143, "y":5.42392, "heading":-2.80928, "vx":0.64995, "vy":0.72848, "omega":0.17513, "ax":-4.24515, "ay":-4.75805, "alpha":-1.14351, "fx":[-75.50201,-67.7913,-62.79825,-71.50875], "fy":[-72.15554,-79.45334,-83.43884,-76.09266]}, + {"t":0.99503, "x":7.08319, "y":5.44832, "heading":-2.80258, "vx":0.48749, "vy":0.54639, "omega":0.13136, "ax":-4.24563, "ay":-4.7586, "alpha":-1.14393, "fx":[-75.49706,-67.75828,-62.81426,-71.56232], "fy":[-72.17774,-79.49604,-83.44199,-76.06023]}, + {"t":1.0333, "x":7.09874, "y":5.46575, "heading":-2.79755, "vx":0.32501, "vy":0.36428, "omega":0.08758, "ax":-4.24604, "ay":-4.75906, "alpha":-1.14421, "fx":[-75.49383,-67.73429,-62.8274,-71.60321], "fy":[-72.1955,-79.5288,-83.44499,-76.03693]}, + {"t":1.07157, "x":7.10807, "y":5.4762, "heading":-2.7942, "vx":0.16251, "vy":0.18215, "omega":0.04379, "ax":-4.24639, "ay":-4.75946, "alpha":-1.14436, "fx":[-75.49277,-67.71952,-62.83769,-71.6318], "fy":[-72.20892,-79.55194,-83.4483,-76.02301]}, + {"t":1.10984, "x":7.11118, "y":5.47969, "heading":-2.79253, "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 4af8dce4..f2a26f49 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -9,6 +9,7 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; @@ -143,18 +144,24 @@ public void runCoralPath( endPos.length() == 1 ? scoreCoralInAuto( () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) - : AutoAim.translateToPose( // TODO does this get called? - swerve, - () -> steps.get(startPos + "to" + endPos).getFinalPose().get()) - .until( - () -> - AutoAim.isInTolerance( - swerve.getPose(), - steps - .get(startPos + "to" + endPos) - .getFinalPose() - .get())) - .withTimeout(2.0)), + : Commands.print("autoaligning") + .andThen( + AutoAim.translateToPose( // TODO does this get called? + swerve, + () -> + steps + .get(startPos + "to" + endPos) + .getFinalPose() + .get()) + .until( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + steps + .get(startPos + "to" + endPos) + .getFinalPose() + .get())) + .withTimeout(2.0))), steps.get(endPos + "to" + nextPos).cmd())); } @@ -365,24 +372,29 @@ public Command CMtoGH() { // algae path steps .get("GHtoNI") .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune - .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); - // ------------------sketchy-------- - routine - .observe(steps.get("NItoIJ").done()) .onTrue( Commands.sequence( - intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), - steps.get("IJtoNI").cmd())); + scoreAlgaeInAuto() + // , steps.get("NItoIJ").cmd() + )); + // ------------------sketchy-------- + // routine + // .observe(steps.get("NItoIJ").done()) + // .onTrue( + // Commands.sequence( + // intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), + // steps.get("IJtoNI").cmd())); - routine - .observe( - steps - .get("IJtoNI") - .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune - .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); - routine - .observe(steps.get("NItoEF").done()) - .onTrue(Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); + // routine + // .observe( + // steps + // .get("IJtoNI") + // .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO + // tune + // .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); + // routine + // .observe(steps.get("NItoEF").done()) + // .onTrue(Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); // --------------------------- // Commands.sequence( @@ -551,42 +563,55 @@ public void bindAlgaeElevatorExtension(AutoRoutine routine) { public Command scoreAlgaeInAuto() { // oh good lord return Commands.sequence( - Commands.waitUntil( - new Trigger( + Commands.waitUntil( + new Trigger( + () -> + MathUtil.isNear( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + swerve.getPose().getX(), + Units.inchesToMeters(3.0)) + && MathUtil.isNear( + 0, + Math.hypot( + swerve.getVelocityRobotRelative().vxMetersPerSecond, + swerve.getVelocityRobotRelative().vyMetersPerSecond), + AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) + && MathUtil.isNear( + 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) + // .debounce(0.06)), // TODO + ), + Commands.print("Scoring algae"), + Commands.runOnce( + () -> { + Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); + autoScore = true; + }), + Commands.waitUntil(() -> !manipulator.hasAlgae()) + .alongWith( + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) + : Commands.none()) + .andThen( + Commands.runOnce( + () -> { + autoScore = false; + autoPreScore = false; + }))) + .raceWith( + AutoAim.translateToXCoord( + swerve, + () -> + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_NET_X + : AutoAim.RED_NET_X, + () -> 0, () -> - MathUtil.isNear( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_NET_X - : AutoAim.RED_NET_X, - swerve.getPose().getX(), - Units.inchesToMeters(3.0)) - && MathUtil.isNear( - 0, - Math.hypot( - swerve.getVelocityRobotRelative().vxMetersPerSecond, - swerve.getVelocityRobotRelative().vyMetersPerSecond), - AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - && MathUtil.isNear( - 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) - // .debounce(0.06)), // TODO - ), - Commands.print("Scoring algae"), - Commands.runOnce( - () -> { - Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); - autoScore = true; - }), - Commands.waitUntil(() -> !manipulator.hasAlgae()) - .alongWith( - Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) - : Commands.none()) - .andThen( - Commands.runOnce( - () -> { - autoScore = false; - autoPreScore = false; - }))); + (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? Rotation2d.k180deg + : Rotation2d.kZero) + .plus(Rotation2d.fromDegrees(30.0)))); } public Command intakeAlgaeInAuto(Supplier> pose) { From 9e8ec06dadc90737de9e344190ac16e8c5b9feb0 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 12 Apr 2025 00:52:11 -0700 Subject: [PATCH 042/154] works except for last intake --- src/main/java/frc/robot/Autos.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f2a26f49..cf30e0b5 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -372,18 +372,14 @@ public Command CMtoGH() { // algae path steps .get("GHtoNI") .atTime(steps.get("GHtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO tune + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoIJ").cmd())); + // ------------------sketchy-------- + routine + .observe(steps.get("NItoIJ").done()) .onTrue( Commands.sequence( - scoreAlgaeInAuto() - // , steps.get("NItoIJ").cmd() - )); - // ------------------sketchy-------- - // routine - // .observe(steps.get("NItoIJ").done()) - // .onTrue( - // Commands.sequence( - // intakeAlgaeInAuto(() -> steps.get("NItoJ").getFinalPose()), - // steps.get("IJtoNI").cmd())); + intakeAlgaeInAuto(() -> steps.get("NItoIJ").getFinalPose()), + swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2))); // routine // .observe( From 25ac5637581cdbafd8549a800a2ebb06a8600a9e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 12 Apr 2025 01:00:48 -0700 Subject: [PATCH 043/154] change idle shoulder position --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index a9d7b936..a157f09e 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -30,7 +30,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = From 96cab99e61cd47b18fb99a68d0b083cd60a7492b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 12 Apr 2025 10:24:18 -0700 Subject: [PATCH 044/154] copy over stuff from other branch --- src/main/deploy/choreo/AtoB.traj | 159 +++++++++++++++++++++++++++++ src/main/deploy/choreo/BtoB.traj | 143 ++++++++++++++++++++++++++ src/main/deploy/choreo/LOtoA.traj | 98 ++++++++++++++++++ src/main/java/frc/robot/Autos.java | 38 +++++++ 4 files changed, 438 insertions(+) create mode 100644 src/main/deploy/choreo/AtoB.traj create mode 100644 src/main/deploy/choreo/BtoB.traj create mode 100644 src/main/deploy/choreo/LOtoA.traj diff --git a/src/main/deploy/choreo/AtoB.traj b/src/main/deploy/choreo/AtoB.traj new file mode 100644 index 00000000..10ad9e0f --- /dev/null +++ b/src/main/deploy/choreo/AtoB.traj @@ -0,0 +1,159 @@ +{ + "name":"AtoB", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.6018691062927246, "y":4.209113121032715, "heading":1.610775050878982, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.667668342590332, "y":4.032181262969971, "heading":3.141592653589793, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.4166265726089478, "y":4.017168045043945, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.6018691062927246 m", "val":2.6018691062927246}, "y":{"exp":"4.209113121032715 m", "val":4.209113121032715}, "heading":{"exp":"1.610775050878982 rad", "val":1.610775050878982}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.667668342590332 m", "val":1.667668342590332}, "y":{"exp":"4.032181262969971 m", "val":4.032181262969971}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.4166265726089478 m", "val":1.4166265726089478}, "y":{"exp":"4.017168045043945 m", "val":4.017168045043945}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.6431,1.21973,1.45584,1.7246,2.85006], + "samples":[ + {"t":0.0, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.37447, "ay":0.35577, "alpha":0.11969, "fx":[-104.17788,-104.24949,-104.24389,-104.17052], "fy":[6.42746,5.13407,5.19713,6.50629]}, + {"t":0.02297, "x":3.16347, "y":4.1821, "heading":0.0, "vx":-0.14641, "vy":0.00817, "omega":0.00275, "ax":-6.37201, "ay":0.35565, "alpha":0.13604, "fx":[-104.13356,-104.21475,-104.20796,-104.12449], "fy":[6.50811,5.03982,5.11022,6.59887]}, + {"t":0.04594, "x":3.15843, "y":4.18238, "heading":0.00006, "vx":-0.29276, "vy":0.01634, "omega":0.00587, "ax":-6.36833, "ay":0.35547, "alpha":0.16039, "fx":[-104.0676,-104.16299,-104.15422,-104.05567], "fy":[6.62778,4.89981,4.98067,6.73692]}, + {"t":0.0689, "x":3.15002, "y":4.18285, "heading":0.0002, "vx":-0.43902, "vy":0.0245, "omega":0.00956, "ax":-6.36226, "ay":0.35517, "alpha":0.20052, "fx":[-103.95907,-104.07762,-104.06507,-103.94158], "fy":[6.82392,4.67004,4.76682,6.9648]}, + {"t":0.09187, "x":3.13826, "y":4.1835, "heading":0.00042, "vx":-0.58515, "vy":0.03266, "omega":0.01416, "ax":-6.3503, "ay":0.35458, "alpha":0.27909, "fx":[-103.74713,-103.91023,-103.88838,-103.71575], "fy":[7.20427,4.22379,4.34665,7.41224]}, + {"t":0.11484, "x":3.12315, "y":4.18435, "heading":0.00074, "vx":-0.731, "vy":0.04081, "omega":0.02057, "ax":-6.31591, "ay":0.35287, "alpha":0.50218, "fx":[-103.14999,-103.43392,-103.37149,-103.05713], "fy":[8.25795,2.98385,3.1435,8.69001]}, + {"t":0.13781, "x":3.10469, "y":4.18538, "heading":0.00122, "vx":-0.87607, "vy":0.04891, "omega":0.03211, "ax":-5.30787, "ay":0.29232, "alpha":6.01403, "fx":[-91.4594,-93.03418,-83.2136,-79.38706], "fy":[25.00769,-17.32721,-28.10325,39.53826]}, + {"t":0.16077, "x":3.08317, "y":4.18658, "heading":0.00195, "vx":-0.99798, "vy":0.05562, "omega":0.17024, "ax":-0.00174, "ay":0.00207, "alpha":19.17611, "fx":[-52.47809,-52.30427,52.45621,52.21222], "fy":[53.40592,-53.54289,-53.36712,53.63915]}, + {"t":0.18374, "x":3.06025, "y":4.18786, "heading":0.00586, "vx":-0.99802, "vy":0.05567, "omega":0.61067, "ax":0.00003, "ay":0.00054, "alpha":18.68349, "fx":[-51.31744,-50.71507,51.32711,50.70736], "fy":[51.82391,-52.40441,-51.80564,52.42154]}, + {"t":0.20671, "x":3.03733, "y":4.18913, "heading":0.01989, "vx":-0.99802, "vy":0.05568, "omega":1.03979, "ax":0.00001, "ay":0.00014, "alpha":18.13965, "fx":[-50.52785,-48.51752,50.53027,48.51563], "fy":[49.6052,-51.57065,-49.60033,51.57511]}, + {"t":0.22968, "x":3.0144, "y":4.19041, "heading":0.04377, "vx":-0.99802, "vy":0.05569, "omega":1.45641, "ax":0.0, "ay":0.00004, "alpha":17.53746, "fx":[-49.98257,-45.70215,49.98313,45.70172], "fy":[46.77648,-50.96612,-46.77525,50.9672]}, + {"t":0.25265, "x":2.99148, "y":4.19169, "heading":0.07722, "vx":-0.99802, "vy":0.05569, "omega":1.85921, "ax":0.0, "ay":0.00001, "alpha":16.86878, "fx":[-49.55485,-42.29525,49.55495,42.29518], "fy":[43.35939,-50.46602,-43.35915,50.46622]}, + {"t":0.27561, "x":2.96856, "y":4.19297, "heading":0.11992, "vx":-0.99802, "vy":0.05569, "omega":2.24665, "ax":0.0, "ay":0.0, "alpha":16.1244, "fx":[-49.09427,-38.33269,49.09425,38.3327], "fy":[39.38603,-49.92108,-39.38607,49.92103]}, + {"t":0.29858, "x":2.94564, "y":4.19425, "heading":0.17152, "vx":-0.99802, "vy":0.05569, "omega":2.61699, "ax":0.0, "ay":0.0, "alpha":15.29407, "fx":[-48.43103,-33.86812,48.43098,33.86816], "fy":[34.90628,-49.16272,-34.90646,49.16258]}, + {"t":0.32155, "x":2.92271, "y":4.19553, "heading":0.23163, "vx":-0.99802, "vy":0.05569, "omega":2.96826, "ax":0.0, "ay":-0.00001, "alpha":14.36661, "fx":[-47.38163,-28.98272,47.38156,28.98277], "fy":[29.9974,-48.00914,-29.99768,48.00894]}, + {"t":0.34452, "x":2.89979, "y":4.19681, "heading":0.2998, "vx":-0.99802, "vy":0.05569, "omega":3.29823, "ax":0.0, "ay":-0.00001, "alpha":13.33021, "fx":[-45.75745,-23.79492,45.75738,23.79497], "fy":[24.77383,-46.27424,-24.77418,46.27398]}, + {"t":0.36748, "x":2.87687, "y":4.19809, "heading":0.37556, "vx":-0.99802, "vy":0.05569, "omega":3.60439, "ax":0.0, "ay":-0.00001, "alpha":12.17302, "fx":[-43.37758,-18.46894,43.37752,18.46897], "fy":[19.39598,-43.78051,-19.39636,43.78023]}, + {"t":0.39045, "x":2.85395, "y":4.19937, "heading":0.45834, "vx":-0.99802, "vy":0.05569, "omega":3.88398, "ax":0.0, "ay":-0.00001, "alpha":10.88424, "fx":[-40.08637,-13.22012,40.08634,13.22014], "fy":[14.07597,-40.37686,-14.07627,40.37662]}, + {"t":0.41342, "x":2.83103, "y":4.20065, "heading":0.54755, "vx":-0.99802, "vy":0.05569, "omega":4.13397, "ax":0.0, "ay":0.0, "alpha":9.45574, "fx":[-35.77619,-8.3144,35.77619,8.31441], "fy":[9.0775,-35.96126,-9.07763,35.96116]}, + {"t":0.43639, "x":2.8081, "y":4.20193, "heading":0.6425, "vx":-0.99802, "vy":0.05569, "omega":4.35115, "ax":0.0, "ay":0.0, "alpha":7.88428, "fx":[-30.4137,-4.05867,30.41372,4.05868], "fy":[4.70682,-30.50691,-4.70671,30.50701]}, + {"t":0.45936, "x":2.78518, "y":4.2032, "heading":0.74243, "vx":-0.99802, "vy":0.05569, "omega":4.53223, "ax":0.0, "ay":0.0, "alpha":6.17426, "fx":[-24.06621,-0.77899,24.06622,0.779], "fy":[1.29128,-24.08823,-1.29127,24.08824]}, + {"t":0.48232, "x":2.76226, "y":4.20448, "heading":0.84653, "vx":-0.99802, "vy":0.05569, "omega":4.67404, "ax":0.0, "ay":-0.00008, "alpha":4.34051, "fx":[-16.92137,1.21482,16.92123,-1.21498], "fy":[-0.85653,-16.90063,0.85368,16.89793]}, + {"t":0.50529, "x":2.73934, "y":4.20576, "heading":0.95388, "vx":-0.99802, "vy":0.05569, "omega":4.77373, "ax":-0.00005, "ay":-0.00082, "alpha":2.41003, "fx":[-9.29113,1.67526,9.28973,-1.67683], "fy":[-1.49221,-9.27014,1.46526,9.24359]}, + {"t":0.52826, "x":2.71641, "y":4.20704, "heading":1.06352, "vx":-0.99802, "vy":0.05567, "omega":4.82908, "ax":-0.00038, "ay":-0.00679, "alpha":0.42162, "fx":[-1.59342,0.46246,1.58108,-0.47486], "fy":[-0.54604,-1.68864,0.32394,1.46662]}, + {"t":0.55123, "x":2.69349, "y":4.20832, "heading":1.17443, "vx":-0.99803, "vy":0.05551, "omega":4.83877, "ax":-0.00302, "ay":-0.05486, "alpha":-1.57742, "fx":[5.67514,-2.44778,-5.76916,2.34454], "fy":[1.37557,4.77692,-3.17345,-6.56642]}, + {"t":0.57419, "x":2.67057, "y":4.20958, "heading":1.28557, "vx":-0.99809, "vy":0.05425, "omega":4.80254, "ax":-0.02121, "ay":-0.42953, "alpha":-3.50787, "fx":[11.89237,-7.18222,-12.37596,6.27882], "fy":[-0.67914,4.93109,-13.48359,-18.85667]}, + {"t":0.59716, "x":2.64764, "y":4.21071, "heading":1.39587, "vx":-0.99858, "vy":0.04439, "omega":4.72197, "ax":-0.03524, "ay":-2.74147, "alpha":-3.80169, "fx":[14.79398,-11.87691,-13.95872,8.73693], "fy":[-38.24697,-34.80196,-51.70822,-54.51381]}, + {"t":0.62013, "x":2.6247, "y":4.21101, "heading":1.50433, "vx":-0.99939, "vy":-0.01858, "omega":4.63465, "ax":0.48388, "ay":-5.56312, "alpha":-1.0493, "fx":[13.83436,3.37209,2.43482,12.00069], "fy":[-89.8387,-90.59438,-91.96375,-91.38867]}, + {"t":0.6431, "x":2.60187, "y":4.20911, "heading":1.61078, "vx":-0.98828, "vy":-0.14635, "omega":4.61055, "ax":1.11518, "ay":-5.48023, "alpha":-0.9287, "fx":[23.34321,14.26096,13.45897,21.86136], "fy":[-88.03068,-89.72896,-91.03964,-89.56608]}, + {"t":0.66369, "x":2.58175, "y":4.20494, "heading":1.70573, "vx":-0.96531, "vy":-0.25921, "omega":4.59143, "ax":0.98255, "ay":-3.21186, "alpha":-3.7997, "fx":[30.21085,1.52074,4.4663,28.05371], "fy":[-41.87615,-47.85148,-62.57908,-57.72468]}, + {"t":0.68429, "x":2.56208, "y":4.19892, "heading":1.80028, "vx":-0.94508, "vy":-0.32536, "omega":4.51318, "ax":0.24919, "ay":-0.70616, "alpha":-6.31426, "fx":[18.30591,-17.80656,-8.79906,24.59507], "fy":[9.20817,0.82019,-32.11962,-24.08631]}, + {"t":0.70488, "x":2.54267, "y":4.19207, "heading":1.89323, "vx":-0.93994, "vy":-0.3399, "omega":4.38314, "ax":0.04443, "ay":-0.12234, "alpha":-6.505, "fx":[12.43735,-22.23621,-10.74513,23.4491], "fy":[20.62884,9.05096,-24.55575,-13.12434]}, + {"t":0.72547, "x":2.52332, "y":4.18504, "heading":1.98349, "vx":-0.93903, "vy":-0.34242, "omega":4.24918, "ax":0.00769, "ay":-0.02107, "alpha":-6.57242, "fx":[9.74105,-23.88579,-9.4505,24.09799], "fy":[23.45848,8.7323,-24.12719,-9.44136]}, + {"t":0.74607, "x":2.50399, "y":4.17799, "heading":2.071, "vx":-0.93887, "vy":-0.34285, "omega":4.11382, "ax":0.00133, "ay":-0.00363, "alpha":-6.66423, "fx":[7.60862,-25.02049,-7.55904,25.05764], "fy":[24.82634,6.99098,-24.94036,-7.11446]}, + {"t":0.76666, "x":2.48465, "y":4.17092, "heading":2.15572, "vx":-0.93884, "vy":-0.34293, "omega":3.97658, "ax":0.00023, "ay":-0.00062, "alpha":-6.78376, "fx":[5.55298,-26.00099,-5.54458,26.00749], "fy":[25.88235,4.98597,-25.90176,-5.00741]}, + {"t":0.78726, "x":2.46532, "y":4.16386, "heading":2.23762, "vx":-0.93884, "vy":-0.34294, "omega":3.83687, "ax":0.00004, "ay":-0.00011, "alpha":-6.92994, "fx":[3.48637,-26.89258,-3.48499,26.89369], "fy":[26.82344,2.91239,-26.82667,-2.91603]}, + {"t":0.80785, "x":2.44598, "y":4.1568, "heading":2.31663, "vx":-0.93884, "vy":-0.34294, "omega":3.69416, "ax":0.00001, "ay":-0.00002, "alpha":-7.10161, "fx":[1.3911,-27.709,-1.3909,27.70916], "fy":[27.68552,0.80142,-27.68599,-0.80196]}, + {"t":0.82844, "x":2.42665, "y":4.14974, "heading":2.39271, "vx":-0.93884, "vy":-0.34294, "omega":3.54791, "ax":0.0, "ay":0.0, "alpha":-7.29759, "fx":[-0.73705,-28.45399,0.73706,28.45399], "fy":[28.4761,-1.34271,-28.47612,1.34268]}, + {"t":0.84904, "x":2.40731, "y":4.14267, "heading":2.46578, "vx":-0.93884, "vy":-0.34294, "omega":3.39762, "ax":0.0, "ay":0.0, "alpha":-7.51666, "fx":[-2.89835,-29.12903,2.89834,29.12902], "fy":[29.19731,-3.51882,-29.19728,3.51885]}, + {"t":0.86963, "x":2.38798, "y":4.13561, "heading":2.53575, "vx":-0.93884, "vy":-0.34294, "omega":3.24282, "ax":0.0, "ay":0.0, "alpha":-7.75759, "fx":[-5.09065,-29.73524,5.09064,29.73523], "fy":[29.85029,-5.72453,-29.85028,5.72454]}, + {"t":0.89023, "x":2.36864, "y":4.12855, "heading":2.60253, "vx":-0.93884, "vy":-0.34294, "omega":3.08306, "ax":0.0, "ay":0.0, "alpha":-8.01908, "fx":[-7.30966,-30.27411,7.30966,30.27412], "fy":[30.4365,-7.95553,-30.43652,7.95551]}, + {"t":0.91082, "x":2.34931, "y":4.12149, "heading":2.66602, "vx":-0.93884, "vy":-0.34294, "omega":2.91791, "ax":0.0, "ay":0.0, "alpha":-8.29977, "fx":[-9.54907,-30.74821,9.54908,30.74822], "fy":[30.95835,-10.20556,-30.95839,10.2055]}, + {"t":0.93141, "x":2.32998, "y":4.11442, "heading":2.72611, "vx":-0.93884, "vy":-0.34294, "omega":2.74699, "ax":0.0, "ay":0.0, "alpha":-8.59826, "fx":[-11.80071,-31.16156,11.80073,31.16159], "fy":[31.41971,-12.46651,-31.41976,12.46645]}, + {"t":0.95201, "x":2.31064, "y":4.10736, "heading":2.78269, "vx":-0.93884, "vy":-0.34294, "omega":2.56991, "ax":0.0, "ay":0.0, "alpha":-8.91307, "fx":[-14.05477,-31.52014,14.05479,31.52016], "fy":[31.82633,-14.72871,-31.82638,14.72865]}, + {"t":0.9726, "x":2.29131, "y":4.1003, "heading":2.83561, "vx":-0.93884, "vy":-0.34294, "omega":2.38636, "ax":0.0, "ay":0.0, "alpha":-9.24269, "fx":[-16.30004,-31.8321,16.30004,31.83209], "fy":[32.18615,-16.98109,-32.18615,16.98108]}, + {"t":0.9932, "x":2.27197, "y":4.09324, "heading":2.88476, "vx":-0.93884, "vy":-0.34294, "omega":2.19601, "ax":0.0, "ay":0.00001, "alpha":-9.58558, "fx":[-18.52417,-32.10807,18.52409,32.10794], "fy":[32.50957,-19.2114,-32.50932,19.21171]}, + {"t":1.01379, "x":2.25264, "y":4.08617, "heading":2.92998, "vx":-0.93884, "vy":-0.34294, "omega":1.99861, "ax":-0.00002, "ay":0.00005, "alpha":-9.94015, "fx":[-20.71402,-32.36137,20.71357,32.36055], "fy":[32.80983,-21.40614,-32.80827,21.40805]}, + {"t":1.03439, "x":2.2333, "y":4.07911, "heading":2.97114, "vx":-0.93884, "vy":-0.34294, "omega":1.7939, "ax":-0.0001, "ay":0.00028, "alpha":-10.30482, "fx":[-22.85617,-32.60878,22.85396,32.60436], "fy":[33.10437,-23.54908,-33.0962,23.55907]}, + {"t":1.05498, "x":2.21397, "y":4.07205, "heading":3.00808, "vx":-0.93884, "vy":-0.34294, "omega":1.58168, "ax":-0.00051, "ay":0.0014, "alpha":-10.67799, "fx":[-24.9386,-32.87434,24.92824,32.85134], "fy":[33.42144,-25.61329,-33.38023,25.66341]}, + {"t":1.07557, "x":2.19463, "y":4.06499, "heading":3.04066, "vx":-0.93885, "vy":-0.34291, "omega":1.36178, "ax":-0.00251, "ay":0.00688, "alpha":-11.05809, "fx":[-26.95742,-33.20844,26.91016,33.09145], "fy":[33.83227,-27.52214,-33.6287,27.76837]}, + {"t":1.09617, "x":2.1753, "y":4.05792, "heading":3.0687, "vx":-0.9389, "vy":-0.34277, "omega":1.13405, "ax":-0.01214, "ay":0.03328, "alpha":-11.44308, "fx":[-28.94482,-33.78116,28.73465,33.19774], "fy":[34.60248,-28.96376,-33.6145,30.15204]}, + {"t":1.11676, "x":2.15596, "y":4.05087, "heading":3.09206, "vx":-0.93915, "vy":-0.34208, "omega":0.89839, "ax":-0.05732, "ay":0.15822, "alpha":-11.81935, "fx":[-31.06693,-35.31582,30.16257,32.47192], "fy":[36.85688,-28.50036,-32.14356,34.13352]}, + {"t":1.13736, "x":2.13661, "y":4.04386, "heading":3.11056, "vx":-0.94033, "vy":-0.33882, "omega":0.65498, "ax":-0.25957, "ay":0.73909, "alpha":-11.92141, "fx":[-33.7054,-40.75283,29.90925,27.57507], "fy":[44.8117,-18.91953,-22.5108,44.94967]}, + {"t":1.15795, "x":2.11719, "y":4.03704, "heading":3.12405, "vx":-0.94568, "vy":-0.3236, "omega":0.40947, "ax":-0.94809, "ay":3.10725, "alpha":-8.0478, "fx":[-34.92718,-50.02994,13.65593,9.30341], "fy":[65.91283,30.10913,36.37127,70.79697]}, + {"t":1.17854, "x":2.09751, "y":4.03104, "heading":3.13248, "vx":-0.9652, "vy":-0.25961, "omega":0.24373, "ax":-1.13595, "ay":5.46041, "alpha":-2.18115, "fx":[-25.97417,-31.62122,-9.30922,-7.3778], "fy":[89.68269,85.16752,89.56231,92.65658]}, + {"t":1.19914, "x":2.07739, "y":4.02685, "heading":3.1375, "vx":-0.9886, "vy":-0.14716, "omega":0.19881, "ax":-0.53658, "ay":6.07458, "alpha":-0.81954, "fx":[-12.52412,-13.65558,-4.66182,-4.24676], "fy":[99.22551,98.66377,99.45242,99.88968]}, + {"t":1.21973, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-0.99965, "vy":-0.02206, "omega":0.18193, "ax":-6.34649, "ay":0.6032, "alpha":-0.11968, "fx":[-103.70039,-103.8213,-103.80702,-103.68314], "fy":[10.44354,9.15915,9.27123,10.57104]}, + {"t":1.24334, "x":2.03155, "y":4.02475, "heading":-3.1373, "vx":-1.14949, "vy":-0.00782, "omega":0.17911, "ax":-6.34332, "ay":0.59602, "alpha":-0.19683, "fx":[-103.6177,-103.81235,-103.78876,-103.58603], "fy":[10.69328,8.59079,8.77345,10.91752]}, + {"t":1.26695, "x":2.00264, "y":4.02473, "heading":-3.13307, "vx":-1.29926, "vy":0.00626, "omega":0.17446, "ax":-6.3387, "ay":0.58594, "alpha":-0.30497, "fx":[-103.50245,-103.79502,-103.75862,-103.44661], "fy":[11.03577,7.79893,8.07253,11.40889]}, + {"t":1.29056, "x":1.9702, "y":4.02504, "heading":-3.12895, "vx":-1.44893, "vy":0.02009, "omega":0.16726, "ax":-6.33136, "ay":0.57079, "alpha":-0.46754, "fx":[-103.33043,-103.75855,-103.70346,-103.22998], "fy":[11.53738,6.62227,7.00889,12.15703]}, + {"t":1.31417, "x":1.93422, "y":4.02568, "heading":-3.125, "vx":-1.59841, "vy":0.03357, "omega":0.15622, "ax":-6.318, "ay":0.54547, "alpha":-0.73969, "fx":[-103.04567,-103.67069,-103.58405,-102.84863], "fy":[12.3475,4.69564,5.19735,13.42907]}, + {"t":1.33779, "x":1.89472, "y":4.02662, "heading":-3.12131, "vx":-1.74759, "vy":0.04645, "omega":0.13876, "ax":-6.28701, "ay":0.49457, "alpha":-1.28993, "fx":[-102.4831,-103.40156,-103.23243,-102.00565], "fy":[13.88667,0.98168,1.414,16.05855]}, + {"t":1.3614, "x":1.85171, "y":4.02786, "heading":-3.11804, "vx":-1.89603, "vy":0.05812, "omega":0.1083, "ax":-6.15711, "ay":0.34034, "alpha":-3.01694, "fx":[-100.85972,-102.00983,-101.02889,-98.72977], "fy":[17.96481,-9.03166,-11.27696,24.59949]}, + {"t":1.38501, "x":1.80522, "y":4.02932, "heading":-3.11548, "vx":-2.0414, "vy":0.06616, "omega":0.03707, "ax":0.16103, "ay":-0.62745, "alpha":-23.96208, "fx":[-74.192,-53.5957,62.73079,75.58684], "fy":[55.62923,-79.21608,-72.74262,55.29874]}, + {"t":1.40862, "x":1.75707, "y":4.03071, "heading":-3.1146, "vx":-2.0376, "vy":0.05134, "omega":-0.52869, "ax":6.07287, "ay":-0.88295, "alpha":-3.63047, "fx":[101.50877,94.26545,98.90885,102.43639], "fy":[4.25846,-38.56098,-27.26035,3.82452]}, + {"t":1.43223, "x":1.71065, "y":4.03168, "heading":-3.12709, "vx":-1.89421, "vy":0.0305, "omega":-0.61441, "ax":6.24082, "ay":-0.774, "alpha":-1.96634, "fx":[103.17388,100.10138,101.38786,103.43876], "fy":[-2.66872,-25.31922,-20.7118,-1.91378]}, + {"t":1.45584, "x":1.66767, "y":4.03218, "heading":3.14159, "vx":-1.74686, "vy":0.01222, "omega":-0.66084, "ax":6.26666, "ay":-0.65809, "alpha":2.68746, "fx":[101.79209,104.04901,103.86737,100.08361], "fy":[-21.81125,2.99499,3.92326,-28.14104]}, + {"t":1.48943, "x":1.61252, "y":4.03222, "heading":3.11939, "vx":-1.53633, "vy":-0.00989, "omega":-0.57055, "ax":6.14578, "ay":-0.54097, "alpha":4.92427, "fx":[100.60953,103.55047,101.99589,95.73146], "fy":[-27.39213,12.19047,20.73578,-40.90961]}, + {"t":1.52303, "x":1.56437, "y":4.03158, "heading":3.10022, "vx":-1.32986, "vy":-0.02806, "omega":-0.40512, "ax":6.03479, "ay":-0.47644, "alpha":6.28344, "fx":[99.83532,103.09383,99.35504,92.34478], "fy":[-30.36762,16.13459,31.33847,-48.26076]}, + {"t":1.55663, "x":1.5231, "y":4.03037, "heading":3.08661, "vx":-1.12712, "vy":-0.04407, "omega":-0.19402, "ax":5.94597, "ay":-0.43863, "alpha":7.19145, "fx":[99.29796,102.78828,96.95899,89.77576], "fy":[-32.23253,18.25727,38.29824,-53.00634]}, + {"t":1.59022, "x":1.48859, "y":4.02864, "heading":3.08009, "vx":-0.92736, "vy":-0.0588, "omega":0.04758, "ax":5.87616, "ay":-0.41372, "alpha":7.83602, "fx":[98.94025,102.55915,94.99777,87.7588], "fy":[-33.41042,19.6709,43.05148,-56.3658]}, + {"t":1.62382, "x":1.46075, "y":4.02643, "heading":3.08169, "vx":-0.72995, "vy":-0.0727, "omega":0.31083, "ax":5.82088, "ay":-0.39353, "alpha":8.3154, "fx":[98.74303,102.3484,93.44787,86.10204], "fy":[-34.05589,20.84976,46.39975,-58.92755]}, + {"t":1.65741, "x":1.43951, "y":4.02377, "heading":3.09214, "vx":-0.53439, "vy":-0.08592, "omega":0.59019, "ax":5.77674, "ay":-0.37306, "alpha":8.68358, "fx":[98.69967,102.11689,92.24967,84.68852], "fy":[-34.2314,22.03203,48.79476,-60.99064]}, + {"t":1.69101, "x":1.42482, "y":4.02067, "heading":3.11196, "vx":-0.34032, "vy":-0.09846, "omega":0.88192, "ax":5.7417, "ay":-0.34911, "alpha":8.9686, "fx":[98.80671,101.83625,91.35928,83.46098], "fy":[-33.96066,23.34957,50.48244,-62.70045]}, + {"t":1.7246, "x":1.41663, "y":4.01717, "heading":3.14159, "vx":-0.14742, "vy":-0.11018, "omega":1.18323, "ax":5.84895, "ay":-0.34164, "alpha":8.11487, "fx":[99.58595,101.95077,94.31104,86.62908], "fy":[-31.54245,22.75429,44.64852,-58.20132]}, + {"t":1.75274, "x":1.41479, "y":4.01393, "heading":-3.1083, "vx":0.01715, "vy":-0.1198, "omega":1.41155, "ax":5.85595, "ay":-0.32389, "alpha":8.05202, "fx":[99.93674,101.70968,94.72387,86.56412], "fy":[-30.3925,23.77571,43.72234,-58.28518]}, + {"t":1.78088, "x":1.41759, "y":4.01043, "heading":-3.06858, "vx":0.18191, "vy":-0.12891, "omega":1.63811, "ax":5.8658, "ay":-0.30419, "alpha":7.95882, "fx":[100.33878,101.41521,95.2763,86.54847], "fy":[-29.01416,24.96568,42.45196,-58.29544]}, + {"t":1.80901, "x":1.42503, "y":4.00669, "heading":-3.02249, "vx":0.34695, "vy":-0.13747, "omega":1.86204, "ax":5.87926, "ay":-0.2836, "alpha":7.82603, "fx":[100.78322,101.06565,95.99512,86.61506], "fy":[-27.40259,26.30506,40.73493,-58.1829]}, + {"t":1.83715, "x":1.43712, "y":4.00271, "heading":-2.9701, "vx":0.51237, "vy":-0.14545, "omega":2.08224, "ax":5.89698, "ay":-0.26359, "alpha":7.64519, "fx":[101.25899,100.661,96.90156,86.79597], "fy":[-25.55452,27.76895,38.44685,-57.89804]}, + {"t":1.86529, "x":1.45387, "y":3.99851, "heading":-2.91152, "vx":0.67829, "vy":-0.15287, "omega":2.29734, "ax":5.91931, "ay":-0.24608, "alpha":7.41059, "fx":[101.75264,100.20402,98.00128,87.11995], "fy":[-23.46932,29.32541,35.44528,-57.39305]}, + {"t":1.89342, "x":1.4753, "y":3.99411, "heading":-2.84688, "vx":0.84484, "vy":-0.15979, "omega":2.50585, "ax":5.9461, "ay":-0.23331, "alpha":7.12195, "fx":[102.24837,99.70158,99.26953,87.60987], "fy":[-21.15016,30.93404,31.58259,-56.62341]}, + {"t":1.92156, "x":1.50143, "y":3.98952, "heading":-2.77637, "vx":1.01214, "vy":-0.16635, "omega":2.70624, "ax":5.97637, "ay":-0.22748, "alpha":6.78761, "fx":[102.72837,99.16623,100.63353,88.28088], "fy":[-18.60505,32.54426,26.73415,-55.54863]}, + {"t":1.94969, "x":1.53227, "y":3.98475, "heading":-2.70023, "vx":1.1803, "vy":-0.17275, "omega":2.89722, "ax":6.0082, "ay":-0.22998, "alpha":6.42707, "fx":[103.17335,98.61807,101.95961,89.13943], "fy":[-15.84776,34.09292,20.84768,-54.13179]}, + {"t":1.97783, "x":1.56786, "y":3.9798, "heading":-2.61871, "vx":1.34935, "vy":-0.17922, "omega":3.07805, "ax":6.03884, "ay":-0.24045, "alpha":6.0703, "fx":[103.56332,98.08694,103.06011,90.18351], "fy":[-12.89859,35.50057,14.01139,-52.33728]}, + {"t":2.00597, "x":1.60821, "y":3.97466, "heading":-2.53211, "vx":1.51926, "vy":-0.18599, "omega":3.24885, "ax":6.06547, "ay":-0.25578, "alpha":5.75089, "fx":[103.87854,97.61539,103.73736,91.40433], "fy":[-9.78515,36.66481,6.52035,-50.12594]}, + {"t":2.0341, "x":1.65336, "y":3.96933, "heading":-2.4407, "vx":1.68992, "vy":-0.19319, "omega":3.41066, "ax":6.08661, "ay":-0.26977, "alpha":5.49156, "fx":[104.1005,97.26258,103.86554,92.78911], "fy":[-6.54403,37.44799,-1.09906,-47.44604]}, + {"t":2.06224, "x":1.70332, "y":3.96379, "heading":-2.34473, "vx":1.86117, "vy":-0.20078, "omega":3.56517, "ax":6.1035, "ay":-0.27413, "alpha":5.28583, "fx":[104.21278,97.10963,103.4755,94.32445], "fy":[-3.22523,37.65441,-8.13772,-44.2173]}, + {"t":2.09038, "x":1.7581, "y":3.95803, "heading":-2.24442, "vx":2.0329, "vy":-0.20849, "omega":3.71389, "ax":6.12081, "ay":-0.2602, "alpha":5.0831, "fx":[104.20223,97.26558,102.78657,95.99985], "fy":[0.09645,36.98758,-13.7993,-40.30002]}, + {"t":2.11851, "x":1.81772, "y":3.95206, "heading":-2.13993, "vx":2.20512, "vy":-0.21581, "omega":3.85691, "ax":6.14593, "ay":-0.22039, "alpha":4.78005, "fx":[104.06077,97.8687,102.1573,97.81044], "fy":[3.29282,34.96852,-17.24695,-35.42622]}, + {"t":2.14665, "x":1.8822, "y":3.9459, "heading":-2.03141, "vx":2.37805, "vy":-0.22201, "omega":3.99141, "ax":6.18693, "ay":-0.14688, "alpha":4.20739, "fx":[103.78899,99.0617,101.97371,99.75373], "fy":[6.10371,30.7736,-17.46935,-29.01278]}, + {"t":2.17479, "x":1.95156, "y":3.93959, "heading":-1.9191, "vx":2.55212, "vy":-0.22615, "omega":4.10979, "ax":6.24656, "ay":-0.02315, "alpha":3.07665, "fx":[103.39986,100.85667,102.44938,101.77166], "fy":[7.86962,22.90384,-12.77133,-19.51588]}, + {"t":2.20292, "x":2.02584, "y":3.93322, "heading":-1.80347, "vx":2.72788, "vy":-0.2268, "omega":4.19635, "ax":6.28969, "ay":0.21549, "alpha":0.72674, "fx":[102.853,102.56343,102.8809,103.00072], "fy":[6.16031,8.55121,0.62284,-1.24292]}, + {"t":2.23106, "x":2.10508, "y":3.92693, "heading":-1.6854, "vx":2.90485, "vy":-0.22073, "omega":4.2168, "ax":5.74498, "ay":0.80567, "alpha":-6.76842, "fx":[96.96588,100.63435,97.33981,80.73813], "fy":[-22.06121,-16.99721,31.40199,60.34103]}, + {"t":2.25919, "x":2.18909, "y":3.92103, "heading":-1.56675, "vx":3.06649, "vy":-0.19806, "omega":4.02636, "ax":3.85197, "ay":0.5714, "alpha":-16.28546, "fx":[43.40187,96.193,87.35228,24.94228], "fy":[-78.05493,-26.09701,48.91593,92.60089]}, + {"t":2.28733, "x":2.27689, "y":3.91569, "heading":-1.45346, "vx":3.17487, "vy":-0.18199, "omega":3.56815, "ax":-3.37927, "ay":0.4614, "alpha":3.56819, "fx":[-48.03291,-62.64727,-62.42794,-47.87048], "fy":[23.14719,16.59389,-5.8208,-3.74803]}, + {"t":2.31547, "x":2.36488, "y":3.91075, "heading":-1.35307, "vx":3.07979, "vy":-0.16901, "omega":3.66854, "ax":-5.40396, "ay":-0.53171, "alpha":8.4829, "fx":[-84.86835,-99.43312,-92.19567,-76.88095], "fy":[46.77274,14.0839,-39.30457,-56.32211]}, + {"t":2.3436, "x":2.4494, "y":3.90578, "heading":-1.24985, "vx":2.92774, "vy":-0.18397, "omega":3.90722, "ax":-6.19669, "ay":-0.20555, "alpha":1.93814, "fx":[-101.19688,-102.18148,-100.93308,-100.90468], "fy":[10.49715,2.79108,-15.43374,-11.2958]}, + {"t":2.37174, "x":2.52932, "y":3.90053, "heading":-1.13991, "vx":2.75339, "vy":-0.18975, "omega":3.96175, "ax":-6.21522, "ay":0.09789, "alpha":-3.20587, "fx":[-101.39625,-102.35439,-99.59851,-103.07893], "fy":[-19.60826,-8.6776,26.46207,8.22517]}, + {"t":2.39988, "x":2.60433, "y":3.89523, "heading":-1.02845, "vx":2.57852, "vy":-0.187, "omega":3.87155, "ax":-6.10736, "ay":0.18993, "alpha":-4.96891, "fx":[-99.04277,-102.60906,-94.2778,-103.44489], "fy":[-30.79522,-9.02289,42.91148,9.32661]}, + {"t":2.42801, "x":2.67446, "y":3.89004, "heading":-0.91951, "vx":2.40668, "vy":-0.18165, "omega":3.73175, "ax":-6.04357, "ay":0.25138, "alpha":-5.68773, "fx":[-96.84219,-103.16534,-91.38059,-103.81529], "fy":[-37.70635,-2.46118,49.38608,7.21963]}, + {"t":2.45615, "x":2.73979, "y":3.88503, "heading":-0.81452, "vx":2.23663, "vy":-0.17458, "omega":3.57171, "ax":-5.9975, "ay":0.33053, "alpha":-6.12659, "fx":[-94.82038,-102.99433,-90.27874,-104.09709], "fy":[-42.81544,8.59929,51.71806,4.11234]}, + {"t":2.48429, "x":2.80034, "y":3.88025, "heading":-0.71402, "vx":2.06789, "vy":-0.16528, "omega":3.39933, "ax":-5.94314, "ay":0.42188, "alpha":-6.61688, "fx":[-92.97124,-101.19705,-90.22083,-104.24721], "fy":[-46.85597,21.74233,52.04059,0.66053]}, + {"t":2.51242, "x":2.85617, "y":3.87576, "heading":-0.61838, "vx":1.90067, "vy":-0.15341, "omega":3.21316, "ax":-5.87319, "ay":0.50157, "alpha":-7.24226, "fx":[-91.31032,-97.74815,-90.74499,-104.25868], "fy":[-50.1172,34.50274,51.27543,-2.86227]}, + {"t":2.54056, "x":2.90733, "y":3.87165, "heading":-0.52797, "vx":1.73542, "vy":-0.1393, "omega":3.00939, "ax":-5.79613, "ay":0.55114, "alpha":-7.93524, "fx":[-89.86222,-93.44367,-91.57481,-104.14213], "fy":[-52.73796,45.19792,49.89228,-6.31177]}, + {"t":2.5687, "x":2.95386, "y":3.86795, "heading":-0.4433, "vx":1.57233, "vy":-0.12379, "omega":2.78612, "ax":-5.72418, "ay":0.56696, "alpha":-8.59245, "fx":[-88.6496,-89.21166,-92.53912,-103.91757], "fy":[-54.80001,53.30422,48.16814,-9.59773]}, + {"t":2.59683, "x":2.99584, "y":3.86469, "heading":-0.3649, "vx":1.41128, "vy":-0.10784, "omega":2.54436, "ax":-5.66491, "ay":0.55578, "alpha":-9.14615, "fx":[-87.6855,-85.61689,-93.52963,-103.61019], "fy":[-56.36538,59.08261,46.2857,-12.65886]}, + {"t":2.62497, "x":3.0333, "y":3.86187, "heading":-0.29332, "vx":1.25189, "vy":-0.0922, "omega":2.28702, "ax":-5.62029, "ay":0.52743, "alpha":-9.57403, "fx":[-86.96853,-82.83002,-94.47865,-103.24718], "fy":[-57.4929,63.06285,44.374,-15.45393]}, + {"t":2.6531, "x":3.0663, "y":3.85949, "heading":-0.22897, "vx":1.09375, "vy":-0.07736, "omega":2.01764, "ax":-5.58894, "ay":0.49056, "alpha":-9.883, "fx":[-86.48127,-80.79165,-95.34573,-102.85533], "fy":[-58.24556,65.75451,42.52714,-17.9571]}, + {"t":2.68124, "x":3.09486, "y":3.85751, "heading":-0.1722, "vx":0.9365, "vy":-0.06356, "omega":1.73957, "ax":-5.56816, "ay":0.45151, "alpha":-10.09321, "fx":[-86.19146,-79.35558,-96.10874,-102.45923], "fy":[-58.69276,67.55784,40.81445,-20.15438]}, + {"t":2.70938, "x":3.11901, "y":3.8559, "heading":-0.12325, "vx":0.77983, "vy":-0.05085, "omega":1.45558, "ax":-5.55507, "ay":0.41443, "alpha":-10.22852, "fx":[-86.05527,-78.3659,-96.75752,-102.08036], "fy":[-58.90977,68.76351,39.28696,-22.04024]}, + {"t":2.73751, "x":3.13875, "y":3.85463, "heading":-0.0823, "vx":0.62353, "vy":-0.03919, "omega":1.16779, "ax":-5.54706, "ay":0.38183, "alpha":-10.31186, "fx":[-86.0221,-77.68765,-97.28919,-101.73663], "fy":[-58.97507,69.57626,37.98189,-23.61462]}, + {"t":2.76565, "x":3.1541, "y":3.85368, "heading":-0.04944, "vx":0.46746, "vy":-0.02845, "omega":0.87765, "ax":-5.54196, "ay":0.35506, "alpha":-10.36319, "fx":[-86.03993,-77.21466,-97.70491,-101.44248], "fy":[-58.96605,70.13854,36.92618,-24.88039]}, + {"t":2.79379, "x":3.16506, "y":3.85302, "heading":-0.02475, "vx":0.31153, "vy":-0.01846, "omega":0.58607, "ax":-5.53804, "ay":0.33478, "alpha":-10.39874, "fx":[-86.06049,-76.86821,-98.00746,-101.20918], "fy":[-58.95363,70.5483,36.13904,-25.84133]}, + {"t":2.82192, "x":3.17163, "y":3.85263, "heading":-0.00826, "vx":0.15571, "vy":-0.00904, "omega":0.29348, "ax":-5.53401, "ay":0.32127, "alpha":-10.43073, "fx":[-86.04393,-76.593,-98.19971,-101.04519], "fy":[-58.99629,70.87131,35.63399,-26.50058]}, + {"t":2.85006, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/BtoB.traj b/src/main/deploy/choreo/BtoB.traj new file mode 100644 index 00000000..cc888d85 --- /dev/null +++ b/src/main/deploy/choreo/BtoB.traj @@ -0,0 +1,143 @@ +{ + "name":"BtoB", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.178781509399414, "y":3.3376660346984863, "heading":-1.377584993861348, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.299587607383728, "y":2.5006520748138428, "heading":-1.2021004738085692, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.178781509399414 m", "val":1.178781509399414}, "y":{"exp":"3.3376660346984863 m", "val":3.3376660346984863}, "heading":{"exp":"-1.377584993861348 rad", "val":-1.377584993861348}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.299587607383728 m", "val":1.299587607383728}, "y":{"exp":"2.5006520748138428 m", "val":2.5006520748138428}, "heading":{"exp":"-1.2021004738085692 rad", "val":-1.2021004738085692}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.10213,1.62735,2.95102], + "samples":[ + {"t":0.0, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.30779, "ay":0.91263, "alpha":-1.31429, "fx":[-104.08674,-101.79162,-102.41701,-104.18618], "fy":[8.44915,23.33635,20.50695,7.38645]}, + {"t":0.03149, "x":3.17069, "y":3.85296, "heading":0.0, "vx":-0.19863, "vy":0.02874, "omega":-0.04139, "ax":-6.31086, "ay":0.88074, "alpha":-1.35419, "fx":[-104.13719,-101.84541,-102.47352,-104.22594], "fy":[7.67273,23.05777,20.18257,6.68049]}, + {"t":0.06298, "x":3.16131, "y":3.8543, "heading":-0.0013, "vx":-0.39735, "vy":0.05647, "omega":-0.08403, "ax":-6.31411, "ay":0.84453, "alpha":-1.39955, "fx":[-104.18838,-101.90744,-102.53413,-104.26466], "fy":[6.77623,22.73267,19.82616,5.89109]}, + {"t":0.09447, "x":3.14567, "y":3.85649, "heading":-0.00395, "vx":-0.59618, "vy":0.08307, "omega":-0.1281, "ax":-6.31753, "ay":0.80309, "alpha":-1.45156, "fx":[-104.23836,-101.97915,-102.59974,-104.30108], "fy":[5.73399,22.35089,19.43013,5.00064]}, + {"t":0.12596, "x":3.12376, "y":3.85951, "heading":-0.00798, "vx":-0.79512, "vy":0.10835, "omega":-0.17381, "ax":-6.32109, "ay":0.75517, "alpha":-1.51178, "fx":[-104.28383,-102.06239,-102.67141,-104.33323], "fy":[4.51228,21.89888,18.98457,3.9865]}, + {"t":0.15745, "x":3.09559, "y":3.86329, "heading":-0.01346, "vx":-0.99416, "vy":0.13213, "omega":-0.22141, "ax":-6.3247, "ay":0.69916, "alpha":-1.58226, "fx":[-104.31933,-102.1595,-102.75049,-104.35797], "fy":[3.06595,21.35822,18.47629,2.81896]}, + {"t":0.18894, "x":3.06115, "y":3.8678, "heading":-0.02043, "vx":-1.19333, "vy":0.15415, "omega":-0.27124, "ax":-6.32823, "ay":0.63283, "alpha":-1.66582, "fx":[-104.33567,-102.27348,-102.83865,-104.37026], "fy":[1.33327,20.70312,17.88744,1.45834]}, + {"t":0.22043, "x":3.02043, "y":3.87297, "heading":-0.02897, "vx":-1.3926, "vy":0.17408, "omega":-0.32369, "ax":-6.3314, "ay":0.55309, "alpha":-1.76639, "fx":[-104.31701,-102.40819,-102.93792,-104.36188], "fy":[-0.77235,19.89656,17.19314,-0.14955]}, + {"t":0.25192, "x":2.97344, "y":3.87873, "heading":-0.03916, "vx":-1.59197, "vy":0.19149, "omega":-0.37932, "ax":-6.33367, "ay":0.45548, "alpha":-1.8896, "fx":[-104.23543,-102.5685,-103.05082,-104.31904], "fy":[-3.37572,18.88335,16.35782,-2.08058]}, + {"t":0.2834, "x":2.92017, "y":3.88498, "heading":-0.05111, "vx":-1.79141, "vy":0.20584, "omega":-0.43882, "ax":-6.33404, "ay":0.33337, "alpha":-2.04381, "fx":[-104.03968,-102.7604,-103.18026,-104.21791], "fy":[-6.66274,17.57764,15.32869,-4.44401]}, + {"t":0.31489, "x":2.86062, "y":3.89163, "heading":-0.06493, "vx":-1.99087, "vy":0.21633, "omega":-0.50318, "ax":-6.3305, "ay":0.17647, "alpha":-2.24187, "fx":[-103.63123,-102.99029,-103.32925,-104.01557], "fy":[-10.92026,15.83838,14.02414,-7.40253]}, + {"t":0.34638, "x":2.79479, "y":3.89853, "heading":-0.08077, "vx":-2.19021, "vy":0.22189, "omega":-0.57377, "ax":-6.31879, "ay":-0.03193, "alpha":-2.50421, "fx":[-102.80902,-103.26148,-103.49958,-103.63079], "fy":[-16.60833,13.41716,12.31095,-11.20766]}, + {"t":0.37787, "x":2.72269, "y":3.9055, "heading":-0.09884, "vx":-2.38919, "vy":0.22089, "omega":-0.65263, "ax":-6.28939, "ay":-0.32061, "alpha":-2.86444, "fx":[-101.13244,-103.55896,-103.68716,-102.89991], "fy":[-24.49131,9.83436,9.9565,-16.26515]}, + {"t":0.40936, "x":2.64434, "y":3.9123, "heading":-0.11939, "vx":-2.58724, "vy":0.21079, "omega":-0.74283, "ax":-6.21886, "ay":-0.74256, "alpha":-3.37709, "fx":[-97.55425,-103.77959,-103.86525,-101.46668], "fy":[-35.85392,4.03921,6.51637,-23.25951]}, + {"t":0.44085, "x":2.55978, "y":3.91857, "heading":-0.14278, "vx":-2.78307, "vy":0.18741, "omega":-0.84917, "ax":-6.04262, "ay":-1.40283, "alpha":-4.11391, "fx":[-89.40855,-103.33727,-103.9148,-98.48042], "fy":[-52.65486,-6.72413,1.03045,-33.38588]}, + {"t":0.47234, "x":2.46915, "y":3.92377, "heading":-0.16952, "vx":-2.97334, "vy":0.14323, "omega":-0.97872, "ax":-5.55178, "ay":-2.52948, "alpha":-5.06122, "fx":[-70.17272,-97.90626,-103.2632,-91.70212], "fy":[-76.13118,-31.63585,-8.96747,-48.67423]}, + {"t":0.50383, "x":2.37277, "y":3.92703, "heading":-0.20034, "vx":-3.14817, "vy":0.06358, "omega":-1.13809, "ax":-3.80488, "ay":-4.47666, "alpha":-6.84919, "fx":[-30.80241,-44.82054,-98.15321,-75.03383], "fy":[-98.67879,-91.19504,-31.45282,-71.41274]}, + {"t":0.53532, "x":2.27175, "y":3.92681, "heading":-0.23618, "vx":-3.26798, "vy":-0.07739, "omega":-1.35377, "ax":-0.23156, "ay":-5.6989, "alpha":-8.40403, "fx":[18.15155,55.16845,-51.66103,-36.80147], "fy":[-101.852,-86.47431,-87.74703,-96.59152]}, + {"t":0.56681, "x":2.16873, "y":3.92155, "heading":-0.27881, "vx":-3.27527, "vy":-0.25684, "omega":-1.61841, "ax":3.14052, "ay":-5.23571, "alpha":-4.84015, "fx":[52.07439,81.18042,54.83517,17.27581], "fy":[-89.65312,-64.08424,-86.68113,-101.95735]}, + {"t":0.5983, "x":2.06715, "y":3.91086, "heading":-0.32977, "vx":-3.17638, "vy":-0.42171, "omega":-1.77082, "ax":4.55278, "ay":-4.26874, "alpha":-3.42102, "fx":[70.21324,88.97405,83.18674,55.34284], "fy":[-76.54952,-53.44948,-61.49854,-87.64546]}, + {"t":0.62979, "x":1.96938, "y":3.89547, "heading":-0.38553, "vx":-3.03301, "vy":-0.55613, "omega":-1.87854, "ax":5.16651, "ay":-3.61733, "alpha":-2.40513, "fx":[80.03142,92.4196,91.02372,74.37568], "fy":[-66.43437,-47.66467,-49.93615,-72.51056]}, + {"t":0.66128, "x":1.87644, "y":3.87616, "heading":-0.44468, "vx":-2.87032, "vy":-0.67004, "omega":-1.95428, "ax":5.48, "ay":-3.1864, "alpha":-1.74616, "fx":[85.80171,94.32059,94.30314,83.92491], "fy":[-58.96583,-44.06039,-43.88081,-61.45943]}, + {"t":0.69277, "x":1.78877, "y":3.85348, "heading":-0.50622, "vx":-2.69776, "vy":-0.77038, "omega":-2.00927, "ax":5.66154, "ay":-2.88929, "alpha":-1.30788, "fx":[89.4803,95.52339,96.02598,89.19221], "fy":[-53.35687,-41.58482,-40.26764,-53.72824]}, + {"t":0.72426, "x":1.70663, "y":3.82779, "heading":-0.56949, "vx":-2.51948, "vy":-0.86136, "omega":-2.05045, "ax":5.77711, "ay":-2.67475, "alpha":-1.00039, "fx":[91.98667,96.35904,97.05944,92.37358], "fy":[-49.02278,-39.75701,-37.91817,-48.21016]}, + {"t":0.75575, "x":1.63015, "y":3.79934, "heading":-0.63406, "vx":-2.33757, "vy":-0.94559, "omega":-2.08195, "ax":5.85602, "ay":-2.5135, "alpha":-0.77403, "fx":[93.78603,96.98084,97.73433,94.43792], "fy":[-45.58023,-38.32947,-36.29855,-44.15578]}, + {"t":0.78724, "x":1.55945, "y":3.76832, "heading":-0.69962, "vx":-2.15316, "vy":-1.02473, "omega":-2.10633, "ax":5.91284, "ay":-2.38828, "alpha":-0.60084, "fx":[95.13182,97.46865,98.20174,95.85267], "fy":[-42.78022,-37.16309,-35.13408,-41.09784]}, + {"t":0.81872, "x":1.49458, "y":3.73487, "heading":-0.76595, "vx":-1.96697, "vy":-1.09994, "omega":-2.12525, "ax":5.95547, "ay":-2.28839, "alpha":-0.46424, "fx":[96.17135,97.86774,98.53984,96.86336], "fy":[-40.45781,-36.17424,-34.26931,-38.74196]}, + {"t":0.85021, "x":1.43559, "y":3.6991, "heading":-0.83287, "vx":-1.77944, "vy":-1.172, "omega":-2.13986, "ax":5.9885, "ay":-2.20695, "alpha":-0.35388, "fx":[96.99507,98.20548,98.79297,97.60865], "fy":[-38.50087,-35.31008,-33.60966,-36.89683]}, + {"t":0.8817, "x":1.38253, "y":3.6611, "heading":-0.90025, "vx":-1.59086, "vy":-1.24149, "omega":-2.15101, "ax":6.01477, "ay":-2.13932, "alpha":-0.26297, "fx":[97.66113,98.49923,98.98812,98.17168], "fy":[-36.83111,-34.53591,-33.0942,-35.43378]}, + {"t":0.91319, "x":1.33541, "y":3.62094, "heading":-0.96799, "vx":-1.40146, "vy":-1.30886, "omega":-2.15929, "ax":6.03612, "ay":-2.08229, "alpha":-0.1869, "fx":[98.20844,98.76039,99.14267,98.6049], "fy":[-35.39243,-33.82828,-32.68185,-34.26338]}, + {"t":0.94468, "x":1.29427, "y":3.5787, "heading":-1.03598, "vx":-1.21139, "vy":-1.37443, "omega":-2.16517, "ax":6.05379, "ay":-2.03357, "alpha":-0.12242, "fx":[98.66393,98.99668,99.2683,98.94282], "fy":[-34.14369,-33.17106,-32.34386,-33.32161]}, + {"t":0.97617, "x":1.25913, "y":3.53441, "heading":-1.10416, "vx":-1.02076, "vy":-1.43847, "omega":-2.16903, "ax":6.06863, "ay":-1.99148, "alpha":-0.06714, "fx":[99.04678,99.21344,99.37317,99.20899], "fy":[-33.05404,-32.55297,-32.05951,-32.56138]}, + {"t":1.00766, "x":1.23, "y":3.48812, "heading":-1.17246, "vx":-0.82966, "vy":-1.50118, "omega":-2.17114, "ax":6.08127, "ay":-1.95476, "alpha":-0.0193, "fx":[99.37104,99.41439,99.46314,99.42002], "fy":[-32.09987,-31.96615,-31.8135,-31.94722]}, + {"t":1.03915, "x":1.20689, "y":3.43988, "heading":-1.24083, "vx":-0.63816, "vy":-1.56273, "omega":-2.17175, "ax":6.09214, "ay":-1.92246, "alpha":0.02245, "fx":[99.64722,99.60216,99.54252,99.58791], "fy":[-31.26273,-31.4051,-31.59427,-31.45182]}, + {"t":1.07064, "x":1.18981, "y":3.38972, "heading":-1.30922, "vx":-0.44632, "vy":-1.62327, "omega":-2.17104, "ax":6.1016, "ay":-1.89381, "alpha":0.05916, "fx":[99.88335,99.77863,99.61457,99.72159], "fy":[-30.52796,-30.86607,-31.393,-31.05374]}, + {"t":1.10213, "x":1.17878, "y":3.33767, "heading":-1.37758, "vx":-0.25419, "vy":-1.6829, "omega":-2.16918, "ax":1.91547, "ay":-0.54898, "alpha":13.57729, "fx":[63.68219,-13.25648,6.93328,67.89787], "fy":[31.3742,29.17201,-62.516,-33.92917]}, + {"t":1.11907, "x":1.17475, "y":3.30907, "heading":-1.41434, "vx":-0.22174, "vy":-1.69221, "omega":-1.93915, "ax":1.93968, "ay":-0.47168, "alpha":13.25873, "fx":[63.62646,-9.69559,6.13388,66.77536], "fy":[30.51356,31.9512,-59.72259,-33.58671]}, + {"t":1.13602, "x":1.17127, "y":3.28034, "heading":-1.44719, "vx":-0.18887, "vy":-1.7002, "omega":-1.71451, "ax":1.95783, "ay":-0.3891, "alpha":12.93195, "fx":[63.31641,-6.41831,5.4743,65.65507], "fy":[29.915,34.32946,-56.69552,-32.99302]}, + {"t":1.15296, "x":1.16835, "y":3.25147, "heading":-1.47624, "vx":-0.1557, "vy":-1.70679, "omega":-1.4954, "ax":1.97301, "ay":-0.30238, "alpha":12.59978, "fx":[62.89593,-3.72393,5.18445,64.66352], "fy":[29.50821,36.22782,-53.43577,-32.0735]}, + {"t":1.1699, "x":1.166, "y":3.22251, "heading":-1.50158, "vx":-0.12227, "vy":-1.71191, "omega":-1.28193, "ax":1.98471, "ay":-0.21174, "alpha":12.24499, "fx":[62.27827,-1.16285,4.998,63.6718], "fy":[29.3041,37.7457,-49.97418,-30.92204]}, + {"t":1.18684, "x":1.16421, "y":3.19348, "heading":-1.5233, "vx":-0.08865, "vy":-1.7155, "omega":-1.07447, "ax":1.99245, "ay":-0.11754, "alpha":11.88407, "fx":[61.55879,0.79515,5.16446,62.77262], "fy":[29.25738,38.90917,-46.33747,-29.51516]}, + {"t":1.20379, "x":1.16299, "y":3.1644, "heading":-1.5415, "vx":-0.05489, "vy":-1.71749, "omega":-0.87312, "ax":1.99575, "ay":-0.02021, "alpha":11.50234, "fx":[60.67162,2.67379,5.31952,61.84221], "fy":[29.35214,39.79679,-42.56695,-27.90344]}, + {"t":1.22073, "x":1.16235, "y":3.13529, "heading":-1.55629, "vx":-0.02107, "vy":-1.71783, "omega":-0.67824, "ax":1.99422, "ay":0.07967, "alpha":11.11587, "fx":[59.67892,4.0424,5.72199,60.96335], "fy":[29.57441,40.43024,-38.7037,-26.09095]}, + {"t":1.23767, "x":1.16228, "y":3.1062, "heading":-1.56778, "vx":0.01271, "vy":-1.71648, "omega":-0.4899, "ax":1.98751, "ay":0.18144, "alpha":10.71252, "fx":[58.53586,5.26522,6.15076,60.01594], "fy":[29.89311,40.86757,-34.78888,-24.10719]}, + {"t":1.25461, "x":1.16278, "y":3.07714, "heading":-1.57608, "vx":0.04639, "vy":-1.71341, "omega":-0.3084, "ax":1.97539, "ay":0.28432, "alpha":10.30957, "fx":[57.26718,6.10838,6.71716,59.08258], "fy":[30.334,41.14395,-30.94033,-21.94549]}, + {"t":1.27156, "x":1.16385, "y":3.04816, "heading":-1.58131, "vx":0.07985, "vy":-1.70859, "omega":-0.13373, "ax":1.95775, "ay":0.38749, "alpha":9.89122, "fx":[55.89072,6.84558,7.23191,58.05363], "fy":[30.79712,41.25345,-27.05706,-19.65458]}, + {"t":1.2885, "x":1.16548, "y":3.01926, "heading":-1.58358, "vx":0.11302, "vy":-1.70203, "omega":0.03385, "ax":1.93461, "ay":0.49009, "alpha":9.46858, "fx":[54.36168,7.35801,7.81736,56.97177], "fy":[31.35762,41.22535,-23.27902,-17.25556]}, + {"t":1.30544, "x":1.16768, "y":2.9905, "heading":-1.583, "vx":0.1458, "vy":-1.69372, "omega":0.19428, "ax":1.90613, "ay":0.59127, "alpha":9.04103, "fx":[52.78226,7.66809,8.38396,55.81215], "fy":[31.87956,41.08692,-19.56909,-14.73255]}, + {"t":1.32239, "x":1.17042, "y":2.96189, "heading":-1.57971, "vx":0.1781, "vy":-1.68371, "omega":0.34746, "ax":1.8726, "ay":0.69021, "alpha":8.60287, "fx":[51.02132,7.91455,8.98297,54.53503], "fy":[32.47538,40.82015,-16.01393,-12.14745]}, + {"t":1.33933, "x":1.17371, "y":2.93346, "heading":-1.57382, "vx":0.20982, "vy":-1.67201, "omega":0.49321, "ax":1.83443, "ay":0.78614, "alpha":8.17118, "fx":[49.25932,7.98938,9.50335,53.20548], "fy":[33.0018,40.49413,-12.62693,-9.46129]}, + {"t":1.35627, "x":1.17753, "y":2.90524, "heading":-1.56547, "vx":0.2409, "vy":-1.65869, "omega":0.63165, "ax":1.79211, "ay":0.87842, "alpha":7.71836, "fx":[47.30074,8.10138,10.09515,51.69338], "fy":[33.55434,40.03028,-9.36463,-6.7777]}, + {"t":1.37321, "x":1.18186, "y":2.87727, "heading":-1.55476, "vx":0.27127, "vy":-1.64381, "omega":0.76242, "ax":1.74624, "ay":0.96652, "alpha":7.2817, "fx":[45.4052,8.04993,10.5602,50.17515], "fy":[33.9982,39.53288,-6.28885,-4.03888]}, + {"t":1.39016, "x":1.18671, "y":2.84955, "heading":-1.54185, "vx":0.30085, "vy":-1.62743, "omega":0.8858, "ax":1.69741, "ay":1.05002, "alpha":6.81747, "fx":[43.30472,8.09314,11.17413,48.42559], "fy":[34.43596,38.91702,-3.35721,-1.33228]}, + {"t":1.4071, "x":1.19205, "y":2.82213, "heading":-1.52684, "vx":0.32961, "vy":-1.60964, "omega":1.0013, "ax":1.64626, "ay":1.12865, "alpha":6.37673, "fx":[41.31342,8.06368,11.55906,46.71655], "fy":[34.74966,38.28521,-0.61197,1.38202]}, + {"t":1.42404, "x":1.19787, "y":2.79502, "heading":-1.50987, "vx":0.3575, "vy":-1.59052, "omega":1.10934, "ax":1.59341, "ay":1.20223, "alpha":5.90023, "fx":[39.11913,8.18297,12.15956,44.73489], "fy":[35.01802,37.54452,2.03443,4.01975]}, + {"t":1.44098, "x":1.20416, "y":2.76825, "heading":-1.49108, "vx":0.3845, "vy":-1.57015, "omega":1.20931, "ax":1.53943, "ay":1.27074, "alpha":5.45565, "fx":[37.09747,8.20721,12.5055,42.8567], "fy":[35.14038,36.80917,4.52162,6.62524]}, + {"t":1.45793, "x":1.21089, "y":2.74183, "heading":-1.47059, "vx":0.41058, "vy":-1.54862, "omega":1.30174, "ax":1.48487, "ay":1.3342, "alpha":4.96785, "fx":[34.86548,8.4862,13.08611,40.66138], "fy":[35.20211,35.98809,6.92018,9.13626]}, + {"t":1.47487, "x":1.21806, "y":2.71578, "heading":-1.44854, "vx":0.43574, "vy":-1.52602, "omega":1.38591, "ax":1.4302, "ay":1.39277, "alpha":4.51961, "fx":[32.87112,8.61583,13.39091,38.64631], "fy":[35.0948,35.18526,9.2372,11.55901]}, + {"t":1.49181, "x":1.22565, "y":2.69012, "heading":-1.42505, "vx":0.45997, "vy":-1.50242, "omega":1.46249, "ax":1.37584, "ay":1.44661, "alpha":4.02301, "fx":[30.67259,9.09525,13.93258,36.26905], "fy":[34.92013,34.33394,11.44916,13.8939]}, + {"t":1.50876, "x":1.23364, "y":2.66488, "heading":-1.40028, "vx":0.48328, "vy":-1.47791, "omega":1.53065, "ax":1.32213, "ay":1.49597, "alpha":3.57351, "fx":[28.7375,9.38392,14.19282,34.14321], "fy":[34.59105,33.52077,13.60858,16.10451]}, + {"t":1.5257, "x":1.24202, "y":2.64005, "heading":-1.37434, "vx":0.50568, "vy":-1.45257, "omega":1.59119, "ax":1.26937, "ay":1.5411, "alpha":3.06651, "fx":[26.60545,10.11294,14.68701,31.60199], "fy":[34.17702,32.6799,15.69607,18.22329]}, + {"t":1.54264, "x":1.25077, "y":2.61566, "heading":-1.34738, "vx":0.52719, "vy":-1.42645, "omega":1.64315, "ax":1.21778, "ay":1.58229, "alpha":2.61565, "fx":[24.80849,10.56047,14.86634,29.39848], "fy":[33.58646,31.89952,17.79613,20.1878]}, + {"t":1.55958, "x":1.25988, "y":2.59172, "heading":-1.31954, "vx":0.54782, "vy":-1.39965, "omega":1.68746, "ax":1.16755, "ay":1.61982, "alpha":2.10388, "fx":[22.83451,11.51022,15.24286,26.76104], "fy":[32.91502,31.11256,19.84744,22.04905]}, + {"t":1.57653, "x":1.26933, "y":2.56824, "heading":-1.29095, "vx":0.5676, "vy":-1.3722, "omega":1.72311, "ax":1.11879, "ay":1.65398, "alpha":1.6488, "fx":[21.17161,12.16428,15.36168,24.46246], "fy":[32.0985,30.41832,21.86822,23.77256]}, + {"t":1.59347, "x":1.2791, "y":2.54523, "heading":-1.26176, "vx":0.58656, "vy":-1.34418, "omega":1.75104, "ax":1.0716, "ay":1.68503, "alpha":1.12866, "fx":[19.35726,13.37538,15.65497,21.68665], "fy":[31.20721,29.74429,23.83784,25.39896]}, + {"t":1.61041, "x":1.28919, "y":2.5227, "heading":-1.23209, "vx":0.60472, "vy":-1.31563, "omega":1.77017, "ax":1.02603, "ay":1.71325, "alpha":0.6774, "fx":[17.90246,14.17593,15.64089,19.37535], "fy":[30.16337,29.1787,25.83293,26.8587]}, + {"t":1.62735, "x":1.29959, "y":2.50065, "heading":-1.2021, "vx":0.6221, "vy":-1.2866, "omega":1.78164, "ax":1.00316, "ay":1.7286, "alpha":-8.41315, "fx":[3.48183,49.28221,28.40516,-15.57049], "fy":[-1.128,15.85829,54.08636,44.22083]}, + {"t":1.66412, "x":1.32314, "y":2.45451, "heading":-1.13659, "vx":0.65898, "vy":-1.22305, "omega":1.47231, "ax":3.19748, "ay":5.53358, "alpha":-0.02586, "fx":[52.30115,52.44564,52.24454,52.09963], "fy":[90.44693,90.36348,90.48008,90.56334]}, + {"t":1.70089, "x":1.34953, "y":2.41329, "heading":-1.08246, "vx":0.77655, "vy":-1.01958, "omega":1.47135, "ax":3.18575, "ay":5.53994, "alpha":-0.04218, "fx":[52.14232,52.36061,52.02028,51.80073], "fy":[90.53205,90.40643,90.60296,90.72821]}, + {"t":1.73766, "x":1.38024, "y":2.37954, "heading":-1.02836, "vx":0.89368, "vy":-0.81589, "omega":1.4698, "ax":3.17258, "ay":5.54704, "alpha":-0.06043, "fx":[51.97479,52.26169,51.7578,51.468], "fy":[90.62098,90.45654,90.74619,90.91016]}, + {"t":1.77443, "x":1.41524, "y":2.35329, "heading":-0.97432, "vx":1.01033, "vy":-0.61193, "omega":1.46758, "ax":3.15767, "ay":5.55501, "alpha":-0.081, "fx":[51.79655,52.14529,51.45003,51.09561], "fy":[90.71467,90.51571,90.9132,91.11183]}, + {"t":1.8112, "x":1.45452, "y":2.33455, "heading":-0.92036, "vx":1.12644, "vy":-0.40769, "omega":1.4646, "ax":3.14067, "ay":5.56403, "alpha":-0.10436, "fx":[51.60495,52.00686,51.08804,50.67601], "fy":[90.81434,90.58627,91.10822,91.33659]}, + {"t":1.84796, "x":1.49806, "y":2.32332, "heading":-0.8665, "vx":1.24191, "vy":-0.20311, "omega":1.46077, "ax":3.12111, "ay":5.57432, "alpha":-0.13114, "fx":[51.39643,51.84057,50.66031,50.19925], "fy":[90.92164,90.67119,91.3366,91.58882]}, + {"t":1.88473, "x":1.54584, "y":2.31962, "heading":-0.81279, "vx":1.35667, "vy":0.00185, "omega":1.45594, "ax":3.09835, "ay":5.58616, "alpha":-0.16219, "fx":[51.16617,51.63882,50.15172,49.65202], "fy":[91.03879,90.77431,91.60513,91.87423]}, + {"t":1.9215, "x":1.59781, "y":2.32346, "heading":-0.75926, "vx":1.47059, "vy":0.20725, "omega":1.44998, "ax":3.07157, "ay":5.59993, "alpha":-0.1986, "fx":[50.90752,51.39151,49.54197,49.01608], "fy":[91.16883,90.90064,91.92266,92.20051]}, + {"t":1.95827, "x":1.65396, "y":2.33487, "heading":-0.70595, "vx":1.58353, "vy":0.41315, "omega":1.44268, "ax":3.03957, "ay":5.61613, "alpha":-0.24195, "fx":[50.61113,51.08487,48.80314,48.26581], "fy":[91.31599,91.05688,92.3009,92.57811]}, + {"t":1.99504, "x":1.71424, "y":2.35386, "heading":-0.6529, "vx":1.69529, "vy":0.61964, "omega":1.43378, "ax":3.0007, "ay":5.63546, "alpha":-0.29441, "fx":[50.26347,50.69959,47.89562,47.3641], "fy":[91.48633,91.25224,92.7557,93.02161]}, + {"t":2.03181, "x":1.7786, "y":2.38045, "heading":-0.60019, "vx":1.80562, "vy":0.82685, "omega":1.42296, "ax":2.95246, "ay":5.65891, "alpha":-0.35921, "fx":[49.84431,50.20767,46.7614,46.25523], "fy":[91.68879,91.49972,93.309,93.55189]}, + {"t":2.06857, "x":1.84699, "y":2.41467, "heading":-0.54787, "vx":1.91418, "vy":1.03492, "omega":1.40975, "ax":2.89105, "ay":5.68793, "alpha":-0.44128, "fx":[49.32205,49.56686,45.31187,44.85172], "fy":[91.93702,91.81832,93.99193,94.19978]}, + {"t":2.10534, "x":1.91932, "y":2.45657, "heading":-0.49603, "vx":2.02048, "vy":1.24405, "omega":1.39353, "ax":2.81024, "ay":5.7247, "alpha":-0.54857, "fx":[48.64469,48.71012,43.40486,43.00836], "fy":[92.25291,92.23701,94.84979,95.01227]}, + {"t":2.14211, "x":1.99551, "y":2.50618, "heading":-0.44479, "vx":2.12381, "vy":1.45454, "omega":1.37336, "ax":2.69922, "ay":5.77268, "alpha":-0.69473, "fx":[47.7203,47.5242,40.79762,40.46612], "fy":[92.67379,92.80212,95.94983,96.06349]}, + {"t":2.17888, "x":2.07542, "y":2.56357, "heading":-0.3943, "vx":2.22305, "vy":1.66679, "omega":1.34781, "ax":2.53744, "ay":5.83746, "alpha":-0.9054, "fx":[46.3701,45.80142,37.04129,36.71669], "fy":[93.2686,93.59246,97.39175,97.47286]}, + {"t":2.21565, "x":2.15888, "y":2.6288, "heading":-0.34474, "vx":2.31635, "vy":1.88143, "omega":1.31452, "ax":2.2808, "ay":5.92827, "alpha":-1.23493, "fx":[44.19558,43.11593,31.21388,30.62172], "fy":[94.17897,94.75225,99.30967,99.42262]}, + {"t":2.25242, "x":2.24559, "y":2.70198, "heading":-0.29641, "vx":2.40021, "vy":2.0994, "omega":1.26911, "ax":1.81628, "ay":6.05731, "alpha":-1.82133, "fx":[40.09763,38.44043,21.13511,19.09802], "fy":[95.74167,96.5666,101.76536,102.02815]}, + {"t":2.28919, "x":2.33507, "y":2.78327, "heading":-0.24975, "vx":2.46699, "vy":2.32212, "omega":1.20215, "ax":0.7677, "ay":6.19394, "alpha":-3.11884, "fx":[29.62503,28.56878,0.6662,-8.65835], "fy":[98.88177,99.58924,103.63186,102.93379]}, + {"t":2.32595, "x":2.42629, "y":2.87284, "heading":-0.20554, "vx":2.49522, "vy":2.54986, "omega":1.08747, "ax":-2.45909, "ay":5.40186, "alpha":-5.91716, "fx":[-30.84697,-2.15136,-47.33873,-80.46848], "fy":[95.6113,102.66685,91.55722,63.40516]}, + {"t":2.36272, "x":2.51638, "y":2.97024, "heading":-0.16556, "vx":2.4048, "vy":2.74848, "omega":0.86991, "ax":-5.85942, "ay":-0.7352, "alpha":-6.59554, "fx":[-83.87998,-99.24313,-101.1979,-98.84039], "fy":[-58.5756,20.88873,19.09642,-29.48631]}, + {"t":2.39949, "x":2.60084, "y":3.0708, "heading":-0.13357, "vx":2.18936, "vy":2.72145, "omega":0.6274, "ax":-5.13606, "ay":-3.54484, "alpha":-3.19838, "fx":[-68.57415,-85.95926,-96.67288,-84.65292], "fy":[-77.59211,-57.09699,-37.1593,-59.9572]}, + {"t":2.43626, "x":2.67786, "y":3.16847, "heading":-0.11051, "vx":2.00052, "vy":2.59111, "omega":0.5098, "ax":-4.60133, "ay":-4.32297, "alpha":-2.16642, "fx":[-63.4324,-74.6474,-86.34368,-76.46829], "fy":[-82.3175,-72.08723,-57.79833,-70.48617]}, + {"t":2.47303, "x":2.74831, "y":3.26082, "heading":-0.09176, "vx":1.83133, "vy":2.43216, "omega":0.43015, "ax":-4.30725, "ay":-4.65506, "alpha":-1.65792, "fx":[-60.92755,-69.31525,-79.688,-71.73052], "fy":[-84.40393,-77.55388,-66.95139,-75.49652]}, + {"t":2.5098, "x":2.81273, "y":3.3471, "heading":-0.07595, "vx":1.67296, "vy":2.261, "omega":0.36919, "ax":-4.12643, "ay":-4.83518, "alpha":-1.35991, "fx":[-59.45405,-66.29069,-75.38446,-68.70764], "fy":[-85.57171,-80.32464,-71.91525,-78.37254]}, + {"t":2.54656, "x":2.87146, "y":3.42696, "heading":-0.06237, "vx":1.52124, "vy":2.08322, "omega":0.31919, "ax":-4.00499, "ay":-4.94731, "alpha":-1.16541, "fx":[-58.48639,-64.3519,-72.43025,-66.62725], "fy":[-86.31582,-81.99009,-74.98599,-80.22428]}, + {"t":2.58333, "x":2.92468, "y":3.50021, "heading":-0.05064, "vx":1.37398, "vy":1.90131, "omega":0.27634, "ax":-3.91809, "ay":-5.02354, "alpha":-1.02884, "fx":[-57.80353,-63.00445,-70.29172,-65.11378], "fy":[-86.83045,-83.1,-77.06011,-81.51092]}, + {"t":2.6201, "x":2.97255, "y":3.56673, "heading":-0.04047, "vx":1.22992, "vy":1.71661, "omega":0.23851, "ax":-3.85294, "ay":-5.07864, "alpha":-0.92782, "fx":[-57.29661,-62.01317,-68.67701,-63.96612], "fy":[-87.20702,-83.89262,-78.55041,-82.45436]}, + {"t":2.65687, "x":3.01517, "y":3.62641, "heading":-0.03171, "vx":1.08826, "vy":1.52987, "omega":0.20439, "ax":-3.80232, "ay":-5.12027, "alpha":-0.8501, "fx":[-56.90592,-61.25251,-67.41657,-63.06768], "fy":[-87.49416,-84.48746,-79.67108,-83.17425]}, + {"t":2.69364, "x":3.05261, "y":3.6792, "heading":-0.02419, "vx":0.94845, "vy":1.34161, "omega":0.17313, "ax":-3.76188, "ay":-5.15282, "alpha":-0.78848, "fx":[-56.59596,-60.64949,-66.40612,-62.3465], "fy":[-87.7201,-84.95089,-80.54366,-83.7406]}, + {"t":2.73041, "x":3.08494, "y":3.72505, "heading":-0.01782, "vx":0.81013, "vy":1.15215, "omega":0.14414, "ax":-3.72883, "ay":-5.17895, "alpha":-0.73843, "fx":[-56.34436,-60.15884,-65.57831,-61.75585], "fy":[-87.90232,-85.32272,-81.24195,-84.19698]}, + {"t":2.76717, "x":3.11221, "y":3.76391, "heading":-0.01252, "vx":0.67303, "vy":0.96173, "omega":0.11699, "ax":-3.70134, "ay":-5.20039, "alpha":-0.69697, "fx":[-56.1363,-59.75101,-64.88785,-61.26412], "fy":[-88.05223,-85.6282,-81.81328,-84.57193]}, + {"t":2.80394, "x":3.13445, "y":3.79575, "heading":-0.00822, "vx":0.53694, "vy":0.77052, "omega":0.09137, "ax":-3.6781, "ay":-5.21828, "alpha":-0.66206, "fx":[-55.96159,-59.40588,-64.30318,-60.84915], "fy":[-88.17759,-85.88417,-82.28932,-84.88488]}, + {"t":2.84071, "x":3.15171, "y":3.82056, "heading":-0.00486, "vx":0.4017, "vy":0.57865, "omega":0.06702, "ax":-3.65821, "ay":-5.23345, "alpha":-0.63226, "fx":[-55.813,-59.10933,-63.80169,-60.49499], "fy":[-88.28385,-86.10223,-82.69208,-85.14952]}, + {"t":2.87748, "x":3.16401, "y":3.8383, "heading":-0.0024, "vx":0.26719, "vy":0.38622, "omega":0.04378, "ax":-3.64099, "ay":-5.24646, "alpha":-0.60652, "fx":[-55.68526,-58.8511,-63.36674,-60.18984], "fy":[-88.37496,-86.29068,-83.0373,-85.37575]}, + {"t":2.91425, "x":3.17137, "y":3.84895, "heading":-0.00079, "vx":0.13332, "vy":0.19332, "omega":0.02147, "ax":-3.62594, "ay":-5.25775, "alpha":-0.58406, "fx":[-55.57442,-58.6236,-62.98584,-59.9248], "fy":[-88.45384,-86.45558,-83.33654,-85.57095]}, + {"t":2.95102, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/LOtoA.traj b/src/main/deploy/choreo/LOtoA.traj new file mode 100644 index 00000000..07fea6e6 --- /dev/null +++ b/src/main/deploy/choreo/LOtoA.traj @@ -0,0 +1,98 @@ +{ + "name":"LOtoA", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.104679584503174, "y":7.582505702972412, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9416842460632324, "y":5.109439373016357, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.104679584503174 m", "val":7.104679584503174}, "y":{"exp":"7.582505702972412 m", "val":7.582505702972412}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9416842460632324 m", "val":2.9416842460632324}, "y":{"exp":"5.109439373016357 m", "val":5.109439373016357}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.81898,2.52188], + "samples":[ + {"t":0.0, "x":7.10468, "y":7.58251, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.49563, "ay":-1.04553, "alpha":0.0, "fx":[-73.49506,-73.49506,-73.49506,-73.49506], "fy":[-17.09239,-17.09239,-17.09239,-17.09239]}, + {"t":0.04331, "x":7.10046, "y":7.58153, "heading":0.0, "vx":-0.1947, "vy":-0.04528, "omega":0.0, "ax":-4.49318, "ay":-1.0551, "alpha":0.0, "fx":[-73.45494,-73.45494,-73.45494,-73.45494], "fy":[-17.24894,-17.24894,-17.24894,-17.24894]}, + {"t":0.08662, "x":7.08782, "y":7.57857, "heading":0.0, "vx":-0.3893, "vy":-0.09098, "omega":0.0, "ax":-4.49045, "ay":-1.06565, "alpha":0.0, "fx":[-73.41036,-73.41036,-73.41036,-73.41036], "fy":[-17.42128,-17.42128,-17.42128,-17.42128]}, + {"t":0.12993, "x":7.06675, "y":7.57364, "heading":0.0, "vx":-0.58378, "vy":-0.13713, "omega":0.0, "ax":-4.48741, "ay":-1.07731, "alpha":0.0, "fx":[-73.36056,-73.36056,-73.36056,-73.36056], "fy":[-17.6119,-17.6119,-17.6119,-17.6119]}, + {"t":0.17324, "x":7.03725, "y":7.56669, "heading":0.0, "vx":-0.77812, "vy":-0.18379, "omega":0.0, "ax":-4.48398, "ay":-1.09027, "alpha":0.0, "fx":[-73.30454,-73.30454,-73.30454,-73.30454], "fy":[-17.82388,-17.82388,-17.82388,-17.82388]}, + {"t":0.21655, "x":6.99935, "y":7.5577, "heading":0.0, "vx":-0.97232, "vy":-0.231, "omega":0.0, "ax":-4.4801, "ay":-1.10478, "alpha":0.0, "fx":[-73.24111,-73.24111,-73.24111,-73.24111], "fy":[-18.061,-18.061,-18.061,-18.061]}, + {"t":0.25985, "x":6.95304, "y":7.54666, "heading":0.0, "vx":-1.16635, "vy":-0.27885, "omega":0.0, "ax":-4.47567, "ay":-1.12111, "alpha":0.0, "fx":[-73.16869,-73.16869,-73.16869,-73.16869], "fy":[-18.32801,-18.32801,-18.32801,-18.32801]}, + {"t":0.30316, "x":6.89833, "y":7.53353, "heading":0.0, "vx":-1.36019, "vy":-0.32741, "omega":0.0, "ax":-4.47057, "ay":-1.13964, "alpha":0.0, "fx":[-73.08526,-73.08526,-73.08526,-73.08526], "fy":[-18.63092,-18.63092,-18.63092,-18.63092]}, + {"t":0.34647, "x":6.83523, "y":7.51829, "heading":0.0, "vx":-1.5538, "vy":-0.37676, "omega":0.0, "ax":-4.46463, "ay":-1.16084, "alpha":0.0, "fx":[-72.98814,-72.98814,-72.98814,-72.98814], "fy":[-18.97747,-18.97747,-18.97747,-18.97747]}, + {"t":0.38978, "x":6.76374, "y":7.50088, "heading":0.0, "vx":-1.74716, "vy":-0.42704, "omega":0.0, "ax":-4.45763, "ay":-1.18532, "alpha":0.0, "fx":[-72.87374,-72.87374,-72.87374,-72.87374], "fy":[-19.37778,-19.37778,-19.37778,-19.37778]}, + {"t":0.43309, "x":6.6839, "y":7.48127, "heading":0.0, "vx":-1.94022, "vy":-0.47837, "omega":0.0, "ax":-4.44927, "ay":-1.21392, "alpha":0.0, "fx":[-72.73709,-72.73709,-72.73709,-72.73709], "fy":[-19.84534,-19.84534,-19.84534,-19.84534]}, + {"t":0.4764, "x":6.59569, "y":7.45942, "heading":0.0, "vx":-2.13291, "vy":-0.53095, "omega":0.0, "ax":-4.43912, "ay":-1.24776, "alpha":0.0, "fx":[-72.57116,-72.57116,-72.57116,-72.57116], "fy":[-20.39858,-20.39858,-20.39858,-20.39858]}, + {"t":0.51971, "x":6.49916, "y":7.43525, "heading":0.0, "vx":-2.32517, "vy":-0.58499, "omega":0.0, "ax":-4.42655, "ay":-1.28842, "alpha":0.0, "fx":[-72.36571,-72.36571,-72.36571,-72.36571], "fy":[-21.06323,-21.06323,-21.06323,-21.06323]}, + {"t":0.56302, "x":6.3943, "y":7.40871, "heading":0.0, "vx":-2.51688, "vy":-0.64079, "omega":0.0, "ax":-4.41062, "ay":-1.33816, "alpha":0.0, "fx":[-72.10517,-72.10517,-72.10517,-72.10517], "fy":[-21.87642,-21.87642,-21.87642,-21.87642]}, + {"t":0.60633, "x":6.28116, "y":7.3797, "heading":0.0, "vx":-2.7079, "vy":-0.69874, "omega":0.0, "ax":-4.3898, "ay":-1.40039, "alpha":0.0, "fx":[-71.7649,-71.7649,-71.7649,-71.7649], "fy":[-22.89366,-22.89366,-22.89366,-22.89366]}, + {"t":0.64964, "x":6.15977, "y":7.34813, "heading":0.0, "vx":-2.89801, "vy":-0.75939, "omega":0.0, "ax":-4.36158, "ay":-1.48039, "alpha":0.0, "fx":[-71.30359,-71.30359,-71.30359,-71.30359], "fy":[-24.20161,-24.20161,-24.20161,-24.20161]}, + {"t":0.69295, "x":6.03017, "y":7.31385, "heading":0.0, "vx":-3.08691, "vy":-0.82351, "omega":0.0, "ax":-4.32142, "ay":-1.58692, "alpha":0.0, "fx":[-70.64698,-70.64698,-70.64698,-70.64698], "fy":[-25.94306,-25.94306,-25.94306,-25.94306]}, + {"t":0.73626, "x":5.89243, "y":7.2767, "heading":0.0, "vx":-3.27407, "vy":-0.89223, "omega":0.0, "ax":-4.26037, "ay":-1.73534, "alpha":0.0, "fx":[-69.64888,-69.64888,-69.64888,-69.64888], "fy":[-28.36956,-28.36956,-28.36956,-28.36956]}, + {"t":0.77956, "x":5.74663, "y":7.23643, "heading":0.0, "vx":-3.45858, "vy":-0.96739, "omega":0.0, "ax":-4.15854, "ay":-1.95517, "alpha":0.0, "fx":[-67.98415,-67.98415,-67.98415,-67.98415], "fy":[-31.96324,-31.96324,-31.96324,-31.96324]}, + {"t":0.82287, "x":5.59294, "y":7.1927, "heading":0.0, "vx":-3.63868, "vy":-1.05207, "omega":0.0, "ax":-3.96353, "ay":-2.30897, "alpha":0.0, "fx":[-64.79624,-64.79624,-64.79624,-64.79624], "fy":[-37.74729,-37.74729,-37.74729,-37.74729]}, + {"t":0.86618, "x":5.43164, "y":7.14497, "heading":0.0, "vx":-3.81034, "vy":-1.15207, "omega":0.0, "ax":-3.50025, "ay":-2.94127, "alpha":0.0, "fx":[-57.22241,-57.22241,-57.22241,-57.22241], "fy":[-48.08418,-48.08418,-48.08418,-48.08418]}, + {"t":0.90949, "x":5.26333, "y":7.09231, "heading":0.0, "vx":-3.96193, "vy":-1.27945, "omega":0.0, "ax":-2.02456, "ay":-4.0665, "alpha":0.0, "fx":[-33.09767,-33.09767,-33.09767,-33.09767], "fy":[-66.47947,-66.47947,-66.47947,-66.47947]}, + {"t":0.9528, "x":5.08985, "y":7.03309, "heading":0.0, "vx":-4.04961, "vy":-1.45557, "omega":0.0, "ax":1.23757, "ay":-4.35398, "alpha":0.0, "fx":[20.23194,20.23194,20.23194,20.23194], "fy":[-71.17931,-71.17931,-71.17931,-71.17931]}, + {"t":0.99611, "x":4.91562, "y":6.96597, "heading":0.0, "vx":-3.99602, "vy":-1.64413, "omega":0.0, "ax":1.81979, "ay":-4.16129, "alpha":0.0, "fx":[29.75005,29.75005,29.75005,29.75005], "fy":[-68.02918,-68.02918,-68.02918,-68.02918]}, + {"t":1.03942, "x":4.74427, "y":6.89086, "heading":0.0, "vx":-3.9172, "vy":-1.82436, "omega":0.0, "ax":2.12408, "ay":-4.02967, "alpha":0.0, "fx":[34.72466,34.72466,34.72466,34.72466], "fy":[-65.87743,-65.87743,-65.87743,-65.87743]}, + {"t":1.08273, "x":4.57661, "y":6.80807, "heading":0.0, "vx":-3.82521, "vy":-1.99888, "omega":0.0, "ax":3.23503, "ay":-3.22582, "alpha":0.0, "fx":[52.88657,52.88657,52.88657,52.88657], "fy":[-52.73603,-52.73603,-52.73603,-52.73603]}, + {"t":1.12604, "x":4.41397, "y":6.71847, "heading":0.0, "vx":-3.6851, "vy":-2.13858, "omega":0.0, "ax":4.07879, "ay":-2.08823, "alpha":0.0, "fx":[66.6804,66.6804,66.6804,66.6804], "fy":[-34.13858,-34.13858,-34.13858,-34.13858]}, + {"t":1.16935, "x":4.2582, "y":6.62389, "heading":0.0, "vx":-3.50846, "vy":-2.22902, "omega":0.0, "ax":4.39352, "ay":-1.33432, "alpha":0.0, "fx":[71.82561,71.82561,71.82561,71.82561], "fy":[-21.81365,-21.81365,-21.81365,-21.81365]}, + {"t":1.21266, "x":4.11037, "y":6.5261, "heading":0.0, "vx":-3.31818, "vy":-2.28681, "omega":0.0, "ax":4.51649, "ay":-0.86025, "alpha":0.0, "fx":[73.83609,73.83609,73.83609,73.83609], "fy":[-14.06347,-14.06347,-14.06347,-14.06347]}, + {"t":1.25596, "x":3.9709, "y":6.42626, "heading":0.0, "vx":-3.12257, "vy":-2.32407, "omega":0.0, "ax":4.56915, "ay":-0.54659, "alpha":0.0, "fx":[74.69683,74.69683,74.69683,74.69683], "fy":[-8.93567,-8.93567,-8.93567,-8.93567]}, + {"t":1.29927, "x":3.83995, "y":6.32509, "heading":0.0, "vx":-2.92469, "vy":-2.34774, "omega":0.0, "ax":4.59294, "ay":-0.32699, "alpha":0.0, "fx":[75.08586,75.08586,75.08586,75.08586], "fy":[-5.34559,-5.34559,-5.34559,-5.34559]}, + {"t":1.34258, "x":3.71759, "y":6.22311, "heading":0.0, "vx":-2.72577, "vy":-2.3619, "omega":0.0, "ax":4.60368, "ay":-0.16578, "alpha":0.0, "fx":[75.26143,75.26143,75.26143,75.26143], "fy":[-2.71017,-2.71017,-2.71017,-2.71017]}, + {"t":1.38589, "x":3.60386, "y":6.12066, "heading":0.0, "vx":-2.52639, "vy":-2.36908, "omega":0.0, "ax":4.60807, "ay":-0.04286, "alpha":0.0, "fx":[75.3332,75.3332,75.3332,75.3332], "fy":[-0.70071,-0.70071,-0.70071,-0.70071]}, + {"t":1.4292, "x":3.49877, "y":6.01802, "heading":0.0, "vx":-2.32682, "vy":-2.37094, "omega":0.0, "ax":4.60922, "ay":0.05375, "alpha":0.0, "fx":[75.35203,75.35203,75.35203,75.35203], "fy":[0.87876,0.87876,0.87876,0.87876]}, + {"t":1.47251, "x":3.40232, "y":5.91538, "heading":0.0, "vx":-2.12719, "vy":-2.36861, "omega":0.0, "ax":4.60868, "ay":0.13159, "alpha":0.0, "fx":[75.34316,75.34316,75.34316,75.34316], "fy":[2.15126,2.15126,2.15126,2.15126]}, + {"t":1.51582, "x":3.31451, "y":5.81292, "heading":0.0, "vx":-1.9276, "vy":-2.36291, "omega":0.0, "ax":4.60725, "ay":0.19559, "alpha":0.0, "fx":[75.31979,75.31979,75.31979,75.31979], "fy":[3.19747,3.19747,3.19747,3.19747]}, + {"t":1.55913, "x":3.23535, "y":5.71077, "heading":0.0, "vx":-1.72806, "vy":-2.35444, "omega":0.0, "ax":4.60537, "ay":0.2491, "alpha":0.0, "fx":[75.2891,75.2891,75.2891,75.2891], "fy":[4.07229,4.07229,4.07229,4.07229]}, + {"t":1.60244, "x":3.16483, "y":5.60904, "heading":0.0, "vx":-1.52861, "vy":-2.34365, "omega":0.0, "ax":4.60329, "ay":0.29449, "alpha":0.0, "fx":[75.2551,75.2551,75.2551,75.2551], "fy":[4.81436,4.81436,4.81436,4.81436]}, + {"t":1.64575, "x":3.10294, "y":5.50781, "heading":0.0, "vx":-1.32924, "vy":-2.3309, "omega":0.0, "ax":4.60115, "ay":0.33347, "alpha":0.0, "fx":[75.22005,75.22005,75.22005,75.22005], "fy":[5.45157,5.45157,5.45157,5.45157]}, + {"t":1.68906, "x":3.04969, "y":5.40717, "heading":0.0, "vx":-1.12997, "vy":-2.31646, "omega":0.0, "ax":4.59902, "ay":0.36729, "alpha":0.0, "fx":[75.18526,75.18526,75.18526,75.18526], "fy":[6.00453,6.00453,6.00453,6.00453]}, + {"t":1.73237, "x":3.00506, "y":5.3072, "heading":0.0, "vx":-0.93079, "vy":-2.30055, "omega":0.0, "ax":4.59695, "ay":0.39692, "alpha":0.0, "fx":[75.15144,75.15144,75.15144,75.15144], "fy":[6.48884,6.48884,6.48884,6.48884]}, + {"t":1.77567, "x":2.96906, "y":5.20793, "heading":0.0, "vx":-0.7317, "vy":-2.28336, "omega":0.0, "ax":4.59497, "ay":0.42307, "alpha":0.0, "fx":[75.11897,75.11897,75.11897,75.11897], "fy":[6.91645,6.91645,6.91645,6.91645]}, + {"t":1.81898, "x":2.94168, "y":5.10944, "heading":0.0, "vx":-0.53269, "vy":-2.26504, "omega":0.0, "ax":4.57905, "ay":0.56139, "alpha":0.0, "fx":[74.85868,74.85868,74.85868,74.85868], "fy":[9.17768,9.17768,9.17768,9.17768]}, + {"t":1.85598, "x":2.92511, "y":5.02603, "heading":0.0, "vx":-0.3633, "vy":-2.24427, "omega":0.0, "ax":4.53311, "ay":0.85281, "alpha":0.0, "fx":[74.10775,74.10775,74.10775,74.10775], "fy":[13.94186,13.94186,13.94186,13.94186]}, + {"t":1.89297, "x":2.91477, "y":4.94359, "heading":0.0, "vx":-0.1956, "vy":-2.21272, "omega":0.0, "ax":4.4514, "ay":1.20579, "alpha":0.0, "fx":[72.77191,72.77191,72.77191,72.77191], "fy":[19.71231,19.71231,19.71231,19.71231]}, + {"t":1.92997, "x":2.91058, "y":4.86255, "heading":0.0, "vx":-0.03092, "vy":-2.16811, "omega":0.0, "ax":4.3121, "ay":1.6328, "alpha":0.0, "fx":[70.49455,70.49455,70.49455,70.49455], "fy":[26.69322,26.69322,26.69322,26.69322]}, + {"t":1.96696, "x":2.91239, "y":4.78346, "heading":0.0, "vx":0.12861, "vy":-2.10771, "omega":0.0, "ax":4.0818, "ay":2.1423, "alpha":0.0, "fx":[66.7297,66.7297,66.7297,66.7297], "fy":[35.02248,35.02248,35.02248,35.02248]}, + {"t":2.00396, "x":2.91994, "y":4.70696, "heading":0.0, "vx":0.27961, "vy":-2.02845, "omega":0.0, "ax":3.71461, "ay":2.72803, "alpha":0.0, "fx":[60.72686,60.72686,60.72686,60.72686], "fy":[44.59812,44.59812,44.59812,44.59812]}, + {"t":2.04095, "x":2.93283, "y":4.63378, "heading":0.0, "vx":0.41703, "vy":-1.92753, "omega":0.0, "ax":3.162, "ay":3.35161, "alpha":0.0, "fx":[51.69265,51.69265,51.69265,51.69265], "fy":[54.79243,54.79243,54.79243,54.79243]}, + {"t":2.07794, "x":2.95042, "y":4.56477, "heading":0.0, "vx":0.53401, "vy":-1.80354, "omega":0.0, "ax":2.40421, "ay":3.93008, "alpha":0.0, "fx":[39.30431,39.30431,39.30431,39.30431], "fy":[64.24928,64.24928,64.24928,64.24928]}, + {"t":2.11494, "x":2.97182, "y":4.50074, "heading":0.0, "vx":0.62295, "vy":-1.65815, "omega":0.0, "ax":1.4919, "ay":4.35883, "alpha":0.0, "fx":[24.38972,24.38972,24.38972,24.38972], "fy":[71.25854,71.25854,71.25854,71.25854]}, + {"t":2.15193, "x":2.99589, "y":4.44238, "heading":0.0, "vx":0.67814, "vy":-1.4969, "omega":0.0, "ax":0.54449, "ay":4.57531, "alpha":0.0, "fx":[8.90133,8.90133,8.90133,8.90133], "fy":[74.79764,74.79764,74.79764,74.79764]}, + {"t":2.18893, "x":3.02135, "y":4.39013, "heading":0.0, "vx":0.69828, "vy":-1.32764, "omega":0.0, "ax":-0.3172, "ay":4.59759, "alpha":0.0, "fx":[-5.18561,-5.18561,-5.18561,-5.18561], "fy":[75.16184,75.16184,75.16184,75.16184]}, + {"t":2.22592, "x":3.04696, "y":4.34416, "heading":0.0, "vx":0.68655, "vy":-1.15755, "omega":0.0, "ax":-1.03095, "ay":4.49284, "alpha":0.0, "fx":[-16.85401,-16.85401,-16.85401,-16.85401], "fy":[73.44929,73.44929,73.44929,73.44929]}, + {"t":2.26292, "x":3.07165, "y":4.30441, "heading":0.0, "vx":0.64841, "vy":-0.99134, "omega":0.0, "ax":-1.59248, "ay":4.32692, "alpha":0.0, "fx":[-26.03407,-26.03407,-26.03407,-26.03407], "fy":[70.7369,70.7369,70.7369,70.7369]}, + {"t":2.29991, "x":3.09455, "y":4.2707, "heading":0.0, "vx":0.5895, "vy":-0.83127, "omega":0.0, "ax":-2.02556, "ay":4.14297, "alpha":0.0, "fx":[-33.11407,-33.11407,-33.11407,-33.11407], "fy":[67.72975,67.72975,67.72975,67.72975]}, + {"t":2.33691, "x":3.11497, "y":4.24278, "heading":0.0, "vx":0.51456, "vy":-0.678, "omega":0.0, "ax":-2.35931, "ay":3.9634, "alpha":0.0, "fx":[-38.57018,-38.57018,-38.57018,-38.57018], "fy":[64.79409,64.79409,64.79409,64.79409]}, + {"t":2.3739, "x":3.1324, "y":4.22041, "heading":0.0, "vx":0.42728, "vy":-0.53138, "omega":0.0, "ax":-2.61887, "ay":3.79777, "alpha":0.0, "fx":[-42.81357,-42.81357,-42.81357,-42.81357], "fy":[62.0863,62.0863,62.0863,62.0863]}, + {"t":2.41089, "x":3.14641, "y":4.20335, "heading":0.0, "vx":0.3304, "vy":-0.39088, "omega":0.0, "ax":-2.8235, "ay":3.64898, "alpha":0.0, "fx":[-46.15883,-46.15883,-46.15883,-46.15883], "fy":[59.65391,59.65391,59.65391,59.65391]}, + {"t":2.44789, "x":3.1567, "y":4.19139, "heading":0.0, "vx":0.22594, "vy":-0.25589, "omega":0.0, "ax":-2.98725, "ay":3.51688, "alpha":0.0, "fx":[-48.83579,-48.83579,-48.83579,-48.83579], "fy":[57.49424,57.49424,57.49424,57.49424]}, + {"t":2.48488, "x":3.16302, "y":4.18433, "heading":0.0, "vx":0.11543, "vy":-0.12578, "omega":0.0, "ax":-3.12024, "ay":3.40005, "alpha":0.0, "fx":[-51.00997,-51.00997,-51.00997,-51.00997], "fy":[55.58434,55.58434,55.58434,55.58434]}, + {"t":2.52188, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index cf30e0b5..4775089d 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -444,6 +444,44 @@ public Command CMtoGH() { // algae path // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); return routine.cmd(); } + public Command LOtoA() { // 2910 + final var routine = factory.newRoutine("LO to A"); + bindCoralElevatorExtension(routine, 2.0); + HashMap steps = new HashMap(); + // lo a b4 b2 dealgae + steps.put("LOtoA", routine.trajectory("LOtoA")); + steps.put("AtoB", routine.trajectory("AtoB")); + routine + // run first path + .active() + .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .whileTrue( + Commands.sequence( + routine.trajectory("LOtoA").resetOdometry(), routine.trajectory("LOtoA").cmd())); + + runGroundPath(routine, "LO", "A", "B", steps); + // ---------------- + // runGroundPath(routine, "A", "B", "B", steps); + // TODO dealgae - merge from prechamps + + // --------- + return routine.cmd(); + } + public void runGroundPath( + AutoRoutine routine, + String startPos, + String endPos, + String nextPos, + HashMap steps) { + routine + .observe(steps.get(startPos + "to" + endPos).done()) + .onTrue( + Commands.sequence( + scoreCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose().get()), + Commands.runOnce(() -> autoGroundCoralIntake = true), + steps.get(endPos + "to" + nextPos).cmd())); +} + public Command scoreCoralInAuto(Supplier trajEndPose) { return Commands.sequence( From a25d8ebc3d3f5ea1287e35a2b0e42dd3b2cf8527 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 12 Apr 2025 11:22:50 -0700 Subject: [PATCH 045/154] made path less aggressive --- src/main/deploy/choreo/LOtoA.traj | 144 +++++++++++++++-------------- src/main/java/frc/robot/Autos.java | 36 +++++--- src/main/java/frc/robot/Robot.java | 3 +- 3 files changed, 99 insertions(+), 84 deletions(-) diff --git a/src/main/deploy/choreo/LOtoA.traj b/src/main/deploy/choreo/LOtoA.traj index 07fea6e6..c9f2372f 100644 --- a/src/main/deploy/choreo/LOtoA.traj +++ b/src/main/deploy/choreo/LOtoA.traj @@ -3,24 +3,28 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.104679584503174, "y":7.582505702972412, "heading":0.0, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.9416842460632324, "y":5.109439373016357, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.104679584503174, "y":7.582505702972412, "heading":0.0, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.636875629425049, "y":5.11732816696167, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.104679584503174 m", "val":7.104679584503174}, "y":{"exp":"7.582505702972412 m", "val":7.582505702972412}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.9416842460632324 m", "val":2.9416842460632324}, "y":{"exp":"5.109439373016357 m", "val":5.109439373016357}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.104679584503174 m", "val":7.104679584503174}, "y":{"exp":"7.582505702972412 m", "val":7.582505702972412}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.636875629425049 m", "val":2.636875629425049}, "y":{"exp":"5.11732816696167 m", "val":5.11732816696167}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -28,70 +32,72 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.81898,2.52188], + "waypoints":[0.0,1.76157,2.94513], "samples":[ - {"t":0.0, "x":7.10468, "y":7.58251, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.49563, "ay":-1.04553, "alpha":0.0, "fx":[-73.49506,-73.49506,-73.49506,-73.49506], "fy":[-17.09239,-17.09239,-17.09239,-17.09239]}, - {"t":0.04331, "x":7.10046, "y":7.58153, "heading":0.0, "vx":-0.1947, "vy":-0.04528, "omega":0.0, "ax":-4.49318, "ay":-1.0551, "alpha":0.0, "fx":[-73.45494,-73.45494,-73.45494,-73.45494], "fy":[-17.24894,-17.24894,-17.24894,-17.24894]}, - {"t":0.08662, "x":7.08782, "y":7.57857, "heading":0.0, "vx":-0.3893, "vy":-0.09098, "omega":0.0, "ax":-4.49045, "ay":-1.06565, "alpha":0.0, "fx":[-73.41036,-73.41036,-73.41036,-73.41036], "fy":[-17.42128,-17.42128,-17.42128,-17.42128]}, - {"t":0.12993, "x":7.06675, "y":7.57364, "heading":0.0, "vx":-0.58378, "vy":-0.13713, "omega":0.0, "ax":-4.48741, "ay":-1.07731, "alpha":0.0, "fx":[-73.36056,-73.36056,-73.36056,-73.36056], "fy":[-17.6119,-17.6119,-17.6119,-17.6119]}, - {"t":0.17324, "x":7.03725, "y":7.56669, "heading":0.0, "vx":-0.77812, "vy":-0.18379, "omega":0.0, "ax":-4.48398, "ay":-1.09027, "alpha":0.0, "fx":[-73.30454,-73.30454,-73.30454,-73.30454], "fy":[-17.82388,-17.82388,-17.82388,-17.82388]}, - {"t":0.21655, "x":6.99935, "y":7.5577, "heading":0.0, "vx":-0.97232, "vy":-0.231, "omega":0.0, "ax":-4.4801, "ay":-1.10478, "alpha":0.0, "fx":[-73.24111,-73.24111,-73.24111,-73.24111], "fy":[-18.061,-18.061,-18.061,-18.061]}, - {"t":0.25985, "x":6.95304, "y":7.54666, "heading":0.0, "vx":-1.16635, "vy":-0.27885, "omega":0.0, "ax":-4.47567, "ay":-1.12111, "alpha":0.0, "fx":[-73.16869,-73.16869,-73.16869,-73.16869], "fy":[-18.32801,-18.32801,-18.32801,-18.32801]}, - {"t":0.30316, "x":6.89833, "y":7.53353, "heading":0.0, "vx":-1.36019, "vy":-0.32741, "omega":0.0, "ax":-4.47057, "ay":-1.13964, "alpha":0.0, "fx":[-73.08526,-73.08526,-73.08526,-73.08526], "fy":[-18.63092,-18.63092,-18.63092,-18.63092]}, - {"t":0.34647, "x":6.83523, "y":7.51829, "heading":0.0, "vx":-1.5538, "vy":-0.37676, "omega":0.0, "ax":-4.46463, "ay":-1.16084, "alpha":0.0, "fx":[-72.98814,-72.98814,-72.98814,-72.98814], "fy":[-18.97747,-18.97747,-18.97747,-18.97747]}, - {"t":0.38978, "x":6.76374, "y":7.50088, "heading":0.0, "vx":-1.74716, "vy":-0.42704, "omega":0.0, "ax":-4.45763, "ay":-1.18532, "alpha":0.0, "fx":[-72.87374,-72.87374,-72.87374,-72.87374], "fy":[-19.37778,-19.37778,-19.37778,-19.37778]}, - {"t":0.43309, "x":6.6839, "y":7.48127, "heading":0.0, "vx":-1.94022, "vy":-0.47837, "omega":0.0, "ax":-4.44927, "ay":-1.21392, "alpha":0.0, "fx":[-72.73709,-72.73709,-72.73709,-72.73709], "fy":[-19.84534,-19.84534,-19.84534,-19.84534]}, - {"t":0.4764, "x":6.59569, "y":7.45942, "heading":0.0, "vx":-2.13291, "vy":-0.53095, "omega":0.0, "ax":-4.43912, "ay":-1.24776, "alpha":0.0, "fx":[-72.57116,-72.57116,-72.57116,-72.57116], "fy":[-20.39858,-20.39858,-20.39858,-20.39858]}, - {"t":0.51971, "x":6.49916, "y":7.43525, "heading":0.0, "vx":-2.32517, "vy":-0.58499, "omega":0.0, "ax":-4.42655, "ay":-1.28842, "alpha":0.0, "fx":[-72.36571,-72.36571,-72.36571,-72.36571], "fy":[-21.06323,-21.06323,-21.06323,-21.06323]}, - {"t":0.56302, "x":6.3943, "y":7.40871, "heading":0.0, "vx":-2.51688, "vy":-0.64079, "omega":0.0, "ax":-4.41062, "ay":-1.33816, "alpha":0.0, "fx":[-72.10517,-72.10517,-72.10517,-72.10517], "fy":[-21.87642,-21.87642,-21.87642,-21.87642]}, - {"t":0.60633, "x":6.28116, "y":7.3797, "heading":0.0, "vx":-2.7079, "vy":-0.69874, "omega":0.0, "ax":-4.3898, "ay":-1.40039, "alpha":0.0, "fx":[-71.7649,-71.7649,-71.7649,-71.7649], "fy":[-22.89366,-22.89366,-22.89366,-22.89366]}, - {"t":0.64964, "x":6.15977, "y":7.34813, "heading":0.0, "vx":-2.89801, "vy":-0.75939, "omega":0.0, "ax":-4.36158, "ay":-1.48039, "alpha":0.0, "fx":[-71.30359,-71.30359,-71.30359,-71.30359], "fy":[-24.20161,-24.20161,-24.20161,-24.20161]}, - {"t":0.69295, "x":6.03017, "y":7.31385, "heading":0.0, "vx":-3.08691, "vy":-0.82351, "omega":0.0, "ax":-4.32142, "ay":-1.58692, "alpha":0.0, "fx":[-70.64698,-70.64698,-70.64698,-70.64698], "fy":[-25.94306,-25.94306,-25.94306,-25.94306]}, - {"t":0.73626, "x":5.89243, "y":7.2767, "heading":0.0, "vx":-3.27407, "vy":-0.89223, "omega":0.0, "ax":-4.26037, "ay":-1.73534, "alpha":0.0, "fx":[-69.64888,-69.64888,-69.64888,-69.64888], "fy":[-28.36956,-28.36956,-28.36956,-28.36956]}, - {"t":0.77956, "x":5.74663, "y":7.23643, "heading":0.0, "vx":-3.45858, "vy":-0.96739, "omega":0.0, "ax":-4.15854, "ay":-1.95517, "alpha":0.0, "fx":[-67.98415,-67.98415,-67.98415,-67.98415], "fy":[-31.96324,-31.96324,-31.96324,-31.96324]}, - {"t":0.82287, "x":5.59294, "y":7.1927, "heading":0.0, "vx":-3.63868, "vy":-1.05207, "omega":0.0, "ax":-3.96353, "ay":-2.30897, "alpha":0.0, "fx":[-64.79624,-64.79624,-64.79624,-64.79624], "fy":[-37.74729,-37.74729,-37.74729,-37.74729]}, - {"t":0.86618, "x":5.43164, "y":7.14497, "heading":0.0, "vx":-3.81034, "vy":-1.15207, "omega":0.0, "ax":-3.50025, "ay":-2.94127, "alpha":0.0, "fx":[-57.22241,-57.22241,-57.22241,-57.22241], "fy":[-48.08418,-48.08418,-48.08418,-48.08418]}, - {"t":0.90949, "x":5.26333, "y":7.09231, "heading":0.0, "vx":-3.96193, "vy":-1.27945, "omega":0.0, "ax":-2.02456, "ay":-4.0665, "alpha":0.0, "fx":[-33.09767,-33.09767,-33.09767,-33.09767], "fy":[-66.47947,-66.47947,-66.47947,-66.47947]}, - {"t":0.9528, "x":5.08985, "y":7.03309, "heading":0.0, "vx":-4.04961, "vy":-1.45557, "omega":0.0, "ax":1.23757, "ay":-4.35398, "alpha":0.0, "fx":[20.23194,20.23194,20.23194,20.23194], "fy":[-71.17931,-71.17931,-71.17931,-71.17931]}, - {"t":0.99611, "x":4.91562, "y":6.96597, "heading":0.0, "vx":-3.99602, "vy":-1.64413, "omega":0.0, "ax":1.81979, "ay":-4.16129, "alpha":0.0, "fx":[29.75005,29.75005,29.75005,29.75005], "fy":[-68.02918,-68.02918,-68.02918,-68.02918]}, - {"t":1.03942, "x":4.74427, "y":6.89086, "heading":0.0, "vx":-3.9172, "vy":-1.82436, "omega":0.0, "ax":2.12408, "ay":-4.02967, "alpha":0.0, "fx":[34.72466,34.72466,34.72466,34.72466], "fy":[-65.87743,-65.87743,-65.87743,-65.87743]}, - {"t":1.08273, "x":4.57661, "y":6.80807, "heading":0.0, "vx":-3.82521, "vy":-1.99888, "omega":0.0, "ax":3.23503, "ay":-3.22582, "alpha":0.0, "fx":[52.88657,52.88657,52.88657,52.88657], "fy":[-52.73603,-52.73603,-52.73603,-52.73603]}, - {"t":1.12604, "x":4.41397, "y":6.71847, "heading":0.0, "vx":-3.6851, "vy":-2.13858, "omega":0.0, "ax":4.07879, "ay":-2.08823, "alpha":0.0, "fx":[66.6804,66.6804,66.6804,66.6804], "fy":[-34.13858,-34.13858,-34.13858,-34.13858]}, - {"t":1.16935, "x":4.2582, "y":6.62389, "heading":0.0, "vx":-3.50846, "vy":-2.22902, "omega":0.0, "ax":4.39352, "ay":-1.33432, "alpha":0.0, "fx":[71.82561,71.82561,71.82561,71.82561], "fy":[-21.81365,-21.81365,-21.81365,-21.81365]}, - {"t":1.21266, "x":4.11037, "y":6.5261, "heading":0.0, "vx":-3.31818, "vy":-2.28681, "omega":0.0, "ax":4.51649, "ay":-0.86025, "alpha":0.0, "fx":[73.83609,73.83609,73.83609,73.83609], "fy":[-14.06347,-14.06347,-14.06347,-14.06347]}, - {"t":1.25596, "x":3.9709, "y":6.42626, "heading":0.0, "vx":-3.12257, "vy":-2.32407, "omega":0.0, "ax":4.56915, "ay":-0.54659, "alpha":0.0, "fx":[74.69683,74.69683,74.69683,74.69683], "fy":[-8.93567,-8.93567,-8.93567,-8.93567]}, - {"t":1.29927, "x":3.83995, "y":6.32509, "heading":0.0, "vx":-2.92469, "vy":-2.34774, "omega":0.0, "ax":4.59294, "ay":-0.32699, "alpha":0.0, "fx":[75.08586,75.08586,75.08586,75.08586], "fy":[-5.34559,-5.34559,-5.34559,-5.34559]}, - {"t":1.34258, "x":3.71759, "y":6.22311, "heading":0.0, "vx":-2.72577, "vy":-2.3619, "omega":0.0, "ax":4.60368, "ay":-0.16578, "alpha":0.0, "fx":[75.26143,75.26143,75.26143,75.26143], "fy":[-2.71017,-2.71017,-2.71017,-2.71017]}, - {"t":1.38589, "x":3.60386, "y":6.12066, "heading":0.0, "vx":-2.52639, "vy":-2.36908, "omega":0.0, "ax":4.60807, "ay":-0.04286, "alpha":0.0, "fx":[75.3332,75.3332,75.3332,75.3332], "fy":[-0.70071,-0.70071,-0.70071,-0.70071]}, - {"t":1.4292, "x":3.49877, "y":6.01802, "heading":0.0, "vx":-2.32682, "vy":-2.37094, "omega":0.0, "ax":4.60922, "ay":0.05375, "alpha":0.0, "fx":[75.35203,75.35203,75.35203,75.35203], "fy":[0.87876,0.87876,0.87876,0.87876]}, - {"t":1.47251, "x":3.40232, "y":5.91538, "heading":0.0, "vx":-2.12719, "vy":-2.36861, "omega":0.0, "ax":4.60868, "ay":0.13159, "alpha":0.0, "fx":[75.34316,75.34316,75.34316,75.34316], "fy":[2.15126,2.15126,2.15126,2.15126]}, - {"t":1.51582, "x":3.31451, "y":5.81292, "heading":0.0, "vx":-1.9276, "vy":-2.36291, "omega":0.0, "ax":4.60725, "ay":0.19559, "alpha":0.0, "fx":[75.31979,75.31979,75.31979,75.31979], "fy":[3.19747,3.19747,3.19747,3.19747]}, - {"t":1.55913, "x":3.23535, "y":5.71077, "heading":0.0, "vx":-1.72806, "vy":-2.35444, "omega":0.0, "ax":4.60537, "ay":0.2491, "alpha":0.0, "fx":[75.2891,75.2891,75.2891,75.2891], "fy":[4.07229,4.07229,4.07229,4.07229]}, - {"t":1.60244, "x":3.16483, "y":5.60904, "heading":0.0, "vx":-1.52861, "vy":-2.34365, "omega":0.0, "ax":4.60329, "ay":0.29449, "alpha":0.0, "fx":[75.2551,75.2551,75.2551,75.2551], "fy":[4.81436,4.81436,4.81436,4.81436]}, - {"t":1.64575, "x":3.10294, "y":5.50781, "heading":0.0, "vx":-1.32924, "vy":-2.3309, "omega":0.0, "ax":4.60115, "ay":0.33347, "alpha":0.0, "fx":[75.22005,75.22005,75.22005,75.22005], "fy":[5.45157,5.45157,5.45157,5.45157]}, - {"t":1.68906, "x":3.04969, "y":5.40717, "heading":0.0, "vx":-1.12997, "vy":-2.31646, "omega":0.0, "ax":4.59902, "ay":0.36729, "alpha":0.0, "fx":[75.18526,75.18526,75.18526,75.18526], "fy":[6.00453,6.00453,6.00453,6.00453]}, - {"t":1.73237, "x":3.00506, "y":5.3072, "heading":0.0, "vx":-0.93079, "vy":-2.30055, "omega":0.0, "ax":4.59695, "ay":0.39692, "alpha":0.0, "fx":[75.15144,75.15144,75.15144,75.15144], "fy":[6.48884,6.48884,6.48884,6.48884]}, - {"t":1.77567, "x":2.96906, "y":5.20793, "heading":0.0, "vx":-0.7317, "vy":-2.28336, "omega":0.0, "ax":4.59497, "ay":0.42307, "alpha":0.0, "fx":[75.11897,75.11897,75.11897,75.11897], "fy":[6.91645,6.91645,6.91645,6.91645]}, - {"t":1.81898, "x":2.94168, "y":5.10944, "heading":0.0, "vx":-0.53269, "vy":-2.26504, "omega":0.0, "ax":4.57905, "ay":0.56139, "alpha":0.0, "fx":[74.85868,74.85868,74.85868,74.85868], "fy":[9.17768,9.17768,9.17768,9.17768]}, - {"t":1.85598, "x":2.92511, "y":5.02603, "heading":0.0, "vx":-0.3633, "vy":-2.24427, "omega":0.0, "ax":4.53311, "ay":0.85281, "alpha":0.0, "fx":[74.10775,74.10775,74.10775,74.10775], "fy":[13.94186,13.94186,13.94186,13.94186]}, - {"t":1.89297, "x":2.91477, "y":4.94359, "heading":0.0, "vx":-0.1956, "vy":-2.21272, "omega":0.0, "ax":4.4514, "ay":1.20579, "alpha":0.0, "fx":[72.77191,72.77191,72.77191,72.77191], "fy":[19.71231,19.71231,19.71231,19.71231]}, - {"t":1.92997, "x":2.91058, "y":4.86255, "heading":0.0, "vx":-0.03092, "vy":-2.16811, "omega":0.0, "ax":4.3121, "ay":1.6328, "alpha":0.0, "fx":[70.49455,70.49455,70.49455,70.49455], "fy":[26.69322,26.69322,26.69322,26.69322]}, - {"t":1.96696, "x":2.91239, "y":4.78346, "heading":0.0, "vx":0.12861, "vy":-2.10771, "omega":0.0, "ax":4.0818, "ay":2.1423, "alpha":0.0, "fx":[66.7297,66.7297,66.7297,66.7297], "fy":[35.02248,35.02248,35.02248,35.02248]}, - {"t":2.00396, "x":2.91994, "y":4.70696, "heading":0.0, "vx":0.27961, "vy":-2.02845, "omega":0.0, "ax":3.71461, "ay":2.72803, "alpha":0.0, "fx":[60.72686,60.72686,60.72686,60.72686], "fy":[44.59812,44.59812,44.59812,44.59812]}, - {"t":2.04095, "x":2.93283, "y":4.63378, "heading":0.0, "vx":0.41703, "vy":-1.92753, "omega":0.0, "ax":3.162, "ay":3.35161, "alpha":0.0, "fx":[51.69265,51.69265,51.69265,51.69265], "fy":[54.79243,54.79243,54.79243,54.79243]}, - {"t":2.07794, "x":2.95042, "y":4.56477, "heading":0.0, "vx":0.53401, "vy":-1.80354, "omega":0.0, "ax":2.40421, "ay":3.93008, "alpha":0.0, "fx":[39.30431,39.30431,39.30431,39.30431], "fy":[64.24928,64.24928,64.24928,64.24928]}, - {"t":2.11494, "x":2.97182, "y":4.50074, "heading":0.0, "vx":0.62295, "vy":-1.65815, "omega":0.0, "ax":1.4919, "ay":4.35883, "alpha":0.0, "fx":[24.38972,24.38972,24.38972,24.38972], "fy":[71.25854,71.25854,71.25854,71.25854]}, - {"t":2.15193, "x":2.99589, "y":4.44238, "heading":0.0, "vx":0.67814, "vy":-1.4969, "omega":0.0, "ax":0.54449, "ay":4.57531, "alpha":0.0, "fx":[8.90133,8.90133,8.90133,8.90133], "fy":[74.79764,74.79764,74.79764,74.79764]}, - {"t":2.18893, "x":3.02135, "y":4.39013, "heading":0.0, "vx":0.69828, "vy":-1.32764, "omega":0.0, "ax":-0.3172, "ay":4.59759, "alpha":0.0, "fx":[-5.18561,-5.18561,-5.18561,-5.18561], "fy":[75.16184,75.16184,75.16184,75.16184]}, - {"t":2.22592, "x":3.04696, "y":4.34416, "heading":0.0, "vx":0.68655, "vy":-1.15755, "omega":0.0, "ax":-1.03095, "ay":4.49284, "alpha":0.0, "fx":[-16.85401,-16.85401,-16.85401,-16.85401], "fy":[73.44929,73.44929,73.44929,73.44929]}, - {"t":2.26292, "x":3.07165, "y":4.30441, "heading":0.0, "vx":0.64841, "vy":-0.99134, "omega":0.0, "ax":-1.59248, "ay":4.32692, "alpha":0.0, "fx":[-26.03407,-26.03407,-26.03407,-26.03407], "fy":[70.7369,70.7369,70.7369,70.7369]}, - {"t":2.29991, "x":3.09455, "y":4.2707, "heading":0.0, "vx":0.5895, "vy":-0.83127, "omega":0.0, "ax":-2.02556, "ay":4.14297, "alpha":0.0, "fx":[-33.11407,-33.11407,-33.11407,-33.11407], "fy":[67.72975,67.72975,67.72975,67.72975]}, - {"t":2.33691, "x":3.11497, "y":4.24278, "heading":0.0, "vx":0.51456, "vy":-0.678, "omega":0.0, "ax":-2.35931, "ay":3.9634, "alpha":0.0, "fx":[-38.57018,-38.57018,-38.57018,-38.57018], "fy":[64.79409,64.79409,64.79409,64.79409]}, - {"t":2.3739, "x":3.1324, "y":4.22041, "heading":0.0, "vx":0.42728, "vy":-0.53138, "omega":0.0, "ax":-2.61887, "ay":3.79777, "alpha":0.0, "fx":[-42.81357,-42.81357,-42.81357,-42.81357], "fy":[62.0863,62.0863,62.0863,62.0863]}, - {"t":2.41089, "x":3.14641, "y":4.20335, "heading":0.0, "vx":0.3304, "vy":-0.39088, "omega":0.0, "ax":-2.8235, "ay":3.64898, "alpha":0.0, "fx":[-46.15883,-46.15883,-46.15883,-46.15883], "fy":[59.65391,59.65391,59.65391,59.65391]}, - {"t":2.44789, "x":3.1567, "y":4.19139, "heading":0.0, "vx":0.22594, "vy":-0.25589, "omega":0.0, "ax":-2.98725, "ay":3.51688, "alpha":0.0, "fx":[-48.83579,-48.83579,-48.83579,-48.83579], "fy":[57.49424,57.49424,57.49424,57.49424]}, - {"t":2.48488, "x":3.16302, "y":4.18433, "heading":0.0, "vx":0.11543, "vy":-0.12578, "omega":0.0, "ax":-3.12024, "ay":3.40005, "alpha":0.0, "fx":[-51.00997,-51.00997,-51.00997,-51.00997], "fy":[55.58434,55.58434,55.58434,55.58434]}, - {"t":2.52188, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10468, "y":7.58251, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.45556, "ay":-2.4909, "alpha":-0.00187, "fx":[-89.18229,-89.19099,-89.19312,-89.18572], "fy":[-40.73407,-40.71492,-40.71042,-40.72673]}, + {"t":0.04636, "x":7.09882, "y":7.57983, "heading":0.0, "vx":-0.2529, "vy":-0.11547, "omega":-0.00009, "ax":-5.45669, "ay":-2.49145, "alpha":-0.00152, "fx":[-89.20193,-89.20777,-89.21125,-89.20488], "fy":[-40.74035,-40.7275,-40.72003,-40.73404]}, + {"t":0.09271, "x":7.08123, "y":7.5718, "heading":0.0, "vx":-0.50586, "vy":-0.23097, "omega":-0.00016, "ax":-5.45658, "ay":-2.49144, "alpha":-0.00121, "fx":[-89.20112,-89.20514,-89.20884,-89.20357], "fy":[-40.73802,-40.72914,-40.72116,-40.73279]}, + {"t":0.13907, "x":7.05192, "y":7.55842, "heading":-0.00001, "vx":-0.75881, "vy":-0.34646, "omega":-0.00021, "ax":-5.45645, "ay":-2.49142, "alpha":-0.00103, "fx":[-89.19957,-89.20329,-89.20597,-89.20159], "fy":[-40.73664,-40.72846,-40.72267,-40.7323]}, + {"t":0.18543, "x":7.01088, "y":7.53968, "heading":-0.00002, "vx":-1.01176, "vy":-0.46196, "omega":-0.00026, "ax":-5.4563, "ay":-2.49141, "alpha":-0.00073, "fx":[-89.19811,-89.19996,-89.20298,-89.19965], "fy":[-40.73428,-40.73015,-40.72363,-40.73098]}, + {"t":0.23179, "x":6.95811, "y":7.51559, "heading":-0.00003, "vx":-1.2647, "vy":-0.57745, "omega":-0.00029, "ax":-5.45613, "ay":-2.49138, "alpha":-0.00054, "fx":[-89.19576,-89.19734,-89.1993,-89.19688], "fy":[-40.73277,-40.72932,-40.72506,-40.73038]}, + {"t":0.27814, "x":6.89362, "y":7.48614, "heading":-0.00005, "vx":-1.51763, "vy":-0.69295, "omega":-0.00032, "ax":-5.45592, "ay":-2.49136, "alpha":-0.00025, "fx":[-89.19324,-89.19294,-89.19532,-89.1939], "fy":[-40.73032,-40.73096,-40.72579,-40.72892]}, + {"t":0.3245, "x":6.81741, "y":7.45134, "heading":-0.00006, "vx":-1.77055, "vy":-0.80844, "omega":-0.00033, "ax":-5.45566, "ay":-2.49133, "alpha":-0.0002, "fx":[-89.18887,-89.1909,-89.18958,-89.1891], "fy":[-40.73008,-40.72564,-40.72853,-40.72959]}, + {"t":0.37086, "x":6.72947, "y":7.41119, "heading":-0.00008, "vx":-2.02346, "vy":-0.92393, "omega":-0.00034, "ax":-5.45533, "ay":-2.49129, "alpha":0.00024, "fx":[-89.18518,-89.18236,-89.18449,-89.18496], "fy":[-40.72582,-40.73196,-40.72733,-40.72629]}, + {"t":0.41721, "x":6.62981, "y":7.36568, "heading":-0.00009, "vx":-2.27635, "vy":-1.03942, "omega":-0.00033, "ax":-5.45491, "ay":-2.49124, "alpha":0.00021, "fx":[-89.17777,-89.17872,-89.17571,-89.17714], "fy":[-40.72599,-40.72395,-40.7305,-40.72737]}, + {"t":0.46357, "x":6.51842, "y":7.31482, "heading":-0.00011, "vx":-2.52922, "vy":-1.15491, "omega":-0.00032, "ax":-5.45433, "ay":-2.49117, "alpha":0.00068, "fx":[-89.17018,-89.16565,-89.16678,-89.16911], "fy":[-40.72101,-40.73091,-40.72844,-40.72333]}, + {"t":0.50993, "x":6.39531, "y":7.2586, "heading":-0.00012, "vx":-2.78207, "vy":-1.27039, "omega":-0.00029, "ax":-5.45352, "ay":-2.49106, "alpha":0.00106, "fx":[-89.15828,-89.15024,-89.15346,-89.15679], "fy":[-40.71629,-40.73393,-40.72683,-40.71954]}, + {"t":0.55629, "x":6.26048, "y":7.19703, "heading":-0.00014, "vx":-3.03488, "vy":-1.38587, "omega":-0.00024, "ax":-5.45227, "ay":-2.49091, "alpha":0.00125, "fx":[-89.13842,-89.12988,-89.13234,-89.13652], "fy":[-40.71258,-40.73128,-40.7259,-40.71672]}, + {"t":0.60264, "x":6.11394, "y":7.13011, "heading":-0.00015, "vx":-3.28763, "vy":-1.50134, "omega":-0.00018, "ax":-5.45013, "ay":-2.49061, "alpha":0.00086, "fx":[-89.10122,-89.10317,-89.09368,-89.09888], "fy":[-40.71244,-40.70823,-40.72893,-40.71756]}, + {"t":0.649, "x":5.95567, "y":7.05784, "heading":-0.00016, "vx":-3.54028, "vy":-1.6168, "omega":-0.00014, "ax":-5.44555, "ay":-2.48996, "alpha":0.00138, "fx":[-89.02843,-89.02391,-89.01959,-89.0257], "fy":[-40.69726,-40.70708,-40.71661,-40.70326]}, + {"t":0.69536, "x":5.78571, "y":6.98021, "heading":-0.00016, "vx":-3.79272, "vy":-1.73222, "omega":-0.00008, "ax":-5.42958, "ay":-2.48663, "alpha":0.00087, "fx":[-88.76452,-88.77361,-88.75376,-88.76125], "fy":[-40.64897,-40.62897,-40.6726,-40.65627]}, + {"t":0.74171, "x":5.60405, "y":6.89724, "heading":-0.00017, "vx":-4.04442, "vy":-1.8475, "omega":-0.00004, "ax":-1.10748, "ay":-0.47243, "alpha":0.0008, "fx":[-18.17474,-18.17828,-18.03343,-18.03445], "fy":[-7.78819,-7.38333,-7.93239,-7.78926]}, + {"t":0.78807, "x":5.41537, "y":6.81109, "heading":-0.00017, "vx":-4.09576, "vy":-1.8694, "omega":0.0, "ax":-0.03199, "ay":0.06871, "alpha":-0.00001, "fx":[-0.54291,-0.49152,-0.52872,-0.52873], "fy":[1.1289,1.11771,1.11773,1.12891]}, + {"t":0.83443, "x":5.22547, "y":6.7245, "heading":-0.00017, "vx":-4.09725, "vy":-1.86621, "omega":0.0, "ax":-0.00613, "ay":0.01348, "alpha":0.0, "fx":[-0.14762,-0.14762,-0.05267,-0.05264], "fy":[0.17389,0.45446,0.07924,0.17387]}, + {"t":0.88079, "x":5.03553, "y":6.638, "heading":-0.00017, "vx":-4.09753, "vy":-1.86559, "omega":0.0, "ax":0.00315, "ay":-0.00686, "alpha":0.00001, "fx":[0.12154,-0.06823,0.07643,0.07642], "fy":[-0.13643,-0.08776,-0.08774,-0.13641]}, + {"t":0.92714, "x":4.84558, "y":6.55151, "heading":-0.00017, "vx":-4.09738, "vy":-1.8659, "omega":0.0, "ax":0.09822, "ay":-0.2152, "alpha":-0.00002, "fx":[1.54861,1.5487,1.66283,1.66292], "fy":[-3.57413,-3.2377,-3.68647,-3.57412]}, + {"t":0.9735, "x":4.65575, "y":6.46478, "heading":-0.00017, "vx":-4.09283, "vy":-1.87588, "omega":0.0, "ax":0.30035, "ay":-0.64873, "alpha":0.00002, "fx":[4.9811,4.78934,4.93522,4.9351], "fy":[-10.62988,-10.58137,-10.58106,-10.62996]}, + {"t":1.01986, "x":4.46634, "y":6.37713, "heading":-0.00017, "vx":-4.07891, "vy":-1.90595, "omega":0.0, "ax":0.7512, "ay":-1.57054, "alpha":0.0, "fx":[12.31643,12.31176,12.24816,12.24655], "fy":[-25.64198,-25.8435,-25.57271,-25.64348]}, + {"t":1.06621, "x":4.27806, "y":6.28708, "heading":-0.00017, "vx":-4.04408, "vy":-1.97876, "omega":0.0, "ax":1.67323, "ay":-3.256, "alpha":-0.00009, "fx":[27.20844,27.59211,27.30437,27.31183], "fy":[-53.18809,-53.26365,-53.28441,-53.18146]}, + {"t":1.11257, "x":4.09238, "y":6.19186, "heading":-0.00017, "vx":-3.96652, "vy":-2.1297, "omega":0.0, "ax":3.90315, "ay":-3.66968, "alpha":0.02327, "fx":[64.08435,63.44397,63.97183,63.73645], "fy":[-59.79694,-60.5263,-59.62428,-60.02141]}, + {"t":1.15893, "x":3.9127, "y":6.08919, "heading":-0.00017, "vx":-3.78558, "vy":-2.29981, "omega":0.00107, "ax":5.66386, "ay":1.89899, "alpha":-0.00449, "fx":[92.60303,92.59137,92.58169,92.59738], "fy":[31.01731,31.0523,31.07893,31.0309]}, + {"t":1.20529, "x":3.7433, "y":5.98461, "heading":-0.00012, "vx":-3.52302, "vy":-2.21178, "omega":0.00087, "ax":5.57182, "ay":2.19384, "alpha":-0.00471, "fx":[91.10203,91.07032,91.08504,91.09708], "fy":[35.83124,35.91261,35.87372,35.84262]}, + {"t":1.25164, "x":3.58597, "y":5.88444, "heading":-0.00008, "vx":-3.26472, "vy":-2.11008, "omega":0.00065, "ax":5.53648, "ay":2.29263, "alpha":-0.00306, "fx":[90.51961,90.5039,90.50481,90.51518], "fy":[37.45931,37.49734,37.49461,37.46926]}, + {"t":1.298, "x":3.44057, "y":5.78909, "heading":-0.00005, "vx":-3.00807, "vy":-2.0038, "omega":0.00051, "ax":5.51797, "ay":2.34197, "alpha":-0.00315, "fx":[90.21797,90.19614,90.20495,90.21402], "fy":[38.26406,38.31593,38.29445,38.27282]}, + {"t":1.34436, "x":3.30706, "y":5.69871, "heading":-0.00002, "vx":-2.75227, "vy":-1.89524, "omega":0.00036, "ax":5.50656, "ay":2.37163, "alpha":-0.00199, "fx":[90.02742,90.01998,90.01594,90.02389], "fy":[38.75875,38.77611,38.78515,38.76649]}, + {"t":1.39071, "x":3.18539, "y":5.6134, "heading":-0.00001, "vx":-2.497, "vy":-1.78529, "omega":0.00027, "ax":5.49885, "ay":2.39138, "alpha":-0.00236, "fx":[89.90331,89.88609,89.89339,89.90023], "fy":[39.07714,39.11703,39.09974,39.08383]}, + {"t":1.43707, "x":3.07554, "y":5.53321, "heading":0.0, "vx":-2.24209, "vy":-1.67444, "omega":0.00016, "ax":5.49327, "ay":2.40552, "alpha":-0.00143, "fx":[89.80852,89.80352,89.79995,89.80585], "fy":[39.31659,39.32807,39.33596,39.32236]}, + {"t":1.48343, "x":2.97751, "y":5.45817, "heading":0.00001, "vx":-1.98744, "vy":-1.56292, "omega":0.00009, "ax":5.48906, "ay":2.41612, "alpha":-0.00163, "fx":[89.74087,89.7291,89.7338,89.73864], "fy":[39.487,39.51398,39.50292,39.49177]}, + {"t":1.52978, "x":2.89127, "y":5.38832, "heading":0.00002, "vx":-1.73298, "vy":-1.45092, "omega":0.00002, "ax":5.48575, "ay":2.42438, "alpha":-0.0011, "fx":[89.68494,89.6791,89.67919,89.68312], "fy":[39.62646,39.63972,39.63932,39.63033]}, + {"t":1.57614, "x":2.81683, "y":5.32366, "heading":0.00002, "vx":-1.47868, "vy":-1.33853, "omega":-0.00003, "ax":5.4831, "ay":2.43097, "alpha":-0.00114, "fx":[89.64212,89.63237,89.63783,89.64074], "fy":[39.73315,39.75531,39.74269,39.73603]}, + {"t":1.6225, "x":2.75418, "y":5.26422, "heading":0.00002, "vx":-1.2245, "vy":-1.22584, "omega":-0.00009, "ax":5.48092, "ay":2.43639, "alpha":-0.00059, "fx":[89.60443,89.60092,89.60149,89.60346], "fy":[39.8262,39.83411,39.83269,39.82816]}, + {"t":1.66886, "x":2.7033, "y":5.21001, "heading":0.00001, "vx":-0.97042, "vy":-1.1129, "omega":-0.00011, "ax":5.4791, "ay":2.44089, "alpha":-0.00061, "fx":[89.57504,89.5681,89.57353,89.57452], "fy":[39.89891,39.91464,39.90221,39.8999]}, + {"t":1.71521, "x":2.6642, "y":5.16105, "heading":0.00001, "vx":-0.71642, "vy":-0.99974, "omega":-0.00014, "ax":5.47755, "ay":2.44471, "alpha":-0.00009, "fx":[89.54788,89.54659,89.54776,89.54779], "fy":[39.96549,39.9684,39.96569,39.96555]}, + {"t":1.76157, "x":2.63688, "y":5.11733, "heading":0.0, "vx":-0.4625, "vy":-0.88641, "omega":-0.00015, "ax":5.65884, "ay":-1.9843, "alpha":0.00107, "fx":[92.51286,92.50668,92.51279,92.51281], "fy":[-32.43517,-32.45274,-32.43514,-32.43516]}, + {"t":1.80891, "x":2.62132, "y":5.07314, "heading":-0.00001, "vx":-0.1946, "vy":-0.98035, "omega":-0.0001, "ax":6.36356, "ay":-0.28848, "alpha":-0.00001, "fx":[104.0321,104.03211,104.03211,104.0321], "fy":[-4.71614,-4.71607,-4.71607,-4.71614]}, + {"t":1.85626, "x":2.61924, "y":5.0264, "heading":-0.00001, "vx":0.10667, "vy":-0.99401, "omega":-0.0001, "ax":6.09873, "ay":1.5999, "alpha":-0.00002, "fx":[99.70264,99.70258,99.70256,99.70263], "fy":[26.15517,26.15541,26.15547,26.15523]}, + {"t":1.9036, "x":2.63112, "y":4.98114, "heading":-0.00002, "vx":0.3954, "vy":-0.91827, "omega":-0.0001, "ax":3.37542, "ay":1.83354, "alpha":0.00039, "fx":[55.18053,55.18112,55.18284,55.18226], "fy":[29.97642,29.97389,29.97332,29.97584]}, + {"t":1.95094, "x":2.65363, "y":4.93972, "heading":-0.00002, "vx":0.5552, "vy":-0.83147, "omega":-0.00008, "ax":0.09113, "ay":0.06119, "alpha":0.00055, "fx":[1.48829,1.48829,1.49131,1.4913], "fy":[1.00189,0.99881,0.99881,1.00189]}, + {"t":1.99828, "x":2.68001, "y":4.90043, "heading":-0.00002, "vx":0.55951, "vy":-0.82857, "omega":-0.00005, "ax":-0.00078, "ay":-0.00052, "alpha":0.00049, "fx":[-0.01402,-0.01402,-0.01135,-0.01135], "fy":[-0.0072,-0.00993,-0.00993,-0.0072]}, + {"t":2.04562, "x":2.7065, "y":4.8612, "heading":-0.00003, "vx":0.55947, "vy":-0.82859, "omega":-0.00003, "ax":0.00119, "ay":0.00081, "alpha":0.00043, "fx":[0.01833,0.01833,0.02067,0.02067], "fy":[0.01437,0.01197,0.01197,0.01437]}, + {"t":2.09297, "x":2.73299, "y":4.82197, "heading":-0.00003, "vx":0.55953, "vy":-0.82855, "omega":-0.00001, "ax":-0.00022, "ay":-0.00015, "alpha":0.00037, "fx":[-0.00454,-0.00454,-0.00251,-0.00251], "fy":[-0.00135,-0.00342,-0.00342,-0.00135]}, + {"t":2.14031, "x":2.75948, "y":4.78275, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00001, "ax":-0.00008, "ay":-0.00005, "alpha":0.00032, "fx":[-0.00209,-0.00209,-0.00037,-0.00037], "fy":[0.00005,-0.00171,-0.00171,0.00005]}, + {"t":2.18765, "x":2.78597, "y":4.74352, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00002, "ax":0.00003, "ay":0.00002, "alpha":0.00026, "fx":[-0.00018,-0.00018,0.00125,0.00125], "fy":[0.00109,-0.00037,-0.00037,0.00109]}, + {"t":2.23499, "x":2.81245, "y":4.70429, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00004, "ax":0.00001, "ay":0.00001, "alpha":0.00021, "fx":[-0.00041,-0.00041,0.00074,0.00074], "fy":[0.0007,-0.00047,-0.00047,0.0007]}, + {"t":2.28234, "x":2.83894, "y":4.66507, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.00051,-0.00051,0.00036,0.00036], "fy":[0.00039,-0.00049,-0.00049,0.00039]}, + {"t":2.32968, "x":2.86543, "y":4.62584, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[-0.00034,-0.00034,0.00025,0.00025], "fy":[0.00027,-0.00033,-0.00033,0.00027]}, + {"t":2.37702, "x":2.89192, "y":4.58662, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00016,-0.00016,0.00016,0.00016], "fy":[0.00016,-0.00016,-0.00016,0.00016]}, + {"t":2.42436, "x":2.91841, "y":4.54739, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00002,-0.00002,0.00003,0.00003], "fy":[0.00003,-0.00002,-0.00002,0.00003]}, + {"t":2.4717, "x":2.9449, "y":4.50816, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00011,0.00011,-0.00011,-0.00011], "fy":[-0.00011,0.00011,0.00011,-0.00011]}, + {"t":2.51905, "x":2.97139, "y":4.46894, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00024,0.00024,-0.00025,-0.00025], "fy":[-0.00025,0.00024,0.00024,-0.00025]}, + {"t":2.56639, "x":2.99788, "y":4.42971, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00037,0.00037,-0.00039,-0.00039], "fy":[-0.00039,0.00038,0.00038,-0.00039]}, + {"t":2.61373, "x":3.02437, "y":4.39049, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.00057,0.00057,-0.00046,-0.00046], "fy":[-0.00049,0.00057,0.00057,-0.00049]}, + {"t":2.66107, "x":3.05085, "y":4.35126, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00004, "ax":-0.00001, "ay":0.0, "alpha":-0.00024, "fx":[0.00054,0.00054,-0.00078,-0.00078], "fy":[-0.00075,0.00059,0.00059,-0.00075]}, + {"t":2.70842, "x":3.07734, "y":4.31203, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00003, "ax":0.00001, "ay":0.00001, "alpha":-0.0003, "fx":[0.00089,0.00089,-0.00072,-0.00072], "fy":[-0.0006,0.00105,0.00105,-0.0006]}, + {"t":2.75576, "x":3.10383, "y":4.27281, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00002, "ax":-1.09714, "ay":1.6247, "alpha":-0.00032, "fx":[-17.93537,-17.9353,-17.93709,-17.93716], "fy":[26.55992,26.56168,26.56161,26.55984]}, + {"t":2.8031, "x":3.12909, "y":4.2354, "heading":0.0, "vx":0.50758, "vy":-0.75165, "omega":0.0, "ax":-3.56991, "ay":5.28651, "alpha":0.0, "fx":[-58.36125,-58.36126,-58.36124,-58.36123], "fy":[86.42445,86.42444,86.42445,86.42446]}, + {"t":2.85044, "x":3.14912, "y":4.20574, "heading":0.0, "vx":0.33857, "vy":-0.50137, "omega":0.0, "ax":-3.57491, "ay":5.29392, "alpha":0.0, "fx":[-58.44302,-58.44302,-58.44301,-58.44301], "fy":[86.54554,86.54553,86.54554,86.54555]}, + {"t":2.89779, "x":3.16114, "y":4.18794, "heading":0.0, "vx":0.16932, "vy":-0.25074, "omega":0.0, "ax":-3.57658, "ay":5.29639, "alpha":0.0, "fx":[-58.4703,-58.47031,-58.4703,-58.4703], "fy":[86.58595,86.58595,86.58595,86.58596]}, + {"t":2.94513, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 4775089d..93c2b392 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -444,6 +444,7 @@ public Command CMtoGH() { // algae path // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); return routine.cmd(); } + public Command LOtoA() { // 2910 final var routine = factory.newRoutine("LO to A"); bindCoralElevatorExtension(routine, 2.0); @@ -467,21 +468,28 @@ public Command LOtoA() { // 2910 // --------- return routine.cmd(); } - public void runGroundPath( - AutoRoutine routine, - String startPos, - String endPos, - String nextPos, - HashMap steps) { - routine - .observe(steps.get(startPos + "to" + endPos).done()) - .onTrue( - Commands.sequence( - scoreCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose().get()), - Commands.runOnce(() -> autoGroundCoralIntake = true), - steps.get(endPos + "to" + nextPos).cmd())); -} + public void runGroundPath( + AutoRoutine routine, + String startPos, + String endPos, + String nextPos, + HashMap steps) { + routine + .observe( + steps + .get(startPos + "to" + endPos) + .atTime( + steps.get(startPos + "to" + endPos).getRawTrajectory().getTotalTime() + - (endPos.length() == 1 ? 0.3 : 0.0))) + .onTrue( + Commands.sequence( + scoreCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose().get()) + // , + // Commands.runOnce(() -> autoGroundCoralIntake = true), + // steps.get(endPos + "to" + nextPos).cmd() + )); + } public Command scoreCoralInAuto(Supplier trajEndPose) { return Commands.sequence( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f08e6e77..1fe6fbb6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1165,7 +1165,8 @@ private void addAutos() { autoChooser.addOption("4.5 L Inside", autos.LItoK()); autoChooser.addOption("4.5 R Inside", autos.RItoD()); autoChooser.addOption("Push Auto", autos.PMtoPL()); - autoChooser.addDefaultOption("Algae auto", autos.CMtoGH()); + autoChooser.addOption("Algae auto", autos.CMtoGH()); + autoChooser.addOption("2910 auto", autos.LOtoA()); } /** Scales a joystick value for teleop driving */ From bbeece32aeccb0a353ee47428179ab7d20fa948f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 12 Apr 2025 12:34:18 -0700 Subject: [PATCH 046/154] welp i tried --- src/main/deploy/choreo/AtoB.traj | 205 ++++++++++++----------------- src/main/deploy/choreo/LOtoA.traj | 146 ++++++++++---------- src/main/java/frc/robot/Autos.java | 37 ++++-- src/main/java/frc/robot/Robot.java | 2 +- 4 files changed, 187 insertions(+), 203 deletions(-) diff --git a/src/main/deploy/choreo/AtoB.traj b/src/main/deploy/choreo/AtoB.traj index 10ad9e0f..bcf37a9a 100644 --- a/src/main/deploy/choreo/AtoB.traj +++ b/src/main/deploy/choreo/AtoB.traj @@ -3,32 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.6018691062927246, "y":4.209113121032715, "heading":1.610775050878982, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.7247390747070312, "y":4.084929943084717, "heading":3.141592653589793, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.667668342590332, "y":4.032181262969971, "heading":3.141592653589793, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.4166265726089478, "y":4.017168045043945, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.6018691062927246 m", "val":2.6018691062927246}, "y":{"exp":"4.209113121032715 m", "val":4.209113121032715}, "heading":{"exp":"1.610775050878982 rad", "val":1.610775050878982}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7247390747070312 m", "val":2.7247390747070312}, "y":{"exp":"4.084929943084717 m", "val":4.084929943084717}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.667668342590332 m", "val":1.667668342590332}, "y":{"exp":"4.032181262969971 m", "val":4.032181262969971}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.4166265726089478 m", "val":1.4166265726089478}, "y":{"exp":"4.017168045043945 m", "val":4.017168045043945}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,123 +36,80 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.6431,1.21973,1.45584,1.7246,2.85006], + "waypoints":[0.0,0.52985,0.76716,1.03679,2.16108], "samples":[ - {"t":0.0, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.37447, "ay":0.35577, "alpha":0.11969, "fx":[-104.17788,-104.24949,-104.24389,-104.17052], "fy":[6.42746,5.13407,5.19713,6.50629]}, - {"t":0.02297, "x":3.16347, "y":4.1821, "heading":0.0, "vx":-0.14641, "vy":0.00817, "omega":0.00275, "ax":-6.37201, "ay":0.35565, "alpha":0.13604, "fx":[-104.13356,-104.21475,-104.20796,-104.12449], "fy":[6.50811,5.03982,5.11022,6.59887]}, - {"t":0.04594, "x":3.15843, "y":4.18238, "heading":0.00006, "vx":-0.29276, "vy":0.01634, "omega":0.00587, "ax":-6.36833, "ay":0.35547, "alpha":0.16039, "fx":[-104.0676,-104.16299,-104.15422,-104.05567], "fy":[6.62778,4.89981,4.98067,6.73692]}, - {"t":0.0689, "x":3.15002, "y":4.18285, "heading":0.0002, "vx":-0.43902, "vy":0.0245, "omega":0.00956, "ax":-6.36226, "ay":0.35517, "alpha":0.20052, "fx":[-103.95907,-104.07762,-104.06507,-103.94158], "fy":[6.82392,4.67004,4.76682,6.9648]}, - {"t":0.09187, "x":3.13826, "y":4.1835, "heading":0.00042, "vx":-0.58515, "vy":0.03266, "omega":0.01416, "ax":-6.3503, "ay":0.35458, "alpha":0.27909, "fx":[-103.74713,-103.91023,-103.88838,-103.71575], "fy":[7.20427,4.22379,4.34665,7.41224]}, - {"t":0.11484, "x":3.12315, "y":4.18435, "heading":0.00074, "vx":-0.731, "vy":0.04081, "omega":0.02057, "ax":-6.31591, "ay":0.35287, "alpha":0.50218, "fx":[-103.14999,-103.43392,-103.37149,-103.05713], "fy":[8.25795,2.98385,3.1435,8.69001]}, - {"t":0.13781, "x":3.10469, "y":4.18538, "heading":0.00122, "vx":-0.87607, "vy":0.04891, "omega":0.03211, "ax":-5.30787, "ay":0.29232, "alpha":6.01403, "fx":[-91.4594,-93.03418,-83.2136,-79.38706], "fy":[25.00769,-17.32721,-28.10325,39.53826]}, - {"t":0.16077, "x":3.08317, "y":4.18658, "heading":0.00195, "vx":-0.99798, "vy":0.05562, "omega":0.17024, "ax":-0.00174, "ay":0.00207, "alpha":19.17611, "fx":[-52.47809,-52.30427,52.45621,52.21222], "fy":[53.40592,-53.54289,-53.36712,53.63915]}, - {"t":0.18374, "x":3.06025, "y":4.18786, "heading":0.00586, "vx":-0.99802, "vy":0.05567, "omega":0.61067, "ax":0.00003, "ay":0.00054, "alpha":18.68349, "fx":[-51.31744,-50.71507,51.32711,50.70736], "fy":[51.82391,-52.40441,-51.80564,52.42154]}, - {"t":0.20671, "x":3.03733, "y":4.18913, "heading":0.01989, "vx":-0.99802, "vy":0.05568, "omega":1.03979, "ax":0.00001, "ay":0.00014, "alpha":18.13965, "fx":[-50.52785,-48.51752,50.53027,48.51563], "fy":[49.6052,-51.57065,-49.60033,51.57511]}, - {"t":0.22968, "x":3.0144, "y":4.19041, "heading":0.04377, "vx":-0.99802, "vy":0.05569, "omega":1.45641, "ax":0.0, "ay":0.00004, "alpha":17.53746, "fx":[-49.98257,-45.70215,49.98313,45.70172], "fy":[46.77648,-50.96612,-46.77525,50.9672]}, - {"t":0.25265, "x":2.99148, "y":4.19169, "heading":0.07722, "vx":-0.99802, "vy":0.05569, "omega":1.85921, "ax":0.0, "ay":0.00001, "alpha":16.86878, "fx":[-49.55485,-42.29525,49.55495,42.29518], "fy":[43.35939,-50.46602,-43.35915,50.46622]}, - {"t":0.27561, "x":2.96856, "y":4.19297, "heading":0.11992, "vx":-0.99802, "vy":0.05569, "omega":2.24665, "ax":0.0, "ay":0.0, "alpha":16.1244, "fx":[-49.09427,-38.33269,49.09425,38.3327], "fy":[39.38603,-49.92108,-39.38607,49.92103]}, - {"t":0.29858, "x":2.94564, "y":4.19425, "heading":0.17152, "vx":-0.99802, "vy":0.05569, "omega":2.61699, "ax":0.0, "ay":0.0, "alpha":15.29407, "fx":[-48.43103,-33.86812,48.43098,33.86816], "fy":[34.90628,-49.16272,-34.90646,49.16258]}, - {"t":0.32155, "x":2.92271, "y":4.19553, "heading":0.23163, "vx":-0.99802, "vy":0.05569, "omega":2.96826, "ax":0.0, "ay":-0.00001, "alpha":14.36661, "fx":[-47.38163,-28.98272,47.38156,28.98277], "fy":[29.9974,-48.00914,-29.99768,48.00894]}, - {"t":0.34452, "x":2.89979, "y":4.19681, "heading":0.2998, "vx":-0.99802, "vy":0.05569, "omega":3.29823, "ax":0.0, "ay":-0.00001, "alpha":13.33021, "fx":[-45.75745,-23.79492,45.75738,23.79497], "fy":[24.77383,-46.27424,-24.77418,46.27398]}, - {"t":0.36748, "x":2.87687, "y":4.19809, "heading":0.37556, "vx":-0.99802, "vy":0.05569, "omega":3.60439, "ax":0.0, "ay":-0.00001, "alpha":12.17302, "fx":[-43.37758,-18.46894,43.37752,18.46897], "fy":[19.39598,-43.78051,-19.39636,43.78023]}, - {"t":0.39045, "x":2.85395, "y":4.19937, "heading":0.45834, "vx":-0.99802, "vy":0.05569, "omega":3.88398, "ax":0.0, "ay":-0.00001, "alpha":10.88424, "fx":[-40.08637,-13.22012,40.08634,13.22014], "fy":[14.07597,-40.37686,-14.07627,40.37662]}, - {"t":0.41342, "x":2.83103, "y":4.20065, "heading":0.54755, "vx":-0.99802, "vy":0.05569, "omega":4.13397, "ax":0.0, "ay":0.0, "alpha":9.45574, "fx":[-35.77619,-8.3144,35.77619,8.31441], "fy":[9.0775,-35.96126,-9.07763,35.96116]}, - {"t":0.43639, "x":2.8081, "y":4.20193, "heading":0.6425, "vx":-0.99802, "vy":0.05569, "omega":4.35115, "ax":0.0, "ay":0.0, "alpha":7.88428, "fx":[-30.4137,-4.05867,30.41372,4.05868], "fy":[4.70682,-30.50691,-4.70671,30.50701]}, - {"t":0.45936, "x":2.78518, "y":4.2032, "heading":0.74243, "vx":-0.99802, "vy":0.05569, "omega":4.53223, "ax":0.0, "ay":0.0, "alpha":6.17426, "fx":[-24.06621,-0.77899,24.06622,0.779], "fy":[1.29128,-24.08823,-1.29127,24.08824]}, - {"t":0.48232, "x":2.76226, "y":4.20448, "heading":0.84653, "vx":-0.99802, "vy":0.05569, "omega":4.67404, "ax":0.0, "ay":-0.00008, "alpha":4.34051, "fx":[-16.92137,1.21482,16.92123,-1.21498], "fy":[-0.85653,-16.90063,0.85368,16.89793]}, - {"t":0.50529, "x":2.73934, "y":4.20576, "heading":0.95388, "vx":-0.99802, "vy":0.05569, "omega":4.77373, "ax":-0.00005, "ay":-0.00082, "alpha":2.41003, "fx":[-9.29113,1.67526,9.28973,-1.67683], "fy":[-1.49221,-9.27014,1.46526,9.24359]}, - {"t":0.52826, "x":2.71641, "y":4.20704, "heading":1.06352, "vx":-0.99802, "vy":0.05567, "omega":4.82908, "ax":-0.00038, "ay":-0.00679, "alpha":0.42162, "fx":[-1.59342,0.46246,1.58108,-0.47486], "fy":[-0.54604,-1.68864,0.32394,1.46662]}, - {"t":0.55123, "x":2.69349, "y":4.20832, "heading":1.17443, "vx":-0.99803, "vy":0.05551, "omega":4.83877, "ax":-0.00302, "ay":-0.05486, "alpha":-1.57742, "fx":[5.67514,-2.44778,-5.76916,2.34454], "fy":[1.37557,4.77692,-3.17345,-6.56642]}, - {"t":0.57419, "x":2.67057, "y":4.20958, "heading":1.28557, "vx":-0.99809, "vy":0.05425, "omega":4.80254, "ax":-0.02121, "ay":-0.42953, "alpha":-3.50787, "fx":[11.89237,-7.18222,-12.37596,6.27882], "fy":[-0.67914,4.93109,-13.48359,-18.85667]}, - {"t":0.59716, "x":2.64764, "y":4.21071, "heading":1.39587, "vx":-0.99858, "vy":0.04439, "omega":4.72197, "ax":-0.03524, "ay":-2.74147, "alpha":-3.80169, "fx":[14.79398,-11.87691,-13.95872,8.73693], "fy":[-38.24697,-34.80196,-51.70822,-54.51381]}, - {"t":0.62013, "x":2.6247, "y":4.21101, "heading":1.50433, "vx":-0.99939, "vy":-0.01858, "omega":4.63465, "ax":0.48388, "ay":-5.56312, "alpha":-1.0493, "fx":[13.83436,3.37209,2.43482,12.00069], "fy":[-89.8387,-90.59438,-91.96375,-91.38867]}, - {"t":0.6431, "x":2.60187, "y":4.20911, "heading":1.61078, "vx":-0.98828, "vy":-0.14635, "omega":4.61055, "ax":1.11518, "ay":-5.48023, "alpha":-0.9287, "fx":[23.34321,14.26096,13.45897,21.86136], "fy":[-88.03068,-89.72896,-91.03964,-89.56608]}, - {"t":0.66369, "x":2.58175, "y":4.20494, "heading":1.70573, "vx":-0.96531, "vy":-0.25921, "omega":4.59143, "ax":0.98255, "ay":-3.21186, "alpha":-3.7997, "fx":[30.21085,1.52074,4.4663,28.05371], "fy":[-41.87615,-47.85148,-62.57908,-57.72468]}, - {"t":0.68429, "x":2.56208, "y":4.19892, "heading":1.80028, "vx":-0.94508, "vy":-0.32536, "omega":4.51318, "ax":0.24919, "ay":-0.70616, "alpha":-6.31426, "fx":[18.30591,-17.80656,-8.79906,24.59507], "fy":[9.20817,0.82019,-32.11962,-24.08631]}, - {"t":0.70488, "x":2.54267, "y":4.19207, "heading":1.89323, "vx":-0.93994, "vy":-0.3399, "omega":4.38314, "ax":0.04443, "ay":-0.12234, "alpha":-6.505, "fx":[12.43735,-22.23621,-10.74513,23.4491], "fy":[20.62884,9.05096,-24.55575,-13.12434]}, - {"t":0.72547, "x":2.52332, "y":4.18504, "heading":1.98349, "vx":-0.93903, "vy":-0.34242, "omega":4.24918, "ax":0.00769, "ay":-0.02107, "alpha":-6.57242, "fx":[9.74105,-23.88579,-9.4505,24.09799], "fy":[23.45848,8.7323,-24.12719,-9.44136]}, - {"t":0.74607, "x":2.50399, "y":4.17799, "heading":2.071, "vx":-0.93887, "vy":-0.34285, "omega":4.11382, "ax":0.00133, "ay":-0.00363, "alpha":-6.66423, "fx":[7.60862,-25.02049,-7.55904,25.05764], "fy":[24.82634,6.99098,-24.94036,-7.11446]}, - {"t":0.76666, "x":2.48465, "y":4.17092, "heading":2.15572, "vx":-0.93884, "vy":-0.34293, "omega":3.97658, "ax":0.00023, "ay":-0.00062, "alpha":-6.78376, "fx":[5.55298,-26.00099,-5.54458,26.00749], "fy":[25.88235,4.98597,-25.90176,-5.00741]}, - {"t":0.78726, "x":2.46532, "y":4.16386, "heading":2.23762, "vx":-0.93884, "vy":-0.34294, "omega":3.83687, "ax":0.00004, "ay":-0.00011, "alpha":-6.92994, "fx":[3.48637,-26.89258,-3.48499,26.89369], "fy":[26.82344,2.91239,-26.82667,-2.91603]}, - {"t":0.80785, "x":2.44598, "y":4.1568, "heading":2.31663, "vx":-0.93884, "vy":-0.34294, "omega":3.69416, "ax":0.00001, "ay":-0.00002, "alpha":-7.10161, "fx":[1.3911,-27.709,-1.3909,27.70916], "fy":[27.68552,0.80142,-27.68599,-0.80196]}, - {"t":0.82844, "x":2.42665, "y":4.14974, "heading":2.39271, "vx":-0.93884, "vy":-0.34294, "omega":3.54791, "ax":0.0, "ay":0.0, "alpha":-7.29759, "fx":[-0.73705,-28.45399,0.73706,28.45399], "fy":[28.4761,-1.34271,-28.47612,1.34268]}, - {"t":0.84904, "x":2.40731, "y":4.14267, "heading":2.46578, "vx":-0.93884, "vy":-0.34294, "omega":3.39762, "ax":0.0, "ay":0.0, "alpha":-7.51666, "fx":[-2.89835,-29.12903,2.89834,29.12902], "fy":[29.19731,-3.51882,-29.19728,3.51885]}, - {"t":0.86963, "x":2.38798, "y":4.13561, "heading":2.53575, "vx":-0.93884, "vy":-0.34294, "omega":3.24282, "ax":0.0, "ay":0.0, "alpha":-7.75759, "fx":[-5.09065,-29.73524,5.09064,29.73523], "fy":[29.85029,-5.72453,-29.85028,5.72454]}, - {"t":0.89023, "x":2.36864, "y":4.12855, "heading":2.60253, "vx":-0.93884, "vy":-0.34294, "omega":3.08306, "ax":0.0, "ay":0.0, "alpha":-8.01908, "fx":[-7.30966,-30.27411,7.30966,30.27412], "fy":[30.4365,-7.95553,-30.43652,7.95551]}, - {"t":0.91082, "x":2.34931, "y":4.12149, "heading":2.66602, "vx":-0.93884, "vy":-0.34294, "omega":2.91791, "ax":0.0, "ay":0.0, "alpha":-8.29977, "fx":[-9.54907,-30.74821,9.54908,30.74822], "fy":[30.95835,-10.20556,-30.95839,10.2055]}, - {"t":0.93141, "x":2.32998, "y":4.11442, "heading":2.72611, "vx":-0.93884, "vy":-0.34294, "omega":2.74699, "ax":0.0, "ay":0.0, "alpha":-8.59826, "fx":[-11.80071,-31.16156,11.80073,31.16159], "fy":[31.41971,-12.46651,-31.41976,12.46645]}, - {"t":0.95201, "x":2.31064, "y":4.10736, "heading":2.78269, "vx":-0.93884, "vy":-0.34294, "omega":2.56991, "ax":0.0, "ay":0.0, "alpha":-8.91307, "fx":[-14.05477,-31.52014,14.05479,31.52016], "fy":[31.82633,-14.72871,-31.82638,14.72865]}, - {"t":0.9726, "x":2.29131, "y":4.1003, "heading":2.83561, "vx":-0.93884, "vy":-0.34294, "omega":2.38636, "ax":0.0, "ay":0.0, "alpha":-9.24269, "fx":[-16.30004,-31.8321,16.30004,31.83209], "fy":[32.18615,-16.98109,-32.18615,16.98108]}, - {"t":0.9932, "x":2.27197, "y":4.09324, "heading":2.88476, "vx":-0.93884, "vy":-0.34294, "omega":2.19601, "ax":0.0, "ay":0.00001, "alpha":-9.58558, "fx":[-18.52417,-32.10807,18.52409,32.10794], "fy":[32.50957,-19.2114,-32.50932,19.21171]}, - {"t":1.01379, "x":2.25264, "y":4.08617, "heading":2.92998, "vx":-0.93884, "vy":-0.34294, "omega":1.99861, "ax":-0.00002, "ay":0.00005, "alpha":-9.94015, "fx":[-20.71402,-32.36137,20.71357,32.36055], "fy":[32.80983,-21.40614,-32.80827,21.40805]}, - {"t":1.03439, "x":2.2333, "y":4.07911, "heading":2.97114, "vx":-0.93884, "vy":-0.34294, "omega":1.7939, "ax":-0.0001, "ay":0.00028, "alpha":-10.30482, "fx":[-22.85617,-32.60878,22.85396,32.60436], "fy":[33.10437,-23.54908,-33.0962,23.55907]}, - {"t":1.05498, "x":2.21397, "y":4.07205, "heading":3.00808, "vx":-0.93884, "vy":-0.34294, "omega":1.58168, "ax":-0.00051, "ay":0.0014, "alpha":-10.67799, "fx":[-24.9386,-32.87434,24.92824,32.85134], "fy":[33.42144,-25.61329,-33.38023,25.66341]}, - {"t":1.07557, "x":2.19463, "y":4.06499, "heading":3.04066, "vx":-0.93885, "vy":-0.34291, "omega":1.36178, "ax":-0.00251, "ay":0.00688, "alpha":-11.05809, "fx":[-26.95742,-33.20844,26.91016,33.09145], "fy":[33.83227,-27.52214,-33.6287,27.76837]}, - {"t":1.09617, "x":2.1753, "y":4.05792, "heading":3.0687, "vx":-0.9389, "vy":-0.34277, "omega":1.13405, "ax":-0.01214, "ay":0.03328, "alpha":-11.44308, "fx":[-28.94482,-33.78116,28.73465,33.19774], "fy":[34.60248,-28.96376,-33.6145,30.15204]}, - {"t":1.11676, "x":2.15596, "y":4.05087, "heading":3.09206, "vx":-0.93915, "vy":-0.34208, "omega":0.89839, "ax":-0.05732, "ay":0.15822, "alpha":-11.81935, "fx":[-31.06693,-35.31582,30.16257,32.47192], "fy":[36.85688,-28.50036,-32.14356,34.13352]}, - {"t":1.13736, "x":2.13661, "y":4.04386, "heading":3.11056, "vx":-0.94033, "vy":-0.33882, "omega":0.65498, "ax":-0.25957, "ay":0.73909, "alpha":-11.92141, "fx":[-33.7054,-40.75283,29.90925,27.57507], "fy":[44.8117,-18.91953,-22.5108,44.94967]}, - {"t":1.15795, "x":2.11719, "y":4.03704, "heading":3.12405, "vx":-0.94568, "vy":-0.3236, "omega":0.40947, "ax":-0.94809, "ay":3.10725, "alpha":-8.0478, "fx":[-34.92718,-50.02994,13.65593,9.30341], "fy":[65.91283,30.10913,36.37127,70.79697]}, - {"t":1.17854, "x":2.09751, "y":4.03104, "heading":3.13248, "vx":-0.9652, "vy":-0.25961, "omega":0.24373, "ax":-1.13595, "ay":5.46041, "alpha":-2.18115, "fx":[-25.97417,-31.62122,-9.30922,-7.3778], "fy":[89.68269,85.16752,89.56231,92.65658]}, - {"t":1.19914, "x":2.07739, "y":4.02685, "heading":3.1375, "vx":-0.9886, "vy":-0.14716, "omega":0.19881, "ax":-0.53658, "ay":6.07458, "alpha":-0.81954, "fx":[-12.52412,-13.65558,-4.66182,-4.24676], "fy":[99.22551,98.66377,99.45242,99.88968]}, - {"t":1.21973, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-0.99965, "vy":-0.02206, "omega":0.18193, "ax":-6.34649, "ay":0.6032, "alpha":-0.11968, "fx":[-103.70039,-103.8213,-103.80702,-103.68314], "fy":[10.44354,9.15915,9.27123,10.57104]}, - {"t":1.24334, "x":2.03155, "y":4.02475, "heading":-3.1373, "vx":-1.14949, "vy":-0.00782, "omega":0.17911, "ax":-6.34332, "ay":0.59602, "alpha":-0.19683, "fx":[-103.6177,-103.81235,-103.78876,-103.58603], "fy":[10.69328,8.59079,8.77345,10.91752]}, - {"t":1.26695, "x":2.00264, "y":4.02473, "heading":-3.13307, "vx":-1.29926, "vy":0.00626, "omega":0.17446, "ax":-6.3387, "ay":0.58594, "alpha":-0.30497, "fx":[-103.50245,-103.79502,-103.75862,-103.44661], "fy":[11.03577,7.79893,8.07253,11.40889]}, - {"t":1.29056, "x":1.9702, "y":4.02504, "heading":-3.12895, "vx":-1.44893, "vy":0.02009, "omega":0.16726, "ax":-6.33136, "ay":0.57079, "alpha":-0.46754, "fx":[-103.33043,-103.75855,-103.70346,-103.22998], "fy":[11.53738,6.62227,7.00889,12.15703]}, - {"t":1.31417, "x":1.93422, "y":4.02568, "heading":-3.125, "vx":-1.59841, "vy":0.03357, "omega":0.15622, "ax":-6.318, "ay":0.54547, "alpha":-0.73969, "fx":[-103.04567,-103.67069,-103.58405,-102.84863], "fy":[12.3475,4.69564,5.19735,13.42907]}, - {"t":1.33779, "x":1.89472, "y":4.02662, "heading":-3.12131, "vx":-1.74759, "vy":0.04645, "omega":0.13876, "ax":-6.28701, "ay":0.49457, "alpha":-1.28993, "fx":[-102.4831,-103.40156,-103.23243,-102.00565], "fy":[13.88667,0.98168,1.414,16.05855]}, - {"t":1.3614, "x":1.85171, "y":4.02786, "heading":-3.11804, "vx":-1.89603, "vy":0.05812, "omega":0.1083, "ax":-6.15711, "ay":0.34034, "alpha":-3.01694, "fx":[-100.85972,-102.00983,-101.02889,-98.72977], "fy":[17.96481,-9.03166,-11.27696,24.59949]}, - {"t":1.38501, "x":1.80522, "y":4.02932, "heading":-3.11548, "vx":-2.0414, "vy":0.06616, "omega":0.03707, "ax":0.16103, "ay":-0.62745, "alpha":-23.96208, "fx":[-74.192,-53.5957,62.73079,75.58684], "fy":[55.62923,-79.21608,-72.74262,55.29874]}, - {"t":1.40862, "x":1.75707, "y":4.03071, "heading":-3.1146, "vx":-2.0376, "vy":0.05134, "omega":-0.52869, "ax":6.07287, "ay":-0.88295, "alpha":-3.63047, "fx":[101.50877,94.26545,98.90885,102.43639], "fy":[4.25846,-38.56098,-27.26035,3.82452]}, - {"t":1.43223, "x":1.71065, "y":4.03168, "heading":-3.12709, "vx":-1.89421, "vy":0.0305, "omega":-0.61441, "ax":6.24082, "ay":-0.774, "alpha":-1.96634, "fx":[103.17388,100.10138,101.38786,103.43876], "fy":[-2.66872,-25.31922,-20.7118,-1.91378]}, - {"t":1.45584, "x":1.66767, "y":4.03218, "heading":3.14159, "vx":-1.74686, "vy":0.01222, "omega":-0.66084, "ax":6.26666, "ay":-0.65809, "alpha":2.68746, "fx":[101.79209,104.04901,103.86737,100.08361], "fy":[-21.81125,2.99499,3.92326,-28.14104]}, - {"t":1.48943, "x":1.61252, "y":4.03222, "heading":3.11939, "vx":-1.53633, "vy":-0.00989, "omega":-0.57055, "ax":6.14578, "ay":-0.54097, "alpha":4.92427, "fx":[100.60953,103.55047,101.99589,95.73146], "fy":[-27.39213,12.19047,20.73578,-40.90961]}, - {"t":1.52303, "x":1.56437, "y":4.03158, "heading":3.10022, "vx":-1.32986, "vy":-0.02806, "omega":-0.40512, "ax":6.03479, "ay":-0.47644, "alpha":6.28344, "fx":[99.83532,103.09383,99.35504,92.34478], "fy":[-30.36762,16.13459,31.33847,-48.26076]}, - {"t":1.55663, "x":1.5231, "y":4.03037, "heading":3.08661, "vx":-1.12712, "vy":-0.04407, "omega":-0.19402, "ax":5.94597, "ay":-0.43863, "alpha":7.19145, "fx":[99.29796,102.78828,96.95899,89.77576], "fy":[-32.23253,18.25727,38.29824,-53.00634]}, - {"t":1.59022, "x":1.48859, "y":4.02864, "heading":3.08009, "vx":-0.92736, "vy":-0.0588, "omega":0.04758, "ax":5.87616, "ay":-0.41372, "alpha":7.83602, "fx":[98.94025,102.55915,94.99777,87.7588], "fy":[-33.41042,19.6709,43.05148,-56.3658]}, - {"t":1.62382, "x":1.46075, "y":4.02643, "heading":3.08169, "vx":-0.72995, "vy":-0.0727, "omega":0.31083, "ax":5.82088, "ay":-0.39353, "alpha":8.3154, "fx":[98.74303,102.3484,93.44787,86.10204], "fy":[-34.05589,20.84976,46.39975,-58.92755]}, - {"t":1.65741, "x":1.43951, "y":4.02377, "heading":3.09214, "vx":-0.53439, "vy":-0.08592, "omega":0.59019, "ax":5.77674, "ay":-0.37306, "alpha":8.68358, "fx":[98.69967,102.11689,92.24967,84.68852], "fy":[-34.2314,22.03203,48.79476,-60.99064]}, - {"t":1.69101, "x":1.42482, "y":4.02067, "heading":3.11196, "vx":-0.34032, "vy":-0.09846, "omega":0.88192, "ax":5.7417, "ay":-0.34911, "alpha":8.9686, "fx":[98.80671,101.83625,91.35928,83.46098], "fy":[-33.96066,23.34957,50.48244,-62.70045]}, - {"t":1.7246, "x":1.41663, "y":4.01717, "heading":3.14159, "vx":-0.14742, "vy":-0.11018, "omega":1.18323, "ax":5.84895, "ay":-0.34164, "alpha":8.11487, "fx":[99.58595,101.95077,94.31104,86.62908], "fy":[-31.54245,22.75429,44.64852,-58.20132]}, - {"t":1.75274, "x":1.41479, "y":4.01393, "heading":-3.1083, "vx":0.01715, "vy":-0.1198, "omega":1.41155, "ax":5.85595, "ay":-0.32389, "alpha":8.05202, "fx":[99.93674,101.70968,94.72387,86.56412], "fy":[-30.3925,23.77571,43.72234,-58.28518]}, - {"t":1.78088, "x":1.41759, "y":4.01043, "heading":-3.06858, "vx":0.18191, "vy":-0.12891, "omega":1.63811, "ax":5.8658, "ay":-0.30419, "alpha":7.95882, "fx":[100.33878,101.41521,95.2763,86.54847], "fy":[-29.01416,24.96568,42.45196,-58.29544]}, - {"t":1.80901, "x":1.42503, "y":4.00669, "heading":-3.02249, "vx":0.34695, "vy":-0.13747, "omega":1.86204, "ax":5.87926, "ay":-0.2836, "alpha":7.82603, "fx":[100.78322,101.06565,95.99512,86.61506], "fy":[-27.40259,26.30506,40.73493,-58.1829]}, - {"t":1.83715, "x":1.43712, "y":4.00271, "heading":-2.9701, "vx":0.51237, "vy":-0.14545, "omega":2.08224, "ax":5.89698, "ay":-0.26359, "alpha":7.64519, "fx":[101.25899,100.661,96.90156,86.79597], "fy":[-25.55452,27.76895,38.44685,-57.89804]}, - {"t":1.86529, "x":1.45387, "y":3.99851, "heading":-2.91152, "vx":0.67829, "vy":-0.15287, "omega":2.29734, "ax":5.91931, "ay":-0.24608, "alpha":7.41059, "fx":[101.75264,100.20402,98.00128,87.11995], "fy":[-23.46932,29.32541,35.44528,-57.39305]}, - {"t":1.89342, "x":1.4753, "y":3.99411, "heading":-2.84688, "vx":0.84484, "vy":-0.15979, "omega":2.50585, "ax":5.9461, "ay":-0.23331, "alpha":7.12195, "fx":[102.24837,99.70158,99.26953,87.60987], "fy":[-21.15016,30.93404,31.58259,-56.62341]}, - {"t":1.92156, "x":1.50143, "y":3.98952, "heading":-2.77637, "vx":1.01214, "vy":-0.16635, "omega":2.70624, "ax":5.97637, "ay":-0.22748, "alpha":6.78761, "fx":[102.72837,99.16623,100.63353,88.28088], "fy":[-18.60505,32.54426,26.73415,-55.54863]}, - {"t":1.94969, "x":1.53227, "y":3.98475, "heading":-2.70023, "vx":1.1803, "vy":-0.17275, "omega":2.89722, "ax":6.0082, "ay":-0.22998, "alpha":6.42707, "fx":[103.17335,98.61807,101.95961,89.13943], "fy":[-15.84776,34.09292,20.84768,-54.13179]}, - {"t":1.97783, "x":1.56786, "y":3.9798, "heading":-2.61871, "vx":1.34935, "vy":-0.17922, "omega":3.07805, "ax":6.03884, "ay":-0.24045, "alpha":6.0703, "fx":[103.56332,98.08694,103.06011,90.18351], "fy":[-12.89859,35.50057,14.01139,-52.33728]}, - {"t":2.00597, "x":1.60821, "y":3.97466, "heading":-2.53211, "vx":1.51926, "vy":-0.18599, "omega":3.24885, "ax":6.06547, "ay":-0.25578, "alpha":5.75089, "fx":[103.87854,97.61539,103.73736,91.40433], "fy":[-9.78515,36.66481,6.52035,-50.12594]}, - {"t":2.0341, "x":1.65336, "y":3.96933, "heading":-2.4407, "vx":1.68992, "vy":-0.19319, "omega":3.41066, "ax":6.08661, "ay":-0.26977, "alpha":5.49156, "fx":[104.1005,97.26258,103.86554,92.78911], "fy":[-6.54403,37.44799,-1.09906,-47.44604]}, - {"t":2.06224, "x":1.70332, "y":3.96379, "heading":-2.34473, "vx":1.86117, "vy":-0.20078, "omega":3.56517, "ax":6.1035, "ay":-0.27413, "alpha":5.28583, "fx":[104.21278,97.10963,103.4755,94.32445], "fy":[-3.22523,37.65441,-8.13772,-44.2173]}, - {"t":2.09038, "x":1.7581, "y":3.95803, "heading":-2.24442, "vx":2.0329, "vy":-0.20849, "omega":3.71389, "ax":6.12081, "ay":-0.2602, "alpha":5.0831, "fx":[104.20223,97.26558,102.78657,95.99985], "fy":[0.09645,36.98758,-13.7993,-40.30002]}, - {"t":2.11851, "x":1.81772, "y":3.95206, "heading":-2.13993, "vx":2.20512, "vy":-0.21581, "omega":3.85691, "ax":6.14593, "ay":-0.22039, "alpha":4.78005, "fx":[104.06077,97.8687,102.1573,97.81044], "fy":[3.29282,34.96852,-17.24695,-35.42622]}, - {"t":2.14665, "x":1.8822, "y":3.9459, "heading":-2.03141, "vx":2.37805, "vy":-0.22201, "omega":3.99141, "ax":6.18693, "ay":-0.14688, "alpha":4.20739, "fx":[103.78899,99.0617,101.97371,99.75373], "fy":[6.10371,30.7736,-17.46935,-29.01278]}, - {"t":2.17479, "x":1.95156, "y":3.93959, "heading":-1.9191, "vx":2.55212, "vy":-0.22615, "omega":4.10979, "ax":6.24656, "ay":-0.02315, "alpha":3.07665, "fx":[103.39986,100.85667,102.44938,101.77166], "fy":[7.86962,22.90384,-12.77133,-19.51588]}, - {"t":2.20292, "x":2.02584, "y":3.93322, "heading":-1.80347, "vx":2.72788, "vy":-0.2268, "omega":4.19635, "ax":6.28969, "ay":0.21549, "alpha":0.72674, "fx":[102.853,102.56343,102.8809,103.00072], "fy":[6.16031,8.55121,0.62284,-1.24292]}, - {"t":2.23106, "x":2.10508, "y":3.92693, "heading":-1.6854, "vx":2.90485, "vy":-0.22073, "omega":4.2168, "ax":5.74498, "ay":0.80567, "alpha":-6.76842, "fx":[96.96588,100.63435,97.33981,80.73813], "fy":[-22.06121,-16.99721,31.40199,60.34103]}, - {"t":2.25919, "x":2.18909, "y":3.92103, "heading":-1.56675, "vx":3.06649, "vy":-0.19806, "omega":4.02636, "ax":3.85197, "ay":0.5714, "alpha":-16.28546, "fx":[43.40187,96.193,87.35228,24.94228], "fy":[-78.05493,-26.09701,48.91593,92.60089]}, - {"t":2.28733, "x":2.27689, "y":3.91569, "heading":-1.45346, "vx":3.17487, "vy":-0.18199, "omega":3.56815, "ax":-3.37927, "ay":0.4614, "alpha":3.56819, "fx":[-48.03291,-62.64727,-62.42794,-47.87048], "fy":[23.14719,16.59389,-5.8208,-3.74803]}, - {"t":2.31547, "x":2.36488, "y":3.91075, "heading":-1.35307, "vx":3.07979, "vy":-0.16901, "omega":3.66854, "ax":-5.40396, "ay":-0.53171, "alpha":8.4829, "fx":[-84.86835,-99.43312,-92.19567,-76.88095], "fy":[46.77274,14.0839,-39.30457,-56.32211]}, - {"t":2.3436, "x":2.4494, "y":3.90578, "heading":-1.24985, "vx":2.92774, "vy":-0.18397, "omega":3.90722, "ax":-6.19669, "ay":-0.20555, "alpha":1.93814, "fx":[-101.19688,-102.18148,-100.93308,-100.90468], "fy":[10.49715,2.79108,-15.43374,-11.2958]}, - {"t":2.37174, "x":2.52932, "y":3.90053, "heading":-1.13991, "vx":2.75339, "vy":-0.18975, "omega":3.96175, "ax":-6.21522, "ay":0.09789, "alpha":-3.20587, "fx":[-101.39625,-102.35439,-99.59851,-103.07893], "fy":[-19.60826,-8.6776,26.46207,8.22517]}, - {"t":2.39988, "x":2.60433, "y":3.89523, "heading":-1.02845, "vx":2.57852, "vy":-0.187, "omega":3.87155, "ax":-6.10736, "ay":0.18993, "alpha":-4.96891, "fx":[-99.04277,-102.60906,-94.2778,-103.44489], "fy":[-30.79522,-9.02289,42.91148,9.32661]}, - {"t":2.42801, "x":2.67446, "y":3.89004, "heading":-0.91951, "vx":2.40668, "vy":-0.18165, "omega":3.73175, "ax":-6.04357, "ay":0.25138, "alpha":-5.68773, "fx":[-96.84219,-103.16534,-91.38059,-103.81529], "fy":[-37.70635,-2.46118,49.38608,7.21963]}, - {"t":2.45615, "x":2.73979, "y":3.88503, "heading":-0.81452, "vx":2.23663, "vy":-0.17458, "omega":3.57171, "ax":-5.9975, "ay":0.33053, "alpha":-6.12659, "fx":[-94.82038,-102.99433,-90.27874,-104.09709], "fy":[-42.81544,8.59929,51.71806,4.11234]}, - {"t":2.48429, "x":2.80034, "y":3.88025, "heading":-0.71402, "vx":2.06789, "vy":-0.16528, "omega":3.39933, "ax":-5.94314, "ay":0.42188, "alpha":-6.61688, "fx":[-92.97124,-101.19705,-90.22083,-104.24721], "fy":[-46.85597,21.74233,52.04059,0.66053]}, - {"t":2.51242, "x":2.85617, "y":3.87576, "heading":-0.61838, "vx":1.90067, "vy":-0.15341, "omega":3.21316, "ax":-5.87319, "ay":0.50157, "alpha":-7.24226, "fx":[-91.31032,-97.74815,-90.74499,-104.25868], "fy":[-50.1172,34.50274,51.27543,-2.86227]}, - {"t":2.54056, "x":2.90733, "y":3.87165, "heading":-0.52797, "vx":1.73542, "vy":-0.1393, "omega":3.00939, "ax":-5.79613, "ay":0.55114, "alpha":-7.93524, "fx":[-89.86222,-93.44367,-91.57481,-104.14213], "fy":[-52.73796,45.19792,49.89228,-6.31177]}, - {"t":2.5687, "x":2.95386, "y":3.86795, "heading":-0.4433, "vx":1.57233, "vy":-0.12379, "omega":2.78612, "ax":-5.72418, "ay":0.56696, "alpha":-8.59245, "fx":[-88.6496,-89.21166,-92.53912,-103.91757], "fy":[-54.80001,53.30422,48.16814,-9.59773]}, - {"t":2.59683, "x":2.99584, "y":3.86469, "heading":-0.3649, "vx":1.41128, "vy":-0.10784, "omega":2.54436, "ax":-5.66491, "ay":0.55578, "alpha":-9.14615, "fx":[-87.6855,-85.61689,-93.52963,-103.61019], "fy":[-56.36538,59.08261,46.2857,-12.65886]}, - {"t":2.62497, "x":3.0333, "y":3.86187, "heading":-0.29332, "vx":1.25189, "vy":-0.0922, "omega":2.28702, "ax":-5.62029, "ay":0.52743, "alpha":-9.57403, "fx":[-86.96853,-82.83002,-94.47865,-103.24718], "fy":[-57.4929,63.06285,44.374,-15.45393]}, - {"t":2.6531, "x":3.0663, "y":3.85949, "heading":-0.22897, "vx":1.09375, "vy":-0.07736, "omega":2.01764, "ax":-5.58894, "ay":0.49056, "alpha":-9.883, "fx":[-86.48127,-80.79165,-95.34573,-102.85533], "fy":[-58.24556,65.75451,42.52714,-17.9571]}, - {"t":2.68124, "x":3.09486, "y":3.85751, "heading":-0.1722, "vx":0.9365, "vy":-0.06356, "omega":1.73957, "ax":-5.56816, "ay":0.45151, "alpha":-10.09321, "fx":[-86.19146,-79.35558,-96.10874,-102.45923], "fy":[-58.69276,67.55784,40.81445,-20.15438]}, - {"t":2.70938, "x":3.11901, "y":3.8559, "heading":-0.12325, "vx":0.77983, "vy":-0.05085, "omega":1.45558, "ax":-5.55507, "ay":0.41443, "alpha":-10.22852, "fx":[-86.05527,-78.3659,-96.75752,-102.08036], "fy":[-58.90977,68.76351,39.28696,-22.04024]}, - {"t":2.73751, "x":3.13875, "y":3.85463, "heading":-0.0823, "vx":0.62353, "vy":-0.03919, "omega":1.16779, "ax":-5.54706, "ay":0.38183, "alpha":-10.31186, "fx":[-86.0221,-77.68765,-97.28919,-101.73663], "fy":[-58.97507,69.57626,37.98189,-23.61462]}, - {"t":2.76565, "x":3.1541, "y":3.85368, "heading":-0.04944, "vx":0.46746, "vy":-0.02845, "omega":0.87765, "ax":-5.54196, "ay":0.35506, "alpha":-10.36319, "fx":[-86.03993,-77.21466,-97.70491,-101.44248], "fy":[-58.96605,70.13854,36.92618,-24.88039]}, - {"t":2.79379, "x":3.16506, "y":3.85302, "heading":-0.02475, "vx":0.31153, "vy":-0.01846, "omega":0.58607, "ax":-5.53804, "ay":0.33478, "alpha":-10.39874, "fx":[-86.06049,-76.86821,-98.00746,-101.20918], "fy":[-58.95363,70.5483,36.13904,-25.84133]}, - {"t":2.82192, "x":3.17163, "y":3.85263, "heading":-0.00826, "vx":0.15571, "vy":-0.00904, "omega":0.29348, "ax":-5.53401, "ay":0.32127, "alpha":-10.43073, "fx":[-86.04393,-76.593,-98.19971,-101.04519], "fy":[-58.99629,70.87131,35.63399,-26.50058]}, - {"t":2.85006, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":2.72474, "y":4.08493, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.34728, "ay":-0.7307, "alpha":-0.35411, "fx":[-104.00489,-103.58165,-103.51146,-103.96601], "fy":[-9.85684,-13.60535,-14.10103,-10.21906]}, + {"t":0.04076, "x":2.71947, "y":4.08432, "heading":3.14159, "vx":-0.2587, "vy":-0.02978, "omega":-0.01443, "ax":-6.34645, "ay":-0.72982, "alpha":-0.35302, "fx":[-103.99041,-103.56899,-103.49866,-103.95116], "fy":[-9.84921,-13.58599,-14.07937,-10.20996]}, + {"t":0.08152, "x":2.70365, "y":4.0825, "heading":3.141, "vx":-0.51737, "vy":-0.05953, "omega":-0.02882, "ax":-6.34533, "ay":-0.72864, "alpha":-0.35157, "fx":[-103.9712,-103.55226,-103.48146,-103.93123], "fy":[-9.83805,-13.55898,-14.05145,-10.19906]}, + {"t":0.12227, "x":2.67729, "y":4.07947, "heading":3.13983, "vx":-0.77599, "vy":-0.08922, "omega":-0.04315, "ax":-6.34376, "ay":-0.72699, "alpha":-0.34954, "fx":[-103.94435,-103.52892,-103.4573,-103.90327], "fy":[-9.82185,-13.52047,-14.01293,-10.1845]}, + {"t":0.16303, "x":2.6404, "y":4.07523, "heading":3.13807, "vx":-1.03454, "vy":-0.11886, "omega":-0.0574, "ax":-6.34142, "ay":-0.72452, "alpha":-0.34649, "fx":[-103.90406,-103.49387,-103.42105,-103.86135], "fy":[-9.79759,-13.46278,-13.95515,-10.16259]}, + {"t":0.20379, "x":2.59297, "y":4.06979, "heading":3.13573, "vx":-1.293, "vy":-0.14839, "omega":-0.07152, "ax":-6.3375, "ay":-0.7204, "alpha":-0.34142, "fx":[-103.83678,-103.43519,-103.36073,-103.79164], "fy":[-9.75819,-13.36796,-13.85792,-10.12475]}, + {"t":0.24455, "x":2.535, "y":4.06314, "heading":3.13282, "vx":-1.5513, "vy":-0.17775, "omega":-0.08543, "ax":-6.32966, "ay":-0.71217, "alpha":-0.33128, "fx":[-103.70183,-103.317,-103.24032,-103.65271], "fy":[-9.6824,-13.18221,-13.66084,-10.04524]}, + {"t":0.2853, "x":2.46652, "y":4.0553, "heading":3.12933, "vx":-1.80929, "vy":-0.20677, "omega":-0.09894, "ax":-6.30609, "ay":-0.68754, "alpha":-0.30087, "fx":[-103.29529,-102.95848,-102.87893,-103.23787], "fy":[-9.46402,-12.63646,-13.06348,-9.79618]}, + {"t":0.32606, "x":2.38754, "y":4.0463, "heading":3.1253, "vx":-2.06631, "vy":-0.2348, "omega":-0.1112, "ax":0.87667, "ay":2.21037, "alpha":5.25367, "fx":[32.60995,27.83893,-1.11294,-2.00864], "fy":[21.61968,47.65043,50.89191,24.37962]}, + {"t":0.36682, "x":2.30405, "y":4.03857, "heading":3.12077, "vx":-2.03058, "vy":-0.14471, "omega":0.10293, "ax":6.29569, "ay":0.77394, "alpha":0.42214, "fx":[103.24202,102.71919,102.57819,103.15097], "fy":[10.1348,14.54097,15.26161,10.6721]}, + {"t":0.40758, "x":2.22652, "y":4.03332, "heading":3.12497, "vx":-1.77398, "vy":-0.11316, "omega":0.12013, "ax":6.32434, "ay":0.7556, "alpha":0.39225, "fx":[103.67301,103.1942,103.08773,103.60852], "fy":[10.01309,14.13428,14.77254,10.49083]}, + {"t":0.44833, "x":2.15947, "y":4.02933, "heading":3.12986, "vx":-1.51621, "vy":-0.08237, "omega":0.13612, "ax":6.33392, "ay":0.74944, "alpha":0.38227, "fx":[103.81705,103.3523,103.25843,103.76216], "fy":[9.97801,14.00469,14.6025,10.42232]}, + {"t":0.48909, "x":2.10293, "y":4.0266, "heading":3.13541, "vx":-1.25806, "vy":-0.05182, "omega":0.1517, "ax":6.33871, "ay":0.74634, "alpha":0.37728, "fx":[103.88874,103.43062,103.34451,103.83972], "fy":[9.96568,13.94622,14.51207,10.38122]}, + {"t":0.52985, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-0.99971, "vy":-0.0214, "omega":0.16708, "ax":-6.35188, "ay":0.51615, "alpha":0.31489, "fx":[-103.96597,-103.68136,-103.72514,-103.99218], "fy":[6.84312,10.3028,9.9794,6.62719]}, + {"t":0.55358, "x":2.03141, "y":4.02474, "heading":-3.13763, "vx":-1.15044, "vy":-0.00915, "omega":0.17455, "ax":-6.34718, "ay":0.51931, "alpha":0.2891, "fx":[-103.87764,-103.61556,-103.65861,-103.90546], "fy":[7.03573,10.20465,9.89842,6.8198]}, + {"t":0.57731, "x":2.00232, "y":4.02467, "heading":-3.13349, "vx":-1.30107, "vy":0.00317, "omega":0.18141, "ax":-6.34016, "ay":0.52398, "alpha":0.25065, "fx":[-103.7462,-103.5181,-103.55864,-103.77495], "fy":[7.31646,10.05468,9.78181,7.11119]}, + {"t":0.60104, "x":1.96966, "y":4.02489, "heading":-3.12918, "vx":-1.45153, "vy":0.01561, "omega":0.18736, "ax":-6.32852, "ay":0.53163, "alpha":0.18719, "fx":[-103.5297,-103.35839,-103.39213,-103.55663], "fy":[7.76869,9.80267,9.59478,7.59828]}, + {"t":0.62477, "x":1.93343, "y":4.02541, "heading":-3.12473, "vx":-1.60171, "vy":0.02822, "omega":0.1918, "ax":-6.30551, "ay":0.54646, "alpha":0.0626, "fx":[-103.10553,-103.04771,-103.06101,-103.11803], "fy":[8.63068,9.30393,9.23436,8.56525]}, + {"t":0.6485, "x":1.89364, "y":4.02624, "heading":-3.12018, "vx":-1.75134, "vy":0.04119, "omega":0.19329, "ax":-6.23866, "ay":0.58766, "alpha":-0.29303, "fx":[-101.90031,-102.17634,-102.0894,-101.79488], "fy":[10.96534,7.90277,8.2046,11.35568]}, + {"t":0.67223, "x":1.85033, "y":4.02738, "heading":-3.1156, "vx":-1.89939, "vy":0.05513, "omega":0.18633, "ax":-4.13305, "ay":1.06126, "alpha":-9.18885, "fx":[-79.30561,-86.19914,-60.40049,-44.36458], "fy":[40.34184,-12.92778,-23.17735,65.16136]}, + {"t":0.69596, "x":1.80409, "y":4.02899, "heading":-3.11117, "vx":-1.99747, "vy":0.08032, "omega":-0.03173, "ax":-0.01857, "ay":-0.64882, "alpha":-20.65279, "fx":[-65.63821,-49.5858,52.37641,61.63304], "fy":[43.7265,-68.47226,-66.06494,48.38295]}, + {"t":0.7197, "x":1.75668, "y":4.03071, "heading":-3.11193, "vx":-1.99791, "vy":0.06492, "omega":-0.52184, "ax":4.81335, "ay":-1.64598, "alpha":-8.69864, "fx":[83.07657,50.71938,84.85965,96.1006], "fy":[11.68497,-78.11685,-47.96952,6.76662]}, + {"t":0.74343, "x":1.71062, "y":4.03179, "heading":-3.12431, "vx":-1.88369, "vy":0.02586, "omega":-0.72826, "ax":6.20197, "ay":-0.78157, "alpha":-1.7941, "fx":[102.36583,99.59874,100.844,102.75291], "fy":[-3.84271,-24.2306,-20.12601,-2.90954]}, + {"t":0.76716, "x":1.66767, "y":4.03218, "heading":3.14159, "vx":-1.73651, "vy":0.00731, "omega":-0.77084, "ax":6.18502, "ay":-0.59122, "alpha":4.07753, "fx":[100.90283,103.58629,102.80047,97.16366], "fy":[-25.2475,9.25057,13.75074,-36.41529]}, + {"t":0.80086, "x":1.61265, "y":4.03209, "heading":3.11561, "vx":-1.52805, "vy":-0.01261, "omega":-0.63341, "ax":6.04051, "ay":-0.48379, "alpha":6.12686, "fx":[99.91309,102.99619,99.64399,92.44979], "fy":[-29.72907,16.0285,29.73834,-47.67367]}, + {"t":0.83456, "x":1.56458, "y":4.03139, "heading":3.09426, "vx":-1.32447, "vy":-0.02892, "omega":-0.42691, "ax":5.94226, "ay":-0.43778, "alpha":7.17927, "fx":[99.30956,102.69554,96.95314,89.6206], "fy":[-32.00187,18.42935,38.00565,-53.0604]}, + {"t":0.86827, "x":1.52332, "y":4.03017, "heading":3.07988, "vx":-1.12419, "vy":-0.04367, "omega":-0.18495, "ax":5.87567, "ay":-0.41488, "alpha":7.81249, "fx":[98.91183,102.5345,95.00539,87.77245], "fy":[-33.37734,19.59949,42.86371,-56.21563]}, + {"t":0.90197, "x":1.48877, "y":4.02846, "heading":3.07364, "vx":-0.92616, "vy":-0.05765, "omega":0.07836, "ax":5.82852, "ay":-0.40019, "alpha":8.23301, "fx":[98.67369,102.41284,93.61649,86.43759], "fy":[-34.17843,20.40258,45.95387,-58.34726]}, + {"t":0.93567, "x":1.46086, "y":4.02629, "heading":3.07628, "vx":-0.72972, "vy":-0.07114, "omega":0.35584, "ax":5.79375, "ay":-0.38677, "alpha":8.53255, "fx":[98.58113,102.27972,92.62749,85.3789], "fy":[-34.51582,21.17605,48.00602,-59.95818]}, + {"t":0.96938, "x":1.43956, "y":4.02367, "heading":3.08828, "vx":-0.53445, "vy":-0.08418, "omega":0.64342, "ax":5.76754, "ay":-0.37097, "alpha":8.75515, "fx":[98.62881,102.10595,91.94045,84.47772], "fy":[-34.43221,22.07916,49.37143,-61.2772]}, + {"t":1.00308, "x":1.42482, "y":4.02063, "heading":3.10996, "vx":-0.34007, "vy":-0.09668, "omega":0.9385, "ax":5.74805, "ay":-0.35073, "alpha":8.92008, "fx":[98.81273,101.87138,91.50869,83.68574], "fy":[-33.94228,23.19483,50.21162,-62.3995]}, + {"t":1.03679, "x":1.41663, "y":4.01717, "heading":3.14159, "vx":-0.14634, "vy":-0.1085, "omega":1.23913, "ax":5.8589, "ay":-0.34425, "alpha":8.03187, "fx":[99.62575,101.99184,94.57672,86.9329], "fy":[-31.41485,22.56709,44.08301,-57.74632]}, + {"t":1.06489, "x":1.41483, "y":4.01398, "heading":-3.10676, "vx":0.01834, "vy":-0.11818, "omega":1.46489, "ax":5.86626, "ay":-0.3263, "alpha":7.96531, "fx":[99.98899,101.74453,95.01089,86.86407], "fy":[-30.21838,23.62349,43.09471,-57.83727]}, + {"t":1.093, "x":1.41766, "y":4.01053, "heading":-3.06559, "vx":0.18322, "vy":-0.12735, "omega":1.68877, "ax":5.87653, "ay":-0.30664, "alpha":7.86733, "fx":[100.4009,101.44489,95.58567,86.84885], "fy":[-28.79649,24.84184,41.74993,-57.8473]}, + {"t":1.12111, "x":1.42513, "y":4.00683, "heading":-3.01812, "vx":0.3484, "vy":-0.13597, "omega":1.9099, "ax":5.89044, "ay":-0.28635, "alpha":7.72889, "fx":[100.85243,101.09153,96.32622,86.91985], "fy":[-27.14457,26.20217,39.94447,-57.72711]}, + {"t":1.14921, "x":1.43725, "y":4.0029, "heading":-2.96444, "vx":0.51396, "vy":-0.14402, "omega":2.12714, "ax":5.90858, "ay":-0.26693, "alpha":7.54202, "fx":[101.33227,100.68489,97.25067,87.10842], "fy":[-25.2598,27.67858,37.55352,-57.42757]}, + {"t":1.17732, "x":1.45403, "y":3.99874, "heading":-2.90465, "vx":0.68004, "vy":-0.15152, "omega":2.33913, "ax":5.93123, "ay":-0.25032, "alpha":7.30187, "fx":[101.82683,100.22836,98.35959,87.44216], "fy":[-23.14218,29.23802,34.43689,-56.90172]}, + {"t":1.20543, "x":1.47549, "y":3.99439, "heading":-2.83891, "vx":0.84675, "vy":-0.15856, "omega":2.54436, "ax":5.95809, "ay":-0.2387, "alpha":7.00943, "fx":[102.32027,99.72965,99.62127,87.94254], "fy":[-20.79563,30.83882,30.45376,-56.10616]}, + {"t":1.23354, "x":1.50164, "y":3.98983, "heading":-2.76739, "vx":1.01422, "vy":-0.16526, "omega":2.74138, "ax":5.98809, "ay":-0.2341, "alpha":6.67456, "fx":[102.79484,99.20229,100.95518,88.62321], "fy":[-18.22906,32.42897,25.49313,-55.00162]}, + {"t":1.26164, "x":1.53251, "y":3.9851, "heading":-2.69034, "vx":1.18253, "vy":-0.17184, "omega":2.92898, "ax":6.01921, "ay":-0.23766, "alpha":6.31805, "fx":[103.23149,98.6675,102.22222,89.48917], "fy":[-15.45724,33.9436,19.5245,-53.5523]}, + {"t":1.28975, "x":1.56813, "y":3.98017, "heading":-2.60801, "vx":1.35171, "vy":-0.17852, "omega":3.10657, "ax":6.04873, "ay":-0.24865, "alpha":5.97013, "fx":[103.61068,98.15642,103.23659,90.53724], "fy":[-12.50157,35.30104,12.66396,-51.72339]}, + {"t":1.31786, "x":1.60851, "y":3.97506, "heading":-2.52069, "vx":1.52172, "vy":-0.18551, "omega":3.27437, "ax":6.07408, "ay":-0.26361, "alpha":5.66269, "fx":[103.91331,97.71293,103.81419,91.75781], "fy":[-9.39091,36.3958,5.23284,-49.47597]}, + {"t":1.34597, "x":1.65368, "y":3.96974, "heading":-2.42866, "vx":1.69245, "vy":-0.19292, "omega":3.43354, "ax":6.09414, "ay":-0.27626, "alpha":5.41449, "fx":[104.12168,97.39751,103.85329,93.13756], "fy":[-6.1636,37.08566,-2.22972,-46.75773]}, + {"t":1.37407, "x":1.70366, "y":3.96421, "heading":-2.33215, "vx":1.86374, "vy":-0.20069, "omega":3.58572, "ax":6.11058, "ay":-0.27854, "alpha":5.21386, "fx":[104.22044,97.29214,103.40989,94.66273], "fy":[-2.87253,37.16797,-9.02328,-43.48667]}, + {"t":1.40218, "x":1.75846, "y":3.95846, "heading":-2.23137, "vx":2.03549, "vy":-0.20852, "omega":3.73227, "ax":6.12827, "ay":-0.2624, "alpha":5.00559, "fx":[104.19786,97.50509,102.71696,96.32227], "fy":[0.40209,36.33585,-14.37735,-39.51946]}, + {"t":1.43029, "x":1.81809, "y":3.95249, "heading":-2.12646, "vx":2.20774, "vy":-0.21589, "omega":3.87297, "ax":6.15447, "ay":-0.22091, "alpha":4.68346, "fx":[104.048,98.16909,102.12839,98.10987], "fy":[3.52098,34.09571,-17.48234,-34.58039]}, + {"t":1.4584, "x":1.88258, "y":3.94634, "heading":-2.0176, "vx":2.38073, "vy":-0.2221, "omega":4.0046, "ax":6.1966, "ay":-0.14679, "alpha":4.07686, "fx":[103.77493,99.40838,102.00834,100.01881], "fy":[6.19838,29.60726,-17.33777,-28.06662]}, + {"t":1.4865, "x":1.95194, "y":3.94004, "heading":-1.90504, "vx":2.5549, "vy":-0.22623, "omega":4.11919, "ax":6.25585, "ay":-0.02334, "alpha":2.89554, "fx":[103.39668,101.18541,102.52882,101.97375], "fy":[7.70137,21.36916,-12.22719,-18.36947]}, + {"t":1.51461, "x":2.02622, "y":3.93367, "heading":-1.78926, "vx":2.73073, "vy":-0.22688, "omega":4.20058, "ax":6.29161, "ay":0.2164, "alpha":0.45867, "fx":[102.86031,102.7033,102.88386,102.97582], "fy":[5.27532,6.65954,1.69367,0.52262]}, + {"t":1.54272, "x":2.10546, "y":3.92738, "heading":-1.6712, "vx":2.90757, "vy":-0.2208, "omega":4.21347, "ax":5.64499, "ay":0.77739, "alpha":-7.62111, "fx":[95.00698,100.33849,96.80213,76.99165], "fy":[-28.33721,-18.82413,33.09984,64.89713]}, + {"t":1.57083, "x":2.18942, "y":3.92148, "heading":-1.55277, "vx":3.06624, "vy":-0.19895, "omega":3.99926, "ax":3.89493, "ay":0.56269, "alpha":-16.15304, "fx":[45.05433,96.26947,87.4786,25.89659], "fy":[-77.98018,-25.97592,48.65474,92.09669]}, + {"t":1.59893, "x":2.27714, "y":3.91611, "heading":-1.44036, "vx":3.17572, "vy":-0.18313, "omega":3.54524, "ax":-3.39363, "ay":0.3697, "alpha":3.71261, "fx":[-48.3058,-63.33629,-62.66874,-47.60653], "fy":[22.39792,15.4112,-7.8498,-5.78365]}, + {"t":1.62704, "x":2.36506, "y":3.91111, "heading":-1.34071, "vx":3.08033, "vy":-0.17274, "omega":3.6496, "ax":-5.39169, "ay":-0.52206, "alpha":8.58395, "fx":[-84.36102,-99.47637,-92.03051,-76.70743], "fy":[47.8863,14.06075,-39.72991,-56.35619]}, + {"t":1.65515, "x":2.44951, "y":3.90604, "heading":-1.23813, "vx":2.92878, "vy":-0.18742, "omega":3.89087, "ax":-6.21244, "ay":-0.16218, "alpha":1.58289, "fx":[-101.46,-102.19207,-101.30967,-101.28432], "fy":[8.67315,2.31117,-12.8074,-8.78226]}, + {"t":1.68325, "x":2.52938, "y":3.90071, "heading":-1.12877, "vx":2.75417, "vy":-0.19198, "omega":3.93536, "ax":-6.21026, "ay":0.1184, "alpha":-3.29932, "fx":[-101.33102,-102.38588,-99.30565,-103.08131], "fy":[-20.02028,-8.30178,27.61885,8.44572]}, + {"t":1.71136, "x":2.60434, "y":3.89536, "heading":-1.01816, "vx":2.57961, "vy":-0.18865, "omega":3.84262, "ax":-6.10619, "ay":0.20452, "alpha":-4.97787, "fx":[-99.00205,-102.6868,-94.14825,-103.46123], "fy":[-30.93881,-8.12783,43.22421,9.21663]}, + {"t":1.73947, "x":2.67443, "y":3.89014, "heading":-0.91015, "vx":2.40799, "vy":-0.1829, "omega":3.70271, "ax":-6.04464, "ay":0.26536, "alpha":-5.67323, "fx":[-96.83872,-103.19335,-91.4106,-103.83103], "fy":[-37.71738,-1.30311,49.34475,7.02835]}, + {"t":1.76758, "x":2.73972, "y":3.88511, "heading":-0.80608, "vx":2.23809, "vy":-0.17544, "omega":3.54325, "ax":-5.99863, "ay":0.34338, "alpha":-6.11545, "fx":[-94.85951,-102.90237,-90.39706,-104.10592], "fy":[-42.72785,9.75164,51.51875,3.91186]}, + {"t":1.79568, "x":2.80026, "y":3.88031, "heading":-0.70649, "vx":2.06948, "vy":-0.16579, "omega":3.37136, "ax":-5.94409, "ay":0.43133, "alpha":-6.61245, "fx":[-93.05715,-101.01309,-90.37927,-104.24859], "fy":[-46.68335,22.63986,51.76941,0.47991]}, + {"t":1.82379, "x":2.85608, "y":3.87582, "heading":-0.61173, "vx":1.90241, "vy":-0.15367, "omega":3.1855, "ax":-5.8752, "ay":0.50643, "alpha":-7.23419, "fx":[-91.44577,-97.57804,-90.91508,-104.25459], "fy":[-49.86766,35.01743,50.97587,-3.00914]}, + {"t":1.8519, "x":2.90723, "y":3.8717, "heading":-0.52219, "vx":1.73727, "vy":-0.13943, "omega":2.98217, "ax":-5.80046, "ay":0.55208, "alpha":-7.91264, "fx":[-90.04809,-93.38204,-91.74024,-104.13549], "fy":[-52.41806,45.34864,49.58895,-6.41793]}, + {"t":1.88001, "x":2.95377, "y":3.868, "heading":-0.43837, "vx":1.57424, "vy":-0.12391, "omega":2.75976, "ax":-5.73122, "ay":0.56559, "alpha":-8.55095, "fx":[-88.88467,-89.29115,-92.69109,-103.91153], "fy":[-54.41622,53.1862,47.87583,-9.66022]}, + {"t":1.90811, "x":2.99575, "y":3.86474, "heading":-0.3608, "vx":1.41315, "vy":-0.10802, "omega":2.51942, "ax":-5.67436, "ay":0.55367, "alpha":-9.08698, "fx":[-87.96646,-85.82159,-93.66414,-103.6076], "fy":[-55.92447,58.79479,46.0132,-12.67756]}, + {"t":1.93622, "x":3.03323, "y":3.86192, "heading":-0.28998, "vx":1.25365, "vy":-0.09245, "omega":2.26401, "ax":-5.63157, "ay":0.52562, "alpha":-9.50083, "fx":[-87.29034,-83.12615,-94.59474,-103.25032], "fy":[-57.00203,62.67847,44.12597,-15.43072]}, + {"t":1.96433, "x":3.06625, "y":3.85953, "heading":-0.22635, "vx":1.09537, "vy":-0.07768, "omega":1.99696, "ax":-5.60148, "ay":0.48963, "alpha":-9.7997, "fx":[-86.83768,-81.14664,-95.44424,-102.86576], "fy":[-57.71208,65.3202,42.30531,-17.8953]}, + {"t":1.99244, "x":3.09482, "y":3.85754, "heading":-0.17022, "vx":0.93792, "vy":-0.06392, "omega":1.72152, "ax":-5.58153, "ay":0.45169, "alpha":-10.00313, "fx":[-86.57571,-79.74472,-96.19157,-102.47777], "fy":[-58.12402,67.10093,40.61844,-20.05822]}, + {"t":2.02054, "x":3.11898, "y":3.85592, "heading":-0.12183, "vx":0.78104, "vy":-0.05122, "omega":1.44036, "ax":-5.56896, "ay":0.41575, "alpha":-10.13417, "fx":[-86.4608,-78.77272,-96.82701,-102.10712], "fy":[-58.3128,68.29902,39.11484,-21.91443]}, + {"t":2.04865, "x":3.13873, "y":3.85465, "heading":-0.08135, "vx":0.62451, "vy":-0.03954, "omega":1.15551, "ax":-5.56127, "ay":0.38417, "alpha":-10.21498, "fx":[-86.44314,-78.10233,-97.34785,-101.77113], "fy":[-58.35629,69.11174,37.83068,-23.46413]}, + {"t":2.07676, "x":3.15409, "y":3.85369, "heading":-0.04887, "vx":0.4682, "vy":-0.02874, "omega":0.8684, "ax":-5.55635, "ay":0.35827, "alpha":-10.2649, "fx":[-86.47188,-77.63215,-97.7552,-101.48375], "fy":[-58.33108,69.67705,36.79216,-24.71027]}, + {"t":2.10487, "x":3.16505, "y":3.85302, "heading":-0.02446, "vx":0.31202, "vy":-0.01867, "omega":0.57988, "ax":-5.55255, "ay":0.33864, "alpha":-10.29967, "fx":[-86.50013,-77.2867,-98.05176,-101.25588], "fy":[-58.30713,70.0902,36.01797,-25.65661]}, + {"t":2.13297, "x":3.17163, "y":3.85263, "heading":-0.00816, "vx":0.15596, "vy":-0.00915, "omega":0.29038, "ax":-5.54863, "ay":0.32555, "alpha":-10.33113, "fx":[-86.48925,-77.01281,-98.24027,-101.09568], "fy":[-58.34205,70.41533,35.52128,-26.30626]}, + {"t":2.16108, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/LOtoA.traj b/src/main/deploy/choreo/LOtoA.traj index c9f2372f..cbada4bd 100644 --- a/src/main/deploy/choreo/LOtoA.traj +++ b/src/main/deploy/choreo/LOtoA.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.104679584503174, "y":7.582505702972412, "heading":0.0, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.104679584503174, "y":7.582505702972412, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":2.636875629425049, "y":5.11732816696167, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -11,12 +11,13 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":6.0}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.104679584503174 m", "val":7.104679584503174}, "y":{"exp":"7.582505702972412 m", "val":7.582505702972412}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.104679584503174 m", "val":7.104679584503174}, "y":{"exp":"7.582505702972412 m", "val":7.582505702972412}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.636875629425049 m", "val":2.636875629425049}, "y":{"exp":"5.11732816696167 m", "val":5.11732816696167}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -24,7 +25,8 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"6 m / s ^ 2", "val":6.0}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,72 +34,78 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.76157,2.94513], + "waypoints":[0.0,2.06434,3.24276], "samples":[ - {"t":0.0, "x":7.10468, "y":7.58251, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.45556, "ay":-2.4909, "alpha":-0.00187, "fx":[-89.18229,-89.19099,-89.19312,-89.18572], "fy":[-40.73407,-40.71492,-40.71042,-40.72673]}, - {"t":0.04636, "x":7.09882, "y":7.57983, "heading":0.0, "vx":-0.2529, "vy":-0.11547, "omega":-0.00009, "ax":-5.45669, "ay":-2.49145, "alpha":-0.00152, "fx":[-89.20193,-89.20777,-89.21125,-89.20488], "fy":[-40.74035,-40.7275,-40.72003,-40.73404]}, - {"t":0.09271, "x":7.08123, "y":7.5718, "heading":0.0, "vx":-0.50586, "vy":-0.23097, "omega":-0.00016, "ax":-5.45658, "ay":-2.49144, "alpha":-0.00121, "fx":[-89.20112,-89.20514,-89.20884,-89.20357], "fy":[-40.73802,-40.72914,-40.72116,-40.73279]}, - {"t":0.13907, "x":7.05192, "y":7.55842, "heading":-0.00001, "vx":-0.75881, "vy":-0.34646, "omega":-0.00021, "ax":-5.45645, "ay":-2.49142, "alpha":-0.00103, "fx":[-89.19957,-89.20329,-89.20597,-89.20159], "fy":[-40.73664,-40.72846,-40.72267,-40.7323]}, - {"t":0.18543, "x":7.01088, "y":7.53968, "heading":-0.00002, "vx":-1.01176, "vy":-0.46196, "omega":-0.00026, "ax":-5.4563, "ay":-2.49141, "alpha":-0.00073, "fx":[-89.19811,-89.19996,-89.20298,-89.19965], "fy":[-40.73428,-40.73015,-40.72363,-40.73098]}, - {"t":0.23179, "x":6.95811, "y":7.51559, "heading":-0.00003, "vx":-1.2647, "vy":-0.57745, "omega":-0.00029, "ax":-5.45613, "ay":-2.49138, "alpha":-0.00054, "fx":[-89.19576,-89.19734,-89.1993,-89.19688], "fy":[-40.73277,-40.72932,-40.72506,-40.73038]}, - {"t":0.27814, "x":6.89362, "y":7.48614, "heading":-0.00005, "vx":-1.51763, "vy":-0.69295, "omega":-0.00032, "ax":-5.45592, "ay":-2.49136, "alpha":-0.00025, "fx":[-89.19324,-89.19294,-89.19532,-89.1939], "fy":[-40.73032,-40.73096,-40.72579,-40.72892]}, - {"t":0.3245, "x":6.81741, "y":7.45134, "heading":-0.00006, "vx":-1.77055, "vy":-0.80844, "omega":-0.00033, "ax":-5.45566, "ay":-2.49133, "alpha":-0.0002, "fx":[-89.18887,-89.1909,-89.18958,-89.1891], "fy":[-40.73008,-40.72564,-40.72853,-40.72959]}, - {"t":0.37086, "x":6.72947, "y":7.41119, "heading":-0.00008, "vx":-2.02346, "vy":-0.92393, "omega":-0.00034, "ax":-5.45533, "ay":-2.49129, "alpha":0.00024, "fx":[-89.18518,-89.18236,-89.18449,-89.18496], "fy":[-40.72582,-40.73196,-40.72733,-40.72629]}, - {"t":0.41721, "x":6.62981, "y":7.36568, "heading":-0.00009, "vx":-2.27635, "vy":-1.03942, "omega":-0.00033, "ax":-5.45491, "ay":-2.49124, "alpha":0.00021, "fx":[-89.17777,-89.17872,-89.17571,-89.17714], "fy":[-40.72599,-40.72395,-40.7305,-40.72737]}, - {"t":0.46357, "x":6.51842, "y":7.31482, "heading":-0.00011, "vx":-2.52922, "vy":-1.15491, "omega":-0.00032, "ax":-5.45433, "ay":-2.49117, "alpha":0.00068, "fx":[-89.17018,-89.16565,-89.16678,-89.16911], "fy":[-40.72101,-40.73091,-40.72844,-40.72333]}, - {"t":0.50993, "x":6.39531, "y":7.2586, "heading":-0.00012, "vx":-2.78207, "vy":-1.27039, "omega":-0.00029, "ax":-5.45352, "ay":-2.49106, "alpha":0.00106, "fx":[-89.15828,-89.15024,-89.15346,-89.15679], "fy":[-40.71629,-40.73393,-40.72683,-40.71954]}, - {"t":0.55629, "x":6.26048, "y":7.19703, "heading":-0.00014, "vx":-3.03488, "vy":-1.38587, "omega":-0.00024, "ax":-5.45227, "ay":-2.49091, "alpha":0.00125, "fx":[-89.13842,-89.12988,-89.13234,-89.13652], "fy":[-40.71258,-40.73128,-40.7259,-40.71672]}, - {"t":0.60264, "x":6.11394, "y":7.13011, "heading":-0.00015, "vx":-3.28763, "vy":-1.50134, "omega":-0.00018, "ax":-5.45013, "ay":-2.49061, "alpha":0.00086, "fx":[-89.10122,-89.10317,-89.09368,-89.09888], "fy":[-40.71244,-40.70823,-40.72893,-40.71756]}, - {"t":0.649, "x":5.95567, "y":7.05784, "heading":-0.00016, "vx":-3.54028, "vy":-1.6168, "omega":-0.00014, "ax":-5.44555, "ay":-2.48996, "alpha":0.00138, "fx":[-89.02843,-89.02391,-89.01959,-89.0257], "fy":[-40.69726,-40.70708,-40.71661,-40.70326]}, - {"t":0.69536, "x":5.78571, "y":6.98021, "heading":-0.00016, "vx":-3.79272, "vy":-1.73222, "omega":-0.00008, "ax":-5.42958, "ay":-2.48663, "alpha":0.00087, "fx":[-88.76452,-88.77361,-88.75376,-88.76125], "fy":[-40.64897,-40.62897,-40.6726,-40.65627]}, - {"t":0.74171, "x":5.60405, "y":6.89724, "heading":-0.00017, "vx":-4.04442, "vy":-1.8475, "omega":-0.00004, "ax":-1.10748, "ay":-0.47243, "alpha":0.0008, "fx":[-18.17474,-18.17828,-18.03343,-18.03445], "fy":[-7.78819,-7.38333,-7.93239,-7.78926]}, - {"t":0.78807, "x":5.41537, "y":6.81109, "heading":-0.00017, "vx":-4.09576, "vy":-1.8694, "omega":0.0, "ax":-0.03199, "ay":0.06871, "alpha":-0.00001, "fx":[-0.54291,-0.49152,-0.52872,-0.52873], "fy":[1.1289,1.11771,1.11773,1.12891]}, - {"t":0.83443, "x":5.22547, "y":6.7245, "heading":-0.00017, "vx":-4.09725, "vy":-1.86621, "omega":0.0, "ax":-0.00613, "ay":0.01348, "alpha":0.0, "fx":[-0.14762,-0.14762,-0.05267,-0.05264], "fy":[0.17389,0.45446,0.07924,0.17387]}, - {"t":0.88079, "x":5.03553, "y":6.638, "heading":-0.00017, "vx":-4.09753, "vy":-1.86559, "omega":0.0, "ax":0.00315, "ay":-0.00686, "alpha":0.00001, "fx":[0.12154,-0.06823,0.07643,0.07642], "fy":[-0.13643,-0.08776,-0.08774,-0.13641]}, - {"t":0.92714, "x":4.84558, "y":6.55151, "heading":-0.00017, "vx":-4.09738, "vy":-1.8659, "omega":0.0, "ax":0.09822, "ay":-0.2152, "alpha":-0.00002, "fx":[1.54861,1.5487,1.66283,1.66292], "fy":[-3.57413,-3.2377,-3.68647,-3.57412]}, - {"t":0.9735, "x":4.65575, "y":6.46478, "heading":-0.00017, "vx":-4.09283, "vy":-1.87588, "omega":0.0, "ax":0.30035, "ay":-0.64873, "alpha":0.00002, "fx":[4.9811,4.78934,4.93522,4.9351], "fy":[-10.62988,-10.58137,-10.58106,-10.62996]}, - {"t":1.01986, "x":4.46634, "y":6.37713, "heading":-0.00017, "vx":-4.07891, "vy":-1.90595, "omega":0.0, "ax":0.7512, "ay":-1.57054, "alpha":0.0, "fx":[12.31643,12.31176,12.24816,12.24655], "fy":[-25.64198,-25.8435,-25.57271,-25.64348]}, - {"t":1.06621, "x":4.27806, "y":6.28708, "heading":-0.00017, "vx":-4.04408, "vy":-1.97876, "omega":0.0, "ax":1.67323, "ay":-3.256, "alpha":-0.00009, "fx":[27.20844,27.59211,27.30437,27.31183], "fy":[-53.18809,-53.26365,-53.28441,-53.18146]}, - {"t":1.11257, "x":4.09238, "y":6.19186, "heading":-0.00017, "vx":-3.96652, "vy":-2.1297, "omega":0.0, "ax":3.90315, "ay":-3.66968, "alpha":0.02327, "fx":[64.08435,63.44397,63.97183,63.73645], "fy":[-59.79694,-60.5263,-59.62428,-60.02141]}, - {"t":1.15893, "x":3.9127, "y":6.08919, "heading":-0.00017, "vx":-3.78558, "vy":-2.29981, "omega":0.00107, "ax":5.66386, "ay":1.89899, "alpha":-0.00449, "fx":[92.60303,92.59137,92.58169,92.59738], "fy":[31.01731,31.0523,31.07893,31.0309]}, - {"t":1.20529, "x":3.7433, "y":5.98461, "heading":-0.00012, "vx":-3.52302, "vy":-2.21178, "omega":0.00087, "ax":5.57182, "ay":2.19384, "alpha":-0.00471, "fx":[91.10203,91.07032,91.08504,91.09708], "fy":[35.83124,35.91261,35.87372,35.84262]}, - {"t":1.25164, "x":3.58597, "y":5.88444, "heading":-0.00008, "vx":-3.26472, "vy":-2.11008, "omega":0.00065, "ax":5.53648, "ay":2.29263, "alpha":-0.00306, "fx":[90.51961,90.5039,90.50481,90.51518], "fy":[37.45931,37.49734,37.49461,37.46926]}, - {"t":1.298, "x":3.44057, "y":5.78909, "heading":-0.00005, "vx":-3.00807, "vy":-2.0038, "omega":0.00051, "ax":5.51797, "ay":2.34197, "alpha":-0.00315, "fx":[90.21797,90.19614,90.20495,90.21402], "fy":[38.26406,38.31593,38.29445,38.27282]}, - {"t":1.34436, "x":3.30706, "y":5.69871, "heading":-0.00002, "vx":-2.75227, "vy":-1.89524, "omega":0.00036, "ax":5.50656, "ay":2.37163, "alpha":-0.00199, "fx":[90.02742,90.01998,90.01594,90.02389], "fy":[38.75875,38.77611,38.78515,38.76649]}, - {"t":1.39071, "x":3.18539, "y":5.6134, "heading":-0.00001, "vx":-2.497, "vy":-1.78529, "omega":0.00027, "ax":5.49885, "ay":2.39138, "alpha":-0.00236, "fx":[89.90331,89.88609,89.89339,89.90023], "fy":[39.07714,39.11703,39.09974,39.08383]}, - {"t":1.43707, "x":3.07554, "y":5.53321, "heading":0.0, "vx":-2.24209, "vy":-1.67444, "omega":0.00016, "ax":5.49327, "ay":2.40552, "alpha":-0.00143, "fx":[89.80852,89.80352,89.79995,89.80585], "fy":[39.31659,39.32807,39.33596,39.32236]}, - {"t":1.48343, "x":2.97751, "y":5.45817, "heading":0.00001, "vx":-1.98744, "vy":-1.56292, "omega":0.00009, "ax":5.48906, "ay":2.41612, "alpha":-0.00163, "fx":[89.74087,89.7291,89.7338,89.73864], "fy":[39.487,39.51398,39.50292,39.49177]}, - {"t":1.52978, "x":2.89127, "y":5.38832, "heading":0.00002, "vx":-1.73298, "vy":-1.45092, "omega":0.00002, "ax":5.48575, "ay":2.42438, "alpha":-0.0011, "fx":[89.68494,89.6791,89.67919,89.68312], "fy":[39.62646,39.63972,39.63932,39.63033]}, - {"t":1.57614, "x":2.81683, "y":5.32366, "heading":0.00002, "vx":-1.47868, "vy":-1.33853, "omega":-0.00003, "ax":5.4831, "ay":2.43097, "alpha":-0.00114, "fx":[89.64212,89.63237,89.63783,89.64074], "fy":[39.73315,39.75531,39.74269,39.73603]}, - {"t":1.6225, "x":2.75418, "y":5.26422, "heading":0.00002, "vx":-1.2245, "vy":-1.22584, "omega":-0.00009, "ax":5.48092, "ay":2.43639, "alpha":-0.00059, "fx":[89.60443,89.60092,89.60149,89.60346], "fy":[39.8262,39.83411,39.83269,39.82816]}, - {"t":1.66886, "x":2.7033, "y":5.21001, "heading":0.00001, "vx":-0.97042, "vy":-1.1129, "omega":-0.00011, "ax":5.4791, "ay":2.44089, "alpha":-0.00061, "fx":[89.57504,89.5681,89.57353,89.57452], "fy":[39.89891,39.91464,39.90221,39.8999]}, - {"t":1.71521, "x":2.6642, "y":5.16105, "heading":0.00001, "vx":-0.71642, "vy":-0.99974, "omega":-0.00014, "ax":5.47755, "ay":2.44471, "alpha":-0.00009, "fx":[89.54788,89.54659,89.54776,89.54779], "fy":[39.96549,39.9684,39.96569,39.96555]}, - {"t":1.76157, "x":2.63688, "y":5.11733, "heading":0.0, "vx":-0.4625, "vy":-0.88641, "omega":-0.00015, "ax":5.65884, "ay":-1.9843, "alpha":0.00107, "fx":[92.51286,92.50668,92.51279,92.51281], "fy":[-32.43517,-32.45274,-32.43514,-32.43516]}, - {"t":1.80891, "x":2.62132, "y":5.07314, "heading":-0.00001, "vx":-0.1946, "vy":-0.98035, "omega":-0.0001, "ax":6.36356, "ay":-0.28848, "alpha":-0.00001, "fx":[104.0321,104.03211,104.03211,104.0321], "fy":[-4.71614,-4.71607,-4.71607,-4.71614]}, - {"t":1.85626, "x":2.61924, "y":5.0264, "heading":-0.00001, "vx":0.10667, "vy":-0.99401, "omega":-0.0001, "ax":6.09873, "ay":1.5999, "alpha":-0.00002, "fx":[99.70264,99.70258,99.70256,99.70263], "fy":[26.15517,26.15541,26.15547,26.15523]}, - {"t":1.9036, "x":2.63112, "y":4.98114, "heading":-0.00002, "vx":0.3954, "vy":-0.91827, "omega":-0.0001, "ax":3.37542, "ay":1.83354, "alpha":0.00039, "fx":[55.18053,55.18112,55.18284,55.18226], "fy":[29.97642,29.97389,29.97332,29.97584]}, - {"t":1.95094, "x":2.65363, "y":4.93972, "heading":-0.00002, "vx":0.5552, "vy":-0.83147, "omega":-0.00008, "ax":0.09113, "ay":0.06119, "alpha":0.00055, "fx":[1.48829,1.48829,1.49131,1.4913], "fy":[1.00189,0.99881,0.99881,1.00189]}, - {"t":1.99828, "x":2.68001, "y":4.90043, "heading":-0.00002, "vx":0.55951, "vy":-0.82857, "omega":-0.00005, "ax":-0.00078, "ay":-0.00052, "alpha":0.00049, "fx":[-0.01402,-0.01402,-0.01135,-0.01135], "fy":[-0.0072,-0.00993,-0.00993,-0.0072]}, - {"t":2.04562, "x":2.7065, "y":4.8612, "heading":-0.00003, "vx":0.55947, "vy":-0.82859, "omega":-0.00003, "ax":0.00119, "ay":0.00081, "alpha":0.00043, "fx":[0.01833,0.01833,0.02067,0.02067], "fy":[0.01437,0.01197,0.01197,0.01437]}, - {"t":2.09297, "x":2.73299, "y":4.82197, "heading":-0.00003, "vx":0.55953, "vy":-0.82855, "omega":-0.00001, "ax":-0.00022, "ay":-0.00015, "alpha":0.00037, "fx":[-0.00454,-0.00454,-0.00251,-0.00251], "fy":[-0.00135,-0.00342,-0.00342,-0.00135]}, - {"t":2.14031, "x":2.75948, "y":4.78275, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00001, "ax":-0.00008, "ay":-0.00005, "alpha":0.00032, "fx":[-0.00209,-0.00209,-0.00037,-0.00037], "fy":[0.00005,-0.00171,-0.00171,0.00005]}, - {"t":2.18765, "x":2.78597, "y":4.74352, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00002, "ax":0.00003, "ay":0.00002, "alpha":0.00026, "fx":[-0.00018,-0.00018,0.00125,0.00125], "fy":[0.00109,-0.00037,-0.00037,0.00109]}, - {"t":2.23499, "x":2.81245, "y":4.70429, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00004, "ax":0.00001, "ay":0.00001, "alpha":0.00021, "fx":[-0.00041,-0.00041,0.00074,0.00074], "fy":[0.0007,-0.00047,-0.00047,0.0007]}, - {"t":2.28234, "x":2.83894, "y":4.66507, "heading":-0.00003, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.00051,-0.00051,0.00036,0.00036], "fy":[0.00039,-0.00049,-0.00049,0.00039]}, - {"t":2.32968, "x":2.86543, "y":4.62584, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[-0.00034,-0.00034,0.00025,0.00025], "fy":[0.00027,-0.00033,-0.00033,0.00027]}, - {"t":2.37702, "x":2.89192, "y":4.58662, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00016,-0.00016,0.00016,0.00016], "fy":[0.00016,-0.00016,-0.00016,0.00016]}, - {"t":2.42436, "x":2.91841, "y":4.54739, "heading":-0.00002, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00002,-0.00002,0.00003,0.00003], "fy":[0.00003,-0.00002,-0.00002,0.00003]}, - {"t":2.4717, "x":2.9449, "y":4.50816, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00011,0.00011,-0.00011,-0.00011], "fy":[-0.00011,0.00011,0.00011,-0.00011]}, - {"t":2.51905, "x":2.97139, "y":4.46894, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00024,0.00024,-0.00025,-0.00025], "fy":[-0.00025,0.00024,0.00024,-0.00025]}, - {"t":2.56639, "x":2.99788, "y":4.42971, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00037,0.00037,-0.00039,-0.00039], "fy":[-0.00039,0.00038,0.00038,-0.00039]}, - {"t":2.61373, "x":3.02437, "y":4.39049, "heading":-0.00001, "vx":0.55952, "vy":-0.82856, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.00057,0.00057,-0.00046,-0.00046], "fy":[-0.00049,0.00057,0.00057,-0.00049]}, - {"t":2.66107, "x":3.05085, "y":4.35126, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00004, "ax":-0.00001, "ay":0.0, "alpha":-0.00024, "fx":[0.00054,0.00054,-0.00078,-0.00078], "fy":[-0.00075,0.00059,0.00059,-0.00075]}, - {"t":2.70842, "x":3.07734, "y":4.31203, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00003, "ax":0.00001, "ay":0.00001, "alpha":-0.0003, "fx":[0.00089,0.00089,-0.00072,-0.00072], "fy":[-0.0006,0.00105,0.00105,-0.0006]}, - {"t":2.75576, "x":3.10383, "y":4.27281, "heading":0.0, "vx":0.55952, "vy":-0.82856, "omega":0.00002, "ax":-1.09714, "ay":1.6247, "alpha":-0.00032, "fx":[-17.93537,-17.9353,-17.93709,-17.93716], "fy":[26.55992,26.56168,26.56161,26.55984]}, - {"t":2.8031, "x":3.12909, "y":4.2354, "heading":0.0, "vx":0.50758, "vy":-0.75165, "omega":0.0, "ax":-3.56991, "ay":5.28651, "alpha":0.0, "fx":[-58.36125,-58.36126,-58.36124,-58.36123], "fy":[86.42445,86.42444,86.42445,86.42446]}, - {"t":2.85044, "x":3.14912, "y":4.20574, "heading":0.0, "vx":0.33857, "vy":-0.50137, "omega":0.0, "ax":-3.57491, "ay":5.29392, "alpha":0.0, "fx":[-58.44302,-58.44302,-58.44301,-58.44301], "fy":[86.54554,86.54553,86.54554,86.54555]}, - {"t":2.89779, "x":3.16114, "y":4.18794, "heading":0.0, "vx":0.16932, "vy":-0.25074, "omega":0.0, "ax":-3.57658, "ay":5.29639, "alpha":0.0, "fx":[-58.4703,-58.47031,-58.4703,-58.4703], "fy":[86.58595,86.58595,86.58595,86.58596]}, - {"t":2.94513, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10468, "y":7.58251, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.72781, "ay":-2.83568, "alpha":0.0, "fx":[-93.63882,-93.63882,-93.63882,-93.63882], "fy":[-46.35794,-46.35794,-46.35794,-46.35794]}, + {"t":0.04692, "x":7.09838, "y":7.57938, "heading":0.0, "vx":-0.26873, "vy":-0.13304, "omega":0.0, "ax":-5.72725, "ay":-2.8354, "alpha":0.0, "fx":[-93.62966,-93.62966,-93.62966,-93.62966], "fy":[-46.35341,-46.35341,-46.35341,-46.35341]}, + {"t":0.09383, "x":7.07946, "y":7.57002, "heading":0.0, "vx":-0.53743, "vy":-0.26607, "omega":0.0, "ax":-5.72655, "ay":-2.83505, "alpha":0.0, "fx":[-93.61818,-93.61818,-93.61818,-93.61818], "fy":[-46.34773,-46.34773,-46.34773,-46.34773]}, + {"t":0.14075, "x":7.04795, "y":7.55442, "heading":0.0, "vx":-0.80611, "vy":-0.39908, "omega":0.0, "ax":-5.72564, "ay":-2.83461, "alpha":0.0, "fx":[-93.60338,-93.60338,-93.60338,-93.60338], "fy":[-46.3404,-46.3404,-46.3404,-46.3404]}, + {"t":0.18767, "x":7.00383, "y":7.53258, "heading":0.0, "vx":-1.07473, "vy":-0.53207, "omega":0.0, "ax":-5.72443, "ay":-2.83401, "alpha":0.0, "fx":[-93.58357,-93.58357,-93.58357,-93.58357], "fy":[-46.33059,-46.33059,-46.33059,-46.33059]}, + {"t":0.23458, "x":6.9471, "y":7.50449, "heading":0.0, "vx":-1.34331, "vy":-0.66503, "omega":0.0, "ax":-5.72273, "ay":-2.83316, "alpha":0.0, "fx":[-93.55569,-93.55569,-93.55569,-93.55569], "fy":[-46.31679,-46.31679,-46.31679,-46.31679]}, + {"t":0.2815, "x":6.87778, "y":7.47017, "heading":0.0, "vx":-1.6118, "vy":-0.79796, "omega":0.0, "ax":-5.72015, "ay":-2.83188, "alpha":0.0, "fx":[-93.51353,-93.51353,-93.51353,-93.51353], "fy":[-46.29592,-46.29592,-46.29592,-46.29592]}, + {"t":0.32842, "x":6.79586, "y":7.42962, "heading":0.0, "vx":-1.88017, "vy":-0.93082, "omega":0.0, "ax":-5.7158, "ay":-2.82973, "alpha":0.0, "fx":[-93.44239,-93.44239,-93.44239,-93.44239], "fy":[-46.2607,-46.2607,-46.2607,-46.2607]}, + {"t":0.37533, "x":6.70136, "y":7.38283, "heading":0.0, "vx":-2.14833, "vy":-1.06358, "omega":0.0, "ax":-5.70689, "ay":-2.82532, "alpha":0.0, "fx":[-93.29675,-93.29675,-93.29675,-93.29675], "fy":[-46.1886,-46.1886,-46.1886,-46.1886]}, + {"t":0.42225, "x":6.59429, "y":7.32983, "heading":0.0, "vx":-2.41608, "vy":-1.19613, "omega":0.0, "ax":-5.67844, "ay":-2.81124, "alpha":0.0, "fx":[-92.83167,-92.83167,-92.83167,-92.83167], "fy":[-45.95835,-45.95835,-45.95835,-45.95835]}, + {"t":0.46917, "x":6.47468, "y":7.27061, "heading":0.0, "vx":-2.6825, "vy":-1.32803, "omega":0.0, "ax":-0.11687, "ay":-0.05786, "alpha":0.0, "fx":[-1.91065,-1.91065,-1.91065,-1.91065], "fy":[-0.94591,-0.94591,-0.94591,-0.94591]}, + {"t":0.51608, "x":6.3487, "y":7.20824, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00004,-0.00004,-0.00004,-0.00004]}, + {"t":0.563, "x":6.22259, "y":7.14581, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.60992, "x":6.09648, "y":7.08337, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.65683, "x":5.97037, "y":7.02094, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.70375, "x":5.84426, "y":6.95851, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.75067, "x":5.71814, "y":6.89607, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.79758, "x":5.59203, "y":6.83364, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.8445, "x":5.46592, "y":6.7712, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.89142, "x":5.33981, "y":6.70877, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.93833, "x":5.2137, "y":6.64633, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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.98525, "x":5.08759, "y":6.5839, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.03217, "x":4.96148, "y":6.52147, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.07909, "x":4.83537, "y":6.45903, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.126, "x":4.70925, "y":6.3966, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.17292, "x":4.58314, "y":6.33416, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.21984, "x":4.45703, "y":6.27173, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "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":1.26675, "x":4.33092, "y":6.2093, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":1.31367, "x":4.20481, "y":6.14686, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "omega":0.0, "ax":0.0, "ay":-0.00001, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[-0.00009,-0.00009,-0.00009,-0.00009]}, + {"t":1.36059, "x":4.0787, "y":6.08443, "heading":0.0, "vx":-2.68798, "vy":-1.33074, "omega":0.0, "ax":0.00002, "ay":-0.00004, "alpha":0.0, "fx":[0.00035,0.00035,0.00035,0.00035], "fy":[-0.00072,-0.00072,-0.00072,-0.00072]}, + {"t":1.4075, "x":3.95259, "y":6.02199, "heading":0.0, "vx":-2.68798, "vy":-1.33075, "omega":0.0, "ax":0.00017, "ay":-0.00034, "alpha":0.0, "fx":[0.00272,0.00272,0.00272,0.00272], "fy":[-0.0055,-0.0055,-0.0055,-0.0055]}, + {"t":1.45442, "x":3.82648, "y":5.95956, "heading":0.0, "vx":-2.68797, "vy":-1.33076, "omega":0.0, "ax":0.00128, "ay":-0.00258, "alpha":0.0, "fx":[0.0209,0.0209,0.0209,0.0209], "fy":[-0.0422,-0.0422,-0.0422,-0.0422]}, + {"t":1.50134, "x":3.70037, "y":5.89712, "heading":0.0, "vx":-2.68791, "vy":-1.33088, "omega":0.0, "ax":0.00982, "ay":-0.01983, "alpha":0.0, "fx":[0.16056,0.16056,0.16056,0.16056], "fy":[-0.32413,-0.32413,-0.32413,-0.32413]}, + {"t":1.54825, "x":3.57427, "y":5.83466, "heading":0.0, "vx":-2.68745, "vy":-1.33181, "omega":0.0, "ax":0.07562, "ay":-0.15207, "alpha":0.0, "fx":[1.23617,1.23617,1.23617,1.23617], "fy":[-2.48612,-2.48612,-2.48612,-2.48612]}, + {"t":1.59517, "x":3.44827, "y":5.77201, "heading":0.0, "vx":-2.6839, "vy":-1.33895, "omega":0.0, "ax":0.57108, "ay":-1.11711, "alpha":0.0, "fx":[9.33607,9.33607,9.33607,9.33607], "fy":[-18.26258,-18.26258,-18.26258,-18.26258]}, + {"t":1.64209, "x":3.32297, "y":5.70796, "heading":0.0, "vx":-2.65711, "vy":-1.39136, "omega":0.0, "ax":2.29646, "ay":-4.02243, "alpha":0.0, "fx":[37.54277,37.54277,37.54277,37.54277], "fy":[-65.75911,-65.75911,-65.75911,-65.75911]}, + {"t":1.689, "x":3.20084, "y":5.63825, "heading":0.0, "vx":-2.54937, "vy":-1.58008, "omega":0.0, "ax":4.65662, "ay":-3.75571, "alpha":0.0, "fx":[76.12686,76.12686,76.12686,76.12686], "fy":[-61.39864,-61.39864,-61.39864,-61.39864]}, + {"t":1.73592, "x":3.08636, "y":5.55999, "heading":0.0, "vx":-2.33089, "vy":-1.75628, "omega":0.0, "ax":5.96663, "ay":2.1575, "alpha":0.0, "fx":[97.54304,97.54304,97.54304,97.54304], "fy":[35.27101,35.27101,35.27101,35.27101]}, + {"t":1.78284, "x":2.98356, "y":5.47996, "heading":0.0, "vx":-2.05096, "vy":-1.65506, "omega":0.0, "ax":5.8633, "ay":2.48973, "alpha":0.0, "fx":[95.85387,95.85387,95.85387,95.85387], "fy":[40.70229,40.70229,40.70229,40.70229]}, + {"t":1.82975, "x":2.89379, "y":5.40505, "heading":0.0, "vx":-1.77587, "vy":-1.53825, "omega":0.0, "ax":5.82316, "ay":2.60383, "alpha":0.0, "fx":[95.19761,95.19761,95.19761,95.19761], "fy":[42.56771,42.56771,42.56771,42.56771]}, + {"t":1.87667, "x":2.81688, "y":5.33575, "heading":0.0, "vx":-1.50267, "vy":-1.41609, "omega":0.0, "ax":5.80195, "ay":2.66148, "alpha":0.0, "fx":[94.85078,94.85078,94.85078,94.85078], "fy":[43.51016,43.51016,43.51016,43.51016]}, + {"t":1.92359, "x":2.75277, "y":5.27224, "heading":0.0, "vx":-1.23046, "vy":-1.29122, "omega":0.0, "ax":5.78884, "ay":2.69626, "alpha":0.0, "fx":[94.6365,94.6365,94.6365,94.6365], "fy":[44.07876,44.07876,44.07876,44.07876]}, + {"t":1.9705, "x":2.70141, "y":5.21463, "heading":0.0, "vx":-0.95887, "vy":-1.16472, "omega":0.0, "ax":5.77994, "ay":2.71953, "alpha":0.0, "fx":[94.491,94.491,94.491,94.491], "fy":[44.45914,44.45914,44.45914,44.45914]}, + {"t":2.01742, "x":2.66279, "y":5.16298, "heading":0.0, "vx":-0.68769, "vy":-1.03713, "omega":0.0, "ax":5.7735, "ay":2.73619, "alpha":0.0, "fx":[94.38577,94.38577,94.38577,94.38577], "fy":[44.7315,44.7315,44.7315,44.7315]}, + {"t":2.06434, "x":2.63688, "y":5.11733, "heading":0.0, "vx":-0.41682, "vy":-0.90876, "omega":0.0, "ax":6.13606, "ay":-1.75329, "alpha":0.0, "fx":[100.31295,100.31295,100.31295,100.31295], "fy":[-28.66301,-28.66301,-28.66301,-28.66301]}, + {"t":2.11147, "x":2.62405, "y":5.07254, "heading":0.0, "vx":-0.12758, "vy":-0.9914, "omega":0.0, "ax":6.36089, "ay":0.14024, "alpha":0.0, "fx":[103.98844,103.98844,103.98844,103.98844], "fy":[2.29258,2.29258,2.29258,2.29258]}, + {"t":2.15861, "x":2.6251, "y":5.02597, "heading":0.0, "vx":0.17225, "vy":-0.98479, "omega":0.0, "ax":5.92803, "ay":1.96988, "alpha":0.0, "fx":[96.91194,96.91194,96.91194,96.91194], "fy":[32.20377,32.20377,32.20377,32.20377]}, + {"t":2.20575, "x":2.6398, "y":4.98174, "heading":0.0, "vx":0.45168, "vy":-0.89194, "omega":0.0, "ax":2.07262, "ay":1.20108, "alpha":0.0, "fx":[33.88332,33.88332,33.88332,33.88332], "fy":[19.63543,19.63543,19.63543,19.63543]}, + {"t":2.25288, "x":2.6634, "y":4.94103, "heading":0.0, "vx":0.54938, "vy":-0.83532, "omega":0.0, "ax":0.04194, "ay":0.02765, "alpha":0.0, "fx":[0.6856,0.6856,0.6856,0.6856], "fy":[0.45206,0.45206,0.45206,0.45206]}, + {"t":2.30002, "x":2.68934, "y":4.90168, "heading":0.0, "vx":0.55135, "vy":-0.83402, "omega":0.0, "ax":0.00076, "ay":0.0005, "alpha":0.0, "fx":[0.01237,0.01237,0.01237,0.01237], "fy":[0.00818,0.00818,0.00818,0.00818]}, + {"t":2.34716, "x":2.71533, "y":4.86237, "heading":0.0, "vx":0.55139, "vy":-0.83399, "omega":0.0, "ax":0.00001, "ay":0.00001, "alpha":0.0, "fx":[0.00022,0.00022,0.00022,0.00022], "fy":[0.00015,0.00015,0.00015,0.00015]}, + {"t":2.39429, "x":2.74132, "y":4.82306, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.44143, "x":2.76731, "y":4.78375, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.48857, "x":2.7933, "y":4.74444, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.53571, "x":2.81929, "y":4.70513, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.58284, "x":2.84528, "y":4.66581, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.62998, "x":2.87127, "y":4.6265, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.67712, "x":2.89726, "y":4.58719, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.72425, "x":2.92325, "y":4.54788, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.77139, "x":2.94925, "y":4.50857, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.81853, "x":2.97524, "y":4.46925, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.86566, "x":3.00123, "y":4.42994, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.9128, "x":3.02722, "y":4.39063, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":2.95994, "x":3.05321, "y":4.35132, "heading":0.0, "vx":0.55139, "vy":-0.83399, "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":3.00707, "x":3.0792, "y":4.31201, "heading":0.0, "vx":0.55139, "vy":-0.83399, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00008,-0.00008,-0.00008,-0.00008], "fy":[0.00012,0.00012,0.00012,0.00012]}, + {"t":3.05421, "x":3.10519, "y":4.2727, "heading":0.0, "vx":0.55139, "vy":-0.83399, "omega":0.0, "ax":-1.1321, "ay":1.71234, "alpha":0.0, "fx":[-18.50773,-18.50773,-18.50773,-18.50773], "fy":[27.99351,27.99351,27.99351,27.99351]}, + {"t":3.10135, "x":3.12992, "y":4.23529, "heading":0.0, "vx":0.49803, "vy":-0.75328, "omega":0.0, "ax":-3.51797, "ay":5.32104, "alpha":0.0, "fx":[-57.51209,-57.51209,-57.51209,-57.51209], "fy":[86.98881,86.98881,86.98881,86.98881]}, + {"t":3.14848, "x":3.14949, "y":4.20569, "heading":0.0, "vx":0.3322, "vy":-0.50246, "omega":0.0, "ax":-3.52294, "ay":5.32856, "alpha":0.0, "fx":[-57.59336,-57.59336,-57.59336,-57.59336], "fy":[87.11173,87.11173,87.11173,87.11173]}, + {"t":3.19562, "x":3.16124, "y":4.18793, "heading":0.0, "vx":0.16614, "vy":-0.25129, "omega":0.0, "ax":-3.5246, "ay":5.33107, "alpha":0.0, "fx":[-57.62049,-57.62049,-57.62049,-57.62049], "fy":[87.15276,87.15276,87.15276,87.15276]}, + {"t":3.24276, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 93c2b392..7949a764 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -358,7 +358,7 @@ public Command CMtoGH() { // algae path .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), steps.get("CMtoG").cmd())); routine - .observe(steps.get("CMtoG").done()) + .observe(steps.get("CMtoG").done()) // TODO change to time based .onTrue(Commands.sequence(scoreCoralInAuto(() -> steps.get("CMtoG").getFinalPose().get()))); routine .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) @@ -456,11 +456,27 @@ public Command LOtoA() { // 2910 // run first path .active() .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) - .whileTrue( + .whileTrue(Commands.sequence(steps.get("LOtoA").resetOdometry(), steps.get("LOtoA").cmd())); + routine + .observe( + steps.get("LOtoA").atTime(steps.get("LOtoA").getRawTrajectory().getTotalTime() - 0.3)) + .onTrue( Commands.sequence( - routine.trajectory("LOtoA").resetOdometry(), routine.trajectory("LOtoA").cmd())); + scoreCoralInAuto(() -> steps.get("LOtoA").getFinalPose().get()), + Commands.runOnce(() -> autoGroundCoralIntake = true), + swerve + .driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0)) + .until( + () -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)), + steps.get("AtoB").cmd())); - runGroundPath(routine, "LO", "A", "B", steps); + // routine + // .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) + // .onTrue( + // Commands.sequence( + // swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2))); + // runGroundPath(routine, "LO", "A", "B", steps); + // runCoralPath(routine, "LO", "A", "B", steps); // ---------------- // runGroundPath(routine, "A", "B", "B", steps); // TODO dealgae - merge from prechamps @@ -493,6 +509,7 @@ public void runGroundPath( public Command scoreCoralInAuto(Supplier trajEndPose) { return Commands.sequence( + Commands.print("scoring"), Commands.waitUntil( new Trigger( () -> @@ -530,11 +547,13 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { autoPreScore = false; })) .raceWith( - AutoAim.translateToPose( - swerve, - () -> CoralTargets.getClosestTarget(trajEndPose.get()), - ChassisSpeeds::new, - new Constraints(1.5, 1.0))); + Commands.print("autoaiming frrr") + .andThen( + AutoAim.translateToPose( + swerve, + () -> CoralTargets.getClosestTarget(trajEndPose.get()), + ChassisSpeeds::new, + new Constraints(1.5, 1.0)))); } public Command intakeCoralInAuto(Supplier> pose) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1fe6fbb6..5467006d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1166,7 +1166,7 @@ private void addAutos() { autoChooser.addOption("4.5 R Inside", autos.RItoD()); autoChooser.addOption("Push Auto", autos.PMtoPL()); autoChooser.addOption("Algae auto", autos.CMtoGH()); - autoChooser.addOption("2910 auto", autos.LOtoA()); + autoChooser.addOption("!!! DO NOT RUN!! 2910 auto", autos.LOtoA()); } /** Scales a joystick value for teleop driving */ From 361f235c8ac21d55a60a5f9251c549ae6ee46d74 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 15 Apr 2025 19:26:38 -0700 Subject: [PATCH 047/154] pov plane :plane: --- src/main/deploy/choreo/AtoB better.traj | 155 +++++++++++++++++ src/main/deploy/choreo/AtoB.traj | 160 +++++++++-------- src/main/deploy/choreo/BtoB.traj | 217 +++++++++++------------- src/main/java/frc/robot/Autos.java | 47 ++++- 4 files changed, 378 insertions(+), 201 deletions(-) create mode 100644 src/main/deploy/choreo/AtoB better.traj diff --git a/src/main/deploy/choreo/AtoB better.traj b/src/main/deploy/choreo/AtoB better.traj new file mode 100644 index 00000000..8e287702 --- /dev/null +++ b/src/main/deploy/choreo/AtoB better.traj @@ -0,0 +1,155 @@ +{ + "name":"AtoB better", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.7247390747070312, "y":4.084929943084717, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.307336926460266, "y":5.2087788581848145, "heading":-0.08749516032270202, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":0.852199912071228, "y":5.028620719909668, "heading":-0.7188302916734751, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":0.7194516062736511, "y":4.516591548919678, "heading":-0.7140904273857694, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.601279616355896, "y":3.3692667484283447, "heading":-0.5813800977925542, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":false}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.7247390747070312 m", "val":2.7247390747070312}, "y":{"exp":"4.084929943084717 m", "val":4.084929943084717}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3073369264602661 m", "val":1.307336926460266}, "y":{"exp":"5.2087788581848145 m", "val":5.2087788581848145}, "heading":{"exp":"-87.49516032270202 mrad", "val":-0.08749516032270202}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"0.852199912071228 m", "val":0.852199912071228}, "y":{"exp":"5.028620719909668 m", "val":5.028620719909668}, "heading":{"exp":"-0.7188302916734751 rad", "val":-0.7188302916734751}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"0.7194516062736511 m", "val":0.7194516062736511}, "y":{"exp":"4.516591548919678 m", "val":4.516591548919678}, "heading":{"exp":"-0.7140904273857694 rad", "val":-0.7140904273857694}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.601279616355896 m", "val":1.601279616355896}, "y":{"exp":"3.3692667484283447 m", "val":3.3692667484283447}, "heading":{"exp":"-0.5813800977925542 rad", "val":-0.5813800977925542}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":false}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.91306,1.14726,1.4959,2.05812,3.0279], + "samples":[ + {"t":0.0, "x":2.72474, "y":4.08493, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.08498, "ay":5.47462, "alpha":-3.39028, "fx":[-43.05186,-29.48869,-56.11438,-73.07916], "fy":[95.09497,100.19714,88.1108,74.59577]}, + {"t":0.03044, "x":2.72331, "y":4.08747, "heading":0.0, "vx":-0.09389, "vy":0.16662, "omega":-0.10318, "ax":-3.12893, "ay":5.45011, "alpha":-3.37382, "fx":[-44.09088,-30.27696,-56.65449,-73.58615], "fy":[94.60444,99.9528,87.75574,74.08278]}, + {"t":0.06087, "x":2.719, "y":4.09506, "heading":-0.00314, "vx":-0.18912, "vy":0.3325, "omega":-0.20587, "ax":-3.17872, "ay":5.42186, "alpha":-3.35386, "fx":[-45.35875,-31.14023,-57.21059,-74.15454], "fy":[93.98794,99.67688,87.38426,73.49935]}, + {"t":0.09131, "x":2.71178, "y":4.10769, "heading":-0.00941, "vx":-0.28587, "vy":0.49751, "omega":-0.30794, "ax":-3.23552, "ay":5.38894, "alpha":-3.33013, "fx":[-46.87937,-32.10561,-57.79926,-74.79412], "fy":[93.22126,99.35809,86.98455,72.83197]}, + {"t":0.12174, "x":2.70158, "y":4.12533, "heading":-0.01878, "vx":-0.38434, "vy":0.66153, "omega":-0.4093, "ax":-3.30085, "ay":5.35011, "alpha":-3.30233, "fx":[-48.68271,-33.20892,-58.44205,-75.51707], "fy":[92.27188,98.98062,86.54068,72.06325]}, + {"t":0.15218, "x":2.68835, "y":4.14794, "heading":-0.03124, "vx":-0.4848, "vy":0.82436, "omega":-0.5098, "ax":-3.37671, "ay":5.30365, "alpha":-3.27011, "fx":[-50.80641,-34.49822,-59.16748,-76.33916], "fy":[91.09572,98.52181,86.03067,71.1699]}, + {"t":0.18261, "x":2.67203, "y":4.17549, "heading":-0.04675, "vx":-0.58757, "vy":0.98578, "omega":-0.60933, "ax":-3.46573, "ay":5.24715, "alpha":-3.23295, "fx":[-53.29783,-36.03895,-60.01415,-77.28143], "fy":[89.63213,97.94837,85.42349,70.11971]}, + {"t":0.21305, "x":2.65254, "y":4.20792, "heading":-0.0653, "vx":-0.69305, "vy":1.14547, "omega":-0.70772, "ax":-3.57147, "ay":5.17716, "alpha":-3.19012, "fx":[-56.21681,-37.92218,-61.03558,-78.37266], "fy":[87.79609,97.20978,84.67396,68.86681]}, + {"t":0.24348, "x":2.6298, "y":4.24518, "heading":-0.08684, "vx":-0.80175, "vy":1.30304, "omega":-0.80482, "ax":-3.69887, "ay":5.0885, "alpha":-3.1403, "fx":[-59.63905,-40.27773,-62.30791,-79.65304], "fy":[85.46555,96.22638,83.71389,67.3436]}, + {"t":0.27392, "x":2.60368, "y":4.28719, "heading":-0.11133, "vx":-0.91433, "vy":1.45791, "omega":-0.90039, "ax":-3.85485, "ay":4.97321, "alpha":-3.08102, "fx":[-63.65977,-43.29574,-63.94282,-81.17977], "fy":[82.46056,94.86654,82.43584,65.44696]}, + {"t":0.30435, "x":2.57407, "y":4.33387, "heading":-0.13873, "vx":-1.03165, "vy":1.60927, "omega":-0.99416, "ax":-4.04945, "ay":4.81833, "alpha":-3.00726, "fx":[-68.39561,-47.26271,-66.10945,-83.03548], "fy":[78.50757,92.9005,80.66115,63.01305]}, + {"t":0.33479, "x":2.54079, "y":4.38508, "heading":-0.16899, "vx":-1.1549, "vy":1.75592, "omega":-1.08569, "ax":-4.29733, "ay":4.60175, "alpha":-2.90813, "fx":[-73.97847,-52.62039,-69.07298,-85.34059], "fy":[73.17761,89.90049,78.07274,59.76843]}, + {"t":0.36522, "x":2.50365, "y":4.44065, "heading":-0.20204, "vx":-1.28569, "vy":1.89597, "omega":-1.1742, "ax":-4.61981, "ay":4.28305, "alpha":-2.75898, "fx":[-80.52112,-60.05,-73.26111,-88.26841], "fy":[65.77938,85.00993,74.0605,55.22885]}, + {"t":0.39566, "x":2.46238, "y":4.50034, "heading":-0.23777, "vx":-1.42629, "vy":2.02633, "omega":-1.25817, "ax":-5.04512, "ay":3.78297, "alpha":-2.5023, "fx":[-87.99542,-70.50996,-79.35969,-92.04724], "fy":[55.18699,76.38789,67.3296,48.47297]}, + {"t":0.42609, "x":2.41664, "y":4.56377, "heading":-0.27606, "vx":-1.57984, "vy":2.14146, "omega":-1.33433, "ax":-5.59222, "ay":2.93694, "alpha":-2.00981, "fx":[-95.8671,-84.69035,-88.27966,-96.85128], "fy":[39.6401,59.985,54.82504,37.6036]}, + {"t":0.45653, "x":2.36597, "y":4.6303, "heading":-0.31668, "vx":-1.75004, "vy":2.23085, "omega":-1.3955, "ax":-6.16747, "ay":1.42362, "alpha":-1.0632, "fx":[-102.20519,-99.54168,-99.53131,-102.02739], "fy":[16.89639,28.64586,29.04864,18.50279]}, + {"t":0.48696, "x":2.30985, "y":4.69886, "heading":-0.35915, "vx":-1.93775, "vy":2.27418, "omega":-1.42786, "ax":-6.23608, "ay":-1.06453, "alpha":0.48334, "fx":[-102.52868,-101.85417,-101.30166,-102.10733], "fy":[-13.90043,-18.34784,-20.95977,-16.40444]}, + {"t":0.5174, "x":2.24798, "y":4.76758, "heading":-0.4026, "vx":-2.12755, "vy":2.24178, "omega":-1.41315, "ax":-5.02987, "ay":-3.77252, "alpha":2.14469, "fx":[-91.68032,-84.999,-72.68086,-79.55522], "fy":[-47.8819,-59.25932,-73.72042,-65.8321]}, + {"t":0.54783, "x":2.1809, "y":4.83406, "heading":-0.44561, "vx":-2.28063, "vy":2.12696, "omega":-1.34787, "ax":-3.22226, "ay":-5.34929, "alpha":3.27636, "fx":[-71.04242,-64.34853,-38.94056,-36.3799], "fy":[-75.28496,-81.45537,-96.20603,-96.85652]}, + {"t":0.57827, "x":2.11, "y":4.89632, "heading":-0.48664, "vx":-2.3787, "vy":1.96416, "omega":-1.24815, "ax":-1.81625, "ay":-5.95849, "alpha":3.72354, "fx":[-48.45143,-48.60118,-17.95745,-3.75884], "fy":[-91.63494,-91.8991,-102.42692,-103.67907]}, + {"t":0.6087, "x":2.03676, "y":4.95334, "heading":-0.52462, "vx":-2.43398, "vy":1.78281, "omega":-1.13483, "ax":-0.88061, "ay":-6.16966, "alpha":3.79044, "fx":[-29.25663,-37.6061,-5.72864,15.00591], "fy":[-99.59084,-97.03696,-103.9608,-102.86]}, + {"t":0.63914, "x":1.96227, "y":5.00474, "heading":-0.55916, "vx":-2.46078, "vy":1.59503, "omega":-1.01946, "ax":-0.25068, "ay":-6.23414, "alpha":3.76423, "fx":[-14.42288,-29.82036,1.82763,26.02319], "fy":[-102.91386,-99.78779,-104.18805,-100.77537]}, + {"t":0.66957, "x":1.88726, "y":5.0504, "heading":-0.59019, "vx":-2.46841, "vy":1.4053, "omega":-0.9049, "ax":0.19093, "ay":-6.24236, "alpha":3.73424, "fx":[-3.19158,-24.10642,6.8136,32.97002], "fy":[-103.96601,-101.38016,-104.04083,-98.81566]}, + {"t":0.70001, "x":1.81222, "y":5.09028, "heading":-0.61773, "vx":-2.4626, "vy":1.21531, "omega":-0.79125, "ax":0.5132, "ay":-6.22871, "alpha":3.71635, "fx":[5.37701,-19.75686,10.29349,37.6456], "fy":[-103.95095,-102.36235,-103.79811,-97.19893]}, + {"t":0.73045, "x":1.73751, "y":5.12438, "heading":-0.64181, "vx":-2.44698, "vy":1.02574, "omega":-0.67814, "ax":0.75671, "ay":-6.20713, "alpha":3.70911, "fx":[12.01584,-16.33688,12.84067,40.96362], "fy":[-103.45379,-102.99919,-103.5472,-95.899]}, + {"t":0.76088, "x":1.66339, "y":5.15272, "heading":-0.66245, "vx":-2.42395, "vy":0.83682, "omega":-0.56525, "ax":0.9462, "ay":-6.1835, "alpha":3.70853, "fx":[17.23949,-13.57292,14.78547,43.42217], "fy":[-102.76083,-103.42838,-103.31361,-94.85075]}, + {"t":0.79132, "x":1.59005, "y":5.17533, "heading":-0.67966, "vx":-2.39515, "vy":0.64862, "omega":-0.45238, "ax":1.09731, "ay":-6.16031, "alpha":3.71126, "fx":[21.40171,-11.28734,16.32969,45.31169], "fy":[-102.01487,-103.72627,-103.10189,-93.99419]}, + {"t":0.82175, "x":1.51766, "y":5.19222, "heading":-0.69342, "vx":-2.36176, "vy":0.46113, "omega":-0.33943, "ax":1.22034, "ay":-6.13859, "alpha":3.71495, "fx":[24.74744,-9.36114,17.60286,46.81157], "fy":[-101.28822,-103.93758,-102.90951,-93.28176]}, + {"t":0.85219, "x":1.44635, "y":5.20341, "heading":-0.70376, "vx":-2.32461, "vy":0.2743, "omega":-0.22636, "ax":1.32227, "ay":-6.1187, "alpha":3.71803, "fx":[27.44905,-7.71209,18.69155,48.03797], "fy":[-100.61744,-104.08977,-102.73187,-92.67718]}, + {"t":0.88262, "x":1.37621, "y":5.20892, "heading":-0.71065, "vx":-2.28437, "vy":0.08808, "omega":-0.11321, "ax":1.40801, "ay":-6.10068, "alpha":3.71954, "fx":[29.63011,-6.28173,19.65568,49.06905], "fy":[-100.02047,-104.20044,-102.5641,-92.15301]}, + {"t":0.91306, "x":1.30734, "y":5.20878, "heading":-0.71409, "vx":-2.24152, "vy":-0.09759, "omega":0.0, "ax":1.55462, "ay":-6.18682, "alpha":0.0, "fx":[25.41508,25.41508,25.41508,25.41508], "fy":[-101.14267,-101.14267,-101.14267,-101.14267]}, + {"t":0.93257, "x":1.26389, "y":5.2057, "heading":-0.71409, "vx":-2.21118, "vy":-0.21834, "omega":0.0, "ax":1.75968, "ay":-6.13043, "alpha":0.0, "fx":[28.76736,28.76736,28.76736,28.76736], "fy":[-100.22093,-100.22093,-100.22093,-100.22093]}, + {"t":0.95209, "x":1.22107, "y":5.20027, "heading":-0.71409, "vx":-2.17683, "vy":-0.33799, "omega":0.0, "ax":1.99083, "ay":-6.05794, "alpha":0.0, "fx":[32.54633,32.54633,32.54633,32.54633], "fy":[-99.03583,-99.03583,-99.03583,-99.03583]}, + {"t":0.97161, "x":1.17896, "y":5.19252, "heading":-0.71409, "vx":-2.13798, "vy":-0.45622, "omega":0.0, "ax":2.25213, "ay":-5.96418, "alpha":0.0, "fx":[36.81805,36.81805,36.81805,36.81805], "fy":[-97.50299,-97.50299,-97.50299,-97.50299]}, + {"t":0.99112, "x":1.13766, "y":5.18248, "heading":-0.71409, "vx":-2.09403, "vy":-0.57262, "omega":0.0, "ax":2.54794, "ay":-5.84216, "alpha":0.0, "fx":[41.65395,41.65395,41.65395,41.65395], "fy":[-95.50817,-95.50817,-95.50817,-95.50817]}, + {"t":1.01064, "x":1.09728, "y":5.17019, "heading":-0.71409, "vx":-2.0443, "vy":-0.68664, "omega":0.0, "ax":2.8826, "ay":-5.68246, "alpha":0.0, "fx":[47.12509,47.12509,47.12509,47.12509], "fy":[-92.89738,-92.89738,-92.89738,-92.89738]}, + {"t":1.03016, "x":1.05793, "y":5.15571, "heading":-0.71409, "vx":-1.98804, "vy":-0.79754, "omega":0.0, "ax":3.2597, "ay":-5.47254, "alpha":0.0, "fx":[53.28983,53.28983,53.28983,53.28983], "fy":[-89.46553,-89.46553,-89.46553,-89.46553]}, + {"t":1.04967, "x":1.01975, "y":5.1391, "heading":-0.71409, "vx":-1.92442, "vy":-0.90434, "omega":0.0, "ax":3.68058, "ay":-5.19614, "alpha":0.0, "fx":[60.1705,60.1705,60.1705,60.1705], "fy":[-84.947,-84.947,-84.947,-84.947]}, + {"t":1.06919, "x":0.9829, "y":5.12046, "heading":-0.71409, "vx":-1.85259, "vy":-1.00575, "omega":0.0, "ax":4.142, "ay":-4.83332, "alpha":0.0, "fx":[67.71388,67.71388,67.71388,67.71388], "fy":[-79.01563,-79.01563,-79.01563,-79.01563]}, + {"t":1.08871, "x":0.94753, "y":5.09991, "heading":-0.71409, "vx":-1.77175, "vy":-1.10008, "omega":0.0, "ax":4.63256, "ay":-4.36198, "alpha":0.0, "fx":[75.73357,75.73357,75.73357,75.73357], "fy":[-71.31005,-71.31005,-71.31005,-71.31005]}, + {"t":1.10822, "x":0.91383, "y":5.07761, "heading":-0.71409, "vx":-1.68134, "vy":-1.18522, "omega":0.0, "ax":5.12876, "ay":-3.76237, "alpha":0.0, "fx":[83.8454,83.8454,83.8454,83.8454], "fy":[-61.50757,-61.50757,-61.50757,-61.50757]}, + {"t":1.12774, "x":0.882, "y":5.05376, "heading":-0.71409, "vx":-1.58124, "vy":-1.25864, "omega":0.0, "ax":5.59303, "ay":-3.02563, "alpha":0.0, "fx":[91.43541,91.43541,91.43541,91.43541], "fy":[-49.46335,-49.46335,-49.46335,-49.46335]}, + {"t":1.14726, "x":0.8522, "y":5.02862, "heading":-0.71409, "vx":-1.47209, "vy":-1.31769, "omega":0.0, "ax":5.96372, "ay":-2.24642, "alpha":0.0, "fx":[97.49542,97.49542,97.49542,97.49542], "fy":[-36.72461,-36.72461,-36.72461,-36.72461]}, + {"t":1.17631, "x":0.81195, "y":4.98939, "heading":-0.71409, "vx":-1.29882, "vy":-1.38296, "omega":0.0, "ax":6.16271, "ay":-1.63518, "alpha":0.0, "fx":[100.74863,100.74863,100.74863,100.74863], "fy":[-26.73212,-26.73212,-26.73212,-26.73212]}, + {"t":1.20536, "x":0.77681, "y":4.94852, "heading":-0.71409, "vx":-1.11977, "vy":-1.43047, "omega":0.0, "ax":6.27246, "ay":-1.15835, "alpha":0.0, "fx":[102.54283,102.54283,102.54283,102.54283], "fy":[-18.93686,-18.93686,-18.93686,-18.93686]}, + {"t":1.23442, "x":0.74693, "y":4.90647, "heading":-0.71409, "vx":-0.93753, "vy":-1.46412, "omega":0.0, "ax":6.33245, "ay":-0.78236, "alpha":0.0, "fx":[103.52349,103.52349,103.52349,103.52349], "fy":[-12.79011,-12.79011,-12.79011,-12.79011]}, + {"t":1.26347, "x":0.72236, "y":4.8636, "heading":-0.71409, "vx":-0.75354, "vy":-1.48686, "omega":0.0, "ax":6.36411, "ay":-0.48132, "alpha":0.0, "fx":[104.04107,104.04107,104.04107,104.04107], "fy":[-7.8686,-7.8686,-7.8686,-7.8686]}, + {"t":1.29253, "x":0.70315, "y":4.8202, "heading":-0.71409, "vx":-0.56864, "vy":-1.50084, "omega":0.0, "ax":6.3793, "ay":-0.23639, "alpha":0.0, "fx":[104.28939,104.28939,104.28939,104.28939], "fy":[-3.86455,-3.86455,-3.86455,-3.86455]}, + {"t":1.32158, "x":0.68932, "y":4.77649, "heading":-0.71409, "vx":-0.3833, "vy":-1.50771, "omega":0.0, "ax":6.38475, "ay":-0.03407, "alpha":0.0, "fx":[104.37851,104.37851,104.37851,104.37851], "fy":[-0.55704,-0.55704,-0.55704,-0.55704]}, + {"t":1.35063, "x":0.68088, "y":4.73267, "heading":-0.71409, "vx":-0.19779, "vy":-1.5087, "omega":0.0, "ax":6.38439, "ay":0.13539, "alpha":0.0, "fx":[104.37261,104.37261,104.37261,104.37261], "fy":[2.2133,2.2133,2.2133,2.2133]}, + {"t":1.37969, "x":0.67783, "y":4.6889, "heading":-0.71409, "vx":-0.0123, "vy":-1.50476, "omega":0.0, "ax":6.38056, "ay":0.2791, "alpha":0.0, "fx":[104.31008,104.31008,104.31008,104.31008], "fy":[4.5628,4.5628,4.5628,4.5628]}, + {"t":1.40874, "x":0.68017, "y":4.64529, "heading":-0.71409, "vx":0.17308, "vy":-1.49665, "omega":0.0, "ax":6.37471, "ay":0.40235, "alpha":0.0, "fx":[104.21432,104.21432,104.21432,104.21432], "fy":[6.57767,6.57767,6.57767,6.57767]}, + {"t":1.4378, "x":0.68789, "y":4.60198, "heading":-0.71409, "vx":0.35829, "vy":-1.48497, "omega":0.0, "ax":6.36771, "ay":0.5091, "alpha":0.0, "fx":[104.09985,104.09985,104.09985,104.09985], "fy":[8.32278,8.32278,8.32278,8.32278]}, + {"t":1.46685, "x":0.70098, "y":4.55905, "heading":-0.71409, "vx":0.54329, "vy":-1.47017, "omega":0.0, "ax":6.36012, "ay":0.60237, "alpha":0.0, "fx":[103.9758,103.9758,103.9758,103.9758], "fy":[9.84765,9.84765,9.84765,9.84765]}, + {"t":1.4959, "x":0.71945, "y":4.51659, "heading":-0.71409, "vx":0.72808, "vy":-1.45267, "omega":0.0, "ax":6.37916, "ay":0.26873, "alpha":-0.363, "fx":[104.38718,104.30632,104.14907,104.30618], "fy":[1.57207,4.50455,7.21722,4.27903]}, + {"t":1.52035, "x":0.73915, "y":4.48116, "heading":-0.71409, "vx":0.88402, "vy":-1.4461, "omega":-0.00887, "ax":6.31793, "ay":-0.89127, "alpha":-0.43673, "fx":[102.80554,103.41902,103.72415,103.19591], "fy":[-17.81706,-13.86076,-11.27301,-15.33149]}, + {"t":1.54479, "x":0.76265, "y":4.44555, "heading":-0.71431, "vx":1.03845, "vy":-1.46789, "omega":-0.01955, "ax":5.62794, "ay":-2.99191, "alpha":-0.53655, "fx":[90.39374,93.03736,93.65564,90.93794], "fy":[-51.92792,-47.03612,-45.7525,-50.93142]}, + {"t":1.56924, "x":0.78972, "y":4.40877, "heading":-0.71479, "vx":1.17602, "vy":-1.54102, "omega":-0.03266, "ax":4.38624, "ay":-4.61835, "alpha":-0.55225, "fx":[69.81872,74.02297,73.72566,69.25909], "fy":[-77.32554,-73.30799,-73.57442,-77.7969]}, + {"t":1.59368, "x":0.81977, "y":4.36972, "heading":-0.71558, "vx":1.28324, "vy":-1.65392, "omega":-0.04616, "ax":4.08604, "ay":-4.88235, "alpha":-0.51041, "fx":[65.09325,69.17869,68.62423,64.29989], "fy":[-81.279,-77.82657,-78.28414,-81.87847]}, + {"t":1.61812, "x":0.85236, "y":4.32784, "heading":-0.71671, "vx":1.38312, "vy":-1.77326, "omega":-0.05864, "ax":4.00048, "ay":-4.9483, "alpha":-0.45447, "fx":[63.88963,67.57905,67.00668,63.12575], "fy":[-82.14842,-79.13588,-79.58954,-82.70722]}, + {"t":1.64257, "x":0.88737, "y":4.28301, "heading":-0.71815, "vx":1.48091, "vy":-1.89422, "omega":-0.06975, "ax":3.94105, "ay":-4.98992, "alpha":-0.38044, "fx":[63.16598,66.2882,65.75896,62.5018], "fy":[-82.60046,-80.11109,-80.51591,-83.07555]}, + {"t":1.66701, "x":0.92474, "y":4.23522, "heading":-0.71985, "vx":1.57725, "vy":-2.01619, "omega":-0.07905, "ax":3.8901, "ay":-5.02156, "alpha":-0.27814, "fx":[62.67142,64.97858,64.55635,62.17684], "fy":[-82.8295,-81.02729,-81.33822,-83.17651]}, + {"t":1.69146, "x":0.96446, "y":4.18443, "heading":-0.72178, "vx":1.67234, "vy":-2.13894, "omega":-0.08585, "ax":3.83927, "ay":-5.04816, "alpha":-0.12763, "fx":[62.33889,63.41066,63.19823,62.1112], "fy":[-82.86142,-82.04123,-82.19058,-83.01808]}, + {"t":1.7159, "x":1.00649, "y":4.13064, "heading":-0.72388, "vx":1.76618, "vy":-2.26234, "omega":-0.08897, "ax":3.77929, "ay":-5.07237, "alpha":0.11575, "fx":[62.17328,61.18514,61.40123,62.37685], "fy":[-82.62438,-83.3624,-83.22002,-82.48773]}, + {"t":1.74035, "x":1.05079, "y":4.07382, "heading":-0.72606, "vx":1.85857, "vy":-2.38633, "omega":-0.08614, "ax":3.68956, "ay":-5.0956, "alpha":0.57691, "fx":[62.28409,57.22785,58.50468,63.25261], "fy":[-81.8267,-85.46797,-84.71376,-81.20475]}, + {"t":1.76479, "x":1.09732, "y":4.01397, "heading":-0.72816, "vx":1.94875, "vy":-2.51089, "omega":-0.07204, "ax":3.48452, "ay":-5.10906, "alpha":1.79173, "fx":[63.25668,46.66893,52.09883,65.83658], "fy":[-78.98824,-89.98805,-87.52842,-77.58897]}, + {"t":1.78923, "x":1.146, "y":3.95107, "heading":-0.72992, "vx":2.03393, "vy":-2.63578, "omega":-0.02824, "ax":1.85008, "ay":-3.48864, "alpha":13.94557, "fx":[67.01358,-45.47403,20.56147,78.88034], "fy":[-0.49972,-79.89413,-94.57029,-53.16664]}, + {"t":1.81368, "x":1.19627, "y":3.88559, "heading":-0.73061, "vx":2.07915, "vy":-2.72105, "omega":0.31265, "ax":-3.91396, "ay":4.32265, "alpha":5.69518, "fx":[-52.39935,-83.30131,-86.06466,-34.17803], "fy":[86.2236,56.70222,47.14182,92.60102]}, + {"t":1.83812, "x":1.24592, "y":3.82037, "heading":-0.72297, "vx":1.98348, "vy":-2.61539, "omega":0.45186, "ax":-3.89329, "ay":4.819, "alpha":2.87964, "fx":[-55.94924,-75.74773,-74.84431,-48.05003], "fy":[86.19393,69.33928,69.37899,90.2138]}, + {"t":1.86257, "x":1.29325, "y":3.75788, "heading":-0.71192, "vx":1.88831, "vy":-2.49759, "omega":0.52225, "ax":-3.8494, "ay":4.95319, "alpha":2.15683, "fx":[-56.89443,-72.70029,-71.03937,-51.08718], "fy":[86.3067,73.42713,74.61102,89.55625]}, + {"t":1.88701, "x":1.33825, "y":3.69831, "heading":-0.69916, "vx":1.79422, "vy":-2.37652, "omega":0.57498, "ax":-3.80793, "ay":5.02723, "alpha":1.8235, "fx":[-57.12969,-70.89514,-68.88988,-52.09484], "fy":[86.51573,75.60439,77.17986,89.44274]}, + {"t":1.91145, "x":1.38097, "y":3.64172, "heading":-0.6851, "vx":1.70114, "vy":-2.25363, "omega":0.61955, "ax":-3.76194, "ay":5.08489, "alpha":1.63079, "fx":[-56.98403,-69.51345,-67.24068,-52.26405], "fy":[86.82937,77.12982,78.94183,89.61188]}, + {"t":1.9359, "x":1.42143, "y":3.58815, "heading":-0.66996, "vx":1.60918, "vy":-2.12933, "omega":0.65941, "ax":-3.70216, "ay":5.14316, "alpha":1.50446, "fx":[-56.46929,-68.17518,-65.62131,-51.82741], "fy":[87.30929,78.48035,80.49553,90.03812]}, + {"t":1.96034, "x":1.45966, "y":3.53764, "heading":-0.65384, "vx":1.51868, "vy":-2.00361, "omega":0.69619, "ax":-3.60257, "ay":5.2234, "alpha":1.4139, "fx":[-55.25329,-66.38883,-63.4537,-50.48468], "fy":[88.18625,80.11224,82.35322,90.91861]}, + {"t":1.98479, "x":1.49571, "y":3.49022, "heading":-0.63682, "vx":1.43062, "vy":-1.87593, "omega":0.73075, "ax":-1.07699, "ay":6.26647, "alpha":1.16858, "fx":[-17.37195,-26.50191,-17.94137,-8.61149], "fy":[102.72201,100.71975,102.54719,103.79062]}, + {"t":2.00923, "x":1.53036, "y":3.44624, "heading":-0.61896, "vx":1.40429, "vy":-1.72275, "omega":0.75932, "ax":1.63503, "ay":6.16222, "alpha":0.76674, "fx":[24.50678,21.27723,29.2374,31.89735], "fy":[101.40756,102.11401,100.11694,99.3239]}, + {"t":2.03368, "x":1.56517, "y":3.40597, "heading":-0.6004, "vx":1.44426, "vy":-1.57212, "omega":0.77806, "ax":2.68574, "ay":5.7905, "alpha":0.56201, "fx":[41.68075,40.54263,46.30913,47.09457], "fy":[95.72255,96.19858,93.5571,93.17648]}, + {"t":2.05812, "x":1.60128, "y":3.36927, "heading":-0.58138, "vx":1.50991, "vy":-1.43058, "omega":0.7918, "ax":2.95783, "ay":5.6619, "alpha":0.44767, "fx":[46.43897,45.87444,50.38178,50.72448], "fy":[93.57384,93.84566,91.50423,91.3212]}, + {"t":2.09156, "x":1.65343, "y":3.32459, "heading":-0.5549, "vx":1.60882, "vy":-1.24124, "omega":0.80677, "ax":2.91081, "ay":5.68583, "alpha":0.42631, "fx":[45.70839,45.25198,49.56465,49.81968], "fy":[93.92365,94.13804,91.93995,91.80859]}, + {"t":2.125, "x":1.70885, "y":3.28626, "heading":-0.52792, "vx":1.70616, "vy":-1.0511, "omega":0.82102, "ax":2.85629, "ay":5.71297, "alpha":0.40165, "fx":[44.87607,44.52081,48.60312,48.77951], "fy":[94.31355,94.47532,92.44103,92.35472]}, + {"t":2.15844, "x":1.7675, "y":3.25431, "heading":-0.50047, "vx":1.80168, "vy":-0.86006, "omega":0.83445, "ax":2.79235, "ay":5.74396, "alpha":0.37286, "fx":[43.91589,43.65273,47.46046,47.56938], "fy":[94.75212,94.86718,93.02036,92.97134]}, + {"t":2.19188, "x":1.82932, "y":3.22876, "heading":-0.47256, "vx":1.89505, "vy":-0.66798, "omega":0.84692, "ax":2.71636, "ay":5.77963, "alpha":0.33882, "fx":[42.79192,42.60942,46.08656,46.14156], "fy":[95.2505,95.3259,93.69405,93.67351]}, + {"t":2.22532, "x":1.89421, "y":3.20965, "heading":-0.44424, "vx":1.98589, "vy":-0.4747, "omega":0.85825, "ax":2.62463, "ay":5.82107, "alpha":0.29795, "fx":[41.45341,41.33755,44.41142,44.42844], "fy":[95.8233,95.86719,94.48234,94.48059]}, + {"t":2.25876, "x":1.96208, "y":3.19704, "heading":-0.41554, "vx":2.07366, "vy":-0.28004, "omega":0.86822, "ax":2.5118, "ay":5.86963, "alpha":0.24802, "fx":[39.82649,39.76083,42.33432,42.33118], "fy":[96.48985,96.51128,95.4104,95.4175]}, + {"t":2.2922, "x":2.03283, "y":3.19095, "heading":-0.38651, "vx":2.15766, "vy":-0.08376, "omega":0.87651, "ax":2.36987, "ay":5.92706, "alpha":0.18568, "fx":[37.79958,37.7664,39.70553,39.69985], "fy":[97.27579,97.28396,96.50873,96.51582]}, + {"t":2.32564, "x":2.10631, "y":3.19147, "heading":-0.3572, "vx":2.23691, "vy":0.11444, "omega":0.88272, "ax":2.18631, "ay":5.99545, "alpha":0.10582, "fx":[35.19658,35.18071,36.29379,36.29689], "fy":[98.21464,98.2173,97.81144,97.81333]}, + {"t":2.35908, "x":2.18234, "y":3.19864, "heading":-0.32768, "vx":2.31002, "vy":0.31494, "omega":0.88626, "ax":1.94059, "ay":6.07699, "alpha":0.00016, "fx":[31.72407,31.72403,31.72575,31.72578], "fy":[99.34756,99.34756,99.34701,99.34701]}, + {"t":2.39253, "x":2.26067, "y":3.21257, "heading":-0.29804, "vx":2.37491, "vy":0.51815, "omega":0.88626, "ax":1.59698, "ay":6.1726, "alpha":-0.1454, "fx":[26.85917,26.9198,25.36856,25.28302], "fy":[100.71313,100.70252,101.10468,101.12056]}, + {"t":2.42597, "x":2.34098, "y":3.23335, "heading":-0.2684, "vx":2.42832, "vy":0.72457, "omega":0.8814, "ax":1.08913, "ay":6.27642, "alpha":-0.35639, "fx":[19.58994,19.90148,16.0956,15.6337], "fy":[102.29186,102.24817,102.91739,102.97239]}, + {"t":2.45941, "x":2.42279, "y":3.26109, "heading":-0.23893, "vx":2.46474, "vy":0.93445, "omega":0.86948, "ax":0.28585, "ay":6.35381, "alpha":-0.68141, "fx":[7.75997,8.98175,1.84534,0.10561], "fy":[103.7331,103.67381,104.05241,104.03113]}, + {"t":2.49285, "x":2.50537, "y":3.29589, "heading":-0.20985, "vx":2.4743, "vy":1.14693, "omega":0.8467, "ax":-1.07032, "ay":6.24984, "alpha":-1.20707, "fx":[-13.5767,-9.25193,-20.85287,-26.30909], "fy":[102.92047,103.4902,101.81705,100.464]}, + {"t":2.52629, "x":2.58752, "y":3.33774, "heading":-0.18154, "vx":2.4385, "vy":1.35593, "omega":0.80633, "ax":-3.28961, "ay":5.37908, "alpha":-1.98455, "fx":[-53.28267,-40.51458,-54.63856,-66.67937], "fy":[88.72593,95.43171,88.2279,79.36547]}, + {"t":2.55973, "x":2.66722, "y":3.38609, "heading":-0.15458, "vx":2.3285, "vy":1.53581, "omega":0.73997, "ax":-5.60943, "ay":2.806, "alpha":-2.70164, "fx":[-96.95902,-82.1259,-88.83912,-98.88984], "fy":[35.8508,62.98824,53.54108,31.11074]}, + {"t":2.59317, "x":2.74195, "y":3.43902, "heading":-0.12983, "vx":2.14092, "vy":1.62964, "omega":0.64962, "ax":-6.2855, "ay":-0.16631, "alpha":-2.61256, "fx":[-101.67639,-103.05969,-103.21473,-103.07325], "fy":[-20.23346,10.76227,11.40891,-12.81298]}, + {"t":2.62661, "x":2.81003, "y":3.49342, "heading":-0.10811, "vx":1.93072, "vy":1.62408, "omega":0.56226, "ax":-5.99676, "ay":-1.98727, "alpha":-2.18077, "fx":[-92.25182,-100.55681,-102.35164,-96.98223], "fy":[-47.8746,-25.96669,-18.4386,-37.67276]}, + {"t":2.66005, "x":2.87124, "y":3.54662, "heading":-0.0893, "vx":1.73019, "vy":1.55762, "omega":0.48933, "ax":-5.59885, "ay":-2.96853, "alpha":-1.86449, "fx":[-84.30942,-93.5403,-97.58836,-90.68435], "fy":[-61.05521,-45.52463,-36.29796,-51.24161]}, + {"t":2.69349, "x":2.92597, "y":3.59705, "heading":-0.07294, "vx":1.54296, "vy":1.45835, "omega":0.42698, "ax":-5.27495, "ay":-3.53412, "alpha":-1.65238, "fx":[-78.72503,-87.57426,-92.90712,-85.73497], "fy":[-68.25745,-56.3646,-47.19209,-59.29011]}, + {"t":2.72693, "x":2.97462, "y":3.64384, "heading":-0.05866, "vx":1.36656, "vy":1.34017, "omega":0.37173, "ax":-5.02845, "ay":-3.89002, "alpha":-1.50453, "fx":[-74.75593,-83.05875,-89.03693,-81.97072], "fy":[-72.67894,-62.9576,-54.25802,-64.48329]}, + {"t":2.76037, "x":3.01751, "y":3.68648, "heading":-0.04623, "vx":1.19841, "vy":1.21009, "omega":0.32141, "ax":-4.83996, "ay":-4.13084, "alpha":-1.3968, "fx":[-71.8342,-79.64926,-85.93895,-79.07392], "fy":[-75.6335,-67.30444,-59.12129,-68.06612]}, + {"t":2.79381, "x":3.05488, "y":3.72464, "heading":-0.03548, "vx":1.03656, "vy":1.07195, "omega":0.2747, "ax":-4.69294, "ay":-4.30319, "alpha":-1.31527, "fx":[-69.60886,-77.02213,-83.4531,-76.79857], "fy":[-77.73377,-70.35603,-62.63772,-70.66837]}, + {"t":2.82725, "x":3.08691, "y":3.75808, "heading":-0.0263, "vx":0.87963, "vy":0.92805, "omega":0.23072, "ax":-4.57579, "ay":-4.43203, "alpha":-1.25161, "fx":[-67.86382,-74.94936,-81.4341,-74.97448], "fy":[-79.29753,-72.60493,-65.28335,-72.63494]}, + {"t":2.86069, "x":3.11377, "y":3.78664, "heading":-0.01858, "vx":0.72661, "vy":0.77984, "omega":0.18887, "ax":-4.48057, "ay":-4.53168, "alpha":-1.20062, "fx":[-66.46188,-73.27706,-79.77067,-73.48531], "fy":[-80.50408,-74.32688,-67.33862,-74.16805]}, + {"t":2.89413, "x":3.13556, "y":3.81018, "heading":-0.01227, "vx":0.57678, "vy":0.6283, "omega":0.14872, "ax":-4.40181, "ay":-4.61092, "alpha":-1.15889, "fx":[-65.3127,-71.90062,-78.38089,-72.25041], "fy":[-81.46156,-75.68657,-68.9775,-75.39315]}, + {"t":2.92757, "x":3.15239, "y":3.82861, "heading":-0.00729, "vx":0.42958, "vy":0.47411, "omega":0.10996, "ax":-4.33567, "ay":-4.67533, "alpha":-1.12412, "fx":[-64.35472,-70.74748,-77.20468,-71.21276], "fy":[-82.23881,-76.78778,-70.31283,-76.39179]}, + {"t":2.96101, "x":3.16433, "y":3.84185, "heading":-0.00362, "vx":0.28459, "vy":0.31776, "omega":0.07237, "ax":-4.27939, "ay":-4.72869, "alpha":-1.0947, "fx":[-63.54476,-69.76617,-76.19761,-70.33112], "fy":[-82.88157,-77.69884,-71.42065,-77.21912]}, + {"t":2.99445, "x":3.17146, "y":3.84983, "heading":-0.0012, "vx":0.14149, "vy":0.15963, "omega":0.03576, "ax":-4.23096, "ay":-4.77357, "alpha":-1.06948, "fx":[-62.85165,-68.91933,-75.32634,-69.57504], "fy":[-83.42137,-78.46644,-72.3539,-77.91365]}, + {"t":3.0279, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/AtoB.traj b/src/main/deploy/choreo/AtoB.traj index bcf37a9a..16e33a66 100644 --- a/src/main/deploy/choreo/AtoB.traj +++ b/src/main/deploy/choreo/AtoB.traj @@ -4,31 +4,29 @@ "snapshot":{ "waypoints":[ {"x":2.7247390747070312, "y":4.084929943084717, "heading":3.141592653589793, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.667668342590332, "y":4.032181262969971, "heading":3.141592653589793, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4166265726089478, "y":4.017168045043945, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2978549003601074, "y":4.0045623779296875, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"2.7247390747070312 m", "val":2.7247390747070312}, "y":{"exp":"4.084929943084717 m", "val":4.084929943084717}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.667668342590332 m", "val":1.667668342590332}, "y":{"exp":"4.032181262969971 m", "val":4.032181262969971}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4166265726089478 m", "val":1.4166265726089478}, "y":{"exp":"4.017168045043945 m", "val":4.017168045043945}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2978549003601074 m", "val":1.2978549003601074}, "y":{"exp":"4.0045623779296875 m", "val":4.0045623779296875}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,80 +34,76 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.52985,0.76716,1.03679,2.16108], + "waypoints":[0.0,0.45838,0.91847,2.10529], "samples":[ - {"t":0.0, "x":2.72474, "y":4.08493, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.34728, "ay":-0.7307, "alpha":-0.35411, "fx":[-104.00489,-103.58165,-103.51146,-103.96601], "fy":[-9.85684,-13.60535,-14.10103,-10.21906]}, - {"t":0.04076, "x":2.71947, "y":4.08432, "heading":3.14159, "vx":-0.2587, "vy":-0.02978, "omega":-0.01443, "ax":-6.34645, "ay":-0.72982, "alpha":-0.35302, "fx":[-103.99041,-103.56899,-103.49866,-103.95116], "fy":[-9.84921,-13.58599,-14.07937,-10.20996]}, - {"t":0.08152, "x":2.70365, "y":4.0825, "heading":3.141, "vx":-0.51737, "vy":-0.05953, "omega":-0.02882, "ax":-6.34533, "ay":-0.72864, "alpha":-0.35157, "fx":[-103.9712,-103.55226,-103.48146,-103.93123], "fy":[-9.83805,-13.55898,-14.05145,-10.19906]}, - {"t":0.12227, "x":2.67729, "y":4.07947, "heading":3.13983, "vx":-0.77599, "vy":-0.08922, "omega":-0.04315, "ax":-6.34376, "ay":-0.72699, "alpha":-0.34954, "fx":[-103.94435,-103.52892,-103.4573,-103.90327], "fy":[-9.82185,-13.52047,-14.01293,-10.1845]}, - {"t":0.16303, "x":2.6404, "y":4.07523, "heading":3.13807, "vx":-1.03454, "vy":-0.11886, "omega":-0.0574, "ax":-6.34142, "ay":-0.72452, "alpha":-0.34649, "fx":[-103.90406,-103.49387,-103.42105,-103.86135], "fy":[-9.79759,-13.46278,-13.95515,-10.16259]}, - {"t":0.20379, "x":2.59297, "y":4.06979, "heading":3.13573, "vx":-1.293, "vy":-0.14839, "omega":-0.07152, "ax":-6.3375, "ay":-0.7204, "alpha":-0.34142, "fx":[-103.83678,-103.43519,-103.36073,-103.79164], "fy":[-9.75819,-13.36796,-13.85792,-10.12475]}, - {"t":0.24455, "x":2.535, "y":4.06314, "heading":3.13282, "vx":-1.5513, "vy":-0.17775, "omega":-0.08543, "ax":-6.32966, "ay":-0.71217, "alpha":-0.33128, "fx":[-103.70183,-103.317,-103.24032,-103.65271], "fy":[-9.6824,-13.18221,-13.66084,-10.04524]}, - {"t":0.2853, "x":2.46652, "y":4.0553, "heading":3.12933, "vx":-1.80929, "vy":-0.20677, "omega":-0.09894, "ax":-6.30609, "ay":-0.68754, "alpha":-0.30087, "fx":[-103.29529,-102.95848,-102.87893,-103.23787], "fy":[-9.46402,-12.63646,-13.06348,-9.79618]}, - {"t":0.32606, "x":2.38754, "y":4.0463, "heading":3.1253, "vx":-2.06631, "vy":-0.2348, "omega":-0.1112, "ax":0.87667, "ay":2.21037, "alpha":5.25367, "fx":[32.60995,27.83893,-1.11294,-2.00864], "fy":[21.61968,47.65043,50.89191,24.37962]}, - {"t":0.36682, "x":2.30405, "y":4.03857, "heading":3.12077, "vx":-2.03058, "vy":-0.14471, "omega":0.10293, "ax":6.29569, "ay":0.77394, "alpha":0.42214, "fx":[103.24202,102.71919,102.57819,103.15097], "fy":[10.1348,14.54097,15.26161,10.6721]}, - {"t":0.40758, "x":2.22652, "y":4.03332, "heading":3.12497, "vx":-1.77398, "vy":-0.11316, "omega":0.12013, "ax":6.32434, "ay":0.7556, "alpha":0.39225, "fx":[103.67301,103.1942,103.08773,103.60852], "fy":[10.01309,14.13428,14.77254,10.49083]}, - {"t":0.44833, "x":2.15947, "y":4.02933, "heading":3.12986, "vx":-1.51621, "vy":-0.08237, "omega":0.13612, "ax":6.33392, "ay":0.74944, "alpha":0.38227, "fx":[103.81705,103.3523,103.25843,103.76216], "fy":[9.97801,14.00469,14.6025,10.42232]}, - {"t":0.48909, "x":2.10293, "y":4.0266, "heading":3.13541, "vx":-1.25806, "vy":-0.05182, "omega":0.1517, "ax":6.33871, "ay":0.74634, "alpha":0.37728, "fx":[103.88874,103.43062,103.34451,103.83972], "fy":[9.96568,13.94622,14.51207,10.38122]}, - {"t":0.52985, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-0.99971, "vy":-0.0214, "omega":0.16708, "ax":-6.35188, "ay":0.51615, "alpha":0.31489, "fx":[-103.96597,-103.68136,-103.72514,-103.99218], "fy":[6.84312,10.3028,9.9794,6.62719]}, - {"t":0.55358, "x":2.03141, "y":4.02474, "heading":-3.13763, "vx":-1.15044, "vy":-0.00915, "omega":0.17455, "ax":-6.34718, "ay":0.51931, "alpha":0.2891, "fx":[-103.87764,-103.61556,-103.65861,-103.90546], "fy":[7.03573,10.20465,9.89842,6.8198]}, - {"t":0.57731, "x":2.00232, "y":4.02467, "heading":-3.13349, "vx":-1.30107, "vy":0.00317, "omega":0.18141, "ax":-6.34016, "ay":0.52398, "alpha":0.25065, "fx":[-103.7462,-103.5181,-103.55864,-103.77495], "fy":[7.31646,10.05468,9.78181,7.11119]}, - {"t":0.60104, "x":1.96966, "y":4.02489, "heading":-3.12918, "vx":-1.45153, "vy":0.01561, "omega":0.18736, "ax":-6.32852, "ay":0.53163, "alpha":0.18719, "fx":[-103.5297,-103.35839,-103.39213,-103.55663], "fy":[7.76869,9.80267,9.59478,7.59828]}, - {"t":0.62477, "x":1.93343, "y":4.02541, "heading":-3.12473, "vx":-1.60171, "vy":0.02822, "omega":0.1918, "ax":-6.30551, "ay":0.54646, "alpha":0.0626, "fx":[-103.10553,-103.04771,-103.06101,-103.11803], "fy":[8.63068,9.30393,9.23436,8.56525]}, - {"t":0.6485, "x":1.89364, "y":4.02624, "heading":-3.12018, "vx":-1.75134, "vy":0.04119, "omega":0.19329, "ax":-6.23866, "ay":0.58766, "alpha":-0.29303, "fx":[-101.90031,-102.17634,-102.0894,-101.79488], "fy":[10.96534,7.90277,8.2046,11.35568]}, - {"t":0.67223, "x":1.85033, "y":4.02738, "heading":-3.1156, "vx":-1.89939, "vy":0.05513, "omega":0.18633, "ax":-4.13305, "ay":1.06126, "alpha":-9.18885, "fx":[-79.30561,-86.19914,-60.40049,-44.36458], "fy":[40.34184,-12.92778,-23.17735,65.16136]}, - {"t":0.69596, "x":1.80409, "y":4.02899, "heading":-3.11117, "vx":-1.99747, "vy":0.08032, "omega":-0.03173, "ax":-0.01857, "ay":-0.64882, "alpha":-20.65279, "fx":[-65.63821,-49.5858,52.37641,61.63304], "fy":[43.7265,-68.47226,-66.06494,48.38295]}, - {"t":0.7197, "x":1.75668, "y":4.03071, "heading":-3.11193, "vx":-1.99791, "vy":0.06492, "omega":-0.52184, "ax":4.81335, "ay":-1.64598, "alpha":-8.69864, "fx":[83.07657,50.71938,84.85965,96.1006], "fy":[11.68497,-78.11685,-47.96952,6.76662]}, - {"t":0.74343, "x":1.71062, "y":4.03179, "heading":-3.12431, "vx":-1.88369, "vy":0.02586, "omega":-0.72826, "ax":6.20197, "ay":-0.78157, "alpha":-1.7941, "fx":[102.36583,99.59874,100.844,102.75291], "fy":[-3.84271,-24.2306,-20.12601,-2.90954]}, - {"t":0.76716, "x":1.66767, "y":4.03218, "heading":3.14159, "vx":-1.73651, "vy":0.00731, "omega":-0.77084, "ax":6.18502, "ay":-0.59122, "alpha":4.07753, "fx":[100.90283,103.58629,102.80047,97.16366], "fy":[-25.2475,9.25057,13.75074,-36.41529]}, - {"t":0.80086, "x":1.61265, "y":4.03209, "heading":3.11561, "vx":-1.52805, "vy":-0.01261, "omega":-0.63341, "ax":6.04051, "ay":-0.48379, "alpha":6.12686, "fx":[99.91309,102.99619,99.64399,92.44979], "fy":[-29.72907,16.0285,29.73834,-47.67367]}, - {"t":0.83456, "x":1.56458, "y":4.03139, "heading":3.09426, "vx":-1.32447, "vy":-0.02892, "omega":-0.42691, "ax":5.94226, "ay":-0.43778, "alpha":7.17927, "fx":[99.30956,102.69554,96.95314,89.6206], "fy":[-32.00187,18.42935,38.00565,-53.0604]}, - {"t":0.86827, "x":1.52332, "y":4.03017, "heading":3.07988, "vx":-1.12419, "vy":-0.04367, "omega":-0.18495, "ax":5.87567, "ay":-0.41488, "alpha":7.81249, "fx":[98.91183,102.5345,95.00539,87.77245], "fy":[-33.37734,19.59949,42.86371,-56.21563]}, - {"t":0.90197, "x":1.48877, "y":4.02846, "heading":3.07364, "vx":-0.92616, "vy":-0.05765, "omega":0.07836, "ax":5.82852, "ay":-0.40019, "alpha":8.23301, "fx":[98.67369,102.41284,93.61649,86.43759], "fy":[-34.17843,20.40258,45.95387,-58.34726]}, - {"t":0.93567, "x":1.46086, "y":4.02629, "heading":3.07628, "vx":-0.72972, "vy":-0.07114, "omega":0.35584, "ax":5.79375, "ay":-0.38677, "alpha":8.53255, "fx":[98.58113,102.27972,92.62749,85.3789], "fy":[-34.51582,21.17605,48.00602,-59.95818]}, - {"t":0.96938, "x":1.43956, "y":4.02367, "heading":3.08828, "vx":-0.53445, "vy":-0.08418, "omega":0.64342, "ax":5.76754, "ay":-0.37097, "alpha":8.75515, "fx":[98.62881,102.10595,91.94045,84.47772], "fy":[-34.43221,22.07916,49.37143,-61.2772]}, - {"t":1.00308, "x":1.42482, "y":4.02063, "heading":3.10996, "vx":-0.34007, "vy":-0.09668, "omega":0.9385, "ax":5.74805, "ay":-0.35073, "alpha":8.92008, "fx":[98.81273,101.87138,91.50869,83.68574], "fy":[-33.94228,23.19483,50.21162,-62.3995]}, - {"t":1.03679, "x":1.41663, "y":4.01717, "heading":3.14159, "vx":-0.14634, "vy":-0.1085, "omega":1.23913, "ax":5.8589, "ay":-0.34425, "alpha":8.03187, "fx":[99.62575,101.99184,94.57672,86.9329], "fy":[-31.41485,22.56709,44.08301,-57.74632]}, - {"t":1.06489, "x":1.41483, "y":4.01398, "heading":-3.10676, "vx":0.01834, "vy":-0.11818, "omega":1.46489, "ax":5.86626, "ay":-0.3263, "alpha":7.96531, "fx":[99.98899,101.74453,95.01089,86.86407], "fy":[-30.21838,23.62349,43.09471,-57.83727]}, - {"t":1.093, "x":1.41766, "y":4.01053, "heading":-3.06559, "vx":0.18322, "vy":-0.12735, "omega":1.68877, "ax":5.87653, "ay":-0.30664, "alpha":7.86733, "fx":[100.4009,101.44489,95.58567,86.84885], "fy":[-28.79649,24.84184,41.74993,-57.8473]}, - {"t":1.12111, "x":1.42513, "y":4.00683, "heading":-3.01812, "vx":0.3484, "vy":-0.13597, "omega":1.9099, "ax":5.89044, "ay":-0.28635, "alpha":7.72889, "fx":[100.85243,101.09153,96.32622,86.91985], "fy":[-27.14457,26.20217,39.94447,-57.72711]}, - {"t":1.14921, "x":1.43725, "y":4.0029, "heading":-2.96444, "vx":0.51396, "vy":-0.14402, "omega":2.12714, "ax":5.90858, "ay":-0.26693, "alpha":7.54202, "fx":[101.33227,100.68489,97.25067,87.10842], "fy":[-25.2598,27.67858,37.55352,-57.42757]}, - {"t":1.17732, "x":1.45403, "y":3.99874, "heading":-2.90465, "vx":0.68004, "vy":-0.15152, "omega":2.33913, "ax":5.93123, "ay":-0.25032, "alpha":7.30187, "fx":[101.82683,100.22836,98.35959,87.44216], "fy":[-23.14218,29.23802,34.43689,-56.90172]}, - {"t":1.20543, "x":1.47549, "y":3.99439, "heading":-2.83891, "vx":0.84675, "vy":-0.15856, "omega":2.54436, "ax":5.95809, "ay":-0.2387, "alpha":7.00943, "fx":[102.32027,99.72965,99.62127,87.94254], "fy":[-20.79563,30.83882,30.45376,-56.10616]}, - {"t":1.23354, "x":1.50164, "y":3.98983, "heading":-2.76739, "vx":1.01422, "vy":-0.16526, "omega":2.74138, "ax":5.98809, "ay":-0.2341, "alpha":6.67456, "fx":[102.79484,99.20229,100.95518,88.62321], "fy":[-18.22906,32.42897,25.49313,-55.00162]}, - {"t":1.26164, "x":1.53251, "y":3.9851, "heading":-2.69034, "vx":1.18253, "vy":-0.17184, "omega":2.92898, "ax":6.01921, "ay":-0.23766, "alpha":6.31805, "fx":[103.23149,98.6675,102.22222,89.48917], "fy":[-15.45724,33.9436,19.5245,-53.5523]}, - {"t":1.28975, "x":1.56813, "y":3.98017, "heading":-2.60801, "vx":1.35171, "vy":-0.17852, "omega":3.10657, "ax":6.04873, "ay":-0.24865, "alpha":5.97013, "fx":[103.61068,98.15642,103.23659,90.53724], "fy":[-12.50157,35.30104,12.66396,-51.72339]}, - {"t":1.31786, "x":1.60851, "y":3.97506, "heading":-2.52069, "vx":1.52172, "vy":-0.18551, "omega":3.27437, "ax":6.07408, "ay":-0.26361, "alpha":5.66269, "fx":[103.91331,97.71293,103.81419,91.75781], "fy":[-9.39091,36.3958,5.23284,-49.47597]}, - {"t":1.34597, "x":1.65368, "y":3.96974, "heading":-2.42866, "vx":1.69245, "vy":-0.19292, "omega":3.43354, "ax":6.09414, "ay":-0.27626, "alpha":5.41449, "fx":[104.12168,97.39751,103.85329,93.13756], "fy":[-6.1636,37.08566,-2.22972,-46.75773]}, - {"t":1.37407, "x":1.70366, "y":3.96421, "heading":-2.33215, "vx":1.86374, "vy":-0.20069, "omega":3.58572, "ax":6.11058, "ay":-0.27854, "alpha":5.21386, "fx":[104.22044,97.29214,103.40989,94.66273], "fy":[-2.87253,37.16797,-9.02328,-43.48667]}, - {"t":1.40218, "x":1.75846, "y":3.95846, "heading":-2.23137, "vx":2.03549, "vy":-0.20852, "omega":3.73227, "ax":6.12827, "ay":-0.2624, "alpha":5.00559, "fx":[104.19786,97.50509,102.71696,96.32227], "fy":[0.40209,36.33585,-14.37735,-39.51946]}, - {"t":1.43029, "x":1.81809, "y":3.95249, "heading":-2.12646, "vx":2.20774, "vy":-0.21589, "omega":3.87297, "ax":6.15447, "ay":-0.22091, "alpha":4.68346, "fx":[104.048,98.16909,102.12839,98.10987], "fy":[3.52098,34.09571,-17.48234,-34.58039]}, - {"t":1.4584, "x":1.88258, "y":3.94634, "heading":-2.0176, "vx":2.38073, "vy":-0.2221, "omega":4.0046, "ax":6.1966, "ay":-0.14679, "alpha":4.07686, "fx":[103.77493,99.40838,102.00834,100.01881], "fy":[6.19838,29.60726,-17.33777,-28.06662]}, - {"t":1.4865, "x":1.95194, "y":3.94004, "heading":-1.90504, "vx":2.5549, "vy":-0.22623, "omega":4.11919, "ax":6.25585, "ay":-0.02334, "alpha":2.89554, "fx":[103.39668,101.18541,102.52882,101.97375], "fy":[7.70137,21.36916,-12.22719,-18.36947]}, - {"t":1.51461, "x":2.02622, "y":3.93367, "heading":-1.78926, "vx":2.73073, "vy":-0.22688, "omega":4.20058, "ax":6.29161, "ay":0.2164, "alpha":0.45867, "fx":[102.86031,102.7033,102.88386,102.97582], "fy":[5.27532,6.65954,1.69367,0.52262]}, - {"t":1.54272, "x":2.10546, "y":3.92738, "heading":-1.6712, "vx":2.90757, "vy":-0.2208, "omega":4.21347, "ax":5.64499, "ay":0.77739, "alpha":-7.62111, "fx":[95.00698,100.33849,96.80213,76.99165], "fy":[-28.33721,-18.82413,33.09984,64.89713]}, - {"t":1.57083, "x":2.18942, "y":3.92148, "heading":-1.55277, "vx":3.06624, "vy":-0.19895, "omega":3.99926, "ax":3.89493, "ay":0.56269, "alpha":-16.15304, "fx":[45.05433,96.26947,87.4786,25.89659], "fy":[-77.98018,-25.97592,48.65474,92.09669]}, - {"t":1.59893, "x":2.27714, "y":3.91611, "heading":-1.44036, "vx":3.17572, "vy":-0.18313, "omega":3.54524, "ax":-3.39363, "ay":0.3697, "alpha":3.71261, "fx":[-48.3058,-63.33629,-62.66874,-47.60653], "fy":[22.39792,15.4112,-7.8498,-5.78365]}, - {"t":1.62704, "x":2.36506, "y":3.91111, "heading":-1.34071, "vx":3.08033, "vy":-0.17274, "omega":3.6496, "ax":-5.39169, "ay":-0.52206, "alpha":8.58395, "fx":[-84.36102,-99.47637,-92.03051,-76.70743], "fy":[47.8863,14.06075,-39.72991,-56.35619]}, - {"t":1.65515, "x":2.44951, "y":3.90604, "heading":-1.23813, "vx":2.92878, "vy":-0.18742, "omega":3.89087, "ax":-6.21244, "ay":-0.16218, "alpha":1.58289, "fx":[-101.46,-102.19207,-101.30967,-101.28432], "fy":[8.67315,2.31117,-12.8074,-8.78226]}, - {"t":1.68325, "x":2.52938, "y":3.90071, "heading":-1.12877, "vx":2.75417, "vy":-0.19198, "omega":3.93536, "ax":-6.21026, "ay":0.1184, "alpha":-3.29932, "fx":[-101.33102,-102.38588,-99.30565,-103.08131], "fy":[-20.02028,-8.30178,27.61885,8.44572]}, - {"t":1.71136, "x":2.60434, "y":3.89536, "heading":-1.01816, "vx":2.57961, "vy":-0.18865, "omega":3.84262, "ax":-6.10619, "ay":0.20452, "alpha":-4.97787, "fx":[-99.00205,-102.6868,-94.14825,-103.46123], "fy":[-30.93881,-8.12783,43.22421,9.21663]}, - {"t":1.73947, "x":2.67443, "y":3.89014, "heading":-0.91015, "vx":2.40799, "vy":-0.1829, "omega":3.70271, "ax":-6.04464, "ay":0.26536, "alpha":-5.67323, "fx":[-96.83872,-103.19335,-91.4106,-103.83103], "fy":[-37.71738,-1.30311,49.34475,7.02835]}, - {"t":1.76758, "x":2.73972, "y":3.88511, "heading":-0.80608, "vx":2.23809, "vy":-0.17544, "omega":3.54325, "ax":-5.99863, "ay":0.34338, "alpha":-6.11545, "fx":[-94.85951,-102.90237,-90.39706,-104.10592], "fy":[-42.72785,9.75164,51.51875,3.91186]}, - {"t":1.79568, "x":2.80026, "y":3.88031, "heading":-0.70649, "vx":2.06948, "vy":-0.16579, "omega":3.37136, "ax":-5.94409, "ay":0.43133, "alpha":-6.61245, "fx":[-93.05715,-101.01309,-90.37927,-104.24859], "fy":[-46.68335,22.63986,51.76941,0.47991]}, - {"t":1.82379, "x":2.85608, "y":3.87582, "heading":-0.61173, "vx":1.90241, "vy":-0.15367, "omega":3.1855, "ax":-5.8752, "ay":0.50643, "alpha":-7.23419, "fx":[-91.44577,-97.57804,-90.91508,-104.25459], "fy":[-49.86766,35.01743,50.97587,-3.00914]}, - {"t":1.8519, "x":2.90723, "y":3.8717, "heading":-0.52219, "vx":1.73727, "vy":-0.13943, "omega":2.98217, "ax":-5.80046, "ay":0.55208, "alpha":-7.91264, "fx":[-90.04809,-93.38204,-91.74024,-104.13549], "fy":[-52.41806,45.34864,49.58895,-6.41793]}, - {"t":1.88001, "x":2.95377, "y":3.868, "heading":-0.43837, "vx":1.57424, "vy":-0.12391, "omega":2.75976, "ax":-5.73122, "ay":0.56559, "alpha":-8.55095, "fx":[-88.88467,-89.29115,-92.69109,-103.91153], "fy":[-54.41622,53.1862,47.87583,-9.66022]}, - {"t":1.90811, "x":2.99575, "y":3.86474, "heading":-0.3608, "vx":1.41315, "vy":-0.10802, "omega":2.51942, "ax":-5.67436, "ay":0.55367, "alpha":-9.08698, "fx":[-87.96646,-85.82159,-93.66414,-103.6076], "fy":[-55.92447,58.79479,46.0132,-12.67756]}, - {"t":1.93622, "x":3.03323, "y":3.86192, "heading":-0.28998, "vx":1.25365, "vy":-0.09245, "omega":2.26401, "ax":-5.63157, "ay":0.52562, "alpha":-9.50083, "fx":[-87.29034,-83.12615,-94.59474,-103.25032], "fy":[-57.00203,62.67847,44.12597,-15.43072]}, - {"t":1.96433, "x":3.06625, "y":3.85953, "heading":-0.22635, "vx":1.09537, "vy":-0.07768, "omega":1.99696, "ax":-5.60148, "ay":0.48963, "alpha":-9.7997, "fx":[-86.83768,-81.14664,-95.44424,-102.86576], "fy":[-57.71208,65.3202,42.30531,-17.8953]}, - {"t":1.99244, "x":3.09482, "y":3.85754, "heading":-0.17022, "vx":0.93792, "vy":-0.06392, "omega":1.72152, "ax":-5.58153, "ay":0.45169, "alpha":-10.00313, "fx":[-86.57571,-79.74472,-96.19157,-102.47777], "fy":[-58.12402,67.10093,40.61844,-20.05822]}, - {"t":2.02054, "x":3.11898, "y":3.85592, "heading":-0.12183, "vx":0.78104, "vy":-0.05122, "omega":1.44036, "ax":-5.56896, "ay":0.41575, "alpha":-10.13417, "fx":[-86.4608,-78.77272,-96.82701,-102.10712], "fy":[-58.3128,68.29902,39.11484,-21.91443]}, - {"t":2.04865, "x":3.13873, "y":3.85465, "heading":-0.08135, "vx":0.62451, "vy":-0.03954, "omega":1.15551, "ax":-5.56127, "ay":0.38417, "alpha":-10.21498, "fx":[-86.44314,-78.10233,-97.34785,-101.77113], "fy":[-58.35629,69.11174,37.83068,-23.46413]}, - {"t":2.07676, "x":3.15409, "y":3.85369, "heading":-0.04887, "vx":0.4682, "vy":-0.02874, "omega":0.8684, "ax":-5.55635, "ay":0.35827, "alpha":-10.2649, "fx":[-86.47188,-77.63215,-97.7552,-101.48375], "fy":[-58.33108,69.67705,36.79216,-24.71027]}, - {"t":2.10487, "x":3.16505, "y":3.85302, "heading":-0.02446, "vx":0.31202, "vy":-0.01867, "omega":0.57988, "ax":-5.55255, "ay":0.33864, "alpha":-10.29967, "fx":[-86.50013,-77.2867,-98.05176,-101.25588], "fy":[-58.30713,70.0902,36.01797,-25.65661]}, - {"t":2.13297, "x":3.17163, "y":3.85263, "heading":-0.00816, "vx":0.15596, "vy":-0.00915, "omega":0.29038, "ax":-5.54863, "ay":0.32555, "alpha":-10.33113, "fx":[-86.48925,-77.01281,-98.24027,-101.09568], "fy":[-58.34205,70.41533,35.52128,-26.30626]}, - {"t":2.16108, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":2.72474, "y":4.08493, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.35666, "ay":-0.64557, "alpha":0.0, "fx":[-103.91934,-103.91934,-103.91934,-103.91934], "fy":[-10.55377,-10.55377,-10.55377,-10.55377]}, + {"t":0.03526, "x":2.72079, "y":4.08453, "heading":3.14159, "vx":-0.22414, "vy":-0.02276, "omega":0.0, "ax":-6.35687, "ay":-0.63724, "alpha":0.0, "fx":[-103.92273,-103.92273,-103.92273,-103.92273], "fy":[-10.41766,-10.41766,-10.41766,-10.41766]}, + {"t":0.07052, "x":2.70893, "y":4.08333, "heading":3.14159, "vx":-0.44828, "vy":-0.04523, "omega":0.0, "ax":-6.3571, "ay":-0.62739, "alpha":0.0, "fx":[-103.92652,-103.92652,-103.92652,-103.92652], "fy":[-10.25669,-10.25669,-10.25669,-10.25669]}, + {"t":0.10578, "x":2.68917, "y":4.08135, "heading":3.14159, "vx":-0.67243, "vy":-0.06735, "omega":0.0, "ax":-6.35736, "ay":-0.61557, "alpha":0.0, "fx":[-103.93074,-103.93074,-103.93074,-103.93074], "fy":[-10.06337,-10.06337,-10.06337,-10.06337]}, + {"t":0.14104, "x":2.66151, "y":4.07859, "heading":3.14159, "vx":-0.89659, "vy":-0.08906, "omega":0.0, "ax":-6.35765, "ay":-0.6011, "alpha":0.0, "fx":[-103.93543,-103.93543,-103.93543,-103.93543], "fy":[-9.82684,-9.82684,-9.82684,-9.82684]}, + {"t":0.1763, "x":2.62595, "y":4.07507, "heading":3.14159, "vx":-1.12076, "vy":-0.11025, "omega":0.0, "ax":-6.35796, "ay":-0.58299, "alpha":0.0, "fx":[-103.94054,-103.94054,-103.94054,-103.94054], "fy":[-9.53083,-9.53083,-9.53083,-9.53083]}, + {"t":0.21156, "x":2.58248, "y":4.07082, "heading":3.14159, "vx":-1.34495, "vy":-0.13081, "omega":0.0, "ax":-6.35829, "ay":-0.55968, "alpha":0.0, "fx":[-103.9459,-103.9459,-103.9459,-103.9459], "fy":[-9.14969,-9.14969,-9.14969,-9.14969]}, + {"t":0.24682, "x":2.5311, "y":4.06586, "heading":3.14159, "vx":-1.56914, "vy":-0.15054, "omega":0.0, "ax":-6.35859, "ay":-0.52854, "alpha":0.0, "fx":[-103.9509,-103.9509,-103.9509,-103.9509], "fy":[-8.64056,-8.64056,-8.64056,-8.64056]}, + {"t":0.28208, "x":2.47182, "y":4.06023, "heading":3.14159, "vx":-1.79334, "vy":-0.16918, "omega":0.0, "ax":-6.35877, "ay":-0.48483, "alpha":0.0, "fx":[-103.95374,-103.95374,-103.95374,-103.95374], "fy":[-7.92605,-7.92605,-7.92605,-7.92605]}, + {"t":0.31734, "x":2.40463, "y":4.05396, "heading":3.14159, "vx":-2.01755, "vy":-0.18628, "omega":0.0, "ax":-6.35847, "ay":-0.41906, "alpha":0.0, "fx":[-103.94887,-103.94887,-103.94887,-103.94887], "fy":[-6.8508,-6.8508,-6.8508,-6.8508]}, + {"t":0.3526, "x":2.32954, "y":4.04713, "heading":3.14159, "vx":-2.24175, "vy":-0.20105, "omega":0.0, "ax":-6.35646, "ay":-0.30892, "alpha":0.0, "fx":[-103.91603,-103.91603,-103.91603,-103.91603], "fy":[-5.05031,-5.05031,-5.05031,-5.05031]}, + {"t":0.38786, "x":2.24655, "y":4.03985, "heading":3.14159, "vx":-2.46588, "vy":-0.21194, "omega":0.0, "ax":-6.34667, "ay":-0.0871, "alpha":0.0, "fx":[-103.75593,-103.75593,-103.75593,-103.75593], "fy":[-1.42394,-1.42394,-1.42394,-1.42394]}, + {"t":0.42312, "x":2.15565, "y":4.03232, "heading":3.14159, "vx":-2.68967, "vy":-0.21502, "omega":0.0, "ax":-6.26988, "ay":0.58335, "alpha":0.0, "fx":[-102.50058,-102.50058,-102.50058,-102.50058], "fy":[9.53666,9.53666,9.53666,9.53666]}, + {"t":0.45838, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-2.91074, "vy":-0.19445, "omega":0.0, "ax":0.03195, "ay":5.56699, "alpha":0.0, "fx":[0.52234,0.52234,0.52234,0.52234], "fy":[91.00975,91.00975,91.00975,91.00975]}, + {"t":0.49124, "x":1.96128, "y":4.02172, "heading":3.14159, "vx":-2.90969, "vy":-0.0115, "omega":0.0, "ax":6.27511, "ay":0.37619, "alpha":0.0, "fx":[102.58603,102.58603,102.58603,102.58603], "fy":[6.15005,6.15005,6.15005,6.15005]}, + {"t":0.52411, "x":1.86904, "y":4.02155, "heading":3.14159, "vx":-2.70347, "vy":0.00087, "omega":0.0, "ax":6.34125, "ay":-0.01693, "alpha":0.0, "fx":[103.66734,103.66734,103.66734,103.66734], "fy":[-0.27684,-0.27684,-0.27684,-0.27684]}, + {"t":0.55697, "x":1.78362, "y":4.02156, "heading":3.14159, "vx":-2.49508, "vy":0.00031, "omega":0.0, "ax":6.358, "ay":-0.14972, "alpha":0.0, "fx":[103.94119,103.94119,103.94119,103.94119], "fy":[-2.44766,-2.44766,-2.44766,-2.44766]}, + {"t":0.58983, "x":1.70506, "y":4.02149, "heading":3.14159, "vx":-2.28613, "vy":-0.00461, "omega":0.0, "ax":6.36536, "ay":-0.21634, "alpha":0.0, "fx":[104.06144,104.06144,104.06144,104.06144], "fy":[-3.53668,-3.53668,-3.53668,-3.53668]}, + {"t":0.6227, "x":1.63337, "y":4.02123, "heading":3.14159, "vx":-2.07694, "vy":-0.01172, "omega":0.0, "ax":6.36944, "ay":-0.25636, "alpha":0.0, "fx":[104.12822,104.12822,104.12822,104.12822], "fy":[-4.19096,-4.19096,-4.19096,-4.19096]}, + {"t":0.65556, "x":1.56855, "y":4.0207, "heading":3.14159, "vx":-1.86762, "vy":-0.02014, "omega":0.0, "ax":6.37203, "ay":-0.28306, "alpha":0.0, "fx":[104.17049,104.17049,104.17049,104.17049], "fy":[-4.62743,-4.62743,-4.62743,-4.62743]}, + {"t":0.68842, "x":1.51062, "y":4.01989, "heading":3.14159, "vx":-1.65821, "vy":-0.02945, "omega":0.0, "ax":6.37381, "ay":-0.30213, "alpha":0.0, "fx":[104.19958,104.19958,104.19958,104.19958], "fy":[-4.93928,-4.93928,-4.93928,-4.93928]}, + {"t":0.72129, "x":1.45956, "y":4.01876, "heading":3.14159, "vx":-1.44875, "vy":-0.03938, "omega":0.0, "ax":6.3751, "ay":-0.31644, "alpha":0.0, "fx":[104.22079,104.22079,104.22079,104.22079], "fy":[-5.17321,-5.17321,-5.17321,-5.17321]}, + {"t":0.75415, "x":1.41539, "y":4.01729, "heading":3.14159, "vx":-1.23924, "vy":-0.04977, "omega":0.0, "ax":6.37609, "ay":-0.32757, "alpha":0.0, "fx":[104.23694,104.23694,104.23694,104.23694], "fy":[-5.35516,-5.35516,-5.35516,-5.35516]}, + {"t":0.78702, "x":1.37811, "y":4.01548, "heading":3.14159, "vx":-1.0297, "vy":-0.06054, "omega":0.0, "ax":6.37687, "ay":-0.33648, "alpha":0.0, "fx":[104.24963,104.24963,104.24963,104.24963], "fy":[-5.50073,-5.50073,-5.50073,-5.50073]}, + {"t":0.81988, "x":1.34772, "y":4.01331, "heading":3.14159, "vx":-0.82013, "vy":-0.0716, "omega":0.0, "ax":6.37749, "ay":-0.34376, "alpha":0.0, "fx":[104.25986,104.25986,104.25986,104.25986], "fy":[-5.61982,-5.61982,-5.61982,-5.61982]}, + {"t":0.85274, "x":1.32421, "y":4.01077, "heading":3.14159, "vx":-0.61055, "vy":-0.08289, "omega":0.0, "ax":6.37801, "ay":-0.34983, "alpha":0.0, "fx":[104.26829,104.26829,104.26829,104.26829], "fy":[-5.71906,-5.71906,-5.71906,-5.71906]}, + {"t":0.88561, "x":1.30759, "y":4.00786, "heading":3.14159, "vx":-0.40094, "vy":-0.09439, "omega":0.0, "ax":6.37844, "ay":-0.35497, "alpha":0.0, "fx":[104.27534,104.27534,104.27534,104.27534], "fy":[-5.80303,-5.80303,-5.80303,-5.80303]}, + {"t":0.91847, "x":1.29785, "y":4.00456, "heading":3.14159, "vx":-0.19133, "vy":-0.10606, "omega":0.0, "ax":5.53486, "ay":-0.25983, "alpha":10.45437, "fx":[98.56364,100.76743,85.26448,77.34183], "fy":[-34.6479,27.58124,60.16118,-70.08566]}, + {"t":0.94814, "x":1.29461, "y":4.0013, "heading":3.14159, "vx":-0.0271, "vy":-0.11377, "omega":0.31019, "ax":5.53904, "ay":-0.26103, "alpha":10.42252, "fx":[98.57077,100.77826,85.39593,77.46626], "fy":[-34.61017,27.51917,59.95247,-69.93099]}, + {"t":0.97781, "x":1.29625, "y":3.99781, "heading":-3.13239, "vx":0.13724, "vy":-0.12151, "omega":0.61943, "ax":5.54572, "ay":-0.25446, "alpha":10.37309, "fx":[98.69782,100.69998,85.64465,77.60563], "fy":[-34.22694,27.7784,59.56823,-69.75969]}, + {"t":1.00748, "x":1.30276, "y":3.99409, "heading":-3.11401, "vx":0.30179, "vy":-0.12906, "omega":0.9272, "ax":5.55561, "ay":-0.24065, "alpha":10.29948, "fx":[98.94084,100.5327,86.04001,77.78109], "fy":[-33.49623,28.34822,58.95942,-69.54783]}, + {"t":1.03715, "x":1.31416, "y":3.99016, "heading":-3.0865, "vx":0.46662, "vy":-0.1362, "omega":1.23279, "ax":5.56992, "ay":-0.22052, "alpha":10.19007, "fx":[99.29233,100.27544,86.63379,78.02879], "fy":[-32.41444,29.21135,58.03694,-69.25386]}, + {"t":1.06682, "x":1.33046, "y":3.98602, "heading":-3.04992, "vx":0.63189, "vy":-0.14274, "omega":1.53514, "ax":5.59032, "ay":-0.19568, "alpha":10.02858, "fx":[99.74074,99.92727,87.50062,78.39544], "fy":[-30.97751,30.34222,56.6618,-68.82239]}, + {"t":1.09649, "x":1.35167, "y":3.9817, "heading":-3.00437, "vx":0.79775, "vy":-0.14855, "omega":1.83269, "ax":5.6188, "ay":-0.16877, "alpha":9.79497, "fx":[100.27,99.48918,88.73488,78.93253], "fy":[-29.18258,31.70448,54.63069,-68.18873]}, + {"t":1.12616, "x":1.37781, "y":3.97722, "heading":-2.95, "vx":0.96447, "vy":-0.15356, "omega":2.12331, "ax":5.65744, "ay":-0.14386, "alpha":9.46763, "fx":[100.85907,98.96671,90.43791,79.68963], "fy":[-27.03041,33.24746,51.65909,-67.28355]}, + {"t":1.15583, "x":1.40892, "y":3.9726, "heading":-2.887, "vx":1.13233, "vy":-0.15783, "omega":2.40422, "ax":5.70781, "ay":-0.12682, "alpha":9.02809, "fx":[101.48175,98.37373,92.68399,80.70785], "fy":[-24.52846,34.90141,47.37064,-66.03667]}, + {"t":1.1855, "x":1.44503, "y":3.96786, "heading":-2.81566, "vx":1.30168, "vy":-0.16159, "omega":2.67209, "ax":5.76991, "ay":-0.12515, "alpha":8.47034, "fx":[102.10698,97.73767,95.44925,82.01453], "fy":[-21.69472,36.57061,41.31954,-64.3793]}, + {"t":1.21517, "x":1.48619, "y":3.96301, "heading":-2.73638, "vx":1.47288, "vy":-0.1653, "omega":2.92341, "ax":5.84056, "ay":-0.14648, "alpha":7.81533, "fx":[102.69992,97.10664,98.5009,83.6204], "fy":[-18.56218,38.12269,33.10441,-62.24343]}, + {"t":1.24484, "x":1.53246, "y":3.95804, "heading":-2.64964, "vx":1.64617, "vy":-0.16965, "omega":3.15529, "ax":5.91221, "ay":-0.19455, "alpha":7.12395, "fx":[103.22395,96.55981,101.30853,85.52098], "fy":[-15.18434,39.36942,22.6486,-59.55582]}, + {"t":1.27452, "x":1.5839, "y":3.95292, "heading":-2.55602, "vx":1.82159, "vy":-0.17542, "omega":3.36666, "ax":5.97507, "ay":-0.2634, "alpha":6.48539, "fx":[103.64339,96.22299,103.15459,87.70332], "fy":[-11.6436,40.02764,10.61269,-56.22086]}, + {"t":1.30419, "x":1.64058, "y":3.9476, "heading":-2.45613, "vx":1.99887, "vy":-0.18324, "omega":3.55909, "ax":6.02456, "ay":-0.33516, "alpha":5.95469, "fx":[103.92656,96.29286,103.58196,90.15867], "fy":[-8.06941,39.63356,-1.39992,-52.0813]}, + {"t":1.33386, "x":1.70254, "y":3.94202, "heading":-2.35053, "vx":2.17762, "vy":-0.19318, "omega":3.73577, "ax":6.06935, "ay":-0.38868, "alpha":5.45895, "fx":[104.04858,97.06698,102.87299,92.90062], "fy":[-4.68841,37.34341,-11.24467,-46.82728]}, + {"t":1.36353, "x":1.76982, "y":3.93611, "heading":-2.23969, "vx":2.3577, "vy":-0.20471, "omega":3.89774, "ax":6.13039, "ay":-0.41588, "alpha":4.72817, "fx":[103.99139,98.92519,101.97575,95.98847], "fy":[-1.98435,31.44225,-16.90518,-39.74804]}, + {"t":1.3932, "x":1.84248, "y":3.92986, "heading":-2.12404, "vx":2.53959, "vy":-0.21705, "omega":4.03802, "ax":6.22377, "ay":-0.43607, "alpha":3.21049, "fx":[103.7121,101.86896,101.88151,99.52473], "fy":[-1.33304,18.12725,-16.48305,-28.82677]}, + {"t":1.42287, "x":1.92057, "y":3.92322, "heading":-2.00423, "vx":2.72426, "vy":-0.22999, "omega":4.13328, "ax":6.27522, "ay":-0.49067, "alpha":-0.31652, "fx":[102.48064,102.42355,102.71001,102.73728], "fy":[-9.04463,-10.22737,-7.04386,-5.77013]}, + {"t":1.45254, "x":2.00416, "y":3.91618, "heading":-1.8816, "vx":2.91044, "vy":-0.24455, "omega":4.12389, "ax":5.74348, "ay":-0.45566, "alpha":-7.18806, "fx":[88.40834,94.53892,101.80008,90.83255], "fy":[-45.81763,-38.79245,10.92707,43.88636]}, + {"t":1.48221, "x":2.09304, "y":3.90873, "heading":-1.75924, "vx":3.08086, "vy":-0.25807, "omega":3.91062, "ax":5.49808, "ay":0.79308, "alpha":-7.2306, "fx":[91.46888,97.65758,95.69116,74.7147], "fy":[-20.80471,-20.1839,30.00567,62.84451]}, + {"t":1.51188, "x":2.18687, "y":3.90142, "heading":-1.64321, "vx":3.24399, "vy":-0.23454, "omega":3.69608, "ax":-0.57467, "ay":4.66236, "alpha":-2.24527, "fx":[-20.89985,-0.72755,0.64529,-16.59663], "fy":[72.52684,74.01695,79.63933,78.69974]}, + {"t":1.54155, "x":2.28287, "y":3.89651, "heading":-1.53354, "vx":3.22694, "vy":-0.0962, "omega":3.62946, "ax":-5.51541, "ay":-0.03196, "alpha":7.62199, "fx":[-83.69796,-97.05373,-96.28227,-83.63198], "fy":[46.89703,23.81487,-26.55893,-46.24301]}, + {"t":1.57122, "x":2.37619, "y":3.89365, "heading":-1.42586, "vx":3.06329, "vy":-0.09715, "omega":3.85561, "ax":-5.65838, "ay":-0.58898, "alpha":7.79747, "fx":[-91.41781,-101.20491,-95.23783,-82.15442], "fy":[40.95074,14.43955,-37.05541,-56.8499]}, + {"t":1.60089, "x":2.46458, "y":3.8905, "heading":-1.31146, "vx":2.8954, "vy":-0.11463, "omega":4.08697, "ax":-6.27301, "ay":-0.33611, "alpha":0.80135, "fx":[-102.73462,-102.85545,-102.31523,-102.3016], "fy":[-0.1291,-2.30064,-10.51464,-9.0349]}, + {"t":1.63056, "x":2.54773, "y":3.88695, "heading":-1.19019, "vx":2.70928, "vy":-0.1246, "omega":4.11074, "ax":-6.22586, "ay":-0.08489, "alpha":-3.38016, "fx":[-101.13889,-102.10264,-100.4424,-103.43994], "fy":[-22.43124,-14.66443,24.47136,7.07306]}, + {"t":1.66023, "x":2.62538, "y":3.88322, "heading":-1.06823, "vx":2.52456, "vy":-0.12712, "omega":4.01045, "ax":-6.10597, "ay":0.00207, "alpha":-5.15664, "fx":[-98.51345,-101.95846,-95.15345,-103.65832], "fy":[-33.00755,-16.61744,41.34721,8.4134]}, + {"t":1.6899, "x":2.69759, "y":3.87945, "heading":-0.94923, "vx":2.34339, "vy":-0.12706, "omega":3.85745, "ax":-6.03406, "ay":0.05703, "alpha":-5.89746, "fx":[-95.95104,-102.81079,-91.85797,-103.96194], "fy":[-40.22986,-11.03754,48.70988,6.28695]}, + {"t":1.71957, "x":2.76447, "y":3.8757, "heading":-0.83478, "vx":2.16436, "vy":-0.12536, "omega":3.68247, "ax":-5.98895, "ay":0.14044, "alpha":-6.29036, "fx":[-93.49159,-103.48586,-90.45605,-104.19848], "fy":[-45.85,0.52248,51.55291,2.95849]}, + {"t":1.74924, "x":2.82605, "y":3.87205, "heading":-0.72552, "vx":1.98666, "vy":-0.1212, "omega":3.49583, "ax":-5.93474, "ay":0.2553, "alpha":-6.75327, "fx":[-91.16266,-102.36619,-90.26318,-104.2946], "fy":[-50.43504,15.87089,52.07775,-0.81881]}, + {"t":1.77891, "x":2.88238, "y":3.86856, "heading":-0.6218, "vx":1.81057, "vy":-0.11362, "omega":3.29546, "ax":-5.85341, "ay":0.36802, "alpha":-7.4468, "fx":[-89.00788,-98.75583,-90.77386,-104.23112], "fy":[-54.2186,31.68267,51.31496,-4.71333]}, + {"t":1.80858, "x":2.93353, "y":3.86535, "heading":-0.52402, "vx":1.6369, "vy":-0.1027, "omega":3.07451, "ax":-5.75485, "ay":0.44331, "alpha":-8.28597, "fx":[-87.07618,-93.56962,-91.6612,-104.01639], "fy":[-57.31694,45.03882,49.81135,-8.5442]}, + {"t":1.83825, "x":2.97956, "y":3.8625, "heading":-0.4328, "vx":1.46615, "vy":-0.08955, "omega":2.82866, "ax":-5.65994, "ay":0.47031, "alpha":-9.10039, "fx":[-85.41119,-88.31751,-92.71421,-103.67413], "fy":[-59.80255,54.85151,47.89952,-12.19409]}, + {"t":1.86793, "x":3.02057, "y":3.86005, "heading":-0.34887, "vx":1.29822, "vy":-0.0756, "omega":2.55865, "ax":-5.58208, "ay":0.45854, "alpha":-9.77879, "fx":[-84.04212,-83.94981,-93.79677,-103.23703], "fy":[-61.73541,61.49604,45.8049,-15.58042]}, + {"t":1.8976, "x":3.05663, "y":3.85801, "heading":-0.27295, "vx":1.13259, "vy":-0.06199, "omega":2.26851, "ax":-5.52439, "ay":0.42328, "alpha":-10.29019, "fx":[-82.97695,-80.71051,-94.82394,-102.7417], "fy":[-63.17778,65.81395,43.68944,-18.64651]}, + {"t":1.92727, "x":3.08781, "y":3.85636, "heading":-0.20565, "vx":0.96868, "vy":-0.04943, "omega":1.96319, "ax":-5.48438, "ay":0.37743, "alpha":-10.6496, "fx":[-82.1998,-78.46611,-95.74636,-102.22448], "fy":[-64.20018,68.56676,41.67161,-21.35684]}, + {"t":1.95694, "x":3.11413, "y":3.85506, "heading":-0.1474, "vx":0.80596, "vy":-0.03823, "omega":1.64721, "ax":-5.45786, "ay":0.32989, "alpha":-10.88863, "fx":[-81.67263,-76.97281,-96.53864,-101.71861], "fy":[-64.88226,70.30878,39.838,-23.69231]}, + {"t":1.98661, "x":3.13564, "y":3.85407, "heading":-0.09852, "vx":0.64402, "vy":-0.02844, "omega":1.32414, "ax":-5.44071, "ay":0.28613, "alpha":-11.04097, "fx":[-81.3404,-75.99774,-97.19053,-101.25262], "fy":[-65.31046,71.41502,38.25133,-25.6452]}, + {"t":2.01628, "x":3.15236, "y":3.85335, "heading":-0.05924, "vx":0.48259, "vy":-0.01995, "omega":0.99655, "ax":-5.42945, "ay":0.2493, "alpha":-11.13668, "fx":[-81.13875,-75.35546,-97.70052,-100.84983], "fy":[-65.57334,72.13377,36.95641,-27.21477]}, + {"t":2.04595, "x":3.16429, "y":3.85287, "heading":-0.02967, "vx":0.3215, "vy":-0.01256, "omega":0.66612, "ax":-5.42132, "ay":0.22105, "alpha":-11.20028, "fx":[-81.00279,-74.91036,-98.07119,-100.52851], "fy":[-65.75469,72.6282,35.98484,-28.40347]}, + {"t":2.07562, "x":3.17144, "y":3.85259, "heading":-0.0099, "vx":0.16064, "vy":-0.006, "omega":0.3338, "ax":-5.41429, "ay":0.20221, "alpha":-11.25026, "fx":[-80.87597,-74.56908,-98.30618,-100.30231], "fy":[-65.92544,73.00386,35.35843,-29.21416]}, + {"t":2.10529, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/BtoB.traj b/src/main/deploy/choreo/BtoB.traj index cc888d85..de609a5d 100644 --- a/src/main/deploy/choreo/BtoB.traj +++ b/src/main/deploy/choreo/BtoB.traj @@ -3,28 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.178781509399414, "y":3.3376660346984863, "heading":-1.377584993861348, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.299587607383728, "y":2.5006520748138428, "heading":-1.2021004738085692, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.9659841656684875, "y":2.8667194843292236, "heading":-1.377584993861348, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.2125167846679688, "y":2.193495988845825, "heading":-1.2021004738085692, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.178781509399414 m", "val":1.178781509399414}, "y":{"exp":"3.3376660346984863 m", "val":3.3376660346984863}, "heading":{"exp":"-1.377584993861348 rad", "val":-1.377584993861348}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.299587607383728 m", "val":1.299587607383728}, "y":{"exp":"2.5006520748138428 m", "val":2.5006520748138428}, "heading":{"exp":"-1.2021004738085692 rad", "val":-1.2021004738085692}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.9659841656684875 m", "val":0.9659841656684875}, "y":{"exp":"2.8667194843292236 m", "val":2.8667194843292236}, "heading":{"exp":"-1.377584993861348 rad", "val":-1.377584993861348}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.2125167846679688 m", "val":1.2125167846679688}, "y":{"exp":"2.193495988845825 m", "val":2.193495988845825}, "heading":{"exp":"-1.2021004738085692 rad", "val":-1.2021004738085692}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,111 +36,96 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.10213,1.62735,2.95102], + "waypoints":[0.0,1.22863,1.63136,2.94312], "samples":[ - {"t":0.0, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.30779, "ay":0.91263, "alpha":-1.31429, "fx":[-104.08674,-101.79162,-102.41701,-104.18618], "fy":[8.44915,23.33635,20.50695,7.38645]}, - {"t":0.03149, "x":3.17069, "y":3.85296, "heading":0.0, "vx":-0.19863, "vy":0.02874, "omega":-0.04139, "ax":-6.31086, "ay":0.88074, "alpha":-1.35419, "fx":[-104.13719,-101.84541,-102.47352,-104.22594], "fy":[7.67273,23.05777,20.18257,6.68049]}, - {"t":0.06298, "x":3.16131, "y":3.8543, "heading":-0.0013, "vx":-0.39735, "vy":0.05647, "omega":-0.08403, "ax":-6.31411, "ay":0.84453, "alpha":-1.39955, "fx":[-104.18838,-101.90744,-102.53413,-104.26466], "fy":[6.77623,22.73267,19.82616,5.89109]}, - {"t":0.09447, "x":3.14567, "y":3.85649, "heading":-0.00395, "vx":-0.59618, "vy":0.08307, "omega":-0.1281, "ax":-6.31753, "ay":0.80309, "alpha":-1.45156, "fx":[-104.23836,-101.97915,-102.59974,-104.30108], "fy":[5.73399,22.35089,19.43013,5.00064]}, - {"t":0.12596, "x":3.12376, "y":3.85951, "heading":-0.00798, "vx":-0.79512, "vy":0.10835, "omega":-0.17381, "ax":-6.32109, "ay":0.75517, "alpha":-1.51178, "fx":[-104.28383,-102.06239,-102.67141,-104.33323], "fy":[4.51228,21.89888,18.98457,3.9865]}, - {"t":0.15745, "x":3.09559, "y":3.86329, "heading":-0.01346, "vx":-0.99416, "vy":0.13213, "omega":-0.22141, "ax":-6.3247, "ay":0.69916, "alpha":-1.58226, "fx":[-104.31933,-102.1595,-102.75049,-104.35797], "fy":[3.06595,21.35822,18.47629,2.81896]}, - {"t":0.18894, "x":3.06115, "y":3.8678, "heading":-0.02043, "vx":-1.19333, "vy":0.15415, "omega":-0.27124, "ax":-6.32823, "ay":0.63283, "alpha":-1.66582, "fx":[-104.33567,-102.27348,-102.83865,-104.37026], "fy":[1.33327,20.70312,17.88744,1.45834]}, - {"t":0.22043, "x":3.02043, "y":3.87297, "heading":-0.02897, "vx":-1.3926, "vy":0.17408, "omega":-0.32369, "ax":-6.3314, "ay":0.55309, "alpha":-1.76639, "fx":[-104.31701,-102.40819,-102.93792,-104.36188], "fy":[-0.77235,19.89656,17.19314,-0.14955]}, - {"t":0.25192, "x":2.97344, "y":3.87873, "heading":-0.03916, "vx":-1.59197, "vy":0.19149, "omega":-0.37932, "ax":-6.33367, "ay":0.45548, "alpha":-1.8896, "fx":[-104.23543,-102.5685,-103.05082,-104.31904], "fy":[-3.37572,18.88335,16.35782,-2.08058]}, - {"t":0.2834, "x":2.92017, "y":3.88498, "heading":-0.05111, "vx":-1.79141, "vy":0.20584, "omega":-0.43882, "ax":-6.33404, "ay":0.33337, "alpha":-2.04381, "fx":[-104.03968,-102.7604,-103.18026,-104.21791], "fy":[-6.66274,17.57764,15.32869,-4.44401]}, - {"t":0.31489, "x":2.86062, "y":3.89163, "heading":-0.06493, "vx":-1.99087, "vy":0.21633, "omega":-0.50318, "ax":-6.3305, "ay":0.17647, "alpha":-2.24187, "fx":[-103.63123,-102.99029,-103.32925,-104.01557], "fy":[-10.92026,15.83838,14.02414,-7.40253]}, - {"t":0.34638, "x":2.79479, "y":3.89853, "heading":-0.08077, "vx":-2.19021, "vy":0.22189, "omega":-0.57377, "ax":-6.31879, "ay":-0.03193, "alpha":-2.50421, "fx":[-102.80902,-103.26148,-103.49958,-103.63079], "fy":[-16.60833,13.41716,12.31095,-11.20766]}, - {"t":0.37787, "x":2.72269, "y":3.9055, "heading":-0.09884, "vx":-2.38919, "vy":0.22089, "omega":-0.65263, "ax":-6.28939, "ay":-0.32061, "alpha":-2.86444, "fx":[-101.13244,-103.55896,-103.68716,-102.89991], "fy":[-24.49131,9.83436,9.9565,-16.26515]}, - {"t":0.40936, "x":2.64434, "y":3.9123, "heading":-0.11939, "vx":-2.58724, "vy":0.21079, "omega":-0.74283, "ax":-6.21886, "ay":-0.74256, "alpha":-3.37709, "fx":[-97.55425,-103.77959,-103.86525,-101.46668], "fy":[-35.85392,4.03921,6.51637,-23.25951]}, - {"t":0.44085, "x":2.55978, "y":3.91857, "heading":-0.14278, "vx":-2.78307, "vy":0.18741, "omega":-0.84917, "ax":-6.04262, "ay":-1.40283, "alpha":-4.11391, "fx":[-89.40855,-103.33727,-103.9148,-98.48042], "fy":[-52.65486,-6.72413,1.03045,-33.38588]}, - {"t":0.47234, "x":2.46915, "y":3.92377, "heading":-0.16952, "vx":-2.97334, "vy":0.14323, "omega":-0.97872, "ax":-5.55178, "ay":-2.52948, "alpha":-5.06122, "fx":[-70.17272,-97.90626,-103.2632,-91.70212], "fy":[-76.13118,-31.63585,-8.96747,-48.67423]}, - {"t":0.50383, "x":2.37277, "y":3.92703, "heading":-0.20034, "vx":-3.14817, "vy":0.06358, "omega":-1.13809, "ax":-3.80488, "ay":-4.47666, "alpha":-6.84919, "fx":[-30.80241,-44.82054,-98.15321,-75.03383], "fy":[-98.67879,-91.19504,-31.45282,-71.41274]}, - {"t":0.53532, "x":2.27175, "y":3.92681, "heading":-0.23618, "vx":-3.26798, "vy":-0.07739, "omega":-1.35377, "ax":-0.23156, "ay":-5.6989, "alpha":-8.40403, "fx":[18.15155,55.16845,-51.66103,-36.80147], "fy":[-101.852,-86.47431,-87.74703,-96.59152]}, - {"t":0.56681, "x":2.16873, "y":3.92155, "heading":-0.27881, "vx":-3.27527, "vy":-0.25684, "omega":-1.61841, "ax":3.14052, "ay":-5.23571, "alpha":-4.84015, "fx":[52.07439,81.18042,54.83517,17.27581], "fy":[-89.65312,-64.08424,-86.68113,-101.95735]}, - {"t":0.5983, "x":2.06715, "y":3.91086, "heading":-0.32977, "vx":-3.17638, "vy":-0.42171, "omega":-1.77082, "ax":4.55278, "ay":-4.26874, "alpha":-3.42102, "fx":[70.21324,88.97405,83.18674,55.34284], "fy":[-76.54952,-53.44948,-61.49854,-87.64546]}, - {"t":0.62979, "x":1.96938, "y":3.89547, "heading":-0.38553, "vx":-3.03301, "vy":-0.55613, "omega":-1.87854, "ax":5.16651, "ay":-3.61733, "alpha":-2.40513, "fx":[80.03142,92.4196,91.02372,74.37568], "fy":[-66.43437,-47.66467,-49.93615,-72.51056]}, - {"t":0.66128, "x":1.87644, "y":3.87616, "heading":-0.44468, "vx":-2.87032, "vy":-0.67004, "omega":-1.95428, "ax":5.48, "ay":-3.1864, "alpha":-1.74616, "fx":[85.80171,94.32059,94.30314,83.92491], "fy":[-58.96583,-44.06039,-43.88081,-61.45943]}, - {"t":0.69277, "x":1.78877, "y":3.85348, "heading":-0.50622, "vx":-2.69776, "vy":-0.77038, "omega":-2.00927, "ax":5.66154, "ay":-2.88929, "alpha":-1.30788, "fx":[89.4803,95.52339,96.02598,89.19221], "fy":[-53.35687,-41.58482,-40.26764,-53.72824]}, - {"t":0.72426, "x":1.70663, "y":3.82779, "heading":-0.56949, "vx":-2.51948, "vy":-0.86136, "omega":-2.05045, "ax":5.77711, "ay":-2.67475, "alpha":-1.00039, "fx":[91.98667,96.35904,97.05944,92.37358], "fy":[-49.02278,-39.75701,-37.91817,-48.21016]}, - {"t":0.75575, "x":1.63015, "y":3.79934, "heading":-0.63406, "vx":-2.33757, "vy":-0.94559, "omega":-2.08195, "ax":5.85602, "ay":-2.5135, "alpha":-0.77403, "fx":[93.78603,96.98084,97.73433,94.43792], "fy":[-45.58023,-38.32947,-36.29855,-44.15578]}, - {"t":0.78724, "x":1.55945, "y":3.76832, "heading":-0.69962, "vx":-2.15316, "vy":-1.02473, "omega":-2.10633, "ax":5.91284, "ay":-2.38828, "alpha":-0.60084, "fx":[95.13182,97.46865,98.20174,95.85267], "fy":[-42.78022,-37.16309,-35.13408,-41.09784]}, - {"t":0.81872, "x":1.49458, "y":3.73487, "heading":-0.76595, "vx":-1.96697, "vy":-1.09994, "omega":-2.12525, "ax":5.95547, "ay":-2.28839, "alpha":-0.46424, "fx":[96.17135,97.86774,98.53984,96.86336], "fy":[-40.45781,-36.17424,-34.26931,-38.74196]}, - {"t":0.85021, "x":1.43559, "y":3.6991, "heading":-0.83287, "vx":-1.77944, "vy":-1.172, "omega":-2.13986, "ax":5.9885, "ay":-2.20695, "alpha":-0.35388, "fx":[96.99507,98.20548,98.79297,97.60865], "fy":[-38.50087,-35.31008,-33.60966,-36.89683]}, - {"t":0.8817, "x":1.38253, "y":3.6611, "heading":-0.90025, "vx":-1.59086, "vy":-1.24149, "omega":-2.15101, "ax":6.01477, "ay":-2.13932, "alpha":-0.26297, "fx":[97.66113,98.49923,98.98812,98.17168], "fy":[-36.83111,-34.53591,-33.0942,-35.43378]}, - {"t":0.91319, "x":1.33541, "y":3.62094, "heading":-0.96799, "vx":-1.40146, "vy":-1.30886, "omega":-2.15929, "ax":6.03612, "ay":-2.08229, "alpha":-0.1869, "fx":[98.20844,98.76039,99.14267,98.6049], "fy":[-35.39243,-33.82828,-32.68185,-34.26338]}, - {"t":0.94468, "x":1.29427, "y":3.5787, "heading":-1.03598, "vx":-1.21139, "vy":-1.37443, "omega":-2.16517, "ax":6.05379, "ay":-2.03357, "alpha":-0.12242, "fx":[98.66393,98.99668,99.2683,98.94282], "fy":[-34.14369,-33.17106,-32.34386,-33.32161]}, - {"t":0.97617, "x":1.25913, "y":3.53441, "heading":-1.10416, "vx":-1.02076, "vy":-1.43847, "omega":-2.16903, "ax":6.06863, "ay":-1.99148, "alpha":-0.06714, "fx":[99.04678,99.21344,99.37317,99.20899], "fy":[-33.05404,-32.55297,-32.05951,-32.56138]}, - {"t":1.00766, "x":1.23, "y":3.48812, "heading":-1.17246, "vx":-0.82966, "vy":-1.50118, "omega":-2.17114, "ax":6.08127, "ay":-1.95476, "alpha":-0.0193, "fx":[99.37104,99.41439,99.46314,99.42002], "fy":[-32.09987,-31.96615,-31.8135,-31.94722]}, - {"t":1.03915, "x":1.20689, "y":3.43988, "heading":-1.24083, "vx":-0.63816, "vy":-1.56273, "omega":-2.17175, "ax":6.09214, "ay":-1.92246, "alpha":0.02245, "fx":[99.64722,99.60216,99.54252,99.58791], "fy":[-31.26273,-31.4051,-31.59427,-31.45182]}, - {"t":1.07064, "x":1.18981, "y":3.38972, "heading":-1.30922, "vx":-0.44632, "vy":-1.62327, "omega":-2.17104, "ax":6.1016, "ay":-1.89381, "alpha":0.05916, "fx":[99.88335,99.77863,99.61457,99.72159], "fy":[-30.52796,-30.86607,-31.393,-31.05374]}, - {"t":1.10213, "x":1.17878, "y":3.33767, "heading":-1.37758, "vx":-0.25419, "vy":-1.6829, "omega":-2.16918, "ax":1.91547, "ay":-0.54898, "alpha":13.57729, "fx":[63.68219,-13.25648,6.93328,67.89787], "fy":[31.3742,29.17201,-62.516,-33.92917]}, - {"t":1.11907, "x":1.17475, "y":3.30907, "heading":-1.41434, "vx":-0.22174, "vy":-1.69221, "omega":-1.93915, "ax":1.93968, "ay":-0.47168, "alpha":13.25873, "fx":[63.62646,-9.69559,6.13388,66.77536], "fy":[30.51356,31.9512,-59.72259,-33.58671]}, - {"t":1.13602, "x":1.17127, "y":3.28034, "heading":-1.44719, "vx":-0.18887, "vy":-1.7002, "omega":-1.71451, "ax":1.95783, "ay":-0.3891, "alpha":12.93195, "fx":[63.31641,-6.41831,5.4743,65.65507], "fy":[29.915,34.32946,-56.69552,-32.99302]}, - {"t":1.15296, "x":1.16835, "y":3.25147, "heading":-1.47624, "vx":-0.1557, "vy":-1.70679, "omega":-1.4954, "ax":1.97301, "ay":-0.30238, "alpha":12.59978, "fx":[62.89593,-3.72393,5.18445,64.66352], "fy":[29.50821,36.22782,-53.43577,-32.0735]}, - {"t":1.1699, "x":1.166, "y":3.22251, "heading":-1.50158, "vx":-0.12227, "vy":-1.71191, "omega":-1.28193, "ax":1.98471, "ay":-0.21174, "alpha":12.24499, "fx":[62.27827,-1.16285,4.998,63.6718], "fy":[29.3041,37.7457,-49.97418,-30.92204]}, - {"t":1.18684, "x":1.16421, "y":3.19348, "heading":-1.5233, "vx":-0.08865, "vy":-1.7155, "omega":-1.07447, "ax":1.99245, "ay":-0.11754, "alpha":11.88407, "fx":[61.55879,0.79515,5.16446,62.77262], "fy":[29.25738,38.90917,-46.33747,-29.51516]}, - {"t":1.20379, "x":1.16299, "y":3.1644, "heading":-1.5415, "vx":-0.05489, "vy":-1.71749, "omega":-0.87312, "ax":1.99575, "ay":-0.02021, "alpha":11.50234, "fx":[60.67162,2.67379,5.31952,61.84221], "fy":[29.35214,39.79679,-42.56695,-27.90344]}, - {"t":1.22073, "x":1.16235, "y":3.13529, "heading":-1.55629, "vx":-0.02107, "vy":-1.71783, "omega":-0.67824, "ax":1.99422, "ay":0.07967, "alpha":11.11587, "fx":[59.67892,4.0424,5.72199,60.96335], "fy":[29.57441,40.43024,-38.7037,-26.09095]}, - {"t":1.23767, "x":1.16228, "y":3.1062, "heading":-1.56778, "vx":0.01271, "vy":-1.71648, "omega":-0.4899, "ax":1.98751, "ay":0.18144, "alpha":10.71252, "fx":[58.53586,5.26522,6.15076,60.01594], "fy":[29.89311,40.86757,-34.78888,-24.10719]}, - {"t":1.25461, "x":1.16278, "y":3.07714, "heading":-1.57608, "vx":0.04639, "vy":-1.71341, "omega":-0.3084, "ax":1.97539, "ay":0.28432, "alpha":10.30957, "fx":[57.26718,6.10838,6.71716,59.08258], "fy":[30.334,41.14395,-30.94033,-21.94549]}, - {"t":1.27156, "x":1.16385, "y":3.04816, "heading":-1.58131, "vx":0.07985, "vy":-1.70859, "omega":-0.13373, "ax":1.95775, "ay":0.38749, "alpha":9.89122, "fx":[55.89072,6.84558,7.23191,58.05363], "fy":[30.79712,41.25345,-27.05706,-19.65458]}, - {"t":1.2885, "x":1.16548, "y":3.01926, "heading":-1.58358, "vx":0.11302, "vy":-1.70203, "omega":0.03385, "ax":1.93461, "ay":0.49009, "alpha":9.46858, "fx":[54.36168,7.35801,7.81736,56.97177], "fy":[31.35762,41.22535,-23.27902,-17.25556]}, - {"t":1.30544, "x":1.16768, "y":2.9905, "heading":-1.583, "vx":0.1458, "vy":-1.69372, "omega":0.19428, "ax":1.90613, "ay":0.59127, "alpha":9.04103, "fx":[52.78226,7.66809,8.38396,55.81215], "fy":[31.87956,41.08692,-19.56909,-14.73255]}, - {"t":1.32239, "x":1.17042, "y":2.96189, "heading":-1.57971, "vx":0.1781, "vy":-1.68371, "omega":0.34746, "ax":1.8726, "ay":0.69021, "alpha":8.60287, "fx":[51.02132,7.91455,8.98297,54.53503], "fy":[32.47538,40.82015,-16.01393,-12.14745]}, - {"t":1.33933, "x":1.17371, "y":2.93346, "heading":-1.57382, "vx":0.20982, "vy":-1.67201, "omega":0.49321, "ax":1.83443, "ay":0.78614, "alpha":8.17118, "fx":[49.25932,7.98938,9.50335,53.20548], "fy":[33.0018,40.49413,-12.62693,-9.46129]}, - {"t":1.35627, "x":1.17753, "y":2.90524, "heading":-1.56547, "vx":0.2409, "vy":-1.65869, "omega":0.63165, "ax":1.79211, "ay":0.87842, "alpha":7.71836, "fx":[47.30074,8.10138,10.09515,51.69338], "fy":[33.55434,40.03028,-9.36463,-6.7777]}, - {"t":1.37321, "x":1.18186, "y":2.87727, "heading":-1.55476, "vx":0.27127, "vy":-1.64381, "omega":0.76242, "ax":1.74624, "ay":0.96652, "alpha":7.2817, "fx":[45.4052,8.04993,10.5602,50.17515], "fy":[33.9982,39.53288,-6.28885,-4.03888]}, - {"t":1.39016, "x":1.18671, "y":2.84955, "heading":-1.54185, "vx":0.30085, "vy":-1.62743, "omega":0.8858, "ax":1.69741, "ay":1.05002, "alpha":6.81747, "fx":[43.30472,8.09314,11.17413,48.42559], "fy":[34.43596,38.91702,-3.35721,-1.33228]}, - {"t":1.4071, "x":1.19205, "y":2.82213, "heading":-1.52684, "vx":0.32961, "vy":-1.60964, "omega":1.0013, "ax":1.64626, "ay":1.12865, "alpha":6.37673, "fx":[41.31342,8.06368,11.55906,46.71655], "fy":[34.74966,38.28521,-0.61197,1.38202]}, - {"t":1.42404, "x":1.19787, "y":2.79502, "heading":-1.50987, "vx":0.3575, "vy":-1.59052, "omega":1.10934, "ax":1.59341, "ay":1.20223, "alpha":5.90023, "fx":[39.11913,8.18297,12.15956,44.73489], "fy":[35.01802,37.54452,2.03443,4.01975]}, - {"t":1.44098, "x":1.20416, "y":2.76825, "heading":-1.49108, "vx":0.3845, "vy":-1.57015, "omega":1.20931, "ax":1.53943, "ay":1.27074, "alpha":5.45565, "fx":[37.09747,8.20721,12.5055,42.8567], "fy":[35.14038,36.80917,4.52162,6.62524]}, - {"t":1.45793, "x":1.21089, "y":2.74183, "heading":-1.47059, "vx":0.41058, "vy":-1.54862, "omega":1.30174, "ax":1.48487, "ay":1.3342, "alpha":4.96785, "fx":[34.86548,8.4862,13.08611,40.66138], "fy":[35.20211,35.98809,6.92018,9.13626]}, - {"t":1.47487, "x":1.21806, "y":2.71578, "heading":-1.44854, "vx":0.43574, "vy":-1.52602, "omega":1.38591, "ax":1.4302, "ay":1.39277, "alpha":4.51961, "fx":[32.87112,8.61583,13.39091,38.64631], "fy":[35.0948,35.18526,9.2372,11.55901]}, - {"t":1.49181, "x":1.22565, "y":2.69012, "heading":-1.42505, "vx":0.45997, "vy":-1.50242, "omega":1.46249, "ax":1.37584, "ay":1.44661, "alpha":4.02301, "fx":[30.67259,9.09525,13.93258,36.26905], "fy":[34.92013,34.33394,11.44916,13.8939]}, - {"t":1.50876, "x":1.23364, "y":2.66488, "heading":-1.40028, "vx":0.48328, "vy":-1.47791, "omega":1.53065, "ax":1.32213, "ay":1.49597, "alpha":3.57351, "fx":[28.7375,9.38392,14.19282,34.14321], "fy":[34.59105,33.52077,13.60858,16.10451]}, - {"t":1.5257, "x":1.24202, "y":2.64005, "heading":-1.37434, "vx":0.50568, "vy":-1.45257, "omega":1.59119, "ax":1.26937, "ay":1.5411, "alpha":3.06651, "fx":[26.60545,10.11294,14.68701,31.60199], "fy":[34.17702,32.6799,15.69607,18.22329]}, - {"t":1.54264, "x":1.25077, "y":2.61566, "heading":-1.34738, "vx":0.52719, "vy":-1.42645, "omega":1.64315, "ax":1.21778, "ay":1.58229, "alpha":2.61565, "fx":[24.80849,10.56047,14.86634,29.39848], "fy":[33.58646,31.89952,17.79613,20.1878]}, - {"t":1.55958, "x":1.25988, "y":2.59172, "heading":-1.31954, "vx":0.54782, "vy":-1.39965, "omega":1.68746, "ax":1.16755, "ay":1.61982, "alpha":2.10388, "fx":[22.83451,11.51022,15.24286,26.76104], "fy":[32.91502,31.11256,19.84744,22.04905]}, - {"t":1.57653, "x":1.26933, "y":2.56824, "heading":-1.29095, "vx":0.5676, "vy":-1.3722, "omega":1.72311, "ax":1.11879, "ay":1.65398, "alpha":1.6488, "fx":[21.17161,12.16428,15.36168,24.46246], "fy":[32.0985,30.41832,21.86822,23.77256]}, - {"t":1.59347, "x":1.2791, "y":2.54523, "heading":-1.26176, "vx":0.58656, "vy":-1.34418, "omega":1.75104, "ax":1.0716, "ay":1.68503, "alpha":1.12866, "fx":[19.35726,13.37538,15.65497,21.68665], "fy":[31.20721,29.74429,23.83784,25.39896]}, - {"t":1.61041, "x":1.28919, "y":2.5227, "heading":-1.23209, "vx":0.60472, "vy":-1.31563, "omega":1.77017, "ax":1.02603, "ay":1.71325, "alpha":0.6774, "fx":[17.90246,14.17593,15.64089,19.37535], "fy":[30.16337,29.1787,25.83293,26.8587]}, - {"t":1.62735, "x":1.29959, "y":2.50065, "heading":-1.2021, "vx":0.6221, "vy":-1.2866, "omega":1.78164, "ax":1.00316, "ay":1.7286, "alpha":-8.41315, "fx":[3.48183,49.28221,28.40516,-15.57049], "fy":[-1.128,15.85829,54.08636,44.22083]}, - {"t":1.66412, "x":1.32314, "y":2.45451, "heading":-1.13659, "vx":0.65898, "vy":-1.22305, "omega":1.47231, "ax":3.19748, "ay":5.53358, "alpha":-0.02586, "fx":[52.30115,52.44564,52.24454,52.09963], "fy":[90.44693,90.36348,90.48008,90.56334]}, - {"t":1.70089, "x":1.34953, "y":2.41329, "heading":-1.08246, "vx":0.77655, "vy":-1.01958, "omega":1.47135, "ax":3.18575, "ay":5.53994, "alpha":-0.04218, "fx":[52.14232,52.36061,52.02028,51.80073], "fy":[90.53205,90.40643,90.60296,90.72821]}, - {"t":1.73766, "x":1.38024, "y":2.37954, "heading":-1.02836, "vx":0.89368, "vy":-0.81589, "omega":1.4698, "ax":3.17258, "ay":5.54704, "alpha":-0.06043, "fx":[51.97479,52.26169,51.7578,51.468], "fy":[90.62098,90.45654,90.74619,90.91016]}, - {"t":1.77443, "x":1.41524, "y":2.35329, "heading":-0.97432, "vx":1.01033, "vy":-0.61193, "omega":1.46758, "ax":3.15767, "ay":5.55501, "alpha":-0.081, "fx":[51.79655,52.14529,51.45003,51.09561], "fy":[90.71467,90.51571,90.9132,91.11183]}, - {"t":1.8112, "x":1.45452, "y":2.33455, "heading":-0.92036, "vx":1.12644, "vy":-0.40769, "omega":1.4646, "ax":3.14067, "ay":5.56403, "alpha":-0.10436, "fx":[51.60495,52.00686,51.08804,50.67601], "fy":[90.81434,90.58627,91.10822,91.33659]}, - {"t":1.84796, "x":1.49806, "y":2.32332, "heading":-0.8665, "vx":1.24191, "vy":-0.20311, "omega":1.46077, "ax":3.12111, "ay":5.57432, "alpha":-0.13114, "fx":[51.39643,51.84057,50.66031,50.19925], "fy":[90.92164,90.67119,91.3366,91.58882]}, - {"t":1.88473, "x":1.54584, "y":2.31962, "heading":-0.81279, "vx":1.35667, "vy":0.00185, "omega":1.45594, "ax":3.09835, "ay":5.58616, "alpha":-0.16219, "fx":[51.16617,51.63882,50.15172,49.65202], "fy":[91.03879,90.77431,91.60513,91.87423]}, - {"t":1.9215, "x":1.59781, "y":2.32346, "heading":-0.75926, "vx":1.47059, "vy":0.20725, "omega":1.44998, "ax":3.07157, "ay":5.59993, "alpha":-0.1986, "fx":[50.90752,51.39151,49.54197,49.01608], "fy":[91.16883,90.90064,91.92266,92.20051]}, - {"t":1.95827, "x":1.65396, "y":2.33487, "heading":-0.70595, "vx":1.58353, "vy":0.41315, "omega":1.44268, "ax":3.03957, "ay":5.61613, "alpha":-0.24195, "fx":[50.61113,51.08487,48.80314,48.26581], "fy":[91.31599,91.05688,92.3009,92.57811]}, - {"t":1.99504, "x":1.71424, "y":2.35386, "heading":-0.6529, "vx":1.69529, "vy":0.61964, "omega":1.43378, "ax":3.0007, "ay":5.63546, "alpha":-0.29441, "fx":[50.26347,50.69959,47.89562,47.3641], "fy":[91.48633,91.25224,92.7557,93.02161]}, - {"t":2.03181, "x":1.7786, "y":2.38045, "heading":-0.60019, "vx":1.80562, "vy":0.82685, "omega":1.42296, "ax":2.95246, "ay":5.65891, "alpha":-0.35921, "fx":[49.84431,50.20767,46.7614,46.25523], "fy":[91.68879,91.49972,93.309,93.55189]}, - {"t":2.06857, "x":1.84699, "y":2.41467, "heading":-0.54787, "vx":1.91418, "vy":1.03492, "omega":1.40975, "ax":2.89105, "ay":5.68793, "alpha":-0.44128, "fx":[49.32205,49.56686,45.31187,44.85172], "fy":[91.93702,91.81832,93.99193,94.19978]}, - {"t":2.10534, "x":1.91932, "y":2.45657, "heading":-0.49603, "vx":2.02048, "vy":1.24405, "omega":1.39353, "ax":2.81024, "ay":5.7247, "alpha":-0.54857, "fx":[48.64469,48.71012,43.40486,43.00836], "fy":[92.25291,92.23701,94.84979,95.01227]}, - {"t":2.14211, "x":1.99551, "y":2.50618, "heading":-0.44479, "vx":2.12381, "vy":1.45454, "omega":1.37336, "ax":2.69922, "ay":5.77268, "alpha":-0.69473, "fx":[47.7203,47.5242,40.79762,40.46612], "fy":[92.67379,92.80212,95.94983,96.06349]}, - {"t":2.17888, "x":2.07542, "y":2.56357, "heading":-0.3943, "vx":2.22305, "vy":1.66679, "omega":1.34781, "ax":2.53744, "ay":5.83746, "alpha":-0.9054, "fx":[46.3701,45.80142,37.04129,36.71669], "fy":[93.2686,93.59246,97.39175,97.47286]}, - {"t":2.21565, "x":2.15888, "y":2.6288, "heading":-0.34474, "vx":2.31635, "vy":1.88143, "omega":1.31452, "ax":2.2808, "ay":5.92827, "alpha":-1.23493, "fx":[44.19558,43.11593,31.21388,30.62172], "fy":[94.17897,94.75225,99.30967,99.42262]}, - {"t":2.25242, "x":2.24559, "y":2.70198, "heading":-0.29641, "vx":2.40021, "vy":2.0994, "omega":1.26911, "ax":1.81628, "ay":6.05731, "alpha":-1.82133, "fx":[40.09763,38.44043,21.13511,19.09802], "fy":[95.74167,96.5666,101.76536,102.02815]}, - {"t":2.28919, "x":2.33507, "y":2.78327, "heading":-0.24975, "vx":2.46699, "vy":2.32212, "omega":1.20215, "ax":0.7677, "ay":6.19394, "alpha":-3.11884, "fx":[29.62503,28.56878,0.6662,-8.65835], "fy":[98.88177,99.58924,103.63186,102.93379]}, - {"t":2.32595, "x":2.42629, "y":2.87284, "heading":-0.20554, "vx":2.49522, "vy":2.54986, "omega":1.08747, "ax":-2.45909, "ay":5.40186, "alpha":-5.91716, "fx":[-30.84697,-2.15136,-47.33873,-80.46848], "fy":[95.6113,102.66685,91.55722,63.40516]}, - {"t":2.36272, "x":2.51638, "y":2.97024, "heading":-0.16556, "vx":2.4048, "vy":2.74848, "omega":0.86991, "ax":-5.85942, "ay":-0.7352, "alpha":-6.59554, "fx":[-83.87998,-99.24313,-101.1979,-98.84039], "fy":[-58.5756,20.88873,19.09642,-29.48631]}, - {"t":2.39949, "x":2.60084, "y":3.0708, "heading":-0.13357, "vx":2.18936, "vy":2.72145, "omega":0.6274, "ax":-5.13606, "ay":-3.54484, "alpha":-3.19838, "fx":[-68.57415,-85.95926,-96.67288,-84.65292], "fy":[-77.59211,-57.09699,-37.1593,-59.9572]}, - {"t":2.43626, "x":2.67786, "y":3.16847, "heading":-0.11051, "vx":2.00052, "vy":2.59111, "omega":0.5098, "ax":-4.60133, "ay":-4.32297, "alpha":-2.16642, "fx":[-63.4324,-74.6474,-86.34368,-76.46829], "fy":[-82.3175,-72.08723,-57.79833,-70.48617]}, - {"t":2.47303, "x":2.74831, "y":3.26082, "heading":-0.09176, "vx":1.83133, "vy":2.43216, "omega":0.43015, "ax":-4.30725, "ay":-4.65506, "alpha":-1.65792, "fx":[-60.92755,-69.31525,-79.688,-71.73052], "fy":[-84.40393,-77.55388,-66.95139,-75.49652]}, - {"t":2.5098, "x":2.81273, "y":3.3471, "heading":-0.07595, "vx":1.67296, "vy":2.261, "omega":0.36919, "ax":-4.12643, "ay":-4.83518, "alpha":-1.35991, "fx":[-59.45405,-66.29069,-75.38446,-68.70764], "fy":[-85.57171,-80.32464,-71.91525,-78.37254]}, - {"t":2.54656, "x":2.87146, "y":3.42696, "heading":-0.06237, "vx":1.52124, "vy":2.08322, "omega":0.31919, "ax":-4.00499, "ay":-4.94731, "alpha":-1.16541, "fx":[-58.48639,-64.3519,-72.43025,-66.62725], "fy":[-86.31582,-81.99009,-74.98599,-80.22428]}, - {"t":2.58333, "x":2.92468, "y":3.50021, "heading":-0.05064, "vx":1.37398, "vy":1.90131, "omega":0.27634, "ax":-3.91809, "ay":-5.02354, "alpha":-1.02884, "fx":[-57.80353,-63.00445,-70.29172,-65.11378], "fy":[-86.83045,-83.1,-77.06011,-81.51092]}, - {"t":2.6201, "x":2.97255, "y":3.56673, "heading":-0.04047, "vx":1.22992, "vy":1.71661, "omega":0.23851, "ax":-3.85294, "ay":-5.07864, "alpha":-0.92782, "fx":[-57.29661,-62.01317,-68.67701,-63.96612], "fy":[-87.20702,-83.89262,-78.55041,-82.45436]}, - {"t":2.65687, "x":3.01517, "y":3.62641, "heading":-0.03171, "vx":1.08826, "vy":1.52987, "omega":0.20439, "ax":-3.80232, "ay":-5.12027, "alpha":-0.8501, "fx":[-56.90592,-61.25251,-67.41657,-63.06768], "fy":[-87.49416,-84.48746,-79.67108,-83.17425]}, - {"t":2.69364, "x":3.05261, "y":3.6792, "heading":-0.02419, "vx":0.94845, "vy":1.34161, "omega":0.17313, "ax":-3.76188, "ay":-5.15282, "alpha":-0.78848, "fx":[-56.59596,-60.64949,-66.40612,-62.3465], "fy":[-87.7201,-84.95089,-80.54366,-83.7406]}, - {"t":2.73041, "x":3.08494, "y":3.72505, "heading":-0.01782, "vx":0.81013, "vy":1.15215, "omega":0.14414, "ax":-3.72883, "ay":-5.17895, "alpha":-0.73843, "fx":[-56.34436,-60.15884,-65.57831,-61.75585], "fy":[-87.90232,-85.32272,-81.24195,-84.19698]}, - {"t":2.76717, "x":3.11221, "y":3.76391, "heading":-0.01252, "vx":0.67303, "vy":0.96173, "omega":0.11699, "ax":-3.70134, "ay":-5.20039, "alpha":-0.69697, "fx":[-56.1363,-59.75101,-64.88785,-61.26412], "fy":[-88.05223,-85.6282,-81.81328,-84.57193]}, - {"t":2.80394, "x":3.13445, "y":3.79575, "heading":-0.00822, "vx":0.53694, "vy":0.77052, "omega":0.09137, "ax":-3.6781, "ay":-5.21828, "alpha":-0.66206, "fx":[-55.96159,-59.40588,-64.30318,-60.84915], "fy":[-88.17759,-85.88417,-82.28932,-84.88488]}, - {"t":2.84071, "x":3.15171, "y":3.82056, "heading":-0.00486, "vx":0.4017, "vy":0.57865, "omega":0.06702, "ax":-3.65821, "ay":-5.23345, "alpha":-0.63226, "fx":[-55.813,-59.10933,-63.80169,-60.49499], "fy":[-88.28385,-86.10223,-82.69208,-85.14952]}, - {"t":2.87748, "x":3.16401, "y":3.8383, "heading":-0.0024, "vx":0.26719, "vy":0.38622, "omega":0.04378, "ax":-3.64099, "ay":-5.24646, "alpha":-0.60652, "fx":[-55.68526,-58.8511,-63.36674,-60.18984], "fy":[-88.37496,-86.29068,-83.0373,-85.37575]}, - {"t":2.91425, "x":3.17137, "y":3.84895, "heading":-0.00079, "vx":0.13332, "vy":0.19332, "omega":0.02147, "ax":-3.62594, "ay":-5.25775, "alpha":-0.58406, "fx":[-55.57442,-58.6236,-62.98584,-59.9248], "fy":[-88.45384,-86.45558,-83.33654,-85.57095]}, - {"t":2.95102, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.28147, "ay":-0.36295, "alpha":-3.36884, "fx":[-100.73663,-103.50075,-103.99191,-102.53097], "fy":[-27.5229,13.86092,9.95784,-20.0299]}, + {"t":0.03413, "x":3.17016, "y":3.85229, "heading":0.0, "vx":-0.21438, "vy":-0.01239, "omega":-0.11497, "ax":-6.27958, "ay":-0.38675, "alpha":-3.36839, "fx":[-100.60794,-103.55196,-104.01661,-102.46009], "fy":[-27.95568,13.39812,9.6221,-20.35516]}, + {"t":0.06826, "x":3.15919, "y":3.85164, "heading":-0.00392, "vx":-0.42869, "vy":-0.02559, "omega":-0.22993, "ax":-6.27737, "ay":-0.4136, "alpha":-3.36711, "fx":[-100.44599,-103.61624,-104.03769,-102.39216], "fy":[-28.49504,12.8015,9.30346,-20.65624]}, + {"t":0.10239, "x":3.1409, "y":3.85053, "heading":-0.01177, "vx":-0.64293, "vy":-0.0397, "omega":-0.34485, "ax":-6.27475, "ay":-0.44417, "alpha":-3.36471, "fx":[-100.24639,-103.69296,-104.05606,-102.32553], "fy":[-29.14875,12.05371,8.99095,-20.94139]}, + {"t":0.13651, "x":3.11531, "y":3.84892, "heading":-0.02354, "vx":-0.85708, "vy":-0.05486, "omega":-0.45968, "ax":-6.27161, "ay":-0.47936, "alpha":-3.36074, "fx":[-100.00371,-103.78096,-104.07272,-102.25792], "fy":[-29.92587,11.13104,8.66979,-21.22116]}, + {"t":0.17064, "x":3.0824, "y":3.84676, "heading":-0.03923, "vx":-1.07112, "vy":-0.07122, "omega":-0.57438, "ax":-6.26776, "ay":-0.52033, "alpha":-3.35458, "fx":[-99.71082,-103.87811,-104.08881,-102.18611], "fy":[-30.83778,10.0015,8.31994,-21.50954]}, + {"t":0.20477, "x":3.0422, "y":3.84403, "heading":-0.05883, "vx":-1.28503, "vy":-0.08898, "omega":-0.68887, "ax":-6.26296, "ay":-0.56872, "alpha":-3.34539, "fx":[-99.35808,-103.98051,-104.10562,-102.10547], "fy":[-31.89997,8.62182,7.91385,-21.82571]}, + {"t":0.2389, "x":2.99469, "y":3.84066, "heading":-0.08234, "vx":-1.49878, "vy":-0.10839, "omega":-0.80304, "ax":-6.2568, "ay":-0.62677, "alpha":-3.33216, "fx":[-98.93186,-104.08114,-104.12452,-102.00924], "fy":[-33.13469,6.93281,7.41286,-22.19682]}, + {"t":0.27303, "x":2.9399, "y":3.8366, "heading":-0.10975, "vx":-1.71232, "vy":-0.12978, "omega":-0.91676, "ax":-6.24864, "ay":-0.69771, "alpha":-3.31361, "fx":[-98.41195,-104.16743,-104.14672,-101.88706], "fy":[-34.57533,4.85171,6.76127,-22.66256]}, + {"t":0.30716, "x":2.87782, "y":3.83176, "heading":-0.14104, "vx":-1.92557, "vy":-0.15359, "omega":-1.02985, "ax":-6.2374, "ay":-0.78635, "alpha":-3.28812, "fx":[-97.76699,-104.21646,-104.17252,-101.72253], "fy":[-36.27379,2.25925,5.87588,-23.28295]}, + {"t":0.34129, "x":2.80847, "y":3.82606, "heading":-0.17618, "vx":-2.13845, "vy":-0.18043, "omega":-1.14207, "ax":-6.22119, "ay":-0.90015, "alpha":-3.25369, "fx":[-96.94565,-104.18527,-104.1993,-101.48813], "fy":[-38.31352,-1.02374,4.62671,-24.15247]}, + {"t":0.37542, "x":2.73186, "y":3.81938, "heading":-0.21516, "vx":-2.35077, "vy":-0.21115, "omega":-1.25312, "ax":-6.19641, "ay":-1.05124, "alpha":-3.20761, "fx":[-95.8586,-103.98918,-104.21585,-101.13444], "fy":[-40.8338,-5.28063,2.79867,-25.42755]}, + {"t":0.40954, "x":2.64803, "y":3.81156, "heading":-0.25793, "vx":-2.56225, "vy":-0.24703, "omega":-1.36259, "ax":-6.1557, "ay":-1.26075, "alpha":-3.14593, "fx":[-94.33817,-103.44856,-104.18502,-100.56407], "fy":[-44.07821,-10.98912,0.00818,-27.38432]}, + {"t":0.44367, "x":2.55699, "y":3.8024, "heading":-0.30443, "vx":-2.77233, "vy":-0.29006, "omega":-1.46995, "ax":-6.0821, "ay":-1.56852, "alpha":-3.06193, "fx":[-92.03694,-102.141,-103.98496,-99.56034], "fy":[-48.49949,-19.01685,-4.4993,-30.55389]}, + {"t":0.4778, "x":2.45884, "y":3.79159, "heading":-0.3546, "vx":-2.97991, "vy":-0.34359, "omega":-1.57445, "ax":-5.92998, "ay":-2.05789, "alpha":-2.94112, "fx":[-88.13121,-98.92494,-103.16872,-97.55067], "fy":[-55.00664,-31.06575,-12.41516,-36.08291]}, + {"t":0.51193, "x":2.35368, "y":3.77866, "heading":-0.40833, "vx":-3.18229, "vy":-0.41382, "omega":-1.67483, "ax":-5.54603, "ay":-2.92356, "alpha":-2.73554, "fx":[-80.25967,-90.14068,-99.68133,-92.58632], "fy":[-65.57334,-50.54326,-28.20847,-46.85355]}, + {"t":0.54606, "x":2.24185, "y":3.76283, "heading":-0.46549, "vx":-3.37157, "vy":-0.5136, "omega":-1.76819, "ax":-4.25389, "ay":-4.59538, "alpha":-2.13966, "fx":[-59.75928,-62.89174,-80.49507,-75.02557], "fy":[-84.1017,-81.32275,-64.15911,-70.91926]}, + {"t":0.58019, "x":2.1243, "y":3.74263, "heading":-0.52584, "vx":-3.51675, "vy":-0.67043, "omega":-1.84122, "ax":0.20655, "ay":-6.26704, "alpha":0.00075, "fx":[3.37544,3.37104,3.37794,3.38233], "fy":[-102.45402,-102.45431,-102.45417,-102.45388]}, + {"t":0.61432, "x":2.0044, "y":3.7161, "heading":-0.58868, "vx":-3.5097, "vy":-0.88432, "omega":-1.84119, "ax":4.38611, "ay":-4.47855, "alpha":2.09501, "fx":[78.70805,61.19962,66.53516,80.37551], "fy":[-66.28286,-82.83271,-78.98072,-64.76696]}, + {"t":0.64845, "x":1.88717, "y":3.68331, "heading":-0.65152, "vx":-3.36001, "vy":-1.03717, "omega":-1.76969, "ax":5.56221, "ay":-2.89755, "alpha":2.78739, "fx":[99.00647,84.31162,84.03672,96.37132], "fy":[-30.24856,-59.95437,-60.81295,-38.46177]}, + {"t":0.68257, "x":1.77574, "y":3.64623, "heading":-0.71191, "vx":-3.17017, "vy":-1.13605, "omega":-1.67456, "ax":5.92477, "ay":-2.07884, "alpha":2.9592, "fx":[103.07537,93.59819,90.32362,100.43761], "fy":[-12.69656,-44.73395,-51.47567,-27.03388]}, + {"t":0.7167, "x":1.67099, "y":3.60624, "heading":-0.76906, "vx":-2.96797, "vy":-1.207, "omega":-1.57357, "ax":6.07544, "ay":-1.6101, "alpha":3.00311, "fx":[103.98098,98.04669,93.32001,101.93984], "fy":[-3.41184,-34.42747,-46.11803,-21.33068]}, + {"t":0.75083, "x":1.57324, "y":3.56411, "heading":-0.82277, "vx":-2.76062, "vy":-1.26195, "omega":-1.47107, "ax":6.15166, "ay":-1.31111, "alpha":3.01556, "fx":[104.12864,100.48297,95.02581,102.63444], "fy":[2.08405,-26.94615,-42.69863,-18.17561]}, + {"t":0.78496, "x":1.48261, "y":3.52028, "heading":-0.87297, "vx":-2.55067, "vy":-1.3067, "omega":-1.36816, "ax":6.1954, "ay":-1.10484, "alpha":3.02161, "fx":[104.07426,101.93663,96.11868,103.00232], "fy":[5.60224,-21.19596,-40.32951,-16.32487]}, + {"t":0.81909, "x":1.39916, "y":3.47504, "heading":-0.91967, "vx":-2.33923, "vy":-1.34441, "omega":-1.26503, "ax":6.22271, "ay":-0.95428, "alpha":3.02825, "fx":[103.97317,102.8494,96.88092,103.21435], "fy":[7.97623,-16.59279,-38.5786,-15.20732]}, + {"t":0.85322, "x":1.32295, "y":3.4286, "heading":-0.96284, "vx":-2.12686, "vy":-1.37697, "omega":-1.16168, "ax":6.24081, "ay":-0.83968, "alpha":3.03712, "fx":[103.87255,103.43786,97.44786,103.34319], "fy":[9.63768,-12.80427,-37.21545,-14.52695]}, + {"t":0.88735, "x":1.254, "y":3.38112, "heading":-1.00249, "vx":-1.91387, "vy":-1.40563, "omega":-1.05803, "ax":6.25335, "ay":-0.74964, "alpha":3.04809, "fx":[103.78585,103.82,97.8911,103.42419], "fy":[10.83204,-9.62852,-36.10862,-14.1159]}, + {"t":0.92148, "x":1.19232, "y":3.33271, "heading":-1.0386, "vx":-1.70045, "vy":-1.43122, "omega":-0.954, "ax":6.26233, "ay":-0.6771, "alpha":3.06047, "fx":[103.71515,104.06571,98.25132,103.47641], "fy":[11.7095,-6.93533,-35.17938,-13.87222]}, + {"t":0.9556, "x":1.13794, "y":3.28347, "heading":-1.07116, "vx":-1.48673, "vy":-1.45433, "omega":-0.84955, "ax":6.26895, "ay":-0.61748, "alpha":3.07349, "fx":[103.65871,104.21914,98.55281,103.51105], "fy":[12.36754,-4.63615,-34.37904,-13.73061]}, + {"t":0.98973, "x":1.09085, "y":3.23348, "heading":-1.10015, "vx":-1.27277, "vy":-1.4754, "omega":-0.74466, "ax":6.27396, "ay":-0.56765, "alpha":3.08645, "fx":[103.6137,104.30963,98.81067,103.53518], "fy":[12.87242,-2.66785,-33.67683,-13.64745]}, + {"t":1.02386, "x":1.05106, "y":3.18279, "heading":-1.12556, "vx":-1.05865, "vy":-1.49477, "omega":-0.63932, "ax":6.27784, "ay":-0.52542, "alpha":3.09877, "fx":[103.57722,104.3574,99.03453,103.55352], "fy":[13.27093,-0.98369,-33.05306,-13.59259]}, + {"t":1.05799, "x":1.01859, "y":3.13147, "heading":-1.14738, "vx":-0.8444, "vy":-1.5127, "omega":-0.53356, "ax":6.28091, "ay":-0.48921, "alpha":3.11004, "fx":[103.54661,104.37677,99.23074,103.56936], "fy":[13.59711,0.45209,-32.49502,-13.54463]}, + {"t":1.09212, "x":0.99343, "y":3.07956, "heading":-1.16559, "vx":-0.63004, "vy":-1.5294, "omega":-0.42742, "ax":6.2834, "ay":-0.45783, "alpha":3.11997, "fx":[103.5196,104.37804,99.40357,103.58504], "fy":[13.87631,1.66741,-31.99441,-13.48796]}, + {"t":1.12625, "x":0.97559, "y":3.0271, "heading":-1.18018, "vx":-0.41559, "vy":-1.54503, "omega":-0.32094, "ax":6.28546, "ay":-0.43039, "alpha":3.12838, "fx":[103.49428,104.36871,99.55603,103.60231], "fy":[14.12769,2.68448,-31.54572,-13.41099]}, + {"t":1.16038, "x":0.96506, "y":2.97412, "heading":-1.19113, "vx":-0.20108, "vy":-1.55971, "omega":-0.21417, "ax":6.28722, "ay":-0.40621, "alpha":3.13517, "fx":[103.46906,104.35427,99.69033,103.62242], "fy":[14.36594,3.52098,-31.14503,-13.30492]}, + {"t":1.19451, "x":0.96186, "y":2.92065, "heading":-1.19844, "vx":0.0135, "vy":-1.57358, "omega":-0.10717, "ax":6.28874, "ay":-0.38474, "alpha":3.14029, "fx":[103.44262,104.33873,99.8082,103.64629], "fy":[14.60233,4.19104,-30.78931,-13.16294]}, + {"t":1.22863, "x":0.96598, "y":2.86672, "heading":-1.2021, "vx":0.22812, "vy":-1.58671, "omega":0.0, "ax":6.34427, "ay":-0.73227, "alpha":0.0, "fx":[103.71675,103.71675,103.71675,103.71675], "fy":[-11.97117,-11.97117,-11.97117,-11.97117]}, + {"t":1.2574, "x":0.97517, "y":2.82077, "heading":-1.2021, "vx":0.41062, "vy":-1.60777, "omega":0.0, "ax":5.95844, "ay":-2.27438, "alpha":0.0, "fx":[97.40911,97.40911,97.40911,97.40911], "fy":[-37.1818,-37.1818,-37.1818,-37.1818]}, + {"t":1.28617, "x":0.98945, "y":2.77358, "heading":-1.2021, "vx":0.58202, "vy":-1.6732, "omega":0.0, "ax":3.97032, "ay":-4.96867, "alpha":0.0, "fx":[64.90721,64.90721,64.90721,64.90721], "fy":[-81.22825,-81.22825,-81.22825,-81.22825]}, + {"t":1.31493, "x":1.00783, "y":2.7234, "heading":-1.2021, "vx":0.69623, "vy":-1.81613, "omega":0.0, "ax":2.32991, "ay":-5.89524, "alpha":0.0, "fx":[38.08962,38.08962,38.08962,38.08962], "fy":[-96.37602,-96.37602,-96.37602,-96.37602]}, + {"t":1.3437, "x":1.02883, "y":2.66871, "heading":-1.2021, "vx":0.76326, "vy":-1.98571, "omega":0.0, "ax":2.22431, "ay":-5.87525, "alpha":0.0, "fx":[36.36329,36.36329,36.36329,36.36329], "fy":[-96.04921,-96.04921,-96.04921,-96.04921]}, + {"t":1.37246, "x":1.0517, "y":2.60916, "heading":-1.2021, "vx":0.82724, "vy":-2.15472, "omega":0.0, "ax":-0.90508, "ay":-2.2502, "alpha":0.0, "fx":[-14.79638,-14.79638,-14.79638,-14.79638], "fy":[-36.78655,-36.78655,-36.78655,-36.78655]}, + {"t":1.40123, "x":1.07512, "y":2.54625, "heading":-1.2021, "vx":0.8012, "vy":-2.21945, "omega":0.0, "ax":-2.29302, "ay":5.84582, "alpha":0.0, "fx":[-37.48654,-37.48654,-37.48654,-37.48654], "fy":[95.5681,95.5681,95.5681,95.5681]}, + {"t":1.43, "x":1.09722, "y":2.48482, "heading":-1.2021, "vx":0.73524, "vy":-2.05129, "omega":0.0, "ax":-2.25083, "ay":5.92512, "alpha":0.0, "fx":[-36.79685,-36.79685,-36.79685,-36.79685], "fy":[96.86446,96.86446,96.86446,96.86446]}, + {"t":1.45876, "x":1.11744, "y":2.42827, "heading":-1.2021, "vx":0.6705, "vy":-1.88084, "omega":0.0, "ax":-2.21, "ay":5.96134, "alpha":0.0, "fx":[-36.12927,-36.12927,-36.12927,-36.12927], "fy":[97.4566,97.4566,97.4566,97.4566]}, + {"t":1.48753, "x":1.13581, "y":2.37663, "heading":-1.2021, "vx":0.60692, "vy":-1.70936, "omega":0.0, "ax":-2.15973, "ay":5.99013, "alpha":0.0, "fx":[-35.30753,-35.30753,-35.30753,-35.30753], "fy":[97.92723,97.92723,97.92723,97.92723]}, + {"t":1.51629, "x":1.15238, "y":2.32994, "heading":-1.2021, "vx":0.5448, "vy":-1.53705, "omega":0.0, "ax":-2.08291, "ay":6.02348, "alpha":0.0, "fx":[-34.05164,-34.05164,-34.05164,-34.05164], "fy":[98.47247,98.47247,98.47247,98.47247]}, + {"t":1.54506, "x":1.16719, "y":2.28821, "heading":-1.2021, "vx":0.48488, "vy":-1.36377, "omega":0.0, "ax":-0.19977, "ay":6.37521, "alpha":0.0, "fx":[-3.26583,-3.26583,-3.26583,-3.26583], "fy":[104.22259,104.22259,104.22259,104.22259]}, + {"t":1.57383, "x":1.18105, "y":2.25162, "heading":-1.2021, "vx":0.47913, "vy":-1.18038, "omega":0.0, "ax":2.08598, "ay":6.03348, "alpha":0.0, "fx":[34.1018,34.1018,34.1018,34.1018], "fy":[98.63597,98.63597,98.63597,98.63597]}, + {"t":1.60259, "x":1.1957, "y":2.22016, "heading":-1.2021, "vx":0.53914, "vy":-1.00683, "omega":0.0, "ax":3.15961, "ay":5.55121, "alpha":0.0, "fx":[51.65356,51.65356,51.65356,51.65356], "fy":[90.75178,90.75178,90.75178,90.75178]}, + {"t":1.63136, "x":1.21252, "y":2.1935, "heading":-1.2021, "vx":0.63003, "vy":-0.84714, "omega":0.0, "ax":3.43799, "ay":5.30256, "alpha":2.7899, "fx":[54.55485,36.86884,60.26308,73.13148], "fy":[89.11015,97.74005,85.29271,74.6043]}, + {"t":1.66681, "x":1.23701, "y":2.16679, "heading":-1.2021, "vx":0.75191, "vy":-0.65915, "omega":0.09891, "ax":3.42942, "ay":5.30791, "alpha":2.78546, "fx":[54.43789,36.74628,60.07954,72.99428], "fy":[89.17546,97.77935,85.41231,74.73006]}, + {"t":1.70226, "x":1.26583, "y":2.14676, "heading":-1.19859, "vx":0.8735, "vy":-0.47096, "omega":0.19766, "ax":3.41985, "ay":5.31385, "alpha":2.7807, "fx":[54.25225,36.60581,59.95307,72.82123], "fy":[89.28159,97.82426,85.49022,74.88932]}, + {"t":1.73772, "x":1.29894, "y":2.1334, "heading":-1.19159, "vx":0.99474, "vy":-0.28257, "omega":0.29625, "ax":3.40909, "ay":5.32049, "alpha":2.77554, "fx":[53.99536,36.44509,59.87888,72.60894], "fy":[89.42938,97.87541,85.52997,75.08473]}, + {"t":1.77317, "x":1.33635, "y":2.12673, "heading":-1.18108, "vx":1.11561, "vy":-0.09394, "omega":0.39465, "ax":3.39686, "ay":5.32798, "alpha":2.76989, "fx":[53.66394,36.26166,59.85059,72.35277], "fy":[89.61977,97.93337,85.53593,75.3199]}, + {"t":1.80862, "x":1.37804, "y":2.12675, "heading":-1.16709, "vx":1.23603, "vy":0.09495, "omega":0.49285, "ax":3.38285, "ay":5.33649, "alpha":2.76365, "fx":[53.25375,36.05263,59.85978,72.04652], "fy":[89.85396,97.99878,85.51372,75.59965]}, + {"t":1.84408, "x":1.42399, "y":2.13347, "heading":-1.14962, "vx":1.35597, "vy":0.28414, "omega":0.59083, "ax":3.36661, "ay":5.34629, "alpha":2.75668, "fx":[52.75917,35.81425,59.89538,71.68198], "fy":[90.13351,98.07247,85.4706,75.93022]}, + {"t":1.87953, "x":1.47418, "y":2.1469, "heading":-1.12867, "vx":1.47532, "vy":0.47369, "omega":0.68856, "ax":3.34756, "ay":5.35769, "alpha":2.74878, "fx":[52.17268,35.54123,59.94261,71.24828], "fy":[90.46052,98.15563,85.4163,76.31978]}, + {"t":1.91498, "x":1.52858, "y":2.16706, "heading":-1.10426, "vx":1.59401, "vy":0.66363, "omega":0.78602, "ax":3.32488, "ay":5.37113, "alpha":2.73961, "fx":[51.48393,35.22563,59.98154,70.73078], "fy":[90.83802,98.25011,85.36397,76.77905]}, + {"t":1.95044, "x":1.58719, "y":2.19397, "heading":-1.07639, "vx":1.71188, "vy":0.85406, "omega":0.88314, "ax":3.29744, "ay":5.38723, "alpha":2.72864, "fx":[50.67828,34.85515,59.98463,70.10933], "fy":[91.27037,98.35889,85.33184,77.32238]}, + {"t":1.98589, "x":1.64995, "y":2.22763, "heading":-1.04508, "vx":1.82879, "vy":1.04505, "omega":0.97988, "ax":3.26357, "ay":5.40684, "alpha":2.715, "fx":[49.73425,34.41014,59.91271,69.35522], "fy":[91.76421,98.48681,85.3458,77.96965]}, + {"t":2.02134, "x":1.71684, "y":2.26808, "heading":-1.01034, "vx":1.94449, "vy":1.23674, "omega":1.07614, "ax":3.22073, "ay":5.43129, "alpha":2.6973, "fx":[48.61908,33.85834,59.70785,68.42575], "fy":[92.32981,98.64181,85.44363,78.74945]}, + {"t":2.0568, "x":1.7878, "y":2.31534, "heading":-0.97219, "vx":2.05868, "vy":1.4293, "omega":1.17177, "ax":3.16487, "ay":5.46254, "alpha":2.67324, "fx":[47.28002,33.14497,59.27983,67.2537], "fy":[92.98378,98.83715,85.68262,79.70513]}, + {"t":2.09225, "x":1.86278, "y":2.36944, "heading":-0.93065, "vx":2.17088, "vy":1.62296, "omega":1.26654, "ax":3.08909, "ay":5.50385, "alpha":2.63889, "fx":[45.6266,32.17267,58.47836,65.72536], "fy":[93.75407,99.09544,86.15382,80.90667]}, + {"t":2.1277, "x":1.94168, "y":2.43044, "heading":-0.88575, "vx":2.2804, "vy":1.81809, "omega":1.3601, "ax":2.98056, "ay":5.56081, "alpha":2.58713, "fx":[43.48995,30.75668,57.02978,63.62977], "fy":[94.69065,99.45635,87.01117,82.47629]}, + {"t":2.16316, "x":2.0244, "y":2.49839, "heading":-0.83753, "vx":2.38607, "vy":2.01524, "omega":1.45182, "ax":2.81259, "ay":5.64387, "alpha":2.50358, "fx":[40.51639,28.51146,54.37171,60.52273], "fy":[95.88933,99.99166,88.53566,84.64961]}, + {"t":2.19861, "x":2.11076, "y":2.57339, "heading":-0.78605, "vx":2.48578, "vy":2.21533, "omega":1.54058, "ax":2.51936, "ay":5.77447, "alpha":2.3542, "fx":[35.82773,24.50649,49.13224,55.28054], "fy":[97.54991,100.83037,91.28519,87.94067]}, + {"t":2.23406, "x":2.20047, "y":2.65556, "heading":-0.73144, "vx":2.5751, "vy":2.42005, "omega":1.62404, "ax":1.88837, "ay":5.99736, "alpha":2.03386, "fx":[26.54367,15.89424,36.9684,44.07839], "fy":[100.09044,102.11732,96.35873,93.61511]}, + {"t":2.26951, "x":2.29296, "y":2.74512, "heading":-0.67386, "vx":2.64205, "vy":2.63268, "omega":1.69615, "ax":-0.17308, "ay":6.24145, "alpha":1.00936, "fx":[-3.36033,-10.56421,-2.21072,4.81687], "fy":[102.33259,101.66671,102.01018,102.13357]}, + {"t":2.30497, "x":2.38652, "y":2.84238, "heading":-0.61372, "vx":2.63591, "vy":2.85395, "omega":1.73193, "ax":-6.00064, "ay":1.38712, "alpha":-0.72273, "fx":[-99.11695,-97.45771,-97.03907,-98.78214], "fy":[17.72102,24.82487,27.42119,20.74001]}, + {"t":2.34042, "x":2.4762, "y":2.94444, "heading":-0.55232, "vx":2.42317, "vy":2.90313, "omega":1.70631, "ax":-5.27161, "ay":-3.34854, "alpha":-2.40575, "fx":[-77.08044,-82.59443,-95.33171,-89.71663], "fy":[-68.41685,-60.90298,-38.66561,-50.98349]}, + {"t":2.37587, "x":2.55879, "y":3.04526, "heading":-0.49183, "vx":2.23628, "vy":2.78442, "omega":1.62102, "ax":-4.63044, "ay":-4.2339, "alpha":-2.68916, "fx":[-64.04996,-68.69181,-88.60677,-81.4467], "fy":[-81.58908,-77.33258,-53.6178,-64.32534]}, + {"t":2.41133, "x":2.63517, "y":3.14131, "heading":-0.43436, "vx":2.07211, "vy":2.63431, "omega":1.52568, "ax":-4.33278, "ay":-4.55551, "alpha":-2.78703, "fx":[-58.03426,-62.76436,-85.29736,-77.23451], "fy":[-86.28729,-82.64571,-59.27309,-69.68964]}, + {"t":2.44678, "x":2.70591, "y":3.23184, "heading":-0.38027, "vx":1.9185, "vy":2.4728, "omega":1.42687, "ax":-4.16548, "ay":-4.71844, "alpha":-2.83171, "fx":[-54.47062,-59.8882,-83.46713,-74.5647], "fy":[-88.74181,-84.978,-62.10438,-72.72595]}, + {"t":2.48223, "x":2.7713, "y":3.31655, "heading":-0.32968, "vx":1.77083, "vy":2.30552, "omega":1.32648, "ax":-4.05904, "ay":-4.81635, "alpha":-2.85449, "fx":[-52.04268,-58.41562,-82.33969,-72.63258], "fy":[-90.28505,-86.13519,-63.76377,-74.76852]}, + {"t":2.51769, "x":2.83153, "y":3.39526, "heading":-0.28265, "vx":1.62692, "vy":2.13477, "omega":1.22528, "ax":-3.98562, "ay":-4.88154, "alpha":-2.86651, "fx":[-50.24636,-57.68062,-81.5834,-71.11857], "fy":[-91.3625,-86.72291,-64.8446,-76.28526]}, + {"t":2.55314, "x":2.88671, "y":3.46787, "heading":-0.23921, "vx":1.48562, "vy":1.9617, "omega":1.12365, "ax":-3.93202, "ay":-4.92799, "alpha":-2.87286, "fx":[-48.84765,-57.36399,-81.03929,-69.87325], "fy":[-92.16484,-87.00069,-65.60656,-77.48102]}, + {"t":2.58859, "x":2.93691, "y":3.53432, "heading":-0.19938, "vx":1.34621, "vy":1.78699, "omega":1.0218, "ax":-3.89124, "ay":-4.96273, "alpha":-2.8762, "fx":[-47.72235,-57.29262,-80.62425,-68.8184], "fy":[-92.78779,-87.09942,-66.17838,-78.45935]}, + {"t":2.62405, "x":2.98219, "y":3.59456, "heading":-0.16315, "vx":1.20826, "vy":1.61104, "omega":0.91983, "ax":-3.85921, "ay":-4.98966, "alpha":-2.87802, "fx":[-46.79757,-57.36376,-80.29212,-67.90955], "fy":[-93.28513,-87.09335,-66.62941,-79.27817]}, + {"t":2.6595, "x":3.0226, "y":3.64854, "heading":-0.13054, "vx":1.07144, "vy":1.43415, "omega":0.81779, "ax":-3.8334, "ay":-5.01113, "alpha":-2.87918, "fx":[-46.02707,-57.51218,-80.01615,-67.1198], "fy":[-93.68978,-87.02841,-66.99916,-79.97246]}, + {"t":2.69495, "x":3.05818, "y":3.69624, "heading":-0.10155, "vx":0.93553, "vy":1.25649, "omega":0.71572, "ax":-3.81216, "ay":-5.02863, "alpha":-2.88015, "fx":[-45.37963,-57.6944,-79.7804,-66.43206], "fy":[-94.02324,-86.93504,-67.31109,-80.56464]}, + {"t":2.7304, "x":3.08895, "y":3.73762, "heading":-0.07617, "vx":0.80038, "vy":1.0782, "omega":0.61361, "ax":-3.79438, "ay":-5.04316, "alpha":-2.88118, "fx":[-44.83309,-57.88033,-79.57516,-65.83502], "fy":[-94.30029,-86.83436,-67.57955,-81.06982]}, + {"t":2.76586, "x":3.11494, "y":3.77268, "heading":-0.05442, "vx":0.66586, "vy":0.89941, "omega":0.51146, "ax":-3.77926, "ay":-5.05541, "alpha":-2.88236, "fx":[-44.37109,-58.04855,-79.39443,-65.32093], "fy":[-94.53151,-86.74164,-67.81356,-81.49856]}, + {"t":2.80131, "x":3.13617, "y":3.80139, "heading":-0.03628, "vx":0.53187, "vy":0.72018, "omega":0.40927, "ax":-3.76624, "ay":-5.06589, "alpha":-2.88371, "fx":[-43.98124,-58.1835,-79.23443,-64.88431], "fy":[-94.72472,-86.66816,-68.01893,-81.85851]}, + {"t":2.83676, "x":3.15266, "y":3.82374, "heading":-0.02177, "vx":0.39834, "vy":0.54058, "omega":0.30704, "ax":-3.75489, "ay":-5.07495, "alpha":-2.88519, "fx":[-43.65397,-58.27364,-79.09268,-64.52116], "fy":[-94.88578,-86.6224,-68.19955,-82.15534]}, + {"t":2.87222, "x":3.16442, "y":3.83971, "heading":-0.01089, "vx":0.26522, "vy":0.36066, "omega":0.20475, "ax":-3.7449, "ay":-5.08288, "alpha":-2.88677, "fx":[-43.38189,-58.3103,-78.96744,-64.2285], "fy":[-95.01914,-86.61078,-68.35824,-82.39332]}, + {"t":2.90767, "x":3.17147, "y":3.8493, "heading":-0.00363, "vx":0.13245, "vy":0.18045, "omega":0.1024, "ax":-3.73602, "ay":-5.08988, "alpha":-2.88841, "fx":[-43.15928,-58.28687,-78.85734,-64.00399], "fy":[-95.12818,-86.63812,-68.49717,-82.57574]}, + {"t":2.94312, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 7949a764..3c51cefb 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -452,6 +452,9 @@ public Command LOtoA() { // 2910 // lo a b4 b2 dealgae steps.put("LOtoA", routine.trajectory("LOtoA")); steps.put("AtoB", routine.trajectory("AtoB")); + steps.put("BtoB", routine.trajectory("BtoB")); + + if (Robot.isSimulation()) manipulator.setSecondBeambreak(true); // gah routine // run first path .active() @@ -463,12 +466,41 @@ public Command LOtoA() { // 2910 .onTrue( Commands.sequence( scoreCoralInAuto(() -> steps.get("LOtoA").getFinalPose().get()), - Commands.runOnce(() -> autoGroundCoralIntake = true), - swerve - .driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0)) + AutoAim.translateToPose(swerve, () -> steps.get("AtoB").getInitialPose().get()) .until( - () -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)), + () -> + elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + && AutoAim.isInTolerance( + swerve.getPose(), steps.get("AtoB").getInitialPose().get())), + Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)), steps.get("AtoB").cmd())); + routine + .observe( + steps + .get("AtoB") + .active() + .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak())) + .onTrue( + Commands.runOnce( + () -> { + autoGroundCoralIntake = false; + Robot.setCurrentTarget(ReefTarget.L4); + })); + + routine + .observe( + steps.get("AtoB").atTime(steps.get("AtoB").getRawTrajectory().getTotalTime() - 0.3)) + .onTrue( + Commands.sequence( + scoreCoralInAuto(() -> steps.get("AtoB").getFinalPose().get()), + AutoAim.translateToPose(swerve, () -> steps.get("BtoB").getInitialPose().get()) + .until( + () -> + elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + && AutoAim.isInTolerance( + swerve.getPose(), steps.get("BtoB").getInitialPose().get())), + Commands.runOnce(() -> autoGroundCoralIntake = true), + steps.get("BtoB").cmd())); // routine // .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) @@ -603,6 +635,13 @@ public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMete .whileFalse(Commands.run(() -> autoPreScore = false)); } + public void bindGroundCoralElevatorExtension( + AutoRoutine routine, double toleranceMeters, Trigger trigger) { + routine.observe(trigger).debounce(3).onTrue(Commands.run(() -> autoGroundCoralIntake = true)); + } + + // ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ + public void bindAlgaeElevatorExtension(AutoRoutine routine, double toleranceMeters) { routine .observe( From 3b60380b7e17b2159314cb33fef80713bb492aa4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 16 Apr 2025 15:06:25 -0700 Subject: [PATCH 048/154] swap beambreaks --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5467006d..5e774982 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -327,8 +327,8 @@ public static enum AlgaeScoreTarget { new SimpleMotorFeedforward(0.0, 0.7), new ProfiledPIDController( 0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(15, 1))), - new BeambreakIOReal(1, true), - new BeambreakIOReal(0, true)); + new BeambreakIOReal(0, true), + new BeambreakIOReal(1, true)); private final ShoulderSubsystem shoulder = new ShoulderSubsystem( From 04ad2df6529926d52b4b93df7afc465f4368bc0f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 05:52:49 -0700 Subject: [PATCH 049/154] increase auto score debounce time --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 3c51cefb..35725a4f 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -561,7 +561,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) - .debounce(0.06)), + .debounce(0.06 * 2)), Commands.print("Scoring!"), Commands.runOnce( () -> { From f9aea1c8ec9857641879885639f84f8a048b2311 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 07:20:08 -0700 Subject: [PATCH 050/154] adjust jog pose for l2/3 extension --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 473a824b..72641f0e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -475,7 +475,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.15 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -494,7 +494,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.15 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From e3aca34d4f68329c114ead8c43424607c096f6c1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 07:24:31 -0700 Subject: [PATCH 051/154] adjust jog pose more --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 72641f0e..f3ddf301 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -475,7 +475,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.15 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -494,7 +494,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.15 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From 46b84c7f8227bb22d5fd97529ecba1e6623d4d19 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 07:29:18 -0700 Subject: [PATCH 052/154] adjust coral pose zeroing after ground logic --- src/main/java/frc/robot/subsystems/Superstructure.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f3ddf301..4095074a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -337,8 +337,13 @@ private void configureStateTransitionCommands() { .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) - .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.792))) - .onTrue(this.forceState(SuperState.READY_CORAL)); + .onTrue( + Commands.parallel( + manipulator.setVoltage(0.0), + Commands.waitSeconds(0.12) + .andThen( + Commands.runOnce(() -> manipulator.resetPosition(0.792)), + this.forceState(SuperState.READY_CORAL)))); stateTriggers .get(SuperState.INTAKE_CORAL_GROUND) From 363831ba4902626e0ce04917d576e375eb890245 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 07:33:51 -0700 Subject: [PATCH 053/154] adjust l1 jog pose --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4095074a..0860b2e4 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -446,7 +446,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())); + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())); stateTriggers .get(SuperState.PRE_L1) From 4759d8c729275109e99ee252a2e4f2faa0b44fa6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 09:10:33 -0700 Subject: [PATCH 054/154] adjust l4 wrist --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index c9f6d3ce..cfebf6c7 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -50,7 +50,7 @@ public class ExtensionKinematics { Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(115.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); From fe4b131a097e698f2b3ce938642638bb6b919fe4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 10:58:35 -0700 Subject: [PATCH 055/154] =?UTF-8?q?add=20alert=20for=20vision=20data=20com?= =?UTF-8?q?ing=20from=20=E2=9C=A8the=20future=E2=9C=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 32500415..879f7e96 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -93,6 +94,7 @@ public class SwerveSubsystem extends SubsystemBase { private Alert usingSyncOdometryAlert = new Alert("Using Sync Odometry", AlertType.kInfo); private Alert missingModuleData = new Alert("Missing Module Data", AlertType.kError); private Alert missingGyroData = new Alert("Missing Gyro Data", AlertType.kWarning); + private Alert futureVisionData = new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); private boolean useModuleForceFF = !Robot.isSimulation(); @@ -331,6 +333,7 @@ private void updateOdometry() { private void updateVision() { var i = 0; hasFrontTags = false; + var hasFutureData = false; for (var camera : cameras) { if ((camera.getName().equals("Front_Left_Camera") || camera.getName().equals("Front_Right_Camera")) @@ -411,6 +414,7 @@ private void updateVision() { // the sussifier }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; + hasFutureData |= camera.inputs.captureTimestampMicros > RobotController.getTime(); // if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); cameraPoses[i] = visionPose; @@ -448,6 +452,7 @@ private void updateVision() { } if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses on Robot", arr); + futureVisionData.set(hasFutureData); } /** From 271137dba83c048e852c80bf609294cbbf9636de Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 14:02:26 -0700 Subject: [PATCH 056/154] log statuscodes --- .../java/frc/robot/subsystems/climber/ClimberIOReal.java | 6 ++++-- .../java/frc/robot/subsystems/elevator/ElevatorIOReal.java | 4 +++- src/main/java/frc/robot/subsystems/roller/RollerIOReal.java | 4 +++- .../java/frc/robot/subsystems/shoulder/ShoulderIOReal.java | 6 ++++-- src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java | 6 ++++-- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 5 +++-- src/main/java/frc/robot/subsystems/wrist/WristIOReal.java | 6 ++++-- 7 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index eb1ef49f..3bc55f66 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.climber; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -60,8 +62,8 @@ public ClimberIOReal() { @Override public void updateInputs(ClimberIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, position, appliedVoltage); + Logger.recordOutput("Climber refreshall statuscode", BaseStatusSignal.refreshAll( + angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, position, appliedVoltage)); inputs.position = position.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index accda5f0..058b3ea3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.elevator; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -107,7 +109,7 @@ public ElevatorIOReal() { @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp); + Logger.recordOutput("Elevator refreshall statuscode", BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp)); inputs.positionMeters = position.getValueAsDouble(); inputs.velocityMetersPerSec = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java index d9f289f3..fb132dd8 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java @@ -31,6 +31,8 @@ import java.util.Optional; import java.util.function.Consumer; +import org.littletonrobotics.junction.Logger; + public class RollerIOReal implements RollerIO { private final TalonFX motor; @@ -84,7 +86,7 @@ public static TalonFXConfiguration getDefaultConfig() { @Override public void updateInputs(RollerIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp, position); + Logger.recordOutput("Roller" + motor.getDeviceID() + "refreshall statuscode", BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp, position)); inputs.velocityRotationsPerSec = velocity.getValue().in(RotationsPerSecond); inputs.appliedVolts = voltage.getValue().in(Volts); inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index a73261d5..10b37859 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shoulder; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -102,14 +104,14 @@ public ShoulderIOReal() { @Override public void updateInputs(ShoulderIOInputs inputs) { - BaseStatusSignal.refreshAll( + Logger.recordOutput("shoulder refreshall statuscode", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, motorPositionRotations, cancoderPositionRotations, - appliedVoltage); + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.cancoderPosition = cancoderPositionRotations.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 2d3cd0cb..c87702d4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -29,6 +29,8 @@ import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalType; import java.util.Optional; +import org.littletonrobotics.junction.Logger; + /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and * CANcoder @@ -139,7 +141,7 @@ public ModuleIOReal(ModuleConstants moduleConstants, SwerveConstants swerveConst @Override public void updateInputs(final ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( + Logger.recordOutput("module" + constants.prefix() + "refreshall statuscode", BaseStatusSignal.refreshAll( drivePosition, driveVelocity, driveAppliedVolts, @@ -152,7 +154,7 @@ public void updateInputs(final ModuleIOInputs inputs) { turnAppliedVolts, turnStatorCurrent, turnSupplyCurrent, - turnTempC); + turnTempC)); inputs.prefix = constants.prefix(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 879f7e96..37a6afcc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -30,8 +30,8 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -94,7 +94,8 @@ public class SwerveSubsystem extends SubsystemBase { private Alert usingSyncOdometryAlert = new Alert("Using Sync Odometry", AlertType.kInfo); private Alert missingModuleData = new Alert("Missing Module Data", AlertType.kError); private Alert missingGyroData = new Alert("Missing Gyro Data", AlertType.kWarning); - private Alert futureVisionData = new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); + private Alert futureVisionData = + new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); private boolean useModuleForceFF = !Robot.isSimulation(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 521dd93e..8d500023 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.wrist; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -62,13 +64,13 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { @Override public void updateInputs(ArmIOInputs inputs) { - BaseStatusSignal.refreshAll( + Logger.recordOutput("wrist refreshall statuscode", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, motorPositionRotations, - appliedVoltage); + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); From 470e01028dba7065c51efaf223eb52d2ebe8530f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 14:03:11 -0700 Subject: [PATCH 057/154] Update at 'Thu Apr 17 14:03:11 PDT 2025' --- .../subsystems/climber/ClimberIOReal.java | 14 ++++++--- .../subsystems/elevator/ElevatorIOReal.java | 8 +++-- .../robot/subsystems/roller/RollerIOReal.java | 6 ++-- .../subsystems/shoulder/ShoulderIOReal.java | 21 +++++++------ .../robot/subsystems/swerve/ModuleIOReal.java | 31 ++++++++++--------- .../robot/subsystems/wrist/WristIOReal.java | 19 ++++++------ 6 files changed, 56 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index 3bc55f66..691a94de 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.climber; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -16,6 +14,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class ClimberIOReal implements ClimberIO { private final TalonFX motor = new TalonFX(18, "*"); @@ -62,8 +61,15 @@ public ClimberIOReal() { @Override public void updateInputs(ClimberIOInputsAutoLogged inputs) { - Logger.recordOutput("Climber refreshall statuscode", BaseStatusSignal.refreshAll( - angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, position, appliedVoltage)); + Logger.recordOutput( + "Climber refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + position, + appliedVoltage)); inputs.position = position.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 058b3ea3..b8a6d8af 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.elevator; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -22,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; /** Elevator IO using TalonFXs. */ public class ElevatorIOReal implements ElevatorIO { @@ -109,7 +108,10 @@ public ElevatorIOReal() { @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - Logger.recordOutput("Elevator refreshall statuscode", BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp)); + Logger.recordOutput( + "Elevator refreshall statuscode", + BaseStatusSignal.refreshAll( + position, velocity, voltage, statorCurrent, supplyCurrent, temp)); inputs.positionMeters = position.getValueAsDouble(); inputs.velocityMetersPerSec = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java index fb132dd8..0f917ba7 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java @@ -30,7 +30,6 @@ import edu.wpi.first.units.measure.Voltage; import java.util.Optional; import java.util.function.Consumer; - import org.littletonrobotics.junction.Logger; public class RollerIOReal implements RollerIO { @@ -86,7 +85,10 @@ public static TalonFXConfiguration getDefaultConfig() { @Override public void updateInputs(RollerIOInputsAutoLogged inputs) { - Logger.recordOutput("Roller" + motor.getDeviceID() + "refreshall statuscode", BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp, position)); + Logger.recordOutput( + "Roller" + motor.getDeviceID() + "refreshall statuscode", + BaseStatusSignal.refreshAll( + velocity, voltage, statorCurrent, supplyCurrent, temp, position)); inputs.velocityRotationsPerSec = velocity.getValue().in(RotationsPerSecond); inputs.appliedVolts = voltage.getValue().in(Volts); inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 10b37859..54677024 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shoulder; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -22,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class ShoulderIOReal implements ShoulderIO { private final TalonFX motor; @@ -104,14 +103,16 @@ public ShoulderIOReal() { @Override public void updateInputs(ShoulderIOInputs inputs) { - Logger.recordOutput("shoulder refreshall statuscode", BaseStatusSignal.refreshAll( - angularVelocityRPS, - temp, - supplyCurrentAmps, - statorCurrentAmps, - motorPositionRotations, - cancoderPositionRotations, - appliedVoltage)); + Logger.recordOutput( + "shoulder refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + motorPositionRotations, + cancoderPositionRotations, + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.cancoderPosition = cancoderPositionRotations.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index c87702d4..dfbdfe64 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -28,7 +28,6 @@ import frc.robot.subsystems.swerve.PhoenixOdometryThread.Registration; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalType; import java.util.Optional; - import org.littletonrobotics.junction.Logger; /** @@ -141,20 +140,22 @@ public ModuleIOReal(ModuleConstants moduleConstants, SwerveConstants swerveConst @Override public void updateInputs(final ModuleIOInputs inputs) { - Logger.recordOutput("module" + constants.prefix() + "refreshall statuscode", BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - driveSupplyCurrent, - driveTempC, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent, - turnTempC)); + Logger.recordOutput( + "module" + constants.prefix() + "refreshall statuscode", + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + driveSupplyCurrent, + driveTempC, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnStatorCurrent, + turnSupplyCurrent, + turnTempC)); inputs.prefix = constants.prefix(); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 8d500023..639a4131 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.wrist; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -22,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class WristIOReal implements WristIO { private final TalonFX motor; @@ -64,13 +63,15 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { @Override public void updateInputs(ArmIOInputs inputs) { - Logger.recordOutput("wrist refreshall statuscode", BaseStatusSignal.refreshAll( - angularVelocityRPS, - temp, - supplyCurrentAmps, - statorCurrentAmps, - motorPositionRotations, - appliedVoltage)); + Logger.recordOutput( + "wrist refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + motorPositionRotations, + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); From ad3db471ccdaeeefa581d8e5b414f33c177fddaa Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 15:17:01 -0700 Subject: [PATCH 058/154] add anti algae jam --- src/main/java/frc/robot/Robot.java | 1 + .../robot/subsystems/ExtensionPathing.java | 10 +++++ .../frc/robot/subsystems/Superstructure.java | 41 +++++++++++++++---- 3 files changed, 43 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5e774982..a328be8b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -492,6 +492,7 @@ public static enum AlgaeScoreTarget { .debounce(0.5) .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)), driver.a(), + driver.b(), driver.start(), operator.rightBumper(), operator.leftBumper(), diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index b69d7435..fabda17b 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -9,6 +9,7 @@ import com.google.common.graph.MutableGraph; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import frc.robot.subsystems.ExtensionKinematics.ExtensionState; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.shoulder.ShoulderSubsystem; @@ -139,6 +140,15 @@ public class ExtensionPathing { graph.putEdge(l4Tucked, algaeHigh); graph.putEdge(l4Tucked, algaeLow); + + final var algaeHalfTucked = + new ExtensionState( + Units.inchesToMeters(27.6), + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(algaeHalfTucked); + graph.putEdge(algaeHalfTucked, algaeLow); + graph.putEdge(algaeHalfTucked, algaeHigh); } private ExtensionPathing() {} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0860b2e4..bb70fd3a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -48,7 +49,8 @@ public static enum SuperState { PRE_L3, PRE_L4, SCORE_CORAL, - ANTI_JAM, + ANTI_CORAL_JAM, + ANTI_ALGAE_JAM, INTAKE_ALGAE_GROUND, INTAKE_ALGAE_HIGH, INTAKE_ALGAE_LOW, @@ -91,8 +93,11 @@ public static enum SuperState { @AutoLogOutput(key = "Superstructure/Climb Cancel Request") private final Trigger climbCancelReq; - @AutoLogOutput(key = "Superstructure/Anti Jam Request") - private final Trigger antiJamReq; + @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") + private final Trigger antiCoralJamReq; + + @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") + private final Trigger antiAlgaeJamReq; @AutoLogOutput(key = "Superstructure/Home Request") private final Trigger homeReq; @@ -145,7 +150,8 @@ public Superstructure( Trigger climbReq, Trigger climbConfReq, Trigger climbCancelReq, - Trigger antiJamReq, + Trigger antiCoralJamReq, + Trigger antiAlgaeJamReq, Trigger homeReq, Trigger revFunnelReq, Trigger forceFunnelReq, @@ -175,7 +181,8 @@ public Superstructure( this.climbConfReq = climbConfReq; this.climbCancelReq = climbCancelReq; - this.antiJamReq = antiJamReq; + this.antiCoralJamReq = antiCoralJamReq; + this.antiAlgaeJamReq = antiAlgaeJamReq; this.homeReq = homeReq; @@ -627,19 +634,35 @@ private void configureStateTransitionCommands() { ? SuperState.INTAKE_ALGAE_HIGH : SuperState.INTAKE_ALGAE_LOW)); - antiJamReq - .onTrue(this.forceState(SuperState.ANTI_JAM)) + antiCoralJamReq + .onTrue(this.forceState(SuperState.ANTI_CORAL_JAM)) + .onFalse(this.forceState(SuperState.IDLE)); + antiAlgaeJamReq + .onTrue(this.forceState(SuperState.ANTI_ALGAE_JAM)) .onFalse(this.forceState(SuperState.IDLE)); - // ANTI_JAM logic stateTriggers - .get(SuperState.ANTI_JAM) + .get(SuperState.ANTI_CORAL_JAM) .whileTrue(elevator.hold()) .whileTrue(wrist.hold()) .whileTrue(shoulder.hold()) .whileTrue(manipulator.setVelocity(-10)) .whileTrue(funnel.setVoltage(-10.0)); + stateTriggers + .get(SuperState.ANTI_ALGAE_JAM) + .whileTrue( + Commands.parallel( + elevator.hold(), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)) + .until(() -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) + .andThen( + Commands.parallel( + wrist.hold(), + shoulder.hold(), + elevator.setExtension(Units.inchesToMeters(30)).andThen(elevator.hold())))); + stateTriggers .get(SuperState.CHECK_ALGAE) .and(() -> stateTimer.hasElapsed(1.0)) From 673f76ca72b8c9436aa673d0fff2a659a975b8ee Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 17 Apr 2025 15:46:39 -0700 Subject: [PATCH 059/154] increase anti algae jam height --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index bb70fd3a..dcd670da 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -661,7 +661,7 @@ private void configureStateTransitionCommands() { Commands.parallel( wrist.hold(), shoulder.hold(), - elevator.setExtension(Units.inchesToMeters(30)).andThen(elevator.hold())))); + elevator.setExtension(Units.inchesToMeters(40)).andThen(elevator.hold())))); stateTriggers .get(SuperState.CHECK_ALGAE) From 5ca1531b30baaf50f4b1d67782ddc4c71ea97bea Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 06:49:49 -0700 Subject: [PATCH 060/154] log current targets --- src/main/java/frc/robot/Robot.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a328be8b..759f29d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -188,8 +188,11 @@ public static enum AlgaeScoreTarget { PROCESSOR } + @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4; + @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; + @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; From 6776d3f06e105869574b0edbee1759f53664d354 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 07:16:19 -0700 Subject: [PATCH 061/154] fix algae extension pathing positions --- src/main/java/frc/robot/subsystems/ExtensionPathing.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index fabda17b..6fb1c9e0 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -125,7 +125,7 @@ public class ExtensionPathing { new ExtensionState( ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeLow); graph.putEdge(betweenTucked, algaeLow); @@ -133,7 +133,7 @@ public class ExtensionPathing { new ExtensionState( ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeHigh); graph.putEdge(betweenTucked, algaeHigh); graph.putEdge(algaeLow, algaeHigh); From 2c82144ec01da6abd34e4a7b296fd21b43b8cc84 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 08:08:55 -0700 Subject: [PATCH 062/154] add explicit l3 state --- src/main/java/frc/robot/subsystems/ExtensionPathing.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index 6fb1c9e0..d28754ee 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -57,6 +57,10 @@ public class ExtensionPathing { graph.addNode(l3Tucked); graph.putEdge(tucked, l3Tucked); graph.putEdge(l3Tucked, l2Tucked); + final var l3 = + ExtensionKinematics.L3_EXTENSION; + graph.addNode(l3); + graph.putEdge(l3, l3Tucked); final var l4Tucked = new ExtensionState( ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), From bc8add0fb1419a34690e6e3bc36d8b8df11ef7b1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 08:11:51 -0700 Subject: [PATCH 063/154] adjust wrist zeroing --- src/main/java/frc/robot/Robot.java | 9 +++------ src/main/java/frc/robot/subsystems/ExtensionPathing.java | 3 +-- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 8 +++++++- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 759f29d7..1ee8e7fc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -188,12 +188,9 @@ public static enum AlgaeScoreTarget { PROCESSOR } - @AutoLogOutput - private static ReefTarget currentTarget = ReefTarget.L4; - @AutoLogOutput - private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; - @AutoLogOutput - private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4; + @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; + @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; @AutoLogOutput private boolean killVisionIK = true; diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index d28754ee..b445fc03 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -57,8 +57,7 @@ public class ExtensionPathing { graph.addNode(l3Tucked); graph.putEdge(tucked, l3Tucked); graph.putEdge(l3Tucked, l2Tucked); - final var l3 = - ExtensionKinematics.L3_EXTENSION; + final var l3 = ExtensionKinematics.L3_EXTENSION; graph.addNode(l3); graph.putEdge(l3, l3Tucked); final var l4Tucked = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 5d52fc45..aaec10d3 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -144,7 +144,13 @@ public Command currentZero(Supplier shoulderInputs) System.out.println("Wrist Zeroing"); }), this.run(() -> io.setMotorVoltage(-1.0)) - .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 15.0), + .raceWith( + Commands.waitSeconds(0.5) + .andThen( + Commands.waitUntil( + () -> + Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) + > 7.0))), this.runOnce( () -> { hasZeroed = true; From 4d81badebad68093939947ca22489f4eac137fb5 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 08:24:40 -0700 Subject: [PATCH 064/154] reverse wrist zeroing direction --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ee8e7fc..a7e29e55 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -708,7 +708,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - wrist.resetPosition(Rotation2d.fromRadians(3.094)); + wrist.resetPosition(WristSubsystem.AUTO_OFFSET); elevator.resetExtension(0.0); })); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index aaec10d3..64b8d3f1 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,7 +19,9 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); + public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); + // shoulder at auto zero - auto offset + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517).minus(AUTO_OFFSET); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -143,7 +145,7 @@ public Command currentZero(Supplier shoulderInputs) currentFilter.reset(); System.out.println("Wrist Zeroing"); }), - this.run(() -> io.setMotorVoltage(-1.0)) + this.run(() -> io.setMotorVoltage(1.0)) .raceWith( Commands.waitSeconds(0.5) .andThen( From ba071a97c6024e7cfa87a896f493cef7d010fc6b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:11:14 -0700 Subject: [PATCH 065/154] Update at 'Fri Apr 18 09:11:14 PDT 2025' --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 64b8d3f1..29f2c1df 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -152,7 +152,7 @@ public Command currentZero(Supplier shoulderInputs) Commands.waitUntil( () -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) - > 7.0))), + > 7.0 && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), this.runOnce( () -> { hasZeroed = true; From a16bf358e078ecef6b399b0f4bfebb92a1e67bdf Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:16:25 -0700 Subject: [PATCH 066/154] Update at 'Fri Apr 18 09:16:25 PDT 2025' --- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 29f2c1df..cc170f66 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -21,7 +21,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); // shoulder at auto zero - auto offset - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517).minus(AUTO_OFFSET); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -151,8 +151,8 @@ public Command currentZero(Supplier shoulderInputs) .andThen( Commands.waitUntil( () -> - Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) - > 7.0 && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), + Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 7.0 + && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), this.runOnce( () -> { hasZeroed = true; From f4734f3c24b37974b0146e97c2603fe0800394cd Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:20:29 -0700 Subject: [PATCH 067/154] Update at 'Fri Apr 18 09:20:29 PDT 2025' --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index cc170f66..170484df 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -21,7 +21,8 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); // shoulder at auto zero - auto offset - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); + public static final Rotation2d ZEROING_OFFSET = + Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -156,7 +157,7 @@ public Command currentZero(Supplier shoulderInputs) this.runOnce( () -> { hasZeroed = true; - io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); + io.resetEncoder(AUTO_OFFSET.minus(shoulderInputs.get().position.minus(Rotation2d.fromDegrees(40.517)))); })); } From 77b6fb40f208a46affc52d92224d5630d5faf3fa Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:23:53 -0700 Subject: [PATCH 068/154] Revert "Update at 'Fri Apr 18 09:20:29 PDT 2025'" This reverts commit f4734f3c24b37974b0146e97c2603fe0800394cd. --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 170484df..cc170f66 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -21,8 +21,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); // shoulder at auto zero - auto offset - public static final Rotation2d ZEROING_OFFSET = - Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -157,7 +156,7 @@ public Command currentZero(Supplier shoulderInputs) this.runOnce( () -> { hasZeroed = true; - io.resetEncoder(AUTO_OFFSET.minus(shoulderInputs.get().position.minus(Rotation2d.fromDegrees(40.517)))); + io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); })); } From 555f467ca4fc76b874e023e6c5bb59446e93d50e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:23:56 -0700 Subject: [PATCH 069/154] Revert "Update at 'Fri Apr 18 09:16:25 PDT 2025'" This reverts commit a16bf358e078ecef6b399b0f4bfebb92a1e67bdf. --- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index cc170f66..29f2c1df 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -21,7 +21,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); // shoulder at auto zero - auto offset - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517 - AUTO_OFFSET.getDegrees()); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517).minus(AUTO_OFFSET); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -151,8 +151,8 @@ public Command currentZero(Supplier shoulderInputs) .andThen( Commands.waitUntil( () -> - Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 7.0 - && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), + Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) + > 7.0 && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), this.runOnce( () -> { hasZeroed = true; From 506f44e408db7cd8f19ad63cbc9f9aeb2bc26cc2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:23:58 -0700 Subject: [PATCH 070/154] Revert "Update at 'Fri Apr 18 09:11:14 PDT 2025'" This reverts commit ba071a97c6024e7cfa87a896f493cef7d010fc6b. --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 29f2c1df..64b8d3f1 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -152,7 +152,7 @@ public Command currentZero(Supplier shoulderInputs) Commands.waitUntil( () -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) - > 7.0 && MathUtil.isNear(0.0, inputs.angularVelocityRPS, 0.05)))), + > 7.0))), this.runOnce( () -> { hasZeroed = true; From 458cd5c86359fdae43421ac0179b8553dfdc7156 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:25:40 -0700 Subject: [PATCH 071/154] Revert "reverse wrist zeroing direction" This reverts commit 4d81badebad68093939947ca22489f4eac137fb5. --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 6 ++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a7e29e55..1ee8e7fc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -708,7 +708,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - wrist.resetPosition(WristSubsystem.AUTO_OFFSET); + wrist.resetPosition(Rotation2d.fromRadians(3.094)); elevator.resetExtension(0.0); })); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 64b8d3f1..aaec10d3 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,9 +19,7 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d AUTO_OFFSET = Rotation2d.fromRadians(3.094); - // shoulder at auto zero - auto offset - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(40.517).minus(AUTO_OFFSET); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); @@ -145,7 +143,7 @@ public Command currentZero(Supplier shoulderInputs) currentFilter.reset(); System.out.println("Wrist Zeroing"); }), - this.run(() -> io.setMotorVoltage(1.0)) + this.run(() -> io.setMotorVoltage(-1.0)) .raceWith( Commands.waitSeconds(0.5) .andThen( From 038fc460ada7329a5d9ca44cb32372680f599b91 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:36:02 -0700 Subject: [PATCH 072/154] make intake algae high use untucked path --- .../java/frc/robot/subsystems/Superstructure.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index dcd670da..a0a9ede8 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -713,9 +713,17 @@ private void configureStateTransitionCommands() { .get(SuperState.INTAKE_ALGAE_HIGH) .whileTrue( this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) + new ExtensionState( + 0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS)) + .until( + () -> + shoulder.isNearAngle(Rotation2d.fromDegrees(35.0)) + && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)) + .andThen( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS))) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers From 2eb3e486bde1baa0aab15c07be77367450c6cf93 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 09:42:30 -0700 Subject: [PATCH 073/154] ground algae tuning Update at 'Fri Apr 18 09:42:30 PDT 2025' Update at 'Fri Apr 18 09:42:43 PDT 2025' Update at 'Fri Apr 18 09:45:01 PDT 2025' Update at 'Fri Apr 18 09:46:56 PDT 2025' Update at 'Fri Apr 18 09:48:37 PDT 2025' --- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 17e63b96..eb892502 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -51,7 +51,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double L4_EXTENSION_METERS = ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14; + public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index a157f09e..00977b67 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -34,7 +34,9 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(-5.0)); + Rotation2d.fromRadians(0.505) + .plus(Rotation2d.fromDegrees(-5.0)) + .minus(Rotation2d.fromDegrees(2)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = From 382c7957666383c92db3f88e993bffa78fc01d35 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 14:11:15 -0700 Subject: [PATCH 074/154] try reducing barge angle --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ee8e7fc..cd62681c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1008,7 +1008,7 @@ public Robot() { > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) ? Rotation2d.kZero : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(30.0))), + .plus(Rotation2d.fromDegrees(20.0))), Commands.waitUntil( () -> { final var diff = From a3b5827cac44c0786be1c5684d7ad5d29a0d6746 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 15:27:58 -0700 Subject: [PATCH 075/154] slight delay after intaking + revert barge change --- src/main/java/frc/robot/Autos.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 35725a4f..1036df52 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -139,7 +139,7 @@ public void runCoralPath( .onTrue( Commands.sequence( endPos.length() == 3 - ? intakeCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()) + ? intakeCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()).andThen(Commands.waitSeconds(0.1)) : Commands.sequence( endPos.length() == 1 ? scoreCoralInAuto( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd62681c..1ee8e7fc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1008,7 +1008,7 @@ public Robot() { > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) ? Rotation2d.kZero : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(20.0))), + .plus(Rotation2d.fromDegrees(30.0))), Commands.waitUntil( () -> { final var diff = From 75b126710bffe443b81a51a15eda1e1bd2ab85a1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 18 Apr 2025 15:35:44 -0700 Subject: [PATCH 076/154] sike --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ee8e7fc..cd62681c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1008,7 +1008,7 @@ public Robot() { > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) ? Rotation2d.kZero : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(30.0))), + .plus(Rotation2d.fromDegrees(20.0))), Commands.waitUntil( () -> { final var diff = From 0afb156f3e88bab9661b46c2879dce4771dccc63 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 19 Apr 2025 05:03:34 -0700 Subject: [PATCH 077/154] fmt --- src/main/java/frc/robot/Autos.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1036df52..6c265a18 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -139,7 +139,8 @@ public void runCoralPath( .onTrue( Commands.sequence( endPos.length() == 3 - ? intakeCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()).andThen(Commands.waitSeconds(0.1)) + ? intakeCoralInAuto(() -> steps.get(startPos + "to" + endPos).getFinalPose()) + .andThen(Commands.waitSeconds(0.1)) : Commands.sequence( endPos.length() == 1 ? scoreCoralInAuto( From 51a18b68d82917dccbaaf18a7455f0a122cc20b6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 19 Apr 2025 06:28:16 -0700 Subject: [PATCH 078/154] add delay to center auto --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 6c265a18..b6c37505 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -356,7 +356,7 @@ public Command CMtoGH() { // algae path routine // run first path .active() - .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), steps.get("CMtoG").cmd())); + .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), Commands.waitSeconds(2.0), steps.get("CMtoG").cmd())); routine .observe(steps.get("CMtoG").done()) // TODO change to time based From 3346e2bea8f527994092b996d5f0c6f7320ff327 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 19 Apr 2025 06:28:41 -0700 Subject: [PATCH 079/154] Update at 'Sat Apr 19 06:28:41 PDT 2025' --- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index eb892502..2b9e6d34 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -52,7 +52,7 @@ public class ElevatorSubsystem extends SubsystemBase { ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); + public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); From 2bff98cb2186126ee2f335a4a9ca8020ffa3d175 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 19 Apr 2025 06:29:04 -0700 Subject: [PATCH 080/154] adjust stack height --- src/main/java/frc/robot/Autos.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b6c37505..ee800344 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -356,7 +356,11 @@ public Command CMtoGH() { // algae path routine // run first path .active() - .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), Commands.waitSeconds(2.0), steps.get("CMtoG").cmd())); + .whileTrue( + Commands.sequence( + steps.get("CMtoG").resetOdometry(), + Commands.waitSeconds(2.0), + steps.get("CMtoG").cmd())); routine .observe(steps.get("CMtoG").done()) // TODO change to time based From 9362426864675f4c646e65bf86656040c1ed0688 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 19 Apr 2025 07:04:20 -0700 Subject: [PATCH 081/154] wait less in algae auto --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ee800344..1f802ded 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -359,7 +359,7 @@ public Command CMtoGH() { // algae path .whileTrue( Commands.sequence( steps.get("CMtoG").resetOdometry(), - Commands.waitSeconds(2.0), + Commands.waitSeconds(1.5), steps.get("CMtoG").cmd())); routine From 62fe58b5a49be2c7c39b2f5ba83ec19b13eaa759 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 10 Aug 2025 18:53:48 -0700 Subject: [PATCH 082/154] add new camera classes --- .../frc/robot/subsystems/camera/Camera.java | 68 +++++++++++++ .../frc/robot/subsystems/camera/CameraIO.java | 40 ++++++++ .../robot/subsystems/camera/CameraIOReal.java | 68 +++++++++++++ .../robot/subsystems/camera/CameraIOSim.java | 99 +++++++++++++++++++ .../vision/VisionIOInputsLogged.java | 11 +-- 5 files changed, 278 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/camera/Camera.java create mode 100644 src/main/java/frc/robot/subsystems/camera/CameraIO.java create mode 100644 src/main/java/frc/robot/subsystems/camera/CameraIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/camera/CameraIOSim.java diff --git a/src/main/java/frc/robot/subsystems/camera/Camera.java b/src/main/java/frc/robot/subsystems/camera/Camera.java new file mode 100644 index 00000000..4ffa2b6b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/Camera.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import frc.robot.Robot; +import frc.robot.Robot.RobotType; + +/** Add your docs here. */ +public class Camera { + + // TODO add doc comment about how the matrices are actually only used for sim cause i'd forgotten that! + public record CameraConstants( + String name, + Transform3d robotToCamera, + Matrix intrinsicsMatrix, + Matrix distCoeffs) {} + + private final CameraIO io; + private final CameraIOInputsAutoLogged inputs = new CameraIOInputsAutoLogged(); + private final PhotonPoseEstimator estimator = new PhotonPoseEstimator(Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, null); + + public Camera(CameraIO io) { + this.io = io; + estimator.setRobotToCameraTransform(io.getCameraConstants().robotToCamera); + io.updateInputs(inputs); + } + + public void updateInputs() { + io.updateInputs(inputs); + } + + public void processApriltagInputs() { + Logger.processInputs("Apriltag Vision/" + io.getName(), inputs); + } + + public Optional update(PhotonPipelineResult result) { + // Skip if we have no targets (could/should switch to 1?) + if (result.getTargets().size() < 1) { + return Optional.empty(); + } + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Vision/" + io.getName() + " Best Distance", + result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); + Optional estPose = + estimator.update(result); + return estPose; + } + + public void setSimPose(Optional simEst, Camera camera, boolean newResult) { + this.io.setSimPose(simEst, camera, newResult); + } +} diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIO.java b/src/main/java/frc/robot/subsystems/camera/CameraIO.java new file mode 100644 index 00000000..c5f80f29 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraIO.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.subsystems.camera.Camera.CameraConstants; + +import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.targeting.PhotonPipelineResult; + +/** Add your docs here. */ +public interface CameraIO { + @AutoLog + public static class CameraIOInputs { + public String name = ""; + // latency could just be calculated from the timestamp, do we need it as an input or could it be + // an output? + // public double latency = 0.0; + // public PhotonTrackedTarget[] targets = new PhotonTrackedTarget[] {}; + public PhotonPipelineResult result = new PhotonPipelineResult(); + public Transform3d coprocPNPTransform = new Transform3d(); + // public long sequenceID = 0; + // public long captureTimestampMicros = 0; + // public long publishTimestampMicros = 0; + // public long timeSinceLastPong = 0; + public boolean stale = true; + } + + public void updateInputs(CameraIOInputs inputs); + + public void setSimPose(Optional simEst, Camera camera, boolean newResult); + + public String getName(); + + public CameraConstants getCameraConstants(); +} diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java new file mode 100644 index 00000000..937219e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import frc.robot.subsystems.camera.Camera.CameraConstants; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; + +import edu.wpi.first.math.geometry.Transform3d; + +/** Add your docs here. */ +public class CameraIOReal implements CameraIO { + private final CameraConstants constants; + + public PhotonCamera camera; + + public CameraIOReal(CameraConstants constants) { + this.constants = constants; + this.camera = new PhotonCamera(constants.name()); + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + inputs.name = constants.name(); + var results = camera.getAllUnreadResults(); + + if (results.size() > 0) { + inputs.result = results.get(results.size() - 1); + // final var result = results.get(results.size() - 1); + // inputs.latency = result.metadata.getLatencyMillis(); + + // inputs.targets = new PhotonTrackedTarget[result.targets.size()]; + // for (int i = 0; i < result.targets.size(); i++) { + // inputs.targets[i] = result.targets.get(i); + // } + + // inputs.coprocPNPTransform = + // result + // .multitagResult + // .map((pnpResult) -> pnpResult.estimatedPose.best) + // .orElse(Transform3d.kZero); + // inputs.sequenceID = result.metadata.getSequenceID(); + // inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); + // inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); + // inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; + inputs.stale = false; + } else { + // else leave stale data + inputs.stale = true; + } + } + + @Override + public String getName() { + return constants.name(); + } + + @Override + public void setSimPose(Optional simEst, Camera camera, boolean newResult) {} + + @Override + public CameraConstants getCameraConstants() { + return constants; + } +} diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java new file mode 100644 index 00000000..d22a4908 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java @@ -0,0 +1,99 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.Robot; +import frc.robot.subsystems.camera.Camera.CameraConstants; +import java.util.Optional; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** Add your docs here. */ +public class CameraIOSim implements CameraIO { + + private final CameraConstants constants; + private final VisionSystemSim sim; + private final PhotonCamera camera; + private final PhotonCameraSim simCamera; + + public static Supplier pose = () -> Pose3d.kZero; // TODO wtf + + public CameraIOSim(CameraConstants constants) { + this.sim = new VisionSystemSim(constants.name()); + var cameraProp = new SimCameraProperties(); + // TODO Fix these constants + cameraProp.setCalibration(1080, 960, constants.intrinsicsMatrix(), constants.distCoeffs()); + cameraProp.setCalibError(0.0, 0.0); + cameraProp.setFPS(50.0); + cameraProp.setAvgLatencyMs(30.0); + cameraProp.setLatencyStdDevMs(5.0); + this.camera = new PhotonCamera(constants.name()); + this.simCamera = new PhotonCameraSim(camera, cameraProp); + simCamera.enableDrawWireframe(true); + simCamera.setMaxSightRange(7.0); + this.constants = constants; + sim.addCamera(simCamera, constants.robotToCamera()); + + try { + final var field = Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(); + field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + sim.addAprilTags(field); + } catch (Exception e) { + e.printStackTrace(); + } + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + sim.update(pose.get()); + var results = camera.getAllUnreadResults(); + if (results.size() > 0) { + inputs.result = results.get(results.size() - 1); + // final var result = results.get(results.size() - 1); + // inputs.latency = result.metadata.getLatencyMillis(); + + // inputs.targets = new PhotonTrackedTarget[result.targets.size()]; + // for (int i = 0; i < result.targets.size(); i++) { + // inputs.targets[i] = result.targets.get(i); + // } + + // inputs.sequenceID = result.metadata.getSequenceID(); + // inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); + // inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); + // inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; + inputs.stale = false; + } else { + // else leave stale data + inputs.stale = true; + } + } + + @Override + public void setSimPose(Optional simEst, Camera camera, boolean newResult) { + simEst.ifPresentOrElse( + est -> + sim.getDebugField().getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), + () -> { + if (newResult) sim.getDebugField().getObject("VisionEstimation").setPoses(); + }); + } + + @Override + public String getName() { + return constants.name(); + } + + @Override + public CameraConstants getCameraConstants() { + return constants; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java b/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java index 28cc4d75..cd758bf9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java @@ -2,11 +2,8 @@ import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.photonvision.targeting.PhotonTrackedTarget; -/** - * AutoLog does not work with PhotonTrackedTargets as of yet which is very sad but until it does - * here is a manual implementation - */ public class VisionIOInputsLogged extends VisionIO.VisionIOInputs implements LoggableInputs, Cloneable { @Override @@ -14,12 +11,11 @@ public void toLog(LogTable table) { table.put("Latency", latency); for (int i = 0; i < targets.size(); i++) { - VisionHelper.Logging.logPhotonTrackedTarget(targets.get(i), table, String.valueOf(i)); + table.put(String.valueOf(i), targets.get(i)); } table.put("NumTags", targets.size()); table.put("Pose", coprocPNPTransform); table.put("Stale", stale); - VisionHelper.Logging.logVisionConstants(constants, table); table.put("SequenceID", sequenceID); table.put("Capture Timestamp Micros", captureTimestampMicros); table.put("Publish Timestamp Micros", publishTimestampMicros); @@ -30,11 +26,10 @@ public void toLog(LogTable table) { public void fromLog(LogTable table) { latency = table.get("Latency", latency); for (int i = 0; i < table.get("NumTags", numTags); i++) { - this.targets.add(VisionHelper.Logging.getLoggedPhotonTrackedTarget(table, String.valueOf(i))); + this.targets.add(table.get(String.valueOf(i), new PhotonTrackedTarget())); // TODO uhhh } numTags = table.get("NumTags", numTags); coprocPNPTransform = table.get("Pose", coprocPNPTransform); - constants = VisionHelper.Logging.getLoggedVisionConstants(table); sequenceID = table.get("SequenceID", sequenceID); captureTimestampMicros = table.get("Capture Timestamp Micros", captureTimestampMicros); publishTimestampMicros = table.get("Publish Timestamp Micros", publishTimestampMicros); From 14928b3bf749ae28fc13cbb02c7e44a223574c80 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 10 Aug 2025 19:40:54 -0700 Subject: [PATCH 083/154] replace old vision with new cameras!!!!! --- src/main/java/frc/robot/Robot.java | 52 +- .../frc/robot/subsystems/camera/Camera.java | 211 +++++++- .../frc/robot/subsystems/camera/CameraIO.java | 13 +- .../robot/subsystems/camera/CameraIOReal.java | 22 +- .../robot/subsystems/camera/CameraIOSim.java | 15 +- .../swerve/AlphaSwerveConstants.java | 12 +- .../swerve/BansheeSwerveConstants.java | 18 +- .../swerve/KelpieSwerveConstants.java | 26 +- .../subsystems/swerve/SwerveConstants.java | 6 +- .../subsystems/swerve/SwerveSubsystem.java | 302 ++++------- .../frc/robot/subsystems/vision/Vision.java | 88 ---- .../robot/subsystems/vision/VisionHelper.java | 473 ------------------ .../frc/robot/subsystems/vision/VisionIO.java | 49 -- .../vision/VisionIOInputsLogged.java | 52 -- .../robot/subsystems/vision/VisionIOReal.java | 73 --- .../robot/subsystems/vision/VisionIOSim.java | 91 ---- 16 files changed, 354 insertions(+), 1149 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/Vision.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionHelper.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIOSim.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd62681c..cc721032 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -55,6 +55,9 @@ import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.beambreak.BeambreakIOReal; +import frc.robot.subsystems.camera.CameraIO; +import frc.robot.subsystems.camera.CameraIOReal; +import frc.robot.subsystems.camera.CameraIOSim; import frc.robot.subsystems.climber.ClimberIOReal; import frc.robot.subsystems.climber.ClimberIOSim; import frc.robot.subsystems.climber.ClimberSubsystem; @@ -70,9 +73,6 @@ import frc.robot.subsystems.shoulder.ShoulderIOSim; import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; -import frc.robot.subsystems.vision.VisionIOSim; import frc.robot.subsystems.wrist.*; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.Tracer; @@ -252,13 +252,13 @@ public static enum AlgaeScoreTarget { ROBOT_TYPE != RobotType.SIM ? new GyroIOPigeon2(ROBOT_HARDWARE.swerveConstants.getGyroID()) : new GyroIOSim(swerveDriveSimulation.get().getGyroSimulation()), - Stream.of(ROBOT_HARDWARE.swerveConstants.getVisionConstants()) + Stream.of(ROBOT_HARDWARE.swerveConstants.getCameraConstants()) .map( (constants) -> ROBOT_TYPE == RobotType.REAL - ? new VisionIOReal(constants) - : new VisionIOSim(constants)) - .toArray(VisionIO[]::new), + ? new CameraIOReal(constants) + : new CameraIOSim(constants)) + .toArray(CameraIO[]::new), ROBOT_TYPE != RobotType.SIM ? new ModuleIO[] { new ModuleIOReal( @@ -295,8 +295,8 @@ public static enum AlgaeScoreTarget { PhoenixOdometryThread.getInstance(), swerveDriveSimulation, ROBOT_TYPE != RobotType.SIM - ? new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants()) - : new VisionIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); + ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) + : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); private final ElevatorSubsystem elevator = new ElevatorSubsystem( @@ -586,10 +586,10 @@ public Robot() { SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation.orElse(null)); swerve.resetPose(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); // global static is mildly questionable - VisionIOSim.pose = () -> new Pose3d(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); + CameraIOSim.pose = () -> new Pose3d(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); } else { // this should never be called? - VisionIOSim.pose = () -> new Pose3d(); + CameraIOSim.pose = () -> new Pose3d(); } // Add the arms and stuff elevatorRoot.append(carriageLigament); @@ -919,21 +919,21 @@ public Robot() { .and(() -> swerve.hasFrontTags)) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); - driver - .rightBumper() - .or(driver.leftBumper()) - .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) - .whileTrue( - swerve.driveToAlgae( - () -> - modifyJoystick(driver.getLeftY()) - * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getLeftX()) - * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getRightX()) - * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); + // driver + // .rightBumper() + // .or(driver.leftBumper()) + // .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) + // .whileTrue( + // swerve.driveToAlgae( + // () -> + // modifyJoystick(driver.getLeftY()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getLeftX()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getRightX()) + // * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); driver .rightBumper() .or(driver.leftBumper()) diff --git a/src/main/java/frc/robot/subsystems/camera/Camera.java b/src/main/java/frc/robot/subsystems/camera/Camera.java index 4ffa2b6b..e6b9f2e7 100644 --- a/src/main/java/frc/robot/subsystems/camera/Camera.java +++ b/src/main/java/frc/robot/subsystems/camera/Camera.java @@ -4,43 +4,74 @@ package frc.robot.subsystems.camera; -import java.util.Optional; - -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; - import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure.SuperState; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.Tracer; +import java.util.NoSuchElementException; +import java.util.Optional; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; /** Add your docs here. */ public class Camera { - // TODO add doc comment about how the matrices are actually only used for sim cause i'd forgotten that! + // TODO add doc comment about how the matrices are actually only used for sim cause i'd forgotten + // that! public record CameraConstants( String name, Transform3d robotToCamera, Matrix intrinsicsMatrix, Matrix distCoeffs) {} + public static final Matrix visionPointBlankDevs = + new Matrix(Nat.N3(), Nat.N1(), new double[] {0.6, 0.6, 0.5}); + public static final Matrix infiniteDevs = + new Matrix( + Nat.N3(), + Nat.N1(), + new double[] { + Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY + }); + public static final double distanceFactor = 3.0; + private final CameraIO io; private final CameraIOInputsAutoLogged inputs = new CameraIOInputsAutoLogged(); - private final PhotonPoseEstimator estimator = new PhotonPoseEstimator(Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, null); + private final PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + null); + + private Alert futureVisionData; + + private Pose3d pose; public Camera(CameraIO io) { this.io = io; estimator.setRobotToCameraTransform(io.getCameraConstants().robotToCamera); io.updateInputs(inputs); + futureVisionData = + new Alert( + getName() + " Vision Data Coming from ✨The Future✨", + AlertType.kError); // what the hell is this } - public void updateInputs() { + public void updateInputs() { io.updateInputs(inputs); } @@ -48,7 +79,7 @@ public void processApriltagInputs() { Logger.processInputs("Apriltag Vision/" + io.getName(), inputs); } - public Optional update(PhotonPipelineResult result) { + public Optional update(PhotonPipelineResult result) { // Skip if we have no targets (could/should switch to 1?) if (result.getTargets().size() < 1) { return Optional.empty(); @@ -57,12 +88,160 @@ public Optional update(PhotonPipelineResult result) { Logger.recordOutput( "Vision/" + io.getName() + " Best Distance", result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); - Optional estPose = - estimator.update(result); + Optional estPose = estimator.update(result); return estPose; } - public void setSimPose(Optional simEst, Camera camera, boolean newResult) { - this.io.setSimPose(simEst, camera, newResult); + public void setSimPose(Optional simEst, boolean newResult) { + this.io.setSimPose(simEst, newResult); + } + + public String getName() { + return io.getName(); + } + + public static Matrix findVisionMeasurementStdDevs(EstimatedRobotPose estimation) { + double sumDistance = 0; + for (var target : estimation.targetsUsed) { + var t3d = target.getBestCameraToTarget(); + sumDistance += + Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); + } + double avgDistance = sumDistance / estimation.targetsUsed.size(); + + var deviation = visionPointBlankDevs.times(Math.max(avgDistance, 0.0) * distanceFactor); + if (estimation.targetsUsed.size() == 1) { + deviation = deviation.times(3); + } + if (estimation.targetsUsed.size() == 1 && estimation.targetsUsed.get(0).poseAmbiguity > 0.15) { + return infiniteDevs; + } + // Reject if estimated pose is in the air or ground + if (Math.abs(estimation.estimatedPose.getZ()) > 0.125) { + return infiniteDevs; + } + // TAG_COUNT_DEVIATION_PARAMS + // .get( + // MathUtil.clamp( + // estimation.targetsUsed.size() - 1, 0, TAG_COUNT_DEVIATION_PARAMS.size() - 1)) + // .computeDeviation(avgDistance); + return deviation; + } + + public void updateCamera() { + var hasFutureData = false; + try { + if (!inputs.stale) { + var estPose = Tracer.trace("Update Camera", () -> update(inputs.result)); + var visionPose = estPose.get().estimatedPose; + pose = visionPose; + // Sets the pose on the sim field + setSimPose(estPose, !inputs.stale); + + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/Vision Pose From " + getName(), visionPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/Vision Pose2d From " + getName(), visionPose.toPose2d()); + final var deviations = findVisionMeasurementStdDevs(estPose.get()); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData()); + + Tracer.trace( + "Add Measurement From " + getName(), + () -> { + SwerveSubsystem + .addVisionMeasurement( // TODO this makes me uncomfortable but i don't really know + // if it's an issue + visionPose.toPose2d(), + inputs.result.metadata.captureTimestampMicros / 1.0e6, + deviations + // .times(DriverStation.isAutonomous() ? 2.0 : 1.0) + .times( + getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera") + ? 0.75 + : 2.0) + // reef positions + .times( + (getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera")) + && (Robot.state.get().toString().startsWith("PRE_L") + || Robot.state.get() == SuperState.SCORE_CORAL + || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH + || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) + ? 0.5 + : 1.5) // TODO tune these sorts of numbers + // hp tags + .times( + // !camera.getName().equals("Front_Camera") + // && + estPose.get().targetsUsed.stream() + .anyMatch( + t -> + t.getFiducialId() == 12 + || t.getFiducialId() == 13 + || t.getFiducialId() == 2 + || t.getFiducialId() == 1) + ? 1.5 + : 1.0) + // barge tags + .times( + // !camera.getName().equals("Front_Right_Camera") + // && + estPose.get().targetsUsed.stream() + .anyMatch( + t -> + t.getFiducialId() == 4 + || t.getFiducialId() == 5 + || t.getFiducialId() == 15 + || t.getFiducialId() == 14) + ? 1.2 + : 1.0) + .times(Robot.state.get() == SuperState.PRE_NET ? 0.5 : 1.0)); + // the sussifier + }); + + hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime(); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update"); + + Tracer.trace( + "Log Tag Poses", + () -> { + Pose3d[] targetPose3ds = new Pose3d[inputs.result.targets.size()]; + for (int j = 0; j < inputs.result.targets.size(); j++) { + targetPose3ds[j] = + Robot.ROBOT_HARDWARE + .swerveConstants + .getFieldTagLayout() + .getTagPose(inputs.result.targets.get(j).getFiducialId()) + .get(); + } + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds); + }); + + } else { + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale"); + } + } catch (NoSuchElementException e) { + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate"); + } + futureVisionData.set(hasFutureData); + } + + public CameraConstants getCameraConstants() { + return io.getCameraConstants(); + } + + public Pose3d getPose() { + return pose; + } + + public boolean hasFrontTags() { + return getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera") && inputs.result.targets.size() > 0; } } diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIO.java b/src/main/java/frc/robot/subsystems/camera/CameraIO.java index c5f80f29..0976b5df 100644 --- a/src/main/java/frc/robot/subsystems/camera/CameraIO.java +++ b/src/main/java/frc/robot/subsystems/camera/CameraIO.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Transform3d; import frc.robot.subsystems.camera.Camera.CameraConstants; - import java.util.Optional; import org.littletonrobotics.junction.AutoLog; import org.photonvision.EstimatedRobotPose; @@ -16,23 +15,13 @@ public interface CameraIO { @AutoLog public static class CameraIOInputs { - public String name = ""; - // latency could just be calculated from the timestamp, do we need it as an input or could it be - // an output? - // public double latency = 0.0; - // public PhotonTrackedTarget[] targets = new PhotonTrackedTarget[] {}; public PhotonPipelineResult result = new PhotonPipelineResult(); - public Transform3d coprocPNPTransform = new Transform3d(); - // public long sequenceID = 0; - // public long captureTimestampMicros = 0; - // public long publishTimestampMicros = 0; - // public long timeSinceLastPong = 0; public boolean stale = true; } public void updateInputs(CameraIOInputs inputs); - public void setSimPose(Optional simEst, Camera camera, boolean newResult); + public void setSimPose(Optional simEst, boolean newResult); public String getName(); diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java index 937219e1..e8615cbe 100644 --- a/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java @@ -9,8 +9,6 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; -import edu.wpi.first.math.geometry.Transform3d; - /** Add your docs here. */ public class CameraIOReal implements CameraIO { private final CameraConstants constants; @@ -24,28 +22,10 @@ public CameraIOReal(CameraConstants constants) { @Override public void updateInputs(CameraIOInputs inputs) { - inputs.name = constants.name(); var results = camera.getAllUnreadResults(); if (results.size() > 0) { inputs.result = results.get(results.size() - 1); - // final var result = results.get(results.size() - 1); - // inputs.latency = result.metadata.getLatencyMillis(); - - // inputs.targets = new PhotonTrackedTarget[result.targets.size()]; - // for (int i = 0; i < result.targets.size(); i++) { - // inputs.targets[i] = result.targets.get(i); - // } - - // inputs.coprocPNPTransform = - // result - // .multitagResult - // .map((pnpResult) -> pnpResult.estimatedPose.best) - // .orElse(Transform3d.kZero); - // inputs.sequenceID = result.metadata.getSequenceID(); - // inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - // inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - // inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; inputs.stale = false; } else { // else leave stale data @@ -59,7 +39,7 @@ public String getName() { } @Override - public void setSimPose(Optional simEst, Camera camera, boolean newResult) {} + public void setSimPose(Optional simEst, boolean newResult) {} @Override public CameraConstants getCameraConstants() { diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java index d22a4908..ec77f154 100644 --- a/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java @@ -6,7 +6,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; import frc.robot.Robot; import frc.robot.subsystems.camera.Camera.CameraConstants; import java.util.Optional; @@ -58,18 +57,6 @@ public void updateInputs(CameraIOInputs inputs) { var results = camera.getAllUnreadResults(); if (results.size() > 0) { inputs.result = results.get(results.size() - 1); - // final var result = results.get(results.size() - 1); - // inputs.latency = result.metadata.getLatencyMillis(); - - // inputs.targets = new PhotonTrackedTarget[result.targets.size()]; - // for (int i = 0; i < result.targets.size(); i++) { - // inputs.targets[i] = result.targets.get(i); - // } - - // inputs.sequenceID = result.metadata.getSequenceID(); - // inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - // inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - // inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; inputs.stale = false; } else { // else leave stale data @@ -78,7 +65,7 @@ public void updateInputs(CameraIOInputs inputs) { } @Override - public void setSimPose(Optional simEst, Camera camera, boolean newResult) { + public void setSimPose(Optional simEst, boolean newResult) { simEst.ifPresentOrElse( est -> sim.getDebugField().getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), diff --git a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java index 30932277..abfb1356 100644 --- a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java @@ -21,7 +21,7 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; -import frc.robot.subsystems.vision.Vision.VisionConstants; +import frc.robot.subsystems.camera.Camera.CameraConstants; public class AlphaSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -201,7 +201,7 @@ public Mass getMass() { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { // Stolen from banshee for now for testing, fix later once alphabot camera config is known final Matrix CAMERA_MATRIX = MatBuilder.fill( @@ -229,8 +229,8 @@ public VisionConstants[] getVisionConstants() { 0.012202835675939619, 0.0034143496721838872); - return new VisionConstants[] { - new VisionConstants( + return new CameraConstants[] { + new CameraConstants( "Camera", new Transform3d( new Translation3d( @@ -254,8 +254,8 @@ public double getBumperLength() { } @Override - public VisionConstants getAlgaeVisionConstants() { + public CameraConstants getAlgaeCameraConstants() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getAlgaeVisionConstants'"); + throw new UnsupportedOperationException("Unimplemented method 'getAlgaeCameraConstants'"); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java index 1e928567..b66a8a9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java @@ -21,8 +21,8 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; +import frc.robot.subsystems.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.Module.ModuleConstants; -import frc.robot.subsystems.vision.Vision.VisionConstants; public class BansheeSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -98,7 +98,7 @@ public AprilTagFieldLayout getFieldTagLayout() { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { final Matrix LEFT_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), @@ -149,8 +149,8 @@ public VisionConstants[] getVisionConstants() { 0, 0, 0); - final VisionConstants leftCamConstants = - new VisionConstants( + final CameraConstants leftCamConstants = + new CameraConstants( "Left_Camera", new Transform3d( new Translation3d( @@ -163,8 +163,8 @@ public VisionConstants[] getVisionConstants() { Units.degreesToRadians(120))), LEFT_CAMERA_MATRIX, LEFT_DIST_COEFFS); - final VisionConstants rightCamConstants = - new VisionConstants( + final CameraConstants rightCamConstants = + new CameraConstants( "Right_Camera", new Transform3d( new Translation3d( @@ -174,7 +174,7 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-28.125), Units.degreesToRadians(210))), RIGHT_CAMERA_MATRIX, RIGHT_DIST_COEFFS); - return new VisionConstants[] {leftCamConstants, rightCamConstants}; + return new CameraConstants[] {leftCamConstants, rightCamConstants}; } @Override @@ -269,8 +269,8 @@ public double getBumperLength() { } @Override - public VisionConstants getAlgaeVisionConstants() { + public CameraConstants getAlgaeCameraConstants() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getAlgaeVisionConstants'"); + throw new UnsupportedOperationException("Unimplemented method 'getAlgaeCameraConstants'"); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java index e4d032c0..93e0355e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java @@ -21,7 +21,7 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; -import frc.robot.subsystems.vision.Vision.VisionConstants; +import frc.robot.subsystems.camera.Camera.CameraConstants; public class KelpieSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -178,7 +178,7 @@ public TalonFXConfiguration getTurnConfig(int cancoderID) { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { final Matrix BACK_LEFT_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), Nat.N3(), 906.46, 0.0, 675.30, 0.0, 907.49, 394.45, 0.0, 0.0, 1.0); @@ -203,8 +203,8 @@ public VisionConstants[] getVisionConstants() { final Matrix FRONT_LEFT_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.057, -0.09, -0.001, 0.002, 0.043, -0.002, 0.004, -0.002); - final VisionConstants backLeftCamConstants = - new VisionConstants( + final CameraConstants backLeftCamConstants = + new CameraConstants( "Back_Left", new Transform3d( new Translation3d( @@ -217,8 +217,8 @@ public VisionConstants[] getVisionConstants() { Units.degreesToRadians(150))), BACK_LEFT_CAMERA_MATRIX, BACK_LEFT_DIST_COEFFS); - final VisionConstants backRightCamConstants = - new VisionConstants( + final CameraConstants backRightCamConstants = + new CameraConstants( "Back_Right", new Transform3d( new Translation3d( @@ -229,8 +229,8 @@ public VisionConstants[] getVisionConstants() { 0, Units.degreesToRadians(-(90 - 76.875000)), Units.degreesToRadians(210))), BACK_RIGHT_CAMERA_MATRIX, BACK_RIGHT_DIST_COEFFS); - final VisionConstants frontRightCamConstants = - new VisionConstants( + final CameraConstants frontRightCamConstants = + new CameraConstants( "Front_Right_Camera", new Transform3d( new Translation3d( @@ -240,8 +240,8 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(30))), FRONT_RIGHT_CAMERA_MATRIX, FRONT_RIGHT_DIST_COEFFS); - final VisionConstants frontLeftCamConstants = - new VisionConstants( + final CameraConstants frontLeftCamConstants = + new CameraConstants( "Front_Left_Camera", new Transform3d( new Translation3d( @@ -251,18 +251,18 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(-30))), FRONT_LEFT_CAMERA_MATRIX, FRONT_LEFT_DIST_COEFFS); - return new VisionConstants[] {frontRightCamConstants, frontLeftCamConstants}; + return new CameraConstants[] {frontRightCamConstants, frontLeftCamConstants}; } @Override - public VisionConstants getAlgaeVisionConstants() { // TODO calibrate + public CameraConstants getAlgaeCameraConstants() { // TODO calibrate final Matrix ALGAE_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), Nat.N3(), 906.46, 0.0, 675.30, 0.0, 907.49, 394.45, 0.0, 0.0, 1.0); final Matrix ALGAE_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.039, -0.057, -0.005, 0.001, -0.004, -0.001, 0.003, 0.001); - return new VisionConstants( + return new CameraConstants( "Algae_Camera", new Transform3d( new Translation3d( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index bae0cb59..414b0acb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -15,8 +15,8 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.subsystems.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.Module.ModuleConstants; -import frc.robot.subsystems.vision.Vision.VisionConstants; import java.io.File; /** @@ -95,7 +95,7 @@ public Translation2d[] getModuleTranslations() { public abstract AprilTagFieldLayout getFieldTagLayout(); - public abstract VisionConstants[] getVisionConstants(); + public abstract CameraConstants[] getCameraConstants(); public double getWheelRadiusMeters() { return Units.inchesToMeters(2.0); @@ -135,5 +135,5 @@ public double getVisionDistanceFactor() { public abstract double getBumperLength(); - public abstract VisionConstants getAlgaeVisionConstants(); + public abstract CameraConstants getAlgaeCameraConstants(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 37a6afcc..2ad4cc42 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -14,6 +14,7 @@ import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -26,38 +27,33 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.Superstructure.SuperState; +import frc.robot.subsystems.camera.Camera; +import frc.robot.subsystems.camera.CameraIO; import frc.robot.subsystems.swerve.OdometryThreadIO.OdometryThreadIOInputs; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalID; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalType; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionHelper; -import frc.robot.subsystems.vision.VisionIO; import frc.robot.utils.Tracer; import java.util.Arrays; import java.util.List; import java.util.Map; -import java.util.NoSuchElementException; import java.util.Optional; import java.util.function.Consumer; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import org.photonvision.targeting.PhotonPipelineResult; public class SwerveSubsystem extends SubsystemBase { private final SwerveConstants constants; @@ -66,11 +62,11 @@ public class SwerveSubsystem extends SubsystemBase { private final Module[] modules; // FL, FR, BL, BR private final OdometryThreadIO odoThread; private final OdometryThreadIOInputs odoThreadInputs = new OdometryThreadIOInputs(); - private final Vision algaeCamera; + private final Camera algaeCamera; private SwerveDriveKinematics kinematics; - private final Vision[] cameras; + private final Camera[] cameras; /** For delta tracking with PhoenixOdometryThread* */ private SwerveModulePosition[] lastModulePositions = @@ -84,8 +80,7 @@ public class SwerveSubsystem extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); private Rotation2d lastGyroRotation = new Rotation2d(); - private SwerveDrivePoseEstimator estimator; - private double lastEstTimestamp = 0.0; + private static SwerveDrivePoseEstimator estimator; private double lastOdometryUpdateTimestamp = 0.0; final Pose3d[] cameraPoses; @@ -94,8 +89,6 @@ public class SwerveSubsystem extends SubsystemBase { private Alert usingSyncOdometryAlert = new Alert("Using Sync Odometry", AlertType.kInfo); private Alert missingModuleData = new Alert("Missing Module Data", AlertType.kError); private Alert missingGyroData = new Alert("Missing Gyro Data", AlertType.kWarning); - private Alert futureVisionData = - new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); private boolean useModuleForceFF = !Robot.isSimulation(); @@ -104,11 +97,11 @@ public class SwerveSubsystem extends SubsystemBase { public SwerveSubsystem( SwerveConstants constants, GyroIO gyroIO, - VisionIO[] visionIOs, + CameraIO[] CameraIOs, ModuleIO[] moduleIOs, OdometryThreadIO odoThread, Optional simulation, - VisionIO algaeCameraIO) { + CameraIO algaeCameraIO) { this.constants = constants; this.kinematics = new SwerveDriveKinematics(constants.getModuleTranslations()); this.estimator = @@ -122,15 +115,15 @@ public SwerveSubsystem( this.gyroIO = gyroIO; this.odoThread = odoThread; this.simulation = simulation; - this.algaeCamera = new Vision(algaeCameraIO); - cameras = new Vision[visionIOs.length]; + this.algaeCamera = new Camera(algaeCameraIO); + cameras = new Camera[CameraIOs.length]; modules = new Module[moduleIOs.length]; for (int i = 0; i < moduleIOs.length; i++) { modules[i] = new Module(moduleIOs[i]); } - for (int i = 0; i < visionIOs.length; i++) { - cameras[i] = new Vision(visionIOs[i]); + for (int i = 0; i < CameraIOs.length; i++) { + cameras[i] = new Camera(CameraIOs[i]); } cameraPoses = new Pose3d[cameras.length]; @@ -153,7 +146,7 @@ public void periodic() { Tracer.trace("Process cam inputs", camera::processApriltagInputs); } Tracer.trace("Update algae cam inputs", algaeCamera::updateInputs); - Tracer.trace("Process algae cam inputs", algaeCamera::processAlgaeInputs); + // Tracer.trace("Process algae cam inputs", algaeCamera::processAlgaeInputs); Tracer.trace( "Update odo inputs", () -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp)); @@ -194,28 +187,28 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update tag vision", this::updateVision); - Tracer.trace("Update algae vision", this::updateAlgaeVision); + // Tracer.trace("Update algae vision", this::updateAlgaeVision); }); } - private void updateAlgaeVision() { - final PhotonPipelineResult result = - new PhotonPipelineResult( - algaeCamera.inputs.sequenceID, - algaeCamera.inputs.captureTimestampMicros, - algaeCamera.inputs.publishTimestampMicros, - algaeCamera.inputs.timeSinceLastPong, - algaeCamera.inputs.targets); - try { - if (!algaeCamera.inputs.stale) { - Logger.recordOutput("Vision/Algae Result", result); - } else { - Logger.recordOutput("Vision/Algae Camera/Invalid Result", "Stale"); - } - } catch (NullPointerException e) { - Logger.recordOutput("Vision/Algae Camera/Invalid Result", "No Targets"); - } - } + // private void updateAlgaeVision() { + // final PhotonPipelineResult result = + // new PhotonPipelineResult( + // algaeCamera.inputs.sequenceID, + // algaeCamera.inputs.captureTimestampMicros, + // algaeCamera.inputs.publishTimestampMicros, + // algaeCamera.inputs.timeSinceLastPong, + // algaeCamera.inputs.targets); + // try { + // if (!algaeCamera.inputs.stale) { + // Logger.recordOutput("Vision/Algae Result", result); + // } else { + // Logger.recordOutput("Vision/Algae Camera/Invalid Result", "Stale"); + // } + // } catch (NullPointerException e) { + // Logger.recordOutput("Vision/Algae Camera/Invalid Result", "No Targets"); + // } + // } private void updateOdometry() { if (Robot.ROBOT_TYPE != RobotType.REAL) @@ -332,128 +325,28 @@ private void updateOdometry() { } private void updateVision() { - var i = 0; hasFrontTags = false; - var hasFutureData = false; - for (var camera : cameras) { - if ((camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera")) - && camera.inputs.targets.size() > 0) { - hasFrontTags = true; - } - final PhotonPipelineResult result = - new PhotonPipelineResult( - camera.inputs.sequenceID, - camera.inputs.captureTimestampMicros, - camera.inputs.publishTimestampMicros, - camera.inputs.timeSinceLastPong, - camera.inputs.targets); - try { - if (!camera.inputs.stale) { - var estPose = Tracer.trace("Update Camera", () -> camera.update(result)); - var visionPose = estPose.get().estimatedPose; - // Sets the pose on the sim field - camera.setSimPose(estPose, camera, !camera.inputs.stale); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/Vision Pose From " + camera.getName(), visionPose); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/Vision Pose2d From " + camera.getName(), visionPose.toPose2d()); - final var deviations = VisionHelper.findVisionMeasurementStdDevs(estPose.get()); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Deviations", deviations.getData()); - Tracer.trace( - "Add Measurement From " + camera.getName(), - () -> { - estimator.addVisionMeasurement( - visionPose.toPose2d(), - camera.inputs.captureTimestampMicros / 1.0e6, - deviations - // .times(DriverStation.isAutonomous() ? 2.0 : 1.0) - .times( - camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera") - ? 0.75 - : 2.0) - // reef positions - .times( - (camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera")) - && (Robot.state.get().toString().startsWith("PRE_L") - || Robot.state.get() == SuperState.SCORE_CORAL - || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH - || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) - ? 0.5 - : 1.5) // TODO tune these sorts of numbers - // hp tags - .times( - // !camera.getName().equals("Front_Camera") - // && - estPose.get().targetsUsed.stream() - .anyMatch( - t -> - t.getFiducialId() == 12 - || t.getFiducialId() == 13 - || t.getFiducialId() == 2 - || t.getFiducialId() == 1) - ? 1.5 - : 1.0) - // barge tags - .times( - // !camera.getName().equals("Front_Right_Camera") - // && - estPose.get().targetsUsed.stream() - .anyMatch( - t -> - t.getFiducialId() == 4 - || t.getFiducialId() == 5 - || t.getFiducialId() == 15 - || t.getFiducialId() == 14) - ? 1.2 - : 1.0) - .times(Robot.state.get() == SuperState.PRE_NET ? 0.5 : 1.0)); - // the sussifier - }); - lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; - hasFutureData |= camera.inputs.captureTimestampMicros > RobotController.getTime(); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); - cameraPoses[i] = visionPose; - Tracer.trace( - "Log Tag Poses", - () -> { - Pose3d[] targetPose3ds = new Pose3d[result.targets.size()]; - for (int j = 0; j < result.targets.size(); j++) { - targetPose3ds[j] = - Robot.ROBOT_HARDWARE - .swerveConstants - .getFieldTagLayout() - .getTagPose(result.targets.get(j).getFiducialId()) - .get(); - } - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Target Poses", targetPose3ds); - }); - - } else { - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Stale"); - } - } catch (NoSuchElementException e) { - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Bad Estimate"); - } - i++; + for (int i = 0; i < cameras.length; i++) { + if (cameras[i].hasFrontTags()) hasFrontTags = true; + cameras[i].updateCamera(); + cameraPoses[i] = cameras[i].getPose(); } Logger.recordOutput("Vision/Front Cameras Have Tags", hasFrontTags); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses", cameraPoses); Pose3d[] arr = new Pose3d[cameras.length]; for (int k = 0; k < cameras.length; k++) { - arr[k] = getPose3d().transformBy(cameras[k].inputs.constants.robotToCamera()); + arr[k] = getPose3d().transformBy(cameras[k].getCameraConstants().robotToCamera()); } if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses on Robot", arr); - futureVisionData.set(hasFutureData); + } + + public static void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + estimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } /** @@ -788,53 +681,56 @@ public void setCurrentLimits(CurrentLimitsConfigs configs) { } } - @SuppressWarnings("resource") - public Command driveToAlgae(DoubleSupplier xVel, DoubleSupplier yVel, DoubleSupplier theta) { - final PIDController xController = new PIDController(1, 0.0, 0.0); // TODO tune - final PIDController yController = new PIDController(9, 0.0, 0.8); // TODO tune - return this.run( - () -> { - var target = - new PhotonPipelineResult( - algaeCamera.inputs.sequenceID, - algaeCamera.inputs.captureTimestampMicros, - algaeCamera.inputs.publishTimestampMicros, - algaeCamera.inputs.timeSinceLastPong, - algaeCamera.inputs.targets) - .getBestTarget(); - if (target != null) { - double yaw = -target.getYaw(); - double pitch = target.getPitch(); - double r = Units.inchesToMeters(36.990 - 8.125) / Math.tan(Math.toRadians(pitch - 35)); - double yOffset = r * Math.sin(Math.toRadians(yaw)); - double xOffset = r * Math.cos(Math.toRadians(yaw)); - Logger.recordOutput("AutoAim/Algae Detection/X Offset", xOffset); - Logger.recordOutput("AutoAim/Algae Detection/Y Offset", yOffset); - Logger.recordOutput("AutoAim/Algae Detection/R", r); - Logger.recordOutput("AutoAim/Algae Detection/Yaw", yaw); - Logger.recordOutput("AutoAim/Algae Detection/Pitch", pitch); - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - new ChassisSpeeds( - xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), - getRotation()) - .plus( - new ChassisSpeeds( - (1 / (1 + Math.abs(yOffset))) * xController.calculate(xOffset, 0.0), - yController.calculate(yOffset, 0.0), - 0.0)), - false, - new double[4], - new double[4]); - } else { - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), - getRotation()), - false, - new double[4], - new double[4]); - } - }); - } + // it's fine this was cooked anyway + // @SuppressWarnings("resource") + // public Command driveToAlgae(DoubleSupplier xVel, DoubleSupplier yVel, DoubleSupplier theta) { + // final PIDController xController = new PIDController(1, 0.0, 0.0); // TODO tune + // final PIDController yController = new PIDController(9, 0.0, 0.8); // TODO tune + // return this.run( + // () -> { + // var target = + // new PhotonPipelineResult( + // algaeCamera.inputs.sequenceID, + // algaeCamera.inputs.captureTimestampMicros, + // algaeCamera.inputs.publishTimestampMicros, + // algaeCamera.inputs.timeSinceLastPong, + // algaeCamera.inputs.targets) + // .getBestTarget(); + // if (target != null) { + // double yaw = -target.getYaw(); + // double pitch = target.getPitch(); + // double r = Units.inchesToMeters(36.990 - 8.125) / Math.tan(Math.toRadians(pitch - + // 35)); + // double yOffset = r * Math.sin(Math.toRadians(yaw)); + // double xOffset = r * Math.cos(Math.toRadians(yaw)); + // Logger.recordOutput("AutoAim/Algae Detection/X Offset", xOffset); + // Logger.recordOutput("AutoAim/Algae Detection/Y Offset", yOffset); + // Logger.recordOutput("AutoAim/Algae Detection/R", r); + // Logger.recordOutput("AutoAim/Algae Detection/Yaw", yaw); + // Logger.recordOutput("AutoAim/Algae Detection/Pitch", pitch); + // drive( + // ChassisSpeeds.fromFieldRelativeSpeeds( + // new ChassisSpeeds( + // xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), + // getRotation()) + // .plus( + // new ChassisSpeeds( + // (1 / (1 + Math.abs(yOffset))) * xController.calculate(xOffset, 0.0), + // yController.calculate(yOffset, 0.0), + // 0.0)), + // false, + // new double[4], + // new double[4]); + // } else { + // drive( + // ChassisSpeeds.fromFieldRelativeSpeeds( + // new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), + // theta.getAsDouble()), + // getRotation()), + // false, + // new double[4], + // new double[4]); + // } + // }); + // } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index a48e94bb..00000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; -import java.util.Optional; -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; - -/** Add your docs here. */ -public class Vision { - public static final Matrix visionPointBlankDevs = - new Matrix(Nat.N3(), Nat.N1(), new double[] {0.6, 0.6, 0.5}); - public static final Matrix infiniteDevs = - new Matrix( - Nat.N3(), - Nat.N1(), - new double[] { - Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY - }); - public static final double distanceFactor = 3.0; - - public record VisionConstants( - String cameraName, - Transform3d robotToCamera, - Matrix intrinsicsMatrix, - Matrix distCoeffs) {} - - private final VisionIO io; - public final VisionIOInputsLogged inputs = new VisionIOInputsLogged(); - - public Vision(final VisionIO io) { - this.io = io; - io.updateInputs(inputs); - } - - public void setSimPose(Optional simEst, Vision camera, boolean newResult) { - this.io.setSimPose(simEst, camera, newResult); - } - - public void updateInputs() { - io.updateInputs(inputs); - } - - public void processApriltagInputs() { - Logger.processInputs("Apriltag Vision/" + inputs.constants.cameraName, inputs); - } - - public void processAlgaeInputs() { - Logger.processInputs("Algae Vision/" + inputs.constants.cameraName, inputs); - } - - public Optional update(PhotonPipelineResult result) { - // Skip if we have no targets (could/should switch to 1?) - if (result.getTargets().size() < 1) { - return Optional.empty(); - } - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/" + inputs.constants.cameraName + " Best Distance", - result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); - Optional estPose = - VisionHelper.update( - result, - inputs.constants.intrinsicsMatrix(), - inputs.constants.distCoeffs(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - inputs.constants.robotToCamera(), - inputs.coprocPNPTransform); - - return estPose; - } - - public String getName() { - return inputs.constants.cameraName; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java b/src/main/java/frc/robot/subsystems/vision/VisionHelper.java deleted file mode 100644 index e47773b3..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java +++ /dev/null @@ -1,473 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Robot; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.littletonrobotics.junction.LogTable; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -public class VisionHelper { - - /*** - * To be added - */ - public static final List TAG_COUNT_DEVIATION_PARAMS = - List.of( - // 1 tag - new TagCountDeviation( - new UnitDeviationParams(.25, .4, .9), new UnitDeviationParams(.5, .7, 1.5)), - - // 2 tags - new TagCountDeviation( - new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)), - - // 3+ tags - new TagCountDeviation( - new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5))); - - public class Logging { - public static void logPhotonTrackedTarget( - PhotonTrackedTarget target, LogTable table, String name) { - // System.out.println(target); - logTransform3d(target.getBestCameraToTarget(), table, name); - logTransform3d(target.getAlternateCameraToTarget(), table, "Alt " + name); - logCorners(target, table, name, target.getFiducialId()); - - table.put("Targets/Yaw " + name, target.getYaw()); - table.put("Targets/Pitch " + name, target.getPitch()); - table.put("Targets/Area " + name, target.getArea()); - table.put("Targets/Skew " + name, target.getSkew()); - table.put("Targets/Fiducial ID " + name, target.getFiducialId()); - table.put("Targets/Pose Ambiguity " + name, target.getPoseAmbiguity()); - } - - public static void logCorners(PhotonTrackedTarget target, LogTable table, String name, int id) { - double[] detectedCornersX = new double[4]; - double[] detectedCornersY = new double[4]; - double[] minAreaRectCornersX = new double[4]; - double[] minAreaRectCornersY = new double[4]; - for (int i = 0; i < 4; i++) { - if (id > -1) { - detectedCornersX[i] = target.getDetectedCorners().get(i).x; - detectedCornersY[i] = target.getDetectedCorners().get(i).y; - } else { - detectedCornersX[i] = 0; - detectedCornersY[i] = 0; - } - minAreaRectCornersX[i] = target.getMinAreaRectCorners().get(i).x; - minAreaRectCornersY[i] = target.getMinAreaRectCorners().get(i).y; - } - table.put("Targets/Detected Corners X " + name, detectedCornersX); - table.put("Targets/Detected Corners Y " + name, detectedCornersY); - table.put("Targets/Min Area Rect Corners X " + name, minAreaRectCornersX); - table.put("Targets/Min Area Rect Corners Y " + name, minAreaRectCornersY); - } - - public static void logTransform3d(Transform3d transform3d, LogTable table, String name) { - double rotation[] = new double[4]; - rotation[0] = transform3d.getRotation().getQuaternion().getW(); - rotation[1] = transform3d.getRotation().getQuaternion().getX(); - rotation[2] = transform3d.getRotation().getQuaternion().getY(); - rotation[3] = transform3d.getRotation().getQuaternion().getZ(); - table.put("Targets/Rotation " + name, rotation); - - double translation[] = new double[3]; - translation[0] = transform3d.getTranslation().getX(); - translation[1] = transform3d.getTranslation().getY(); - translation[2] = transform3d.getTranslation().getZ(); - table.put("Targets/Translation " + name, translation); - } - - public static void logVisionConstants(VisionConstants constants, LogTable table) { - table.put("Vision Constants/Name ", constants.cameraName()); - table.put("Vision Constants/Transform ", constants.robotToCamera()); - table.put("Vision Constants/Intrinsics ", constants.intrinsicsMatrix().getData()); - table.put("Vision Constants/Distortion ", constants.distCoeffs().getData()); - } - - public static Transform3d getLoggedTransform3d(double[] translation, double[] rotation) { - Transform3d transform3d = - new Transform3d( - new Translation3d(translation[0], translation[1], translation[2]), - new Rotation3d(new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]))); - return transform3d; - } - - public static Transform3d getLoggedTransform3d(LogTable table, String name) { - double[] rotation = table.get("Targets/Rotation " + name, new double[4]); - double[] translation = table.get("Targets/Translation " + name, new double[3]); - return getLoggedTransform3d(translation, rotation); - } - - public static PhotonTrackedTarget getLoggedPhotonTrackedTarget(LogTable table, String name) { - double[] translation = table.get("Targets/Translation " + name, new double[3]); - double[] rotation = table.get("Targets/Rotation " + name, new double[4]); - double[] altTranslation = table.get("Targets/Translation Alt " + name, new double[3]); - double[] altRotation = table.get("Targets/Rotation Alt " + name, new double[4]); - double[] detectedCornersX = table.get("Targets/Detected Corners X " + name, new double[4]); - double[] detectedCornersY = table.get("Targets/Detected Corners Y " + name, new double[4]); - double[] minAreaRectCornersX = - table.get("Targets/Min Area Rect Corners X " + name, new double[4]); - double[] minAreaRectCornersY = - table.get("Targets/Min Area Rect Corners Y " + name, new double[4]); - List detectedCorners = new ArrayList<>(); - List minAreaRectCorners = new ArrayList<>(); - - for (int i = 0; i < 4; i++) { - detectedCorners.add(new TargetCorner(detectedCornersX[i], detectedCornersY[i])); - minAreaRectCorners.add(new TargetCorner(minAreaRectCornersX[i], minAreaRectCornersY[i])); - } - Transform3d pose = getLoggedTransform3d(translation, rotation); - Transform3d altPose = getLoggedTransform3d(altTranslation, altRotation); - return (new PhotonTrackedTarget( - table.get("Targets/Yaw " + name, -1), - table.get("Targets/Pitch " + name, -1), - table.get("Targets/Area " + name, -1), - table.get("Targets/Skew " + name, -1), - (int) (table.get("Targets/Fiducial ID " + name, -1)), - -1, // for obj detection - -1, - pose, - altPose, - table.get("Targets/Pose Ambiguity " + name, -1), - minAreaRectCorners, - detectedCorners)); - } - - public static VisionConstants getLoggedVisionConstants(LogTable table) { - return new VisionConstants( - table.get("Vision Constants Name ", "Default"), - table.get("Vision Constants Transform ", new Transform3d()), - new Matrix( - Nat.N3(), - Nat.N3(), - table.get("Vision Constants Intrinsics ", Matrix.eye(Nat.N3()).getData())), - new Matrix( - Nat.N8(), - Nat.N1(), - table.get( - "Vision Constants Distortion ", - new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}))); - } - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param strat The selected pose solving strategy. - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public static Optional update( - PhotonPipelineResult cameraResult, - Matrix cameraMatrix, - Matrix distCoeffs, - PoseStrategy strat, - Transform3d robotToCamera, - Transform3d bestTF) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult, robotToCamera); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = - multiTagOnRioStrategy( - cameraResult, - Optional.of(cameraMatrix), - Optional.of(distCoeffs), - PoseStrategy.LOWEST_AMBIGUITY, - robotToCamera); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = - multiTagOnCoprocStrategy( - cameraResult, - Optional.of(cameraMatrix), - Optional.of(distCoeffs), - robotToCamera, - PoseStrategy.LOWEST_AMBIGUITY, - bestTF); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - return estimatedPose; - } - - /** - * Runs SolvePNP on the roborio - * - * @param result The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @param multiTagFallbackStrategy Fallback strategy in case the rio fails. Should usually be - * lowest ambiguity - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private static Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrix, - Optional> distCoeffs, // TODO FIX THE 5 VS 8 THING!! - PoseStrategy multiTagFallbackStrategy, - Transform3d robotToCamera) { - boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrix.get(), - distCoeffs.get(), - result.getTargets(), - Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), - TargetModel.kAprilTag36h11); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent()) - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - var best = - new Pose3d() - .plus(pnpResult.get().best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Runs SolvePNP on a coprocessor - * - * @param result The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @param multiTagFallbackStrategy Fallback strategy in case the coproc fails. Should usually be - * the rio or lowest ambiguity - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private static Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrix, - Optional> distCoeffs, - Transform3d robotToCamera, - PoseStrategy multiTagFallbackStrategy, - Transform3d bestTF) { - if (!bestTF.equals(new Transform3d())) { - var best_tf = bestTF; - var best = - new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout().getOrigin()) - .plus(robotToCamera.inverse()); // field-to-robot - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - } - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private static Optional lowestAmbiguityStrategy( - PhotonPipelineResult result, Transform3d robotToCamera) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - - // reject if not a tag on this field - if (target.getFiducialId() < 1 || target.getFiducialId() > 22) continue; - - // reject if too far - if (target.getBestCameraToTarget().getTranslation().getNorm() > 5) continue; - - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = - Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout().getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + targetFiducialId, - false); - return Optional.empty(); - } - - var estimatedRobotPose = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY); - return Optional.of(estimatedRobotPose); - } - - /*** 5026 - to be tuned if necessary*/ - public static record UnitDeviationParams( - double distanceMultiplier, double eulerMultiplier, double minimum) { - private double computeUnitDeviation(double averageDistance) { - return Math.max(minimum, eulerMultiplier * Math.exp(averageDistance * distanceMultiplier)); - } - } - - /*** 5026 - to be tuned if necessary*/ - public static record TagCountDeviation( - UnitDeviationParams xParams, UnitDeviationParams yParams, UnitDeviationParams thetaParams) { - private Matrix computeDeviation(double averageDistance) { - return MatBuilder.fill( - Nat.N3(), - Nat.N1(), - xParams.computeUnitDeviation(averageDistance), - yParams.computeUnitDeviation(averageDistance), - thetaParams.computeUnitDeviation(averageDistance)); - } - - public TagCountDeviation(UnitDeviationParams xyParams, UnitDeviationParams thetaParams) { - this(xyParams, xyParams, thetaParams); - } - } - - public static record VisionMeasurement( - EstimatedRobotPose estimation, Matrix confidence) {} - - public static Matrix findVisionMeasurementStdDevs(EstimatedRobotPose estimation) { - double sumDistance = 0; - for (var target : estimation.targetsUsed) { - var t3d = target.getBestCameraToTarget(); - sumDistance += - Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); - } - double avgDistance = sumDistance / estimation.targetsUsed.size(); - - var deviation = - Vision.visionPointBlankDevs.times(Math.max(avgDistance, 0.0) * Vision.distanceFactor); - if (estimation.targetsUsed.size() == 1) { - deviation = deviation.times(3); - } - if (estimation.targetsUsed.size() == 1 && estimation.targetsUsed.get(0).poseAmbiguity > 0.15) { - return Vision.infiniteDevs; - } - // Reject if estimated pose is in the air or ground - if (Math.abs(estimation.estimatedPose.getZ()) > 0.125) { - return Vision.infiniteDevs; - } - // TAG_COUNT_DEVIATION_PARAMS - // .get( - // MathUtil.clamp( - // estimation.targetsUsed.size() - 1, 0, TAG_COUNT_DEVIATION_PARAMS.size() - 1)) - // .computeDeviation(avgDistance); - return deviation; - } - - /** A Field2d for visualizing our robot and objects on the field. */ - public static Field2d getSimDebugField(VisionSystemSim visionSim) { - if (!Robot.isSimulation()) return null; - return visionSim.getDebugField(); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index a6bd59bc..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Transform3d; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** Add your docs here. */ -public interface VisionIO { - - public static class VisionIOInputs { - // latency could just be calculated from the timestamp, do we need it as an input or could it be - // an output? - public double latency = 0.0; - // We could use protobuf serialization for this instead of custom - // There are som alleged performance considerations for protobuf - // Could be worth testing? This works for now - public List targets = new ArrayList<>(); - public double numTags = 0; // Helps with deserialization - public Transform3d coprocPNPTransform = new Transform3d(); - public VisionConstants constants = - new VisionConstants( - "Default", - new Transform3d(), - Matrix.eye(Nat.N3()), - MatBuilder.fill(Nat.N8(), Nat.N1(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - public long sequenceID = 0; - public long captureTimestampMicros = 0; - public long publishTimestampMicros = 0; - public long timeSinceLastPong = 0; - public boolean stale = true; - } - - public void updateInputs(VisionIOInputs inputs); - - public void setSimPose(Optional simEst, Vision camera, boolean newResult); - - public String getName(); -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java b/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java deleted file mode 100644 index cd758bf9..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.subsystems.vision; - -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; -import org.photonvision.targeting.PhotonTrackedTarget; - -public class VisionIOInputsLogged extends VisionIO.VisionIOInputs - implements LoggableInputs, Cloneable { - @Override - public void toLog(LogTable table) { - table.put("Latency", latency); - - for (int i = 0; i < targets.size(); i++) { - table.put(String.valueOf(i), targets.get(i)); - } - table.put("NumTags", targets.size()); - table.put("Pose", coprocPNPTransform); - table.put("Stale", stale); - table.put("SequenceID", sequenceID); - table.put("Capture Timestamp Micros", captureTimestampMicros); - table.put("Publish Timestamp Micros", publishTimestampMicros); - table.put("Time Since Last Pong", timeSinceLastPong); - } - - @Override - public void fromLog(LogTable table) { - latency = table.get("Latency", latency); - for (int i = 0; i < table.get("NumTags", numTags); i++) { - this.targets.add(table.get(String.valueOf(i), new PhotonTrackedTarget())); // TODO uhhh - } - numTags = table.get("NumTags", numTags); - coprocPNPTransform = table.get("Pose", coprocPNPTransform); - sequenceID = table.get("SequenceID", sequenceID); - captureTimestampMicros = table.get("Capture Timestamp Micros", captureTimestampMicros); - publishTimestampMicros = table.get("Publish Timestamp Micros", publishTimestampMicros); - timeSinceLastPong = table.get("Time Since Last Pong", timeSinceLastPong); - stale = table.get("Stale", stale); - } - - public VisionIOInputsLogged clone() { - VisionIOInputsLogged copy = new VisionIOInputsLogged(); - copy.latency = this.latency; - copy.targets = this.targets; - copy.numTags = this.numTags; - copy.coprocPNPTransform = this.coprocPNPTransform; - copy.sequenceID = this.sequenceID; - copy.captureTimestampMicros = this.captureTimestampMicros; - copy.publishTimestampMicros = this.publishTimestampMicros; - copy.timeSinceLastPong = this.timeSinceLastPong; - return copy; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java deleted file mode 100644 index 418dc178..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; - -/** Add your docs here. */ -public class VisionIOReal implements VisionIO { - // constants - public String cameraName; - public PhotonCamera camera; - public Matrix cameraMatrix; - public Matrix distCoeffs; - private final VisionConstants constants; - - /*** Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - ***/ - public Transform3d robotToCamera; - - public VisionIOReal(VisionConstants constants) { - cameraName = constants.cameraName(); - camera = new PhotonCamera(cameraName); - robotToCamera = constants.robotToCamera(); - cameraMatrix = constants.intrinsicsMatrix(); - distCoeffs = constants.distCoeffs(); - this.constants = constants; - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - var results = camera.getAllUnreadResults(); - if (results.size() > 0) { - final var result = results.get(results.size() - 1); - inputs.latency = result.metadata.getLatencyMillis(); - inputs.targets = result.targets; - inputs.constants = constants; - inputs.coprocPNPTransform = - result - .multitagResult - .map((pnpResult) -> pnpResult.estimatedPose.best) - .orElse(Transform3d.kZero); - inputs.sequenceID = result.metadata.getSequenceID(); - inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; - inputs.stale = false; - } else { - // else leave stale data - inputs.stale = true; - } - } - - @Override - public String getName() { - return cameraName; - } - - @Override - public void setSimPose(Optional simEst, Vision camera, boolean newResult) {} -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java deleted file mode 100644 index a1d2aa37..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; -import edu.wpi.first.math.geometry.Pose3d; -import frc.robot.Robot; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.Optional; -import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; - -/** Add your docs here. */ -public class VisionIOSim implements VisionIO { - private final VisionSystemSim sim; - private final PhotonCamera camera; - private final PhotonCameraSim simCamera; - private final VisionConstants constants; - - public static Supplier pose = () -> Pose3d.kZero; - - public VisionIOSim(VisionConstants constants) { - this.sim = new VisionSystemSim(constants.cameraName()); - var cameraProp = new SimCameraProperties(); - // TODO Fix these constants - cameraProp.setCalibration(1080, 960, constants.intrinsicsMatrix(), constants.distCoeffs()); - cameraProp.setCalibError(0.0, 0.0); - cameraProp.setFPS(50.0); - cameraProp.setAvgLatencyMs(30.0); - cameraProp.setLatencyStdDevMs(5.0); - this.camera = new PhotonCamera(constants.cameraName()); - this.simCamera = new PhotonCameraSim(camera, cameraProp); - simCamera.enableDrawWireframe(true); - simCamera.setMaxSightRange(7.0); - this.constants = constants; - sim.addCamera(simCamera, constants.robotToCamera()); - - try { - final var field = Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(); - field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - sim.addAprilTags(field); - } catch (Exception e) { - e.printStackTrace(); - } - } - - @Override - public void setSimPose(Optional simEst, Vision camera, boolean newResult) { - simEst.ifPresentOrElse( - est -> - VisionHelper.getSimDebugField(sim) - .getObject("VisionEstimation") - .setPose(est.estimatedPose.toPose2d()), - () -> { - if (newResult) - VisionHelper.getSimDebugField(sim).getObject("VisionEstimation").setPoses(); - }); - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - // TODO add sphere for algae sim - sim.update(pose.get()); - var results = camera.getAllUnreadResults(); - inputs.constants = constants; - if (results.size() > 0) { - final var result = results.get(results.size() - 1); - inputs.latency = result.metadata.getLatencyMillis(); - inputs.targets = result.targets; - inputs.sequenceID = result.metadata.getSequenceID(); - inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; - inputs.stale = false; - } else { - // else leave stale data - inputs.stale = true; - } - } - - @Override - public String getName() { - return constants.cameraName(); - } -} From a89ae9b6f4baf46104e3976965d9051e2af9d9c6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 28 Aug 2025 21:02:46 -0700 Subject: [PATCH 084/154] retuned some shoulder and wrist stuff, just need to retune actual scoring positions now --- src/main/java/frc/robot/Robot.java | 132 +++++++++--------- .../robot/subsystems/ExtensionKinematics.java | 4 +- .../frc/robot/subsystems/Superstructure.java | 2 +- .../subsystems/shoulder/ShoulderIOReal.java | 3 +- .../shoulder/ShoulderSubsystem.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 9 +- 6 files changed, 78 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd62681c..072eb6be 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,7 +29,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; @@ -397,72 +396,75 @@ public static enum AlgaeScoreTarget { .rightTrigger() .negate() .and(() -> DriverStation.isTeleop()) - .or( - new Trigger( - () -> { - final var state = - new ExtensionState( - elevator.getExtensionMeters(), - shoulder.getAngle(), - wrist.getAngle()); - final var branch = - ExtensionKinematics.getBranchPose( - swerve.getPose(), state, currentTarget); - final var manipulatorPose = - ExtensionKinematics.getManipulatorPose(swerve.getPose(), state); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("IK/Branch", branch); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Extension Check", - manipulatorPose, - manipulatorPose.transformBy( - new Transform3d( - Units.inchesToMeters(3.0), 0.0, 0.0, new Rotation3d()))); - return false; - // return branch - // .getTranslation() - // .getDistance(manipulatorPose.getTranslation()) - // < Units.inchesToMeters(1.5) - // || branch - // .getTranslation() - // .getDistance( - // manipulatorPose - // .transformBy( - // new Transform3d( - // Units.inchesToMeters(3.0), - // 0.0, - // 0.0, - // new Rotation3d())) - // .getTranslation()) - // < Units.inchesToMeters(1.5); - }) - .debounce(0.15)) + // .or( + // new Trigger( + // () -> { + // final var state = + // new ExtensionState( + // elevator.getExtensionMeters(), + // shoulder.getAngle(), + // wrist.getAngle()); + // final var branch = + // ExtensionKinematics.getBranchPose( + // swerve.getPose(), state, currentTarget); + // final var manipulatorPose = + // ExtensionKinematics.getManipulatorPose(swerve.getPose(), + // state); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Branch", branch); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput( + // "IK/Extension Check", + // manipulatorPose, + // manipulatorPose.transformBy( + // new Transform3d( + // Units.inchesToMeters(3.0), 0.0, 0.0, new + // Rotation3d()))); + // return false; + // // return branch + // // .getTranslation() + // // .getDistance(manipulatorPose.getTranslation()) + // // < Units.inchesToMeters(1.5) + // // || branch + // // .getTranslation() + // // .getDistance( + // // manipulatorPose + // // .transformBy( + // // new Transform3d( + // // Units.inchesToMeters(3.0), + // // 0.0, + // // 0.0, + // // new Rotation3d())) + // // .getTranslation()) + // // < Units.inchesToMeters(1.5); + // }) + // .debounce(0.15)) // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .or(() -> Autos.autoScore && DriverStation.isAutonomous()) - .or( - new Trigger( - () -> - AutoAim.isInToleranceCoral( - swerve.getPose(), - Units.inchesToMeters(1.5), - Units.degreesToRadians(1.5)) - && MathUtil.isNear( - 0, - Math.hypot( - swerve.getVelocityRobotRelative().vxMetersPerSecond, - swerve.getVelocityRobotRelative().vyMetersPerSecond), - AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - && MathUtil.isNear( - 0.0, - swerve.getVelocityRobotRelative().omegaRadiansPerSecond, - 3.0) - && currentTarget != ReefTarget.L4 - && currentTarget != ReefTarget.L1) - .debounce(0.08) - .and(() -> swerve.hasFrontTags)), + // .or( + // new Trigger( + // () -> + // AutoAim.isInToleranceCoral( + // swerve.getPose(), + // Units.inchesToMeters(1.5), + // Units.degreesToRadians(1.5)) + // && MathUtil.isNear( + // 0, + // Math.hypot( + // swerve.getVelocityRobotRelative().vxMetersPerSecond, + // swerve.getVelocityRobotRelative().vyMetersPerSecond), + // AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) + // && MathUtil.isNear( + // 0.0, + // swerve.getVelocityRobotRelative().omegaRadiansPerSecond, + // 3.0) + // && currentTarget != ReefTarget.L4 + // && currentTarget != ReefTarget.L1) + // .debounce(0.08) + // .and(() -> swerve.hasFrontTags)) + , driver .rightTrigger() .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index cfebf6c7..f4f29db9 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -40,13 +40,13 @@ public class ExtensionKinematics { public static final ExtensionState L2_EXTENSION = new ExtensionState( 0.23 + Units.inchesToMeters(1.5), - Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)), + Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20 - 2)), Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = new ExtensionState( 0.60 + Units.inchesToMeters(2.0), - Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)), + Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(6)), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a0a9ede8..3bdba5a6 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -309,7 +309,7 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - shoulder.setTargetAngle(Rotation2d.fromDegrees(55.0)), + shoulder.setTargetAngle(Rotation2d.fromDegrees(50.0)), elevator.runCurrentZeroing(), Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) .andThen(wrist.currentZero(() -> shoulder.getInputs())))) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 54677024..54371de2 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -81,7 +81,8 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; + cancoderConfig.MagnetSensor.MagnetOffset = + -0.36; // -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 00977b67..300cd59f 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -30,7 +30,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0 - 7); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index aaec10d3..00a53e71 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,8 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; import frc.robot.subsystems.ExtensionKinematics; import frc.robot.subsystems.shoulder.ShoulderIOInputsAutoLogged; import java.util.function.Supplier; @@ -70,7 +68,7 @@ public WristSubsystem(WristIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Carriage/Wrist", inputs); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Wrist/Has Zeroed", hasZeroed); + Logger.recordOutput("Wrist/Has Zeroed", hasZeroed); } public Command setTargetAngle(final Supplier target) { @@ -153,8 +151,11 @@ public Command currentZero(Supplier shoulderInputs) > 7.0))), this.runOnce( () -> { + // Logger.recordOutput( + // "shoulder zero pos", shoulderInputs.get().position.minus(ZEROING_OFFSET)); hasZeroed = true; - io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); + // io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); + io.resetEncoder(Rotation2d.fromRadians(-0.687)); })); } From dc8b5ac6613ffc1f249a07ca0935534210df92da Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:49:21 -0700 Subject: [PATCH 085/154] update climber --- .../java/frc/robot/subsystems/climber/ClimberIO.java | 2 -- .../frc/robot/subsystems/climber/ClimberIOReal.java | 9 ++------- .../frc/robot/subsystems/climber/ClimberIOSim.java | 12 ------------ .../robot/subsystems/climber/ClimberSubsystem.java | 8 ++------ 4 files changed, 4 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 80e6666a..b316422c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -17,8 +17,6 @@ class ClimberIOInputs { public void setVoltage(final double volts); - public void setPosition(final double position); - public void setPosition(final double position, final double vel); public void resetEncoder(final double position); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index 691a94de..3247d897 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -47,7 +47,7 @@ public ClimberIOReal() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); + motor.getConfigurator().apply(config); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, angularVelocityRPS, @@ -62,7 +62,7 @@ public ClimberIOReal() { @Override public void updateInputs(ClimberIOInputsAutoLogged inputs) { Logger.recordOutput( - "Climber refreshall statuscode", + "Climber/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -84,11 +84,6 @@ public void setVoltage(double volts) { motor.setControl(voltageOut.withOutput(volts)); } - @Override - public void setPosition(final double position) { - motor.setControl(motionMagic.withPosition(position)); - } - @Override public void setPosition(final double position, final double vel) { motor.setControl(motionMagic.withPosition(position).withVelocity(vel)); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index f4b56ccb..861d2cff 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -49,18 +49,6 @@ public void setVoltage(final double voltage) { armSim.setInputVoltage(voltage); } - @Override - public void setPosition(final double position) { - setVoltage( - pid.calculate( - armSim.getAngleRads(), - Math.asin( - (Units.rotationsToRadians(position) - * ClimberSubsystem.CLIMBER_DRUM_RADIUS_METERS) - / ClimberSubsystem.CLIMBER_ARM_LENGTH_METERS)) - + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); - } - @Override public void resetEncoder(double position) { armSim.setState(position, 0); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index f9df6c29..ad0acf87 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -38,12 +38,8 @@ public void periodic() { Logger.processInputs("Climber", inputs); } - public Command setPosition(double position) { - return this.run(() -> io.setPosition(position, FAST_VEL)); - } - - public Command setPositionSlow(double position) { - return this.run(() -> io.setPosition(position, SLOW_VEL)); + public Command setPosition(double position, double vel) { + return this.run(() -> io.setPosition(position, vel)); } public Command resetClimber() { From a08916829daefc8ec963c724c3541dedd73d6793 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:49:47 -0700 Subject: [PATCH 086/154] update elevator --- .../robot/subsystems/elevator/ElevatorIO.java | 6 +- .../subsystems/elevator/ElevatorIOReal.java | 19 +- .../subsystems/elevator/ElevatorIOSim.java | 18 +- .../elevator/ElevatorSubsystem.java | 222 ++++++------------ 4 files changed, 78 insertions(+), 187 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 64e55c8c..b294f15d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -19,14 +19,10 @@ public static class ElevatorIOInputs { public void updateInputs(final ElevatorIOInputsAutoLogged inputs); - public void setTarget(final double meters, final double maxAccel); - - public void setTarget(final double meters); + public void setPosition(final double meters, final double maxAccel); public void setVoltage(final double voltage); - public void setCurrent(final double amps); - public default void stop() { setVoltage(0); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index b8a6d8af..a1859421 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -28,10 +27,10 @@ public class ElevatorIOReal implements ElevatorIO { private final TalonFX follower = new TalonFX(17, "*"); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final TorqueCurrentFOC torque = new TorqueCurrentFOC(0.0); private final DynamicMotionMagicVoltage positionVoltage; // misusing type system here - these correspond to linear meters, NOT rotations + // i hate this so much private final StatusSignal position = motor.getPosition(); private final StatusSignal velocity = motor.getVelocity(); private final StatusSignal voltage = motor.getMotorVoltage(); @@ -40,6 +39,7 @@ public class ElevatorIOReal implements ElevatorIO { private final StatusSignal temp = motor.getDeviceTemp(); public ElevatorIOReal() { + //i'm just going to trust this for chezy i guess var config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -109,9 +109,10 @@ public ElevatorIOReal() { @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { Logger.recordOutput( - "Elevator refreshall statuscode", + "Elevator/Signal Refresh Status Code", BaseStatusSignal.refreshAll( position, velocity, voltage, statorCurrent, supplyCurrent, temp)); + inputs.positionMeters = position.getValueAsDouble(); inputs.velocityMetersPerSec = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); @@ -121,25 +122,15 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { } @Override - public void setTarget(final double meters, final double maxAccel) { + public void setPosition(final double meters, final double maxAccel) { motor.setControl(positionVoltage.withPosition(meters).withAcceleration(maxAccel)); } - @Override - public void setTarget(final double meters) { - motor.setControl(positionVoltage.withPosition(meters)); - } - @Override public void setVoltage(final double voltage) { motor.setControl(voltageOut.withOutput(voltage)); } - @Override - public void setCurrent(final double amps) { - motor.setControl(torque.withOutput(amps)); - } - @Override public void resetEncoder(final double position) { motor.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 73f4afd4..2f54c9de 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -50,33 +50,19 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { } @Override - public void setTarget(final double meters) { + public void setPosition(double meters, double maxAccel) { + pid.setConstraints(new Constraints(5.0, maxAccel)); setVoltage( pid.calculate(physicsSim.getPositionMeters(), meters) + ff.calculate(pid.getSetpoint().velocity)); } - @Override - public void setTarget(double meters, double maxAccel) { - pid.setConstraints(new Constraints(5.0, maxAccel)); - setTarget(meters); - } - @Override public void setVoltage(final double voltage) { volts = voltage; physicsSim.setInputVoltage(MathUtil.clamp(voltage, -12.0, 12.0)); } - @Override - public void setCurrent(double amps) { - setVoltage( - DCMotor.getKrakenX60Foc(2) - .getVoltage( - amps, - physicsSim.getVelocityMetersPerSecond() / ElevatorSubsystem.DRUM_RADIUS_METERS)); - } - @Override public void resetEncoder(final double position) { // sim always has a perfectly accurate encoder diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 2b9e6d34..419a83e2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -4,9 +4,6 @@ package frc.robot.subsystems.elevator; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose3d; @@ -14,16 +11,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics; import java.util.function.DoubleSupplier; -import java.util.function.Function; + +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; @@ -40,47 +33,75 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double Z_OFFSET_METERS = Units.inchesToMeters(8.175000); public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - - public static final double L1_EXTENSION_METERS = - ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); - public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); - public static final double L2_EXTENSION_METERS = - ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); - public static final double L3_EXTENSION_METERS = - ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); - public static final double L4_EXTENSION_METERS = - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - - public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); - public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); - public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); - - // 1 inch high (maybe not needed?) - public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); - - public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; - - public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); - public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); - + public static final double MAX_ACCELERATION = 10.0; public static final double SLOW_ACCELERATION = 5.0; public static final double MEDIUM_ACCELERATION = 8.5; + // public static final double L1_EXTENSION_METERS = + // ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); + // public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); + // public static final double L2_EXTENSION_METERS = + // ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); + // public static final double L3_EXTENSION_METERS = + // ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); + // public static final double L4_EXTENSION_METERS = + // ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); + // public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); + // public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); + // public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); + // public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); + // // 1 inch high (maybe not needed?) + // public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); + // public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; + // public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); + // public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); + + public enum ElevatorState { + HP(Units.inchesToMeters(0.0)), + INTAKE_CORAL_GROUND(Units.inchesToMeters(0.0)), + L1(0.217), + L2(0.23 + Units.inchesToMeters(1.5)), + L3(0.60 + Units.inchesToMeters(2.0)), + L4(1.383), + INTAKE_ALGAE_LOW(Units.inchesToMeters(Units.inchesToMeters(20.0))), + INTAKE_ALGAE_HIGH(Units.inchesToMeters(35.0)), + INTAKE_ALGAE_STACK(Units.inchesToMeters(9.0)), + INTAKE_ALGAE_GROUND(0.14 - Units.inchesToMeters(0.75)), + READY_ALGAE(0.1), + BARGE(Units.inchesToMeters(62.5)), + PROCESSOR(Units.inchesToMeters(0.01)) // lmao + ; + + private final double extensionMeters; + + private ElevatorState(double extensionMeters) { + this.extensionMeters = extensionMeters; + } + + public double getExtensionMeters() { + return extensionMeters; + } + } + + + @AutoLogOutput(key = "Elevator/State") + public ElevatorState state = ElevatorState.HP; + + private ElevatorState prevState = ElevatorState.HP; + + @AutoLogOutput(key = "Elevator/Setpoint") + private double setpoint = 0.0; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); private final ElevatorIO io; private final LinearFilter currentFilter = LinearFilter.movingAverage(5); public double currentFilterValue = 0.0; + @AutoLogOutput(key = "Elevator/Has Zeroed") public boolean hasZeroed = false; - private double setpoint = 0.0; - - private final SysIdRoutine voltageSysid; - private final SysIdRoutine currentSysid; - // For dashboard private final LoggedMechanism2d mech2d = new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); private final LoggedMechanismRoot2d @@ -93,23 +114,6 @@ public class ElevatorSubsystem extends SubsystemBase { /** Creates a new ElevatorSubsystem. */ public ElevatorSubsystem(ElevatorIO io) { this.io = io; - voltageSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Elevator/SysIdTestStateVolts", state.toString())), - new Mechanism((volts) -> io.setVoltage(volts.in(Volts)), null, this)); - - currentSysid = - new SysIdRoutine( - new Config( - Volts.of(10.0).per(Second), - Volts.of(60.0), - null, - (state) -> Logger.recordOutput("Elevator/SysIdTestStateCurrent", state.toString())), - new Mechanism((volts) -> io.setCurrent(volts.in(Volts)), null, this)); } @Override @@ -119,60 +123,37 @@ public void periodic() { currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); carriage.setLength(inputs.positionMeters); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Mechanism2d", mech2d); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Mechanism2d", mech2d); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose()); - - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Has Zeroed", hasZeroed); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Filtered Current", currentFilterValue); } + public void setState(ElevatorState state) { + this.state = state; + } + public Command setExtension(DoubleSupplier meters) { return this.run( () -> { - io.setTarget(meters.getAsDouble(), MAX_ACCELERATION); + io.setPosition(meters.getAsDouble(), MAX_ACCELERATION); setpoint = meters.getAsDouble(); Logger.recordOutput("Elevator/Setpoint", setpoint); }); } - + public Command setExtension(double meters) { return this.setExtension(() -> meters); } - public Command setExtensionSlow(DoubleSupplier meters) { - return this.run( - () -> { - io.setTarget(meters.getAsDouble(), SLOW_ACCELERATION); - setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); - }); - } - - public Command setExtensionSlow(double meters) { - return this.setExtensionSlow(() -> meters); - } - - public Command setExtensionMedium(DoubleSupplier meters) { - return this.run( - () -> { - io.setTarget(meters.getAsDouble(), MEDIUM_ACCELERATION); - setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); - }); - } - - public Command setExtensionMedium(double meters) { - return this.setExtensionMedium(() -> meters); + public Command setStateExtension() { + return setExtension(() -> state.getExtensionMeters()); } - public Command hold() { - return Commands.sequence( - setExtension(() -> inputs.positionMeters).until(() -> true), this.run(() -> {})); + public boolean atExtension(double expected) { + return MathUtil.isNear(expected, inputs.positionMeters, 0.05); } public Command runCurrentZeroing() { @@ -193,47 +174,6 @@ public Command runCurrentZeroing() { }); } - public Command runSysid() { - final Function runSysid = - (routine) -> - Commands.sequence( - routine - .quasistatic(SysIdRoutine.Direction.kForward) - .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), - Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), - routine - .quasistatic(SysIdRoutine.Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0)), - Commands.waitUntil(() -> Math.abs(inputs.velocityMetersPerSec) < 0.1), - routine - .dynamic(SysIdRoutine.Direction.kForward) - .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), - Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), - routine - .dynamic(SysIdRoutine.Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0))); - return Commands.sequence( - runCurrentZeroing(), runSysid.apply(voltageSysid), runSysid.apply(currentSysid)); - } - - public Command setVoltage(double voltage) { - return this.run( - () -> { - io.setVoltage(voltage); - }); - } - - public Command setVoltage(DoubleSupplier voltage) { - return this.setVoltage(voltage.getAsDouble()); - } - - public Command setCurrent(double amps) { - return this.run( - () -> { - io.setCurrent(amps); - }); - } - public Pose3d getCarriagePose() { return new Pose3d( Units.inchesToMeters(4.5) + inputs.positionMeters * ELEVATOR_ANGLE.getCos(), @@ -242,16 +182,6 @@ public Pose3d getCarriagePose() { new Rotation3d()); } - public Pose3d getFirstStagePose() { - return new Pose3d( - Units.inchesToMeters(2.25) - + (inputs.positionMeters / 2.0) * Math.cos(ELEVATOR_ANGLE.getRadians()), - 0.0, - Units.inchesToMeters(4.25) - + (inputs.positionMeters / 2.0) * Math.sin(ELEVATOR_ANGLE.getRadians()), - new Rotation3d()); - } - public double getExtensionMeters() { return inputs.positionMeters; } @@ -260,18 +190,6 @@ public double getSetpoint() { return setpoint; } - public boolean isNearTarget() { - return isNearExtension(setpoint); - } - - public boolean isNearExtension(double expected) { - return MathUtil.isNear(expected, inputs.positionMeters, 0.05); - } - - public boolean isNearExtension(double expected, double toleranceMeters) { - return MathUtil.isNear(expected, inputs.positionMeters, toleranceMeters); - } - public void resetExtension(double extension) { io.resetEncoder(extension); hasZeroed = true; From 6f35b1be3ab75f3509ecd0f2d2b0b5c755ca9a70 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:50:24 -0700 Subject: [PATCH 087/154] update rollers --- .../frc/robot/subsystems/roller/RollerIO.java | 8 ------ .../robot/subsystems/roller/RollerIOReal.java | 20 +------------ .../robot/subsystems/roller/RollerIOSim.java | 14 ---------- .../subsystems/roller/RollerSubsystem.java | 28 ++++++++++--------- 4 files changed, 16 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/roller/RollerIO.java index 6fa26234..acbe6956 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIO.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIO.java @@ -23,20 +23,12 @@ public static class RollerIOInputs { public void setVoltage(double voltage); - public void setCurrent(double amps); - public default void stop() { setVoltage(0.0); } public void setVelocity(double velocityRPS); - /** - * This method is meant to set a function to be called alongside updateInput to update a - * simulation, such as for routing simulation - */ - public void registerSimulationCallback(Consumer callback); - public void setPosition(Rotation2d rot); public void resetEncoder(double position); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java index 0f917ba7..ce990e75 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java @@ -16,7 +16,6 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -28,15 +27,12 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import java.util.Optional; -import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; public class RollerIOReal implements RollerIO { private final TalonFX motor; private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final TorqueCurrentFOC currentFOC = new TorqueCurrentFOC(0.0); private final VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true).withSlot(0); private final PositionVoltage positionVoltage = @@ -49,8 +45,6 @@ public class RollerIOReal implements RollerIO { private final StatusSignal temp; private final StatusSignal position; - private Optional> callback = Optional.empty(); - public RollerIOReal(final int motorID, final TalonFXConfiguration config) { this.motor = new TalonFX(motorID, "*"); @@ -86,7 +80,7 @@ public static TalonFXConfiguration getDefaultConfig() { @Override public void updateInputs(RollerIOInputsAutoLogged inputs) { Logger.recordOutput( - "Roller" + motor.getDeviceID() + "refreshall statuscode", + "Roller" + motor.getDeviceID() + " Signal Refresh Status Code", BaseStatusSignal.refreshAll( velocity, voltage, statorCurrent, supplyCurrent, temp, position)); inputs.velocityRotationsPerSec = velocity.getValue().in(RotationsPerSecond); @@ -95,8 +89,6 @@ public void updateInputs(RollerIOInputsAutoLogged inputs) { inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); inputs.tempCelsius = temp.getValue().in(Celsius); inputs.positionRotations = position.getValueAsDouble(); - - callback.ifPresent((cb) -> cb.accept(inputs)); } @Override @@ -104,21 +96,11 @@ public void setVoltage(double voltage) { motor.setControl(voltageOut.withOutput(voltage)); } - @Override - public void setCurrent(double amps) { - motor.setControl(currentFOC.withOutput(amps)); - } - @Override public void setVelocity(double velocityRPS) { motor.setControl(velocityVoltage.withVelocity(velocityRPS)); } - @Override - public void registerSimulationCallback(Consumer callback) { - this.callback = Optional.of(callback); - } - @Override public void setPosition(Rotation2d rot) { motor.setControl(positionVoltage.withPosition(rot.getRotations())); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java index 3a37e16b..f95e3bd8 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java @@ -59,11 +59,6 @@ public void setVelocity(double velocityRPS) { + pid.calculate(motorSim.getAngularVelocityRPM() / 60, velocityRPS)); } - @Override - public void registerSimulationCallback(Consumer callback) { - this.callback = Optional.of(callback); - } - @Override public void setPosition(Rotation2d rot) { setVelocity(0.0); @@ -75,13 +70,4 @@ public void setPosition(Rotation2d rot) { public void resetEncoder(double position) { motorSim.setAngle(position); } - - @Override - public void setCurrent(double amps) { - setVoltage( - DCMotor.getKrakenX60Foc(1) - .getVoltage( - DCMotor.getKrakenX60Foc(1).getTorque(amps), - motorSim.getAngularVelocityRadPerSec())); - } } diff --git a/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java index 9fc42bf9..111e35c2 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java @@ -14,7 +14,8 @@ public class RollerSubsystem extends SubsystemBase { protected final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); private final String name; - public static final double INDEXING_VELOCITY = 10; // TODO + + private double setpoint = 0.0; /** * Creates a new RollerSubsystem. @@ -34,23 +35,24 @@ public void periodic() { Logger.processInputs(name, inputs); } - public Command setVelocity(DoubleSupplier vel) { - return this.run(() -> io.setVelocity(vel.getAsDouble())); + public Command setRollerVoltage(DoubleSupplier voltage) { + return this.runOnce( + () -> { + io.setVoltage(voltage.getAsDouble()); + setpoint = voltage.getAsDouble(); + Logger.recordOutput(name + "/Roller Voltage Setpoint", setpoint); + }); } - public Command setVelocity(double vel) { - return this.setVelocity(() -> vel); + public Command setRollerVoltage(double voltage) { + return setRollerVoltage(() -> voltage); } - public Command setVoltage(DoubleSupplier volts) { - return this.run(() -> io.setVoltage(volts.getAsDouble())); - } - - public Command setVoltage(double vel) { - return this.setVoltage(() -> vel); + public Command setRollerVelocity(DoubleSupplier vel) { + return this.run(() -> io.setVelocity(vel.getAsDouble())); } - public Command setCurrent(DoubleSupplier amps) { - return this.run(() -> io.setCurrent(amps.getAsDouble())); + public Command setRollerVelocity(double vel) { + return setRollerVelocity(() -> vel); } } From b9cadb681e4758557251fdbe4d895ac4cba8d5b5 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:50:44 -0700 Subject: [PATCH 088/154] update shoulder --- .../robot/subsystems/shoulder/ShoulderIO.java | 8 +- .../subsystems/shoulder/ShoulderIOReal.java | 9 +- .../subsystems/shoulder/ShoulderIOSim.java | 4 +- .../shoulder/ShoulderSubsystem.java | 180 ++++++++++-------- 4 files changed, 111 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java index d7d2fe49..02cafd19 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java @@ -8,7 +8,7 @@ public interface ShoulderIO { @AutoLog class ShoulderIOInputs { public double angularVelocityRPS = 0.0; - public Rotation2d position = new Rotation2d(); + public Rotation2d motorPosition = new Rotation2d(); public double cancoderPosition = 0.0; public double appliedVoltage = 0.0; public double tempDegreesC = 0.0; @@ -20,12 +20,12 @@ class ShoulderIOInputs { public void setMotorVoltage(final double voltage); - public void setMotorPosition(final Rotation2d targetPosition); + public void setPivotAngle(final Rotation2d targetPosition); - public default void resetEncoder(final Rotation2d rotation) {} + public default void setEncoderPosition(final Rotation2d rotation) {} public default void resetEncoder() { - resetEncoder(Rotation2d.kZero); + setEncoderPosition(Rotation2d.kZero); } public void setMotionMagicConfigs(final MotionMagicConfigs configs); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 54371de2..e88e5fa2 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -105,7 +105,7 @@ public ShoulderIOReal() { @Override public void updateInputs(ShoulderIOInputs inputs) { Logger.recordOutput( - "shoulder refreshall statuscode", + "Shoulder/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -115,7 +115,7 @@ public void updateInputs(ShoulderIOInputs inputs) { cancoderPositionRotations, appliedVoltage)); - inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorPosition = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.cancoderPosition = cancoderPositionRotations.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); @@ -130,16 +130,17 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setPivotAngle(final Rotation2d targetPosition) { motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); } @Override - public void resetEncoder(final Rotation2d rotation) { + public void setEncoderPosition(final Rotation2d rotation) { motor.setPosition(rotation.getRotations()); } @Override + //TODO i hate this can we get rid of this public void setMotionMagicConfigs(final MotionMagicConfigs configs) { motor.getConfigurator().apply(configs); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index 03ee8bab..be9bf62a 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -39,7 +39,7 @@ public void updateInputs(final ShoulderIOInputs inputs) { inputs.angularVelocityRPS = RadiansPerSecond.of(armSim.getVelocityRadPerSec()).in(RotationsPerSecond); - inputs.position = Rotation2d.fromRadians(armSim.getAngleRads()); + inputs.motorPosition = Rotation2d.fromRadians(armSim.getAngleRads()); inputs.statorCurrentAmps = armSim.getCurrentDrawAmps(); inputs.supplyCurrentAmps = 0.0; inputs.tempDegreesC = 0.0; @@ -53,7 +53,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setPivotAngle(final Rotation2d targetPosition) { setMotorVoltage( pid.calculate(armSim.getAngleRads(), targetPosition.getRadians()) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 300cd59f..a3676566 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -1,6 +1,13 @@ package frc.robot.subsystems.shoulder; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; + import com.ctre.phoenix6.configs.MotionMagicConfigs; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -10,50 +17,43 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics; import frc.robot.utils.Tracer; -import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ShoulderSubsystem extends SubsystemBase { - // TODO: UPDATE WITH CAD public static final double SHOULDER_FINAL_STAGE_RATIO = 3.0; public static final double SHOULDER_GEAR_RATIO = 25.0 * (34.0 / 28.0) * SHOULDER_FINAL_STAGE_RATIO; public static final int CANCODER_ID = 5; public static final Rotation2d MAX_SHOULDER_ROTATION = Rotation2d.fromDegrees(120.0); public static final Rotation2d MIN_SHOULDER_ROTATION = Rotation2d.fromDegrees(-5.0); - public static final Rotation2d SHOULDER_RETRACTED_POS = Rotation2d.fromDegrees(90.0); public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0 - 7); - public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505) - .plus(Rotation2d.fromDegrees(-5.0)) - .minus(Rotation2d.fromDegrees(2)); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(60.0); - // may be incorrect as l2-3 poses are derived from ExtensionKinematics now - public static final Rotation2d SHOULDER_SCORE_POS = - ExtensionKinematics.L2_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L1_POS = - ExtensionKinematics.L1_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_SCORE_L4_POS = - ExtensionKinematics.L4_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); - public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); - public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); + + // public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0 - 7); + // public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); + // public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = + // Rotation2d.fromRadians(0.505) + // .plus(Rotation2d.fromDegrees(-5.0)) + // .minus(Rotation2d.fromDegrees(2)); + // public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); + // public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); + // public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = + // Rotation2d.fromDegrees(60.0); + // // may be incorrect as l2-3 poses are derived from ExtensionKinematics now + // public static final Rotation2d SHOULDER_SCORE_POS = + // ExtensionKinematics.L2_EXTENSION.shoulderAngle(); + // public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); + // public static final Rotation2d SHOULDER_SCORE_L1_POS = + // ExtensionKinematics.L1_EXTENSION.shoulderAngle(); + // public static final Rotation2d SHOULDER_SCORE_L4_POS = + // ExtensionKinematics.L4_EXTENSION.shoulderAngle(); + // public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); + // public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); + // public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); + // public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); + // public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); @@ -62,14 +62,56 @@ public class ShoulderSubsystem extends SubsystemBase { .withMotionMagicCruiseVelocity(0.275) .withMotionMagicAcceleration(4.0); - private final ShoulderIO io; - private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); + public enum ShoulderState { + HP(Rotation2d.fromDegrees(50.0)), + PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(35.0)), + INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(8.0)), + PRE_L1(Rotation2d.fromDegrees(35.0)), + L1(Rotation2d.fromRadians(1.617)), // not sure about units tbh + PRE_L2(Rotation2d.fromDegrees(35.0)), + L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20))), + PRE_L3(Rotation2d.fromDegrees(35.0)), + L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3))), + PRE_L4(Rotation2d.fromDegrees(8.0)), + L4(Rotation2d.fromDegrees(25.0)), + PRE_INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(35.0)), + INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(45.0)), + INTAKE_ALGAE_STACK(Rotation2d.fromDegrees(30.0)), + INTAKE_ALGAE_GROUND( + Rotation2d.fromRadians(0.505) + .plus(Rotation2d.fromDegrees(-5.0)) + .minus(Rotation2d.fromDegrees(2))), // hello?? + READY_ALGAE(Rotation2d.fromDegrees(60.0)), + PRE_BARGE(Rotation2d.fromDegrees(30)), + SCORE_BARGE(Rotation2d.fromDegrees(90)), + PROCESSOR(Rotation2d.fromDegrees(60.0)); + + // L4_TUCKED(Rotation2d.fromDegrees(35.0)), // SHOULDER_TUCKED_CLEARANCE_POS + // L4_TUCKED_OUT(Rotation2d.fromDegrees(25.0)), + + private final Rotation2d angle; + + private ShoulderState(Rotation2d angle) { + this.angle = angle; + } + + public Rotation2d getAngle() { + return angle; + } + } + @AutoLogOutput(key = "Shoulder/Setpoint") private Rotation2d setpoint = Rotation2d.kZero; + @AutoLogOutput(key = "Shoulder/State") + private ShoulderState state = ShoulderState.HP; + + private final ShoulderIO io; + private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); + private LoggedNetworkBoolean dashboardZero = new LoggedNetworkBoolean("Zero Shoulder"); - public ShoulderSubsystem(final ShoulderIO io) { + public ShoulderSubsystem(ShoulderIO io) { this.io = io; io.updateInputs(inputs); rezero(); @@ -93,73 +135,43 @@ public void periodic() { Logger.recordOutput("Carriage/Shoulder/Cancoder Pos", getZeroingAngle()); } + public void setState(ShoulderState state) { + this.state = state; + } + + public Command setStateAngle() { + return setAngle(() -> state.getAngle()); + } + @AutoLogOutput(key = "Shoulder/Zeroing Angle") + //what public Rotation2d getZeroingAngle() { return Rotation2d.fromRotations(inputs.cancoderPosition).div(SHOULDER_FINAL_STAGE_RATIO); } public void rezero() { - io.resetEncoder(getZeroingAngle()); + io.setEncoderPosition(getZeroingAngle()); } - public Command setTargetAngle(final Supplier target) { + public Command setAngle(Supplier target) { return this.run( () -> { - io.setMotorPosition(target.get()); + io.setPivotAngle(target.get()); setpoint = target.get(); Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); }); } - public Command setTargetAngle(final Rotation2d target) { - return setTargetAngle(() -> target); - } - - public Command setTargetAngleSlow(final Supplier target) { - return Commands.sequence( - this.runOnce(() -> io.setMotionMagicConfigs(TOSS_CONFIGS)), - this.run( - () -> { - io.setMotorPosition(target.get()); - setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); - })) - .finallyDo(() -> io.setMotionMagicConfigs(DEFAULT_CONFIGS)); + public Command setAngle(Rotation2d target) { + return setAngle(() -> target); } - public Command setTargetAngleSlow(final Rotation2d target) { - return setTargetAngleSlow(() -> target); - } - - public Command setVoltage(final double volts) { + public Command setVoltage(double volts) { return this.run(() -> io.setMotorVoltage(volts)); } - public Command hold() { - return Commands.sequence( - setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); - } - - public Rotation2d getAngle() { - return inputs.position; - } - - public Rotation2d getSetpoint() { - return setpoint; - } - - public boolean isNearTarget() { - return isNearAngle(setpoint); - } - public boolean isNearAngle(Rotation2d target) { - return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); - } - - public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { - return MathUtil.isNear( - target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); + return MathUtil.isNear(target.getDegrees(), inputs.motorPosition.getDegrees(), 10.0); } public ShoulderIOInputsAutoLogged getInputs() { @@ -169,4 +181,12 @@ public ShoulderIOInputsAutoLogged getInputs() { public double getVelocity() { return inputs.angularVelocityRPS; } + + public Rotation2d getAngle() { + return inputs.motorPosition; + } + + public Rotation2d getSetpoint() { + return setpoint; + } } From 32cd1d62497262ac823acdc4fcc98afb1ccbb241 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:50:56 -0700 Subject: [PATCH 089/154] update wrist --- .../frc/robot/subsystems/wrist/WristIO.java | 2 +- .../robot/subsystems/wrist/WristIOReal.java | 4 +- .../robot/subsystems/wrist/WristIOSim.java | 3 +- .../subsystems/wrist/WristSubsystem.java | 136 +++++++++--------- 4 files changed, 75 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java index 89ce07e0..0ebcaed0 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.java @@ -19,7 +19,7 @@ class ArmIOInputs { public void setMotorVoltage(final double voltage); - public void setMotorPosition(final Rotation2d targetPosition); + public void setAngle(final Rotation2d targetPosition); public default void resetEncoder(final Rotation2d rotation) {} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 639a4131..3927470e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -64,7 +64,7 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { @Override public void updateInputs(ArmIOInputs inputs) { Logger.recordOutput( - "wrist refreshall statuscode", + "Wrist/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -87,7 +87,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setAngle(final Rotation2d targetPosition) { motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java index 42e5c350..0a76f97c 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; public class WristIOSim implements WristIO { - // TODO: UPDATE WITH VALUES WHEN CAD IS DONE private final SingleJointedArmSim armSim = new SingleJointedArmSim( DCMotor.getKrakenX60Foc(1), @@ -52,7 +51,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setAngle(final Rotation2d targetPosition) { setMotorVoltage( pid.calculate(armSim.getAngleRads(), targetPosition.getRadians()) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 00a53e71..c11a362a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,40 +7,37 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.ExtensionKinematics; import frc.robot.subsystems.shoulder.ShoulderIOInputsAutoLogged; import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); - // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); - public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); - public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); - public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); - public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); - - public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); - - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(-20.0); - - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(110); - public static final Rotation2d WRIST_PRE_NET_POS = Rotation2d.fromDegrees(100); - public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); + + // public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); + // public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); + // public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); + // public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); + // public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); + // public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); + // public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); + // public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); + // public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); + // public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = + // Rotation2d.fromDegrees(-20.0); + // public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(110); + // public static final Rotation2d WRIST_PRE_NET_POS = Rotation2d.fromDegrees(100); + // public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); @@ -51,13 +48,49 @@ public class WristSubsystem extends SubsystemBase { public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); + public enum WristState { + PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(30.0)), // formerly WRIST_CLEARANCE_POS + INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(0.0)), + HP(Rotation2d.fromDegrees(178.0)), + PRE_L1(Rotation2d.fromRadians(0.349)), + PRE_L2(Rotation2d.fromDegrees(170.0)), + L2(Rotation2d.fromRadians(2.447)), + PRE_L3(Rotation2d.fromDegrees(170.0)), + L3(Rotation2d.fromRadians(2.427)), + L4(Rotation2d.fromDegrees(120.0)), // ?? + PRE_INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(30.0)), + INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(-20.0)), + INTAKE_ALGAE_STACK(Rotation2d.fromDegrees(-10)), + INTAKE_ALGAE_GROUND(Rotation2d.fromDegrees(-65)), + READY_ALGAE(Rotation2d.fromDegrees(20)), + PRE_BARGE(Rotation2d.fromDegrees(100)), + SCORE_BARGE(Rotation2d.fromDegrees(110)), + PROCESSOR(Rotation2d.fromDegrees(-30.0)) + ; + + private final Rotation2d angle; + + private WristState(Rotation2d angle) { + this.angle = angle; + } + + public Rotation2d getAngle() { + return angle; + } + } + + @AutoLogOutput(key = "Wrist/Setpoint") + private Rotation2d setpoint = Rotation2d.kZero; + + @AutoLogOutput(key = "Wrist/State") + private WristState state = WristState.HP; + private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private Rotation2d setpoint = Rotation2d.kZero; - private final LinearFilter currentFilter = LinearFilter.movingAverage(10); + @AutoLogOutput(key = "Wrist/Has Zeroed") public boolean hasZeroed = false; public WristSubsystem(WristIO io) { @@ -68,45 +101,27 @@ public WristSubsystem(WristIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Carriage/Wrist", inputs); - Logger.recordOutput("Wrist/Has Zeroed", hasZeroed); } - public Command setTargetAngle(final Supplier target) { + public void setState(WristState state) { + this.state = state; + } + + public Command setStateAngle() { + return setAngle(() -> state.getAngle()); + } + + public Command setAngle(final Supplier target) { return this.run( () -> { - io.setMotorPosition(target.get()); + io.setAngle(target.get()); setpoint = target.get(); Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); }); } - public Command setTargetAngle(final Rotation2d target) { - return setTargetAngle(() -> target); - } - - public Command setSlowTargetAngle(final Supplier target) { - return this.runOnce(() -> io.setMotionMagicConfigs(SLOW_MOTION_MAGIC)) - .andThen(this.setTargetAngle(target)) - .finallyDo((interrupted) -> io.setMotionMagicConfigs(DEFAULT_MOTION_MAGIC)); - } - - public Command setSlowTargetAngle(final Rotation2d target) { - return this.setSlowTargetAngle(() -> target); - } - - public Command setCrawlTargetAngle(final Supplier target) { - return this.runOnce(() -> io.setMotionMagicConfigs(CRAWL_MOTION_MAGIC)) - .andThen(this.setTargetAngle(target)) - .finallyDo((interrupted) -> io.setMotionMagicConfigs(DEFAULT_MOTION_MAGIC)); - } - - public Command setCrawlTargetAngle(final Rotation2d target) { - return this.setCrawlTargetAngle(() -> target); - } - - public Command hold() { - return Commands.sequence( - setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); + public Command setAngle(final Rotation2d target) { + return setAngle(() -> target); } public Command setVoltage(final double volts) { @@ -121,19 +136,10 @@ public Rotation2d getSetpoint() { return setpoint; } - public boolean isNearTarget() { - return isNearAngle(setpoint); - } - public boolean isNearAngle(Rotation2d target) { return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); } - public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { - return MathUtil.isNear( - target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); - } - public Command currentZero(Supplier shoulderInputs) { return Commands.sequence( this.runOnce( From 7abd3605b5dfb753141fc1ba97e0751d0553e9cf Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:51:17 -0700 Subject: [PATCH 090/154] move field utils into one file --- src/main/java/frc/robot/utils/FieldUtils.java | 374 ++++++++++++++++++ .../utils/autoaim/AlgaeIntakeTargets.java | 78 ---- .../java/frc/robot/utils/autoaim/AutoAim.java | 3 + .../frc/robot/utils/autoaim/CageTargets.java | 86 ---- .../frc/robot/utils/autoaim/CoralTargets.java | 92 ----- .../utils/autoaim/HumanPlayerTargets.java | 40 -- .../frc/robot/utils/autoaim/L1Targets.java | 100 ----- 7 files changed, 377 insertions(+), 396 deletions(-) create mode 100644 src/main/java/frc/robot/utils/FieldUtils.java delete mode 100644 src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java delete mode 100644 src/main/java/frc/robot/utils/autoaim/CageTargets.java delete mode 100644 src/main/java/frc/robot/utils/autoaim/CoralTargets.java delete mode 100644 src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java delete mode 100644 src/main/java/frc/robot/utils/autoaim/L1Targets.java diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java new file mode 100644 index 00000000..a99645f1 --- /dev/null +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -0,0 +1,374 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils; + +import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; +import java.util.stream.Stream; + +import choreo.util.ChoreoAllianceFlipUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; +import frc.robot.Robot.AlgaeIntakeTarget; +import frc.robot.utils.autoaim.AutoAim; + +/** Add your docs here. */ +public class FieldUtils { + public enum AlgaeIntakeTargets { + // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls + // were extended beyond the coral station + // All angles from the center of the coral with 0° across the width of the field, counterclockwise + BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), + BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), + BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), + BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AlgaeIntakeTarget.LOW), + BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AlgaeIntakeTarget.HIGH), + BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AlgaeIntakeTarget.LOW), + + RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location), AlgaeIntakeTarget.HIGH), + RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location), AlgaeIntakeTarget.LOW), + RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location), AlgaeIntakeTarget.HIGH), + RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location), AlgaeIntakeTarget.LOW), + RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location), AlgaeIntakeTarget.HIGH), + RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location), AlgaeIntakeTarget.LOW); + + public final Pose2d location; + public final AlgaeIntakeTarget height; + + private AlgaeIntakeTargets(Pose2d location, AlgaeIntakeTarget height) { + this.location = location; + this.height = height; + } + + private static final List transformedPoses = + Arrays.stream(values()) + .map( + (AlgaeIntakeTargets targets) -> { + return AlgaeIntakeTargets.getRobotTargetLocation(targets.location); + }) + .toList(); + + public static Pose2d getRobotTargetLocation(Pose2d original) { + return original.transformBy( + new Transform2d( + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Pose2d getOffsetLocation(Pose2d original) { + return original.transformBy( + new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getClosestTargetPose(Pose2d pose) { + return pose.nearest(transformedPoses); + } + + public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { + return Collections.min( + Stream.of(AlgaeIntakeTargets.values()).toList(), + Comparator.comparing( + (AlgaeIntakeTargets other) -> + pose.getTranslation().getDistance(other.location.getTranslation())) + .thenComparing( + (AlgaeIntakeTargets other) -> + Math.abs(pose.getRotation().minus(other.location.getRotation()).getRadians()))); + } + } + + public enum CageTargets { + RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), + + BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), + BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), + BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); + + private static final List poses = + Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); + + private final Pose2d location; + private final Alliance alliance; + + private CageTargets(Pose2d location, Alliance alliance) { + this.location = location; + this.alliance = alliance; + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { + if (DriverStation.getAlliance().isPresent()) { + // If it's across the field, x > 8.76 on blue and x < 8.76 on red + return getOffsetClosestTarget( + robotPose, + (DriverStation.getAlliance().get() == Alliance.Blue && robotPose.getX() > 8.76) + || (DriverStation.getAlliance().get() == Alliance.Red && robotPose.getX() < 8.76)); + } + return getOffsetClosestTarget(robotPose, false); + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { + Pose2d nearestPose; + if (DriverStation.getAlliance().isPresent()) { + nearestPose = + robotPose.nearest( + Arrays.stream(values()) + .filter(target -> target.getAlliance() == DriverStation.getAlliance().get()) + .map(target -> target.getLocation()) + .toList()); + } else { + nearestPose = robotPose.nearest(poses); + } + if (far) { + return getFarRobotTargetLocation(nearestPose); + } else { + return getCloseRobotTargetLocation(nearestPose); + } + } + + public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + 0.087, + 0, + Rotation2d.kZero)); + } + + public static Pose2d getFarRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + -(Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) - 0.087, + 0, + Rotation2d.k180deg)); + } + + public Pose2d getLocation() { + return this.location; + } + + public Alliance getAlliance() { + return this.alliance; + } + } + + public enum CoralTargets { + // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls + // were extended beyond the coral station + // All angles from the center of the coral with 0° across the width of the field, counterclockwise + BLUE_A(new Pose2d(3.95, 4.20, Rotation2d.fromDegrees(180)), true), + BLUE_B(new Pose2d(3.95, 3.87, Rotation2d.fromDegrees(180)), false), + BLUE_C(new Pose2d(4.07, 3.66, Rotation2d.fromDegrees(240)), true), + BLUE_D(new Pose2d(4.35, 3.49, Rotation2d.fromDegrees(240)), false), + BLUE_E(new Pose2d(4.60, 3.50, Rotation2d.fromDegrees(300)), false), + BLUE_F(new Pose2d(4.88, 3.66, Rotation2d.fromDegrees(300)), true), + BLUE_G(new Pose2d(5.00, 3.86, Rotation2d.fromDegrees(0)), false), + BLUE_H(new Pose2d(5.00, 4.18, Rotation2d.fromDegrees(0)), true), + BLUE_I(new Pose2d(4.88, 4.41, Rotation2d.fromDegrees(60)), false), + BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), + BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), + BLUE_L(new Pose2d(4.06, 4.41, Rotation2d.fromDegrees(120)), false), + + RED_A(ChoreoAllianceFlipUtil.flip(BLUE_A.location), true), + RED_B(ChoreoAllianceFlipUtil.flip(BLUE_B.location), false), + RED_C(ChoreoAllianceFlipUtil.flip(BLUE_C.location), true), + RED_D(ChoreoAllianceFlipUtil.flip(BLUE_D.location), false), + RED_E(ChoreoAllianceFlipUtil.flip(BLUE_E.location), false), + RED_F(ChoreoAllianceFlipUtil.flip(BLUE_F.location), true), + RED_G(ChoreoAllianceFlipUtil.flip(BLUE_G.location), false), + RED_H(ChoreoAllianceFlipUtil.flip(BLUE_H.location), true), + RED_I(ChoreoAllianceFlipUtil.flip(BLUE_I.location), false), + RED_J(ChoreoAllianceFlipUtil.flip(BLUE_J.location), true), + RED_K(ChoreoAllianceFlipUtil.flip(BLUE_K.location), true), + RED_L(ChoreoAllianceFlipUtil.flip(BLUE_L.location), false); + + public final Pose2d location; + public final boolean leftHanded; + + private CoralTargets(Pose2d location, boolean leftHanded) { + this.location = location; + this.leftHanded = leftHanded; + } + + private static final List transformedPoses = + Arrays.stream(values()) + .map( + (CoralTargets targets) -> { + return CoralTargets.getRobotTargetLocation(targets.location); + }) + .toList(); + + public static Pose2d getRobotTargetLocation(Pose2d original) { + // 0.248 for trough + return original.transformBy( + new Transform2d( + 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Pose2d getBranchLocation(Pose2d transformed) { + // 0.248 for trough + return transformed.transformBy( + new Transform2d( + 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0)) + .inverse()); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getClosestTarget(Pose2d pose) { + return pose.nearest(transformedPoses); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { + return pose.nearest( + Arrays.stream(values()) + .filter((target) -> target.leftHanded == leftHandeed) + .map( + (CoralTargets targets) -> { + return CoralTargets.getRobotTargetLocation(targets.location); + }) + .toList()); + } + } + + public enum HumanPlayerTargets { + BLUE_RIGHT_OUTSIDE( + new Pose2d( + 1.6333351135253906, 0.6246773600578308, Rotation2d.fromRadians(0.9420001549844138))), + BLUE_RIGHT_MIDDLE( + new Pose2d( + 1.1213834285736084, 0.9940196871757507, Rotation2d.fromRadians(0.9940196871757508))), + BLUE_RIGHT_INSIDE( + new Pose2d( + 0.5838082432746887, 1.3407007455825806, Rotation2d.fromRadians(0.9420001549844138))), + + BLUE_LEFT_OUTSIDE( + new Pose2d( + 1.666144609451294, 7.431143760681152, Rotation2d.fromRadians(-0.9350057865774469))), + BLUE_LEFT_MIDDLE( + new Pose2d( + 1.179524540901184, 7.083498477935791, Rotation2d.fromRadians(-0.9350057865774469))), + BLUE_LEFT_INSIDE( + new Pose2d( + 0.6153400540351868, 6.673182487487793, Rotation2d.fromRadians(-0.9350057865774469))), + + RED_RIGHT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_OUTSIDE.location)), + RED_RIGHT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_MIDDLE.location)), + RED_RIGHT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_INSIDE.location)), + RED_LEFT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_OUTSIDE.location)), + RED_LEFT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_MIDDLE.location)), + RED_LEFT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_INSIDE.location)); + + public final Pose2d location; + + private HumanPlayerTargets(Pose2d location) { + this.location = location; + } + } + + public enum L1Targets { + BLUE_AB( + new Rectangle2d( + new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_CD( + new Rectangle2d( + new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_EF( + new Rectangle2d( + new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_GH( + new Rectangle2d( + new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_IJ( + new Rectangle2d( + new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_KL( + new Rectangle2d( + new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + + RED_AB( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_CD( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_EF( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_GH( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_IJ( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_KL( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)); + + public Rectangle2d line; + + private L1Targets(Rectangle2d line) { + this.line = line; + } + + private static final List transformedLines = + Arrays.stream(values()) + .map( + (L1Targets targets) -> { + return L1Targets.getRobotTargetLine(targets.line); + }) + .toList(); + + public static Rectangle2d getRobotTargetLine(Rectangle2d original) { + return original.transformBy( + new Transform2d( + Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Rectangle2d getNearestLine(Pose2d pose) { + // It feels like there should be a better way to do this + return new Rectangle2d( + pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS); + } + } + +} diff --git a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java deleted file mode 100644 index 105e35a4..00000000 --- a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -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.util.Units; -import frc.robot.Robot; -import frc.robot.Robot.AlgaeIntakeTarget; -import java.util.Arrays; -import java.util.Collections; -import java.util.Comparator; -import java.util.List; -import java.util.stream.Stream; - -public enum AlgaeIntakeTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls - // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise - BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), - BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), - BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), - BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AlgaeIntakeTarget.LOW), - BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AlgaeIntakeTarget.HIGH), - BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AlgaeIntakeTarget.LOW), - - RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location), AlgaeIntakeTarget.HIGH), - RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location), AlgaeIntakeTarget.LOW), - RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location), AlgaeIntakeTarget.HIGH), - RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location), AlgaeIntakeTarget.LOW), - RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location), AlgaeIntakeTarget.HIGH), - RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location), AlgaeIntakeTarget.LOW); - - public final Pose2d location; - public final AlgaeIntakeTarget height; - - private AlgaeIntakeTargets(Pose2d location, AlgaeIntakeTarget height) { - this.location = location; - this.height = height; - } - - private static final List transformedPoses = - Arrays.stream(values()) - .map( - (AlgaeIntakeTargets targets) -> { - return AlgaeIntakeTargets.getRobotTargetLocation(targets.location); - }) - .toList(); - - public static Pose2d getRobotTargetLocation(Pose2d original) { - return original.transformBy( - new Transform2d( - (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Pose2d getOffsetLocation(Pose2d original) { - return original.transformBy( - new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getClosestTargetPose(Pose2d pose) { - return pose.nearest(transformedPoses); - } - - public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { - return Collections.min( - Stream.of(AlgaeIntakeTargets.values()).toList(), - Comparator.comparing( - (AlgaeIntakeTargets other) -> - pose.getTranslation().getDistance(other.location.getTranslation())) - .thenComparing( - (AlgaeIntakeTargets other) -> - Math.abs(pose.getRotation().minus(other.location.getRotation()).getRadians()))); - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index f1644467..35379596 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -18,6 +18,9 @@ import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.CoralTargets; + import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/utils/autoaim/CageTargets.java b/src/main/java/frc/robot/utils/autoaim/CageTargets.java deleted file mode 100644 index f585a615..00000000 --- a/src/main/java/frc/robot/utils/autoaim/CageTargets.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -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.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum CageTargets { - RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), - RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), - RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), - - BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), - BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), - BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); - - private static final List poses = - Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); - - private final Pose2d location; - private final Alliance alliance; - - private CageTargets(Pose2d location, Alliance alliance) { - this.location = location; - this.alliance = alliance; - } - - public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { - if (DriverStation.getAlliance().isPresent()) { - // If it's across the field, x > 8.76 on blue and x < 8.76 on red - return getOffsetClosestTarget( - robotPose, - (DriverStation.getAlliance().get() == Alliance.Blue && robotPose.getX() > 8.76) - || (DriverStation.getAlliance().get() == Alliance.Red && robotPose.getX() < 8.76)); - } - return getOffsetClosestTarget(robotPose, false); - } - - public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { - Pose2d nearestPose; - if (DriverStation.getAlliance().isPresent()) { - nearestPose = - robotPose.nearest( - Arrays.stream(values()) - .filter(target -> target.getAlliance() == DriverStation.getAlliance().get()) - .map(target -> target.getLocation()) - .toList()); - } else { - nearestPose = robotPose.nearest(poses); - } - if (far) { - return getFarRobotTargetLocation(nearestPose); - } else { - return getCloseRobotTargetLocation(nearestPose); - } - } - - public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { - return pose.transformBy( - new Transform2d( - (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + 0.087, - 0, - Rotation2d.kZero)); - } - - public static Pose2d getFarRobotTargetLocation(Pose2d pose) { - return pose.transformBy( - new Transform2d( - -(Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) - 0.087, - 0, - Rotation2d.k180deg)); - } - - public Pose2d getLocation() { - return this.location; - } - - public Alliance getAlliance() { - return this.alliance; - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java b/src/main/java/frc/robot/utils/autoaim/CoralTargets.java deleted file mode 100644 index 04893800..00000000 --- a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum CoralTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls - // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise - BLUE_A(new Pose2d(3.95, 4.20, Rotation2d.fromDegrees(180)), true), - BLUE_B(new Pose2d(3.95, 3.87, Rotation2d.fromDegrees(180)), false), - BLUE_C(new Pose2d(4.07, 3.66, Rotation2d.fromDegrees(240)), true), - BLUE_D(new Pose2d(4.35, 3.49, Rotation2d.fromDegrees(240)), false), - BLUE_E(new Pose2d(4.60, 3.50, Rotation2d.fromDegrees(300)), false), - BLUE_F(new Pose2d(4.88, 3.66, Rotation2d.fromDegrees(300)), true), - BLUE_G(new Pose2d(5.00, 3.86, Rotation2d.fromDegrees(0)), false), - BLUE_H(new Pose2d(5.00, 4.18, Rotation2d.fromDegrees(0)), true), - BLUE_I(new Pose2d(4.88, 4.41, Rotation2d.fromDegrees(60)), false), - BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), - BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), - BLUE_L(new Pose2d(4.06, 4.41, Rotation2d.fromDegrees(120)), false), - - RED_A(ChoreoAllianceFlipUtil.flip(BLUE_A.location), true), - RED_B(ChoreoAllianceFlipUtil.flip(BLUE_B.location), false), - RED_C(ChoreoAllianceFlipUtil.flip(BLUE_C.location), true), - RED_D(ChoreoAllianceFlipUtil.flip(BLUE_D.location), false), - RED_E(ChoreoAllianceFlipUtil.flip(BLUE_E.location), false), - RED_F(ChoreoAllianceFlipUtil.flip(BLUE_F.location), true), - RED_G(ChoreoAllianceFlipUtil.flip(BLUE_G.location), false), - RED_H(ChoreoAllianceFlipUtil.flip(BLUE_H.location), true), - RED_I(ChoreoAllianceFlipUtil.flip(BLUE_I.location), false), - RED_J(ChoreoAllianceFlipUtil.flip(BLUE_J.location), true), - RED_K(ChoreoAllianceFlipUtil.flip(BLUE_K.location), true), - RED_L(ChoreoAllianceFlipUtil.flip(BLUE_L.location), false); - - public final Pose2d location; - public final boolean leftHanded; - - private CoralTargets(Pose2d location, boolean leftHanded) { - this.location = location; - this.leftHanded = leftHanded; - } - - private static final List transformedPoses = - Arrays.stream(values()) - .map( - (CoralTargets targets) -> { - return CoralTargets.getRobotTargetLocation(targets.location); - }) - .toList(); - - public static Pose2d getRobotTargetLocation(Pose2d original) { - // 0.248 for trough - return original.transformBy( - new Transform2d( - 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Pose2d getBranchLocation(Pose2d transformed) { - // 0.248 for trough - return transformed.transformBy( - new Transform2d( - 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0)) - .inverse()); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getClosestTarget(Pose2d pose) { - return pose.nearest(transformedPoses); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { - return pose.nearest( - Arrays.stream(values()) - .filter((target) -> target.leftHanded == leftHandeed) - .map( - (CoralTargets targets) -> { - return CoralTargets.getRobotTargetLocation(targets.location); - }) - .toList()); - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java b/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java deleted file mode 100644 index dbd9085f..00000000 --- a/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public enum HumanPlayerTargets { - BLUE_RIGHT_OUTSIDE( - new Pose2d( - 1.6333351135253906, 0.6246773600578308, Rotation2d.fromRadians(0.9420001549844138))), - BLUE_RIGHT_MIDDLE( - new Pose2d( - 1.1213834285736084, 0.9940196871757507, Rotation2d.fromRadians(0.9940196871757508))), - BLUE_RIGHT_INSIDE( - new Pose2d( - 0.5838082432746887, 1.3407007455825806, Rotation2d.fromRadians(0.9420001549844138))), - - BLUE_LEFT_OUTSIDE( - new Pose2d( - 1.666144609451294, 7.431143760681152, Rotation2d.fromRadians(-0.9350057865774469))), - BLUE_LEFT_MIDDLE( - new Pose2d( - 1.179524540901184, 7.083498477935791, Rotation2d.fromRadians(-0.9350057865774469))), - BLUE_LEFT_INSIDE( - new Pose2d( - 0.6153400540351868, 6.673182487487793, Rotation2d.fromRadians(-0.9350057865774469))), - - RED_RIGHT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_OUTSIDE.location)), - RED_RIGHT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_MIDDLE.location)), - RED_RIGHT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_INSIDE.location)), - RED_LEFT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_OUTSIDE.location)), - RED_LEFT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_MIDDLE.location)), - RED_LEFT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_INSIDE.location)); - - public final Pose2d location; - - private HumanPlayerTargets(Pose2d location) { - this.location = location; - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/L1Targets.java b/src/main/java/frc/robot/utils/autoaim/L1Targets.java deleted file mode 100644 index d06bbfe9..00000000 --- a/src/main/java/frc/robot/utils/autoaim/L1Targets.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rectangle2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum L1Targets { - BLUE_AB( - new Rectangle2d( - new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_CD( - new Rectangle2d( - new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_EF( - new Rectangle2d( - new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_GH( - new Rectangle2d( - new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_IJ( - new Rectangle2d( - new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_KL( - new Rectangle2d( - new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - - RED_AB( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_CD( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_EF( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_GH( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_IJ( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_KL( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)); - - public Rectangle2d line; - - private L1Targets(Rectangle2d line) { - this.line = line; - } - - private static final List transformedLines = - Arrays.stream(values()) - .map( - (L1Targets targets) -> { - return L1Targets.getRobotTargetLine(targets.line); - }) - .toList(); - - public static Rectangle2d getRobotTargetLine(Rectangle2d original) { - return original.transformBy( - new Transform2d( - Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Rectangle2d getNearestLine(Pose2d pose) { - // It feels like there should be a better way to do this - return new Rectangle2d( - pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS); - } -} From 79fc4f892b482a56d16ad29000f7d0779ea6859b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:51:29 -0700 Subject: [PATCH 091/154] delete intake template --- .../subsystems/intake/IntakePivotIO.java | 22 ----- .../subsystems/intake/IntakePivotIOReal.java | 94 ------------------- .../subsystems/intake/IntakePivotIOSim.java | 66 ------------- .../intake/IntakePivotSubsystem.java | 43 --------- .../intake/IntakeRollerSubsystem.java | 29 ------ 5 files changed, 254 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java deleted file mode 100644 index b752b562..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface IntakePivotIO { - @AutoLog - public static class IntakePivotIOInputs { - public double angularVelocityRotsPerSec = 0.0; - public Rotation2d position = new Rotation2d(); - public double tempDegreesC = 0.0; - public double supplyCurrentAmps = 0.0; - public double appliedVoltage = 0.0; - public double statorCurrentAmps = 0.0; - } - - void updateInputs(IntakePivotIOInputs inputs); - - void setMotorVoltage(double voltage); - - void setMotorPosition(Rotation2d targetPosition); -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java deleted file mode 100644 index 17000933..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java +++ /dev/null @@ -1,94 +0,0 @@ -package frc.robot.subsystems.intake; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; - -public class IntakePivotIOReal implements IntakePivotIO { - private final TalonFX motor = new TalonFX(14, "*"); - - private final StatusSignal angularVelocityRotsPerSec = motor.getVelocity(); - private final StatusSignal temp = motor.getDeviceTemp(); - private final StatusSignal supplyCurrentAmps = motor.getSupplyCurrent(); - private final StatusSignal statorCurrentAmps = motor.getStatorCurrent(); - private final StatusSignal appliedVoltage = motor.getMotorVoltage(); - private final StatusSignal motorPositionRotations = motor.getPosition(); - - private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0).withEnableFOC(true); - - public IntakePivotIOReal() { - var config = new TalonFXConfiguration(); - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - // Gains currently from the sim. Need to be updated with our own robot's - config.Slot0.kV = 0.543; - config.Slot0.kG = 0.3856; - config.Slot0.kS = 0.0; - config.Slot0.kP = 5.0; - config.Slot0.kI = 0.0; - config.Slot0.kD = 2.6; - - config.CurrentLimits.SupplyCurrentLimit = 20.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - - config.Feedback.SensorToMechanismRatio = IntakePivotSubsystem.PIVOT_RATIO; - - motor.getConfigurator().apply(config); - motor.optimizeBusUtilization(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - angularVelocityRotsPerSec, - temp, - supplyCurrentAmps, - statorCurrentAmps, - appliedVoltage, - motorPositionRotations); - } - - @Override - public void updateInputs(IntakePivotIOInputs inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRotsPerSec, - temp, - supplyCurrentAmps, - statorCurrentAmps, - appliedVoltage, - motorPositionRotations); - - inputs.angularVelocityRotsPerSec = angularVelocityRotsPerSec.getValueAsDouble(); - inputs.tempDegreesC = temp.getValue().in(Units.Celsius); - inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); - inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - } - - @Override - public void setMotorVoltage(double voltage) { - motor.setControl(voltageOut.withOutput(voltage)); - } - - @Override - public void setMotorPosition(Rotation2d targetPosition) { - motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java deleted file mode 100644 index 616bf549..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java +++ /dev/null @@ -1,66 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; - -public class IntakePivotIOSim implements IntakePivotIO { - // TODO: SET TO ACTUAL VALUES WHEN CAD IS FINISHED - // Taken from Citrus circuits 2024 CAD - private final SingleJointedArmSim intakePivotSim = - new SingleJointedArmSim( - DCMotor.getKrakenX60Foc(1), - IntakePivotSubsystem.PIVOT_RATIO, - 0.07, - Units.inchesToMeters(11.5), - IntakePivotSubsystem.MIN_ANGLE.getRadians(), - IntakePivotSubsystem.MAX_ANGLE.getRadians(), - true, - 0.0); - - // TODO: TUNE - private final ProfiledPIDController pivotPid = - new ProfiledPIDController(5.0, 0.0, 2.6, new TrapezoidProfile.Constraints(10.0, 10.0)); - // TODO: TUNE - private final ArmFeedforward pivotFf = new ArmFeedforward(0.0, 0.3856, 0.543); // 0.543 - - private double appliedVoltage = 0.0; - - @Override - public void updateInputs(IntakePivotIOInputs inputs) { - intakePivotSim.update(0.02); - - inputs.position = Rotation2d.fromRadians(intakePivotSim.getAngleRads()); - inputs.angularVelocityRotsPerSec = - RadiansPerSecond.of(intakePivotSim.getVelocityRadPerSec()).in(RotationsPerSecond); - inputs.statorCurrentAmps = intakePivotSim.getCurrentDrawAmps(); - inputs.supplyCurrentAmps = 0.0; - inputs.tempDegreesC = 0.0; - inputs.appliedVoltage = appliedVoltage; - } - - @Override - public void setMotorVoltage(double voltage) { - appliedVoltage = voltage; - intakePivotSim.setInputVoltage(MathUtil.clamp(voltage, -12, 12)); - } - - @Override - public void setMotorPosition(Rotation2d targetPosition) { - setMotorVoltage( - pivotPid.calculate(intakePivotSim.getAngleRads(), targetPosition.getRadians()) - + pivotFf.calculate(pivotPid.getSetpoint().position, pivotPid.getSetpoint().velocity)); - } - - public void setResetSimState() { - intakePivotSim.setState(0.0, 0.0); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java deleted file mode 100644 index f0289a74..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class IntakePivotSubsystem extends SubsystemBase { - // TODO: SET TO ACTUAL RATIO WHEN CAD IS FINISHED - public static double PIVOT_RATIO = 20.0; - public static Rotation2d RETRACTED_ANGLE = Rotation2d.fromDegrees(90); - public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(120); - public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); - - private IntakePivotIO io; - private IntakePivotIOInputsAutoLogged inputs = new IntakePivotIOInputsAutoLogged(); - - public IntakePivotSubsystem(IntakePivotIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake/Pivot", inputs); - } - - public Command setTargetAngle(Rotation2d target) { - return setTargetAngle(() -> target); - } - - public Command setTargetAngle(Supplier target) { - return this.runOnce( - () -> { - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Intake/PivotSetpoint", target.get()); - }) - .andThen(this.run(() -> io.setMotorPosition(target.get()))); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java deleted file mode 100644 index 92ba7039..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.roller.RollerIO; -import frc.robot.subsystems.roller.RollerSubsystem; - -public class IntakeRollerSubsystem extends RollerSubsystem { - // TODO: SET TO GOOD VALUE - static final double INTAKE_VELOCITY = 80.0; - - public IntakeRollerSubsystem(RollerIO io) { - super(io, "Intake/Roller"); - } - - /** Runs intake */ - public Command intake() { - return intake(INTAKE_VELOCITY); - } - - /** Runs intake at specified velocity */ - public Command intake(double velocity) { - return setVelocity(velocity); - } - - /** Runs intake in reverse */ - public Command outtake() { - return intake(-INTAKE_VELOCITY); - } -} From f66b9c8757dbca687fcca9336965bd26bf76ede8 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 01:51:44 -0700 Subject: [PATCH 092/154] update manipulator (wip) --- .../subsystems/ManipulatorSubsystem.java | 115 +++++++----------- 1 file changed, 42 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index c5eaa3cf..134c17cd 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -19,6 +19,8 @@ import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; import frc.robot.utils.Tracer; + +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -41,9 +43,11 @@ public class ManipulatorSubsystem extends RollerSubsystem { private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), secondBBInputs = new BeambreakIOInputsAutoLogged(); - private boolean bb1 = false; - private boolean bb2 = false; - @AutoLogOutput private boolean hasAlgae = false; + private boolean bb1Sim = false; + private boolean bb2Sim = false; + @AutoLogOutput private boolean hasAlgaeSim = false; + + private double stateVelocity = 0.0; private LinearFilter currentFilter = LinearFilter.movingAverage(20); private double currentFilterValue = 0.0; @@ -69,72 +73,17 @@ public void periodic() { Logger.processInputs(NAME + "/First Beambreak", firstBBInputs); Logger.processInputs(NAME + "/Second Beambreak", secondBBInputs); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Has Algae", hasAlgae); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Has Algae", hasAlgaeSim); if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput(NAME + "/Sim First Beambreak Override", bb1); + Logger.recordOutput(NAME + "/Sim First Beambreak Override", bb1Sim); if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput(NAME + "/Sim Second Beambreak Override", bb2); + Logger.recordOutput(NAME + "/Sim Second Beambreak Override", bb2Sim); currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); - - if (getFirstBeambreak() && !getSecondBeambreak()) { - Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); - zeroTimer.reset(); - } - - if (!getFirstBeambreak() && getSecondBeambreak()) { - // Number calculated from coral length, may need tuning - Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); - zeroTimer.reset(); - } } - /** For the old ee */ - @Deprecated - public Command index() { - return Commands.sequence( - setVelocity(9.0) - .until(() -> getFirstBeambreak() || getSecondBeambreak()) - .unless(() -> getFirstBeambreak()), - setVelocity(3.0).until(() -> getSecondBeambreak()).unless(() -> getSecondBeambreak()), - setVelocity(-3.0) - .until(() -> getFirstBeambreak() && !getSecondBeambreak()) - .unless(() -> zeroTimer.get() < 0.25), - // TODO tune timeout - // Commands.runOnce(() -> io.resetEncoder(0.0)), - Commands.run(() -> io.setPosition(Rotation2d.fromRotations(1.1))) - .until(() -> !getFirstBeambreak() && !getSecondBeambreak())); - } // TODO check if anything got lost in merge? - - public Command jog(double rotations) { - return Commands.sequence( - // this.runOnce(() -> io.resetEncoder(0.0)), - this.run( - () -> { - io.setPosition(Rotation2d.fromRotations(rotations)); - positionSetpoint = rotations; - })); - } - - public Command jog(DoubleSupplier rotations) { - return Commands.sequence( - // this.runOnce(() -> io.resetEncoder(0.0)), - this.run( - () -> { - io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())); - positionSetpoint = rotations.getAsDouble(); - })); - } - - public Command hold() { - return this.jog(() -> inputs.positionRotations) - .until(() -> true) - .andThen(this.run(() -> {})) - .until(() -> !MathUtil.isNear(positionSetpoint, inputs.positionRotations, 2.0)) - .repeatedly(); - } public void resetPosition(final double rotations) { io.resetEncoder(rotations); @@ -177,35 +126,55 @@ public Command intakeAlgae() { .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); } + public Command setStateVelocity(BooleanSupplier checkExtension) { + return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); + } + + public void setState(double vel) { + stateVelocity = vel; + } + public double getStatorCurrentAmps() { return currentFilterValue; } - public double getTimeSinceZero() { - return zeroTimer.get(); + public boolean hasAlgae() { // TODO icky + return getStatorCurrentAmps() > ALGAE_CURRENT_THRESHOLD || hasAlgaeSim; } public boolean getFirstBeambreak() { - return firstBBInputs.get || bb1; + return firstBBInputs.get || bb1Sim; } public boolean getSecondBeambreak() { - return secondBBInputs.get || bb2; + return secondBBInputs.get || bb2Sim; } - public void setFirstBeambreak(boolean state) { - bb1 = state; + public boolean eitherBeambreak() { + return getFirstBeambreak() || getSecondBeambreak(); } - public void setSecondBeambreak(boolean state) { - bb2 = state; + public boolean bothBeambreaks() { + return getFirstBeambreak() && getSecondBeambreak(); } - public void setHasAlgae(boolean state) { - hasAlgae = state; + public boolean neitherBeambreak() { + return !eitherBeambreak(); } - public boolean hasAlgae() { // TODO icky - return hasAlgae; + public void setSimFirstBeambreak(boolean b) { + bb1Sim = b; + } + + public void setSimSecondBeambreak(boolean b) { + bb2Sim = b; + } + + public void setSimHasAlgae(boolean state) { + hasAlgaeSim = state; + } + + public double getTimeSinceZero() { + return zeroTimer.get(); } } From b14e634ebf0da83d2a9bfffb1457f5c3ba4ce139 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 17:19:36 -0700 Subject: [PATCH 093/154] start redoing superstructure --- src/main/java/frc/robot/Autos.java | 18 +- src/main/java/frc/robot/Robot.java | 487 +++---- .../robot/subsystems/ExtensionKinematics.java | 239 --- .../robot/subsystems/ExtensionPathing.java | 243 ---- .../subsystems/ManipulatorSubsystem.java | 11 +- .../frc/robot/subsystems/Superstructure.java | 1293 ++++++----------- .../subsystems/swerve/SwerveSubsystem.java | 5 +- 7 files changed, 634 insertions(+), 1662 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/ExtensionKinematics.java delete mode 100644 src/main/java/frc/robot/subsystems/ExtensionPathing.java diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1f802ded..a7ed8c3d 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -182,7 +182,7 @@ public Command LOtoJ() { routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("LOtoJ").resetOdometry(), steps.get("LOtoJ").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -209,7 +209,7 @@ public Command LOtoJ() { routine .observe(steps.get("LtoPLM").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2))); return routine.cmd().alongWith(Commands.print("auto :(")); } @@ -230,7 +230,7 @@ public Command ROtoE() { routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("ROtoE").resetOdometry(), steps.get("ROtoE").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -243,7 +243,7 @@ public Command ROtoE() { routine .observe(steps.get("CtoPRM").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2))); return routine.cmd(); } @@ -463,7 +463,7 @@ public Command LOtoA() { // 2910 routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("LOtoA").resetOdometry(), steps.get("LOtoA").cmd())); routine .observe( @@ -477,7 +477,7 @@ public Command LOtoA() { // 2910 elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) && AutoAim.isInTolerance( swerve.getPose(), steps.get("AtoB").getInitialPose().get())), - Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)), + Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2)), steps.get("AtoB").cmd())); routine .observe( @@ -489,7 +489,7 @@ public Command LOtoA() { // 2910 Commands.runOnce( () -> { autoGroundCoralIntake = false; - Robot.setCurrentTarget(ReefTarget.L4); + Robot.setCoralTarget(ReefTarget.L4); })); routine @@ -690,13 +690,13 @@ public Command scoreAlgaeInAuto() { // oh good lord Commands.print("Scoring algae"), Commands.runOnce( () -> { - Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); + Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.BARGE); autoScore = true; }), Commands.waitUntil(() -> !manipulator.hasAlgae()) .alongWith( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) + ? Commands.runOnce(() -> manipulator.setSimHasAlgae(false)) : Commands.none()) .andThen( Commands.runOnce( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 072eb6be..64f1976c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -46,9 +46,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.ExtensionKinematics; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; -import frc.robot.subsystems.ExtensionPathing; +import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.Superstructure; @@ -74,12 +72,14 @@ import frc.robot.subsystems.vision.VisionIOSim; import frc.robot.subsystems.wrist.*; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.CageTargets; +import frc.robot.utils.FieldUtils.CoralTargets; +import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.Tracer; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.CageTargets; -import frc.robot.utils.autoaim.CoralTargets; -import frc.robot.utils.autoaim.L1Targets; + import java.util.HashMap; import java.util.Optional; import java.util.Set; @@ -125,53 +125,19 @@ private RobotHardware(SwerveConstants swerveConstants) { public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; - // for testing class loading - public static final ExtensionState test = - ExtensionPathing.getNearest(new ExtensionState(0.0, Rotation2d.kZero, Rotation2d.kZero)); public static enum ReefTarget { - L1( - ElevatorSubsystem.L1_EXTENSION_METERS, - 3.0, - WristSubsystem.WRIST_SCORE_L1_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L2( - ElevatorSubsystem.L2_EXTENSION_METERS, - -15.0, - WristSubsystem.WRIST_SCORE_L2_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L3( - ElevatorSubsystem.L3_EXTENSION_METERS, - -15.0, - WristSubsystem.WRIST_SCORE_L3_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L4( - ElevatorSubsystem.L4_EXTENSION_METERS, - -20.0, - WristSubsystem.WRIST_SCORE_L4_POS, - ShoulderSubsystem.SHOULDER_SCORE_L4_POS); - - public final double elevatorHeight; + L1(3.0, SuperState.PRE_L1), + L2(-15.0, SuperState.L2), + L3(-15.0, SuperState.L3), + L4(-20.0, SuperState.L4); + public final double outtakeSpeed; - public final Rotation2d wristAngle; - public final Rotation2d shoulderAngle; - - private ReefTarget( - double elevatorHeight, - double outtakeSpeed, - Rotation2d wristAngle, - Rotation2d shoulderAngle) { - this.elevatorHeight = elevatorHeight; - this.outtakeSpeed = outtakeSpeed; - this.wristAngle = wristAngle; - this.shoulderAngle = shoulderAngle; - } + public final SuperState state; - private ReefTarget(double elevatorHeight, Rotation2d wristAngle, Rotation2d shoulderAngle) { - this.elevatorHeight = elevatorHeight; - this.outtakeSpeed = 15.0; - this.wristAngle = wristAngle; - this.shoulderAngle = shoulderAngle; + private ReefTarget(double outtakeSpeed, SuperState state) { + this.outtakeSpeed = outtakeSpeed; + this.state = state; } } @@ -183,13 +149,13 @@ public static enum AlgaeIntakeTarget { } public static enum AlgaeScoreTarget { - NET, + BARGE, PROCESSOR } - @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4; + @AutoLogOutput private static ReefTarget coralTarget = ReefTarget.L4; @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; - @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.BARGE; private boolean leftHandedTarget = false; @AutoLogOutput private boolean killVisionIK = true; @@ -200,8 +166,123 @@ public static enum AlgaeScoreTarget { private static CANBusStatus canivoreStatus = canivore.getStatus(); - private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); - private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + private static final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); + private static final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + + public static Trigger preScoreReq = + driver + .rightTrigger() + .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) + .or( + () -> + swerve + .getPose() + .getTranslation() + .minus( + DriverStation.getAlliance().orElse(Alliance.Blue) + == Alliance.Blue + ? AutoAim.BLUE_REEF_CENTER + : AutoAim.RED_REEF_CENTER) + .getNorm() + < 3.25 + && DriverStation.isAutonomous()); + + @AutoLogOutput + public static Trigger scoreReq = driver + .rightTrigger() + .negate() + .and(() -> DriverStation.isTeleop()) + // .or( + // new Trigger( + // () -> { + // final var state = + // new ExtensionState( + // elevator.getExtensionMeters(), + // shoulder.getAngle(), + // wrist.getAngle()); + // final var branch = + // ExtensionKinematics.getBranchPose( + // swerve.getPose(), state, currentTarget); + // final var manipulatorPose = + // ExtensionKinematics.getManipulatorPose(swerve.getPose(), + // state); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Branch", branch); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput( + // "IK/Extension Check", + // manipulatorPose, + // manipulatorPose.transformBy( + // new Transform3d( + // Units.inchesToMeters(3.0), 0.0, 0.0, new + // Rotation3d()))); + // return false; + // // return branch + // // .getTranslation() + // // .getDistance(manipulatorPose.getTranslation()) + // // < Units.inchesToMeters(1.5) + // // || branch + // // .getTranslation() + // // .getDistance( + // // manipulatorPose + // // .transformBy( + // // new Transform3d( + // // Units.inchesToMeters(3.0), + // // 0.0, + // // 0.0, + // // new Rotation3d())) + // // .getTranslation()) + // // < Units.inchesToMeters(1.5); + // }) + // .debounce(0.15)) + // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) + .or(() -> Autos.autoScore && DriverStation.isAutonomous()); + + @AutoLogOutput + public static Trigger intakeAlgaeReq = driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()); + + @AutoLogOutput + public static Trigger intakeCoralReq = driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()); + + @AutoLogOutput(key = "Superstructure/Pre Climb Request") + public static Trigger preClimbReq = driver + .x() + .and(driver.pov(-1).negate()) + .debounce(0.5) + .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)); + + @AutoLogOutput(key = "Superstructure/Climb Confirm Request") + public static Trigger climbConfReq = driver.rightTrigger(); + + @AutoLogOutput(key = "Superstructure/Climb Cancel Request") + public static Trigger climbCancelReq = + driver + .y() + .debounce(0.5) + .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); + + //anti coral, anti algae, home + // driver.a(), + // driver.b(), + // driver.start(), + + @AutoLogOutput(key = "Superstructure/Rev Funnel Req") + public static Trigger revFunnelReq = operator.rightBumper(); + + @AutoLogOutput(key = "Superstructure/Force Funnel Req") + public static Trigger forceFunnelReq = operator.leftBumper(); + + @AutoLogOutput(key = "Superstructure/Force Index Req") //TODO what? + public static Trigger forceIndexReq = operator.povDown(); + + + //killVisionIK, DoubleSupplier coralAdjust) + // new Trigger(() -> killVisionIK) +// .or(() -> coralTarget == ReefTarget.L1) +// .or(() -> DriverStation.isAutonomous()), +// () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); // Create and configure a drivetrain simulation configuration private Optional driveTrainSimulationConfig = @@ -379,130 +460,8 @@ public static enum AlgaeScoreTarget { private final ClimberSubsystem climber = new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); - private final Superstructure superstructure = - new Superstructure( - elevator, - manipulator, - shoulder, - wrist, - funnel, - climber, - swerve::getPose, - swerve::getVelocityFieldRelative, - () -> currentTarget, - () -> algaeIntakeTarget, - () -> algaeScoreTarget, - driver - .rightTrigger() - .negate() - .and(() -> DriverStation.isTeleop()) - // .or( - // new Trigger( - // () -> { - // final var state = - // new ExtensionState( - // elevator.getExtensionMeters(), - // shoulder.getAngle(), - // wrist.getAngle()); - // final var branch = - // ExtensionKinematics.getBranchPose( - // swerve.getPose(), state, currentTarget); - // final var manipulatorPose = - // ExtensionKinematics.getManipulatorPose(swerve.getPose(), - // state); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Branch", branch); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput( - // "IK/Extension Check", - // manipulatorPose, - // manipulatorPose.transformBy( - // new Transform3d( - // Units.inchesToMeters(3.0), 0.0, 0.0, new - // Rotation3d()))); - // return false; - // // return branch - // // .getTranslation() - // // .getDistance(manipulatorPose.getTranslation()) - // // < Units.inchesToMeters(1.5) - // // || branch - // // .getTranslation() - // // .getDistance( - // // manipulatorPose - // // .transformBy( - // // new Transform3d( - // // Units.inchesToMeters(3.0), - // // 0.0, - // // 0.0, - // // new Rotation3d())) - // // .getTranslation()) - // // < Units.inchesToMeters(1.5); - // }) - // .debounce(0.15)) - // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore && DriverStation.isAutonomous()) - // .or( - // new Trigger( - // () -> - // AutoAim.isInToleranceCoral( - // swerve.getPose(), - // Units.inchesToMeters(1.5), - // Units.degreesToRadians(1.5)) - // && MathUtil.isNear( - // 0, - // Math.hypot( - // swerve.getVelocityRobotRelative().vxMetersPerSecond, - // swerve.getVelocityRobotRelative().vyMetersPerSecond), - // AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - // && MathUtil.isNear( - // 0.0, - // swerve.getVelocityRobotRelative().omegaRadiansPerSecond, - // 3.0) - // && currentTarget != ReefTarget.L4 - // && currentTarget != ReefTarget.L1) - // .debounce(0.08) - // .and(() -> swerve.hasFrontTags)) - , - driver - .rightTrigger() - .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) - .or( - () -> - swerve - .getPose() - .getTranslation() - .minus( - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance.Blue - ? AutoAim.BLUE_REEF_CENTER - : AutoAim.RED_REEF_CENTER) - .getNorm() - < 3.25 - && DriverStation.isAutonomous()), - driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()), - driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()), - driver - .x() - .and(driver.pov(-1).negate()) - .debounce(0.5) - .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)), - driver.rightTrigger(), - driver - .y() - .debounce(0.5) - .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)), - driver.a(), - driver.b(), - driver.start(), - operator.rightBumper(), - operator.leftBumper(), - operator.povDown(), - new Trigger(() -> killVisionIK) - .or(() -> currentTarget == ReefTarget.L1) - .or(() -> DriverStation.isAutonomous()), - () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); + private final Superstructure superstructure = + new Superstructure(elevator, shoulder, wrist, manipulator, funnel, climber); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -523,7 +482,7 @@ public static enum AlgaeScoreTarget { new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d shoulderLigament = new LoggedMechanismLigament2d( - "Arm", Units.inchesToMeters(15.7), ShoulderSubsystem.SHOULDER_RETRACTED_POS.getDegrees()); + "Arm", Units.inchesToMeters(15.7), 90.0); private final LoggedMechanismLigament2d wristLigament = new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); @@ -601,24 +560,6 @@ public Robot() { autos = new Autos(swerve, manipulator, funnel, elevator, shoulder, wrist); autoChooser.addDefaultOption("None", autos.getNoneAuto()); - SmartDashboard.putData( - "Run Elevator Sysid", - elevator - .runSysid() - .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); - - SmartDashboard.putData( - "Step Elevator Current", - elevator - .setCurrent(60.0) - .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); - - SmartDashboard.putData( - "Check Clear", - Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))); - SmartDashboard.putData( "Manual Zero Extension", Commands.runOnce( @@ -628,60 +569,6 @@ public Robot() { }) .ignoringDisable(true)); - System.out.println("Node Count " + ExtensionPathing.graph.nodes().size()); - - SmartDashboard.putData( - "Traverse Extension Graph", - superstructure - .extendWithClearance( - () -> - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .until( - () -> - elevator.isNearExtension(ElevatorSubsystem.HP_EXTENSION_METERS) - && shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) - .andThen( - Commands.sequence( - ExtensionPathing.graph.nodes().stream() - .map( - (node) -> - superstructure - .extendWithClearance(() -> node) - .alongWith( - Commands.print("Traversing to " + node), - Commands.runOnce( - () -> Logger.recordOutput("Traversal Target", node))) - .until( - () -> - elevator.isNearExtension(node.elevatorHeightMeters()) - && shoulder.isNearAngle(node.shoulderAngle()) - && wrist.isNearAngle(node.wristAngle())) - .finallyDo(() -> System.out.println("done")) - .andThen( - Commands.waitSeconds(0.5), - superstructure - .extendWithClearance( - () -> - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .alongWith(Commands.print("Retracting")) - .until( - () -> - elevator.isNearExtension( - ElevatorSubsystem.HP_EXTENSION_METERS) - && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle( - WristSubsystem.WRIST_HP_POS)), - Commands.waitSeconds(0.5))) - .toArray(Command[]::new)))); - // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); @@ -761,27 +648,34 @@ public Robot() { } }) .ignoringDisable(true)); - elevator.setDefaultCommand( - Commands.sequence( - elevator.runCurrentZeroing().onlyIf(() -> !elevator.hasZeroed), - elevator.setExtension(0.0).until(() -> elevator.isNearExtension(0.0)), - elevator.setVoltage(0.0)) - .withName("Elevator Default Command")); - - manipulator.setDefaultCommand(manipulator.hold()); - - shoulder.setDefaultCommand(shoulder.hold()); - - wrist.setDefaultCommand(wrist.hold()); - - funnel.setDefaultCommand(funnel.setVoltage(0.0)); - - climber.setDefaultCommand(climber.setPosition(0.0)); +elevator.setDefaultCommand(elevator.setStateExtension()); + shoulder.setDefaultCommand(shoulder.setStateAngle()); + wrist.setDefaultCommand(wrist.setStateAngle()); + manipulator.setDefaultCommand(manipulator.setStateVelocity(superstructure::atExtension)); + funnel.setDefaultCommand( + funnel.setRollerVoltage( + () -> + superstructure.getState() == SuperState.IDLE && revFunnelReq.getAsBoolean() + ? -2.0 + : (forceFunnelReq.getAsBoolean() + || (Stream.of(FieldUtils.HumanPlayerTargets.values()) + .map( + (t) -> + t.location.minus(swerve.getPose()).getTranslation().getNorm()) //TODO pose needs to be changed + .min(Double::compare) + .get() + < 1.0) + ? 1.0 + : 0.0))); // at what point do ternary operators do more harm than good + climber.setDefaultCommand( + climber.setPosition( + superstructure.getState().climberPosition, + superstructure.getState().climberSpeed)); // why does it need to be slow leds.setDefaultCommand( Commands.either( leds.setBlinkingCmd( - () -> LEDSubsystem.getReefTargetColor(currentTarget), + () -> LEDSubsystem.getReefTargetColor(coralTarget), () -> superstructure.getState() == SuperState.IDLE ? Color.kBlack @@ -816,7 +710,7 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget != ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -840,7 +734,7 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike() && currentTarget == ReefTarget.L1) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.alignToLine( @@ -992,7 +886,7 @@ public Robot() { () -> superstructure.getState() == SuperState.READY_ALGAE || superstructure.getState() == SuperState.PRE_NET) - .and(() -> algaeScoreTarget == AlgaeScoreTarget.NET) + .and(() -> algaeScoreTarget == AlgaeScoreTarget.BARGE) .whileTrue( Commands.parallel( AutoAim.translateToXCoord( @@ -1034,7 +928,7 @@ public Robot() { driver .povRight() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setHasAlgae(!manipulator.hasAlgae()))); + .onTrue(Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae()))); RobotModeTriggers.autonomous() .and(() -> ROBOT_TYPE == RobotType.SIM) @@ -1055,7 +949,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L1; + coralTarget = ReefTarget.L1; algaeIntakeTarget = AlgaeIntakeTarget.GROUND; })); operator @@ -1063,7 +957,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L2; + coralTarget = ReefTarget.L2; algaeIntakeTarget = AlgaeIntakeTarget.LOW; })); operator @@ -1071,7 +965,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L3; + coralTarget = ReefTarget.L3; algaeIntakeTarget = AlgaeIntakeTarget.HIGH; })); operator @@ -1079,11 +973,11 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L4; + coralTarget = ReefTarget.L4; algaeIntakeTarget = AlgaeIntakeTarget.STACK; })); - operator.leftTrigger().onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.NET)); + operator.leftTrigger().onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.BARGE)); operator .rightTrigger() @@ -1103,7 +997,7 @@ public Robot() { () -> LEDSubsystem.getAlgaeIntakeTargetColor(algaeIntakeTarget), () -> LEDSubsystem.getAlgaeScoringTargetColor( - algaeScoreTarget == AlgaeScoreTarget.NET), + algaeScoreTarget == AlgaeScoreTarget.BARGE), () -> Color.kBlack, 5.0)); // heading reset @@ -1144,15 +1038,6 @@ public Robot() { Stream.of(AlgaeIntakeTargets.values()) .map((target) -> AlgaeIntakeTargets.getRobotTargetLocation(target.location)) .toArray(Pose2d[]::new)); - - Logger.recordOutput("IK/L1 FK Pose", ExtensionKinematics.L1_POSE); - System.out.println("ExtensionKinematics.L1_POSE: " + ExtensionKinematics.L1_POSE); - Logger.recordOutput("IK/L2 FK Pose", ExtensionKinematics.L2_POSE); - System.out.println("ExtensionKinematics.L2_POSE: " + ExtensionKinematics.L2_POSE); - Logger.recordOutput("IK/L3 FK Pose", ExtensionKinematics.L3_POSE); - System.out.println("ExtensionKinematics.L3_POSE: " + ExtensionKinematics.L3_POSE); - Logger.recordOutput("IK/L4 FK Pose", ExtensionKinematics.L4_POSE); - System.out.println("ExtensionKinematics.L4_POSE: " + ExtensionKinematics.L4_POSE); } private void addAutos() { @@ -1215,7 +1100,7 @@ public void robotPeriodic() { } if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Targets/Reef Target", currentTarget); + Logger.recordOutput("Targets/Reef Target", coralTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Targets/Algae Intake Target", algaeIntakeTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) @@ -1346,38 +1231,30 @@ public void robotPeriodic() { state = superstructure::getState; } - public static void setCurrentCoralTarget(ReefTarget target) { - currentTarget = target; + public static void setCoralTarget(ReefTarget target) { + coralTarget = target; } - public ReefTarget getCurrentCoralTarget() { - return currentTarget; + public static ReefTarget getCoralTarget() { + return coralTarget; } - public static void setCurrentAlgaeIntakeTarget(AlgaeIntakeTarget target) { + public static void setAlgaeIntakeTarget(AlgaeIntakeTarget target) { algaeIntakeTarget = target; } - public AlgaeIntakeTarget getCurrentAlgaeIntakeTarget() { + public static AlgaeIntakeTarget getAlgaeIntakeTarget() { return algaeIntakeTarget; } - public static void setCurrentAlgaeScoreTarget(AlgaeScoreTarget target) { + public static void setAlgaeScoreTarget(AlgaeScoreTarget target) { algaeScoreTarget = target; } - public AlgaeScoreTarget getCurrentAlgaeScoreTarget() { + public static AlgaeScoreTarget getAlgaeScoreTarget() { return algaeScoreTarget; } - public static void setCurrentTarget(ReefTarget target) { - currentTarget = target; - } - - public ReefTarget getCurrentTarget() { - return currentTarget; - } - @Override public void disabledInit() {} diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java deleted file mode 100644 index f4f29db9..00000000 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ /dev/null @@ -1,239 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -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.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Robot.ReefTarget; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.shoulder.ShoulderSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.CoralTargets; -import java.util.function.Supplier; - -public class ExtensionKinematics { - - // These need to be here bc their main constants arent loaded when we need the constants in this - // class - private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - static final Transform2d IK_WRIST_TO_CORAL = - new Transform2d( - Units.inchesToMeters(12.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); - private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - - // Not super accurate bc of whack - public static final Pose2d L1_POSE = - new Pose2d(0.33, 0.50, Rotation2d.fromDegrees(20.0)); // solveFK(L1_EXTENSION); - public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); - public static final ExtensionState L2_EXTENSION = - new ExtensionState( - 0.23 + Units.inchesToMeters(1.5), - Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20 - 2)), - Rotation2d.fromRadians(2.447)); - public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); - public static final ExtensionState L3_EXTENSION = - new ExtensionState( - 0.60 + Units.inchesToMeters(2.0), - Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(6)), - Rotation2d.fromRadians(2.427)); - public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); - public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(115.0)); - - public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); - - public static final ExtensionState LOW_ALGAE_EXTENSION = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - public static final Pose2d LOW_ALGAE_POSE = solveFK(LOW_ALGAE_EXTENSION); - - public static final ExtensionState HIGH_ALGAE_EXTENSION = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - public static final Pose2d HIGH_ALGAE_POSE = solveFK(HIGH_ALGAE_EXTENSION); - - public record ExtensionState( - double elevatorHeightMeters, Rotation2d shoulderAngle, Rotation2d wristAngle) {} - - private ExtensionKinematics() {} - - /** - * @param target pose where +x is robot +x from elevator, +y is robot +z from elevator min, and - * rotation is coral angle from horizontal - */ - public static ExtensionState solveIK(Pose2d target) { - // Offset wrist pose from target - final var wristPose = target.transformBy(IK_WRIST_TO_CORAL.inverse()); - // Find shoulder angle from needed horizontal extension - var shoulderAngle = Math.acos(wristPose.getX() / ARM_LENGTH_METERS); - // Set angle to horizontal if we can't reach - if (Double.isNaN(shoulderAngle)) shoulderAngle = 0.0; - // Elevator goes to remaining needed height - var elevatorHeight = - wristPose - .getTranslation() - .minus( - new Translation2d( - ARM_LENGTH_METERS * Math.cos(shoulderAngle), - ARM_LENGTH_METERS * Math.sin(shoulderAngle))) - .getY(); - // If we're extending higher than we can reach, prioritize matching Z instead of X - if (elevatorHeight > MAX_EXTENSION_METERS) { - elevatorHeight = MAX_EXTENSION_METERS - Units.inchesToMeters(1.0); - shoulderAngle = Math.asin((wristPose.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); - // Limit shoulder angle - if (Double.isNaN(shoulderAngle) || shoulderAngle > Units.degreesToRadians(85.0)) { - shoulderAngle = Units.degreesToRadians(85.0); - } - } - - return new ExtensionState( - MathUtil.clamp(elevatorHeight, 0.0, ElevatorSubsystem.MAX_EXTENSION_METERS), - Rotation2d.fromRadians(shoulderAngle), - Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 120.0))); - } - - public static Pose2d solveFK(ExtensionState state) { - return new Pose2d( - state.shoulderAngle().getCos() * ARM_LENGTH_METERS, - state.elevatorHeightMeters() + state.shoulderAngle().getSin() * ARM_LENGTH_METERS, - state.wristAngle()) - .transformBy(IK_WRIST_TO_CORAL); - } - - public static ExtensionState getPoseCompensatedExtension(Pose2d pose, ExtensionState target) { - final var fk = ExtensionKinematics.solveFK(target); - final var diff = pose.minus(CoralTargets.getClosestTarget(pose)); - final var adjustedFk = new Pose2d(fk.getX() - diff.getX(), fk.getY(), fk.getRotation()); - return ExtensionKinematics.solveIK(adjustedFk); - } - - public static Pose3d getManipulatorPose(Pose2d robotPose, ExtensionState state) { - final var fk = solveFK(state); - return new Pose3d( - robotPose.transformBy( - new Transform2d( - fk.getX() + ElevatorSubsystem.X_OFFSET_METERS, 0.0, Rotation2d.kZero))) - .transformBy( - new Transform3d( - 0, - 0, - fk.getY() + ElevatorSubsystem.Z_OFFSET_METERS, - new Rotation3d(0, -state.wristAngle().getRadians(), 0))); - } - - public static Pose3d getBranchPose(Pose2d pose, ExtensionState state, ReefTarget level) { - return new Pose3d(CoralTargets.getClosestTarget(pose)) - .transformBy( - new Transform3d( - ElevatorSubsystem.X_OFFSET_METERS - + switch (level) { - case L1 -> L1_POSE.getX(); - case L2 -> L2_POSE.getX(); - case L3 -> L3_POSE.getX(); - case L4 -> L4_POSE.getX(); - }, - 0, - ElevatorSubsystem.Z_OFFSET_METERS - + switch (level) { - case L1 -> L1_POSE.getY(); - case L2 -> L2_POSE.getY(); - case L3 -> L3_POSE.getY(); - case L4 -> L4_POSE.getY(); - }, - new Rotation3d())); - } - - public static Command holdStateCommand( - ElevatorSubsystem elevator, - ShoulderSubsystem shoulder, - WristSubsystem wrist, - Supplier target) { - final LinearFilter elevatorFilter = LinearFilter.movingAverage(8); - final LinearFilter shoulderFilter = LinearFilter.movingAverage(8); - final LinearFilter wristFilter = LinearFilter.movingAverage(8); - return Commands.runOnce( - () -> { - elevatorFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters() - }, - new double[0]); - shoulderFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations() - }, - new double[0]); - wristFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations() - }, - new double[0]); - }) - .andThen( - Commands.parallel( - elevator.setExtension( - () -> elevatorFilter.calculate(target.get().elevatorHeightMeters())), - shoulder.setTargetAngle( - () -> - Rotation2d.fromRotations( - shoulderFilter.calculate(target.get().shoulderAngle().getRotations()))), - wrist.setTargetAngle( - () -> - Rotation2d.fromRotations( - wristFilter.calculate(target.get().wristAngle().getRotations()))))); - } - - public static ExtensionState getExtensionForLevel(ReefTarget target) { - return switch (target) { - case L2 -> ExtensionKinematics.L2_EXTENSION; - case L3 -> ExtensionKinematics.L3_EXTENSION; - case L4 -> ExtensionKinematics.L4_EXTENSION; - default -> // shouldnt be reachable - new ExtensionState( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS); - }; - } -} diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java deleted file mode 100644 index b445fc03..00000000 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ /dev/null @@ -1,243 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.google.common.collect.Sets; -import com.google.common.graph.GraphBuilder; -import com.google.common.graph.MutableGraph; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.shoulder.ShoulderSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Set; - -public class ExtensionPathing { - public static final MutableGraph graph = - GraphBuilder.undirected().allowsSelfLoops(true).build(); - // TODO make this cache distances so we can do partial caches - private static final Map, List> cache = - new HashMap<>(); - - static { - final var hp = - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS); - graph.addNode(hp); - final var tucked = - new ExtensionState( - 0.0, - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(tucked); - graph.putEdge(hp, tucked); - final var l2Tucked = - new ExtensionState( - ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l2Tucked); - graph.putEdge(tucked, l2Tucked); - final var l3Tucked = - new ExtensionState( - ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l3Tucked); - graph.putEdge(tucked, l3Tucked); - graph.putEdge(l3Tucked, l2Tucked); - final var l3 = ExtensionKinematics.L3_EXTENSION; - graph.addNode(l3); - graph.putEdge(l3, l3Tucked); - final var l4Tucked = - new ExtensionState( - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l4Tucked); - graph.putEdge(tucked, l4Tucked); - graph.putEdge(l4Tucked, l3Tucked); - graph.putEdge(l4Tucked, l2Tucked); - final var l4TuckedOut = - new ExtensionState( - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), - Rotation2d.fromDegrees(25.0), - Rotation2d.fromDegrees(120.0)); - graph.addNode(l4TuckedOut); - graph.putEdge(l4Tucked, l4TuckedOut); - - final var betweenTucked = - new ExtensionState(0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS); - graph.addNode(betweenTucked); - graph.putEdge(tucked, betweenTucked); - - final var untucked = - new ExtensionState( - 0.0, ShoulderSubsystem.SHOULDER_CLEARANCE_POS, WristSubsystem.WRIST_CLEARANCE_POS); - graph.addNode(untucked); - graph.putEdge(betweenTucked, untucked); - - final var preCoralGround = - new ExtensionState( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - Rotation2d.kCCW_90deg); - graph.addNode(preCoralGround); - graph.putEdge(tucked, preCoralGround); - - final var coralGround = - new ExtensionState( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - WristSubsystem.WRIST_CORAL_GROUND); - graph.addNode(coralGround); - graph.putEdge(preCoralGround, coralGround); - - final var retracted = - new ExtensionState( - 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS); - graph.addNode(retracted); - graph.putEdge(untucked, retracted); - graph.putEdge(betweenTucked, retracted); - - final var l1 = ExtensionKinematics.L1_EXTENSION; - graph.addNode(l1); - graph.putEdge(l1, betweenTucked); - - final var algaeGround = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS); - graph.addNode(algaeGround); - graph.putEdge(algaeGround, betweenTucked); - graph.putEdge(algaeGround, untucked); - - final var algaeLow = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - graph.addNode(algaeLow); - graph.putEdge(betweenTucked, algaeLow); - - final var algaeHigh = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - graph.addNode(algaeHigh); - graph.putEdge(betweenTucked, algaeHigh); - graph.putEdge(algaeLow, algaeHigh); - - graph.putEdge(l4Tucked, algaeHigh); - graph.putEdge(l4Tucked, algaeLow); - - final var algaeHalfTucked = - new ExtensionState( - Units.inchesToMeters(27.6), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(algaeHalfTucked); - graph.putEdge(algaeHalfTucked, algaeLow); - graph.putEdge(algaeHalfTucked, algaeHigh); - } - - private ExtensionPathing() {} - - public static ExtensionState getNearest(ExtensionState state) { - return graph.nodes().stream() - .min( - (a, b) -> { - var aFK = ExtensionKinematics.solveFK(a); - var bFK = ExtensionKinematics.solveFK(b); - var stateFK = ExtensionKinematics.solveFK(state); - return (int) - (1000 - * (aFK.getTranslation().getDistance(stateFK.getTranslation()) - + Math.abs( - aFK.getRotation().getRotations() - - stateFK.getRotation().getRotations())) - - 1000 - * (bFK.getTranslation().getDistance(stateFK.getTranslation()) - + Math.abs( - bFK.getRotation().getRotations() - - stateFK.getRotation().getRotations()))); - }) - .get(); - } - - private record TotalMotion(double elevator, double shoulderRotations, double wristRotations) { - public TotalMotion plus(double elevator, double shoulderRotations, double wristRotations) { - return new TotalMotion( - this.elevator + elevator, - this.shoulderRotations + shoulderRotations, - this.wristRotations + wristRotations); - } - } - - private static Pair, TotalMotion> search( - ExtensionState current, ExtensionState target, Set visited) { - if (current == target) { - List result = new ArrayList<>(); - result.add(target); - return Pair.of(result, new TotalMotion(0, 0, 0)); - } - - final var edges = Sets.difference(graph.successors(current), visited); - final List, TotalMotion>> result = new ArrayList<>(); - for (var edge : edges) { - final var path = search(edge, target, Sets.union(visited, Set.of(current))); - if (path != null) result.add(path); - } - if (result.size() == 0) return null; - final var best = - result.stream() - .min( - Comparator.comparing( - (Pair, TotalMotion> s) -> s.getFirst().size()) - .thenComparing( - (Pair, TotalMotion> s) -> s.getSecond().elevator) - .thenComparing((s) -> s.getSecond().wristRotations) - .thenComparing((s) -> s.getSecond().shoulderRotations)) - .get(); - final List newList = new ArrayList<>(); - newList.add(current); - newList.addAll(best.getFirst()); - return Pair.of( - newList, - best.getSecond() - .plus( - Math.abs( - current.elevatorHeightMeters() - best.getFirst().get(0).elevatorHeightMeters()), - Math.abs( - current.shoulderAngle().getRotations() - - best.getFirst().get(0).shoulderAngle().getRotations()), - Math.abs( - current.wristAngle().getRotations() - - best.getFirst().get(0).wristAngle().getRotations()))); - } - - public static List getPath(ExtensionState current, ExtensionState target) { - final var nearestCurrent = getNearest(current); - final var nearestTarget = getNearest(target); - final var path = - cache.containsKey(Pair.of(nearestCurrent, nearestTarget)) - ? cache.get(Pair.of(nearestCurrent, nearestTarget)) - : search(nearestCurrent, nearestTarget, Set.of()).getFirst(); - path.add(target); - cache.putIfAbsent(Pair.of(nearestCurrent, nearestTarget), path); - return path; - } -} diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 134c17cd..b3e800a3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -28,12 +28,13 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; + public static final double MAX_VELOCITY = 20; //holy cooked + public static final double CORAL_INTAKE_VELOCITY = -18.0; public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; public static final double ALGAE_CURRENT_THRESHOLD = 6.0; - public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; public static final double CORAL_HOLD_POS = 0.6; @@ -95,26 +96,26 @@ public Command intakeCoral() { public Command intakeCoralAir(double vel) { return Commands.sequence( - setVelocity(vel) + setRollerVelocity(vel) .until(() -> getSecondBeambreak()) .finallyDo( () -> { io.setPosition(Rotation2d.fromRotations(0.63)); positionSetpoint = 0.63; }), - setVoltage(2.0).until(() -> !getFirstBeambreak()), + setRollerVoltage(2.0).until(() -> !getFirstBeambreak()), jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); } public Command intakeCoral(double vel) { return Commands.sequence( - setVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)), + setRollerVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)), Commands.runOnce( () -> { io.setPosition(Rotation2d.fromRotations(0.5)); positionSetpoint = 0.5; }), - setVelocity(1.0).until(() -> !getFirstBeambreak()), + setRollerVelocity(1.0).until(() -> !getFirstBeambreak()), jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3bdba5a6..3af81e40 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -17,15 +17,16 @@ import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.Robot.ReefTarget; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.HumanPlayerTargets; -import frc.robot.utils.autoaim.L1Targets; import java.util.HashMap; import java.util.List; import java.util.Map; @@ -38,88 +39,159 @@ import org.littletonrobotics.junction.Logger; public class Superstructure { - public static enum SuperState { - IDLE, - HOME, - INTAKE_CORAL_GROUND, - READY_CORAL, - SPIT_CORAL, - PRE_L1, - PRE_L2, - PRE_L3, - PRE_L4, - SCORE_CORAL, - ANTI_CORAL_JAM, - ANTI_ALGAE_JAM, - INTAKE_ALGAE_GROUND, - INTAKE_ALGAE_HIGH, - INTAKE_ALGAE_LOW, - INTAKE_ALGAE_STACK, - CHECK_ALGAE, - READY_ALGAE, - SPIT_ALGAE, - PRE_PROCESSOR, - PRE_NET, - SCORE_ALGAE_NET, - SCORE_ALGAE_PROCESSOR, - PRE_CLIMB, - CLIMB - } - - private final Supplier pose; - private final Supplier chassisVel; - private final Supplier reefTarget; - private final Supplier algaeIntakeTarget; - private final Supplier algaeScoreTarget; - - @AutoLogOutput(key = "Superstructure/Pre Score Request") - private final Trigger preScoreReq; - - @AutoLogOutput(key = "Superstructure/Score Request") - private final Trigger scoreReq; - - @AutoLogOutput(key = "Superstructure/Algae Intake Request") - private final Trigger intakeAlgaeReq; - - @AutoLogOutput(key = "Superstructure/Coral Intake Request") - private final Trigger intakeCoralReq; - - @AutoLogOutput(key = "Superstructure/Pre Climb Request") - private final Trigger preClimbReq; - - @AutoLogOutput(key = "Superstructure/Climb Confirm Request") - private final Trigger climbConfReq; - - @AutoLogOutput(key = "Superstructure/Climb Cancel Request") - private final Trigger climbCancelReq; - - @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") - private final Trigger antiCoralJamReq; - - @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") - private final Trigger antiAlgaeJamReq; - - @AutoLogOutput(key = "Superstructure/Home Request") - private final Trigger homeReq; - - @AutoLogOutput(key = "Superstructure/Rev Funnel Req") - private final Trigger revFunnelReq; - - @AutoLogOutput(key = "Superstructure/Force Funnel Req") - private final Trigger forceFunnelReq; - - @AutoLogOutput(key = "Superstructure/Force Funnel Req") - private final Trigger forceIndexReq; - - @AutoLogOutput(key = "Superstructure/Kill Vision and IK") - private final Trigger killVisionIK; + /** + * We should have a state for every single "pose" the robot will hit. Hopefully we can get named + * positions set up in cad to make this easier? + */ + public enum SuperState { + IDLE(ElevatorState.HP, ShoulderState.HP, WristState.HP, -7.0), + PRE_INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.PRE_INTAKE_CORAL_GROUND, + WristState.PRE_INTAKE_CORAL_GROUND, + -18.0), + INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + -18.0), + POST_INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.PRE_INTAKE_CORAL_GROUND, + WristState.PRE_INTAKE_CORAL_GROUND, + 0.0), + READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), + PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.PRE_L1, 0.0), + L1(ElevatorState.L1, ShoulderState.L1, WristState.PRE_L1, 3.0), + POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.PRE_L1, 0.0), + PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + L2(ElevatorState.L2, ShoulderState.L2, WristState.L2, -15.0), + POST_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), + POST_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + PRE_PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), + PRE_L4( + ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), // worried about shoulder here + L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + POST_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), + POST_POST_L4( + ElevatorState.HP, ShoulderState.L4, WristState.HP, 0.0), // like do we see the vision + PRE_PRE_INTAKE_ALGAE( + ElevatorState.HP, + ShoulderState.PRE_INTAKE_ALGAE_REEF, + WristState.PRE_INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + PRE_INTAKE_ALGAE( + ElevatorState.HP, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_HIGH( + ElevatorState.INTAKE_ALGAE_HIGH, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_LOW( + ElevatorState.INTAKE_ALGAE_LOW, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_STACK( + ElevatorState.INTAKE_ALGAE_STACK, + ShoulderState.INTAKE_ALGAE_STACK, + WristState.INTAKE_ALGAE_STACK, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_GROUND( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + ManipulatorSubsystem.MAX_VELOCITY), // ?? lmao + READY_ALGAE( + ElevatorState.HP, + ShoulderState.READY_ALGAE, + WristState.READY_ALGAE, + ManipulatorSubsystem.MAX_VELOCITY * 1.0 / 12.0), + PRE_BARGE( + ElevatorState.BARGE, + ShoulderState.PRE_BARGE, + WristState.PRE_BARGE, + ManipulatorSubsystem.MAX_VELOCITY * 3.0 / 12.0), + BARGE( + ElevatorState.BARGE, + ShoulderState.SCORE_BARGE, + WristState.SCORE_BARGE, + ManipulatorSubsystem.MAX_VELOCITY * -13.0 / 12.0), // TODO HELLO??? + POST_BARGE(ElevatorState.BARGE, ShoulderState.READY_ALGAE, WristState.PRE_BARGE, 0.0), + POST_POST_BARGE(ElevatorState.HP, ShoulderState.PRE_BARGE, WristState.PRE_BARGE, 0.0), + + // SPIT_ALGAE, + PROCESSOR( + ElevatorState.PROCESSOR, + ShoulderState.PROCESSOR, + WristState.PROCESSOR, + ManipulatorSubsystem.MAX_VELOCITY * -2.0 / 12.0), + PRE_CLIMB( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + 0.0, + 3.4, + 2.0), + CLIMB( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + 0.0, + 1.35, + 0.5) // lowkey why is this so slow + // HOME(), + // SPIT_CORAL(), + // ANTI_JAM, + // L4_TUCKED(ElevatorState.HP, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), + // L4_TUCKED_EXTENDED(ElevatorState.L4, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), + // L4_TUCKED_OUT(ElevatorState.L4, ShoulderState.L4_TUCKED_OUT, WristState.L4_TUCKED_OUT), + + ; + + public final ElevatorState elevatorState; + public final ShoulderState shoulderState; + public final WristState wristState; + public final double manipulatorVelocity; + public final double climberPosition; + public final double climberSpeed; + + private SuperState( + ElevatorState elevatorState, + ShoulderState shoulderState, + WristState wristState, + double manipulatorVelocity) { + this.elevatorState = elevatorState; + this.shoulderState = shoulderState; + this.wristState = wristState; + this.manipulatorVelocity = manipulatorVelocity; + this.climberPosition = 0.0; // TODO is this right? lol + this.climberSpeed = 0.0; + } - @AutoLogOutput(key = "Superstructure/Coral Score Adjust") - private final DoubleSupplier coralAdjust; + private SuperState( + ElevatorState elevatorState, + ShoulderState shoulderState, + WristState wristState, + double manipulatorVelocity, + double climberPosition, + double climberSpeed) { + this.elevatorState = elevatorState; + this.shoulderState = shoulderState; + this.wristState = wristState; + this.manipulatorVelocity = manipulatorVelocity; + this.climberPosition = climberPosition; + this.climberSpeed = climberSpeed; + } + } private SuperState state = SuperState.IDLE; private SuperState prevState = SuperState.IDLE; - private Map stateTriggers = new HashMap(); private Timer stateTimer = new Timer(); @@ -129,161 +201,273 @@ public static enum SuperState { private final ManipulatorSubsystem manipulator; private final FunnelSubsystem funnel; private final ClimberSubsystem climber; - // Intake would be included here, but is cut from cad as of rn - - public Superstructure( - ElevatorSubsystem elevator, - ManipulatorSubsystem manipulator, - ShoulderSubsystem shoulder, - WristSubsystem wrist, - FunnelSubsystem funnel, - ClimberSubsystem climber, - Supplier pose, - Supplier chassisVel, - Supplier reefTarget, - Supplier algaeIntakeTarget, - Supplier algaeScoreTarget, - Trigger scoreReq, - Trigger preScoreReq, - Trigger intakeAlgaeReq, - Trigger intakeCoralReq, - Trigger climbReq, - Trigger climbConfReq, - Trigger climbCancelReq, - Trigger antiCoralJamReq, - Trigger antiAlgaeJamReq, - Trigger homeReq, - Trigger revFunnelReq, - Trigger forceFunnelReq, - Trigger forceIndexReq, - Trigger killVisionIK, - DoubleSupplier coralAdjust) { - this.elevator = elevator; - this.manipulator = manipulator; - this.shoulder = shoulder; - this.wrist = wrist; - this.funnel = funnel; - this.climber = climber; - - this.pose = pose; - this.chassisVel = chassisVel; - this.reefTarget = reefTarget; - this.algaeIntakeTarget = algaeIntakeTarget; - this.algaeScoreTarget = algaeScoreTarget; - - this.preScoreReq = preScoreReq; - this.scoreReq = scoreReq; - - this.intakeAlgaeReq = intakeAlgaeReq; - this.intakeCoralReq = intakeCoralReq; - - this.preClimbReq = climbReq; - this.climbConfReq = climbConfReq; - this.climbCancelReq = climbCancelReq; - - this.antiCoralJamReq = antiCoralJamReq; - this.antiAlgaeJamReq = antiAlgaeJamReq; - - this.homeReq = homeReq; - - this.revFunnelReq = revFunnelReq; - this.forceFunnelReq = forceFunnelReq; - - this.forceIndexReq = forceIndexReq; - - this.killVisionIK = killVisionIK; - - this.coralAdjust = coralAdjust; - - stateTimer.start(); - - for (var state : SuperState.values()) { - stateTriggers.put(state, new Trigger(() -> this.state == state && DriverStation.isEnabled())); - } - configureStateTransitionCommands(); - } + /** Creates a new Superstructure. */ + public Superstructure( + ElevatorSubsystem elevator, + ShoulderSubsystem shoulder, + WristSubsystem wrist, + ManipulatorSubsystem manipulator, + FunnelSubsystem funnel, + ClimberSubsystem climber) { + this.elevator = elevator; + this.shoulder = shoulder; + this.wrist = wrist; + this.manipulator = manipulator; + this.funnel = funnel; + this.climber = climber; + + addTransitions(); + + stateTimer.start(); + } /** This file is not a subsystem, so this MUST be called manually. */ public void periodic() { Logger.recordOutput("Superstructure/Superstructure State", state); + Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); } - private void configureStateTransitionCommands() { - // Prob a better way to impl this - // Vaughn says he wants this available anytime - forceIndexReq.whileTrue(manipulator.setVelocity(1.0)); + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger) { + // maps triggers to the transitions + trigger.and(new Trigger(() -> state == start)).onTrue(changeStateTo(end)); + } - stateTriggers - .get(SuperState.IDLE) - .whileTrue( - // extendWithClearance( - // Units.inchesToMeters(5.0), - // ShoulderSubsystem.SHOULDER_HP_POS, - // WristSubsystem.WRIST_HP_POS) - // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) - // .andThen( - extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS) - .repeatedly()) // ) - .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) - .whileTrue( - funnel.setVoltage( - () -> - revFunnelReq.getAsBoolean() - ? -2.0 - : (forceFunnelReq.getAsBoolean() - || (Stream.of(HumanPlayerTargets.values()) - .map( - (t) -> - t.location - .minus(pose.get()) - .getTranslation() - .getNorm()) - .min(Double::compare) - .get() - < 1.0) - ? 1.0 - : 0.0))) - .and(manipulator::getFirstBeambreak) - .and(() -> manipulator.getTimeSinceZero() < 1.0) - .onTrue(this.forceState(SuperState.READY_CORAL)); + private boolean atExtension(SuperState state) { + return elevator.atExtension(state.elevatorState.getExtensionMeters()) + && shoulder.isNearAngle(state.shoulderState.getAngle()) + && wrist.isNearAngle(state.wristState.getAngle()); + } - stateTriggers - .get(SuperState.IDLE) - .and(intakeCoralReq) - .onTrue(this.forceState(SuperState.INTAKE_CORAL_GROUND)); + public boolean atExtension() { + return atExtension(state); + } - // IDLE -> INTAKE_ALGAE_{location} - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_GROUND)); + private Command changeStateTo(SuperState nextState) { + return Commands.runOnce( + () -> { + System.out.println("Changing state to " + nextState); + stateTimer.reset(); + this.prevState = this.state; + this.state = nextState; + setSubstates(); + }) + .ignoringDisable(true); + } - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_HIGH)); + public Command changeStateTo(Supplier state) { + return changeStateTo(state.get()); + } - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.LOW) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_LOW)); + private void setSubstates() { + elevator.setState(state.elevatorState); + shoulder.setState(state.shoulderState); + wrist.setState(state.wristState); + manipulator.setState(state.manipulatorVelocity); + //funnel and climber don't have states per se + } - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_STACK)); + + private void addTransitions() { + // Prob a better way to impl this + // Vaughn says he wants this available anytime + Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); + + // ---Funnel--- + bindTransition( + SuperState.IDLE, + SuperState.READY_CORAL, + new Trigger(() -> manipulator.getFirstBeambreak() && manipulator.getTimeSinceZero() < 1.0)); + + // ---Intake coral ground--- + bindTransition(SuperState.IDLE, SuperState.PRE_INTAKE_CORAL_GROUND, Robot.intakeCoralReq); + + bindTransition( + SuperState.PRE_INTAKE_CORAL_GROUND, + SuperState.INTAKE_CORAL_GROUND, + new Trigger(this::atExtension)); + + bindTransition( + SuperState.INTAKE_CORAL_GROUND, + SuperState.POST_INTAKE_CORAL_GROUND, + Robot.intakeCoralReq + .negate() + .debounce(0.060) + .and(manipulator::bothBeambreaks) + .debounce(0.12)); // TODO i'm lowkey losing my shit + + bindTransition( + SuperState.POST_INTAKE_CORAL_GROUND, + SuperState.READY_CORAL, + new Trigger(this::atExtension)); + + // ---Intake Algae--- + bindTransition(SuperState.IDLE, SuperState.PRE_PRE_INTAKE_ALGAE, Robot.intakeAlgaeReq); + + bindTransition( + SuperState.PRE_PRE_INTAKE_ALGAE, + SuperState.PRE_INTAKE_ALGAE, + new Trigger(this::atExtension)); + + // ---Intake Low Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_LOW, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW))); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_LOW, + SuperState.READY_ALGAE, + new Trigger( + new Trigger( + () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable + // stateTimer.hasElapsed(1.0) && + // manipulator.getStatorCurrentAmps() > + // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM + // && + // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) + // .getTranslation() + // .minus(pose.get().getTranslation()) + // .getNorm() + // > 0.3 + )); + + // ---Intake High Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_HIGH, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH))); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_HIGH, + SuperState.READY_ALGAE, + new Trigger( + new Trigger( + () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable + // stateTimer.hasElapsed(1.0) && + // manipulator.getStatorCurrentAmps() > + // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM + // && + // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) + // .getTranslation() + // .minus(pose.get().getTranslation()) + // .getNorm() + // > 0.3 + )); + + // ---Intake Stack Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_STACK, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.STACK))); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_STACK, + SuperState.READY_ALGAE, + new Trigger( + new Trigger( + () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable + // stateTimer.hasElapsed(1.0) && + // manipulator.getStatorCurrentAmps() > + // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM + // && + // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) + // .getTranslation() + // .minus(pose.get().getTranslation()) + // .getNorm() + // > 0.3 + )); + + // ---Intake Ground Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_GROUND, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.GROUND))); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_GROUND, + SuperState.READY_ALGAE, + new Trigger( + stateTimer.hasElapsed(1.0) && + manipulator.getStatorCurrentAmps() > + ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM + && + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.3 + )); + + // ---Score in barge--- + bindTransition( + SuperState.READY_ALGAE, + SuperState.PRE_BARGE, + new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.BARGE) + .and(Robot.preScoreReq)); + + bindTransition( + SuperState.PRE_BARGE, SuperState.BARGE, Robot.scoreReq.and(new Trigger(this::atExtension))); + + bindTransition( + SuperState.BARGE, + SuperState.POST_BARGE, + new Trigger(() -> stateTimer.hasElapsed(0.5)) // TODO i don't trust this state timer stuff + ); + + bindTransition( + SuperState.POST_BARGE, + SuperState.POST_POST_BARGE, + // new Trigger(() -> stateTimer.hasElapsed(1.0))); + new Trigger(this::atExtension)); + + bindTransition(SuperState.POST_POST_BARGE, SuperState.IDLE, new Trigger(this::atExtension)); + + // ---Score in processor--- + bindTransition( + SuperState.READY_ALGAE, + SuperState.PROCESSOR, + new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.PROCESSOR) + .and(Robot.preScoreReq)); + + // manipulator voltage gets set elsewhere i guess + bindTransition( + SuperState.PROCESSOR, + SuperState.IDLE, + Robot.scoreReq.negate().and(manipulator::neitherBeambreak) // TODO janky testing only + // .and( + // () -> + // !MathUtil.isNear( + // pose.get().getX(), + // DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + // ? AutoAim.BLUE_PROCESSOR_POS.getX() + // : AutoAim.RED_PROCESSOR_POS.getX(), + // 0.5) + // || !MathUtil.isNear( + // pose.get().getY(), + // DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + // ? AutoAim.BLUE_PROCESSOR_POS.getY() + // : AutoAim.RED_PROCESSOR_POS.getY(), + // 0.5)) + ); // IDLE -> PRE_CLIMB stateTriggers .get(SuperState.IDLE) - .and(preClimbReq) + .and(Robot.preClimbReq) .onTrue(this.forceState(SuperState.PRE_CLIMB)); stateTriggers @@ -309,121 +493,29 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - shoulder.setTargetAngle(Rotation2d.fromDegrees(50.0)), + shoulder.setAngle(Rotation2d.fromDegrees(50.0)), elevator.runCurrentZeroing(), Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) .andThen(wrist.currentZero(() -> shoulder.getInputs())))) .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) .onTrue(this.forceState(prevState)); - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - Rotation2d.fromDegrees(35.0), - WristSubsystem.WRIST_CLEARANCE_POS) - .until(() -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)) - .andThen( - Commands.parallel( - shoulder - .setVoltage(-10.0) - .until(() -> shoulder.getAngle().getDegrees() < 10.0) - .andThen(shoulder.setVoltage(-1.0)), - elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), - wrist - .setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND) - .until( - () -> - wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) - .andThen(wrist.setVoltage(-1.0))))) - .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); - - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) - .and(intakeCoralReq.negate()) - .debounce(0.060) - .onTrue( - Commands.parallel( - manipulator.setVoltage(0.0), - Commands.waitSeconds(0.12) - .andThen( - Commands.runOnce(() -> manipulator.resetPosition(0.792)), - this.forceState(SuperState.READY_CORAL)))); - - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .and(intakeCoralReq.negate()) - .and(() -> !manipulator.getFirstBeambreak() || !manipulator.getSecondBeambreak()) - .onTrue(this.forceState(SuperState.IDLE)); - - // READY_CORAL logic - stateTriggers - .get(SuperState.READY_CORAL) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .whileTrue( - manipulator - .hold() - .until( - () -> - shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) - .andThen(manipulator.jog(ManipulatorSubsystem.JOG_POS))); - // keep indexing to make sure its chilling - - stateTriggers - .get(SuperState.READY_CORAL) - .or(stateTriggers.get(SuperState.PRE_L1)) - .or(stateTriggers.get(SuperState.PRE_L2)) - .or(stateTriggers.get(SuperState.PRE_L3)) - .or(stateTriggers.get(SuperState.PRE_L4)) - .and(() -> (!manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak())) - .onTrue(this.forceState(SuperState.IDLE)); - // SPIT_CORAL logic + -> IDLE stateTriggers .get(SuperState.SPIT_CORAL) - .whileTrue(manipulator.setVelocity(10)) + .whileTrue(manipulator.setRollerVelocity(10)) .and(() -> !manipulator.getFirstBeambreak()) .and(() -> !manipulator.getSecondBeambreak()) - .and(preClimbReq.negate()) + .and(Robot.preClimbReq.negate()) .onTrue(this.forceState(SuperState.IDLE)); // SPIT_CORAL -> PRE_CLIMB stateTriggers .get(SuperState.SPIT_CORAL) .and(() -> !manipulator.getFirstBeambreak()) .and(() -> !manipulator.getSecondBeambreak()) - .and(preClimbReq) + .and(Robot.preClimbReq) .onTrue(forceState(SuperState.PRE_CLIMB)); - // READY_CORAL -> PRE_L{1-4} - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L1) - .onTrue(this.forceState(SuperState.PRE_L1)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L2) - .onTrue(this.forceState(SuperState.PRE_L2)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L3) - .onTrue(this.forceState(SuperState.PRE_L3)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L4) - .onTrue(this.forceState(SuperState.PRE_L4)); + stateTriggers .get(SuperState.READY_CORAL) @@ -432,176 +524,36 @@ private void configureStateTransitionCommands() { // READY_CORAL -> SPIT_CORAL stateTriggers .get(SuperState.READY_CORAL) - .and(preClimbReq) + .and(Robot.preClimbReq) .onTrue(this.forceState(SuperState.SPIT_CORAL)); - // PRE_L{1-4} logic + -> SCORE_CORAL - stateTriggers - .get(SuperState.PRE_L1) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - // .whileTrue( - // this.extendWithClearance( - // ElevatorSubsystem.L1_EXTENSION_METERS, - // ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - // WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue( - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L1_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())); - - stateTriggers - .get(SuperState.PRE_L1) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue( - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L1_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); + stateTriggers .get(SuperState.PRE_L1) .and(() -> reefTarget.get() != ReefTarget.L1) .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.PRE_L2) - .whileTrue( - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L2_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); + stateTriggers .get(SuperState.PRE_L2) .and(() -> reefTarget.get() != ReefTarget.L2) .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.PRE_L3) - .whileTrue( - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L3_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); + stateTriggers .get(SuperState.PRE_L3) .and(() -> reefTarget.get() != ReefTarget.L3) .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.PRE_L4) - .whileTrue( - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L4_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.125 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); + stateTriggers .get(SuperState.PRE_L4) .and(() -> reefTarget.get() != ReefTarget.L4) .onTrue(this.forceState(SuperState.IDLE)); - // SCORE_CORAL -> IDLE - stateTriggers - .get(SuperState.SCORE_CORAL) - .whileTrue( - // Commands.either( - // Commands.parallel( - // ExtensionKinematics.holdStateCommand( - // elevator, - // shoulder, - // wrist, - // () -> - // killVisionIK.getAsBoolean() - // ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - // : ExtensionKinematics.getPoseCompensatedExtension( - // pose.get(), - // ExtensionKinematics.getExtensionForLevel( - // reefTarget.get())))), - Commands.either( - this.holdExtension( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), - () -> - elevator.isNearExtension( - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - .elevatorHeightMeters() - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get())) - .elevatorHeightMeters()) - && shoulder.isNearAngle( - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - .shoulderAngle() - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get())) - .shoulderAngle())) - // // End - // () -> - // MathUtil.isNear( - // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) - ) - .whileTrue( - manipulator - .hold() - .until( - () -> - elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) - .andThen(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed))) - // .and(() -> reefTarget.get() == ReefTarget.L1) - // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) - // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) - // .whileTrue( - // Commands.waitSeconds(0.1) - // .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))) - ; stateTriggers .get(SuperState.SCORE_CORAL) @@ -654,8 +606,8 @@ private void configureStateTransitionCommands() { .whileTrue( Commands.parallel( elevator.hold(), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)) + shoulder.setAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), + wrist.setAngle(WristSubsystem.WRIST_CORAL_GROUND)) .until(() -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) .andThen( Commands.parallel( @@ -687,123 +639,7 @@ private void configureStateTransitionCommands() { .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.STACK) .onTrue(this.forceState(SuperState.IDLE)); - // INTAKE_ALGAE_{location} -> READY_ALGAE - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue( - Commands.waitUntil( - () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS)) - .andThen(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE))); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_LOW) - .whileTrue( - this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_HIGH) - .whileTrue( - this.extendWithClearance( - new ExtensionState( - 0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS)) - .until( - () -> - shoulder.isNearAngle(Rotation2d.fromDegrees(35.0)) - && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)) - .andThen( - this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS))) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_STACK) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) - .whileTrue( - Commands.waitUntil( - () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .andThen(manipulator.setVoltage(12.0))); - - // leave intake - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_LOW)) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_HIGH)) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_STACK)) - .and(intakeAlgaeReq.negate()) - .onTrue(this.forceState(SuperState.CHECK_ALGAE)); - - stateTriggers - .get(SuperState.CHECK_ALGAE) - .whileTrue(elevator.hold()) - .whileTrue(manipulator.intakeAlgae()) - .whileTrue( - shoulder.setTargetAngle( - () -> - (prevState == SuperState.INTAKE_ALGAE_HIGH - || prevState == SuperState.INTAKE_ALGAE_LOW) - ? ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS - : ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue( - wrist.setTargetAngle( - () -> - (algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND - || algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK) - ? WristSubsystem.WRIST_RETRACTED_POS - : WristSubsystem.WRIST_INTAKE_ALGAE_REEF_RETRACT_POS)) - .and(() -> stateTimer.hasElapsed(1.0)) - .and( - () -> - manipulator.getStatorCurrentAmps() > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD - || Robot.ROBOT_TYPE == RobotType.SIM) - .and( - () -> - AlgaeIntakeTargets.getClosestTargetPose(pose.get()) - .getTranslation() - .minus(pose.get().getTranslation()) - .getNorm() - > 0.3 - || (algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND - || algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK)) - .onTrue( - this.forceState(SuperState.READY_ALGAE) - .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(true)))); // gahahaha - - // READY_ALGAE logic - stateTriggers - .get(SuperState.READY_ALGAE) - .whileTrue( - extendWithClearanceSlow( - () -> 0.1, - () -> ShoulderSubsystem.SHOULDER_RETRACTED_POS, - () -> WristSubsystem.WRIST_READY_ALGAE)) - .whileTrue(manipulator.intakeAlgae()); - // READY_ALGAE -> PRE_NET - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preScoreReq) - .and(() -> algaeScoreTarget.get() == AlgaeScoreTarget.NET) - .onTrue(forceState(SuperState.PRE_NET)); - // READY_ALGAE -> PRE_PROCESSOR - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preScoreReq) - .and(() -> algaeScoreTarget.get() == AlgaeScoreTarget.PROCESSOR) - .onTrue(forceState(SuperState.PRE_PROCESSOR)); + // READY_ALGAE -> SPIT_ALGAE stateTriggers @@ -811,10 +647,7 @@ private void configureStateTransitionCommands() { .and(preClimbReq) .onTrue(forceState(SuperState.SPIT_ALGAE)); - stateTriggers - .get(SuperState.READY_ALGAE) - .and(() -> manipulator.getStatorCurrentAmps() < 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(forceState(SuperState.CHECK_ALGAE)); + // SPIT_ALGAE -> PRE_CLIMB stateTriggers .get(SuperState.SPIT_ALGAE) @@ -825,98 +658,12 @@ private void configureStateTransitionCommands() { .and(preClimbReq) .onTrue(forceState(SuperState.PRE_CLIMB)); - // PRE_PROCESSOR logic - stateTriggers - .get(SuperState.PRE_PROCESSOR) - .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .and(scoreReq) - .onTrue(forceState(SuperState.SCORE_ALGAE_PROCESSOR)); - // PRE_NET logic - stateTriggers - .get(SuperState.PRE_NET) - .whileTrue(manipulator.setVoltage(3 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) - .whileTrue( - Commands.parallel( - elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), - // Make it initially extend to the full 90 degrees - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), - wrist.setSlowTargetAngle(WristSubsystem.WRIST_PRE_NET_POS))) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_PRE_NET_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) - .and(scoreReq) - .onTrue(forceState(SuperState.SCORE_ALGAE_NET)); + // ---Climb--- + // Climb could start from any state, so there's no particular transition + Robot.preClimbReq.onTrue( + Commands.parallel(changeStateTo(SuperState.PRE_CLIMB), funnel.unlatch())); - stateTriggers - .get(SuperState.SCORE_ALGAE_NET) - .onTrue(Commands.runOnce(() -> stateTimer.reset())) - .whileTrue( - manipulator - .hold() - .until( - () -> - shoulder.getVelocity() - > ShoulderSubsystem.TOSS_CONFIGS.MotionMagicCruiseVelocity - 0.1) - .andThen(manipulator.setVoltage(-13.0))) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> stateTimer.hasElapsed(0.5)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS)) - .and(() -> stateTimer.hasElapsed(1.0)) - .onTrue( - forceState(SuperState.IDLE) - .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(false)))); - - stateTriggers - .get(SuperState.SCORE_ALGAE_PROCESSOR) - .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) - .whileTrue(manipulator.setVoltage(-2.0)) - .and( - () -> - !MathUtil.isNear( - pose.get().getX(), - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_PROCESSOR_POS.getX() - : AutoAim.RED_PROCESSOR_POS.getX(), - 0.5) - || !MathUtil.isNear( - pose.get().getY(), - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_PROCESSOR_POS.getY() - : AutoAim.RED_PROCESSOR_POS.getY(), - 0.5)) - .onTrue(this.forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.PRE_CLIMB) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) - .onTrue(funnel.unlatch()) // !! - .and(climbConfReq) - .onTrue(forceState(SuperState.CLIMB)); - - stateTriggers - .get(SuperState.CLIMB) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue( - climber - .setPositionSlow(1.35) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, Robot.climbConfReq); // May need more checks to see if canceling is safe stateTriggers @@ -925,194 +672,22 @@ private void configureStateTransitionCommands() { .onTrue(forceState(SuperState.PRE_CLIMB)); } - public ExtensionState getExtensionState() { - return new ExtensionState(elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()); - } - - private Command extendWithClearance(ExtensionState extension) { - return extendWithClearance( - extension.elevatorHeightMeters(), extension.shoulderAngle(), extension.wristAngle()); - } - - public Command extendWithClearance(Supplier extension) { - return extendWithClearance( - () -> extension.get().elevatorHeightMeters(), - () -> extension.get().shoulderAngle(), - () -> extension.get().wristAngle()) - .until( - () -> - elevator.isNearExtension(extension.get().elevatorHeightMeters()) - && shoulder.isNearAngle(extension.get().shoulderAngle()) - && wrist.isNearAngle(extension.get().wristAngle())) - .andThen(ExtensionKinematics.holdStateCommand(elevator, shoulder, wrist, extension)); - } - - private Command extendWithClearance( - double elevatorExtension, Rotation2d shoulderAngle, Rotation2d wristAngle) { - return extendWithClearance(() -> elevatorExtension, () -> shoulderAngle, () -> wristAngle); - } - - private boolean shouldntTuck(Rotation2d shoulderAngle) { - return wrist.getAngle().getDegrees() < 100.0 - || elevator.getExtensionMeters() > 0.75 - || shoulderAngle.getDegrees() > 60.0; - } - - private Command extendWithClearance( - DoubleSupplier elevatorExtension, - Supplier shoulderAngle, - Supplier wristAngle) { - final AtomicReference> path = new AtomicReference<>(); - final AtomicInteger index = new AtomicInteger(0); - return Commands.sequence( - Commands.runOnce( - () -> { - index.set(0); - path.set( - ExtensionPathing.getPath( - getExtensionState(), - new ExtensionState( - elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); - }), - holdExtension(() -> path.get().get(index.get())) - .until( - () -> - elevator.isNearExtension( - path.get().get(index.get()).elevatorHeightMeters(), 0.2) - && shoulder.isNearAngle( - path.get().get(index.get()).shoulderAngle(), - Rotation2d.fromDegrees(10.0)) - && wrist.isNearAngle( - path.get().get(index.get()).wristAngle(), Rotation2d.fromDegrees(20.0))) - .finallyDo(() -> index.set(index.get() + 1)) - .repeatedly() - .until(() -> index.get() == path.get().size() - 1), - holdExtension( - () -> - new ExtensionState( - elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); - } - - private Command holdExtension(Supplier state) { - return Commands.parallel( - elevator.setExtension(() -> state.get().elevatorHeightMeters()), - shoulder.setTargetAngle(() -> state.get().shoulderAngle()), - wrist.setTargetAngle(() -> state.get().wristAngle())); - } - - private Command extendWithClearanceSlow( - DoubleSupplier elevatorExtension, - Supplier shoulderAngle, - Supplier wristAngle) { - return Commands.sequence( - // Retract shoulder + wrist - Commands.parallel( - shoulder - .run(() -> {}) - .until( - () -> - wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 - > shoulder.getAngle().getDegrees()) - .andThen( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle( - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get()))), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.hold()) - // .unless( - // () -> - // shoulder.getAngle().getDegrees() - // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() - // && wrist.getAngle().getDegrees() < 90.0) - .until( - () -> - shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), - // extend elevator - Commands.parallel( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.setExtensionSlow(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), - // re-extend joints - Commands.parallel( - shoulder - .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) - .unless( - () -> - shouldntTuck(shoulderAngle.get()) - || shoulderAngle.get().getDegrees() - < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .until(() -> wrist.isNearTarget()) - .andThen(shoulder.setTargetAngle(shoulderAngle)), - wrist - .hold() - .until(() -> shoulder.isNearTarget()) - .unless( - () -> - wristAngle.get().getDegrees() < 90.0 - || shoulderAngle.get().getDegrees() - > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .andThen(wrist.setTargetAngle(wristAngle)), - elevator.setExtensionSlow(elevatorExtension))); - } public SuperState getState() { return state; } - public boolean stateIsCoralAlike() { - return this.state == SuperState.READY_CORAL - || this.state == SuperState.PRE_L1 - || this.state == SuperState.PRE_L2 - || this.state == SuperState.PRE_L3 - || this.state == SuperState.PRE_L4 - || this.state == SuperState.SCORE_CORAL; - } - - public boolean stateIsAlgaeAlike() { - return this.state == SuperState.READY_ALGAE - || this.state == SuperState.INTAKE_ALGAE_GROUND - || this.state == SuperState.INTAKE_ALGAE_LOW - || this.state == SuperState.INTAKE_ALGAE_HIGH - || this.state == SuperState.INTAKE_ALGAE_STACK - || this.state == SuperState.CHECK_ALGAE - || this.state == SuperState.PRE_NET - || this.state == SuperState.PRE_PROCESSOR - || this.state == SuperState.SCORE_ALGAE_NET - || this.state == SuperState.SCORE_ALGAE_PROCESSOR; + public static boolean stateIsScoreCoral(SuperState state) { + return state == SuperState.L1 + || state == SuperState.L2 + || state == SuperState.L3 + || state == SuperState.L4; } - public boolean intakeTargetOnReef() { - return this.algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH - || this.algaeIntakeTarget.get() == AlgaeIntakeTarget.LOW; - } - - private Command forceState(SuperState nextState) { - return Commands.runOnce( - () -> { - System.out.println("Changing state to " + nextState); - stateTimer.reset(); - this.prevState = this.state; - this.state = nextState; - }) - .ignoringDisable(true); + public static boolean stateIsIntakeAlgae(SuperState state) { + return state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_HIGH; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 37a6afcc..1c76428e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.swerve.OdometryThreadIO.OdometryThreadIOInputs; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; @@ -380,7 +381,7 @@ private void updateVision() { (camera.getName().equals("Front_Left_Camera") || camera.getName().equals("Front_Right_Camera")) && (Robot.state.get().toString().startsWith("PRE_L") - || Robot.state.get() == SuperState.SCORE_CORAL + || Superstructure.stateIsScoreCoral(Robot.state.get()) || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) ? 0.5 @@ -411,7 +412,7 @@ private void updateVision() { || t.getFiducialId() == 14) ? 1.2 : 1.0) - .times(Robot.state.get() == SuperState.PRE_NET ? 0.5 : 1.0)); + .times(Robot.state.get() == SuperState.PRE_BARGE ? 0.5 : 1.0)); // the sussifier }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; From 4822cbe75197a8e4f59ef7bbd7d2dc5ccd98904b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 18:27:05 -0700 Subject: [PATCH 094/154] clean up more superstructure stuff, still need to figure out how to do home/antijam states --- src/main/java/frc/robot/Autos.java | 42 +- src/main/java/frc/robot/Robot.java | 31 +- .../subsystems/ManipulatorSubsystem.java | 34 -- .../frc/robot/subsystems/Superstructure.java | 414 ++++++++++-------- .../frc/robot/subsystems/camera/CameraIO.java | 1 - .../elevator/ElevatorSubsystem.java | 4 + 6 files changed, 270 insertions(+), 256 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index a7ed8c3d..f5bc2b76 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -24,12 +24,14 @@ import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; +import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.CoralTargets; import java.util.HashMap; import java.util.Optional; import java.util.function.Supplier; @@ -459,7 +461,7 @@ public Command LOtoA() { // 2910 steps.put("AtoB", routine.trajectory("AtoB")); steps.put("BtoB", routine.trajectory("BtoB")); - if (Robot.isSimulation()) manipulator.setSecondBeambreak(true); // gah + if (Robot.isSimulation()) manipulator.setSimSecondBeambreak(true); // gah routine // run first path .active() @@ -474,7 +476,7 @@ public Command LOtoA() { // 2910 AutoAim.translateToPose(swerve, () -> steps.get("AtoB").getInitialPose().get()) .until( () -> - elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + elevator.atExtension(ElevatorState.HP.getExtensionMeters()) && AutoAim.isInTolerance( swerve.getPose(), steps.get("AtoB").getInitialPose().get())), Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2)), @@ -501,7 +503,7 @@ public Command LOtoA() { // 2910 AutoAim.translateToPose(swerve, () -> steps.get("BtoB").getInitialPose().get()) .until( () -> - elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + elevator.atExtension(ElevatorState.HP.getExtensionMeters()) && AutoAim.isInTolerance( swerve.getPose(), steps.get("BtoB").getInitialPose().get())), Commands.runOnce(() -> autoGroundCoralIntake = true), @@ -552,7 +554,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { () -> AutoAim.isInTolerance( swerve.getPose(), - CoralTargets.getClosestTarget(trajEndPose.get()), + FieldUtils.CoralTargets.getClosestTarget(trajEndPose.get()), swerve.getVelocityFieldRelative(), Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) @@ -576,7 +578,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { Commands.waitUntil(() -> !manipulator.getSecondBeambreak()) .alongWith( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setSecondBeambreak(false)) + ? Commands.runOnce(() -> manipulator.setSimSecondBeambreak(false)) : Commands.none()), Commands.runOnce( () -> { @@ -588,7 +590,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { .andThen( AutoAim.translateToPose( swerve, - () -> CoralTargets.getClosestTarget(trajEndPose.get()), + () -> FieldUtils.CoralTargets.getClosestTarget(trajEndPose.get()), ChassisSpeeds::new, new Constraints(1.5, 1.0)))); } @@ -599,7 +601,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } else { return Commands.sequence( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setSecondBeambreak(true)) + ? Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)) : Commands.none(), Commands.print("intake - 2nd bb" + manipulator.getSecondBeambreak()), AutoAim.translateToPose( @@ -690,7 +692,7 @@ public Command scoreAlgaeInAuto() { // oh good lord Commands.print("Scoring algae"), Commands.runOnce( () -> { - Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.BARGE); + Robot.setAlgaeScoreTarget(AlgaeScoreTarget.BARGE); autoScore = true; }), Commands.waitUntil(() -> !manipulator.hasAlgae()) @@ -724,30 +726,30 @@ public Command intakeAlgaeInAuto(Supplier> pose) { Commands.runOnce( () -> { autoAlgaeIntake = true; - Robot.setCurrentAlgaeIntakeTarget( - AlgaeIntakeTargets.getClosestTarget(pose.get().get()) // wow + Robot.setAlgaeIntakeTarget( + FieldUtils.AlgaeIntakeTargets.getClosestTarget(pose.get().get()) // wow .height); }), AutoAim.translateToPose( swerve, () -> - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) + FieldUtils.AlgaeIntakeTargets.getOffsetLocation( + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) .until( () -> AutoAim.isInTolerance( swerve.getPose(), - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())), + FieldUtils.AlgaeIntakeTargets.getOffsetLocation( + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())), swerve.getVelocityFieldRelative(), Units.inchesToMeters(0.5), Units.degreesToRadians(1.0)) - && elevator.isNearTarget() + && elevator.atExtension() && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + ShoulderState.INTAKE_ALGAE_REEF.getAngle()) + && wrist.isNearAngle(WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( - swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) + swerve, () -> FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) .withTimeout(0.5)) .andThen( Commands.runOnce( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 140ac074..4f695f6a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -69,8 +69,10 @@ import frc.robot.subsystems.shoulder.ShoulderIOReal; import frc.robot.subsystems.shoulder.ShoulderIOSim; import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; import frc.robot.subsystems.swerve.*; import frc.robot.subsystems.wrist.*; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils; import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; @@ -461,7 +463,7 @@ public static enum AlgaeScoreTarget { new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); private final Superstructure superstructure = - new Superstructure(elevator, shoulder, wrist, manipulator, funnel, climber); + new Superstructure(elevator, shoulder, wrist, manipulator, funnel, climber, swerve); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -787,11 +789,11 @@ public Robot() { swerve.getVelocityFieldRelative(), Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) - && elevator.isNearTarget() + && elevator.atExtension() && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + ShoulderState.INTAKE_ALGAE_REEF.getAngle()) && wrist.isNearAngle( - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), @@ -836,7 +838,7 @@ public Robot() { .and( () -> superstructure.getState() == SuperState.READY_ALGAE - || superstructure.getState() == SuperState.PRE_PROCESSOR) + || superstructure.getState() == SuperState.PROCESSOR) .and(() -> algaeScoreTarget == AlgaeScoreTarget.PROCESSOR) .whileTrue( Commands.parallel( @@ -885,7 +887,7 @@ public Robot() { .and( () -> superstructure.getState() == SuperState.READY_ALGAE - || superstructure.getState() == SuperState.PRE_NET) + || superstructure.getState() == SuperState.PRE_BARGE) .and(() -> algaeScoreTarget == AlgaeScoreTarget.BARGE) .whileTrue( Commands.parallel( @@ -920,11 +922,11 @@ public Robot() { driver .povUp() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(true)).ignoringDisable(true)); + .onTrue(Commands.runOnce(() -> manipulator.setSimFirstBeambreak(true)).ignoringDisable(true)); driver .povDown() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(false)).ignoringDisable(true)); + .onTrue(Commands.runOnce(() -> manipulator.setSimFirstBeambreak(false)).ignoringDisable(true)); driver .povRight() .and(() -> ROBOT_TYPE == RobotType.SIM) @@ -932,7 +934,7 @@ public Robot() { RobotModeTriggers.autonomous() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSecondBeambreak(true)).ignoringDisable(true)); + .onTrue(Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); driver .start() .onTrue( @@ -1217,17 +1219,6 @@ public void robotPeriodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Pre Score", Autos.autoPreScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Manipulator FK Pose", - ExtensionKinematics.getManipulatorPose( - swerve.getPose(), superstructure.getExtensionState())); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Extension FK Pose", - ExtensionKinematics.solveFK( - new ExtensionState( - elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()))); state = superstructure::getState; } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index b3e800a3..4c861d3d 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -4,10 +4,7 @@ package frc.robot.subsystems; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -18,10 +15,8 @@ import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; -import frc.robot.utils.Tracer; import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -90,35 +85,6 @@ public void resetPosition(final double rotations) { io.resetEncoder(rotations); } - public Command intakeCoral() { - return intakeCoral(CORAL_INTAKE_VELOCITY); - } - - public Command intakeCoralAir(double vel) { - return Commands.sequence( - setRollerVelocity(vel) - .until(() -> getSecondBeambreak()) - .finallyDo( - () -> { - io.setPosition(Rotation2d.fromRotations(0.63)); - positionSetpoint = 0.63; - }), - setRollerVoltage(2.0).until(() -> !getFirstBeambreak()), - jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); - } - - public Command intakeCoral(double vel) { - return Commands.sequence( - setRollerVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)), - Commands.runOnce( - () -> { - io.setPosition(Rotation2d.fromRotations(0.5)); - positionSetpoint = 0.5; - }), - setRollerVelocity(1.0).until(() -> !getFirstBeambreak()), - jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); - } - public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) .until( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3af81e40..987ca6d9 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,42 +1,33 @@ package frc.robot.subsystems; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.AlgaeIntakeTarget; import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.Robot.ReefTarget; -import frc.robot.Robot.RobotType; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; +import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.subsystems.wrist.WristSubsystem.WristState; import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.autoaim.AutoAim; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.concurrent.atomic.AtomicReference; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; -import java.util.stream.Stream; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; public class Superstructure { /** @@ -201,6 +192,7 @@ private SuperState( private final ManipulatorSubsystem manipulator; private final FunnelSubsystem funnel; private final ClimberSubsystem climber; + private final SwerveSubsystem swerve; /** Creates a new Superstructure. */ public Superstructure( @@ -209,13 +201,15 @@ public Superstructure( WristSubsystem wrist, ManipulatorSubsystem manipulator, FunnelSubsystem funnel, - ClimberSubsystem climber) { + ClimberSubsystem climber, + SwerveSubsystem swerve) { this.elevator = elevator; this.shoulder = shoulder; this.wrist = wrist; this.manipulator = manipulator; this.funnel = funnel; this.climber = climber; + this.swerve = swerve; addTransitions(); @@ -277,6 +271,7 @@ private void setSubstates() { private void addTransitions() { // Prob a better way to impl this // Vaughn says he wants this available anytime + //TODO this will probably not still work Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); // ---Funnel--- @@ -307,6 +302,142 @@ private void addTransitions() { SuperState.READY_CORAL, new Trigger(this::atExtension)); + // ---L1--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L1, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L1).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L1, SuperState.L1, Robot.scoreReq.and(this::atExtension)); + + //cancel + bindTransition(SuperState.PRE_L1, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L1)); + + bindTransition( + SuperState.L1, + SuperState.POST_L1, + new Trigger(() -> manipulator.neitherBeambreak()) + .and(this::atExtension) + .and(Robot.scoreReq.negate())); + + bindTransition( + SuperState.POST_L1, + SuperState.IDLE, + Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( + () -> + L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + + //go straight to intaking algae from reef + bindTransition( + SuperState.POST_L1, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension) + .and(Robot.intakeAlgaeReq) + .and(() -> intakeAlgaeFromReef())); + + // ---L2--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L2, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L2).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L2, SuperState.L2, Robot.scoreReq.and(this::atExtension)); + + //cancel + bindTransition(SuperState.PRE_L2, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L2)); + + bindTransition( + SuperState.L2, + SuperState.POST_L2, + new Trigger(() -> manipulator.neitherBeambreak()) + .and(this::atExtension) + .and(Robot.scoreReq.negate())); + + bindTransition( + SuperState.POST_L2, + SuperState.IDLE, + Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( + () -> + L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + + //go straight to intaking algae from reef + bindTransition( + SuperState.POST_L2, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension) + .and(Robot.intakeAlgaeReq) + .and(() -> intakeAlgaeFromReef())); + + // ---L3--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L3, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L3).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L3, SuperState.L3, Robot.scoreReq.and(this::atExtension)); + + //cancel + bindTransition(SuperState.PRE_L3, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L3)); + + bindTransition( + SuperState.L3, + SuperState.POST_L3, + new Trigger(() -> manipulator.neitherBeambreak()) + .and(this::atExtension) + .and(Robot.scoreReq.negate())); + + bindTransition( + SuperState.POST_L3, + SuperState.IDLE, + Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( + () -> + L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + + //go straight to intaking algae from reef + bindTransition( + SuperState.POST_L3, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension) + .and(Robot.intakeAlgaeReq) + .and(() -> intakeAlgaeFromReef())); + + // ---L4--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_PRE_L4, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); + + bindTransition(SuperState.PRE_L4, SuperState.L4, Robot.scoreReq.and(this::atExtension)); + + //cancel + bindTransition(SuperState.PRE_L4, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); + + bindTransition( + SuperState.L4, + SuperState.POST_L4, + new Trigger(() -> manipulator.neitherBeambreak()) + .and(this::atExtension) + .and(Robot.scoreReq.negate())); + + bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); + + bindTransition( + SuperState.POST_POST_L4, + SuperState.IDLE, + Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( + () -> + L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + + //go straight to intaking algae from reef + bindTransition( + SuperState.POST_POST_L4, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension) + .and(Robot.intakeAlgaeReq) + .and(() -> intakeAlgaeFromReef())); + // ---Intake Algae--- bindTransition(SuperState.IDLE, SuperState.PRE_PRE_INTAKE_ALGAE, Robot.intakeAlgaeReq); @@ -322,22 +453,24 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW))); + //cancel + bindTransition( + SuperState.INTAKE_ALGAE_LOW, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW)); + // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( SuperState.INTAKE_ALGAE_LOW, SuperState.READY_ALGAE, - new Trigger( - new Trigger( - () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable - // stateTimer.hasElapsed(1.0) && - // manipulator.getStatorCurrentAmps() > - // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM - // && - // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) - // .getTranslation() - // .minus(pose.get().getTranslation()) - // .getNorm() - // > 0.3 + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae) + .and(() -> + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.3 )); // ---Intake High Algae--- @@ -347,22 +480,24 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH))); + //cancel + bindTransition( + SuperState.INTAKE_ALGAE_HIGH, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH)); + // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( SuperState.INTAKE_ALGAE_HIGH, SuperState.READY_ALGAE, - new Trigger( - new Trigger( - () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable - // stateTimer.hasElapsed(1.0) && - // manipulator.getStatorCurrentAmps() > - // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM - // && - // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) - // .getTranslation() - // .minus(pose.get().getTranslation()) - // .getNorm() - // > 0.3 + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae) + .and(() -> + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.3 )); // ---Intake Stack Algae--- @@ -372,23 +507,18 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.STACK))); + //cancel + bindTransition( + SuperState.INTAKE_ALGAE_STACK, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK)); + // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( SuperState.INTAKE_ALGAE_STACK, SuperState.READY_ALGAE, - new Trigger( - new Trigger( - () -> manipulator.eitherBeambreak()) // TODO this is just to make it toggleable - // stateTimer.hasElapsed(1.0) && - // manipulator.getStatorCurrentAmps() > - // ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM - // && - // AlgaeIntakeTargets.getClosestTargetPose(pose.get()) - // .getTranslation() - // .minus(pose.get().getTranslation()) - // .getNorm() - // > 0.3 - )); + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae)); // ---Intake Ground Algae--- bindTransition( @@ -397,23 +527,20 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.GROUND))); + //cancel + bindTransition( + SuperState.INTAKE_ALGAE_GROUND, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND)); + // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.READY_ALGAE, - new Trigger( - stateTimer.hasElapsed(1.0) && - manipulator.getStatorCurrentAmps() > - ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD || Robot.ROBOT_TYPE == RobotType.SIM - && - AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) - .getTranslation() - .minus(swerve.getPose().getTranslation()) - .getNorm() - > 0.3 - )); + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae)); - // ---Score in barge--- + // ---Score in barge--- bindTransition( SuperState.READY_ALGAE, SuperState.PRE_BARGE, @@ -444,31 +571,25 @@ private void addTransitions() { new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.PROCESSOR) .and(Robot.preScoreReq)); - // manipulator voltage gets set elsewhere i guess + // TODO manipulator voltage gets set elsewhere i guess bindTransition( SuperState.PROCESSOR, SuperState.IDLE, - Robot.scoreReq.negate().and(manipulator::neitherBeambreak) // TODO janky testing only - // .and( - // () -> - // !MathUtil.isNear( - // pose.get().getX(), - // DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - // ? AutoAim.BLUE_PROCESSOR_POS.getX() - // : AutoAim.RED_PROCESSOR_POS.getX(), - // 0.5) - // || !MathUtil.isNear( - // pose.get().getY(), - // DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - // ? AutoAim.BLUE_PROCESSOR_POS.getY() - // : AutoAim.RED_PROCESSOR_POS.getY(), - // 0.5)) + new Trigger( + () -> + !MathUtil.isNear( + swerve.getPose().getX(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getX() + : AutoAim.RED_PROCESSOR_POS.getX(), + 0.5) + || !MathUtil.isNear( + swerve.getPose().getY(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getY() + : AutoAim.RED_PROCESSOR_POS.getY(), + 0.5)) ); - // IDLE -> PRE_CLIMB - stateTriggers - .get(SuperState.IDLE) - .and(Robot.preClimbReq) - .onTrue(this.forceState(SuperState.PRE_CLIMB)); stateTriggers .get(SuperState.IDLE) @@ -515,7 +636,6 @@ private void addTransitions() { .and(() -> !manipulator.getSecondBeambreak()) .and(Robot.preClimbReq) .onTrue(forceState(SuperState.PRE_CLIMB)); - stateTriggers .get(SuperState.READY_CORAL) @@ -526,65 +646,6 @@ private void addTransitions() { .get(SuperState.READY_CORAL) .and(Robot.preClimbReq) .onTrue(this.forceState(SuperState.SPIT_CORAL)); - - - stateTriggers - .get(SuperState.PRE_L1) - .and(() -> reefTarget.get() != ReefTarget.L1) - .onTrue(this.forceState(SuperState.IDLE)); - - - - stateTriggers - .get(SuperState.PRE_L2) - .and(() -> reefTarget.get() != ReefTarget.L2) - .onTrue(this.forceState(SuperState.IDLE)); - - - - stateTriggers - .get(SuperState.PRE_L3) - .and(() -> reefTarget.get() != ReefTarget.L3) - .onTrue(this.forceState(SuperState.IDLE)); - - - - stateTriggers - .get(SuperState.PRE_L4) - .and(() -> reefTarget.get() != ReefTarget.L4) - .onTrue(this.forceState(SuperState.IDLE)); - - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - .debounce(0.15) - .onTrue(forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) - .and(killVisionIK) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - .onTrue(forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(intakeAlgaeReq) - .and(() -> intakeTargetOnReef()) - .onTrue( - forceState( - algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH - ? SuperState.INTAKE_ALGAE_HIGH - : SuperState.INTAKE_ALGAE_LOW)); antiCoralJamReq .onTrue(this.forceState(SuperState.ANTI_CORAL_JAM)) @@ -614,31 +675,6 @@ private void addTransitions() { wrist.hold(), shoulder.hold(), elevator.setExtension(Units.inchesToMeters(40)).andThen(elevator.hold())))); - - stateTriggers - .get(SuperState.CHECK_ALGAE) - .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() <= 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(this.forceState(SuperState.IDLE)); - - // change intake target - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.GROUND) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_LOW) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.LOW) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_HIGH) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.HIGH) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_STACK) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.STACK) - .onTrue(this.forceState(SuperState.IDLE)); - // READY_ALGAE -> SPIT_ALGAE @@ -666,10 +702,7 @@ private void addTransitions() { bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, Robot.climbConfReq); // May need more checks to see if canceling is safe - stateTriggers - .get(SuperState.CLIMB) - .and(climbCancelReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); + bindTransition(SuperState.CLIMB, SuperState.PRE_CLIMB, Robot.climbCancelReq); } @@ -677,17 +710,36 @@ public SuperState getState() { return state; } - public static boolean stateIsScoreCoral(SuperState state) { - return state == SuperState.L1 + public boolean intakeAlgaeFromReef() { + return Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH + || Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW; + } + + public boolean stateIsCoralAlike() { + return state == SuperState.READY_CORAL + || state == SuperState.PRE_L1 + || state == SuperState.PRE_L2 + || state == SuperState.PRE_L3 + || state == SuperState.PRE_L4 + || state == SuperState.L1 || state == SuperState.L2 || state == SuperState.L3 || state == SuperState.L4; } - public static boolean stateIsIntakeAlgae(SuperState state) { - return state == SuperState.INTAKE_ALGAE_GROUND - || state == SuperState.INTAKE_ALGAE_STACK - || state == SuperState.INTAKE_ALGAE_LOW - || state == SuperState.INTAKE_ALGAE_HIGH; + public boolean stateIsAlgaeAlike() { + return state == SuperState.PRE_PRE_INTAKE_ALGAE + || state == SuperState.PRE_INTAKE_ALGAE + || state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.READY_ALGAE + || state == SuperState.PRE_BARGE + || state == SuperState.BARGE + || state == SuperState.POST_BARGE + || state == SuperState.POST_POST_BARGE + || state == SuperState.PROCESSOR; + // SPIT_ALGAE, } } diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIO.java b/src/main/java/frc/robot/subsystems/camera/CameraIO.java index 0976b5df..7a2fbf26 100644 --- a/src/main/java/frc/robot/subsystems/camera/CameraIO.java +++ b/src/main/java/frc/robot/subsystems/camera/CameraIO.java @@ -4,7 +4,6 @@ package frc.robot.subsystems.camera; -import edu.wpi.first.math.geometry.Transform3d; import frc.robot.subsystems.camera.Camera.CameraConstants; import java.util.Optional; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 419a83e2..13f18c75 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -156,6 +156,10 @@ public boolean atExtension(double expected) { return MathUtil.isNear(expected, inputs.positionMeters, 0.05); } + public boolean atExtension() { + return atExtension(setpoint); + } + public Command runCurrentZeroing() { return this.run( () -> { From 0c0e9c8e5324d7dc935aa0f199fa905f4e16defc Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 20:26:41 -0700 Subject: [PATCH 095/154] fix home, antijam, spit, and climb states --- src/main/java/frc/robot/Robot.java | 108 +++++---- .../frc/robot/subsystems/Superstructure.java | 216 +++++++++--------- .../elevator/ElevatorSubsystem.java | 26 ++- .../shoulder/ShoulderSubsystem.java | 4 +- .../subsystems/swerve/SwerveSubsystem.java | 4 +- .../subsystems/wrist/WristSubsystem.java | 22 +- 6 files changed, 217 insertions(+), 163 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f695f6a..3006a8d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,30 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.elevator.ElevatorSubsystem.ELEVATOR_ANGLE; +import java.util.HashMap; +import java.util.Optional; +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.Supplier; +import java.util.stream.Stream; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.SignalLogger; @@ -20,6 +44,7 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -46,7 +71,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.Superstructure; @@ -70,8 +94,20 @@ import frc.robot.subsystems.shoulder.ShoulderIOSim; import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.wrist.*; +import frc.robot.subsystems.swerve.AlphaSwerveConstants; +import frc.robot.subsystems.swerve.BansheeSwerveConstants; +import frc.robot.subsystems.swerve.GyroIOPigeon2; +import frc.robot.subsystems.swerve.GyroIOSim; +import frc.robot.subsystems.swerve.KelpieSwerveConstants; +import frc.robot.subsystems.swerve.ModuleIO; +import frc.robot.subsystems.swerve.ModuleIOMapleSim; +import frc.robot.subsystems.swerve.ModuleIOReal; +import frc.robot.subsystems.swerve.PhoenixOdometryThread; +import frc.robot.subsystems.swerve.SwerveConstants; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.wrist.WristIOReal; +import frc.robot.subsystems.wrist.WristIOSim; +import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.subsystems.wrist.WristSubsystem.WristState; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils; @@ -82,29 +118,6 @@ import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; -import java.util.HashMap; -import java.util.Optional; -import java.util.Set; -import java.util.function.BiConsumer; -import java.util.function.Supplier; -import java.util.stream.Stream; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.GyroSimulation; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; - public class Robot extends LoggedRobot { public enum RobotType { REAL, @@ -171,14 +184,15 @@ public static enum AlgaeScoreTarget { private static final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private static final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + private static Supplier pose; + public static Trigger preScoreReq = driver .rightTrigger() .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) .or( () -> - swerve - .getPose() + pose.get() .getTranslation() .minus( DriverStation.getAlliance().orElse(Alliance.Blue) @@ -265,10 +279,14 @@ public static enum AlgaeScoreTarget { .debounce(0.5) .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); - //anti coral, anti algae, home - // driver.a(), - // driver.b(), - // driver.start(), + @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") + public static final Trigger antiCoralJamReq = driver.a(); + + @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") + public static final Trigger antiAlgaeJamReq = driver.b(); + + @AutoLogOutput(key = "Superstructure/Home Request") + public static Trigger homeReq = driver.start(); @AutoLogOutput(key = "Superstructure/Rev Funnel Req") public static Trigger revFunnelReq = operator.rightBumper(); @@ -380,9 +398,7 @@ public static enum AlgaeScoreTarget { ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); - private final ElevatorSubsystem elevator = - new ElevatorSubsystem( - ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim()); + private final ManipulatorSubsystem manipulator = new ManipulatorSubsystem( @@ -437,8 +453,13 @@ public static enum AlgaeScoreTarget { new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(0.5))) - : new WristIOSim()); - + : new WristIOSim(), + () -> shoulder.getAngle()); + private final ElevatorSubsystem elevator = + new ElevatorSubsystem( + ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim(), + () -> shoulder.getAngle(), + () -> wrist.atSetpoint()); private final FunnelSubsystem funnel = new FunnelSubsystem( ROBOT_TYPE != RobotType.SIM @@ -463,7 +484,7 @@ public static enum AlgaeScoreTarget { new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); private final Superstructure superstructure = - new Superstructure(elevator, shoulder, wrist, manipulator, funnel, climber, swerve); + new Superstructure(elevator, shoulder, wrist, manipulator, funnel, swerve); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -651,10 +672,10 @@ public Robot() { }) .ignoringDisable(true)); -elevator.setDefaultCommand(elevator.setStateExtension()); + elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); - manipulator.setDefaultCommand(manipulator.setStateVelocity(superstructure::atExtension)); + manipulator.setDefaultCommand(manipulator.setStateVelocity(() -> superstructure.atExtension() || superstructure.antiJamCoral())); funnel.setDefaultCommand( funnel.setRollerVoltage( () -> @@ -669,7 +690,9 @@ public Robot() { .get() < 1.0) ? 1.0 - : 0.0))); // at what point do ternary operators do more harm than good + : (superstructure.antiJamCoral() + ? -10.0 + : 0.0)))); // at what point do ternary operators do more harm than good climber.setDefaultCommand( climber.setPosition( superstructure.getState().climberPosition, @@ -709,6 +732,9 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed()) .times(-1))); + //????? + pose = () -> swerve.getPose(); + driver .rightBumper() .or(driver.leftBumper()) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 987ca6d9..7d537e5a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -5,8 +5,6 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -17,7 +15,6 @@ import frc.robot.Robot.AlgaeIntakeTarget; import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.Robot.ReefTarget; -import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; @@ -126,7 +123,7 @@ public enum SuperState { ElevatorState.INTAKE_ALGAE_GROUND, ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, - 0.0, + 10.0, 3.4, 2.0), CLIMB( @@ -136,7 +133,19 @@ public enum SuperState { 0.0, 1.35, 0.5) // lowkey why is this so slow - // HOME(), + , + HOME( + ElevatorState.HOME, + ShoulderState.HOME, + WristState.HOME, + 0.0 + ), + ANTIJAM_ALGAE( + ElevatorState.ANTIJAM_ALGAE, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + 0.0 + ) // SPIT_CORAL(), // ANTI_JAM, // L4_TUCKED(ElevatorState.HP, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), @@ -191,9 +200,10 @@ private SuperState( private final WristSubsystem wrist; private final ManipulatorSubsystem manipulator; private final FunnelSubsystem funnel; - private final ClimberSubsystem climber; private final SwerveSubsystem swerve; + public static boolean antiJamCoral; + /** Creates a new Superstructure. */ public Superstructure( ElevatorSubsystem elevator, @@ -201,14 +211,12 @@ public Superstructure( WristSubsystem wrist, ManipulatorSubsystem manipulator, FunnelSubsystem funnel, - ClimberSubsystem climber, SwerveSubsystem swerve) { this.elevator = elevator; this.shoulder = shoulder; this.wrist = wrist; this.manipulator = manipulator; this.funnel = funnel; - this.climber = climber; this.swerve = swerve; addTransitions(); @@ -232,6 +240,17 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger) { // maps triggers to the transitions trigger.and(new Trigger(() -> state == start)).onTrue(changeStateTo(end)); } + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + * @param cmd some command to run while making the transition + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger, Command cmd) { + // maps triggers to the transitions + trigger.and(new Trigger(() -> state == start)).onTrue(Commands.parallel(changeStateTo(end), cmd)); + } private boolean atExtension(SuperState state) { return elevator.atExtension(state.elevatorState.getExtensionMeters()) @@ -591,120 +610,89 @@ private void addTransitions() { 0.5)) ); - stateTriggers - .get(SuperState.IDLE) - .and(() -> !elevator.hasZeroed || !wrist.hasZeroed) - .and(() -> DriverStation.isEnabled()) - // .and(() -> Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(this.forceState(SuperState.HOME)); - - // We might want to make this work when we have a piece as well? - stateTriggers - .get(SuperState.IDLE) - .and(homeReq) - .onTrue(this.forceState(SuperState.HOME)) - .onTrue( - Commands.runOnce( - () -> { - elevator.hasZeroed = false; - wrist.hasZeroed = false; - })); - - stateTriggers - .get(SuperState.HOME) - .whileTrue( - Commands.parallel( - shoulder.setAngle(Rotation2d.fromDegrees(50.0)), - elevator.runCurrentZeroing(), - Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) - .andThen(wrist.currentZero(() -> shoulder.getInputs())))) - .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) - .onTrue(this.forceState(prevState)); - - // SPIT_CORAL logic + -> IDLE - stateTriggers - .get(SuperState.SPIT_CORAL) - .whileTrue(manipulator.setRollerVelocity(10)) - .and(() -> !manipulator.getFirstBeambreak()) - .and(() -> !manipulator.getSecondBeambreak()) - .and(Robot.preClimbReq.negate()) - .onTrue(this.forceState(SuperState.IDLE)); - // SPIT_CORAL -> PRE_CLIMB - stateTriggers - .get(SuperState.SPIT_CORAL) - .and(() -> !manipulator.getFirstBeambreak()) - .and(() -> !manipulator.getSecondBeambreak()) - .and(Robot.preClimbReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(() -> !wrist.hasZeroed || !elevator.hasZeroed) - .onTrue(this.forceState(SuperState.HOME)); - // READY_CORAL -> SPIT_CORAL - stateTriggers - .get(SuperState.READY_CORAL) - .and(Robot.preClimbReq) - .onTrue(this.forceState(SuperState.SPIT_CORAL)); - - antiCoralJamReq - .onTrue(this.forceState(SuperState.ANTI_CORAL_JAM)) - .onFalse(this.forceState(SuperState.IDLE)); - antiAlgaeJamReq - .onTrue(this.forceState(SuperState.ANTI_ALGAE_JAM)) - .onFalse(this.forceState(SuperState.IDLE)); - // ANTI_JAM logic - stateTriggers - .get(SuperState.ANTI_CORAL_JAM) - .whileTrue(elevator.hold()) - .whileTrue(wrist.hold()) - .whileTrue(shoulder.hold()) - .whileTrue(manipulator.setVelocity(-10)) - .whileTrue(funnel.setVoltage(-10.0)); - - stateTriggers - .get(SuperState.ANTI_ALGAE_JAM) - .whileTrue( - Commands.parallel( - elevator.hold(), - shoulder.setAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), - wrist.setAngle(WristSubsystem.WRIST_CORAL_GROUND)) - .until(() -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) - .andThen( - Commands.parallel( - wrist.hold(), - shoulder.hold(), - elevator.setExtension(Units.inchesToMeters(40)).andThen(elevator.hold())))); + // i might be insane for this + bindTransition(SuperState.IDLE, SuperState.HOME, Robot.homeReq, Commands.runOnce( + () -> { + elevator.hasZeroed = false; + wrist.hasZeroed = false; + })); + + //im not sure why this needs to exist separately + bindTransition( + SuperState.IDLE, + SuperState.HOME, + new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed) + .and(DriverStation::isEnabled)); + + bindTransition(SuperState.READY_CORAL, SuperState.HOME, new Trigger(() -> !wrist.hasZeroed || !elevator.hasZeroed)); + + bindTransition(SuperState.HOME, SuperState.IDLE, + Robot.homeReq.negate() + .and(() -> elevator.hasZeroed) + .and(() -> wrist.hasZeroed) + .and(() -> prevState != SuperState.READY_CORAL)); //TODO double check this prevstate thing + + bindTransition(SuperState.HOME, SuperState.READY_CORAL, + Robot.homeReq.negate() + .and(() -> elevator.hasZeroed) + .and(() -> wrist.hasZeroed) + .and(() -> prevState == SuperState.READY_CORAL)); + //getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the manipulator wheels + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_CLIMB, + Robot.preClimbReq + , funnel.unlatch() + ); - // READY_ALGAE -> SPIT_ALGAE - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preClimbReq) - .onTrue(forceState(SuperState.SPIT_ALGAE)); + // the manipulator wheels run at the same speed to spit algae and coral + bindTransition( + SuperState.READY_ALGAE, + SuperState.PRE_CLIMB, + Robot.preClimbReq + , funnel.unlatch() + ); - - // SPIT_ALGAE -> PRE_CLIMB - stateTriggers - .get(SuperState.SPIT_ALGAE) - // Positive bc algae is backwards - .whileTrue(manipulator.setVelocity(10)) - // Wait 1 second - .and(() -> stateTimer.hasElapsed(1)) - .and(preClimbReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); // ---Climb--- - // Climb could start from any state, so there's no particular transition - Robot.preClimbReq.onTrue( - Commands.parallel(changeStateTo(SuperState.PRE_CLIMB), funnel.unlatch())); - bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, Robot.climbConfReq); + bindTransition(SuperState.IDLE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); + + bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, Robot.climbConfReq.and(manipulator::neitherBeambreak)); // May need more checks to see if canceling is safe bindTransition(SuperState.CLIMB, SuperState.PRE_CLIMB, Robot.climbCancelReq); - } + bindTransition(SuperState.PRE_CLIMB, SuperState.IDLE, Robot.preClimbReq.negate()); + + + // ANTI_JAM logic + + // anti coral jam could start from any state, so there's no explicit transition + // in fact the state doesn't ever change--the manipulator velocity (and funnel velocity in robot.java) is just overridden + // which is why once the request is canceled, the manipulator state is manually set back to the normal value for that state + // setSubstates isn't called every loop, so I don't think it can be set there + // i have a bad feeling about this though + Robot.antiCoralJamReq + .onTrue(Commands.runOnce( + () -> { + antiJamCoral = true; + manipulator.setState(-10); + } + )) + .onFalse(Commands.runOnce( + () -> { + antiJamCoral = false; + manipulator.setState(state.manipulatorVelocity); + } + )); + + Robot.antiAlgaeJamReq + .onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); + + bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiAlgaeJamReq.negate()); + } public SuperState getState() { return state; @@ -742,4 +730,8 @@ public boolean stateIsAlgaeAlike() { || state == SuperState.PROCESSOR; // SPIT_ALGAE, } + + public boolean antiJamCoral() { + return antiJamCoral; + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 13f18c75..7e99b356 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -11,10 +11,14 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; + +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -70,7 +74,9 @@ public enum ElevatorState { INTAKE_ALGAE_GROUND(0.14 - Units.inchesToMeters(0.75)), READY_ALGAE(0.1), BARGE(Units.inchesToMeters(62.5)), - PROCESSOR(Units.inchesToMeters(0.01)) // lmao + PROCESSOR(Units.inchesToMeters(0.01)), // lmao + HOME(0.0), //NOT ACTUALLY 0!!! + ANTIJAM_ALGAE(0.0) //NOT ACTUALLY 0!!! ; private final double extensionMeters; @@ -111,9 +117,15 @@ public double getExtensionMeters() { private final LoggedMechanismLigament2d carriage = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); + //i hate myself + private Supplier shoulderAngleSupplier; + private BooleanSupplier wristAtAngleSupplier; + /** Creates a new ElevatorSubsystem. */ - public ElevatorSubsystem(ElevatorIO io) { + public ElevatorSubsystem(ElevatorIO io, Supplier shoulderAngleSupplier, BooleanSupplier wristAtAngleSupplier) { this.io = io; + this.shoulderAngleSupplier = shoulderAngleSupplier; + this.wristAtAngleSupplier = wristAtAngleSupplier; } @Override @@ -149,7 +161,15 @@ public Command setExtension(double meters) { } public Command setStateExtension() { - return setExtension(() -> state.getExtensionMeters()); + if (state == ElevatorState.HOME) { + return runCurrentZeroing(); + } else if (state == ElevatorState.ANTIJAM_ALGAE) { + return Commands.sequence( + setExtension(() -> inputs.positionMeters) + .until(() -> wristAtAngleSupplier.getAsBoolean() && shoulderAngleSupplier.get().getDegrees() < 10.0), + setExtension(Units.inchesToMeters(40))); + } else { + return setExtension(() -> state.getExtensionMeters());} } public boolean atExtension(double expected) { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index a3676566..2f546bc3 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -84,7 +84,9 @@ public enum ShoulderState { READY_ALGAE(Rotation2d.fromDegrees(60.0)), PRE_BARGE(Rotation2d.fromDegrees(30)), SCORE_BARGE(Rotation2d.fromDegrees(90)), - PROCESSOR(Rotation2d.fromDegrees(60.0)); + PROCESSOR(Rotation2d.fromDegrees(60.0)), + HOME(Rotation2d.fromDegrees(50.0)) + ; // L4_TUCKED(Rotation2d.fromDegrees(35.0)), // SHOULDER_TUCKED_CLEARANCE_POS // L4_TUCKED_OUT(Rotation2d.fromDegrees(25.0)), diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 65b1fb96..f0624800 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -81,7 +81,7 @@ public class SwerveSubsystem extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); private Rotation2d lastGyroRotation = new Rotation2d(); - private static SwerveDrivePoseEstimator estimator; + private static SwerveDrivePoseEstimator estimator; //TODO i forgot why this is static private double lastOdometryUpdateTimestamp = 0.0; final Pose3d[] cameraPoses; @@ -105,7 +105,7 @@ public SwerveSubsystem( CameraIO algaeCameraIO) { this.constants = constants; this.kinematics = new SwerveDriveKinematics(constants.getModuleTranslations()); - this.estimator = + estimator = new SwerveDrivePoseEstimator( kinematics, rawGyroRotation, diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index c11a362a..a312638d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,7 +7,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.subsystems.shoulder.ShoulderIOInputsAutoLogged; + import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -65,7 +65,8 @@ public enum WristState { READY_ALGAE(Rotation2d.fromDegrees(20)), PRE_BARGE(Rotation2d.fromDegrees(100)), SCORE_BARGE(Rotation2d.fromDegrees(110)), - PROCESSOR(Rotation2d.fromDegrees(-30.0)) + PROCESSOR(Rotation2d.fromDegrees(-30.0)), + HOME(Rotation2d.kZero) ; private final Rotation2d angle; @@ -93,8 +94,12 @@ public Rotation2d getAngle() { @AutoLogOutput(key = "Wrist/Has Zeroed") public boolean hasZeroed = false; - public WristSubsystem(WristIO io) { + //i hate myself + private Supplier shoulderAngleSupplier; + + public WristSubsystem(WristIO io, Supplier shoulderAngleSupplier) { this.io = io; + this.shoulderAngleSupplier = shoulderAngleSupplier; } @Override @@ -108,8 +113,13 @@ public void setState(WristState state) { } public Command setStateAngle() { + if (state == WristState.HOME) { + return Commands.waitUntil(() -> shoulderAngleSupplier.get().getDegrees() > 20.0) + .andThen(currentZero()); + } else { return setAngle(() -> state.getAngle()); } + } public Command setAngle(final Supplier target) { return this.run( @@ -140,7 +150,11 @@ public boolean isNearAngle(Rotation2d target) { return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); } - public Command currentZero(Supplier shoulderInputs) { + public boolean atSetpoint() { + return isNearAngle(setpoint); + } + + public Command currentZero() { return Commands.sequence( this.runOnce( () -> { From a6de7358f05bea6cb5c6fafc22926f279dd642a8 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 20:45:44 -0700 Subject: [PATCH 096/154] fix camera reference to state --- src/main/java/frc/robot/Autos.java | 14 +- src/main/java/frc/robot/Robot.java | 269 ++++++------ .../subsystems/ManipulatorSubsystem.java | 6 +- .../frc/robot/subsystems/Superstructure.java | 412 ++++++++++-------- .../frc/robot/subsystems/camera/Camera.java | 7 +- .../subsystems/climber/ClimberIOReal.java | 2 +- .../subsystems/elevator/ElevatorIOReal.java | 2 +- .../elevator/ElevatorSubsystem.java | 32 +- .../frc/robot/subsystems/roller/RollerIO.java | 1 - .../subsystems/shoulder/ShoulderIOReal.java | 2 +- .../shoulder/ShoulderSubsystem.java | 20 +- .../subsystems/swerve/SwerveSubsystem.java | 3 +- .../subsystems/wrist/WristSubsystem.java | 25 +- src/main/java/frc/robot/utils/FieldUtils.java | 97 +++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 1 - 15 files changed, 469 insertions(+), 424 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f5bc2b76..b83e1bf7 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -733,23 +733,25 @@ public Command intakeAlgaeInAuto(Supplier> pose) { AutoAim.translateToPose( swerve, () -> - FieldUtils.AlgaeIntakeTargets.getOffsetLocation( - FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) + FieldUtils.AlgaeIntakeTargets.getOffsetLocation( + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) .until( () -> AutoAim.isInTolerance( swerve.getPose(), FieldUtils.AlgaeIntakeTargets.getOffsetLocation( - FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())), + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose( + swerve.getPose())), swerve.getVelocityFieldRelative(), Units.inchesToMeters(0.5), Units.degreesToRadians(1.0)) && elevator.atExtension() - && shoulder.isNearAngle( - ShoulderState.INTAKE_ALGAE_REEF.getAngle()) + && shoulder.isNearAngle(ShoulderState.INTAKE_ALGAE_REEF.getAngle()) && wrist.isNearAngle(WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( - swerve, () -> FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) + swerve, + () -> FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), + 1) .withTimeout(0.5)) .andThen( Commands.runOnce( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3006a8d5..81377dbb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,30 +10,6 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.elevator.ElevatorSubsystem.ELEVATOR_ANGLE; -import java.util.HashMap; -import java.util.Optional; -import java.util.Set; -import java.util.function.BiConsumer; -import java.util.function.Supplier; -import java.util.stream.Stream; - -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.GyroSimulation; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; - import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.SignalLogger; @@ -44,7 +20,6 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -117,6 +92,28 @@ import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; +import java.util.HashMap; +import java.util.Optional; +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.Supplier; +import java.util.stream.Stream; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { public enum RobotType { @@ -181,110 +178,112 @@ public static enum AlgaeScoreTarget { private static CANBusStatus canivoreStatus = canivore.getStatus(); - private static final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); - private static final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + private static final CommandXboxControllerSubsystem driver = + new CommandXboxControllerSubsystem(0); + private static final CommandXboxControllerSubsystem operator = + new CommandXboxControllerSubsystem(1); - private static Supplier pose; + private static Supplier pose = () -> new Pose2d(); public static Trigger preScoreReq = - driver - .rightTrigger() - .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) - .or( - () -> - pose.get() - .getTranslation() - .minus( - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance.Blue - ? AutoAim.BLUE_REEF_CENTER - : AutoAim.RED_REEF_CENTER) - .getNorm() - < 3.25 - && DriverStation.isAutonomous()); + driver + .rightTrigger() + .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) + .or( + () -> + pose.get() + .getTranslation() + .minus( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_REEF_CENTER + : AutoAim.RED_REEF_CENTER) + .getNorm() + < 3.25 + && DriverStation.isAutonomous()); @AutoLogOutput - public static Trigger scoreReq = driver - .rightTrigger() - .negate() - .and(() -> DriverStation.isTeleop()) - // .or( - // new Trigger( - // () -> { - // final var state = - // new ExtensionState( - // elevator.getExtensionMeters(), - // shoulder.getAngle(), - // wrist.getAngle()); - // final var branch = - // ExtensionKinematics.getBranchPose( - // swerve.getPose(), state, currentTarget); - // final var manipulatorPose = - // ExtensionKinematics.getManipulatorPose(swerve.getPose(), - // state); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Branch", branch); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput( - // "IK/Extension Check", - // manipulatorPose, - // manipulatorPose.transformBy( - // new Transform3d( - // Units.inchesToMeters(3.0), 0.0, 0.0, new - // Rotation3d()))); - // return false; - // // return branch - // // .getTranslation() - // // .getDistance(manipulatorPose.getTranslation()) - // // < Units.inchesToMeters(1.5) - // // || branch - // // .getTranslation() - // // .getDistance( - // // manipulatorPose - // // .transformBy( - // // new Transform3d( - // // Units.inchesToMeters(3.0), - // // 0.0, - // // 0.0, - // // new Rotation3d())) - // // .getTranslation()) - // // < Units.inchesToMeters(1.5); - // }) - // .debounce(0.15)) - // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore && DriverStation.isAutonomous()); + public static Trigger scoreReq = + driver + .rightTrigger() + .negate() + .and(() -> DriverStation.isTeleop()) + // .or( + // new Trigger( + // () -> { + // final var state = + // new ExtensionState( + // elevator.getExtensionMeters(), + // shoulder.getAngle(), + // wrist.getAngle()); + // final var branch = + // ExtensionKinematics.getBranchPose( + // swerve.getPose(), state, currentTarget); + // final var manipulatorPose = + // ExtensionKinematics.getManipulatorPose(swerve.getPose(), + // state); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("IK/Branch", branch); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput( + // "IK/Extension Check", + // manipulatorPose, + // manipulatorPose.transformBy( + // new Transform3d( + // Units.inchesToMeters(3.0), 0.0, 0.0, new + // Rotation3d()))); + // return false; + // // return branch + // // .getTranslation() + // // .getDistance(manipulatorPose.getTranslation()) + // // < Units.inchesToMeters(1.5) + // // || branch + // // .getTranslation() + // // .getDistance( + // // manipulatorPose + // // .transformBy( + // // new Transform3d( + // // Units.inchesToMeters(3.0), + // // 0.0, + // // 0.0, + // // new Rotation3d())) + // // .getTranslation()) + // // < Units.inchesToMeters(1.5); + // }) + // .debounce(0.15)) + // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) + .or(() -> Autos.autoScore && DriverStation.isAutonomous()); @AutoLogOutput - public static Trigger intakeAlgaeReq = driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()); + public static Trigger intakeAlgaeReq = + driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()); @AutoLogOutput - public static Trigger intakeCoralReq = driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()); + public static Trigger intakeCoralReq = + driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()); @AutoLogOutput(key = "Superstructure/Pre Climb Request") - public static Trigger preClimbReq = driver - .x() - .and(driver.pov(-1).negate()) - .debounce(0.5) - .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)); + public static Trigger preClimbReq = + driver + .x() + .and(driver.pov(-1).negate()) + .debounce(0.5) + .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)); @AutoLogOutput(key = "Superstructure/Climb Confirm Request") - public static Trigger climbConfReq = driver.rightTrigger(); + public static Trigger climbConfReq = driver.rightTrigger(); @AutoLogOutput(key = "Superstructure/Climb Cancel Request") public static Trigger climbCancelReq = - driver - .y() - .debounce(0.5) - .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); + driver.y().debounce(0.5).or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") public static final Trigger antiCoralJamReq = driver.a(); @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") public static final Trigger antiAlgaeJamReq = driver.b(); - + @AutoLogOutput(key = "Superstructure/Home Request") public static Trigger homeReq = driver.start(); @@ -294,15 +293,14 @@ public static enum AlgaeScoreTarget { @AutoLogOutput(key = "Superstructure/Force Funnel Req") public static Trigger forceFunnelReq = operator.leftBumper(); - @AutoLogOutput(key = "Superstructure/Force Index Req") //TODO what? + @AutoLogOutput(key = "Superstructure/Force Index Req") // TODO what? public static Trigger forceIndexReq = operator.povDown(); - - //killVisionIK, DoubleSupplier coralAdjust) + // killVisionIK, DoubleSupplier coralAdjust) // new Trigger(() -> killVisionIK) -// .or(() -> coralTarget == ReefTarget.L1) -// .or(() -> DriverStation.isAutonomous()), -// () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); + // .or(() -> coralTarget == ReefTarget.L1) + // .or(() -> DriverStation.isAutonomous()), + // () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); // Create and configure a drivetrain simulation configuration private Optional driveTrainSimulationConfig = @@ -398,8 +396,6 @@ public static enum AlgaeScoreTarget { ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); - - private final ManipulatorSubsystem manipulator = new ManipulatorSubsystem( ROBOT_TYPE != RobotType.SIM @@ -454,7 +450,7 @@ public static enum AlgaeScoreTarget { .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(0.5))) : new WristIOSim(), - () -> shoulder.getAngle()); + () -> shoulder.getAngle()); private final ElevatorSubsystem elevator = new ElevatorSubsystem( ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim(), @@ -483,7 +479,7 @@ public static enum AlgaeScoreTarget { private final ClimberSubsystem climber = new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); - private final Superstructure superstructure = + private final Superstructure superstructure = new Superstructure(elevator, shoulder, wrist, manipulator, funnel, swerve); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -504,8 +500,7 @@ public static enum AlgaeScoreTarget { private final LoggedMechanismLigament2d carriageLigament = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d shoulderLigament = - new LoggedMechanismLigament2d( - "Arm", Units.inchesToMeters(15.7), 90.0); + new LoggedMechanismLigament2d("Arm", Units.inchesToMeters(15.7), 90.0); private final LoggedMechanismLigament2d wristLigament = new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); @@ -675,7 +670,9 @@ public Robot() { elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); - manipulator.setDefaultCommand(manipulator.setStateVelocity(() -> superstructure.atExtension() || superstructure.antiJamCoral())); + manipulator.setDefaultCommand( + manipulator.setStateVelocity( + () -> superstructure.atExtension() || superstructure.antiJamCoral())); funnel.setDefaultCommand( funnel.setRollerVoltage( () -> @@ -685,13 +682,16 @@ public Robot() { || (Stream.of(FieldUtils.HumanPlayerTargets.values()) .map( (t) -> - t.location.minus(swerve.getPose()).getTranslation().getNorm()) //TODO pose needs to be changed + t.location + .minus(swerve.getPose()) + .getTranslation() + .getNorm()) // TODO pose needs to be changed .min(Double::compare) .get() < 1.0) ? 1.0 - : (superstructure.antiJamCoral() - ? -10.0 + : (superstructure.antiJamCoral() + ? -10.0 : 0.0)))); // at what point do ternary operators do more harm than good climber.setDefaultCommand( climber.setPosition( @@ -732,9 +732,6 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed()) .times(-1))); - //????? - pose = () -> swerve.getPose(); - driver .rightBumper() .or(driver.leftBumper()) @@ -818,8 +815,7 @@ public Robot() { && elevator.atExtension() && shoulder.isNearAngle( ShoulderState.INTAKE_ALGAE_REEF.getAngle()) - && wrist.isNearAngle( - WristState.INTAKE_ALGAE_REEF.getAngle())), + && wrist.isNearAngle(WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), @@ -948,11 +944,13 @@ public Robot() { driver .povUp() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSimFirstBeambreak(true)).ignoringDisable(true)); + .onTrue( + Commands.runOnce(() -> manipulator.setSimFirstBeambreak(true)).ignoringDisable(true)); driver .povDown() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSimFirstBeambreak(false)).ignoringDisable(true)); + .onTrue( + Commands.runOnce(() -> manipulator.setSimFirstBeambreak(false)).ignoringDisable(true)); driver .povRight() .and(() -> ROBOT_TYPE == RobotType.SIM) @@ -960,7 +958,8 @@ public Robot() { RobotModeTriggers.autonomous() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); + .onTrue( + Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); driver .start() .onTrue( @@ -1005,7 +1004,9 @@ public Robot() { algaeIntakeTarget = AlgaeIntakeTarget.STACK; })); - operator.leftTrigger().onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.BARGE)); + operator + .leftTrigger() + .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.BARGE)); operator .rightTrigger() @@ -1127,8 +1128,7 @@ public void robotPeriodic() { "MapleSim/Pose", swerveDriveSimulation.get().getSimulatedDriveTrainPose()); } - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Targets/Reef Target", coralTarget); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Targets/Reef Target", coralTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Targets/Algae Intake Target", algaeIntakeTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) @@ -1246,6 +1246,7 @@ public void robotPeriodic() { Logger.recordOutput("Autos/Pre Score", Autos.autoPreScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); state = superstructure::getState; + pose = swerve::getPose; } public static void setCoralTarget(ReefTarget target) { diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 4c861d3d..b4747fac 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -15,7 +15,6 @@ import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; - import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -23,7 +22,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; - public static final double MAX_VELOCITY = 20; //holy cooked + public static final double MAX_VELOCITY = 20; // holy cooked public static final double CORAL_INTAKE_VELOCITY = -18.0; public static final double JOG_POS = 0.75; @@ -80,7 +79,6 @@ public void periodic() { Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); } - public void resetPosition(final double rotations) { io.resetEncoder(rotations); } @@ -136,7 +134,7 @@ public void setSimFirstBeambreak(boolean b) { public void setSimSecondBeambreak(boolean b) { bb2Sim = b; } - + public void setSimHasAlgae(boolean state) { hasAlgaeSim = state; } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 7d537e5a..a6bb0c65 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,9 +1,5 @@ package frc.robot.subsystems; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -25,9 +21,11 @@ import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.autoaim.AutoAim; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; public class Superstructure { - /** + /** * We should have a state for every single "pose" the robot will hit. Hopefully we can get named * positions set up in cad to make this easier? */ @@ -134,18 +132,12 @@ public enum SuperState { 1.35, 0.5) // lowkey why is this so slow , - HOME( - ElevatorState.HOME, - ShoulderState.HOME, - WristState.HOME, - 0.0 - ), - ANTIJAM_ALGAE( - ElevatorState.ANTIJAM_ALGAE, - ShoulderState.INTAKE_CORAL_GROUND, - WristState.INTAKE_CORAL_GROUND, - 0.0 - ) + HOME(ElevatorState.HOME, ShoulderState.HOME, WristState.HOME, 0.0), + ANTIJAM_ALGAE( + ElevatorState.ANTIJAM_ALGAE, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + 0.0) // SPIT_CORAL(), // ANTI_JAM, // L4_TUCKED(ElevatorState.HP, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), @@ -204,25 +196,25 @@ private SuperState( public static boolean antiJamCoral; - /** Creates a new Superstructure. */ - public Superstructure( - ElevatorSubsystem elevator, - ShoulderSubsystem shoulder, - WristSubsystem wrist, - ManipulatorSubsystem manipulator, - FunnelSubsystem funnel, - SwerveSubsystem swerve) { - this.elevator = elevator; - this.shoulder = shoulder; - this.wrist = wrist; - this.manipulator = manipulator; - this.funnel = funnel; - this.swerve = swerve; - - addTransitions(); - - stateTimer.start(); - } + /** Creates a new Superstructure. */ + public Superstructure( + ElevatorSubsystem elevator, + ShoulderSubsystem shoulder, + WristSubsystem wrist, + ManipulatorSubsystem manipulator, + FunnelSubsystem funnel, + SwerveSubsystem swerve) { + this.elevator = elevator; + this.shoulder = shoulder; + this.wrist = wrist; + this.manipulator = manipulator; + this.funnel = funnel; + this.swerve = swerve; + + addTransitions(); + + stateTimer.start(); + } /** This file is not a subsystem, so this MUST be called manually. */ public void periodic() { @@ -230,7 +222,7 @@ public void periodic() { Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); } - /** + /** * @param start first state * @param end second state * @param trigger trigger to make it go from the first state to the second (assuming it's already @@ -240,7 +232,7 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger) { // maps triggers to the transitions trigger.and(new Trigger(() -> state == start)).onTrue(changeStateTo(end)); } - /** + /** * @param start first state * @param end second state * @param trigger trigger to make it go from the first state to the second (assuming it's already @@ -249,7 +241,9 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger) { */ private void bindTransition(SuperState start, SuperState end, Trigger trigger, Command cmd) { // maps triggers to the transitions - trigger.and(new Trigger(() -> state == start)).onTrue(Commands.parallel(changeStateTo(end), cmd)); + trigger + .and(new Trigger(() -> state == start)) + .onTrue(Commands.parallel(changeStateTo(end), cmd)); } private boolean atExtension(SuperState state) { @@ -283,14 +277,13 @@ private void setSubstates() { shoulder.setState(state.shoulderState); wrist.setState(state.wristState); manipulator.setState(state.manipulatorVelocity); - //funnel and climber don't have states per se + // funnel and climber don't have states per se } - private void addTransitions() { // Prob a better way to impl this // Vaughn says he wants this available anytime - //TODO this will probably not still work + // TODO this will probably not still work Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); // ---Funnel--- @@ -328,9 +321,12 @@ private void addTransitions() { new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L1).and(Robot.preScoreReq)); bindTransition(SuperState.PRE_L1, SuperState.L1, Robot.scoreReq.and(this::atExtension)); - - //cancel - bindTransition(SuperState.PRE_L1, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L1)); + + // cancel + bindTransition( + SuperState.PRE_L1, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L1)); bindTransition( SuperState.L1, @@ -340,19 +336,24 @@ private void addTransitions() { .and(Robot.scoreReq.negate())); bindTransition( - SuperState.POST_L1, - SuperState.IDLE, - Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( - () -> - L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + SuperState.POST_L1, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); - //go straight to intaking algae from reef + // go straight to intaking algae from reef bindTransition( - SuperState.POST_L1, + SuperState.POST_L1, SuperState.PRE_PRE_INTAKE_ALGAE, - new Trigger(this::atExtension) - .and(Robot.intakeAlgaeReq) - .and(() -> intakeAlgaeFromReef())); + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); // ---L2--- bindTransition( @@ -362,8 +363,11 @@ private void addTransitions() { bindTransition(SuperState.PRE_L2, SuperState.L2, Robot.scoreReq.and(this::atExtension)); - //cancel - bindTransition(SuperState.PRE_L2, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L2)); + // cancel + bindTransition( + SuperState.PRE_L2, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L2)); bindTransition( SuperState.L2, @@ -373,19 +377,24 @@ private void addTransitions() { .and(Robot.scoreReq.negate())); bindTransition( - SuperState.POST_L2, - SuperState.IDLE, - Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( - () -> - L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + SuperState.POST_L2, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); - //go straight to intaking algae from reef + // go straight to intaking algae from reef bindTransition( - SuperState.POST_L2, + SuperState.POST_L2, SuperState.PRE_PRE_INTAKE_ALGAE, - new Trigger(this::atExtension) - .and(Robot.intakeAlgaeReq) - .and(() -> intakeAlgaeFromReef())); + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); // ---L3--- bindTransition( @@ -395,8 +404,11 @@ private void addTransitions() { bindTransition(SuperState.PRE_L3, SuperState.L3, Robot.scoreReq.and(this::atExtension)); - //cancel - bindTransition(SuperState.PRE_L3, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L3)); + // cancel + bindTransition( + SuperState.PRE_L3, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L3)); bindTransition( SuperState.L3, @@ -406,19 +418,24 @@ private void addTransitions() { .and(Robot.scoreReq.negate())); bindTransition( - SuperState.POST_L3, - SuperState.IDLE, - Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( - () -> - L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + SuperState.POST_L3, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); - //go straight to intaking algae from reef + // go straight to intaking algae from reef bindTransition( - SuperState.POST_L3, + SuperState.POST_L3, SuperState.PRE_PRE_INTAKE_ALGAE, - new Trigger(this::atExtension) - .and(Robot.intakeAlgaeReq) - .and(() -> intakeAlgaeFromReef())); + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); // ---L4--- bindTransition( @@ -430,8 +447,11 @@ private void addTransitions() { bindTransition(SuperState.PRE_L4, SuperState.L4, Robot.scoreReq.and(this::atExtension)); - //cancel - bindTransition(SuperState.PRE_L4, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); + // cancel + bindTransition( + SuperState.PRE_L4, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); bindTransition( SuperState.L4, @@ -439,23 +459,28 @@ private void addTransitions() { new Trigger(() -> manipulator.neitherBeambreak()) .and(this::atExtension) .and(Robot.scoreReq.negate())); - + bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); bindTransition( SuperState.POST_POST_L4, - SuperState.IDLE, - Robot.intakeAlgaeReq.negate().or(() -> !intakeAlgaeFromReef()).and(this::atExtension).and( - () -> - L1Targets.getNearestLine(swerve.getPose()).getDistance(swerve.getPose().getTranslation()) > 0.3).debounce(0.15)); + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); - //go straight to intaking algae from reef + // go straight to intaking algae from reef bindTransition( SuperState.POST_POST_L4, SuperState.PRE_PRE_INTAKE_ALGAE, - new Trigger(this::atExtension) - .and(Robot.intakeAlgaeReq) - .and(() -> intakeAlgaeFromReef())); + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); // ---Intake Algae--- bindTransition(SuperState.IDLE, SuperState.PRE_PRE_INTAKE_ALGAE, Robot.intakeAlgaeReq); @@ -472,7 +497,7 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW))); - //cancel + // cancel bindTransition( SuperState.INTAKE_ALGAE_LOW, SuperState.IDLE, @@ -484,13 +509,13 @@ private void addTransitions() { SuperState.READY_ALGAE, new Trigger(() -> stateTimer.hasElapsed(1.0)) .and(manipulator::hasAlgae) - .and(() -> - AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) - .getTranslation() - .minus(swerve.getPose().getTranslation()) - .getNorm() - > 0.3 - )); + .and( + () -> + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.3)); // ---Intake High Algae--- bindTransition( @@ -499,7 +524,7 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH))); - //cancel + // cancel bindTransition( SuperState.INTAKE_ALGAE_HIGH, SuperState.IDLE, @@ -511,13 +536,13 @@ private void addTransitions() { SuperState.READY_ALGAE, new Trigger(() -> stateTimer.hasElapsed(1.0)) .and(manipulator::hasAlgae) - .and(() -> - AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) - .getTranslation() - .minus(swerve.getPose().getTranslation()) - .getNorm() - > 0.3 - )); + .and( + () -> + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.3)); // ---Intake Stack Algae--- bindTransition( @@ -526,7 +551,7 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.STACK))); - //cancel + // cancel bindTransition( SuperState.INTAKE_ALGAE_STACK, SuperState.IDLE, @@ -536,8 +561,7 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_STACK, SuperState.READY_ALGAE, - new Trigger(() -> stateTimer.hasElapsed(1.0)) - .and(manipulator::hasAlgae)); + new Trigger(() -> stateTimer.hasElapsed(1.0)).and(manipulator::hasAlgae)); // ---Intake Ground Algae--- bindTransition( @@ -546,7 +570,7 @@ private void addTransitions() { new Trigger(this::atExtension) .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.GROUND))); - //cancel + // cancel bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.IDLE, @@ -556,8 +580,7 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.READY_ALGAE, - new Trigger(() -> stateTimer.hasElapsed(1.0)) - .and(manipulator::hasAlgae)); + new Trigger(() -> stateTimer.hasElapsed(1.0)).and(manipulator::hasAlgae)); // ---Score in barge--- bindTransition( @@ -607,90 +630,98 @@ private void addTransitions() { DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? AutoAim.BLUE_PROCESSOR_POS.getY() : AutoAim.RED_PROCESSOR_POS.getY(), - 0.5)) - ); + 0.5))); // i might be insane for this - bindTransition(SuperState.IDLE, SuperState.HOME, Robot.homeReq, Commands.runOnce( - () -> { - elevator.hasZeroed = false; - wrist.hasZeroed = false; - })); - - //im not sure why this needs to exist separately - bindTransition( - SuperState.IDLE, - SuperState.HOME, - new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed) - .and(DriverStation::isEnabled)); - - bindTransition(SuperState.READY_CORAL, SuperState.HOME, new Trigger(() -> !wrist.hasZeroed || !elevator.hasZeroed)); - - bindTransition(SuperState.HOME, SuperState.IDLE, - Robot.homeReq.negate() - .and(() -> elevator.hasZeroed) - .and(() -> wrist.hasZeroed) - .and(() -> prevState != SuperState.READY_CORAL)); //TODO double check this prevstate thing - - bindTransition(SuperState.HOME, SuperState.READY_CORAL, - Robot.homeReq.negate() - .and(() -> elevator.hasZeroed) - .and(() -> wrist.hasZeroed) - .and(() -> prevState == SuperState.READY_CORAL)); - - //getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the manipulator wheels - bindTransition( - SuperState.READY_CORAL, - SuperState.PRE_CLIMB, - Robot.preClimbReq - , funnel.unlatch() - ); + bindTransition( + SuperState.IDLE, + SuperState.HOME, + Robot.homeReq, + Commands.runOnce( + () -> { + elevator.hasZeroed = false; + wrist.hasZeroed = false; + })); - // the manipulator wheels run at the same speed to spit algae and coral + // im not sure why this needs to exist separately + bindTransition( + SuperState.IDLE, + SuperState.HOME, + new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed).and(DriverStation::isEnabled)); + + bindTransition( + SuperState.READY_CORAL, + SuperState.HOME, + new Trigger(() -> !wrist.hasZeroed || !elevator.hasZeroed)); + + bindTransition( + SuperState.HOME, + SuperState.IDLE, + Robot.homeReq + .negate() + .and(() -> elevator.hasZeroed) + .and(() -> wrist.hasZeroed) + .and( + () -> + prevState != SuperState.READY_CORAL)); // TODO double check this prevstate thing + + bindTransition( + SuperState.HOME, + SuperState.READY_CORAL, + Robot.homeReq + .negate() + .and(() -> elevator.hasZeroed) + .and(() -> wrist.hasZeroed) + .and(() -> prevState == SuperState.READY_CORAL)); + + // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the + // manipulator wheels bindTransition( - SuperState.READY_ALGAE, - SuperState.PRE_CLIMB, - Robot.preClimbReq - , funnel.unlatch() - ); + SuperState.READY_CORAL, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); + // the manipulator wheels run at the same speed to spit algae and coral + bindTransition( + SuperState.READY_ALGAE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); - // ---Climb--- + // ---Climb--- bindTransition(SuperState.IDLE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); - bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, Robot.climbConfReq.and(manipulator::neitherBeambreak)); + bindTransition( + SuperState.PRE_CLIMB, + SuperState.CLIMB, + Robot.climbConfReq.and(manipulator::neitherBeambreak)); // May need more checks to see if canceling is safe bindTransition(SuperState.CLIMB, SuperState.PRE_CLIMB, Robot.climbCancelReq); bindTransition(SuperState.PRE_CLIMB, SuperState.IDLE, Robot.preClimbReq.negate()); - // ANTI_JAM logic // anti coral jam could start from any state, so there's no explicit transition - // in fact the state doesn't ever change--the manipulator velocity (and funnel velocity in robot.java) is just overridden - // which is why once the request is canceled, the manipulator state is manually set back to the normal value for that state + // in fact the state doesn't ever change--the manipulator velocity (and funnel velocity in + // robot.java) is just overridden + // which is why once the request is canceled, the manipulator state is manually set back to the + // normal value for that state // setSubstates isn't called every loop, so I don't think it can be set there // i have a bad feeling about this though Robot.antiCoralJamReq - .onTrue(Commands.runOnce( - () -> { - antiJamCoral = true; - manipulator.setState(-10); - } - )) - .onFalse(Commands.runOnce( - () -> { - antiJamCoral = false; - manipulator.setState(state.manipulatorVelocity); - } - )); - - Robot.antiAlgaeJamReq - .onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); - + .onTrue( + Commands.runOnce( + () -> { + antiJamCoral = true; + manipulator.setState(-10); + })) + .onFalse( + Commands.runOnce( + () -> { + antiJamCoral = false; + manipulator.setState(state.manipulatorVelocity); + })); + + Robot.antiAlgaeJamReq.onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); + bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiAlgaeJamReq.negate()); } @@ -715,20 +746,27 @@ public boolean stateIsCoralAlike() { || state == SuperState.L4; } + public static boolean stateIsScoreCoral(SuperState state) { + return state == SuperState.L1 + || state == SuperState.L2 + || state == SuperState.L3 + || state == SuperState.L4; + } + public boolean stateIsAlgaeAlike() { return state == SuperState.PRE_PRE_INTAKE_ALGAE - || state == SuperState.PRE_INTAKE_ALGAE - || state == SuperState.INTAKE_ALGAE_HIGH - || state == SuperState.INTAKE_ALGAE_LOW - || state == SuperState.INTAKE_ALGAE_STACK - || state == SuperState.INTAKE_ALGAE_GROUND - || state == SuperState.READY_ALGAE - || state == SuperState.PRE_BARGE - || state == SuperState.BARGE - || state == SuperState.POST_BARGE - || state == SuperState.POST_POST_BARGE - || state == SuperState.PROCESSOR; - // SPIT_ALGAE, + || state == SuperState.PRE_INTAKE_ALGAE + || state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.READY_ALGAE + || state == SuperState.PRE_BARGE + || state == SuperState.BARGE + || state == SuperState.POST_BARGE + || state == SuperState.POST_POST_BARGE + || state == SuperState.PROCESSOR; + // SPIT_ALGAE, } public boolean antiJamCoral() { diff --git a/src/main/java/frc/robot/subsystems/camera/Camera.java b/src/main/java/frc/robot/subsystems/camera/Camera.java index e6b9f2e7..0785bdaf 100644 --- a/src/main/java/frc/robot/subsystems/camera/Camera.java +++ b/src/main/java/frc/robot/subsystems/camera/Camera.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.RobotController; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.Tracer; @@ -166,7 +167,7 @@ public void updateCamera() { (getName().equals("Front_Left_Camera") || getName().equals("Front_Right_Camera")) && (Robot.state.get().toString().startsWith("PRE_L") - || Robot.state.get() == SuperState.SCORE_CORAL + || Superstructure.stateIsScoreCoral(Robot.state.get()) || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) ? 0.5 @@ -197,14 +198,14 @@ public void updateCamera() { || t.getFiducialId() == 14) ? 1.2 : 1.0) - .times(Robot.state.get() == SuperState.PRE_NET ? 0.5 : 1.0)); + .times(Robot.state.get() == SuperState.PRE_BARGE ? 0.5 : 1.0)); // the sussifier }); hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime(); // if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update"); - + Tracer.trace( "Log Tag Poses", () -> { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index 3247d897..b084ff01 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -47,7 +47,7 @@ public ClimberIOReal() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(config); + motor.getConfigurator().apply(config); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, angularVelocityRPS, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index a1859421..523e1198 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -39,7 +39,7 @@ public class ElevatorIOReal implements ElevatorIO { private final StatusSignal temp = motor.getDeviceTemp(); public ElevatorIOReal() { - //i'm just going to trust this for chezy i guess + // i'm just going to trust this for chezy i guess var config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 7e99b356..9c2a2834 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -15,11 +15,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; - import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; @@ -37,7 +35,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double Z_OFFSET_METERS = Units.inchesToMeters(8.175000); public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - + public static final double MAX_ACCELERATION = 10.0; public static final double SLOW_ACCELERATION = 5.0; public static final double MEDIUM_ACCELERATION = 8.5; @@ -75,8 +73,8 @@ public enum ElevatorState { READY_ALGAE(0.1), BARGE(Units.inchesToMeters(62.5)), PROCESSOR(Units.inchesToMeters(0.01)), // lmao - HOME(0.0), //NOT ACTUALLY 0!!! - ANTIJAM_ALGAE(0.0) //NOT ACTUALLY 0!!! + HOME(0.0), // NOT ACTUALLY 0!!! + ANTIJAM_ALGAE(0.0) // NOT ACTUALLY 0!!! ; private final double extensionMeters; @@ -90,7 +88,6 @@ public double getExtensionMeters() { } } - @AutoLogOutput(key = "Elevator/State") public ElevatorState state = ElevatorState.HP; @@ -117,12 +114,15 @@ public double getExtensionMeters() { private final LoggedMechanismLigament2d carriage = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); - //i hate myself + // i hate myself private Supplier shoulderAngleSupplier; private BooleanSupplier wristAtAngleSupplier; /** Creates a new ElevatorSubsystem. */ - public ElevatorSubsystem(ElevatorIO io, Supplier shoulderAngleSupplier, BooleanSupplier wristAtAngleSupplier) { + public ElevatorSubsystem( + ElevatorIO io, + Supplier shoulderAngleSupplier, + BooleanSupplier wristAtAngleSupplier) { this.io = io; this.shoulderAngleSupplier = shoulderAngleSupplier; this.wristAtAngleSupplier = wristAtAngleSupplier; @@ -146,7 +146,7 @@ public void periodic() { public void setState(ElevatorState state) { this.state = state; } - + public Command setExtension(DoubleSupplier meters) { return this.run( () -> { @@ -155,7 +155,7 @@ public Command setExtension(DoubleSupplier meters) { Logger.recordOutput("Elevator/Setpoint", setpoint); }); } - + public Command setExtension(double meters) { return this.setExtension(() -> meters); } @@ -165,11 +165,15 @@ public Command setStateExtension() { return runCurrentZeroing(); } else if (state == ElevatorState.ANTIJAM_ALGAE) { return Commands.sequence( - setExtension(() -> inputs.positionMeters) - .until(() -> wristAtAngleSupplier.getAsBoolean() && shoulderAngleSupplier.get().getDegrees() < 10.0), - setExtension(Units.inchesToMeters(40))); + setExtension(() -> inputs.positionMeters) + .until( + () -> + wristAtAngleSupplier.getAsBoolean() + && shoulderAngleSupplier.get().getDegrees() < 10.0), + setExtension(Units.inchesToMeters(40))); } else { - return setExtension(() -> state.getExtensionMeters());} + return setExtension(() -> state.getExtensionMeters()); + } } public boolean atExtension(double expected) { diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/roller/RollerIO.java index acbe6956..dbcdcf2b 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIO.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIO.java @@ -5,7 +5,6 @@ package frc.robot.subsystems.roller; import edu.wpi.first.math.geometry.Rotation2d; -import java.util.function.Consumer; import org.littletonrobotics.junction.AutoLog; public interface RollerIO { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index e88e5fa2..ab724233 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -140,7 +140,7 @@ public void setEncoderPosition(final Rotation2d rotation) { } @Override - //TODO i hate this can we get rid of this + // TODO i hate this can we get rid of this public void setMotionMagicConfigs(final MotionMagicConfigs configs) { motor.getConfigurator().apply(configs); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 2f546bc3..e55b2e88 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -1,13 +1,6 @@ package frc.robot.subsystems.shoulder; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; - import com.ctre.phoenix6.configs.MotionMagicConfigs; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -18,6 +11,10 @@ import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.utils.Tracer; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ShoulderSubsystem extends SubsystemBase { public static final double SHOULDER_FINAL_STAGE_RATIO = 3.0; @@ -30,7 +27,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - + // public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0 - 7); // public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); // public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = @@ -85,8 +82,7 @@ public enum ShoulderState { PRE_BARGE(Rotation2d.fromDegrees(30)), SCORE_BARGE(Rotation2d.fromDegrees(90)), PROCESSOR(Rotation2d.fromDegrees(60.0)), - HOME(Rotation2d.fromDegrees(50.0)) - ; + HOME(Rotation2d.fromDegrees(50.0)); // L4_TUCKED(Rotation2d.fromDegrees(35.0)), // SHOULDER_TUCKED_CLEARANCE_POS // L4_TUCKED_OUT(Rotation2d.fromDegrees(25.0)), @@ -146,7 +142,7 @@ public Command setStateAngle() { } @AutoLogOutput(key = "Shoulder/Zeroing Angle") - //what + // what public Rotation2d getZeroingAngle() { return Rotation2d.fromRotations(inputs.cancoderPosition).div(SHOULDER_FINAL_STAGE_RATIO); } @@ -183,7 +179,7 @@ public ShoulderIOInputsAutoLogged getInputs() { public double getVelocity() { return inputs.angularVelocityRPS; } - + public Rotation2d getAngle() { return inputs.motorPosition; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index f0624800..aeda0116 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -38,7 +38,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.camera.Camera; import frc.robot.subsystems.camera.CameraIO; import frc.robot.subsystems.swerve.OdometryThreadIO.OdometryThreadIOInputs; @@ -81,7 +80,7 @@ public class SwerveSubsystem extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); private Rotation2d lastGyroRotation = new Rotation2d(); - private static SwerveDrivePoseEstimator estimator; //TODO i forgot why this is static + private static SwerveDrivePoseEstimator estimator; // TODO i forgot why this is static private double lastOdometryUpdateTimestamp = 0.0; final Pose3d[] cameraPoses; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a312638d..9a73f3b4 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,9 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import java.util.function.Supplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -25,11 +23,15 @@ public class WristSubsystem extends SubsystemBase { // public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); // public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); // public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); - // public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L1_POS = + // ExtensionKinematics.L1_EXTENSION.wristAngle(); // public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - // public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L2_POS = + // ExtensionKinematics.L2_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L3_POS = + // ExtensionKinematics.L3_EXTENSION.wristAngle(); + // public static final Rotation2d WRIST_SCORE_L4_POS = + // ExtensionKinematics.L4_EXTENSION.wristAngle(); // public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); // public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); // public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); @@ -66,8 +68,7 @@ public enum WristState { PRE_BARGE(Rotation2d.fromDegrees(100)), SCORE_BARGE(Rotation2d.fromDegrees(110)), PROCESSOR(Rotation2d.fromDegrees(-30.0)), - HOME(Rotation2d.kZero) - ; + HOME(Rotation2d.kZero); private final Rotation2d angle; @@ -94,7 +95,7 @@ public Rotation2d getAngle() { @AutoLogOutput(key = "Wrist/Has Zeroed") public boolean hasZeroed = false; - //i hate myself + // i hate myself private Supplier shoulderAngleSupplier; public WristSubsystem(WristIO io, Supplier shoulderAngleSupplier) { @@ -115,10 +116,10 @@ public void setState(WristState state) { public Command setStateAngle() { if (state == WristState.HOME) { return Commands.waitUntil(() -> shoulderAngleSupplier.get().getDegrees() > 20.0) - .andThen(currentZero()); + .andThen(currentZero()); } else { - return setAngle(() -> state.getAngle()); - } + return setAngle(() -> state.getAngle()); + } } public Command setAngle(final Supplier target) { diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index a99645f1..8d80fd5e 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -4,12 +4,6 @@ package frc.robot.utils; -import java.util.Arrays; -import java.util.Collections; -import java.util.Comparator; -import java.util.List; -import java.util.stream.Stream; - import choreo.util.ChoreoAllianceFlipUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; @@ -21,13 +15,20 @@ import frc.robot.Robot; import frc.robot.Robot.AlgaeIntakeTarget; import frc.robot.utils.autoaim.AutoAim; +import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; +import java.util.stream.Stream; /** Add your docs here. */ public class FieldUtils { public enum AlgaeIntakeTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls + // All coordinates are global coordinates from the lower, blue alliance side corner, if the + // walls // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise + // All angles from the center of the coral with 0° across the width of the field, + // counterclockwise BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), @@ -84,7 +85,8 @@ public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { pose.getTranslation().getDistance(other.location.getTranslation())) .thenComparing( (AlgaeIntakeTargets other) -> - Math.abs(pose.getRotation().minus(other.location.getRotation()).getRadians()))); + Math.abs( + pose.getRotation().minus(other.location.getRotation()).getRadians()))); } } @@ -92,22 +94,22 @@ public enum CageTargets { RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), - + BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); - + private static final List poses = Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); - + private final Pose2d location; private final Alliance alliance; - + private CageTargets(Pose2d location, Alliance alliance) { this.location = location; this.alliance = alliance; } - + public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { if (DriverStation.getAlliance().isPresent()) { // If it's across the field, x > 8.76 on blue and x < 8.76 on red @@ -118,7 +120,7 @@ public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { } return getOffsetClosestTarget(robotPose, false); } - + public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { Pose2d nearestPose; if (DriverStation.getAlliance().isPresent()) { @@ -137,7 +139,7 @@ public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { return getCloseRobotTargetLocation(nearestPose); } } - + public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { return pose.transformBy( new Transform2d( @@ -145,7 +147,7 @@ public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { 0, Rotation2d.kZero)); } - + public static Pose2d getFarRobotTargetLocation(Pose2d pose) { return pose.transformBy( new Transform2d( @@ -153,20 +155,22 @@ public static Pose2d getFarRobotTargetLocation(Pose2d pose) { 0, Rotation2d.k180deg)); } - + public Pose2d getLocation() { return this.location; } - + public Alliance getAlliance() { return this.alliance; } } - + public enum CoralTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls + // All coordinates are global coordinates from the lower, blue alliance side corner, if the + // walls // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise + // All angles from the center of the coral with 0° across the width of the field, + // counterclockwise BLUE_A(new Pose2d(3.95, 4.20, Rotation2d.fromDegrees(180)), true), BLUE_B(new Pose2d(3.95, 3.87, Rotation2d.fromDegrees(180)), false), BLUE_C(new Pose2d(4.07, 3.66, Rotation2d.fromDegrees(240)), true), @@ -179,7 +183,7 @@ public enum CoralTargets { BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), BLUE_L(new Pose2d(4.06, 4.41, Rotation2d.fromDegrees(120)), false), - + RED_A(ChoreoAllianceFlipUtil.flip(BLUE_A.location), true), RED_B(ChoreoAllianceFlipUtil.flip(BLUE_B.location), false), RED_C(ChoreoAllianceFlipUtil.flip(BLUE_C.location), true), @@ -192,15 +196,15 @@ public enum CoralTargets { RED_J(ChoreoAllianceFlipUtil.flip(BLUE_J.location), true), RED_K(ChoreoAllianceFlipUtil.flip(BLUE_K.location), true), RED_L(ChoreoAllianceFlipUtil.flip(BLUE_L.location), false); - + public final Pose2d location; public final boolean leftHanded; - + private CoralTargets(Pose2d location, boolean leftHanded) { this.location = location; this.leftHanded = leftHanded; } - + private static final List transformedPoses = Arrays.stream(values()) .map( @@ -208,7 +212,7 @@ private CoralTargets(Pose2d location, boolean leftHanded) { return CoralTargets.getRobotTargetLocation(targets.location); }) .toList(); - + public static Pose2d getRobotTargetLocation(Pose2d original) { // 0.248 for trough return original.transformBy( @@ -217,7 +221,7 @@ public static Pose2d getRobotTargetLocation(Pose2d original) { 0, Rotation2d.fromDegrees(180.0))); } - + public static Pose2d getBranchLocation(Pose2d transformed) { // 0.248 for trough return transformed.transformBy( @@ -227,12 +231,12 @@ public static Pose2d getBranchLocation(Pose2d transformed) { Rotation2d.fromDegrees(180.0)) .inverse()); } - + /** Gets the closest offset target to the given pose. */ public static Pose2d getClosestTarget(Pose2d pose) { return pose.nearest(transformedPoses); } - + /** Gets the closest offset target to the given pose. */ public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { return pose.nearest( @@ -245,7 +249,7 @@ public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { .toList()); } } - + public enum HumanPlayerTargets { BLUE_RIGHT_OUTSIDE( new Pose2d( @@ -256,7 +260,7 @@ public enum HumanPlayerTargets { BLUE_RIGHT_INSIDE( new Pose2d( 0.5838082432746887, 1.3407007455825806, Rotation2d.fromRadians(0.9420001549844138))), - + BLUE_LEFT_OUTSIDE( new Pose2d( 1.666144609451294, 7.431143760681152, Rotation2d.fromRadians(-0.9350057865774469))), @@ -266,21 +270,21 @@ public enum HumanPlayerTargets { BLUE_LEFT_INSIDE( new Pose2d( 0.6153400540351868, 6.673182487487793, Rotation2d.fromRadians(-0.9350057865774469))), - + RED_RIGHT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_OUTSIDE.location)), RED_RIGHT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_MIDDLE.location)), RED_RIGHT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_INSIDE.location)), RED_LEFT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_OUTSIDE.location)), RED_LEFT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_MIDDLE.location)), RED_LEFT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_INSIDE.location)); - + public final Pose2d location; - + private HumanPlayerTargets(Pose2d location) { this.location = location; } } - + public enum L1Targets { BLUE_AB( new Rectangle2d( @@ -299,16 +303,20 @@ public enum L1Targets { AutoAim.L1_TROUGH_WIDTH_METERS)), BLUE_GH( new Rectangle2d( - new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), BLUE_IJ( new Rectangle2d( - new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), BLUE_KL( new Rectangle2d( new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), - + RED_AB( new Rectangle2d( ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), @@ -339,13 +347,13 @@ public enum L1Targets { ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)); - + public Rectangle2d line; - + private L1Targets(Rectangle2d line) { this.line = line; } - + private static final List transformedLines = Arrays.stream(values()) .map( @@ -353,7 +361,7 @@ private L1Targets(Rectangle2d line) { return L1Targets.getRobotTargetLine(targets.line); }) .toList(); - + public static Rectangle2d getRobotTargetLine(Rectangle2d original) { return original.transformBy( new Transform2d( @@ -361,7 +369,7 @@ public static Rectangle2d getRobotTargetLine(Rectangle2d original) { 0, Rotation2d.fromDegrees(180.0))); } - + public static Rectangle2d getNearestLine(Pose2d pose) { // It feels like there should be a better way to do this return new Rectangle2d( @@ -370,5 +378,4 @@ public static Rectangle2d getNearestLine(Pose2d pose) { AutoAim.L1_TROUGH_WIDTH_METERS); } } - } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 35379596..0de94b96 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -20,7 +20,6 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; import frc.robot.utils.FieldUtils.CoralTargets; - import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; From 09487e4eea01c4fba0a04cecaaef72ed368c6393 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 29 Aug 2025 23:14:04 -0700 Subject: [PATCH 097/154] i think homing works now or something it's really sketchy --- .../frc/robot/subsystems/Superstructure.java | 54 +++------- .../elevator/ElevatorSubsystem.java | 48 ++++----- .../shoulder/ShoulderSubsystem.java | 8 +- .../subsystems/wrist/WristSubsystem.java | 102 +++++++++++------- 4 files changed, 106 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a6bb0c65..ba22162d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -130,9 +131,9 @@ public enum SuperState { WristState.INTAKE_ALGAE_GROUND, 0.0, 1.35, - 0.5) // lowkey why is this so slow - , - HOME(ElevatorState.HOME, ShoulderState.HOME, WristState.HOME, 0.0), + 0.5), // lowkey why is this so slow + HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HOME, 0.0), + HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), ANTIJAM_ALGAE( ElevatorState.ANTIJAM_ALGAE, ShoulderState.INTAKE_CORAL_GROUND, @@ -632,47 +633,26 @@ private void addTransitions() { : AutoAim.RED_PROCESSOR_POS.getY(), 0.5))); - // i might be insane for this + // HOME bindTransition( SuperState.IDLE, - SuperState.HOME, - Robot.homeReq, - Commands.runOnce( - () -> { - elevator.hasZeroed = false; - wrist.hasZeroed = false; - })); - - // im not sure why this needs to exist separately - bindTransition( - SuperState.IDLE, - SuperState.HOME, - new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed).and(DriverStation::isEnabled)); + SuperState.HOME_ELEVATOR, + new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed) + .and(() -> DriverStation.isEnabled())); bindTransition( - SuperState.READY_CORAL, - SuperState.HOME, - new Trigger(() -> !wrist.hasZeroed || !elevator.hasZeroed)); + SuperState.HOME_ELEVATOR, + SuperState.HOME_WRIST, + new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0), + Commands.print("Elevator Zeroing") + .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0)))); bindTransition( - SuperState.HOME, + SuperState.HOME_WRIST, SuperState.IDLE, - Robot.homeReq - .negate() - .and(() -> elevator.hasZeroed) - .and(() -> wrist.hasZeroed) - .and( - () -> - prevState != SuperState.READY_CORAL)); // TODO double check this prevstate thing - - bindTransition( - SuperState.HOME, - SuperState.READY_CORAL, - Robot.homeReq - .negate() - .and(() -> elevator.hasZeroed) - .and(() -> wrist.hasZeroed) - .and(() -> prevState == SuperState.READY_CORAL)); + new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0), + Commands.print("Wrist Zeroing") + .andThen(Commands.runOnce(() -> wrist.resetPosition(Rotation2d.fromRadians(-0.687))))); // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the // manipulator wheels diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 9c2a2834..9e8899a1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -73,7 +73,7 @@ public enum ElevatorState { READY_ALGAE(0.1), BARGE(Units.inchesToMeters(62.5)), PROCESSOR(Units.inchesToMeters(0.01)), // lmao - HOME(0.0), // NOT ACTUALLY 0!!! + HOME(-0.3), // i'm quite scared ANTIJAM_ALGAE(0.0) // NOT ACTUALLY 0!!! ; @@ -161,19 +161,7 @@ public Command setExtension(double meters) { } public Command setStateExtension() { - if (state == ElevatorState.HOME) { - return runCurrentZeroing(); - } else if (state == ElevatorState.ANTIJAM_ALGAE) { - return Commands.sequence( - setExtension(() -> inputs.positionMeters) - .until( - () -> - wristAtAngleSupplier.getAsBoolean() - && shoulderAngleSupplier.get().getDegrees() < 10.0), - setExtension(Units.inchesToMeters(40))); - } else { - return setExtension(() -> state.getExtensionMeters()); - } + return setExtension(() -> state.getExtensionMeters()); } public boolean atExtension(double expected) { @@ -185,21 +173,23 @@ public boolean atExtension() { } public Command runCurrentZeroing() { - return this.run( - () -> { - io.setVoltage(-2.0); - setpoint = 0.0; - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", Double.NaN); - }) - .until(() -> Math.abs(currentFilterValue) > 50.0) - .finallyDo( - (interrupted) -> { - if (!interrupted) { - io.resetEncoder(0.0); - hasZeroed = true; - } - }); + return Commands.print("Elevator Zeroing") + .andThen( + this.run( + () -> { + io.setVoltage(-2.0); + setpoint = 0.0; + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Elevator/Setpoint", Double.NaN); + }) + .until(() -> Math.abs(currentFilterValue) > 50.0) + .finallyDo( + (interrupted) -> { + if (!interrupted) { + io.resetEncoder(0.0); + hasZeroed = true; + } + })); } public Pose3d getCarriagePose() { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index e55b2e88..644c0cdc 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -98,10 +98,10 @@ public Rotation2d getAngle() { } } - @AutoLogOutput(key = "Shoulder/Setpoint") + @AutoLogOutput(key = "Carriage/Shoulder/Setpoint") private Rotation2d setpoint = Rotation2d.kZero; - @AutoLogOutput(key = "Shoulder/State") + @AutoLogOutput(key = "Carriage/Shoulder/State") private ShoulderState state = ShoulderState.HP; private final ShoulderIO io; @@ -123,7 +123,7 @@ public void periodic() { Logger.processInputs("Carriage/Shoulder", inputs); if (dashboardZero.get()) { Tracer.trace( - "Shoulder/Zero", + "Carriage/Shoulder/Zero", () -> { rezero(); dashboardZero.set(false); @@ -141,7 +141,7 @@ public Command setStateAngle() { return setAngle(() -> state.getAngle()); } - @AutoLogOutput(key = "Shoulder/Zeroing Angle") + @AutoLogOutput(key = "Carriage/Shoulder/Zeroing Angle") // what public Rotation2d getZeroingAngle() { return Rotation2d.fromRotations(inputs.cancoderPosition).div(SHOULDER_FINAL_STAGE_RATIO); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 9a73f3b4..b34dd718 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.Robot.RobotType; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -68,7 +70,8 @@ public enum WristState { PRE_BARGE(Rotation2d.fromDegrees(100)), SCORE_BARGE(Rotation2d.fromDegrees(110)), PROCESSOR(Rotation2d.fromDegrees(-30.0)), - HOME(Rotation2d.kZero); + HOME(Rotation2d.fromRadians(-0.687 - 1.0)) // i dunno + ; private final Rotation2d angle; @@ -81,18 +84,19 @@ public Rotation2d getAngle() { } } - @AutoLogOutput(key = "Wrist/Setpoint") + @AutoLogOutput(key = "Carriage/Wrist/Setpoint") private Rotation2d setpoint = Rotation2d.kZero; - @AutoLogOutput(key = "Wrist/State") + @AutoLogOutput(key = "Carriage/Wrist/State") private WristState state = WristState.HP; private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); private final LinearFilter currentFilter = LinearFilter.movingAverage(10); + public double currentFilterValue = 0.0; - @AutoLogOutput(key = "Wrist/Has Zeroed") + @AutoLogOutput(key = "Carriage/Wrist/Has Zeroed") public boolean hasZeroed = false; // i hate myself @@ -107,6 +111,10 @@ public WristSubsystem(WristIO io, Supplier shoulderAngleSupplier) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Carriage/Wrist", inputs); + currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); + + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Carriage/Wrist/Filtered Current", currentFilterValue); } public void setState(WristState state) { @@ -114,12 +122,7 @@ public void setState(WristState state) { } public Command setStateAngle() { - if (state == WristState.HOME) { - return Commands.waitUntil(() -> shoulderAngleSupplier.get().getDegrees() > 20.0) - .andThen(currentZero()); - } else { - return setAngle(() -> state.getAngle()); - } + return setAngle(() -> state.getAngle()); } public Command setAngle(final Supplier target) { @@ -131,10 +134,6 @@ public Command setAngle(final Supplier target) { }); } - public Command setAngle(final Rotation2d target) { - return setAngle(() -> target); - } - public Command setVoltage(final double volts) { return this.run(() -> io.setMotorVoltage(volts)); } @@ -156,28 +155,59 @@ public boolean atSetpoint() { } public Command currentZero() { - return Commands.sequence( - this.runOnce( - () -> { - currentFilter.reset(); - System.out.println("Wrist Zeroing"); - }), - this.run(() -> io.setMotorVoltage(-1.0)) - .raceWith( - Commands.waitSeconds(0.5) - .andThen( - Commands.waitUntil( - () -> - Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) - > 7.0))), - this.runOnce( - () -> { - // Logger.recordOutput( - // "shoulder zero pos", shoulderInputs.get().position.minus(ZEROING_OFFSET)); - hasZeroed = true; - // io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); - io.resetEncoder(Rotation2d.fromRadians(-0.687)); - })); + // return Commands.sequence( + // this.runOnce( + // () -> { + // currentFilter.reset(); + // System.out.println("Wrist Zeroing"); + // }), + // this.run(() -> io.setMotorVoltage(-1.0)) + // .raceWith( + // Commands.waitSeconds(0.5) + // .andThen( + // Commands.waitUntil( + // () -> + // Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) + // > 7.0))), + // this.runOnce( + // () -> { + // // Logger.recordOutput( + // // "shoulder zero pos", + // shoulderInputs.get().position.minus(ZEROING_OFFSET)); + // hasZeroed = true; + // // io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); + // io.resetEncoder(Rotation2d.fromRadians(-0.687)); + // })) + // .finallyDo(() -> Commands.print("DONE")); + return Commands.print("Wrist Zeroing") + .andThen( + this.run(() -> io.setMotorVoltage(-1.0)) + .until(() -> Math.abs(currentFilterValue) > 7.0) + .finallyDo( + (interrupted) -> { + if (!interrupted) { + io.resetEncoder(Rotation2d.fromRadians(-0.687)); + hasZeroed = true; + } + })); + + // return Commands.print("Elevator Zeroing") + // .andThen( + // this.run( + // () -> { + // io.setVoltage(-2.0); + // setpoint = 0.0; + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("Elevator/Setpoint", Double.NaN); + // }) + // .until(() -> Math.abs(currentFilterValue) > 50.0) + // .finallyDo( + // (interrupted) -> { + // if (!interrupted) { + // io.resetEncoder(0.0); + // hasZeroed = true; + // } + // })); } public void resetPosition(Rotation2d angle) { From 92050e05052190081cafa764234e7d7aa299bb24 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 00:00:46 -0700 Subject: [PATCH 098/154] auto l4 works in sim at least lol --- src/main/java/frc/robot/Robot.java | 67 +++---------------- .../subsystems/ManipulatorSubsystem.java | 8 +++ .../frc/robot/subsystems/Superstructure.java | 11 +-- .../elevator/ElevatorSubsystem.java | 32 +-------- .../shoulder/ShoulderSubsystem.java | 27 -------- .../subsystems/wrist/WristSubsystem.java | 32 +-------- 6 files changed, 28 insertions(+), 149 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 81377dbb..5ec43900 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -185,6 +185,7 @@ public static enum AlgaeScoreTarget { private static Supplier pose = () -> new Pose2d(); + @AutoLogOutput(key = "Superstructure/Pre Score Request") public static Trigger preScoreReq = driver .rightTrigger() @@ -201,65 +202,19 @@ public static enum AlgaeScoreTarget { < 3.25 && DriverStation.isAutonomous()); - @AutoLogOutput + @AutoLogOutput(key = "Superstructure/Score Request") public static Trigger scoreReq = driver .rightTrigger() .negate() .and(() -> DriverStation.isTeleop()) - // .or( - // new Trigger( - // () -> { - // final var state = - // new ExtensionState( - // elevator.getExtensionMeters(), - // shoulder.getAngle(), - // wrist.getAngle()); - // final var branch = - // ExtensionKinematics.getBranchPose( - // swerve.getPose(), state, currentTarget); - // final var manipulatorPose = - // ExtensionKinematics.getManipulatorPose(swerve.getPose(), - // state); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput("IK/Branch", branch); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - // Logger.recordOutput( - // "IK/Extension Check", - // manipulatorPose, - // manipulatorPose.transformBy( - // new Transform3d( - // Units.inchesToMeters(3.0), 0.0, 0.0, new - // Rotation3d()))); - // return false; - // // return branch - // // .getTranslation() - // // .getDistance(manipulatorPose.getTranslation()) - // // < Units.inchesToMeters(1.5) - // // || branch - // // .getTranslation() - // // .getDistance( - // // manipulatorPose - // // .transformBy( - // // new Transform3d( - // // Units.inchesToMeters(3.0), - // // 0.0, - // // 0.0, - // // new Rotation3d())) - // // .getTranslation()) - // // < Units.inchesToMeters(1.5); - // }) - // .debounce(0.15)) - // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .or(() -> Autos.autoScore && DriverStation.isAutonomous()); - @AutoLogOutput + @AutoLogOutput(key = "Superstructure/Algae Intake Request") public static Trigger intakeAlgaeReq = driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()); - @AutoLogOutput + @AutoLogOutput(key = "Superstructure/Coral Intake Request") public static Trigger intakeCoralReq = driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()); @@ -449,13 +404,10 @@ public static enum AlgaeScoreTarget { new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(0.5))) - : new WristIOSim(), - () -> shoulder.getAngle()); + : new WristIOSim()); private final ElevatorSubsystem elevator = new ElevatorSubsystem( - ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim(), - () -> shoulder.getAngle(), - () -> wrist.atSetpoint()); + ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim()); private final FunnelSubsystem funnel = new FunnelSubsystem( ROBOT_TYPE != RobotType.SIM @@ -505,10 +457,9 @@ public static enum AlgaeScoreTarget { new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); - public static Supplier state = - () -> SuperState.IDLE; // TODO i feel like im breaking a rule + public static Supplier state = () -> SuperState.IDLE; - @SuppressWarnings("resource") + @SuppressWarnings({"resource", "unlikely-arg-type"}) public Robot() { DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); @@ -685,7 +636,7 @@ public Robot() { t.location .minus(swerve.getPose()) .getTranslation() - .getNorm()) // TODO pose needs to be changed + .getNorm()) .min(Double::compare) .get() < 1.0) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index b4747fac..966482b3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -77,6 +77,14 @@ public void periodic() { currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); + + if (getFirstBeambreak() && !getSecondBeambreak()) { + zeroTimer.reset(); + } + + if (!getFirstBeambreak() && getSecondBeambreak()) { + zeroTimer.reset(); + } } public void resetPosition(final double rotations) { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ba22162d..d18e807e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -58,8 +58,7 @@ public enum SuperState { L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), POST_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), PRE_PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), - PRE_L4( - ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), // worried about shoulder here + PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 0.0), // worried about shoulder here L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), POST_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), POST_POST_L4( @@ -291,7 +290,7 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.READY_CORAL, - new Trigger(() -> manipulator.getFirstBeambreak() && manipulator.getTimeSinceZero() < 1.0)); + new Trigger(manipulator::eitherBeambreak).and(() -> manipulator.getTimeSinceZero() < 1.0)); // ---Intake coral ground--- bindTransition(SuperState.IDLE, SuperState.PRE_INTAKE_CORAL_GROUND, Robot.intakeCoralReq); @@ -441,7 +440,7 @@ private void addTransitions() { // ---L4--- bindTransition( SuperState.READY_CORAL, - SuperState.PRE_PRE_L4, + SuperState.PRE_L4, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); @@ -640,6 +639,10 @@ private void addTransitions() { new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed) .and(() -> DriverStation.isEnabled())); + bindTransition(SuperState.IDLE, SuperState.HOME_ELEVATOR, Robot.homeReq); + + bindTransition(SuperState.READY_CORAL, SuperState.HOME_ELEVATOR, Robot.homeReq); + bindTransition( SuperState.HOME_ELEVATOR, SuperState.HOME_WRIST, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 9e8899a1..d352cc3d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -15,9 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; @@ -40,25 +38,6 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double SLOW_ACCELERATION = 5.0; public static final double MEDIUM_ACCELERATION = 8.5; - // public static final double L1_EXTENSION_METERS = - // ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); - // public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); - // public static final double L2_EXTENSION_METERS = - // ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); - // public static final double L3_EXTENSION_METERS = - // ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); - // public static final double L4_EXTENSION_METERS = - // ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - // public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); - // public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); - // public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); - // public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); - // // 1 inch high (maybe not needed?) - // public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); - // public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; - // public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); - // public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); - public enum ElevatorState { HP(Units.inchesToMeters(0.0)), INTAKE_CORAL_GROUND(Units.inchesToMeters(0.0)), @@ -114,18 +93,9 @@ public double getExtensionMeters() { private final LoggedMechanismLigament2d carriage = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); - // i hate myself - private Supplier shoulderAngleSupplier; - private BooleanSupplier wristAtAngleSupplier; - /** Creates a new ElevatorSubsystem. */ - public ElevatorSubsystem( - ElevatorIO io, - Supplier shoulderAngleSupplier, - BooleanSupplier wristAtAngleSupplier) { + public ElevatorSubsystem(ElevatorIO io) { this.io = io; - this.shoulderAngleSupplier = shoulderAngleSupplier; - this.wristAtAngleSupplier = wristAtAngleSupplier; } @Override diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 644c0cdc..03159229 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -28,30 +28,6 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - // public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0 - 7); - // public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - // public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - // Rotation2d.fromRadians(0.505) - // .plus(Rotation2d.fromDegrees(-5.0)) - // .minus(Rotation2d.fromDegrees(2)); - // public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); - // public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); - // public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = - // Rotation2d.fromDegrees(60.0); - // // may be incorrect as l2-3 poses are derived from ExtensionKinematics now - // public static final Rotation2d SHOULDER_SCORE_POS = - // ExtensionKinematics.L2_EXTENSION.shoulderAngle(); - // public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); - // public static final Rotation2d SHOULDER_SCORE_L1_POS = - // ExtensionKinematics.L1_EXTENSION.shoulderAngle(); - // public static final Rotation2d SHOULDER_SCORE_L4_POS = - // ExtensionKinematics.L4_EXTENSION.shoulderAngle(); - // public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); - // public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - // public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); - // public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - // public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); - public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); public static final MotionMagicConfigs TOSS_CONFIGS = @@ -84,9 +60,6 @@ public enum ShoulderState { PROCESSOR(Rotation2d.fromDegrees(60.0)), HOME(Rotation2d.fromDegrees(50.0)); - // L4_TUCKED(Rotation2d.fromDegrees(35.0)), // SHOULDER_TUCKED_CLEARANCE_POS - // L4_TUCKED_OUT(Rotation2d.fromDegrees(25.0)), - private final Rotation2d angle; private ShoulderState(Rotation2d angle) { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index b34dd718..4c7f5a50 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -16,33 +16,11 @@ public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); - public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); + public static final Rotation2d MIN_ARM_ROTATION = + Rotation2d.fromRadians(-0.687); // Rotation2d.fromDegrees(-90.0); //TODO find?? public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); - // public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - // public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); - // public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); - // public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); - // public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); - // public static final Rotation2d WRIST_SCORE_L1_POS = - // ExtensionKinematics.L1_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - // public static final Rotation2d WRIST_SCORE_L2_POS = - // ExtensionKinematics.L2_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_SCORE_L3_POS = - // ExtensionKinematics.L3_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_SCORE_L4_POS = - // ExtensionKinematics.L4_EXTENSION.wristAngle(); - // public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); - // public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); - // public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); - // public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = - // Rotation2d.fromDegrees(-20.0); - // public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(110); - // public static final Rotation2d WRIST_PRE_NET_POS = Rotation2d.fromDegrees(100); - // public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); - public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); @@ -99,12 +77,8 @@ public Rotation2d getAngle() { @AutoLogOutput(key = "Carriage/Wrist/Has Zeroed") public boolean hasZeroed = false; - // i hate myself - private Supplier shoulderAngleSupplier; - - public WristSubsystem(WristIO io, Supplier shoulderAngleSupplier) { + public WristSubsystem(WristIO io) { this.io = io; - this.shoulderAngleSupplier = shoulderAngleSupplier; } @Override From 7199f84ed8de0661c67e61d3df4893bb756a461c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 00:31:16 -0700 Subject: [PATCH 099/154] some more coral scoring tuning --- .../frc/robot/subsystems/Superstructure.java | 34 +++++++++++-------- .../subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d18e807e..475b7eff 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -48,21 +48,25 @@ public enum SuperState { WristState.PRE_INTAKE_CORAL_GROUND, 0.0), READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), - PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.PRE_L1, 0.0), - L1(ElevatorState.L1, ShoulderState.L1, WristState.PRE_L1, 3.0), - POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.PRE_L1, 0.0), - PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + // TODO make manipulator stuff less ugly + PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), + L1(ElevatorState.L1, ShoulderState.L1, WristState.L1, 3.0), + POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), + + PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.L2, 0.0), L2(ElevatorState.L2, ShoulderState.L2, WristState.L2, -15.0), POST_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), - PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + + PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.L3, 0.0), L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), POST_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), - PRE_PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), - PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 0.0), // worried about shoulder here + + PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), POST_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), POST_POST_L4( ElevatorState.HP, ShoulderState.L4, WristState.HP, 0.0), // like do we see the vision + PRE_PRE_INTAKE_ALGAE( ElevatorState.HP, ShoulderState.PRE_INTAKE_ALGAE_REEF, @@ -276,7 +280,7 @@ private void setSubstates() { elevator.setState(state.elevatorState); shoulder.setState(state.shoulderState); wrist.setState(state.wristState); - manipulator.setState(state.manipulatorVelocity); + // funnel and climber don't have states per se } @@ -320,7 +324,11 @@ private void addTransitions() { SuperState.PRE_L1, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L1).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_L1, SuperState.L1, Robot.scoreReq.and(this::atExtension)); + bindTransition(SuperState.PRE_L1, SuperState.L1, new Trigger(this::atExtension)); + + Robot.scoreReq + .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) + .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); // cancel bindTransition( @@ -361,7 +369,7 @@ private void addTransitions() { SuperState.PRE_L2, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L2).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_L2, SuperState.L2, Robot.scoreReq.and(this::atExtension)); + bindTransition(SuperState.PRE_L2, SuperState.L2, new Trigger(this::atExtension)); // cancel bindTransition( @@ -402,7 +410,7 @@ private void addTransitions() { SuperState.PRE_L3, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L3).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_L3, SuperState.L3, Robot.scoreReq.and(this::atExtension)); + bindTransition(SuperState.PRE_L3, SuperState.L3, new Trigger(this::atExtension)); // cancel bindTransition( @@ -443,9 +451,7 @@ private void addTransitions() { SuperState.PRE_L4, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); - - bindTransition(SuperState.PRE_L4, SuperState.L4, Robot.scoreReq.and(this::atExtension)); + bindTransition(SuperState.PRE_L4, SuperState.L4, new Trigger(this::atExtension)); // cancel bindTransition( diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 4c7f5a50..afec61d2 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -34,7 +34,7 @@ public enum WristState { PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(30.0)), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(0.0)), HP(Rotation2d.fromDegrees(178.0)), - PRE_L1(Rotation2d.fromRadians(0.349)), + L1(Rotation2d.fromRadians(0.349)), PRE_L2(Rotation2d.fromDegrees(170.0)), L2(Rotation2d.fromRadians(2.447)), PRE_L3(Rotation2d.fromDegrees(170.0)), From ccaa838e168af17c3191d8d7c7ccf9c38d8fc898 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 10:22:01 -0700 Subject: [PATCH 100/154] YAYYYY tunable number works --- src/main/java/frc/robot/Robot.java | 1 + .../subsystems/wrist/WristSubsystem.java | 17 ++- .../frc/robot/utils/LoggedTunableNumber.java | 123 ++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/utils/LoggedTunableNumber.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ec43900..3780b320 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -137,6 +137,7 @@ private RobotHardware(SwerveConstants swerveConstants) { public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; + public static final boolean TUNING_MODE = true; public static enum ReefTarget { L1(3.0, SuperState.PRE_L1), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index afec61d2..ff7ef933 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -30,10 +31,14 @@ public class WristSubsystem extends SubsystemBase { public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); + public static LoggedTunableNumber a = new LoggedTunableNumber("dfsdfjl", 5); + public enum WristState { PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(30.0)), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(0.0)), - HP(Rotation2d.fromDegrees(178.0)), + // HP(() -> Rotation2d.fromDegrees(new LoggedTunableNumber("Wrist/HP", 178).get())), + // HP(Rotation2d.fromDegrees(178)), + HP(new LoggedTunableNumber("Wrist/HP", 178)), L1(Rotation2d.fromRadians(0.349)), PRE_L2(Rotation2d.fromDegrees(170.0)), L2(Rotation2d.fromRadians(2.447)), @@ -51,14 +56,18 @@ public enum WristState { HOME(Rotation2d.fromRadians(-0.687 - 1.0)) // i dunno ; - private final Rotation2d angle; + private final Supplier angle; private WristState(Rotation2d angle) { - this.angle = angle; + this.angle = () -> angle; + } + + private WristState(LoggedTunableNumber degrees) { + this.angle = () -> Rotation2d.fromDegrees(degrees.get()); } public Rotation2d getAngle() { - return angle; + return angle.get(); } } diff --git a/src/main/java/frc/robot/utils/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/LoggedTunableNumber.java new file mode 100644 index 00000000..3409454d --- /dev/null +++ b/src/main/java/frc/robot/utils/LoggedTunableNumber.java @@ -0,0 +1,123 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.utils; + +import frc.robot.Robot; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Robot.TUNING_MODE) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Robot.TUNING_MODE ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} From 5c8ea30d1123898467bb05a0b06a3a2b0a492458 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 10:42:50 -0700 Subject: [PATCH 101/154] move wrist to ltn --- .../subsystems/wrist/WristSubsystem.java | 53 +++++++++---------- 1 file changed, 26 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index ff7ef933..50cea89c 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -31,39 +31,38 @@ public class WristSubsystem extends SubsystemBase { public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); - public static LoggedTunableNumber a = new LoggedTunableNumber("dfsdfjl", 5); - public enum WristState { - PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(30.0)), // formerly WRIST_CLEARANCE_POS - INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(0.0)), - // HP(() -> Rotation2d.fromDegrees(new LoggedTunableNumber("Wrist/HP", 178).get())), - // HP(Rotation2d.fromDegrees(178)), - HP(new LoggedTunableNumber("Wrist/HP", 178)), - L1(Rotation2d.fromRadians(0.349)), - PRE_L2(Rotation2d.fromDegrees(170.0)), - L2(Rotation2d.fromRadians(2.447)), - PRE_L3(Rotation2d.fromDegrees(170.0)), - L3(Rotation2d.fromRadians(2.427)), - L4(Rotation2d.fromDegrees(120.0)), // ?? - PRE_INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(30.0)), - INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(-20.0)), - INTAKE_ALGAE_STACK(Rotation2d.fromDegrees(-10)), - INTAKE_ALGAE_GROUND(Rotation2d.fromDegrees(-65)), - READY_ALGAE(Rotation2d.fromDegrees(20)), - PRE_BARGE(Rotation2d.fromDegrees(100)), - SCORE_BARGE(Rotation2d.fromDegrees(110)), - PROCESSOR(Rotation2d.fromDegrees(-30.0)), - HOME(Rotation2d.fromRadians(-0.687 - 1.0)) // i dunno + PRE_INTAKE_CORAL_GROUND(30.0, true), // formerly WRIST_CLEARANCE_POS + INTAKE_CORAL_GROUND(0.0, true), + HP(178.0, true), + L1(0.349, false), + PRE_L2(170.0, true), + L2(2.447, false), + PRE_L3(170.0, true), + L3(2.427, false), + L4(120.0, true), // ?? + PRE_INTAKE_ALGAE_REEF(30.0, true), + INTAKE_ALGAE_REEF(-20.0, true), + INTAKE_ALGAE_STACK(-10, true), + INTAKE_ALGAE_GROUND(-65, true), + READY_ALGAE(20, true), + PRE_BARGE(100, true), + SCORE_BARGE(110, true), + PROCESSOR(-30.0, true), + HOME(-0.687 - 1.0, false) // i dunno ; private final Supplier angle; - private WristState(Rotation2d angle) { - this.angle = () -> angle; - } - private WristState(LoggedTunableNumber degrees) { - this.angle = () -> Rotation2d.fromDegrees(degrees.get()); + private WristState(double defaultValue, boolean fromDegrees) { + LoggedTunableNumber ltn = new LoggedTunableNumber("Wrist/" + this.name(), defaultValue); + // just don't use rotations i guess + if (fromDegrees) { + this.angle = () -> Rotation2d.fromDegrees(ltn.get()); + } else { + this.angle = () -> Rotation2d.fromRadians(ltn.get()); + } } public Rotation2d getAngle() { From 2d08b0df7ecf4b51dc9ff37ff3ec7f134f1b0923 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 10:47:01 -0700 Subject: [PATCH 102/154] make elevator use ltn --- .../subsystems/elevator/ElevatorSubsystem.java | 13 ++++++++----- .../frc/robot/subsystems/wrist/WristSubsystem.java | 5 ++--- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d352cc3d..415f0418 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -45,7 +46,7 @@ public enum ElevatorState { L2(0.23 + Units.inchesToMeters(1.5)), L3(0.60 + Units.inchesToMeters(2.0)), L4(1.383), - INTAKE_ALGAE_LOW(Units.inchesToMeters(Units.inchesToMeters(20.0))), + INTAKE_ALGAE_LOW(Units.inchesToMeters(20.0)), INTAKE_ALGAE_HIGH(Units.inchesToMeters(35.0)), INTAKE_ALGAE_STACK(Units.inchesToMeters(9.0)), INTAKE_ALGAE_GROUND(0.14 - Units.inchesToMeters(0.75)), @@ -56,14 +57,16 @@ public enum ElevatorState { ANTIJAM_ALGAE(0.0) // NOT ACTUALLY 0!!! ; - private final double extensionMeters; + private final DoubleSupplier extensionMeters; - private ElevatorState(double extensionMeters) { - this.extensionMeters = extensionMeters; + private ElevatorState(double defaultExtensionMeters) { + // this.extensionMeters = extensionMeters; + this.extensionMeters = + new LoggedTunableNumber("Elevator/" + this.name(), defaultExtensionMeters); } public double getExtensionMeters() { - return extensionMeters; + return extensionMeters.getAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 50cea89c..7ffe60f5 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -54,9 +54,8 @@ public enum WristState { private final Supplier angle; - - private WristState(double defaultValue, boolean fromDegrees) { - LoggedTunableNumber ltn = new LoggedTunableNumber("Wrist/" + this.name(), defaultValue); + private WristState(double defaultAngle, boolean fromDegrees) { + LoggedTunableNumber ltn = new LoggedTunableNumber("Wrist/" + this.name(), defaultAngle); // just don't use rotations i guess if (fromDegrees) { this.angle = () -> Rotation2d.fromDegrees(ltn.get()); From 53a1651cb9d2b20a406173e718caa195501c3495 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 10:54:57 -0700 Subject: [PATCH 103/154] make shoulder use ltn --- .../shoulder/ShoulderSubsystem.java | 56 ++++++++++--------- .../subsystems/wrist/WristSubsystem.java | 47 ++++++++-------- 2 files changed, 52 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 03159229..5f0194f7 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.Tracer; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -36,38 +37,41 @@ public class ShoulderSubsystem extends SubsystemBase { .withMotionMagicAcceleration(4.0); public enum ShoulderState { - HP(Rotation2d.fromDegrees(50.0)), - PRE_INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(35.0)), - INTAKE_CORAL_GROUND(Rotation2d.fromDegrees(8.0)), - PRE_L1(Rotation2d.fromDegrees(35.0)), - L1(Rotation2d.fromRadians(1.617)), // not sure about units tbh - PRE_L2(Rotation2d.fromDegrees(35.0)), - L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20))), - PRE_L3(Rotation2d.fromDegrees(35.0)), - L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3))), - PRE_L4(Rotation2d.fromDegrees(8.0)), - L4(Rotation2d.fromDegrees(25.0)), - PRE_INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(35.0)), - INTAKE_ALGAE_REEF(Rotation2d.fromDegrees(45.0)), - INTAKE_ALGAE_STACK(Rotation2d.fromDegrees(30.0)), + HP(50.0), + PRE_INTAKE_CORAL_GROUND(35.0), + INTAKE_CORAL_GROUND(8.0), + PRE_L1(35.0), + L1(Units.radiansToDegrees(1.617)), // not sure about units tbh + PRE_L2(35.0), + L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)).getDegrees()), + PRE_L3(35.0), + L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), + PRE_L4(8.0), + L4(25.0), + PRE_INTAKE_ALGAE_REEF(35.0), + INTAKE_ALGAE_REEF(45.0), + INTAKE_ALGAE_STACK(30.0), INTAKE_ALGAE_GROUND( Rotation2d.fromRadians(0.505) .plus(Rotation2d.fromDegrees(-5.0)) - .minus(Rotation2d.fromDegrees(2))), // hello?? - READY_ALGAE(Rotation2d.fromDegrees(60.0)), - PRE_BARGE(Rotation2d.fromDegrees(30)), - SCORE_BARGE(Rotation2d.fromDegrees(90)), - PROCESSOR(Rotation2d.fromDegrees(60.0)), - HOME(Rotation2d.fromDegrees(50.0)); - - private final Rotation2d angle; - - private ShoulderState(Rotation2d angle) { - this.angle = angle; + .minus(Rotation2d.fromDegrees(2)) + .getDegrees()), // hello?? + READY_ALGAE(60.0), + PRE_BARGE(30), + SCORE_BARGE(90), + PROCESSOR(60.0), + HOME(50.0); + + private final Supplier angle; + + private ShoulderState(double defaultAngle) { + LoggedTunableNumber ltn = new LoggedTunableNumber("Shoulder/" + this.name(), defaultAngle); + // we're in real life!! use degrees + this.angle = () -> Rotation2d.fromDegrees(ltn.get()); } public Rotation2d getAngle() { - return angle; + return angle.get(); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 7ffe60f5..b5eaa8c2 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -32,36 +33,32 @@ public class WristSubsystem extends SubsystemBase { new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); public enum WristState { - PRE_INTAKE_CORAL_GROUND(30.0, true), // formerly WRIST_CLEARANCE_POS - INTAKE_CORAL_GROUND(0.0, true), - HP(178.0, true), - L1(0.349, false), - PRE_L2(170.0, true), - L2(2.447, false), - PRE_L3(170.0, true), - L3(2.427, false), - L4(120.0, true), // ?? - PRE_INTAKE_ALGAE_REEF(30.0, true), - INTAKE_ALGAE_REEF(-20.0, true), - INTAKE_ALGAE_STACK(-10, true), - INTAKE_ALGAE_GROUND(-65, true), - READY_ALGAE(20, true), - PRE_BARGE(100, true), - SCORE_BARGE(110, true), - PROCESSOR(-30.0, true), - HOME(-0.687 - 1.0, false) // i dunno + PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS + INTAKE_CORAL_GROUND(0.0), + HP(178.0), + L1(Units.radiansToDegrees(0.349)), + PRE_L2(170.0), + L2(Units.radiansToDegrees(2.447)), + PRE_L3(170.0), + L3(Units.radiansToDegrees(2.427)), + L4(120.0), // ?? + PRE_INTAKE_ALGAE_REEF(30.0), + INTAKE_ALGAE_REEF(-20.0), + INTAKE_ALGAE_STACK(-10), + INTAKE_ALGAE_GROUND(-65), + READY_ALGAE(20), + PRE_BARGE(100), + SCORE_BARGE(110), + PROCESSOR(-30.0), + HOME(Units.radiansToDegrees(-0.687 - 1.0)) // i dunno ; private final Supplier angle; - private WristState(double defaultAngle, boolean fromDegrees) { + private WristState(double defaultAngle) { LoggedTunableNumber ltn = new LoggedTunableNumber("Wrist/" + this.name(), defaultAngle); - // just don't use rotations i guess - if (fromDegrees) { - this.angle = () -> Rotation2d.fromDegrees(ltn.get()); - } else { - this.angle = () -> Rotation2d.fromRadians(ltn.get()); - } + // we're in real life!! use degrees + this.angle = () -> Rotation2d.fromDegrees(ltn.get()); } public Rotation2d getAngle() { From 68a73d271162bb657e19ec24c0f12104b73372ee Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 11:01:51 -0700 Subject: [PATCH 104/154] comment --- src/main/java/frc/robot/utils/LoggedTunableNumber.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/utils/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/LoggedTunableNumber.java index 3409454d..08c79464 100644 --- a/src/main/java/frc/robot/utils/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/utils/LoggedTunableNumber.java @@ -18,6 +18,7 @@ /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. + * Taken from 6328's 2025 code - with the modification that we don't disable the HAL and don't put TUNING_MODE in a Constants file */ public class LoggedTunableNumber implements DoubleSupplier { private static final String tableKey = "/Tuning"; From eefdcdb15b6515b65850b75b6b7614dbe096c9df Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 14:59:52 -0700 Subject: [PATCH 105/154] slow down stuff for testing --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- .../frc/robot/subsystems/elevator/ElevatorIOReal.java | 4 ++-- .../frc/robot/subsystems/elevator/ElevatorSubsystem.java | 4 ++-- .../frc/robot/subsystems/shoulder/ShoulderIOReal.java | 2 +- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 8 +++----- 5 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 475b7eff..ff09fa97 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -135,7 +135,7 @@ public enum SuperState { 0.0, 1.35, 0.5), // lowkey why is this so slow - HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HOME, 0.0), + HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), ANTIJAM_ALGAE( ElevatorState.ANTIJAM_ALGAE, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 523e1198..a46b28e2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -80,9 +80,9 @@ public ElevatorIOReal() { config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerTime = 0.25; - config.MotionMagic.MotionMagicAcceleration = ElevatorSubsystem.MAX_ACCELERATION; + config.MotionMagic.MotionMagicAcceleration = ElevatorSubsystem.SLOW_ACCELERATION; // Estimated from slightly less than motor free speed - config.MotionMagic.MotionMagicCruiseVelocity = 4.5; + config.MotionMagic.MotionMagicCruiseVelocity = 2; // (5500.0 / 60.0) / config.Feedback.SensorToMechanismRatio; config.MotionMagic.MotionMagicExpo_kV = 1.9; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d352cc3d..49de3798 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -109,8 +109,8 @@ public void periodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Mechanism2d", mech2d); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose()); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Filtered Current", currentFilterValue); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Elevator/Filtered Current", currentFilterValue); } public void setState(ElevatorState state) { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index ab724233..47ae4373 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -66,7 +66,7 @@ public ShoulderIOReal() { // guesses config.MotionMagic.MotionMagicCruiseVelocity = 1.0; - config.MotionMagic.MotionMagicAcceleration = 4.0; + config.MotionMagic.MotionMagicAcceleration = 1.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index afec61d2..2954fc73 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -7,8 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -22,7 +20,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(1).withMotionMagicAcceleration(1); public static MotionMagicConfigs SLOW_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(3); @@ -87,8 +85,8 @@ public void periodic() { Logger.processInputs("Carriage/Wrist", inputs); currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Wrist/Filtered Current", currentFilterValue); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Carriage/Wrist/Filtered Current", currentFilterValue); } public void setState(WristState state) { From 4b32bef25a1e5f2d13e2352c8f3e3febb61a1b67 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 15:47:14 -0700 Subject: [PATCH 106/154] some wrist tuning - i guess the range of motion is less than i thought it was?? --- src/main/java/frc/robot/Robot.java | 7 +++- .../frc/robot/subsystems/Superstructure.java | 32 +++++++++++++------ .../elevator/ElevatorSubsystem.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 13 ++++---- .../frc/robot/utils/LoggedTunableNumber.java | 4 +-- 5 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3780b320..66f69098 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -241,7 +241,12 @@ public static enum AlgaeScoreTarget { public static final Trigger antiAlgaeJamReq = driver.b(); @AutoLogOutput(key = "Superstructure/Home Request") - public static Trigger homeReq = driver.start(); + public static Trigger homeReq = + driver + .start() + .or( + new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) + .and(() -> DriverStation.isEnabled())); @AutoLogOutput(key = "Superstructure/Rev Funnel Req") public static Trigger revFunnelReq = operator.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ff09fa97..aa66ec75 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -638,30 +638,42 @@ private void addTransitions() { : AutoAim.RED_PROCESSOR_POS.getY(), 0.5))); - // HOME bindTransition( SuperState.IDLE, SuperState.HOME_ELEVATOR, - new Trigger(() -> !elevator.hasZeroed || !wrist.hasZeroed) - .and(() -> DriverStation.isEnabled())); - - bindTransition(SuperState.IDLE, SuperState.HOME_ELEVATOR, Robot.homeReq); + Robot.homeReq, + Commands.runOnce( + () -> { + ElevatorSubsystem.hasZeroed = false; + WristSubsystem.hasZeroed = false; + })); - bindTransition(SuperState.READY_CORAL, SuperState.HOME_ELEVATOR, Robot.homeReq); + // bindTransition( + // SuperState.READY_CORAL, + // SuperState.HOME_ELEVATOR, + // Robot.homeReq, + // Commands.runOnce(() -> elevator.hasZeroed = false)); bindTransition( SuperState.HOME_ELEVATOR, SuperState.HOME_WRIST, - new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0), + // SuperState.IDLE, + new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0).debounce(0.1), Commands.print("Elevator Zeroing") - .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0)))); + .alongWith(Commands.runOnce(() -> elevator.resetExtension(0.0))) + // .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0))) + ); bindTransition( SuperState.HOME_WRIST, SuperState.IDLE, - new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0), + new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.1), Commands.print("Wrist Zeroing") - .andThen(Commands.runOnce(() -> wrist.resetPosition(Rotation2d.fromRadians(-0.687))))); + .andThen( + Commands.runOnce( + () -> + wrist.resetPosition( + Rotation2d.fromDegrees(178).minus(Rotation2d.fromRadians(3.357)))))); // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the // manipulator wheels diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 568b0ea4..baea9931 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -85,7 +85,7 @@ public double getExtensionMeters() { public double currentFilterValue = 0.0; @AutoLogOutput(key = "Elevator/Has Zeroed") - public boolean hasZeroed = false; + public static boolean hasZeroed = false; // For dashboard private final LoggedMechanism2d mech2d = new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 11aebb2c..753c44d8 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -17,9 +15,10 @@ public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); - public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); + public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(160.0); public static final Rotation2d MIN_ARM_ROTATION = - Rotation2d.fromRadians(-0.687); // Rotation2d.fromDegrees(-90.0); //TODO find?? + // Rotation2d.fromRadians(-0.687); // Rotation2d.fromDegrees(-90.0); //TODO find?? + Rotation2d.fromDegrees(178).minus(Rotation2d.fromRadians(3.357)); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); @@ -35,7 +34,7 @@ public class WristSubsystem extends SubsystemBase { public enum WristState { PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(0.0), - HP(178.0), + HP(160.0), L1(Units.radiansToDegrees(0.349)), PRE_L2(170.0), L2(Units.radiansToDegrees(2.447)), @@ -50,7 +49,7 @@ public enum WristState { PRE_BARGE(100), SCORE_BARGE(110), PROCESSOR(-30.0), - HOME(Units.radiansToDegrees(-0.687 - 1.0)) // i dunno + HOME(Units.radiansToDegrees(-0.687 - 2.0)) // i dunno ; private final Supplier angle; @@ -79,7 +78,7 @@ public Rotation2d getAngle() { public double currentFilterValue = 0.0; @AutoLogOutput(key = "Carriage/Wrist/Has Zeroed") - public boolean hasZeroed = false; + public static boolean hasZeroed = false; public WristSubsystem(WristIO io) { this.io = io; diff --git a/src/main/java/frc/robot/utils/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/LoggedTunableNumber.java index 08c79464..229fc5ea 100644 --- a/src/main/java/frc/robot/utils/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/utils/LoggedTunableNumber.java @@ -17,8 +17,8 @@ /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or - * value not in dashboard. - * Taken from 6328's 2025 code - with the modification that we don't disable the HAL and don't put TUNING_MODE in a Constants file + * value not in dashboard. Taken from 6328's 2025 code - with the modification that we don't disable + * the HAL and don't put TUNING_MODE in a Constants file */ public class LoggedTunableNumber implements DoubleSupplier { private static final String tableKey = "/Tuning"; From fb2a67e98ff7f4398c5dd0ece2bc73141bea6976 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 16:36:11 -0700 Subject: [PATCH 107/154] homing is less cooked now --- src/main/java/frc/robot/Robot.java | 6 ++++-- .../robot/subsystems/ManipulatorSubsystem.java | 1 + .../frc/robot/subsystems/Superstructure.java | 16 ++++++---------- .../subsystems/elevator/ElevatorSubsystem.java | 2 ++ .../robot/subsystems/wrist/WristSubsystem.java | 8 +++++++- 5 files changed, 20 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66f69098..a33d7917 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -244,6 +244,8 @@ public static enum AlgaeScoreTarget { public static Trigger homeReq = driver .start() + .and(() -> ElevatorSubsystem.hasZeroed || WristSubsystem.hasZeroed) + // zeroing on startup .or( new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) .and(() -> DriverStation.isEnabled())); @@ -540,7 +542,7 @@ public Robot() { Commands.runOnce( () -> { elevator.resetExtension(0.0); - wrist.resetPosition(Rotation2d.k180deg); + wrist.rezero(Rotation2d.k180deg); }) .ignoringDisable(true)); @@ -572,7 +574,7 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - wrist.resetPosition(Rotation2d.fromRadians(3.094)); + wrist.rezero(Rotation2d.fromRadians(3.094)); elevator.resetExtension(0.0); })); diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 966482b3..4b65dfaf 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -42,6 +42,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { private boolean bb2Sim = false; @AutoLogOutput private boolean hasAlgaeSim = false; + @AutoLogOutput(key = "Manipulator State Velocity") private double stateVelocity = 0.0; private LinearFilter currentFilter = LinearFilter.movingAverage(20); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index aa66ec75..cebffbd1 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -641,11 +641,12 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.HOME_ELEVATOR, - Robot.homeReq, + Robot.homeReq.and(() -> prevState != SuperState.HOME_ELEVATOR), Commands.runOnce( () -> { ElevatorSubsystem.hasZeroed = false; WristSubsystem.hasZeroed = false; + wrist.resetEncoder(SuperState.IDLE.wristState.getAngle()); })); // bindTransition( @@ -659,21 +660,16 @@ private void addTransitions() { SuperState.HOME_WRIST, // SuperState.IDLE, new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0).debounce(0.1), - Commands.print("Elevator Zeroing") - .alongWith(Commands.runOnce(() -> elevator.resetExtension(0.0))) + Commands.runOnce(() -> elevator.resetExtension(0.0)) // .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0))) ); bindTransition( SuperState.HOME_WRIST, SuperState.IDLE, - new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.1), - Commands.print("Wrist Zeroing") - .andThen( - Commands.runOnce( - () -> - wrist.resetPosition( - Rotation2d.fromDegrees(178).minus(Rotation2d.fromRadians(3.357)))))); + new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), + Commands.runOnce( + () -> wrist.rezero(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))))); // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the // manipulator wheels diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index baea9931..86dc8d1b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -182,7 +182,9 @@ public double getSetpoint() { } public void resetExtension(double extension) { + System.out.println("Elevator zeroing"); io.resetEncoder(extension); hasZeroed = true; + System.out.println("Elevator zeroed!"); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 753c44d8..6741d39d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -187,8 +187,14 @@ public Command currentZero() { // })); } - public void resetPosition(Rotation2d angle) { + public void rezero(Rotation2d angle) { + System.out.println("Wrist rezeroing"); io.resetEncoder(angle); hasZeroed = true; + System.out.println("Wrist rezeroed!"); + } + + public void resetEncoder(Rotation2d angle) { + io.resetEncoder(angle); } } From 3143ca3e70a17d3e4b0af5237a5a32f53e161b13 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 18:13:53 -0700 Subject: [PATCH 108/154] manipulator runs now --- .../robot/subsystems/ManipulatorSubsystem.java | 4 ++-- .../java/frc/robot/subsystems/Superstructure.java | 15 +++++++++++++-- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 4b65dfaf..86052105 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; @@ -101,7 +100,8 @@ public Command intakeAlgae() { } public Command setStateVelocity(BooleanSupplier checkExtension) { - return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); + // return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); + return setRollerVelocity(stateVelocity); } public void setState(double vel) { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index cebffbd1..1bad095b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -318,6 +318,8 @@ private void addTransitions() { SuperState.READY_CORAL, new Trigger(this::atExtension)); + bindTransition( + SuperState.READY_CORAL, SuperState.IDLE, new Trigger(manipulator::neitherBeambreak)); // ---L1--- bindTransition( SuperState.READY_CORAL, @@ -327,8 +329,17 @@ private void addTransitions() { bindTransition(SuperState.PRE_L1, SuperState.L1, new Trigger(this::atExtension)); Robot.scoreReq - .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) - .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); + .and(() -> stateIsScoreCoral(state)) + // .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) + // .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); + .whileTrue(manipulator.setRollerVelocity(state.manipulatorVelocity)); + + Robot.intakeCoralReq + .onTrue(manipulator.setRollerVelocity(-18)) + .onFalse(manipulator.setRollerVelocity(0)); + // Commands.runOnce( + // (() -> manipulator.setState(SuperState.INTAKE_CORAL_GROUND.manipulatorVelocity)))); + // .onFalse(Commands.runOnce((() -> manipulator.setState(0)))); // cancel bindTransition( From 77a59086e2b821c65467af58aab98324a64392f8 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 18:29:02 -0700 Subject: [PATCH 109/154] add cancel coral intake --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 1bad095b..66dcfb0c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -304,6 +304,9 @@ private void addTransitions() { SuperState.INTAKE_CORAL_GROUND, new Trigger(this::atExtension)); + bindTransition( + SuperState.PRE_INTAKE_CORAL_GROUND, SuperState.IDLE, Robot.intakeCoralReq.negate()); + bindTransition( SuperState.INTAKE_CORAL_GROUND, SuperState.POST_INTAKE_CORAL_GROUND, @@ -312,6 +315,10 @@ private void addTransitions() { .debounce(0.060) .and(manipulator::bothBeambreaks) .debounce(0.12)); // TODO i'm lowkey losing my shit + bindTransition( + SuperState.INTAKE_CORAL_GROUND, + SuperState.PRE_INTAKE_CORAL_GROUND, + Robot.intakeCoralReq.negate().and(manipulator::neitherBeambreak)); bindTransition( SuperState.POST_INTAKE_CORAL_GROUND, @@ -335,6 +342,7 @@ private void addTransitions() { .whileTrue(manipulator.setRollerVelocity(state.manipulatorVelocity)); Robot.intakeCoralReq + .and(() -> !manipulator.bothBeambreaks()) .onTrue(manipulator.setRollerVelocity(-18)) .onFalse(manipulator.setRollerVelocity(0)); // Commands.runOnce( From d89c70910896c649da5e45cf6d7c1b35748e115b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 19:22:17 -0700 Subject: [PATCH 110/154] L1 RAHHHHHH --- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/subsystems/Superstructure.java | 22 ++++++++++++------- .../shoulder/ShoulderSubsystem.java | 4 ++-- .../subsystems/wrist/WristSubsystem.java | 2 +- 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a33d7917..caa911f3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -185,6 +185,7 @@ public static enum AlgaeScoreTarget { new CommandXboxControllerSubsystem(1); private static Supplier pose = () -> new Pose2d(); + public static Supplier state = () -> SuperState.IDLE; @AutoLogOutput(key = "Superstructure/Pre Score Request") public static Trigger preScoreReq = @@ -209,6 +210,7 @@ public static enum AlgaeScoreTarget { .rightTrigger() .negate() .and(() -> DriverStation.isTeleop()) + .and(() -> Superstructure.stateIsScoreCoral(state.get()) || state.get() == SuperState.L1) .or(() -> Autos.autoScore && DriverStation.isAutonomous()); @AutoLogOutput(key = "Superstructure/Algae Intake Request") @@ -465,8 +467,6 @@ public static enum AlgaeScoreTarget { new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); - public static Supplier state = () -> SuperState.IDLE; - @SuppressWarnings({"resource", "unlikely-arg-type"}) public Robot() { DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 66dcfb0c..df92b0ab 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -34,14 +34,14 @@ public enum SuperState { IDLE(ElevatorState.HP, ShoulderState.HP, WristState.HP, -7.0), PRE_INTAKE_CORAL_GROUND( ElevatorState.INTAKE_CORAL_GROUND, - ShoulderState.PRE_INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, WristState.PRE_INTAKE_CORAL_GROUND, - -18.0), + -5.0), INTAKE_CORAL_GROUND( ElevatorState.INTAKE_CORAL_GROUND, ShoulderState.INTAKE_CORAL_GROUND, WristState.INTAKE_CORAL_GROUND, - -18.0), + -5.0), POST_INTAKE_CORAL_GROUND( ElevatorState.INTAKE_CORAL_GROUND, ShoulderState.PRE_INTAKE_CORAL_GROUND, @@ -341,9 +341,15 @@ private void addTransitions() { // .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); .whileTrue(manipulator.setRollerVelocity(state.manipulatorVelocity)); + Robot.scoreReq + .and(() -> state == SuperState.L1) + // .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) + // .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); + .whileTrue(manipulator.setRollerVelocity(3.0)); + Robot.intakeCoralReq .and(() -> !manipulator.bothBeambreaks()) - .onTrue(manipulator.setRollerVelocity(-18)) + .onTrue(manipulator.setRollerVelocity(-10)) .onFalse(manipulator.setRollerVelocity(0)); // Commands.runOnce( // (() -> manipulator.setState(SuperState.INTAKE_CORAL_GROUND.manipulatorVelocity)))); @@ -763,10 +769,10 @@ public boolean stateIsCoralAlike() { } public static boolean stateIsScoreCoral(SuperState state) { - return state == SuperState.L1 - || state == SuperState.L2 - || state == SuperState.L3 - || state == SuperState.L4; + return + // state == SuperState.L1 + // || + state == SuperState.L2 || state == SuperState.L3 || state == SuperState.L4; } public boolean stateIsAlgaeAlike() { diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 5f0194f7..2392b78a 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -38,8 +38,8 @@ public class ShoulderSubsystem extends SubsystemBase { public enum ShoulderState { HP(50.0), - PRE_INTAKE_CORAL_GROUND(35.0), - INTAKE_CORAL_GROUND(8.0), + PRE_INTAKE_CORAL_GROUND(15.0), + INTAKE_CORAL_GROUND(0.0), PRE_L1(35.0), L1(Units.radiansToDegrees(1.617)), // not sure about units tbh PRE_L2(35.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 6741d39d..671b76a1 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -35,7 +35,7 @@ public enum WristState { PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(0.0), HP(160.0), - L1(Units.radiansToDegrees(0.349)), + L1(Units.radiansToDegrees(0.349 - 0.1)), PRE_L2(170.0), L2(Units.radiansToDegrees(2.447)), PRE_L3(170.0), From e7ca565c45c779b1648f4112d0ca439145725bdd Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 30 Aug 2025 23:51:38 -0700 Subject: [PATCH 111/154] [not tested irl] maybe fix homing fr --- src/main/java/frc/robot/Autos.java | 3 +- src/main/java/frc/robot/Robot.java | 18 ++++------ .../frc/robot/subsystems/Superstructure.java | 35 +++++++++++++++---- 3 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b83e1bf7..93c80afb 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -461,7 +461,8 @@ public Command LOtoA() { // 2910 steps.put("AtoB", routine.trajectory("AtoB")); steps.put("BtoB", routine.trajectory("BtoB")); - if (Robot.isSimulation()) manipulator.setSimSecondBeambreak(true); // gah + // if (Robot.isSimulation()) manipulator.setSimSecondBeambreak(true); // gah //TODO why did i do + // this???? routine // run first path .active() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index caa911f3..61cacaa7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -243,14 +243,7 @@ public static enum AlgaeScoreTarget { public static final Trigger antiAlgaeJamReq = driver.b(); @AutoLogOutput(key = "Superstructure/Home Request") - public static Trigger homeReq = - driver - .start() - .and(() -> ElevatorSubsystem.hasZeroed || WristSubsystem.hasZeroed) - // zeroing on startup - .or( - new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) - .and(() -> DriverStation.isEnabled())); + public static Trigger homeReq = driver.start(); @AutoLogOutput(key = "Superstructure/Rev Funnel Req") public static Trigger revFunnelReq = operator.rightBumper(); @@ -915,10 +908,11 @@ public Robot() { .and(() -> ROBOT_TYPE == RobotType.SIM) .onTrue(Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae()))); - RobotModeTriggers.autonomous() - .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue( - Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); + // RobotModeTriggers.autonomous() + // .and(() -> ROBOT_TYPE == RobotType.SIM) + // .onTrue( + // Commands.runOnce(() -> + // manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); driver .start() .onTrue( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index df92b0ab..a339b4d5 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -263,7 +263,7 @@ public boolean atExtension() { private Command changeStateTo(SuperState nextState) { return Commands.runOnce( () -> { - System.out.println("Changing state to " + nextState); + System.out.println("Changing state from " + state + " to " + nextState); stateTimer.reset(); this.prevState = this.state; this.state = nextState; @@ -294,7 +294,10 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.READY_CORAL, - new Trigger(manipulator::eitherBeambreak).and(() -> manipulator.getTimeSinceZero() < 1.0)); + new Trigger(manipulator::eitherBeambreak) + .debounce(0.5) + .and(() -> manipulator.getTimeSinceZero() < 1.0), + Commands.print("sdfklsdjf")); // ---Intake coral ground--- bindTransition(SuperState.IDLE, SuperState.PRE_INTAKE_CORAL_GROUND, Robot.intakeCoralReq); @@ -663,15 +666,33 @@ private void addTransitions() { : AutoAim.RED_PROCESSOR_POS.getY(), 0.5))); + // bindTransition( + // SuperState.IDLE, + // SuperState.HOME_ELEVATOR, + // Robot.homeReq.and(() -> prevState != SuperState.HOME_ELEVATOR), + // Commands.runOnce( + // () -> { + // ElevatorSubsystem.hasZeroed = false; + // WristSubsystem.hasZeroed = false; + // wrist.resetEncoder(SuperState.IDLE.wristState.getAngle()); + // })); + + // zero on startup + bindTransition( + SuperState.IDLE, + SuperState.HOME_ELEVATOR, + new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) + .and(() -> DriverStation.isEnabled())); + + // manual request bindTransition( SuperState.IDLE, SuperState.HOME_ELEVATOR, - Robot.homeReq.and(() -> prevState != SuperState.HOME_ELEVATOR), + Robot.homeReq, Commands.runOnce( () -> { - ElevatorSubsystem.hasZeroed = false; - WristSubsystem.hasZeroed = false; - wrist.resetEncoder(SuperState.IDLE.wristState.getAngle()); + elevator.hasZeroed = false; + wrist.hasZeroed = false; })); // bindTransition( @@ -692,7 +713,7 @@ private void addTransitions() { bindTransition( SuperState.HOME_WRIST, SuperState.IDLE, - new Trigger(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), + Robot.homeReq.negate().and(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), Commands.runOnce( () -> wrist.rezero(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))))); From 1f7de5684d90b83965781f4193dbe0ada5411639 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 31 Aug 2025 00:07:23 -0700 Subject: [PATCH 112/154] [not tested irl] goofy ahh sim beambreak override --- src/main/java/frc/robot/Robot.java | 13 +++++++++++++ .../java/frc/robot/subsystems/Superstructure.java | 2 +- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 61cacaa7..2dc1a284 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -619,6 +619,19 @@ public Robot() { }) .ignoringDisable(true)); + SmartDashboard.putData( + "[SIM ONLY] Toggle First Beambreak", + Robot.isSimulation() + ? Commands.runOnce( + () -> manipulator.setSimFirstBeambreak(!manipulator.getFirstBeambreak())) + : Commands.none()); + SmartDashboard.putData( + "[SIM ONLY] Toggle Second Beambreak", + Robot.isSimulation() + ? Commands.runOnce( + () -> manipulator.setSimSecondBeambreak(!manipulator.getSecondBeambreak())) + : Commands.none()); + elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a339b4d5..1267e5bc 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -34,7 +34,7 @@ public enum SuperState { IDLE(ElevatorState.HP, ShoulderState.HP, WristState.HP, -7.0), PRE_INTAKE_CORAL_GROUND( ElevatorState.INTAKE_CORAL_GROUND, - ShoulderState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, WristState.PRE_INTAKE_CORAL_GROUND, -5.0), INTAKE_CORAL_GROUND( From 62f20c5b19374a77943ad09ba65852438680e7ce Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 31 Aug 2025 00:22:06 -0700 Subject: [PATCH 113/154] [not tested irl] wait to be further from reef before retracting --- .../frc/robot/subsystems/Superstructure.java | 46 +++++++++++++++---- 1 file changed, 37 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 1267e5bc..d17276f4 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -367,9 +367,16 @@ private void addTransitions() { bindTransition( SuperState.L1, SuperState.POST_L1, - new Trigger(() -> manipulator.neitherBeambreak()) + new Trigger(manipulator::neitherBeambreak) .and(this::atExtension) - .and(Robot.scoreReq.negate())); + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); bindTransition( SuperState.POST_L1, @@ -386,6 +393,7 @@ private void addTransitions() { .debounce(0.15)); // go straight to intaking algae from reef + //def needs tuning bindTransition( SuperState.POST_L1, SuperState.PRE_PRE_INTAKE_ALGAE, @@ -408,10 +416,16 @@ private void addTransitions() { bindTransition( SuperState.L2, SuperState.POST_L2, - new Trigger(() -> manipulator.neitherBeambreak()) + new Trigger(manipulator::neitherBeambreak) .and(this::atExtension) - .and(Robot.scoreReq.negate())); - + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); bindTransition( SuperState.POST_L2, SuperState.IDLE, @@ -449,9 +463,16 @@ private void addTransitions() { bindTransition( SuperState.L3, SuperState.POST_L3, - new Trigger(() -> manipulator.neitherBeambreak()) + new Trigger(manipulator::neitherBeambreak) .and(this::atExtension) - .and(Robot.scoreReq.negate())); + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); bindTransition( SuperState.POST_L3, @@ -490,9 +511,16 @@ private void addTransitions() { bindTransition( SuperState.L4, SuperState.POST_L4, - new Trigger(() -> manipulator.neitherBeambreak()) + new Trigger(manipulator::neitherBeambreak) .and(this::atExtension) - .and(Robot.scoreReq.negate())); + .and( + () -> + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); From 5ecb625dacef9caf8db162f4ec7c30e9d950447e Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 31 Aug 2025 01:25:08 -0700 Subject: [PATCH 114/154] [not tested irl] tune l4, it's ugly but hopefully more functional! --- .../frc/robot/subsystems/Superstructure.java | 22 ++++++++++++++----- .../elevator/ElevatorSubsystem.java | 1 + .../shoulder/ShoulderSubsystem.java | 3 ++- .../subsystems/wrist/WristSubsystem.java | 3 ++- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d17276f4..8f4ee289 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -61,8 +61,12 @@ public enum SuperState { L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), POST_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), - PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), - L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + // PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), + // L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), + PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), + YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), + L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.L4, 20.0), POST_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), POST_POST_L4( ElevatorState.HP, ShoulderState.L4, WristState.HP, 0.0), // like do we see the vision @@ -393,7 +397,7 @@ private void addTransitions() { .debounce(0.15)); // go straight to intaking algae from reef - //def needs tuning + // def needs tuning bindTransition( SuperState.POST_L1, SuperState.PRE_PRE_INTAKE_ALGAE, @@ -497,14 +501,19 @@ private void addTransitions() { // ---L4--- bindTransition( SuperState.READY_CORAL, - SuperState.PRE_L4, + SuperState.PRE_PRE_L4, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_L4, SuperState.L4, new Trigger(this::atExtension)); + bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); + bindTransition(SuperState.PRE_L4, SuperState.YAP_L4, new Trigger(this::atExtension)); + + bindTransition(SuperState.YAP_L4, SuperState.L4, new Trigger(this::atExtension)); + + // TODO i don't think any of the cancels work // cancel bindTransition( - SuperState.PRE_L4, + SuperState.PRE_PRE_L4, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); @@ -810,6 +819,7 @@ public boolean stateIsCoralAlike() { || state == SuperState.PRE_L1 || state == SuperState.PRE_L2 || state == SuperState.PRE_L3 + || state == SuperState.PRE_PRE_L4 || state == SuperState.PRE_L4 || state == SuperState.L1 || state == SuperState.L2 diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 86dc8d1b..d0f65e81 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -45,6 +45,7 @@ public enum ElevatorState { L1(0.217), L2(0.23 + Units.inchesToMeters(1.5)), L3(0.60 + Units.inchesToMeters(2.0)), + PRE_L4(1), L4(1.383), INTAKE_ALGAE_LOW(Units.inchesToMeters(20.0)), INTAKE_ALGAE_HIGH(Units.inchesToMeters(35.0)), diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 2392b78a..6447db5c 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -47,7 +47,8 @@ public enum ShoulderState { PRE_L3(35.0), L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), PRE_L4(8.0), - L4(25.0), + L4(30.0), + L4REAL(42), PRE_INTAKE_ALGAE_REEF(35.0), INTAKE_ALGAE_REEF(45.0), INTAKE_ALGAE_STACK(30.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 671b76a1..85374271 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -40,7 +40,8 @@ public enum WristState { L2(Units.radiansToDegrees(2.447)), PRE_L3(170.0), L3(Units.radiansToDegrees(2.427)), - L4(120.0), // ?? + PRE_L4(140), + L4(90.0), // ?? PRE_INTAKE_ALGAE_REEF(30.0), INTAKE_ALGAE_REEF(-20.0), INTAKE_ALGAE_STACK(-10), From 83a509b5c9b3819ea97386495a3013501fa5cfbc Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 1 Sep 2025 23:07:35 -0700 Subject: [PATCH 115/154] [not tested irl] new l4 lineup --- src/main/java/frc/robot/Robot.java | 68 ++++++++++++++++++- .../frc/robot/subsystems/Superstructure.java | 20 +++--- .../shoulder/ShoulderSubsystem.java | 6 +- 3 files changed, 80 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2dc1a284..fc059802 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -700,7 +701,11 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike() && coralTarget != ReefTarget.L1) + .and( + () -> + superstructure.stateIsCoralAlike() + && coralTarget != ReefTarget.L1 + && coralTarget != ReefTarget.L4) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -746,6 +751,67 @@ public Robot() { L1Targets.getNearestLine(swerve.getPose()).getRotation()))) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to score L4 + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L4) + .and(() -> !superstructure.atExtension(SuperState.L4)) + .whileTrue( + // AutoAim.alignToLine( + // swerve, + // () -> + // modifyJoystick(driver.getLeftY()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getLeftX()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> L1Targets.getNearestLine(swerve.getPose())) + Commands.parallel( + AutoAim.translateToPose( + swerve, + () -> { + var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); + return CoralTargets.getHandedClosestTarget( + swerve + .getPose() + .plus( + new Transform2d( + twist.dx, + twist.dy, + Rotation2d.fromRadians(twist.dtheta))), + driver.leftBumper().getAsBoolean()) + .exp(new Twist2d(-0.5, 0, 0)); + }), + Commands.waitUntil( + () -> + AutoAim.isInToleranceCoral( + swerve.getPose())) // don't know if this does anything + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L4) + .and(() -> superstructure.atExtension(SuperState.L4)) + .debounce(0.25) + .whileTrue( + Commands.parallel( + AutoAim.translateToPose( + swerve, + () -> { + var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); + return CoralTargets.getHandedClosestTarget( + swerve + .getPose() + .plus( + new Transform2d( + twist.dx, twist.dy, Rotation2d.fromRadians(twist.dtheta))), + driver.leftBumper().getAsBoolean()); + }), + Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + driver .rightBumper() .and( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 8f4ee289..a26cb02f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -63,13 +63,15 @@ public enum SuperState { // PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), // L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), - PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), - PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), - YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), - L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.L4, 20.0), - POST_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), + // PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), + // PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), + // YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), + PRE_PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), + PRE_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.L4, 0.0), + L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), POST_POST_L4( - ElevatorState.HP, ShoulderState.L4, WristState.HP, 0.0), // like do we see the vision + ElevatorState.HP, ShoulderState.PRE_L4, WristState.HP, 0.0), // like do we see the vision PRE_PRE_INTAKE_ALGAE( ElevatorState.HP, @@ -254,7 +256,7 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger, C .onTrue(Commands.parallel(changeStateTo(end), cmd)); } - private boolean atExtension(SuperState state) { + public boolean atExtension(SuperState state) { return elevator.atExtension(state.elevatorState.getExtensionMeters()) && shoulder.isNearAngle(state.shoulderState.getAngle()) && wrist.isNearAngle(state.wristState.getAngle()); @@ -506,9 +508,7 @@ private void addTransitions() { bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); - bindTransition(SuperState.PRE_L4, SuperState.YAP_L4, new Trigger(this::atExtension)); - - bindTransition(SuperState.YAP_L4, SuperState.L4, new Trigger(this::atExtension)); + bindTransition(SuperState.PRE_L4, SuperState.L4, new Trigger(this::atExtension)); // TODO i don't think any of the cancels work // cancel diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 6447db5c..c8575292 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -46,9 +46,9 @@ public enum ShoulderState { L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)).getDegrees()), PRE_L3(35.0), L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), - PRE_L4(8.0), - L4(30.0), - L4REAL(42), + // PRE_L4(8.0), + PRE_L4(30.0), + L4(42), PRE_INTAKE_ALGAE_REEF(35.0), INTAKE_ALGAE_REEF(45.0), INTAKE_ALGAE_STACK(30.0), From bee2317dc9cda38534e2d4dde473bb655305d295 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 1 Sep 2025 23:17:35 -0700 Subject: [PATCH 116/154] [not tested irl] tune l4 intermediate autoalign --- src/main/java/frc/robot/Robot.java | 11 +---------- .../frc/robot/subsystems/Superstructure.java | 16 +++++++++------- 2 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fc059802..4eec829d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -758,15 +758,6 @@ public Robot() { .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L4) .and(() -> !superstructure.atExtension(SuperState.L4)) .whileTrue( - // AutoAim.alignToLine( - // swerve, - // () -> - // modifyJoystick(driver.getLeftY()) - // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - // () -> - // modifyJoystick(driver.getLeftX()) - // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - // () -> L1Targets.getNearestLine(swerve.getPose())) Commands.parallel( AutoAim.translateToPose( swerve, @@ -781,7 +772,7 @@ public Robot() { twist.dy, Rotation2d.fromRadians(twist.dtheta))), driver.leftBumper().getAsBoolean()) - .exp(new Twist2d(-0.5, 0, 0)); + .exp(new Twist2d(-0.25, 0, 0)); }), Commands.waitUntil( () -> diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a26cb02f..a1938932 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -66,12 +66,12 @@ public enum SuperState { // PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), // PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), // YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), - PRE_PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), - PRE_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.L4, 0.0), + PRE_PRE_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), + PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 0.0), L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), - POST_POST_L4( - ElevatorState.HP, ShoulderState.PRE_L4, WristState.HP, 0.0), // like do we see the vision + // POST_POST_L4( + // ElevatorState.HP, ShoulderState.PRE_L4, WristState.HP, 0.0), // like do we see the vision PRE_PRE_INTAKE_ALGAE( ElevatorState.HP, @@ -531,10 +531,11 @@ private void addTransitions() { // .and(Robot.scoreReq.negate()) ); - bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); + // bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); bindTransition( - SuperState.POST_POST_L4, + // SuperState.POST_POST_L4, + SuperState.POST_L4, SuperState.IDLE, Robot.intakeAlgaeReq .negate() @@ -549,7 +550,8 @@ private void addTransitions() { // go straight to intaking algae from reef bindTransition( - SuperState.POST_POST_L4, + // SuperState.POST_POST_L4, + SuperState.POST_L4, SuperState.PRE_PRE_INTAKE_ALGAE, new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); From b052508ef2de61ef96983afe5688568800b8958b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 1 Sep 2025 23:35:10 -0700 Subject: [PATCH 117/154] add comments to triggers and stuff but i just remembered i have an apes test tomorrow --- src/main/java/frc/robot/Robot.java | 88 +++++++++++++++++++----------- 1 file changed, 57 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4eec829d..12ce8303 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,30 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.elevator.ElevatorSubsystem.ELEVATOR_ANGLE; +import java.util.HashMap; +import java.util.Optional; +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.Supplier; +import java.util.stream.Stream; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.SignalLogger; @@ -20,6 +44,7 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -93,28 +118,6 @@ import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; -import java.util.HashMap; -import java.util.Optional; -import java.util.Set; -import java.util.function.BiConsumer; -import java.util.function.Supplier; -import java.util.stream.Stream; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.GyroSimulation; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { public enum RobotType { @@ -172,8 +175,6 @@ public static enum AlgaeScoreTarget { @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.BARGE; private boolean leftHandedTarget = false; - @AutoLogOutput private boolean killVisionIK = true; - @AutoLogOutput private boolean haveAutosGenerated = false; private static CANBus canivore = new CANBus("*"); @@ -555,6 +556,7 @@ public Robot() { || superstructure.getState() == SuperState.READY_CORAL) .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.5)); + // Set auto scoring, etc bindings to false at the start of teleop new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoScore = false)); @@ -564,6 +566,7 @@ public Robot() { new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoGroundCoralIntake = false)); + // Zero elevator/wrist at the start of auto new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) .onTrue( Commands.runOnce( @@ -572,6 +575,7 @@ public Robot() { elevator.resetExtension(0.0); })); + // Add autos on alliance change new Trigger( () -> { var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); @@ -583,6 +587,7 @@ public Robot() { .alongWith(leds.setBlinkingCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + // Add autos when first connecting to DS new Trigger( () -> DriverStation.isDSAttached() @@ -594,6 +599,7 @@ public Robot() { .alongWith(leds.setBlinkingCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + // Disable swerve current limits in auto I think? new Trigger(() -> DriverStation.isAutonomousEnabled()) .onTrue( Commands.runOnce( @@ -632,6 +638,11 @@ public Robot() { ? Commands.runOnce( () -> manipulator.setSimSecondBeambreak(!manipulator.getSecondBeambreak())) : Commands.none()); + SmartDashboard.putData( + "[SIM ONLY] Toggle Has Algae", + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae())) + : Commands.none()); elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); @@ -698,6 +709,8 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed()) .times(-1))); + // ----Controller bindings---- + // Auto align for coral scoring L2/3 driver .rightBumper() .or(driver.leftBumper()) @@ -726,6 +739,7 @@ public Robot() { Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align for coral scoring L1 driver .rightBumper() .or(driver.leftBumper()) @@ -751,7 +765,7 @@ public Robot() { L1Targets.getNearestLine(swerve.getPose()).getRotation()))) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); - // Auto align to score L4 + // Auto align to intermediate pose to score L4 driver .rightBumper() .or(driver.leftBumper()) @@ -780,6 +794,7 @@ public Robot() { swerve.getPose())) // don't know if this does anything .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to score L4 driver .rightBumper() .or(driver.leftBumper()) @@ -803,6 +818,7 @@ public Robot() { Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to intake algae (high, low, stack) driver .rightBumper() .and( @@ -875,7 +891,9 @@ public Robot() { // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), // () -> // modifyJoystick(driver.getRightX()) - // * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); + // * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())) + + // Auto align to processor driver .rightBumper() .or(driver.leftBumper()) @@ -909,6 +927,7 @@ public Robot() { swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to cage driver .rightBumper() .and( @@ -926,6 +945,7 @@ public Robot() { swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to barge driver .rightBumper() .and( @@ -963,6 +983,7 @@ public Robot() { }) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Set sim beambreak/algae driver .povUp() .and(() -> ROBOT_TYPE == RobotType.SIM) @@ -983,6 +1004,9 @@ public Robot() { // .onTrue( // Commands.runOnce(() -> // manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); + + // Reset sim pose (?) + // Has this literally ever been used driver .start() .onTrue( @@ -992,8 +1016,11 @@ public Robot() { swerveDriveSimulation.get().setSimulationWorldPose(swerve.getPose()); } })); + + // Rezero shoulder driver.x().onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); + // Operator - Set scoring/intaking levels operator .a() .onTrue( @@ -1035,14 +1062,12 @@ public Robot() { .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); + //Enable/disable left handed auto align + //TODO isn't this already accounted for by the autoaim method? operator.povLeft().onTrue(Commands.runOnce(() -> leftHandedTarget = true)); operator.povRight().onTrue(Commands.runOnce(() -> leftHandedTarget = false)); - operator - .back() - .and(operator.start()) - .onTrue(Commands.runOnce(() -> killVisionIK = !killVisionIK)); - + // Set LEDs when the robot has an algae (alga?) new Trigger(() -> superstructure.stateIsAlgaeAlike()) .whileTrue( leds.setBlinkingSplitCmd( @@ -1052,6 +1077,7 @@ public Robot() { algaeScoreTarget == AlgaeScoreTarget.BARGE), () -> Color.kBlack, 5.0)); + // heading reset driver .leftStick() From 896223e1558ace650a564145d176210d8148ca5a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 17:01:05 -0700 Subject: [PATCH 118/154] tune l4 --- src/main/java/frc/robot/Robot.java | 55 +++++++++---------- .../shoulder/ShoulderSubsystem.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 2 +- 3 files changed, 28 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 12ce8303..58d31b29 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,30 +10,6 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.elevator.ElevatorSubsystem.ELEVATOR_ANGLE; -import java.util.HashMap; -import java.util.Optional; -import java.util.Set; -import java.util.function.BiConsumer; -import java.util.function.Supplier; -import java.util.stream.Stream; - -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.GyroSimulation; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; -import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; - import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; import com.ctre.phoenix6.SignalLogger; @@ -44,7 +20,6 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -118,6 +93,28 @@ import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; +import java.util.HashMap; +import java.util.Optional; +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.Supplier; +import java.util.stream.Stream; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; +import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { public enum RobotType { @@ -1004,7 +1001,7 @@ public Robot() { // .onTrue( // Commands.runOnce(() -> // manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); - + // Reset sim pose (?) // Has this literally ever been used driver @@ -1062,8 +1059,8 @@ public Robot() { .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); - //Enable/disable left handed auto align - //TODO isn't this already accounted for by the autoaim method? + // Enable/disable left handed auto align + // TODO isn't this already accounted for by the autoaim method? operator.povLeft().onTrue(Commands.runOnce(() -> leftHandedTarget = true)); operator.povRight().onTrue(Commands.runOnce(() -> leftHandedTarget = false)); @@ -1077,7 +1074,7 @@ public Robot() { algaeScoreTarget == AlgaeScoreTarget.BARGE), () -> Color.kBlack, 5.0)); - + // heading reset driver .leftStick() diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index c8575292..244c9788 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -48,7 +48,7 @@ public enum ShoulderState { L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), // PRE_L4(8.0), PRE_L4(30.0), - L4(42), + L4(50), PRE_INTAKE_ALGAE_REEF(35.0), INTAKE_ALGAE_REEF(45.0), INTAKE_ALGAE_STACK(30.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 85374271..5d85ef32 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -41,7 +41,7 @@ public enum WristState { PRE_L3(170.0), L3(Units.radiansToDegrees(2.427)), PRE_L4(140), - L4(90.0), // ?? + L4(98.0), // ?? PRE_INTAKE_ALGAE_REEF(30.0), INTAKE_ALGAE_REEF(-20.0), INTAKE_ALGAE_STACK(-10), From c8d7b4178f5c37546ccb5a7d21199351102e6530 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 17:24:43 -0700 Subject: [PATCH 119/154] tune algae stuff. forgot to turn on wheels but i'll do that next --- .../subsystems/ManipulatorSubsystem.java | 17 ++++++------- .../frc/robot/subsystems/Superstructure.java | 25 ++++++++++++++++--- 2 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 86052105..b4792766 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.beambreak.BeambreakIO; @@ -91,13 +90,13 @@ public void resetPosition(final double rotations) { io.resetEncoder(rotations); } - public Command intakeAlgae() { - return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) - .until( - new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) - .debounce(0.75)) - .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); - } + // public Command intakeAlgae() { + // return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) + // .until( + // new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + // .debounce(0.75)) + // .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); + // } public Command setStateVelocity(BooleanSupplier checkExtension) { // return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); @@ -113,7 +112,7 @@ public double getStatorCurrentAmps() { } public boolean hasAlgae() { // TODO icky - return getStatorCurrentAmps() > ALGAE_CURRENT_THRESHOLD || hasAlgaeSim; + return Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD || hasAlgaeSim; } public boolean getFirstBeambreak() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a1938932..4921cfe2 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -574,7 +574,11 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_LOW, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW) + .or(Robot.intakeAlgaeReq) + .negate() + .and(() -> !manipulator.hasAlgae()) + .debounce(0.25)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -601,7 +605,11 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_HIGH, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH) + .or(Robot.intakeAlgaeReq) + .negate() + .and(() -> !manipulator.hasAlgae()) + .debounce(0.25)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -628,7 +636,11 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_STACK, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK) + .or(Robot.intakeAlgaeReq) + .negate() + .and(() -> !manipulator.hasAlgae()) + .debounce(0.25)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -647,9 +659,14 @@ private void addTransitions() { bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND) + .or(Robot.intakeAlgaeReq) + .negate() + .and(() -> !manipulator.hasAlgae()) + .debounce(0.25)); // seems like the post pickup state is different for reef/ground?? why would you do this + // TODO tune intake algae ground bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.READY_ALGAE, From 63260e4cace9fd3ecedb81b48994d9a2c73c476d Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 18:53:56 -0700 Subject: [PATCH 120/154] intake algae works now i think --- .../subsystems/ManipulatorSubsystem.java | 31 +++--- .../frc/robot/subsystems/Superstructure.java | 97 +++++++++++++++---- 2 files changed, 96 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index b4792766..a23679ce 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -7,6 +7,8 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.beambreak.BeambreakIO; @@ -26,7 +28,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 6.0; + public static final double ALGAE_CURRENT_THRESHOLD = 10.0; public static final double CORAL_HOLD_POS = 0.6; @@ -39,12 +41,13 @@ public class ManipulatorSubsystem extends RollerSubsystem { private boolean bb1Sim = false; private boolean bb2Sim = false; @AutoLogOutput private boolean hasAlgaeSim = false; + @AutoLogOutput public boolean hasAlgaeReal = false; @AutoLogOutput(key = "Manipulator State Velocity") private double stateVelocity = 0.0; - private LinearFilter currentFilter = LinearFilter.movingAverage(20); - private double currentFilterValue = 0.0; + private LinearFilter currentFilter = LinearFilter.movingAverage(10); + @AutoLogOutput private double currentFilterValue = 0.0; private Timer zeroTimer = new Timer(); @@ -90,13 +93,15 @@ public void resetPosition(final double rotations) { io.resetEncoder(rotations); } - // public Command intakeAlgae() { - // return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) - // .until( - // new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) - // .debounce(0.75)) - // .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); - // } + public Command intakeAlgae() { + return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) + .until( + new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + .debounce(0.25)) + .andThen( + Commands.runOnce(() -> hasAlgaeReal = true) + .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE)))); + } public Command setStateVelocity(BooleanSupplier checkExtension) { // return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); @@ -111,8 +116,12 @@ public double getStatorCurrentAmps() { return currentFilterValue; } + // @AutoLogOutput(key = "Manipulator/Has Algae") public boolean hasAlgae() { // TODO icky - return Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD || hasAlgaeSim; + // return new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + // .debounce(0.75) + // .getAsBoolean() + return hasAlgaeReal || hasAlgaeSim; } public boolean getFirstBeambreak() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4921cfe2..17766037 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -159,7 +159,8 @@ public enum SuperState { public final ElevatorState elevatorState; public final ShoulderState shoulderState; public final WristState wristState; - public final double manipulatorVelocity; + public final double + manipulatorVelocity; // negative is scoring coral l2-4 positive is intaking algae public final double climberPosition; public final double climberSpeed; @@ -364,6 +365,37 @@ private void addTransitions() { // (() -> manipulator.setState(SuperState.INTAKE_CORAL_GROUND.manipulatorVelocity)))); // .onFalse(Commands.runOnce((() -> manipulator.setState(0)))); + // Robot.intakeAlgaeReq + // .and(() -> !manipulator.hasAlgae()) + // // .whileTrue(manipulator.setRollerVelocity(5.0)); + // .whileTrue(manipulator.intakeAlgae()); + + // new Trigger(() -> manipulator.hasAlgae()) + // .debounce(0.75) + // .and(Robot.scoreReq) + // .negate() + // .whileTrue(manipulator.setRollerVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)); + + // Robot.intakeAlgaeReq.and(this::stateIsIntakeAlgae).whileTrue(manipulator.setRollerVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); + + // Robot.intakeAlgaeReq.negate().and(()-> + // stateTimer.hasElapsed(1.0)).whileTrue(manipulator.intakeAlgae()); + + new Trigger( + () -> + state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.READY_ALGAE) + .whileTrue(manipulator.intakeAlgae()); + + // The way i'm handling the manipulator rn completely undermines the states i was trying to get + // at tbh + + new Trigger(() -> state == SuperState.READY_ALGAE) + .and(() -> manipulator.getStatorCurrentAmps() < 20.0) + .debounce(1.0) + .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + // cancel bindTransition( SuperState.PRE_L1, @@ -571,14 +603,18 @@ private void addTransitions() { .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW))); // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_LOW, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + bindTransition( SuperState.INTAKE_ALGAE_LOW, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW) - .or(Robot.intakeAlgaeReq) - .negate() - .and(() -> !manipulator.hasAlgae()) - .debounce(0.25)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -602,14 +638,18 @@ private void addTransitions() { .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH))); // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_HIGH, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + bindTransition( SuperState.INTAKE_ALGAE_HIGH, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH) - .or(Robot.intakeAlgaeReq) - .negate() - .and(() -> !manipulator.hasAlgae()) - .debounce(0.25)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -633,14 +673,18 @@ private void addTransitions() { .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.STACK))); // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_STACK, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + bindTransition( SuperState.INTAKE_ALGAE_STACK, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK) - .or(Robot.intakeAlgaeReq) - .negate() - .and(() -> !manipulator.hasAlgae()) - .debounce(0.25)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK)); // seems like the post pickup state is different for reef/ground?? why would you do this bindTransition( @@ -656,14 +700,18 @@ private void addTransitions() { .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.GROUND))); // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_GROUND, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + bindTransition( SuperState.INTAKE_ALGAE_GROUND, SuperState.IDLE, - new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND) - .or(Robot.intakeAlgaeReq) - .negate() - .and(() -> !manipulator.hasAlgae()) - .debounce(0.25)); + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND)); // seems like the post pickup state is different for reef/ground?? why would you do this // TODO tune intake algae ground @@ -869,6 +917,13 @@ public boolean stateIsAlgaeAlike() { // SPIT_ALGAE, } + public boolean stateIsIntakeAlgae() { + return state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_GROUND; + } + public boolean antiJamCoral() { return antiJamCoral; } From 078e4a4514e2e3ae2f569a880e694dfcc9081742 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 19:11:48 -0700 Subject: [PATCH 121/154] funnel --- .../frc/robot/subsystems/Superstructure.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 17766037..6c2f1e63 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -19,10 +19,12 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.subsystems.wrist.WristSubsystem.WristState; +import frc.robot.utils.FieldUtils; import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.autoaim.AutoAim; import java.util.function.Supplier; +import java.util.stream.Stream; import org.littletonrobotics.junction.Logger; public class Superstructure { @@ -345,6 +347,7 @@ private void addTransitions() { bindTransition(SuperState.PRE_L1, SuperState.L1, new Trigger(this::atExtension)); + // manipulator stuff because ?? Robot.scoreReq .and(() -> stateIsScoreCoral(state)) // .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) @@ -396,6 +399,20 @@ private void addTransitions() { .debounce(1.0) .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + Robot.forceFunnelReq + .or( + new Trigger( + () -> + (Stream.of(FieldUtils.HumanPlayerTargets.values()) + .map( + (t) -> + t.location.minus(swerve.getPose()).getTranslation().getNorm()) + .min(Double::compare) + .get() + < 1.0))) + .and(manipulator::neitherBeambreak) + .whileTrue(manipulator.setRollerVelocity(-7.0)) + .whileFalse(manipulator.setRollerVelocity(0.0)); // cancel bindTransition( SuperState.PRE_L1, From 9b370d79482d2d7b3842feca8e78f9af0f2ac562 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 19:18:15 -0700 Subject: [PATCH 122/154] re speed up elevator --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 ++--- .../java/frc/robot/subsystems/elevator/ElevatorIOReal.java | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 6c2f1e63..072ac28a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -303,9 +303,8 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.READY_CORAL, - new Trigger(manipulator::eitherBeambreak) - .debounce(0.5) - .and(() -> manipulator.getTimeSinceZero() < 1.0), + new Trigger(manipulator::eitherBeambreak).debounce(0.5), + // .and(() -> manipulator.getTimeSinceZero() < 1.0), Commands.print("sdfklsdjf")); // ---Intake coral ground--- diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index a46b28e2..523e1198 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -80,9 +80,9 @@ public ElevatorIOReal() { config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerTime = 0.25; - config.MotionMagic.MotionMagicAcceleration = ElevatorSubsystem.SLOW_ACCELERATION; + config.MotionMagic.MotionMagicAcceleration = ElevatorSubsystem.MAX_ACCELERATION; // Estimated from slightly less than motor free speed - config.MotionMagic.MotionMagicCruiseVelocity = 2; + config.MotionMagic.MotionMagicCruiseVelocity = 4.5; // (5500.0 / 60.0) / config.Feedback.SensorToMechanismRatio; config.MotionMagic.MotionMagicExpo_kV = 1.9; From b8b4155ad17356d237656fb7b5c80821edc7aae6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 2 Sep 2025 19:29:55 -0700 Subject: [PATCH 123/154] re speed up shoulder and wrist --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 ++--- .../java/frc/robot/subsystems/shoulder/ShoulderIOReal.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 072ac28a..433da78e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -303,9 +303,8 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.READY_CORAL, - new Trigger(manipulator::eitherBeambreak).debounce(0.5), - // .and(() -> manipulator.getTimeSinceZero() < 1.0), - Commands.print("sdfklsdjf")); + new Trigger(manipulator::eitherBeambreak).debounce(0.5)); + // .and(() -> manipulator.getTimeSinceZero() < 1.0), // ---Intake coral ground--- bindTransition(SuperState.IDLE, SuperState.PRE_INTAKE_CORAL_GROUND, Robot.intakeCoralReq); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 47ae4373..ab724233 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -66,7 +66,7 @@ public ShoulderIOReal() { // guesses config.MotionMagic.MotionMagicCruiseVelocity = 1.0; - config.MotionMagic.MotionMagicAcceleration = 1.0; + config.MotionMagic.MotionMagicAcceleration = 4.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 5d85ef32..a4b01845 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(1).withMotionMagicAcceleration(1); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); public static MotionMagicConfigs SLOW_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(3); From 81b7acf56930fc60b1291f12326d01c2f35c199c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 6 Sep 2025 09:44:02 -0700 Subject: [PATCH 124/154] [not tested irl] special case elevator and wrist zeroing --- src/main/java/frc/robot/Robot.java | 7 ++- .../frc/robot/subsystems/Superstructure.java | 52 +++++++++++++------ .../elevator/ElevatorSubsystem.java | 7 ++- .../subsystems/wrist/WristSubsystem.java | 11 ++-- 4 files changed, 56 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 58d31b29..067e2c5d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -241,8 +241,11 @@ public static enum AlgaeScoreTarget { @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") public static final Trigger antiAlgaeJamReq = driver.b(); - @AutoLogOutput(key = "Superstructure/Home Request") - public static Trigger homeReq = driver.start(); + @AutoLogOutput(key = "Superstructure/Home Elevator Request") + public static Trigger homeElevatorReq = driver.start(); + + @AutoLogOutput(key = "Superstructure/Home Wrist Request") + public static Trigger homeWristReq = driver.povDown(); //TODO i want to use the back button but it didn't seem to work @AutoLogOutput(key = "Superstructure/Rev Funnel Req") public static Trigger revFunnelReq = operator.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 433da78e..70cb5349 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -800,19 +800,26 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.HOME_ELEVATOR, - new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) + new Trigger(() -> !ElevatorSubsystem.hasZeroed + // || !WristSubsystem.hasZeroed + ) .and(() -> DriverStation.isEnabled())); - // manual request - bindTransition( - SuperState.IDLE, - SuperState.HOME_ELEVATOR, - Robot.homeReq, - Commands.runOnce( - () -> { - elevator.hasZeroed = false; - wrist.hasZeroed = false; - })); + // // manual request + // bindTransition( + // SuperState.IDLE, + // SuperState.HOME_ELEVATOR, + // Robot.homeElevatorReq, + // Commands.runOnce( + // () -> { + // elevator.hasZeroed = false; + // // wrist.hasZeroed = false; + // })); + Robot.homeElevatorReq.onTrue(Commands.runOnce( + () -> { + elevator.hasZeroed = false; + // wrist.hasZeroed = false; + })); // bindTransition( // SuperState.READY_CORAL, @@ -822,20 +829,35 @@ private void addTransitions() { bindTransition( SuperState.HOME_ELEVATOR, - SuperState.HOME_WRIST, - // SuperState.IDLE, + // SuperState.HOME_WRIST, + SuperState.IDLE, new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0).debounce(0.1), - Commands.runOnce(() -> elevator.resetExtension(0.0)) + Commands.runOnce(() -> elevator.resetExtension(0.0)) //not sure if i still need this but // .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0))) ); + Robot.homeWristReq.onTrue(Commands.runOnce( + () -> { + wrist.hasZeroed = false; + })); + + bindTransition( + SuperState.IDLE, + SuperState.HOME_WRIST, + new Trigger(() -> !WristSubsystem.hasZeroed && ElevatorSubsystem.hasZeroed) + .and(() -> DriverStation.isEnabled())); + bindTransition( SuperState.HOME_WRIST, SuperState.IDLE, - Robot.homeReq.negate().and(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), + // Robot.homeElevatorReq.negate().and + new Trigger + (() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), Commands.runOnce( () -> wrist.rezero(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))))); + //i shall just hope and pray + // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the // manipulator wheels bindTransition( diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d0f65e81..27eef156 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -135,7 +136,11 @@ public Command setExtension(double meters) { } public Command setStateExtension() { - return setExtension(() -> state.getExtensionMeters()); + if (state == SuperState.HOME_ELEVATOR.elevatorState) { + return runCurrentZeroing(); + } else { + return setExtension(() -> state.getExtensionMeters()); + } } public boolean atExtension(double expected) { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a4b01845..88c05d52 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -8,6 +8,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.subsystems.Superstructure.SuperState; import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -50,7 +51,7 @@ public enum WristState { PRE_BARGE(100), SCORE_BARGE(110), PROCESSOR(-30.0), - HOME(Units.radiansToDegrees(-0.687 - 2.0)) // i dunno + HOME(Units.radiansToDegrees(-0.687 - 5.0)) // i dunno ; private final Supplier angle; @@ -100,7 +101,11 @@ public void setState(WristState state) { } public Command setStateAngle() { - return setAngle(() -> state.getAngle()); + if (state == SuperState.HOME_WRIST.wristState) { + return currentZero(); + } else { + return setAngle(() -> state.getAngle()); + } } public Command setAngle(final Supplier target) { @@ -164,7 +169,7 @@ public Command currentZero() { .finallyDo( (interrupted) -> { if (!interrupted) { - io.resetEncoder(Rotation2d.fromRadians(-0.687)); + io.resetEncoder(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))); //TODO zero position may need tuning hasZeroed = true; } })); From c91f34a5ce3b1485dc0d1e351f663888277f05f0 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 6 Sep 2025 16:17:26 -0700 Subject: [PATCH 125/154] shoulder offset --- src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index ab724233..fdd9fc8d 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -82,7 +82,7 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; cancoderConfig.MagnetSensor.MagnetOffset = - -0.36; // -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; + -0.524; // -0.36; // -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); From 2dc4d6f64d910dcf468a73c17ee1cdf5178d445f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 6 Sep 2025 17:57:35 -0700 Subject: [PATCH 126/154] Revert "[not tested irl] special case elevator and wrist zeroing" This reverts commit 81b7acf56930fc60b1291f12326d01c2f35c199c. --- src/main/java/frc/robot/Robot.java | 7 +-- .../frc/robot/subsystems/Superstructure.java | 52 ++++++------------- .../elevator/ElevatorSubsystem.java | 7 +-- .../subsystems/wrist/WristSubsystem.java | 11 ++-- 4 files changed, 21 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 067e2c5d..58d31b29 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -241,11 +241,8 @@ public static enum AlgaeScoreTarget { @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") public static final Trigger antiAlgaeJamReq = driver.b(); - @AutoLogOutput(key = "Superstructure/Home Elevator Request") - public static Trigger homeElevatorReq = driver.start(); - - @AutoLogOutput(key = "Superstructure/Home Wrist Request") - public static Trigger homeWristReq = driver.povDown(); //TODO i want to use the back button but it didn't seem to work + @AutoLogOutput(key = "Superstructure/Home Request") + public static Trigger homeReq = driver.start(); @AutoLogOutput(key = "Superstructure/Rev Funnel Req") public static Trigger revFunnelReq = operator.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 70cb5349..433da78e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -800,26 +800,19 @@ private void addTransitions() { bindTransition( SuperState.IDLE, SuperState.HOME_ELEVATOR, - new Trigger(() -> !ElevatorSubsystem.hasZeroed - // || !WristSubsystem.hasZeroed - ) + new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) .and(() -> DriverStation.isEnabled())); - // // manual request - // bindTransition( - // SuperState.IDLE, - // SuperState.HOME_ELEVATOR, - // Robot.homeElevatorReq, - // Commands.runOnce( - // () -> { - // elevator.hasZeroed = false; - // // wrist.hasZeroed = false; - // })); - Robot.homeElevatorReq.onTrue(Commands.runOnce( - () -> { - elevator.hasZeroed = false; - // wrist.hasZeroed = false; - })); + // manual request + bindTransition( + SuperState.IDLE, + SuperState.HOME_ELEVATOR, + Robot.homeReq, + Commands.runOnce( + () -> { + elevator.hasZeroed = false; + wrist.hasZeroed = false; + })); // bindTransition( // SuperState.READY_CORAL, @@ -829,35 +822,20 @@ private void addTransitions() { bindTransition( SuperState.HOME_ELEVATOR, - // SuperState.HOME_WRIST, - SuperState.IDLE, + SuperState.HOME_WRIST, + // SuperState.IDLE, new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0).debounce(0.1), - Commands.runOnce(() -> elevator.resetExtension(0.0)) //not sure if i still need this but + Commands.runOnce(() -> elevator.resetExtension(0.0)) // .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0))) ); - Robot.homeWristReq.onTrue(Commands.runOnce( - () -> { - wrist.hasZeroed = false; - })); - - bindTransition( - SuperState.IDLE, - SuperState.HOME_WRIST, - new Trigger(() -> !WristSubsystem.hasZeroed && ElevatorSubsystem.hasZeroed) - .and(() -> DriverStation.isEnabled())); - bindTransition( SuperState.HOME_WRIST, SuperState.IDLE, - // Robot.homeElevatorReq.negate().and - new Trigger - (() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), + Robot.homeReq.negate().and(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), Commands.runOnce( () -> wrist.rezero(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))))); - //i shall just hope and pray - // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the // manipulator wheels bindTransition( diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 27eef156..d0f65e81 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -136,11 +135,7 @@ public Command setExtension(double meters) { } public Command setStateExtension() { - if (state == SuperState.HOME_ELEVATOR.elevatorState) { - return runCurrentZeroing(); - } else { - return setExtension(() -> state.getExtensionMeters()); - } + return setExtension(() -> state.getExtensionMeters()); } public boolean atExtension(double expected) { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 88c05d52..a4b01845 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -51,7 +50,7 @@ public enum WristState { PRE_BARGE(100), SCORE_BARGE(110), PROCESSOR(-30.0), - HOME(Units.radiansToDegrees(-0.687 - 5.0)) // i dunno + HOME(Units.radiansToDegrees(-0.687 - 2.0)) // i dunno ; private final Supplier angle; @@ -101,11 +100,7 @@ public void setState(WristState state) { } public Command setStateAngle() { - if (state == SuperState.HOME_WRIST.wristState) { - return currentZero(); - } else { - return setAngle(() -> state.getAngle()); - } + return setAngle(() -> state.getAngle()); } public Command setAngle(final Supplier target) { @@ -169,7 +164,7 @@ public Command currentZero() { .finallyDo( (interrupted) -> { if (!interrupted) { - io.resetEncoder(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))); //TODO zero position may need tuning + io.resetEncoder(Rotation2d.fromRadians(-0.687)); hasZeroed = true; } })); From 9ceea7e094f5317abe02af41044db975af342fa2 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 6 Sep 2025 19:00:35 -0700 Subject: [PATCH 127/154] try to fix jog --- .../frc/robot/subsystems/Superstructure.java | 28 ++++++++++++++++++- .../subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 433da78e..3c687709 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -25,6 +25,7 @@ import frc.robot.utils.autoaim.AutoAim; import java.util.function.Supplier; import java.util.stream.Stream; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Superstructure { @@ -49,6 +50,7 @@ public enum SuperState { ShoulderState.PRE_INTAKE_CORAL_GROUND, WristState.PRE_INTAKE_CORAL_GROUND, 0.0), + CHECK_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), // TODO make manipulator stuff less ugly PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), @@ -209,6 +211,8 @@ private SuperState( public static boolean antiJamCoral; + @AutoLogOutput public static boolean coralIndexed; + /** Creates a new Superstructure. */ public Superstructure( ElevatorSubsystem elevator, @@ -302,8 +306,30 @@ private void addTransitions() { // ---Funnel--- bindTransition( SuperState.IDLE, + SuperState.CHECK_CORAL, + new Trigger(manipulator::getSecondBeambreak).debounce(0.1)); + + new Trigger(() -> state == SuperState.CHECK_CORAL) + .onTrue( + Commands.runOnce(() -> coralIndexed = false) + .andThen( + manipulator + .setRollerVelocity(2) + .until( + () -> + manipulator.getSecondBeambreak() + && !manipulator.getFirstBeambreak()) + .andThen(manipulator.setRollerVelocity(-1.5)) + .until(new Trigger(manipulator::bothBeambreaks).debounce(0.2)) + .andThen(Commands.runOnce(() -> coralIndexed = true)))); + + // first beambreak is the one closest to the outside when scoring l2-4 + // second beambreak is the one closer to the funnel when scoring l2-4 + bindTransition( + SuperState.CHECK_CORAL, SuperState.READY_CORAL, - new Trigger(manipulator::eitherBeambreak).debounce(0.5)); + // new Trigger(manipulator::bothBeambreaks).debounce(0.5)); + new Trigger(() -> coralIndexed)); // .and(() -> manipulator.getTimeSinceZero() < 1.0), // ---Intake coral ground--- diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a4b01845..80bb712b 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -50,7 +50,7 @@ public enum WristState { PRE_BARGE(100), SCORE_BARGE(110), PROCESSOR(-30.0), - HOME(Units.radiansToDegrees(-0.687 - 2.0)) // i dunno + HOME(Units.radiansToDegrees(-0.687 - 6.0)) // i dunno ; private final Supplier angle; From 64c6fe0b9f0c7866bb01e713ba3862d6bd55a489 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 9 Sep 2025 17:48:26 -0700 Subject: [PATCH 128/154] Tune algae intake (barge seems to be working fine ig) --- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 58d31b29..247d7bc4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -209,7 +209,6 @@ public static enum AlgaeScoreTarget { .rightTrigger() .negate() .and(() -> DriverStation.isTeleop()) - .and(() -> Superstructure.stateIsScoreCoral(state.get()) || state.get() == SuperState.L1) .or(() -> Autos.autoScore && DriverStation.isAutonomous()); @AutoLogOutput(key = "Superstructure/Algae Intake Request") diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index a23679ce..d4dff7dd 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -28,7 +28,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 10.0; + public static final double ALGAE_CURRENT_THRESHOLD = 15.0; public static final double CORAL_HOLD_POS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3c687709..fb31953c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -704,7 +704,7 @@ private void addTransitions() { .getTranslation() .minus(swerve.getPose().getTranslation()) .getNorm() - > 0.3)); + > 0.4)); // ---Intake Stack Algae--- bindTransition( From 219c87f50f951b20cc598da067093cbe656802c5 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 9 Sep 2025 18:50:48 -0700 Subject: [PATCH 129/154] Tune processor --- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- .../frc/robot/subsystems/climber/ClimberSubsystem.java | 10 ++++++++-- 4 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 247d7bc4..47d192d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -668,8 +668,8 @@ public Robot() { : 0.0)))); // at what point do ternary operators do more harm than good climber.setDefaultCommand( climber.setPosition( - superstructure.getState().climberPosition, - superstructure.getState().climberSpeed)); // why does it need to be slow + () -> superstructure.getState().climberPosition, + () -> superstructure.getState().climberSpeed)); // why does it need to be slow leds.setDefaultCommand( Commands.either( leds.setBlinkingCmd( diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index d4dff7dd..d48ac266 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -28,7 +28,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 15.0; + public static final double ALGAE_CURRENT_THRESHOLD = 25.0; public static final double CORAL_HOLD_POS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index fb31953c..d10dd18e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -803,13 +803,13 @@ private void addTransitions() { DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? AutoAim.BLUE_PROCESSOR_POS.getX() : AutoAim.RED_PROCESSOR_POS.getX(), - 0.5) + 2) || !MathUtil.isNear( swerve.getPose().getY(), DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? AutoAim.BLUE_PROCESSOR_POS.getY() : AutoAim.RED_PROCESSOR_POS.getY(), - 0.5))); + 2))); // bindTransition( // SuperState.IDLE, diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index ad0acf87..69a4ffab 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { @@ -38,8 +39,13 @@ public void periodic() { Logger.processInputs("Climber", inputs); } - public Command setPosition(double position, double vel) { - return this.run(() -> io.setPosition(position, vel)); + public Command setPosition(DoubleSupplier position, DoubleSupplier vel) { + return this.run( + () -> { + Logger.recordOutput("Climber/Setpoint/Pos", position); + Logger.recordOutput("Climber/Setpoint/Velocity", vel); + io.setPosition(position.getAsDouble(), vel.getAsDouble()); + }); } public Command resetClimber() { From 00f03e60d1d3a27d7c45a2ccad6fe6709019f6cf Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 9 Sep 2025 18:53:12 -0700 Subject: [PATCH 130/154] Tune climb --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d10dd18e..7e03f1eb 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -136,14 +136,14 @@ public enum SuperState { ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 10.0, - 3.4, + 3.0, 2.0), CLIMB( ElevatorState.INTAKE_ALGAE_GROUND, ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 0.0, - 1.35, + 1.0, 0.5), // lowkey why is this so slow HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), From 5c713b64d351d9601f9b4687d5a8557fc8a0fd07 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 9 Sep 2025 22:57:15 -0700 Subject: [PATCH 131/154] This should prevent the robot from going from READY_ALGAE to PROCESSOR, and then directly to IDLE with algae still inside (NOT TESTED) --- .../java/frc/robot/subsystems/Superstructure.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 7e03f1eb..a8e6ac13 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -790,7 +790,20 @@ private void addTransitions() { SuperState.READY_ALGAE, SuperState.PROCESSOR, new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.PROCESSOR) - .and(Robot.preScoreReq)); + .and(Robot.preScoreReq) + .and(() -> + MathUtil.isNear( + swerve.getPose().getX(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getX() + : AutoAim.RED_PROCESSOR_POS.getX(), + 2) + || MathUtil.isNear( + swerve.getPose().getY(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getY() + : AutoAim.RED_PROCESSOR_POS.getY(), + 2))); // TODO manipulator voltage gets set elsewhere i guess bindTransition( From d9aa8b0b99b2a400aad585e0ad6f8c3712e113c6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 09:54:42 -0700 Subject: [PATCH 132/154] [not tested irl] fix auto in sim apart from that weird l2 thing --- src/main/java/frc/robot/Autos.java | 10 ++++- .../frc/robot/subsystems/Superstructure.java | 43 +++++++++---------- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 63d83d1a..3d337e0a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -164,6 +164,14 @@ public Command LOtoJ() { // run first path .active() .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) + .onTrue( + Commands.runOnce( + () -> { + if (Robot.isSimulation()) { + // manipulator.setSimFirstBeambreak(true); + manipulator.setSimSecondBeambreak(true); + } + })) .whileTrue(Commands.sequence(steps.get("LOtoJ").resetOdometry(), steps.get("LOtoJ").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -597,7 +605,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } public void bindCoralElevatorExtension(AutoRoutine routine) { - bindCoralElevatorExtension(routine, 3.75); // TODO tune + bindCoralElevatorExtension(routine, 3.25); // TODO tune } public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMeters) { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a8e6ac13..2805bf1d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -70,10 +70,10 @@ public enum SuperState { // PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), // PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), // YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), - PRE_PRE_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), - PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 0.0), + PRE_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), - POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), + // POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), + POST_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), // POST_POST_L4( // ElevatorState.HP, ShoulderState.PRE_L4, WristState.HP, 0.0), // like do we see the vision @@ -329,7 +329,7 @@ private void addTransitions() { SuperState.CHECK_CORAL, SuperState.READY_CORAL, // new Trigger(manipulator::bothBeambreaks).debounce(0.5)); - new Trigger(() -> coralIndexed)); + new Trigger(() -> coralIndexed).or(() -> Robot.isSimulation())); // .and(() -> manipulator.getTimeSinceZero() < 1.0), // ---Intake coral ground--- @@ -518,7 +518,6 @@ private void addTransitions() { .getDistance(swerve.getPose().getTranslation()) > 0.3) .debounce(0.15)); - // go straight to intaking algae from reef bindTransition( SuperState.POST_L2, @@ -576,17 +575,15 @@ private void addTransitions() { // ---L4--- bindTransition( SuperState.READY_CORAL, - SuperState.PRE_PRE_L4, + SuperState.PRE_L4, new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); - bindTransition(SuperState.PRE_PRE_L4, SuperState.PRE_L4, new Trigger(this::atExtension)); - bindTransition(SuperState.PRE_L4, SuperState.L4, new Trigger(this::atExtension)); // TODO i don't think any of the cancels work // cancel bindTransition( - SuperState.PRE_PRE_L4, + SuperState.PRE_L4, SuperState.IDLE, new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); @@ -791,19 +788,20 @@ private void addTransitions() { SuperState.PROCESSOR, new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.PROCESSOR) .and(Robot.preScoreReq) - .and(() -> - MathUtil.isNear( - swerve.getPose().getX(), - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_PROCESSOR_POS.getX() - : AutoAim.RED_PROCESSOR_POS.getX(), - 2) - || MathUtil.isNear( - swerve.getPose().getY(), - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue - ? AutoAim.BLUE_PROCESSOR_POS.getY() - : AutoAim.RED_PROCESSOR_POS.getY(), - 2))); + .and( + () -> + MathUtil.isNear( + swerve.getPose().getX(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getX() + : AutoAim.RED_PROCESSOR_POS.getX(), + 2) + || MathUtil.isNear( + swerve.getPose().getY(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getY() + : AutoAim.RED_PROCESSOR_POS.getY(), + 2))); // TODO manipulator voltage gets set elsewhere i guess bindTransition( @@ -940,7 +938,6 @@ public boolean stateIsCoralAlike() { || state == SuperState.PRE_L1 || state == SuperState.PRE_L2 || state == SuperState.PRE_L3 - || state == SuperState.PRE_PRE_L4 || state == SuperState.PRE_L4 || state == SuperState.L1 || state == SuperState.L2 From 4e9cb7694ee5991e43816a4a0fc08cf99a1548cb Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 10:03:49 -0700 Subject: [PATCH 133/154] [not tested irl] tune extension tolerances --- src/main/java/frc/robot/Autos.java | 10 +--------- src/main/java/frc/robot/Robot.java | 4 ++++ src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 3d337e0a..3fa64bf2 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -164,14 +164,6 @@ public Command LOtoJ() { // run first path .active() .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) - .onTrue( - Commands.runOnce( - () -> { - if (Robot.isSimulation()) { - // manipulator.setSimFirstBeambreak(true); - manipulator.setSimSecondBeambreak(true); - } - })) .whileTrue(Commands.sequence(steps.get("LOtoJ").resetOdometry(), steps.get("LOtoJ").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -605,7 +597,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } public void bindCoralElevatorExtension(AutoRoutine routine) { - bindCoralElevatorExtension(routine, 3.25); // TODO tune + bindCoralElevatorExtension(routine, 4); // TODO tune } public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMeters) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47d192d6..4733c0cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -562,6 +562,10 @@ public Robot() { new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoGroundCoralIntake = false)); + new Trigger(Robot::isSimulation) + .and(() -> DriverStation.isAutonomousEnabled()) + .onTrue(Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true))); + // Zero elevator/wrist at the start of auto new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) .onTrue( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2805bf1d..50812acd 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -596,7 +596,7 @@ private void addTransitions() { () -> L1Targets.getNearestLine(swerve.getPose()) .getDistance(swerve.getPose().getTranslation()) - > 0.3) + > 0.2) // TODO tune .debounce(0.15) // .and(Robot.scoreReq.negate()) ); From d10f2a98049a0da91e71852cf50541a857b90638 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 12:14:42 -0700 Subject: [PATCH 134/154] tune idle pose --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 244c9788..3f6bc877 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -37,7 +37,7 @@ public class ShoulderSubsystem extends SubsystemBase { .withMotionMagicAcceleration(4.0); public enum ShoulderState { - HP(50.0), + HP(55.0), PRE_INTAKE_CORAL_GROUND(15.0), INTAKE_CORAL_GROUND(0.0), PRE_L1(35.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 80bb712b..cf73e890 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -34,7 +34,7 @@ public class WristSubsystem extends SubsystemBase { public enum WristState { PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(0.0), - HP(160.0), + HP(170.0), L1(Units.radiansToDegrees(0.349 - 0.1)), PRE_L2(170.0), L2(Units.radiansToDegrees(2.447)), From e50a063c5e3ca627071446fc7d34fc64f603bdb3 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 13:25:58 -0700 Subject: [PATCH 135/154] tune scoring and such --- .../frc/robot/subsystems/Superstructure.java | 28 ++++++++++++------- .../shoulder/ShoulderSubsystem.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 1 + 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 50812acd..a882f102 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -47,8 +47,8 @@ public enum SuperState { -5.0), POST_INTAKE_CORAL_GROUND( ElevatorState.INTAKE_CORAL_GROUND, - ShoulderState.PRE_INTAKE_CORAL_GROUND, - WristState.PRE_INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.POST_INTAKE_CORAL_GROUND, 0.0), CHECK_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), @@ -57,13 +57,13 @@ public enum SuperState { L1(ElevatorState.L1, ShoulderState.L1, WristState.L1, 3.0), POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), - PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.L2, 0.0), + PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), L2(ElevatorState.L2, ShoulderState.L2, WristState.L2, -15.0), - POST_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + POST_L2(ElevatorState.HP, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), - PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.L3, 0.0), + PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), - POST_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + POST_L3(ElevatorState.HP, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), // PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), // L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), @@ -340,8 +340,8 @@ private void addTransitions() { SuperState.INTAKE_CORAL_GROUND, new Trigger(this::atExtension)); - bindTransition( - SuperState.PRE_INTAKE_CORAL_GROUND, SuperState.IDLE, Robot.intakeCoralReq.negate()); + // bindTransition( + // SuperState.PRE_INTAKE_CORAL_GROUND, SuperState.IDLE, Robot.intakeCoralReq.negate()); bindTransition( SuperState.INTAKE_CORAL_GROUND, @@ -351,15 +351,23 @@ private void addTransitions() { .debounce(0.060) .and(manipulator::bothBeambreaks) .debounce(0.12)); // TODO i'm lowkey losing my shit + bindTransition( SuperState.INTAKE_CORAL_GROUND, - SuperState.PRE_INTAKE_CORAL_GROUND, + SuperState.POST_INTAKE_CORAL_GROUND, Robot.intakeCoralReq.negate().and(manipulator::neitherBeambreak)); + // has coral bindTransition( SuperState.POST_INTAKE_CORAL_GROUND, SuperState.READY_CORAL, - new Trigger(this::atExtension)); + new Trigger(this::atExtension).and(manipulator::bothBeambreaks)); + + // cancel + bindTransition( + SuperState.POST_INTAKE_CORAL_GROUND, + SuperState.IDLE, + new Trigger(this::atExtension).and(manipulator::neitherBeambreak)); bindTransition( SuperState.READY_CORAL, SuperState.IDLE, new Trigger(manipulator::neitherBeambreak)); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 3f6bc877..6187a081 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -44,7 +44,7 @@ public enum ShoulderState { L1(Units.radiansToDegrees(1.617)), // not sure about units tbh PRE_L2(35.0), L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)).getDegrees()), - PRE_L3(35.0), + PRE_L3(40.0), L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), // PRE_L4(8.0), PRE_L4(30.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index cf73e890..865fda25 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -34,6 +34,7 @@ public class WristSubsystem extends SubsystemBase { public enum WristState { PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(0.0), + POST_INTAKE_CORAL_GROUND(40), HP(170.0), L1(Units.radiansToDegrees(0.349 - 0.1)), PRE_L2(170.0), From 486715c48b252be62e065e2d47746b1fe34b6574 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 15:16:51 -0700 Subject: [PATCH 136/154] remove algae camera + increase algae current threshold --- src/main/java/frc/robot/Robot.java | 8 ++++---- .../frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 11 ++++++----- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4733c0cf..9ba85ab1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -347,10 +347,10 @@ public static enum AlgaeScoreTarget { swerveDriveSimulation.get().getModules()[3]) }, PhoenixOdometryThread.getInstance(), - swerveDriveSimulation, - ROBOT_TYPE != RobotType.SIM - ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) - : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); + swerveDriveSimulation); + // ROBOT_TYPE != RobotType.SIM + // ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) + // : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); private final ManipulatorSubsystem manipulator = new ManipulatorSubsystem( diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index d48ac266..6abb633f 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -28,7 +28,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 25.0; + public static final double ALGAE_CURRENT_THRESHOLD = 40.0; public static final double CORAL_HOLD_POS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index aeda0116..6772b2fb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -62,7 +62,7 @@ public class SwerveSubsystem extends SubsystemBase { private final Module[] modules; // FL, FR, BL, BR private final OdometryThreadIO odoThread; private final OdometryThreadIOInputs odoThreadInputs = new OdometryThreadIOInputs(); - private final Camera algaeCamera; + // private final Camera algaeCamera; private SwerveDriveKinematics kinematics; @@ -100,8 +100,9 @@ public SwerveSubsystem( CameraIO[] CameraIOs, ModuleIO[] moduleIOs, OdometryThreadIO odoThread, - Optional simulation, - CameraIO algaeCameraIO) { + Optional simulation + // CameraIO algaeCameraIO) { + ) { this.constants = constants; this.kinematics = new SwerveDriveKinematics(constants.getModuleTranslations()); estimator = @@ -115,7 +116,7 @@ public SwerveSubsystem( this.gyroIO = gyroIO; this.odoThread = odoThread; this.simulation = simulation; - this.algaeCamera = new Camera(algaeCameraIO); + // this.algaeCamera = new Camera(algaeCameraIO); cameras = new Camera[CameraIOs.length]; modules = new Module[moduleIOs.length]; @@ -145,7 +146,7 @@ public void periodic() { Tracer.trace("Update cam inputs", camera::updateInputs); Tracer.trace("Process cam inputs", camera::processApriltagInputs); } - Tracer.trace("Update algae cam inputs", algaeCamera::updateInputs); + // Tracer.trace("Update algae cam inputs", algaeCamera::updateInputs); // Tracer.trace("Process algae cam inputs", algaeCamera::processAlgaeInputs); Tracer.trace( "Update odo inputs", From 4b794c120a4bd3de0b94c3972cd13e9a7cce5db6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 15:46:41 -0700 Subject: [PATCH 137/154] scores the first algae now yay --- src/main/deploy/choreo/GHtoNI.traj | 87 +++++++++++-------- src/main/java/frc/robot/Autos.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 5 +- 3 files changed, 57 insertions(+), 37 deletions(-) diff --git a/src/main/deploy/choreo/GHtoNI.traj b/src/main/deploy/choreo/GHtoNI.traj index 712dee5a..62989b5a 100644 --- a/src/main/deploy/choreo/GHtoNI.traj +++ b/src/main/deploy/choreo/GHtoNI.traj @@ -3,22 +3,26 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.510753631591797, "y":4.746592998504639, "heading":-2.866875372751583, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.11117696762085, "y":5.479686737060547, "heading":3.490658503988659, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.75}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.510753631591797 m", "val":6.510753631591797}, "y":{"exp":"4.746592998504639 m", "val":4.746592998504639}, "heading":{"exp":"-2.866875372751583 rad", "val":-2.866875372751583}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.11117696762085 m", "val":7.11117696762085}, "y":{"exp":"5.479686737060547 m", "val":5.479686737060547}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.75 m / s", "val":1.75}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -26,38 +30,51 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.10984], + "waypoints":[0.0,0.61325,1.29213], "samples":[ - {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24646, "ay":4.75959, "alpha":1.12862, "fx":[75.89529,69.82763,62.76526,69.19804], "fy":[71.78112,77.70855,83.50697,78.24401]}, - {"t":0.03827, "x":5.80943, "y":4.02065, "heading":3.14159, "vx":0.16251, "vy":0.18215, "omega":0.04319, "ax":4.24611, "ay":4.75919, "alpha":1.12834, "fx":[75.88743,69.82221,62.76182,69.19184], "fy":[71.77674,77.70264,83.49878,78.23681]}, - {"t":0.07654, "x":5.81876, "y":4.03111, "heading":-3.13994, "vx":0.32501, "vy":0.36429, "omega":0.08637, "ax":4.2457, "ay":4.75873, "alpha":1.12813, "fx":[75.87833,69.80587,62.75579,69.19654], "fy":[71.7716,77.70474,83.4907,78.21785]}, - {"t":0.11481, "x":5.83431, "y":4.04854, "heading":-3.13663, "vx":0.4875, "vy":0.54641, "omega":0.12955, "ax":4.24522, "ay":4.75819, "alpha":1.12798, "fx":[75.86762,69.77837,62.74702,69.21184], "fy":[71.76549,77.71457,83.48238,78.18682]}, - {"t":0.15308, "x":5.85607, "y":4.07293, "heading":-3.13168, "vx":0.64996, "vy":0.7285, "omega":0.17272, "ax":4.24464, "ay":4.75753, "alpha":1.1279, "fx":[75.8548,69.73939,62.73532,69.23732], "fy":[71.75817,77.73173,83.47325,78.14326]}, - {"t":0.19135, "x":5.88405, "y":4.1043, "heading":-3.12507, "vx":0.81241, "vy":0.91058, "omega":0.21588, "ax":4.24392, "ay":4.75673, "alpha":1.12787, "fx":[75.8391,69.68842,62.72042,69.27237], "fy":[71.74929,77.75565,83.46251,78.08652]}, - {"t":0.22962, "x":5.91825, "y":4.14263, "heading":-3.1168, "vx":0.97482, "vy":1.09262, "omega":0.25905, "ax":4.24303, "ay":4.75573, "alpha":1.12789, "fx":[75.81942,69.62474,62.70186,69.31609], "fy":[71.73828,77.78548,83.44901,78.01561]}, - {"t":0.26789, "x":5.95867, "y":4.18792, "heading":-3.10689, "vx":1.13721, "vy":1.27462, "omega":0.30221, "ax":4.24189, "ay":4.75444, "alpha":1.12796, "fx":[75.79411,69.54718,62.67887,69.36705], "fy":[71.72418,77.81988,83.43096,77.92899]}, - {"t":0.30616, "x":6.0053, "y":4.24019, "heading":-3.09532, "vx":1.29955, "vy":1.45658, "omega":0.34538, "ax":4.24036, "ay":4.75272, "alpha":1.12806, "fx":[75.76056,69.45384,62.64999,69.42291], "fy":[71.70525,77.85665,83.40552,77.82412]}, - {"t":0.34443, "x":6.05813, "y":4.29941, "heading":-3.08211, "vx":1.46183, "vy":1.63846, "omega":0.38855, "ax":4.23822, "ay":4.75031, "alpha":1.12817, "fx":[75.71427,69.34129,62.61234,69.47946], "fy":[71.67811,77.89181,83.36778,77.69645]}, - {"t":0.3827, "x":6.11718, "y":4.36559, "heading":-3.06724, "vx":1.62402, "vy":1.82026, "omega":0.43173, "ax":4.23501, "ay":4.7467, "alpha":1.12829, "fx":[75.64662,69.20267,62.55977,69.5283], "fy":[71.63564,77.91749,83.30822,77.53691]}, - {"t":0.42097, "x":6.18244, "y":4.43873, "heading":-3.05071, "vx":1.7861, "vy":2.00192, "omega":0.47491, "ax":4.22966, "ay":4.7407, "alpha":1.12838, "fx":[75.53819,69.02204,62.47716,69.55005], "fy":[71.56056,77.91536,83.20524,77.32434]}, - {"t":0.45924, "x":6.25389, "y":4.51882, "heading":-3.03254, "vx":1.94797, "vy":2.18335, "omega":0.51809, "ax":4.21897, "ay":4.72871, "alpha":1.12843, "fx":[75.33208,68.7516,62.3178,69.48686], "fy":[71.39985,77.83077,82.99532,76.99544]}, - {"t":0.49751, "x":6.33153, "y":4.60584, "heading":-3.01271, "vx":2.10943, "vy":2.36432, "omega":0.56127, "ax":4.18703, "ay":4.6929, "alpha":1.12875, "fx":[74.74931,68.15337,61.84491,69.05245], "fy":[70.88599,77.39257,82.36628,76.23525]}, - {"t":0.53579, "x":6.41532, "y":4.69976, "heading":-2.99123, "vx":2.26967, "vy":2.54391, "omega":0.60447, "ax":-0.00061, "ay":-0.00125, "alpha":0.15067, "fx":[0.45973,0.33382,-0.47976,-0.35385], "fy":[-0.37433,0.45675,0.33349,-0.49758]}, - {"t":0.57406, "x":6.50218, "y":4.79711, "heading":-2.9681, "vx":2.26965, "vy":2.54387, "omega":0.61024, "ax":-4.18707, "ay":-4.69297, "alpha":-1.12637, "fx":[-74.68492,-67.88782,-61.87018,-69.35932], "fy":[-70.96195,-77.62436,-82.33994,-75.95815]}, - {"t":0.61233, "x":6.58598, "y":4.89103, "heading":-2.94475, "vx":2.10941, "vy":2.36426, "omega":0.56713, "ax":-4.21897, "ay":-4.72871, "alpha":-1.13065, "fx":[-75.23551,-68.22235,-62.33018,-70.10043], "fy":[-71.50972,-78.29429,-82.97869,-76.43858]}, - {"t":0.6506, "x":6.66362, "y":4.97805, "heading":-2.92304, "vx":1.94795, "vy":2.1833, "omega":0.52386, "ax":-4.22964, "ay":-4.74066, "alpha":-1.13348, "fx":[-75.40372,-68.24941,-62.48719,-70.44614], "fy":[-71.71015,-78.59231,-83.19058,-76.51005]}, - {"t":0.68887, "x":6.73507, "y":5.05813, "heading":-2.90299, "vx":1.78608, "vy":2.00187, "omega":0.48048, "ax":-4.23498, "ay":-4.74664, "alpha":-1.13569, "fx":[-75.47469,-68.20697,-62.57093,-70.68298], "fy":[-71.82441,-78.78989,-83.29295,-76.48691]}, - {"t":0.72714, "x":6.80032, "y":5.13127, "heading":-2.8846, "vx":1.624, "vy":1.82021, "omega":0.43702, "ax":-4.23818, "ay":-4.75023, "alpha":-1.13749, "fx":[-75.50666,-68.14288,-62.6266,-70.86874], "fy":[-71.90412,-78.94171,-83.35043,-76.43247]}, - {"t":0.76541, "x":6.85937, "y":5.19745, "heading":-2.86788, "vx":1.4618, "vy":1.63842, "omega":0.39349, "ax":-4.24031, "ay":-4.75262, "alpha":-1.139, "fx":[-75.51961,-68.07312,-62.66868,-71.02287], "fy":[-71.96602,-79.06608,-83.3851,-76.36785]}, - {"t":0.80368, "x":6.9122, "y":5.25667, "heading":-2.85282, "vx":1.29953, "vy":1.45654, "omega":0.3499, "ax":-4.24184, "ay":-4.75433, "alpha":-1.14027, "fx":[-75.52242,-68.00454,-62.70299,-71.15378], "fy":[-72.01695,-79.1709,-83.40673,-76.30211]}, - {"t":0.84195, "x":6.95883, "y":5.30893, "heading":-2.83943, "vx":1.13719, "vy":1.27459, "omega":0.30626, "ax":-4.24297, "ay":-4.75561, "alpha":-1.14134, "fx":[-75.51975,-67.94054,-62.73215,-71.26579], "fy":[-72.0601,-79.26017,-83.42041,-76.23972]}, - {"t":0.88022, "x":6.99925, "y":5.35423, "heading":-2.82771, "vx":0.97481, "vy":1.09259, "omega":0.26258, "ax":-4.24386, "ay":-4.7566, "alpha":-1.14222, "fx":[-75.51438,-67.883,-62.75739,-71.36135], "fy":[-72.09708,-79.33616,-83.42916,-76.18314]}, - {"t":0.91849, "x":7.03344, "y":5.39256, "heading":-2.81766, "vx":0.81239, "vy":0.91055, "omega":0.21887, "ax":-4.24457, "ay":-4.7574, "alpha":-1.14294, "fx":[-75.50807,-67.83302,-62.77934,-71.442], "fy":[-72.12874,-79.40025,-83.43487,-76.13382]}, - {"t":0.95676, "x":7.06143, "y":5.42392, "heading":-2.80928, "vx":0.64995, "vy":0.72848, "omega":0.17513, "ax":-4.24515, "ay":-4.75805, "alpha":-1.14351, "fx":[-75.50201,-67.7913,-62.79825,-71.50875], "fy":[-72.15554,-79.45334,-83.43884,-76.09266]}, - {"t":0.99503, "x":7.08319, "y":5.44832, "heading":-2.80258, "vx":0.48749, "vy":0.54639, "omega":0.13136, "ax":-4.24563, "ay":-4.7586, "alpha":-1.14393, "fx":[-75.49706,-67.75828,-62.81426,-71.56232], "fy":[-72.17774,-79.49604,-83.44199,-76.06023]}, - {"t":1.0333, "x":7.09874, "y":5.46575, "heading":-2.79755, "vx":0.32501, "vy":0.36428, "omega":0.08758, "ax":-4.24604, "ay":-4.75906, "alpha":-1.14421, "fx":[-75.49383,-67.73429,-62.8274,-71.60321], "fy":[-72.1955,-79.5288,-83.44499,-76.03693]}, - {"t":1.07157, "x":7.10807, "y":5.4762, "heading":-2.7942, "vx":0.16251, "vy":0.18215, "omega":0.04379, "ax":-4.24639, "ay":-4.75946, "alpha":-1.14436, "fx":[-75.49277,-67.71952,-62.83769,-71.6318], "fy":[-72.20892,-79.55194,-83.4483,-76.02301]}, - {"t":1.10984, "x":7.11118, "y":5.47969, "heading":-2.79253, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.44832, "ay":4.57253, "alpha":0.95227, "fx":[77.95667,72.83283,67.31326,72.78319], "fy":[69.4746,74.84295,79.83107,74.86029]}, + {"t":0.02787, "x":5.80805, "y":4.01894, "heading":3.14159, "vx":0.124, "vy":0.12746, "omega":0.02654, "ax":4.44728, "ay":4.57223, "alpha":0.9688, "fx":[78.02899,72.8203,67.20117,72.76777], "fy":[69.37719,74.84137,79.91147,74.85878]}, + {"t":0.05575, "x":5.81323, "y":4.02427, "heading":-3.14085, "vx":0.24796, "vy":0.25491, "omega":0.05355, "ax":4.44609, "ay":4.57187, "alpha":0.98784, "fx":[78.11188,72.80212,67.07171,72.75442], "fy":[69.26525,74.84327,80.00411,74.85275]}, + {"t":0.08362, "x":5.82187, "y":4.03316, "heading":-3.13936, "vx":0.3719, "vy":0.38235, "omega":0.08109, "ax":4.44469, "ay":4.57145, "alpha":1.00999, "fx":[78.20787,72.77755,66.92067,72.74296], "fy":[69.13523,74.84889,80.11187,74.84173]}, + {"t":0.1115, "x":5.83396, "y":4.04559, "heading":-3.1371, "vx":0.49579, "vy":0.50978, "omega":0.10924, "ax":4.44305, "ay":4.57094, "alpha":1.03608, "fx":[78.32035,72.74559,66.74241,72.73312], "fy":[68.98235,74.85854,80.23858,74.82514]}, + {"t":0.13937, "x":5.84951, "y":4.06158, "heading":-3.13405, "vx":0.61964, "vy":0.63719, "omega":0.13812, "ax":4.44108, "ay":4.57032, "alpha":1.06723, "fx":[78.45397,72.70489,66.52909,72.72455], "fy":[68.79997,74.87266,80.38952,74.80214]}, + {"t":0.16725, "x":5.86851, "y":4.08111, "heading":-3.1302, "vx":0.74344, "vy":0.76459, "omega":0.16787, "ax":4.43867, "ay":4.56956, "alpha":1.10505, "fx":[78.61533,72.65354,66.26948,72.71671], "fy":[68.57862,74.89183,80.57216,74.77155]}, + {"t":0.19512, "x":5.89096, "y":4.1042, "heading":-3.12553, "vx":0.86716, "vy":0.89197, "omega":0.19867, "ax":4.43566, "ay":4.56858, "alpha":1.1519, "fx":[78.81407,72.58874,65.94698,72.70877], "fy":[68.30432,74.91691,80.79746,74.73161]}, + {"t":0.223, "x":5.91685, "y":4.13084, "heading":-3.11999, "vx":0.99081, "vy":1.01931, "omega":0.23078, "ax":4.43181, "ay":4.56729, "alpha":1.21143, "fx":[79.06493,72.50629,65.53585,72.69944], "fy":[67.95541,74.94917,81.08208,74.6796]}, + {"t":0.25087, "x":5.94619, "y":4.16103, "heading":-3.11355, "vx":1.11434, "vy":1.14663, "omega":0.26455, "ax":4.42669, "ay":4.56553, "alpha":1.28957, "fx":[79.39151,72.39942,64.99408,72.68653], "fy":[67.49664,74.99065,81.45275,74.61102]}, + {"t":0.27875, "x":5.97897, "y":4.19476, "heading":-3.10618, "vx":1.23774, "vy":1.27389, "omega":0.3005, "ax":4.41955, "ay":4.56298, "alpha":1.39659, "fx":[79.83418,72.25663,64.24796,72.66615], "fy":[66.86636,75.04475,81.95512,74.51791]}, + {"t":0.30662, "x":6.01519, "y":4.23204, "heading":-3.0978, "vx":1.36093, "vy":1.40108, "omega":0.33943, "ax":4.40893, "ay":4.55898, "alpha":1.55205, "fx":[80.46809,72.05659,63.15558,72.63049], "fy":[65.9464,75.11775,82.67404,74.38454]}, + {"t":0.3345, "x":6.05484, "y":4.27287, "heading":-3.08834, "vx":1.48383, "vy":1.52816, "omega":0.38269, "ax":4.39151, "ay":4.55194, "alpha":1.79828, "fx":[81.45044,71.75459,61.40453,72.56169], "fy":[64.47817,75.22259,83.78636,74.17505]}, + {"t":0.36237, "x":6.09791, "y":4.31724, "heading":-3.07768, "vx":1.60624, "vy":1.65505, "omega":0.43282, "ax":4.35782, "ay":4.53682, "alpha":2.24681, "fx":[83.17221,71.23859,58.14952,72.40767], "fy":[61.76829,75.39112,85.72852,73.78542]}, + {"t":0.39025, "x":6.14438, "y":4.36513, "heading":-3.06561, "vx":1.72772, "vy":1.78151, "omega":0.49545, "ax":4.26733, "ay":4.48829, "alpha":3.31397, "fx":[86.91845,70.1254,50.08083,71.92635], "fy":[55.14178,75.7286,89.89445,72.73533]}, + {"t":0.41812, "x":6.19419, "y":4.41654, "heading":-3.0518, "vx":1.84667, "vy":1.90662, "omega":0.58782, "ax":3.5218, "ay":3.79584, "alpha":9.2402, "fx":[97.98295,65.68955,5.62091,61.00554], "fy":[19.10819,76.90486,99.58998,52.61629]}, + {"t":0.446, "x":6.24704, "y":4.47116, "heading":-3.03541, "vx":1.94484, "vy":2.01243, "omega":0.84539, "ax":-4.28525, "ay":-4.09291, "alpha":5.1111, "fx":[-40.1275,-76.56844,-93.01057,-70.5161], "fy":[-92.57695,-61.78055,-39.80783,-73.48005]}, + {"t":0.47387, "x":6.29959, "y":4.52566, "heading":-3.01185, "vx":1.82539, "vy":1.89834, "omega":0.98786, "ax":-4.44002, "ay":-4.44724, "alpha":1.60702, "fx":[-63.54553,-74.06505,-80.88251,-71.85008], "fy":[-81.18911,-71.45504,-64.00033,-74.17105]}, + {"t":0.50175, "x":6.34874, "y":4.57685, "heading":-2.98431, "vx":1.70162, "vy":1.77438, "omega":1.03266, "ax":-4.45644, "ay":-4.50478, "alpha":0.7283, "fx":[-68.82484,-73.54803,-76.7209,-72.3232], "fy":[-77.51886,-72.98853,-69.73363,-74.33747]}, + {"t":0.52962, "x":6.39444, "y":4.62456, "heading":-2.95553, "vx":1.5774, "vy":1.64881, "omega":1.05296, "ax":-4.46075, "ay":-4.52728, "alpha":0.33202, "fx":[-71.10908,-73.27447,-74.70449,-72.61082], "fy":[-75.77696,-73.66581,-72.24272,-74.36436]}, + {"t":0.5575, "x":6.43668, "y":4.66876, "heading":-2.92618, "vx":1.45306, "vy":1.52261, "omega":1.06221, "ax":-4.46234, "ay":-4.53914, "alpha":0.10662, "fx":[-72.37384,-73.07637,-73.52385,-72.82911], "fy":[-74.77036,-74.07938,-73.64206,-74.33357]}, + {"t":0.58537, "x":6.47545, "y":4.70944, "heading":-2.89657, "vx":1.32867, "vy":1.39608, "omega":1.06519, "ax":-4.46304, "ay":-4.54644, "alpha":-0.03892, "fx":[-73.17067,-72.91116,-72.75326,-73.01386], "fy":[-74.12107,-74.37762,-74.53006,-74.2735]}, + {"t":0.61325, "x":6.51075, "y":4.74659, "heading":-2.86688, "vx":1.20426, "vy":1.26935, "omega":1.0641, "ax":-2.44055, "ay":2.1635, "alpha":-5.42363, "fx":[-54.11864,-55.62183,-23.06998,-26.78293], "fy":[41.85674,13.41314,29.65636,56.55029]}, + {"t":0.64719, "x":6.55023, "y":4.79093, "heading":-2.83076, "vx":1.12142, "vy":1.34279, "omega":0.88, "ax":-0.44111, "ay":0.36427, "alpha":-7.76268, "fx":[-33.16555,-21.69825,20.24332,5.77518], "fy":[19.72552,-21.05323,-8.19369,33.34159]}, + {"t":0.68114, "x":6.58804, "y":4.83672, "heading":-2.80088, "vx":1.10645, "vy":1.35515, "omega":0.61651, "ax":-0.06208, "ay":0.0506, "alpha":-6.79631, "fx":[-24.75657,-12.25733,22.89195,10.06263], "fy":[12.47341,-23.22638,-10.853,24.91507]}, + {"t":0.71508, "x":6.62556, "y":4.88274, "heading":-2.77996, "vx":1.10434, "vy":1.35687, "omega":0.38581, "ax":-0.00836, "ay":0.0068, "alpha":-5.70586, "fx":[-20.33251,-9.08634,20.07509,8.79739], "fy":[9.48355,-20.28626,-9.26386,20.51116]}, + {"t":0.74902, "x":6.66304, "y":4.92881, "heading":-2.76686, "vx":1.10406, "vy":1.3571, "omega":0.19213, "ax":-0.00109, "ay":0.00089, "alpha":-4.60515, "fx":[-16.42116,-7.01913,16.38688,6.98216], "fy":[7.36569,-16.54212,-7.33689,16.57128]}, + {"t":0.78297, "x":6.70051, "y":4.97487, "heading":-2.76034, "vx":1.10402, "vy":1.35713, "omega":0.03582, "ax":-0.00014, "ay":0.00011, "alpha":-3.49927, "fx":[-12.50314,-5.23966,12.49872,5.23504], "fy":[5.50638,-12.61336,-5.50271,12.61705]}, + {"t":0.81691, "x":6.73799, "y":5.02094, "heading":-2.75912, "vx":1.10402, "vy":1.35713, "omega":-0.08296, "ax":-0.00002, "ay":0.00001, "alpha":-2.39061, "fx":[-8.54515,-3.56782,8.54459,3.56726], "fy":[3.7504,-8.62249,-3.74994,8.62295]}, + {"t":0.85085, "x":6.77546, "y":5.067, "heading":-2.76194, "vx":1.10401, "vy":1.35713, "omega":-0.16411, "ax":0.0, "ay":0.0, "alpha":-1.28068, "fx":[-4.57197,-1.92422,4.5719,1.92415], "fy":[2.02193,-4.61388,-2.02188,4.61394]}, + {"t":0.8848, "x":6.81294, "y":5.11307, "heading":-2.76751, "vx":1.10401, "vy":1.35713, "omega":-0.20758, "ax":0.0, "ay":0.0, "alpha":-0.17031, "fx":[-0.6065,-0.25931,0.6065,0.2593], "fy":[0.27227,-0.61215,-0.27227,0.61216]}, + {"t":0.91874, "x":6.85041, "y":5.15914, "heading":-2.77456, "vx":1.10401, "vy":1.35713, "omega":-0.21336, "ax":0.0, "ay":0.0, "alpha":0.9401, "fx":[3.33711,1.45511,-3.33707,-1.45506], "fy":[-1.52644,3.36879,1.52641,-3.36882]}, + {"t":0.95269, "x":6.88789, "y":5.2052, "heading":-2.7818, "vx":1.10401, "vy":1.35713, "omega":-0.18145, "ax":-0.00001, "ay":-0.00003, "alpha":2.05022, "fx":[7.25331,3.22637,-7.25348,-3.22654], "fy":[-3.38201,7.32322,3.38105,-7.32417]}, + {"t":0.98663, "x":6.92536, "y":5.25127, "heading":-2.78796, "vx":1.10401, "vy":1.35713, "omega":-0.11186, "ax":-0.33801, "ay":-0.41547, "alpha":3.11622, "fx":[5.4952,-0.60239,-16.58611,-10.40989], "fy":[-12.18154,4.3763,-1.6329,-17.73043]}, + {"t":1.02057, "x":6.96264, "y":5.2971, "heading":-2.79176, "vx":1.09254, "vy":1.34303, "omega":-0.00608, "ax":-3.99683, "ay":-4.91319, "alpha":0.04395, "fx":[-65.08604,-65.40546,-65.5944,-65.27637], "fy":[-80.52708,-80.26468,-80.11556,-80.37803]}, + {"t":1.05452, "x":6.99742, "y":5.33985, "heading":-2.79196, "vx":0.95687, "vy":1.17626, "omega":-0.00459, "ax":-4.017, "ay":-4.93799, "alpha":0.02753, "fx":[-65.51038,-65.71173,-65.83005,-65.62925], "fy":[-80.85642,-80.69188,-80.59699,-80.76157]}, + {"t":1.08846, "x":7.02759, "y":5.37694, "heading":-2.79212, "vx":0.82052, "vy":1.00864, "omega":-0.00366, "ax":-4.02364, "ay":-4.94615, "alpha":0.02213, "fx":[-65.65014,-65.81238,-65.90753,-65.74565], "fy":[-80.96468,-80.83234,-80.75564,-80.88801]}, + {"t":1.12241, "x":7.05312, "y":5.40832, "heading":-2.79224, "vx":0.68394, "vy":0.84075, "omega":-0.0029, "ax":-4.02695, "ay":-4.95021, "alpha":0.01945, "fx":[-65.71968,-65.86241,-65.94605,-65.80359], "fy":[-81.01851,-80.90217,-80.83458,-80.95094]}, + {"t":1.15635, "x":7.07402, "y":5.43401, "heading":-2.79234, "vx":0.54725, "vy":0.67272, "omega":-0.00224, "ax":-4.02892, "ay":-4.95264, "alpha":0.01785, "fx":[-65.76128,-65.89233,-65.96908,-65.83827], "fy":[-81.0507,-80.94394,-80.88182,-80.98859]}, + {"t":1.19029, "x":7.09027, "y":5.45399, "heading":-2.79242, "vx":0.41049, "vy":0.50461, "omega":-0.00164, "ax":-4.03024, "ay":-4.95426, "alpha":0.01678, "fx":[-65.78897,-65.91223,-65.98441,-65.86135], "fy":[-81.07212,-80.97174,-80.91326,-81.01365]}, + {"t":1.22424, "x":7.10189, "y":5.46827, "heading":-2.79247, "vx":0.27369, "vy":0.33644, "omega":-0.00107, "ax":-4.03118, "ay":-4.95541, "alpha":0.01602, "fx":[-65.80872,-65.92643,-65.99534,-65.87782], "fy":[-81.0874,-80.99156,-80.93568,-81.03153]}, + {"t":1.25818, "x":7.10885, "y":5.47683, "heading":-2.79251, "vx":0.13686, "vy":0.16824, "omega":-0.00052, "ax":-4.03188, "ay":-4.95628, "alpha":0.01545, "fx":[-65.82352,-65.93707,-66.00353,-65.89015], "fy":[-81.09884,-81.00641,-80.95249,-81.04493]}, + {"t":1.29213, "x":7.11118, "y":5.47969, "heading":-2.79253, "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 3fa64bf2..c4e5afda 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -340,7 +340,7 @@ public Command CMtoGH() { // algae path .whileTrue( Commands.sequence( steps.get("CMtoG").resetOdometry(), - Commands.waitSeconds(1.5), + // Commands.waitSeconds(1.5), steps.get("CMtoG").cmd())); routine diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a882f102..1f081d47 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -420,7 +420,10 @@ private void addTransitions() { () -> state == SuperState.INTAKE_ALGAE_HIGH || state == SuperState.INTAKE_ALGAE_LOW - || state == SuperState.READY_ALGAE) + || state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.READY_ALGAE + || state == SuperState.PRE_BARGE) .whileTrue(manipulator.intakeAlgae()); // The way i'm handling the manipulator rn completely undermines the states i was trying to get From 7052b0ef138988ae5b61eb92f8412f2251f240b1 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 13 Sep 2025 16:26:34 -0700 Subject: [PATCH 138/154] not have prescore on by default --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index c4e5afda..905d2c18 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -47,7 +47,7 @@ public class Autos { private final ShoulderSubsystem shoulder; private final WristSubsystem wrist; - @AutoLogOutput public static boolean autoPreScore = true; + @AutoLogOutput public static boolean autoPreScore = false; // true; @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static @AutoLogOutput public static boolean autoGroundCoralIntake = false; @AutoLogOutput public static boolean autoAlgaeIntake = false; From d2485c2419c28bb547675abe199f41657ab2a89d Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 14 Sep 2025 15:37:45 -0700 Subject: [PATCH 139/154] fixing algae auto with full routine --- src/main/java/frc/robot/Autos.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 905d2c18..8e8e9cc0 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -342,7 +342,6 @@ public Command CMtoGH() { // algae path steps.get("CMtoG").resetOdometry(), // Commands.waitSeconds(1.5), steps.get("CMtoG").cmd())); - routine .observe(steps.get("CMtoG").done()) // TODO change to time based .onTrue(Commands.sequence(scoreCoralInAuto(() -> steps.get("CMtoG").getFinalPose().get()))); @@ -365,7 +364,14 @@ public Command CMtoGH() { // algae path .onTrue( Commands.sequence( intakeAlgaeInAuto(() -> steps.get("NItoIJ").getFinalPose()), - swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2))); + swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), + steps.get("IJtoNI").cmd())); + routine.observe( + steps + .get("IJtoNI") + .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2) + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd()))); + return routine.cmd(); // routine // .observe( @@ -428,7 +434,6 @@ public Command CMtoGH() { // algae path // routine // .observe(steps.get("NItoEF").done()) // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); - return routine.cmd(); } public Command LOtoA() { // 2910 From fa5768b3a715eef0c828bad3ea6c1a0fcefe7d98 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 17 Sep 2025 19:26:58 -0700 Subject: [PATCH 140/154] loom testing until the plate popped off --- src/main/java/frc/robot/Robot.java | 16 +- .../subsystems/ManipulatorSubsystem.java | 43 ++-- .../frc/robot/subsystems/Superstructure.java | 218 ++++++++++-------- 3 files changed, 160 insertions(+), 117 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9ba85ab1..ab9e47de 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -249,8 +249,14 @@ public static enum AlgaeScoreTarget { @AutoLogOutput(key = "Superstructure/Force Funnel Req") public static Trigger forceFunnelReq = operator.leftBumper(); - @AutoLogOutput(key = "Superstructure/Force Index Req") // TODO what? - public static Trigger forceIndexReq = operator.povDown(); + // @AutoLogOutput(key = "Superstructure/Force Index Req") // TODO what? + // public static Trigger forceIndexReq = operator.povDown(); + + @AutoLogOutput(key = "Superstructure/Jog Coral Up Req") // i dont like it either + public static Trigger jogCoralUpReq = operator.povUp(); + + @AutoLogOutput(key = "Superstructure/Jog Coral Down Req") // i dont like it either + public static Trigger jogCoralDownReq = operator.povDown(); // killVisionIK, DoubleSupplier coralAdjust) // new Trigger(() -> killVisionIK) @@ -647,9 +653,9 @@ public Robot() { elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); - manipulator.setDefaultCommand( - manipulator.setStateVelocity( - () -> superstructure.atExtension() || superstructure.antiJamCoral())); + // manipulator.setDefaultCommand( + // manipulator.setStateVelocity( + // () -> superstructure.atExtension() || superstructure.antiJamCoral())); funnel.setDefaultCommand( funnel.setRollerVoltage( () -> diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 6abb633f..6c338443 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -15,7 +15,6 @@ import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; -import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -24,11 +23,11 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double MAX_VELOCITY = 20; // holy cooked - public static final double CORAL_INTAKE_VELOCITY = -18.0; + public static final double CORAL_INTAKE_VELOCITY = -10.0; public static final double JOG_POS = 0.75; - public static final double ALGAE_INTAKE_VOLTAGE = 10.0; - public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 40.0; + public static final double ALGAE_INTAKE_VOLTAGE = 8.0; + public static final double ALGAE_HOLDING_VOLTAGE = 2.0; + public static final double ALGAE_CURRENT_THRESHOLD = 30.0; public static final double CORAL_HOLD_POS = 0.6; @@ -43,9 +42,6 @@ public class ManipulatorSubsystem extends RollerSubsystem { @AutoLogOutput private boolean hasAlgaeSim = false; @AutoLogOutput public boolean hasAlgaeReal = false; - @AutoLogOutput(key = "Manipulator State Velocity") - private double stateVelocity = 0.0; - private LinearFilter currentFilter = LinearFilter.movingAverage(10); @AutoLogOutput private double currentFilterValue = 0.0; @@ -89,34 +85,39 @@ public void periodic() { } } - public void resetPosition(final double rotations) { - io.resetEncoder(rotations); - } - public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) .until( new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) .debounce(0.25)) - .andThen( - Commands.runOnce(() -> hasAlgaeReal = true) - .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE)))); + .andThen(Commands.runOnce(() -> hasAlgaeReal = true)); + } + + public Command holdAlgae() { + return this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE)); + } + + public Command holdAlgaeExtra() { + return this.run(() -> io.setVoltage(3 * ALGAE_HOLDING_VOLTAGE)); + } + + public Command scoreAlgaeProcessor() { + return this.run(() -> io.setVoltage(-2.0)); } - public Command setStateVelocity(BooleanSupplier checkExtension) { - // return Commands.waitUntil(checkExtension).andThen(setRollerVelocity(stateVelocity)); - return setRollerVelocity(stateVelocity); + public Command scoreAlgaeBarge() { + return this.run(() -> io.setVoltage(-13.0)); } - public void setState(double vel) { - stateVelocity = vel; + public Command intakeCoral() { + return setRollerVelocity(CORAL_INTAKE_VELOCITY); } public double getStatorCurrentAmps() { return currentFilterValue; } - // @AutoLogOutput(key = "Manipulator/Has Algae") + @AutoLogOutput(key = "Manipulator/Has Algae") public boolean hasAlgae() { // TODO icky // return new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) // .debounce(0.75) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 1f081d47..88ccd9cc 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -52,7 +52,6 @@ public enum SuperState { 0.0), CHECK_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), - // TODO make manipulator stuff less ugly PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), L1(ElevatorState.L1, ShoulderState.L1, WristState.L1, 3.0), POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), @@ -229,6 +228,7 @@ public Superstructure( this.swerve = swerve; addTransitions(); + addManipulatorStates(); stateTimer.start(); } @@ -301,7 +301,7 @@ private void addTransitions() { // Prob a better way to impl this // Vaughn says he wants this available anytime // TODO this will probably not still work - Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); + // Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); // ---Funnel--- bindTransition( @@ -379,75 +379,6 @@ private void addTransitions() { bindTransition(SuperState.PRE_L1, SuperState.L1, new Trigger(this::atExtension)); - // manipulator stuff because ?? - Robot.scoreReq - .and(() -> stateIsScoreCoral(state)) - // .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) - // .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); - .whileTrue(manipulator.setRollerVelocity(state.manipulatorVelocity)); - - Robot.scoreReq - .and(() -> state == SuperState.L1) - // .onTrue(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))) - // .onFalse(Commands.runOnce(() -> manipulator.setState(state.manipulatorVelocity))); - .whileTrue(manipulator.setRollerVelocity(3.0)); - - Robot.intakeCoralReq - .and(() -> !manipulator.bothBeambreaks()) - .onTrue(manipulator.setRollerVelocity(-10)) - .onFalse(manipulator.setRollerVelocity(0)); - // Commands.runOnce( - // (() -> manipulator.setState(SuperState.INTAKE_CORAL_GROUND.manipulatorVelocity)))); - // .onFalse(Commands.runOnce((() -> manipulator.setState(0)))); - - // Robot.intakeAlgaeReq - // .and(() -> !manipulator.hasAlgae()) - // // .whileTrue(manipulator.setRollerVelocity(5.0)); - // .whileTrue(manipulator.intakeAlgae()); - - // new Trigger(() -> manipulator.hasAlgae()) - // .debounce(0.75) - // .and(Robot.scoreReq) - // .negate() - // .whileTrue(manipulator.setRollerVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)); - - // Robot.intakeAlgaeReq.and(this::stateIsIntakeAlgae).whileTrue(manipulator.setRollerVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); - - // Robot.intakeAlgaeReq.negate().and(()-> - // stateTimer.hasElapsed(1.0)).whileTrue(manipulator.intakeAlgae()); - - new Trigger( - () -> - state == SuperState.INTAKE_ALGAE_HIGH - || state == SuperState.INTAKE_ALGAE_LOW - || state == SuperState.INTAKE_ALGAE_GROUND - || state == SuperState.INTAKE_ALGAE_STACK - || state == SuperState.READY_ALGAE - || state == SuperState.PRE_BARGE) - .whileTrue(manipulator.intakeAlgae()); - - // The way i'm handling the manipulator rn completely undermines the states i was trying to get - // at tbh - - new Trigger(() -> state == SuperState.READY_ALGAE) - .and(() -> manipulator.getStatorCurrentAmps() < 20.0) - .debounce(1.0) - .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); - - Robot.forceFunnelReq - .or( - new Trigger( - () -> - (Stream.of(FieldUtils.HumanPlayerTargets.values()) - .map( - (t) -> - t.location.minus(swerve.getPose()).getTranslation().getNorm()) - .min(Double::compare) - .get() - < 1.0))) - .and(manipulator::neitherBeambreak) - .whileTrue(manipulator.setRollerVelocity(-7.0)) - .whileFalse(manipulator.setRollerVelocity(0.0)); // cancel bindTransition( SuperState.PRE_L1, @@ -907,6 +838,112 @@ private void addTransitions() { bindTransition(SuperState.PRE_CLIMB, SuperState.IDLE, Robot.preClimbReq.negate()); + Robot.antiAlgaeJamReq.onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); + + bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiAlgaeJamReq.negate()); + } + + private void addManipulatorStates() { + // Score coral + Robot.scoreReq + .and(() -> stateIsScoreCoral(state)) + .whileTrue(manipulator.setRollerVelocity(() -> state.manipulatorVelocity)); + + Robot.intakeCoralReq + .and(() -> !manipulator.bothBeambreaks()) + .onTrue(manipulator.setRollerVelocity(-10)) + .onFalse(manipulator.setRollerVelocity(0)); + + new Trigger( + () -> + state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.READY_ALGAE + || state == SuperState.PRE_BARGE) + .whileTrue(manipulator.intakeAlgae()); + + new Trigger(() -> state == SuperState.READY_ALGAE) + .and(() -> manipulator.getStatorCurrentAmps() < 20.0) + .debounce(1.0) + .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + + Robot.forceFunnelReq + .or( + new Trigger( + () -> + (Stream.of(FieldUtils.HumanPlayerTargets.values()) + .map( + (t) -> + t.location.minus(swerve.getPose()).getTranslation().getNorm()) + .min(Double::compare) + .get() + < 1.0))) + .and(manipulator::neitherBeambreak) + .whileTrue(manipulator.setRollerVelocity(-7.0)) + .whileFalse(manipulator.setRollerVelocity(0.0)); + + // // Intake coral ground + // Robot.intakeCoralReq + // .and(() -> !manipulator.bothBeambreaks()) + // // .whileTrue(manipulator.intakeCoral()); + // .onTrue(manipulator.setRollerVelocity(-10)) + // .onFalse(manipulator.setRollerVelocity(0)); + + // // intake coral funnel + // Robot.forceFunnelReq + // .or( // near hp station + // new Trigger( + // () -> + // (Stream.of(FieldUtils.HumanPlayerTargets.values()) + // .map( + // (t) -> + // + // t.location.minus(swerve.getPose()).getTranslation().getNorm()) + // .min(Double::compare) + // .get() + // < 1.0))) + // .and(manipulator::neitherBeambreak) + // .whileTrue(manipulator.setRollerVelocity(-7.0)); + + // intake algae + new Trigger(this::stateIsIntakeAlgae).whileTrue(manipulator.intakeAlgae()); + + // hold algae + new Trigger(() -> state == SuperState.READY_ALGAE).whileTrue(manipulator.holdAlgae()); + + // hold algae extra hard + new Trigger(() -> state == SuperState.PRE_BARGE).whileTrue(manipulator.holdAlgaeExtra()); + + // score algae processor + new Trigger(() -> state == SuperState.PROCESSOR).whileTrue(manipulator.scoreAlgaeProcessor()); + + // score algae barge + new Trigger(() -> state == SuperState.BARGE) + .and( + () -> + shoulder.getVelocity() + > ShoulderSubsystem.TOSS_CONFIGS.MotionMagicCruiseVelocity - 0.1) + .whileTrue(manipulator.scoreAlgaeBarge()); + + // in case algae drops + new Trigger(this::stateIsAlgaeAlike) + .and( + () -> + manipulator.getStatorCurrentAmps() + < ManipulatorSubsystem + .ALGAE_CURRENT_THRESHOLD) // TODO might be 20? need to check what the normal + // holding amperage is + .debounce(1.0) + .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + + // // goofy ahh jog + // new + // Trigger(Robot.jogCoralUpReq).whileTrue(manipulator.setRollerVelocity(3.0).withTimeout(0.1)); + // new Trigger(Robot.jogCoralDownReq) + // .whileTrue(manipulator.setRollerVelocity(-3.0).withTimeout(0.1)); + // ANTI_JAM logic // anti coral jam could start from any state, so there's no explicit transition @@ -916,23 +953,22 @@ private void addTransitions() { // normal value for that state // setSubstates isn't called every loop, so I don't think it can be set there // i have a bad feeling about this though - Robot.antiCoralJamReq - .onTrue( - Commands.runOnce( - () -> { - antiJamCoral = true; - manipulator.setState(-10); - })) - .onFalse( - Commands.runOnce( - () -> { - antiJamCoral = false; - manipulator.setState(state.manipulatorVelocity); - })); - Robot.antiAlgaeJamReq.onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); - - bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiAlgaeJamReq.negate()); + // the bad feeling did not go away so i'm just going to ignore this + + // Robot.antiCoralJamReq + // .onTrue( + // Commands.runOnce( + // () -> { + // antiJamCoral = true; + // manipulator.setState(-10); + // })) + // .onFalse( + // Commands.runOnce( + // () -> { + // antiJamCoral = false; + // manipulator.setState(state.manipulatorVelocity); + // })); } public SuperState getState() { @@ -957,10 +993,10 @@ public boolean stateIsCoralAlike() { } public static boolean stateIsScoreCoral(SuperState state) { - return - // state == SuperState.L1 - // || - state == SuperState.L2 || state == SuperState.L3 || state == SuperState.L4; + return state == SuperState.L1 + || state == SuperState.L2 + || state == SuperState.L3 + || state == SuperState.L4; } public boolean stateIsAlgaeAlike() { From f66334a620f004b9ea6dda26f2624356d83965fd Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 19 Sep 2025 18:46:37 -0700 Subject: [PATCH 141/154] maybe make mech sim better for dt dashboard? --- src/main/java/frc/robot/Robot.java | 25 ++++++++++++++++++- .../subsystems/climber/ClimberSubsystem.java | 4 +++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ab9e47de..9419a929 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -42,6 +42,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -455,7 +456,13 @@ public static enum AlgaeScoreTarget { new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); private final LoggedMechanismRoot2d elevatorRoot = // CAD distance from origin to center of carriage at full retraction - elevatorMech2d.getRoot("Elevator", Units.inchesToMeters(21.5), 0.0); + elevatorMech2d.getRoot( + "Elevator", Units.inchesToMeters(21.5), 0.0); // now what on earth is this number + // doesn't get updated or actually do anything it's just so i remember there's actually an + // elevator there when i'm looking at glass + private final LoggedMechanismLigament2d firstStage = + new LoggedMechanismLigament2d( + "First Stage", Units.inchesToMeters(41.925), ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d carriageLigament = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d shoulderLigament = @@ -464,6 +471,13 @@ public static enum AlgaeScoreTarget { new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); + private final LoggedMechanismRoot2d climberRoot = + elevatorMech2d.getRoot("Climber", Units.inchesToMeters(2), 0); + private final LoggedMechanismLigament2d climberBase = + new LoggedMechanismLigament2d("Climber Base", Units.inchesToMeters(9.5), 90); + private final LoggedMechanismLigament2d climberLigament = + new LoggedMechanismLigament2d("Climber", Units.inchesToMeters(12), 0.0); + @SuppressWarnings({"resource", "unlikely-arg-type"}) public Robot() { DriverStation.silenceJoystickConnectionWarning(true); @@ -527,9 +541,16 @@ public Robot() { CameraIOSim.pose = () -> new Pose3d(); } // Add the arms and stuff + elevatorRoot.append(firstStage); elevatorRoot.append(carriageLigament); carriageLigament.append(shoulderLigament); shoulderLigament.append(wristLigament); + shoulderLigament.setColor(new Color8Bit(Color.kBlue)); + wristLigament.setColor(new Color8Bit(Color.kGreen)); + + climberRoot.append(climberBase); + climberBase.append(climberLigament); + climberLigament.setColor(new Color8Bit(Color.kPurple)); autos = new Autos(swerve, manipulator, funnel, elevator, shoulder, wrist); autoChooser.addDefaultOption("None", autos.getNoneAuto()); @@ -1292,6 +1313,8 @@ public void robotPeriodic() { // Minus 90 to make it relative to horizontal shoulderLigament.setAngle(shoulder.getAngle().getDegrees() - 90); wristLigament.setAngle(wrist.getAngle().getDegrees() + shoulderLigament.getAngle()); + climberLigament.setAngle(climber.getAngle() - 90 - 18); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Mechanism/Elevator", elevatorMech2d); superstructure.periodic(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 69a4ffab..dd89669e 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -55,4 +55,8 @@ public Command resetClimber() { public Command zeroClimber() { return Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true); } + + public double getAngle() { + return inputs.position; + } } From ce3ed260ff5a7dc24d9f5528d342d8ce43fdf5bb Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 19 Sep 2025 20:50:54 -0700 Subject: [PATCH 142/154] add rumble when the climber's fully extended + elevator and wrist alerts --- src/main/java/frc/robot/Robot.java | 21 +++++++++++++++++++ .../subsystems/climber/ClimberSubsystem.java | 2 +- .../elevator/ElevatorSubsystem.java | 15 +++++++++++++ .../subsystems/wrist/WristSubsystem.java | 13 ++++++++++++ 4 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9419a929..1f75c444 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -643,6 +643,12 @@ public Robot() { ROBOT_HARDWARE.swerveConstants.getDriveConfig().CurrentLimits)) .ignoringDisable(true)); + // Rumble controller when climber is fully extended + new Trigger(() -> state.get() == SuperState.PRE_CLIMB) + .and(superstructure::atExtension) + .debounce(0.1) + .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()); + SmartDashboard.putData( "Add Autos", Commands.runOnce( @@ -671,6 +677,21 @@ public Robot() { ? Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae())) : Commands.none()); + new Trigger(wrist::atSetpoint) + .negate() + .debounce(2) + .whileTrue( + Commands.runOnce( + () -> + SmartDashboard.putString( + "Wrist has not hit the setpoint for 2 seconds", "FF0000"))) + .whileFalse( + Commands.runOnce( + () -> + SmartDashboard.putString( + "Wrist has not hit the setpoint for 2 seconds", + "00FF00"))); // tune specific time maybe + elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index dd89669e..cf21e05d 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -29,7 +29,7 @@ public ClimberSubsystem(ClimberIO io) { this.io = io; SmartDashboard.putData( - "rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); + "Rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); SmartDashboard.putData("Reset Climber (MANUAL STOP)", resetClimber()); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d0f65e81..d3e976d8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -10,9 +10,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.utils.LoggedTunableNumber; @@ -39,6 +42,9 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double SLOW_ACCELERATION = 5.0; public static final double MEDIUM_ACCELERATION = 8.5; + public static final double CHECK_ZERO_SECONDS = 2; + + public enum ElevatorState { HP(Units.inchesToMeters(0.0)), INTAKE_CORAL_GROUND(Units.inchesToMeters(0.0)), @@ -97,9 +103,18 @@ public double getExtensionMeters() { private final LoggedMechanismLigament2d carriage = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); + private final Alert notZeroedAlert = new Alert("Elevator may not be zeroed!", AlertType.kWarning); + /** Creates a new ElevatorSubsystem. */ public ElevatorSubsystem(ElevatorIO io) { this.io = io; + + new Trigger(this::atExtension) + .negate() + .debounce(CHECK_ZERO_SECONDS) + .or(() -> !hasZeroed) + .onTrue(Commands.runOnce(() -> notZeroedAlert.set(true))) + .onFalse(Commands.runOnce(() -> notZeroedAlert.set(false))); } @Override diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 865fda25..57bea9bf 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -5,9 +5,12 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -21,6 +24,7 @@ public class WristSubsystem extends SubsystemBase { Rotation2d.fromDegrees(178).minus(Rotation2d.fromRadians(3.357)); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); + public static final double CHECK_ZERO_SECONDS = 2; public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); @@ -82,8 +86,17 @@ public Rotation2d getAngle() { @AutoLogOutput(key = "Carriage/Wrist/Has Zeroed") public static boolean hasZeroed = false; + private final Alert notZeroedAlert = new Alert("Wrist may not be zeroed!", AlertType.kWarning); + public WristSubsystem(WristIO io) { this.io = io; + + new Trigger(this::atSetpoint) + .negate() + .debounce(CHECK_ZERO_SECONDS) + .or(() -> !hasZeroed) + .onTrue(Commands.runOnce(() -> notZeroedAlert.set(true))) + .onFalse(Commands.runOnce(() -> notZeroedAlert.set(false))); } @Override From c623d13a76cc3395284ae46b321dd762d557816f Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 20 Sep 2025 20:15:33 -0700 Subject: [PATCH 143/154] [not tested irl] antijam algae --- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/subsystems/Superstructure.java | 19 ++++++++++++++++--- .../elevator/ElevatorSubsystem.java | 8 +++++--- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1f75c444..2566942c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -236,10 +236,10 @@ public static enum AlgaeScoreTarget { driver.y().debounce(0.5).or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") - public static final Trigger antiCoralJamReq = driver.a(); + public static final Trigger antiJamCoralReq = driver.a(); @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") - public static final Trigger antiAlgaeJamReq = driver.b(); + public static final Trigger antiJamAlgaeReq = driver.b(); @AutoLogOutput(key = "Superstructure/Home Request") public static Trigger homeReq = driver.start(); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 88ccd9cc..dcb834e5 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -146,6 +146,11 @@ public enum SuperState { 0.5), // lowkey why is this so slow HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), + PRE_ANTIJAM_ALGAE( + ElevatorState.PRE_ANTIJAM_ALGAE, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + 0.0), ANTIJAM_ALGAE( ElevatorState.ANTIJAM_ALGAE, ShoulderState.INTAKE_CORAL_GROUND, @@ -825,7 +830,6 @@ private void addTransitions() { SuperState.READY_ALGAE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); // ---Climb--- - bindTransition(SuperState.IDLE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); bindTransition( @@ -838,9 +842,18 @@ private void addTransitions() { bindTransition(SuperState.PRE_CLIMB, SuperState.IDLE, Robot.preClimbReq.negate()); - Robot.antiAlgaeJamReq.onTrue(this.changeStateTo(SuperState.ANTIJAM_ALGAE)); + // Algae antijam + Robot.antiJamAlgaeReq.onTrue(this.changeStateTo(SuperState.PRE_ANTIJAM_ALGAE)); + + bindTransition( + SuperState.PRE_ANTIJAM_ALGAE, + SuperState.ANTIJAM_ALGAE, + new Trigger( + () -> + shoulder.isNearAngle(state.shoulderState.getAngle()) + && wrist.isNearAngle(state.wristState.getAngle()))); - bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiAlgaeJamReq.negate()); + bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiJamAlgaeReq.negate()); } private void addManipulatorStates() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d3e976d8..9c3cf4f0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -44,7 +44,6 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CHECK_ZERO_SECONDS = 2; - public enum ElevatorState { HP(Units.inchesToMeters(0.0)), INTAKE_CORAL_GROUND(Units.inchesToMeters(0.0)), @@ -61,8 +60,11 @@ public enum ElevatorState { BARGE(Units.inchesToMeters(62.5)), PROCESSOR(Units.inchesToMeters(0.01)), // lmao HOME(-0.3), // i'm quite scared - ANTIJAM_ALGAE(0.0) // NOT ACTUALLY 0!!! - ; + PRE_ANTIJAM_ALGAE( + Units.inchesToMeters( + 20)), // TODO pre algae antijam elevator extension: this needs to be adjusted to a + // guaranteed safe position + ANTIJAM_ALGAE(Units.inchesToMeters(40)); private final DoubleSupplier extensionMeters; From c7eee8aa137af6cad8581d2dc9609423da47e3d9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 20 Sep 2025 20:26:08 -0700 Subject: [PATCH 144/154] [not tested irl] antijam coral --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 37 ++----------------- 2 files changed, 5 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2566942c..6957fb60 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -715,7 +715,7 @@ public Robot() { .get() < 1.0) ? 1.0 - : (superstructure.antiJamCoral() + : (antiJamCoralReq.getAsBoolean() ? -10.0 : 0.0)))); // at what point do ternary operators do more harm than good climber.setDefaultCommand( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index dcb834e5..33bcb17b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -213,8 +213,6 @@ private SuperState( private final FunnelSubsystem funnel; private final SwerveSubsystem swerve; - public static boolean antiJamCoral; - @AutoLogOutput public static boolean coralIndexed; /** Creates a new Superstructure. */ @@ -895,7 +893,7 @@ private void addManipulatorStates() { < 1.0))) .and(manipulator::neitherBeambreak) .whileTrue(manipulator.setRollerVelocity(-7.0)) - .whileFalse(manipulator.setRollerVelocity(0.0)); + .whileFalse(manipulator.setRollerVelocity(0.0)); //TODO well that seems wrong // // Intake coral ground // Robot.intakeCoralReq @@ -951,37 +949,14 @@ private void addManipulatorStates() { .debounce(1.0) .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + // Antijam coral + Robot.antiJamCoralReq.whileTrue(manipulator.setRollerVelocity(-10.0)); + // // goofy ahh jog // new // Trigger(Robot.jogCoralUpReq).whileTrue(manipulator.setRollerVelocity(3.0).withTimeout(0.1)); // new Trigger(Robot.jogCoralDownReq) // .whileTrue(manipulator.setRollerVelocity(-3.0).withTimeout(0.1)); - - // ANTI_JAM logic - - // anti coral jam could start from any state, so there's no explicit transition - // in fact the state doesn't ever change--the manipulator velocity (and funnel velocity in - // robot.java) is just overridden - // which is why once the request is canceled, the manipulator state is manually set back to the - // normal value for that state - // setSubstates isn't called every loop, so I don't think it can be set there - // i have a bad feeling about this though - - // the bad feeling did not go away so i'm just going to ignore this - - // Robot.antiCoralJamReq - // .onTrue( - // Commands.runOnce( - // () -> { - // antiJamCoral = true; - // manipulator.setState(-10); - // })) - // .onFalse( - // Commands.runOnce( - // () -> { - // antiJamCoral = false; - // manipulator.setState(state.manipulatorVelocity); - // })); } public SuperState getState() { @@ -1034,8 +1009,4 @@ public boolean stateIsIntakeAlgae() { || state == SuperState.INTAKE_ALGAE_STACK || state == SuperState.INTAKE_ALGAE_GROUND; } - - public boolean antiJamCoral() { - return antiJamCoral; - } } From abf58d3cbf06fc8299f5ae4984e873581acee5fa Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 20 Sep 2025 20:27:28 -0700 Subject: [PATCH 145/154] [not tested irl] change operator algae level selection --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6957fb60..4a4fe225 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1083,7 +1083,7 @@ public Robot() { Commands.runOnce( () -> { coralTarget = ReefTarget.L2; - algaeIntakeTarget = AlgaeIntakeTarget.LOW; + algaeIntakeTarget = AlgaeIntakeTarget.STACK; })); operator .b() @@ -1091,7 +1091,7 @@ public Robot() { Commands.runOnce( () -> { coralTarget = ReefTarget.L3; - algaeIntakeTarget = AlgaeIntakeTarget.HIGH; + algaeIntakeTarget = AlgaeIntakeTarget.LOW; })); operator .y() @@ -1099,7 +1099,7 @@ public Robot() { Commands.runOnce( () -> { coralTarget = ReefTarget.L4; - algaeIntakeTarget = AlgaeIntakeTarget.STACK; + algaeIntakeTarget = AlgaeIntakeTarget.HIGH; })); operator From 78d1de0eca9ea973871ae8ed32ddef1c77af8880 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 21 Sep 2025 16:05:49 -0700 Subject: [PATCH 146/154] fix antijam coral --- src/main/java/frc/robot/subsystems/Superstructure.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 33bcb17b..2ef44b18 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -893,7 +893,7 @@ private void addManipulatorStates() { < 1.0))) .and(manipulator::neitherBeambreak) .whileTrue(manipulator.setRollerVelocity(-7.0)) - .whileFalse(manipulator.setRollerVelocity(0.0)); //TODO well that seems wrong + .whileFalse(manipulator.setRollerVelocity(0.0)); // TODO well that seems wrong // // Intake coral ground // Robot.intakeCoralReq @@ -950,7 +950,10 @@ private void addManipulatorStates() { .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); // Antijam coral - Robot.antiJamCoralReq.whileTrue(manipulator.setRollerVelocity(-10.0)); + Robot.antiJamCoralReq + .whileTrue(manipulator.setRollerVelocity(-10.0)) + .whileFalse(manipulator.setRollerVelocity(0.0)); + // ,whileFalse(manipulator.setRollerVelocity(0.0)); // // goofy ahh jog // new From c228633b2e8daef2e1c6406d8294c254be7606c9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 21 Sep 2025 16:40:38 -0700 Subject: [PATCH 147/154] make antijam algae better --- .../frc/robot/subsystems/Superstructure.java | 39 +++++++------------ .../elevator/ElevatorSubsystem.java | 7 +--- 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2ef44b18..0ca56f25 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -155,6 +155,11 @@ public enum SuperState { ElevatorState.ANTIJAM_ALGAE, ShoulderState.INTAKE_CORAL_GROUND, WristState.INTAKE_CORAL_GROUND, + 0.0), + POST_ANTIJAM_ALGAE( + ElevatorState.HP, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.POST_INTAKE_CORAL_GROUND, 0.0) // SPIT_CORAL(), // ANTI_JAM, @@ -851,7 +856,13 @@ private void addTransitions() { shoulder.isNearAngle(state.shoulderState.getAngle()) && wrist.isNearAngle(state.wristState.getAngle()))); - bindTransition(SuperState.ANTIJAM_ALGAE, SuperState.IDLE, Robot.antiJamAlgaeReq.negate()); + bindTransition( + SuperState.ANTIJAM_ALGAE, SuperState.POST_ANTIJAM_ALGAE, Robot.antiJamAlgaeReq.negate()); + + bindTransition( + SuperState.POST_ANTIJAM_ALGAE, + SuperState.IDLE, + Robot.antiJamAlgaeReq.negate().and(new Trigger(this::atExtension))); } private void addManipulatorStates() { @@ -880,8 +891,9 @@ private void addManipulatorStates() { .debounce(1.0) .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + // intake coral funnel Robot.forceFunnelReq - .or( + .or( // near hp station new Trigger( () -> (Stream.of(FieldUtils.HumanPlayerTargets.values()) @@ -895,29 +907,6 @@ private void addManipulatorStates() { .whileTrue(manipulator.setRollerVelocity(-7.0)) .whileFalse(manipulator.setRollerVelocity(0.0)); // TODO well that seems wrong - // // Intake coral ground - // Robot.intakeCoralReq - // .and(() -> !manipulator.bothBeambreaks()) - // // .whileTrue(manipulator.intakeCoral()); - // .onTrue(manipulator.setRollerVelocity(-10)) - // .onFalse(manipulator.setRollerVelocity(0)); - - // // intake coral funnel - // Robot.forceFunnelReq - // .or( // near hp station - // new Trigger( - // () -> - // (Stream.of(FieldUtils.HumanPlayerTargets.values()) - // .map( - // (t) -> - // - // t.location.minus(swerve.getPose()).getTranslation().getNorm()) - // .min(Double::compare) - // .get() - // < 1.0))) - // .and(manipulator::neitherBeambreak) - // .whileTrue(manipulator.setRollerVelocity(-7.0)); - // intake algae new Trigger(this::stateIsIntakeAlgae).whileTrue(manipulator.intakeAlgae()); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 9c3cf4f0..2cbca8e4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -60,11 +60,8 @@ public enum ElevatorState { BARGE(Units.inchesToMeters(62.5)), PROCESSOR(Units.inchesToMeters(0.01)), // lmao HOME(-0.3), // i'm quite scared - PRE_ANTIJAM_ALGAE( - Units.inchesToMeters( - 20)), // TODO pre algae antijam elevator extension: this needs to be adjusted to a - // guaranteed safe position - ANTIJAM_ALGAE(Units.inchesToMeters(40)); + PRE_ANTIJAM_ALGAE(Units.inchesToMeters(20)), + ANTIJAM_ALGAE(Units.inchesToMeters(45)); private final DoubleSupplier extensionMeters; From 560fb71a9cd5a1ec7087f15e9b72e16d3b54df21 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 21 Sep 2025 16:46:53 -0700 Subject: [PATCH 148/154] clean up manipulator states --- .../frc/robot/subsystems/Superstructure.java | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0ca56f25..630257ad 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -871,26 +871,12 @@ private void addManipulatorStates() { .and(() -> stateIsScoreCoral(state)) .whileTrue(manipulator.setRollerVelocity(() -> state.manipulatorVelocity)); + // Intake coral Robot.intakeCoralReq .and(() -> !manipulator.bothBeambreaks()) .onTrue(manipulator.setRollerVelocity(-10)) .onFalse(manipulator.setRollerVelocity(0)); - new Trigger( - () -> - state == SuperState.INTAKE_ALGAE_HIGH - || state == SuperState.INTAKE_ALGAE_LOW - || state == SuperState.INTAKE_ALGAE_GROUND - || state == SuperState.INTAKE_ALGAE_STACK - || state == SuperState.READY_ALGAE - || state == SuperState.PRE_BARGE) - .whileTrue(manipulator.intakeAlgae()); - - new Trigger(() -> state == SuperState.READY_ALGAE) - .and(() -> manipulator.getStatorCurrentAmps() < 20.0) - .debounce(1.0) - .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); - // intake coral funnel Robot.forceFunnelReq .or( // near hp station @@ -928,6 +914,7 @@ private void addManipulatorStates() { .whileTrue(manipulator.scoreAlgaeBarge()); // in case algae drops + // I'm not convinced this works but wtv new Trigger(this::stateIsAlgaeAlike) .and( () -> From fc6f40d44b9d0b57b8779005d2a0b08d7fe89254 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 21 Sep 2025 17:28:06 -0700 Subject: [PATCH 149/154] i may be stupid --- src/main/java/frc/robot/Robot.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4a4fe225..293d6efe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -695,9 +695,7 @@ public Robot() { elevator.setDefaultCommand(elevator.setStateExtension()); shoulder.setDefaultCommand(shoulder.setStateAngle()); wrist.setDefaultCommand(wrist.setStateAngle()); - // manipulator.setDefaultCommand( - // manipulator.setStateVelocity( - // () -> superstructure.atExtension() || superstructure.antiJamCoral())); + manipulator.setDefaultCommand(manipulator.setRollerVelocity(0.0)); funnel.setDefaultCommand( funnel.setRollerVoltage( () -> From 33192fdd527a8d4a8e68ec1e3d85f69b8783b081 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 21 Sep 2025 17:30:16 -0700 Subject: [PATCH 150/154] operator jog is not great but hopefully ok --- .../frc/robot/subsystems/Superstructure.java | 43 ++++++++----------- 1 file changed, 17 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 630257ad..7af20e61 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -306,38 +306,19 @@ private void setSubstates() { } private void addTransitions() { - // Prob a better way to impl this - // Vaughn says he wants this available anytime - // TODO this will probably not still work - // Robot.forceIndexReq.whileTrue(manipulator.setRollerVelocity(1.0)); - // ---Funnel--- bindTransition( SuperState.IDLE, SuperState.CHECK_CORAL, new Trigger(manipulator::getSecondBeambreak).debounce(0.1)); - new Trigger(() -> state == SuperState.CHECK_CORAL) - .onTrue( - Commands.runOnce(() -> coralIndexed = false) - .andThen( - manipulator - .setRollerVelocity(2) - .until( - () -> - manipulator.getSecondBeambreak() - && !manipulator.getFirstBeambreak()) - .andThen(manipulator.setRollerVelocity(-1.5)) - .until(new Trigger(manipulator::bothBeambreaks).debounce(0.2)) - .andThen(Commands.runOnce(() -> coralIndexed = true)))); - // first beambreak is the one closest to the outside when scoring l2-4 // second beambreak is the one closer to the funnel when scoring l2-4 bindTransition( SuperState.CHECK_CORAL, SuperState.READY_CORAL, // new Trigger(manipulator::bothBeambreaks).debounce(0.5)); - new Trigger(() -> coralIndexed).or(() -> Robot.isSimulation())); + new Trigger(manipulator::bothBeambreaks).debounce(0.2).or(() -> Robot.isSimulation())); // .and(() -> manipulator.getTimeSinceZero() < 1.0), // ---Intake coral ground--- @@ -378,7 +359,14 @@ private void addTransitions() { new Trigger(this::atExtension).and(manipulator::neitherBeambreak)); bindTransition( - SuperState.READY_CORAL, SuperState.IDLE, new Trigger(manipulator::neitherBeambreak)); + SuperState.READY_CORAL, + SuperState.IDLE, + new Trigger(manipulator::neitherBeambreak).debounce(0.1)); + + bindTransition( + SuperState.CHECK_CORAL, + SuperState.IDLE, + new Trigger(manipulator::neitherBeambreak).debounce(0.1)); // ---L1--- bindTransition( SuperState.READY_CORAL, @@ -931,11 +919,14 @@ private void addManipulatorStates() { .whileFalse(manipulator.setRollerVelocity(0.0)); // ,whileFalse(manipulator.setRollerVelocity(0.0)); - // // goofy ahh jog - // new - // Trigger(Robot.jogCoralUpReq).whileTrue(manipulator.setRollerVelocity(3.0).withTimeout(0.1)); - // new Trigger(Robot.jogCoralDownReq) - // .whileTrue(manipulator.setRollerVelocity(-3.0).withTimeout(0.1)); + // goofy ahh jog + new Trigger(Robot.jogCoralUpReq) + .whileTrue(manipulator.setRollerVoltage(-2.0).withTimeout(0.05)) + .whileFalse(manipulator.setRollerVoltage(0.0)); + + new Trigger(Robot.jogCoralDownReq) + .whileTrue(manipulator.setRollerVoltage(2.0).withTimeout(0.05)) + .whileFalse(manipulator.setRollerVoltage(0.0)); } public SuperState getState() { From d4603b8514171b17e58756f6d43662312ae8560b Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 23 Sep 2025 17:59:20 -0700 Subject: [PATCH 151/154] reverse l4 direction lmao --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 7af20e61..f3878063 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -70,7 +70,7 @@ public enum SuperState { // PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), // YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), PRE_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), - L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, -20.0), // POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), POST_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), // POST_POST_L4( From ea1275a8b7d066f70100b5b5fcd0c95b47c1f244 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Tue, 23 Sep 2025 19:45:54 -0700 Subject: [PATCH 152/154] fix idle position --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 293d6efe..7e007484 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -225,7 +225,7 @@ public static enum AlgaeScoreTarget { driver .x() .and(driver.pov(-1).negate()) - .debounce(0.5) + .debounce(0.25) .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)); @AutoLogOutput(key = "Superstructure/Climb Confirm Request") diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f3878063..109a8c79 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -142,7 +142,7 @@ public enum SuperState { ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 0.0, - 1.0, + 1.0 - 0.2, 0.5), // lowkey why is this so slow HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 6187a081..e2e0d5b9 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -37,7 +37,7 @@ public class ShoulderSubsystem extends SubsystemBase { .withMotionMagicAcceleration(4.0); public enum ShoulderState { - HP(55.0), + HP(57.0), PRE_INTAKE_CORAL_GROUND(15.0), INTAKE_CORAL_GROUND(0.0), PRE_L1(35.0), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 57bea9bf..b7cede94 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,7 +39,7 @@ public enum WristState { PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS INTAKE_CORAL_GROUND(0.0), POST_INTAKE_CORAL_GROUND(40), - HP(170.0), + HP(172.0), L1(Units.radiansToDegrees(0.349 - 0.1)), PRE_L2(170.0), L2(Units.radiansToDegrees(2.447)), From 4d47b58632dbdd6d9f2bb4fb68658bb660f4b588 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 24 Sep 2025 10:23:47 -0700 Subject: [PATCH 153/154] climb tuning --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 109a8c79..b6884b90 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -135,14 +135,14 @@ public enum SuperState { ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 10.0, - 3.0, + 3.0 + 0.35, 2.0), CLIMB( ElevatorState.INTAKE_ALGAE_GROUND, ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 0.0, - 1.0 - 0.2, + 1.0, // TODO + 0.35, 0.5), // lowkey why is this so slow HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), From 37cfd4999ec410133221d7f0687f3d03c293afbc Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 24 Sep 2025 17:30:25 +0000 Subject: [PATCH 154/154] run spotless --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index b6884b90..e7a56044 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -142,7 +142,7 @@ public enum SuperState { ShoulderState.INTAKE_ALGAE_GROUND, WristState.INTAKE_ALGAE_GROUND, 0.0, - 1.0, // TODO + 0.35, + 1.0, // TODO + 0.35, 0.5), // lowkey why is this so slow HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0),