From 09cd8f44c0406918bf717e6e45f1995a2cd6e15d Mon Sep 17 00:00:00 2001 From: IRoot <74614800+imroot07@users.noreply.github.com> Date: Fri, 10 Jan 2025 20:34:47 -0500 Subject: [PATCH 01/48] started climb io layers --- .../frc/robot/constants/ClimbConstants.java | 151 +++++++++++++++++- .../robot/subsystems/climb/ClimbArmIO.java | 23 +++ .../robot/subsystems/climb/ClimbArmIOFX.java | 66 ++++++++ .../subsystems/climb/ClimbSubsystem.java | 39 ++++- .../robot/subsystems/climb/ClimbWheelIO.java | 21 +++ .../subsystems/climb/ClimbWheelIOFX.java | 53 ++++++ 6 files changed, 351 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 07b22b1c..c7caa99c 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -1,3 +1,152 @@ package frc.robot.constants; -public class ClimbConstants {} +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.ForwardLimitSourceValue; +import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.ReverseLimitSourceValue; +import com.ctre.phoenix6.signals.ReverseLimitTypeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public class ClimbConstants { + public static int kWheelFxId = 0; + public static int kArmFxId = 0; + + public static final AngularVelocity kWheelCloseEnough = DegreesPerSecond.of(5.0); + public static final Angle kArmCloseEnough = Degrees.of(5.0); + public static final Angle kArmMaxFwd = Degrees.of(100); + public static final Angle kArmMaxRev = Degrees.of(-100); + public static final Angle kArmZeroTicks = Degrees.of(1530); + + public static TalonFXConfiguration getWheelFXConfig() { + TalonFXConfiguration wheelFxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(10) + .withStatorCurrentLimitEnable(false) + .withStatorCurrentLimit(20) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + wheelFxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + wheelFxConfig.HardwareLimitSwitch = hwLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0) + .withKA(0); + wheelFxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration(0) + .withMotionMagicCruiseVelocity(0) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(0); + wheelFxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Coast); + wheelFxConfig.MotorOutput = motorOut; + + return wheelFxConfig; + } + + public static TalonFXConfiguration getArmFxConfig() { + TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); + + CurrentLimitsConfigs current = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(10) + .withStatorCurrentLimitEnable(false) + .withStatorCurrentLimit(20) + .withSupplyCurrentLimit(10) + .withSupplyCurrentLowerLimit(8) + .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimitEnable(true); + armFxConfig.CurrentLimits = current; + + HardwareLimitSwitchConfigs hwLimit = + new HardwareLimitSwitchConfigs() + .withForwardLimitAutosetPositionEnable(false) + .withForwardLimitEnable(false) + .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) + .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) + .withReverseLimitAutosetPositionEnable(false) + .withReverseLimitEnable(false) + .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) + .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); + armFxConfig.HardwareLimitSwitch = hwLimit; + + SoftwareLimitSwitchConfigs swLimit = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kArmMaxFwd) + .withReverseSoftLimitEnable(true) + .withReverseSoftLimitThreshold(kArmMaxRev); + armFxConfig.SoftwareLimitSwitch = swLimit; + + Slot0Configs slot0 = + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withGravityType(GravityTypeValue.Elevator_Static) + .withKG(0) + .withKS(0) + .withKV(0) + .withKA(0); + armFxConfig.Slot0 = slot0; + + MotionMagicConfigs motionMagic = + new MotionMagicConfigs() + .withMotionMagicAcceleration(0) + .withMotionMagicCruiseVelocity(0) + .withMotionMagicExpo_kA(0) + .withMotionMagicExpo_kV(0) + .withMotionMagicJerk(0); + armFxConfig.MotionMagic = motionMagic; + + MotorOutputConfigs motorOut = + new MotorOutputConfigs() + .withDutyCycleNeutralDeadband(0.01) + .withNeutralMode(NeutralModeValue.Coast); + armFxConfig.MotorOutput = motorOut; + + return armFxConfig; + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java new file mode 100644 index 00000000..335559c7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.climb; + +import static edu.wpi.first.units.Units.Rotations; + +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +import edu.wpi.first.units.measure.Angle; + +public interface ClimbArmIO { + + @AutoLog static class ClimbArmIOInputs { + public Angle position = Rotations.of(0.0); + } + + public default void setPosition(Angle position) {} + + public default void updateInputs(ClimbArmIOInputs inputs) {} + + public default void zero() {} + + public default void registerWith(TelemetryService telemetryService) {} +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java new file mode 100644 index 00000000..4b874c86 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.climb; + +import static edu.wpi.first.units.Units.Degrees; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.ClimbConstants; +import frc.robot.constants.ExampleConstants; + +public class ClimbArmIOFX implements ClimbArmIO { + private Logger logger; + private TalonFX talonFx; + + private final Angle absSensorInitial; + private Angle relSetpointOffset; + private Angle setpoint; + + TalonFXConfigurator configurator; + private MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); + StatusSignal currPosition; + + public ClimbArmIOFX() { + logger = LoggerFactory.getLogger(this.getClass()); + talonFx = new TalonFX(ClimbConstants.kArmFxId); + absSensorInitial = + talonFx.getPosition().getValue(); // relative encoder starts up as absolute position offset + + configurator = talonFx.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // Factory default motor controller + configurator.apply(ClimbConstants.getArmFxConfig()); + + currPosition = talonFx.getPosition(); + } + + @Override + public void setPosition(Angle position) { + setpoint = position.plus(relSetpointOffset); + + logger.info("Setting position to {} degrees", setpoint.in(Degrees)); + + talonFx.setControl(positionRequest.withPosition(setpoint)); + } + + @Override + public void updateInputs(ClimbArmIOInputs inputs) { + inputs.position = currPosition.refresh().getValue(); + } + + @Override + public void zero() { + relSetpointOffset = ExampleConstants.kZeroTicks; + logger.info( + "Abs: {}, Zero Pos: {}, Offset: {}", + absSensorInitial, + ExampleConstants.kZeroTicks, + absSensorInitial.minus(ExampleConstants.kZeroTicks)); + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 7f0629f4..6e0f2e4b 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -1,3 +1,40 @@ package frc.robot.subsystems.climb; -public class ClimbSubsystem {} +import static edu.wpi.first.units.Units.Rotations; + +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; + +import edu.wpi.first.units.measure.Angle; +import frc.robot.standards.ClosedLoopPosSubsystem; +import frc.robot.subsystems.climb.ClimbArmIO.ClimbArmIOInputs; +import frc.robot.subsystems.example.ExampleIOInputsAutoLogged; +import frc.robot.subsystems.example.ExampleSubsystem.ExampleState; + +public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem { + private final ClimbWheelIO wheelIo; + private final ClimbArmIO armIo; + + private final ClimbWheelIOInputsAutoLogged wheelInputs = new ExampleIOInputsAutoLogged(); + private final ClimbArmIOInputsAutoLogged armInputs = new ExampleIOInputsAutoLogged(); + + private Angle setpoint = Rotations.of(0.0); + private ClimbState curState = ClimbState.INIT; + + public ClimbSubsystem(ClimbWheelIO wheelIo, ClimbArmIO armIo) { + this.wheelIo = wheelIo; + this.armIo=armIo; + + zero(); + } + + @Override + public void zero() { + armIo.zero(); + curState = ClimbState.ZEROED; + } + + public enum ClimbState { + INIT, + ZEROED + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java new file mode 100644 index 00000000..56edc11f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems.climb; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; + +import edu.wpi.first.units.measure.AngularVelocity; + +public interface ClimbWheelIO { + + @AutoLog static class ClimbWheelIOInputs { + public AngularVelocity velocity = RotationsPerSecond.of(0.0); + } + + public default void setVelocity(AngularVelocity velocity) {} + + public default void updateInputs(ClimbWheelIOInputs inputs) {} + + public default void registerWith(TelemetryService telemetryService) {} +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java new file mode 100644 index 00000000..e53b995f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.climb; + +import static edu.wpi.first.units.Units.DegreesPerSecond; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.constants.ClimbConstants; + +public class ClimbWheelIOFX implements ClimbWheelIO { + private Logger logger; + private TalonFX talonFx; + + TalonFXConfigurator configurator; + private MotionMagicVelocityDutyCycle velocityRequest = new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0); + StatusSignal curVelocity; + + public ClimbWheelIOFX() { + logger = LoggerFactory.getLogger(this.getClass()); + talonFx = new TalonFX(ClimbConstants.kWheelFxId); + + configurator = talonFx.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // Factory default motor controller + configurator.apply(ClimbConstants.getWheelFXConfig()); + + curVelocity = talonFx.getVelocity(); + } + + @Override + public void setVelocity(AngularVelocity velocity) { + logger.info("Setting velocity to {} degrees per second", velocity.in(DegreesPerSecond)); + + talonFx.setControl(velocityRequest.withVelocity(velocity)); + } + + @Override + public void updateInputs(ClimbWheelIOInputs inputs) { + inputs.velocity = curVelocity.refresh().getValue(); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFx, true); + } +} From cb0cb50dedb85c52826e66a285839f10094dfdd3 Mon Sep 17 00:00:00 2001 From: IRoot <74614800+imroot07@users.noreply.github.com> Date: Fri, 10 Jan 2025 21:01:46 -0500 Subject: [PATCH 02/48] stubbed out climb subsystem --- .../frc/robot/constants/ClimbConstants.java | 1 - .../robot/subsystems/climb/ClimbArmIO.java | 14 +-- .../robot/subsystems/climb/ClimbArmIOFX.java | 21 ++-- .../subsystems/climb/ClimbSubsystem.java | 97 ++++++++++++++----- .../robot/subsystems/climb/ClimbWheelIO.java | 13 +-- .../subsystems/climb/ClimbWheelIOFX.java | 30 ++---- .../robot/subsystems/example/ExampleIOFX.java | 14 +-- 7 files changed, 105 insertions(+), 85 deletions(-) diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index c7caa99c..918ff86b 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -16,7 +16,6 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; - import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java index 335559c7..7ef57741 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java @@ -2,19 +2,19 @@ import static edu.wpi.first.units.Units.Rotations; +import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; -import edu.wpi.first.units.measure.Angle; - public interface ClimbArmIO { - - @AutoLog static class ClimbArmIOInputs { - public Angle position = Rotations.of(0.0); - } + + @AutoLog + static class ClimbArmIOInputs { + public Angle position = Rotations.of(0.0); + } public default void setPosition(Angle position) {} - + public default void updateInputs(ClimbArmIOInputs inputs) {} public default void zero() {} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java index 4b874c86..830a85d5 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java @@ -1,31 +1,30 @@ package frc.robot.subsystems.climb; -import static edu.wpi.first.units.Units.Degrees; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; - import edu.wpi.first.units.measure.Angle; import frc.robot.constants.ClimbConstants; import frc.robot.constants.ExampleConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class ClimbArmIOFX implements ClimbArmIO { private Logger logger; private TalonFX talonFx; - + private final Angle absSensorInitial; private Angle relSetpointOffset; private Angle setpoint; TalonFXConfigurator configurator; - private MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); - StatusSignal currPosition; + private MotionMagicDutyCycle positionRequest = + new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); + StatusSignal curPosition; public ClimbArmIOFX() { logger = LoggerFactory.getLogger(this.getClass()); @@ -37,21 +36,21 @@ public ClimbArmIOFX() { configurator.apply(new TalonFXConfiguration()); // Factory default motor controller configurator.apply(ClimbConstants.getArmFxConfig()); - currPosition = talonFx.getPosition(); + curPosition = talonFx.getPosition(); } @Override public void setPosition(Angle position) { setpoint = position.plus(relSetpointOffset); - logger.info("Setting position to {} degrees", setpoint.in(Degrees)); + logger.info("Setting position to {} rotations", setpoint.in(Rotations)); talonFx.setControl(positionRequest.withPosition(setpoint)); } @Override public void updateInputs(ClimbArmIOInputs inputs) { - inputs.position = currPosition.refresh().getValue(); + inputs.position = curPosition.refresh().getValue(); } @Override diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 6e0f2e4b..b4538219 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -2,39 +2,88 @@ import static edu.wpi.first.units.Units.Rotations; -import org.strykeforce.telemetry.measurable.MeasurableSubsystem; - import edu.wpi.first.units.measure.Angle; +import frc.robot.constants.ClimbConstants; import frc.robot.standards.ClosedLoopPosSubsystem; -import frc.robot.subsystems.climb.ClimbArmIO.ClimbArmIOInputs; -import frc.robot.subsystems.example.ExampleIOInputsAutoLogged; -import frc.robot.subsystems.example.ExampleSubsystem.ExampleState; +import java.util.Set; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem { - private final ClimbWheelIO wheelIo; - private final ClimbArmIO armIo; + private final ClimbArmIO io; + private final ClimbWheelIO wheelIo; - private final ClimbWheelIOInputsAutoLogged wheelInputs = new ExampleIOInputsAutoLogged(); - private final ClimbArmIOInputsAutoLogged armInputs = new ExampleIOInputsAutoLogged(); + private final ClimbArmIOInputsAutoLogged inputs = new ClimbArmIOInputsAutoLogged(); - private Angle setpoint = Rotations.of(0.0); - private ClimbState curState = ClimbState.INIT; + private Angle setpoint = Rotations.of(0.0); + private ClimbState curState = ClimbState.INIT; - public ClimbSubsystem(ClimbWheelIO wheelIo, ClimbArmIO armIo) { - this.wheelIo = wheelIo; - this.armIo=armIo; + public ClimbSubsystem(ClimbArmIO io, ClimbWheelIO wheelIo) { + this.io = io; + this.wheelIo = wheelIo; - zero(); - } + zero(); + } - @Override - public void zero() { - armIo.zero(); - curState = ClimbState.ZEROED; - } + @Override + public Angle getPosition() { + return inputs.position; + } + + @Override + public void setPosition(Angle position) { + io.setPosition(position); + } + + @Override + public boolean isFinished() { + return setpoint.minus(inputs.position).abs(Rotations) + <= ClimbConstants.kArmCloseEnough.in(Rotations); + } - public enum ClimbState { - INIT, - ZEROED + @Override + public void zero() { + io.zero(); + + curState = ClimbState.ZEROED; + } + + @Override + public void periodic() { + // Read Inputs + io.updateInputs(inputs); + + // State Machine + switch (curState) { + case INIT: + break; + case ZEROED: + break; + default: + break; } + + // Log Outputs + Logger.recordOutput("Coral/curState", curState.ordinal()); + Logger.recordOutput("Coral/setpoint", setpoint.in(Rotations)); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); + wheelIo.registerWith(telemetryService); + io.registerWith(telemetryService); + } + + @Override + public Set getMeasures() { + return Set.of(); + } + + public enum ClimbState { + INIT, + ZEROED + } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java index 56edc11f..0ff876ef 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java @@ -1,21 +1,10 @@ package frc.robot.subsystems.climb; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; -import edu.wpi.first.units.measure.AngularVelocity; - public interface ClimbWheelIO { - - @AutoLog static class ClimbWheelIOInputs { - public AngularVelocity velocity = RotationsPerSecond.of(0.0); - } - public default void setVelocity(AngularVelocity velocity) {} - - public default void updateInputs(ClimbWheelIOInputs inputs) {} + public default void setPercent(double pct) {} public default void registerWith(TelemetryService telemetryService) {} } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java index e53b995f..f0083b58 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java @@ -1,27 +1,18 @@ package frc.robot.subsystems.climb; -import static edu.wpi.first.units.Units.DegreesPerSecond; - -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; -import org.strykeforce.telemetry.TelemetryService; - -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.ClimbConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; public class ClimbWheelIOFX implements ClimbWheelIO { private Logger logger; private TalonFX talonFx; TalonFXConfigurator configurator; - private MotionMagicVelocityDutyCycle velocityRequest = new MotionMagicVelocityDutyCycle(0).withEnableFOC(false).withSlot(0); - StatusSignal curVelocity; public ClimbWheelIOFX() { logger = LoggerFactory.getLogger(this.getClass()); @@ -30,24 +21,17 @@ public ClimbWheelIOFX() { configurator = talonFx.getConfigurator(); configurator.apply(new TalonFXConfiguration()); // Factory default motor controller configurator.apply(ClimbConstants.getWheelFXConfig()); - - curVelocity = talonFx.getVelocity(); } @Override - public void setVelocity(AngularVelocity velocity) { - logger.info("Setting velocity to {} degrees per second", velocity.in(DegreesPerSecond)); + public void setPercent(double pct) { + logger.info("Setting percent to {}", pct); - talonFx.setControl(velocityRequest.withVelocity(velocity)); - } - - @Override - public void updateInputs(ClimbWheelIOInputs inputs) { - inputs.velocity = curVelocity.refresh().getValue(); + talonFx.set(pct); } @Override public void registerWith(TelemetryService telemetryService) { - telemetryService.register(talonFx, true); + telemetryService.register(talonFx, true); } } diff --git a/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java b/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java index bee51796..c7d2d4de 100644 --- a/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java +++ b/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java @@ -27,8 +27,8 @@ public class ExampleIOFX implements ExampleIO { TalonFXConfigurator configurator; private MotionMagicDutyCycle positionRequest = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); - StatusSignal currPosition; - StatusSignal currVelocity; + StatusSignal curPosition; + StatusSignal curVelocity; public ExampleIOFX() { logger = LoggerFactory.getLogger(this.getClass()); @@ -42,8 +42,8 @@ public ExampleIOFX() { configurator.apply(ExampleConstants.getFXConfig()); // Attach status signals - currPosition = talonFx.getPosition(); - currVelocity = talonFx.getVelocity(); + curPosition = talonFx.getPosition(); + curVelocity = talonFx.getVelocity(); } @Override @@ -64,9 +64,9 @@ public void setPosition(Angle position) { @Override public void updateInputs(ExampleIOInputs inputs) { - BaseStatusSignal.refreshAll(currVelocity, currPosition); - inputs.velocity = currVelocity.getValue(); - inputs.position = currPosition.getValue().minus(relSetpointOffset); + BaseStatusSignal.refreshAll(curVelocity, curPosition); + inputs.velocity = curVelocity.getValue(); + inputs.position = curPosition.getValue().minus(relSetpointOffset); } @Override From f931e745aab47b995450b269b325181e060fc488 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 16 Jan 2025 17:04:09 -0500 Subject: [PATCH 03/48] started the LEDsubsystem --- .../java/frc/robot/subsystems/led/LEDIO.java | 58 ++++++++++++++++++- .../robot/subsystems/led/LEDSubsystem.java | 25 +++++++- 2 files changed, 81 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index cdad1a71..a5d9e609 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -1,3 +1,59 @@ package frc.robot.subsystems.led; -public class LEDIO {} +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; + +public class LEDIO { + public AddressableLED ledBase; + public AddressableLEDBuffer led; + + public LEDIO(int port, int length) { + ledBase = new AddressableLED(port); + ledBase.setLength(length); + ledBase.start(); + led = new AddressableLEDBuffer(length); + ledBase.setData(led); + } + + public void setPort(int port) { + ledBase.close(); + ledBase = new AddressableLED(port); + ledBase.setLength(led.getLength()); + ledBase.start(); + ledBase.setData(led); + } + + public void setLength(int length) { + ledBase.setLength(length); + led = new AddressableLEDBuffer(length); + ledBase.setData(led); + } + + public void updateLEDs() { + ledBase.setData(led); + } + + public void setLED(int index, Color color) { + led.setLED(index, color); + } + + public void setLED(int index, int r, int g, int b) { + led.setRGB(index, r, g, b); + } + + public Color getLED(int index) { + return led.getLED(index); + } + + public void setOff() { + for (int i = 0; i < led.getLength(); i++) { + setLED(i, Color.kBlack); + } + } + + public void setPattern(LEDPattern pattern) { + pattern.applyTo(led); + } +} diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 8d662836..115473ae 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -1,3 +1,26 @@ package frc.robot.subsystems.led; -public class LEDSubsystem {} +import java.util.Set; + +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class LEDSubsystem extends MeasurableSubsystem{ + public LEDStates currState = LEDStates.OFF; + + public LEDSubsystem() { + + } + + @Override + public Set getMeasures() { + return Set.of(new Measure("state", "the current state of the LEDSubsystem", () -> currState.ordinal())); + } + + public enum LEDStates { + OFF, + SOLID, + FLAMES, + RAINBOW + } +} From 66e4da013f7609eed40aa682f526180747de4829 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 20 Jan 2025 20:40:03 -0500 Subject: [PATCH 04/48] did some small things --- .../java/frc/robot/subsystems/led/LEDIO.java | 2 +- .../robot/subsystems/led/LEDSubsystem.java | 39 ++++++++++++++++--- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index a5d9e609..01efcac7 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -53,7 +53,7 @@ public void setOff() { } } - public void setPattern(LEDPattern pattern) { + public void setStrip(LEDPattern pattern) { pattern.applyTo(led); } } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 115473ae..1d595888 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -1,26 +1,53 @@ package frc.robot.subsystems.led; +import static edu.wpi.first.units.Units.Seconds; + import java.util.Set; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; + public class LEDSubsystem extends MeasurableSubsystem{ - public LEDStates currState = LEDStates.OFF; + private LEDIO io; + private LEDPattern base = LEDPattern.steps(map.of(0, Color.kAqua, )); + LEDPattern red = LEDPattern.solid(Color.kRed); + public LEDStates currState = LEDStates.OFF; + public CoralStates coralState = CoralStates.NO_PIECE; + public boolean autoPlacing = false; + public boolean hasAlgae = false; + public boolean isLimiting = false; - public LEDSubsystem() { + public LEDSubsystem(LEDIO io) { + this.io = io; + red.blink(Seconds.of(1), Seconds.of(2)); } + public void periodic() { + red.blink(Seconds.of(1), Seconds.of(2)); + if (isLimiting) { + red.overlayOn(base); + } + } + @Override public Set getMeasures() { - return Set.of(new Measure("state", "the current state of the LEDSubsystem", () -> currState.ordinal())); + return Set.of(new Measure("State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), + new Measure("CoralState", "the current Coral state of the LEDSubsystem", () -> coralState.ordinal())); } public enum LEDStates { OFF, - SOLID, - FLAMES, - RAINBOW + NORMAL, + CLIMB + } + + public enum CoralStates { + HAS_PIECE, + IN_MECH, + NO_PIECE, } } From 83a67948d0c8679b05cea44b2e3dcb571da9b615 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 23 Jan 2025 20:59:35 -0500 Subject: [PATCH 05/48] made proggress in knowing what subdivisions to make. also made initial color decisions --- .../frc/robot/constants/LEDConstants.java | 25 +++++++++- .../java/frc/robot/subsystems/led/LEDIO.java | 8 ++++ .../robot/subsystems/led/LEDSubsystem.java | 48 +++++++++++++++---- 3 files changed, 72 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 9a082f2c..a53dacb4 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -1,3 +1,26 @@ package frc.robot.constants; -public class LEDConstants {} +import edu.wpi.first.wpilibj.util.Color; + +public class LEDConstants { + public static final int stripLength = 42; + + public static final Color kAlmostBlack = new Color(0, 0, 1); + // LED Colors + public static final Color kHasAlgea = Color.kAquamarine; + public static final Color kNotHasAlgea = kAlmostBlack; + public static final Color kCoralNotInRobot = Color.kOrange; + public static final Color kCoralInFunnel = Color.kHotPink; + public static final Color kCoralInRobot = Color.kWhite; + public static final Color kCurrentLimiting = Color.kRed; + public static final Color kAutoPlacing = Color.kPurple; + public static final Color kManual = kAlmostBlack; + public static final Color kRight = Color.kSaddleBrown; + public static final Color kLeft = Color.kGreenYellow; + public static final Color kL1 = Color.kBlue; + public static final Color kL2 = Color.kGreen; + public static final Color kL3 = Color.kYellow; + public static final Color kL4 = Color.kRed; + public static final Color kGettingAlgea = Color.kAquamarine; + public static final Color kNotGettingAlgea = kAlmostBlack; +} diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index 01efcac7..bc02f8d3 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -51,9 +51,17 @@ public void setOff() { for (int i = 0; i < led.getLength(); i++) { setLED(i, Color.kBlack); } + ledBase.setData(led); } public void setStrip(LEDPattern pattern) { pattern.applyTo(led); } + + public void setStrip(Color color) { + for (int i = 0; i < led.getLength(); i++) { + setLED(i, color); + } + ledBase.setData(led); + } } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 1d595888..e45a5b44 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Seconds; +import java.util.Map; import java.util.Set; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; @@ -9,11 +10,16 @@ import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; +import frc.robot.constants.LEDConstants; public class LEDSubsystem extends MeasurableSubsystem{ private LEDIO io; - private LEDPattern base = LEDPattern.steps(map.of(0, Color.kAqua, )); - LEDPattern red = LEDPattern.solid(Color.kRed); + private LEDPattern base = LEDPattern.solid(Color.kBlack); + private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + private LEDPattern algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); + + private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); + private LEDPattern autoplace = LEDPattern.steps(Map.of(LEDConstants.stripLength/3*2, LEDConstants.kAutoPlacing)).blink(Seconds.of(0.25)); public LEDStates currState = LEDStates.OFF; public CoralStates coralState = CoralStates.NO_PIECE; public boolean autoPlacing = false; @@ -22,15 +28,41 @@ public class LEDSubsystem extends MeasurableSubsystem{ public LEDSubsystem(LEDIO io) { this.io = io; - red.blink(Seconds.of(1), Seconds.of(2)); + } + + private void buildBase() { + switch (coralState) { + case IN_FUNNEL: + coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); + break; + case IN_ROBOT: + coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + break; + case NO_PIECE: + coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); + break; + } + algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); } public void periodic() { - red.blink(Seconds.of(1), Seconds.of(2)); - if (isLimiting) { - red.overlayOn(base); + switch (currState) { + case OFF: + break; + case NORMAL: + if (autoPlacing) { + base = autoplace.overlayOn(base); + } + if (isLimiting) { + base = currentLimiting.overlayOn(base); + } + io.setStrip(base); + break; + case CLIMB: + break; } + io.updateLEDs(); } @Override @@ -46,8 +78,8 @@ public enum LEDStates { } public enum CoralStates { - HAS_PIECE, - IN_MECH, + IN_FUNNEL, + IN_ROBOT, NO_PIECE, } } From a78dcc7c7577c59f8c8b2955a51114d29bc87cc3 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 23 Jan 2025 21:01:26 -0500 Subject: [PATCH 06/48] spotlessApply --- .../frc/robot/constants/LEDConstants.java | 38 ++--- .../robot/subsystems/led/LEDSubsystem.java | 139 ++++++++++-------- 2 files changed, 93 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index a53dacb4..1183d1db 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -3,24 +3,24 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDConstants { - public static final int stripLength = 42; + public static final int stripLength = 42; - public static final Color kAlmostBlack = new Color(0, 0, 1); - // LED Colors - public static final Color kHasAlgea = Color.kAquamarine; - public static final Color kNotHasAlgea = kAlmostBlack; - public static final Color kCoralNotInRobot = Color.kOrange; - public static final Color kCoralInFunnel = Color.kHotPink; - public static final Color kCoralInRobot = Color.kWhite; - public static final Color kCurrentLimiting = Color.kRed; - public static final Color kAutoPlacing = Color.kPurple; - public static final Color kManual = kAlmostBlack; - public static final Color kRight = Color.kSaddleBrown; - public static final Color kLeft = Color.kGreenYellow; - public static final Color kL1 = Color.kBlue; - public static final Color kL2 = Color.kGreen; - public static final Color kL3 = Color.kYellow; - public static final Color kL4 = Color.kRed; - public static final Color kGettingAlgea = Color.kAquamarine; - public static final Color kNotGettingAlgea = kAlmostBlack; + public static final Color kAlmostBlack = new Color(0, 0, 1); + // LED Colors + public static final Color kHasAlgea = Color.kAquamarine; + public static final Color kNotHasAlgea = kAlmostBlack; + public static final Color kCoralNotInRobot = Color.kOrange; + public static final Color kCoralInFunnel = Color.kHotPink; + public static final Color kCoralInRobot = Color.kWhite; + public static final Color kCurrentLimiting = Color.kRed; + public static final Color kAutoPlacing = Color.kPurple; + public static final Color kManual = kAlmostBlack; + public static final Color kRight = Color.kSaddleBrown; + public static final Color kLeft = Color.kGreenYellow; + public static final Color kL1 = Color.kBlue; + public static final Color kL2 = Color.kGreen; + public static final Color kL3 = Color.kYellow; + public static final Color kL4 = Color.kRed; + public static final Color kGettingAlgea = Color.kAquamarine; + public static final Color kNotGettingAlgea = kAlmostBlack; } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index e45a5b44..426f4616 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -2,84 +2,93 @@ import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.constants.LEDConstants; import java.util.Map; import java.util.Set; - import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.util.Color; -import frc.robot.constants.LEDConstants; +public class LEDSubsystem extends MeasurableSubsystem { + private LEDIO io; + private LEDPattern base = LEDPattern.solid(Color.kBlack); + private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + private LEDPattern algea = + LEDPattern.steps( + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); -public class LEDSubsystem extends MeasurableSubsystem{ - private LEDIO io; - private LEDPattern base = LEDPattern.solid(Color.kBlack); - private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); - private LEDPattern algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); + private LEDPattern currentLimiting = + LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); + private LEDPattern autoplace = + LEDPattern.steps(Map.of(LEDConstants.stripLength / 3 * 2, LEDConstants.kAutoPlacing)) + .blink(Seconds.of(0.25)); + public LEDStates currState = LEDStates.OFF; + public CoralStates coralState = CoralStates.NO_PIECE; + public boolean autoPlacing = false; + public boolean hasAlgae = false; + public boolean isLimiting = false; - private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); - private LEDPattern autoplace = LEDPattern.steps(Map.of(LEDConstants.stripLength/3*2, LEDConstants.kAutoPlacing)).blink(Seconds.of(0.25)); - public LEDStates currState = LEDStates.OFF; - public CoralStates coralState = CoralStates.NO_PIECE; - public boolean autoPlacing = false; - public boolean hasAlgae = false; - public boolean isLimiting = false; - - public LEDSubsystem(LEDIO io) { - this.io = io; - } + public LEDSubsystem(LEDIO io) { + this.io = io; + } - private void buildBase() { - switch (coralState) { - case IN_FUNNEL: - coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); - break; - case IN_ROBOT: - coral = LEDPattern.solid(LEDConstants.kCoralInRobot); - break; - case NO_PIECE: - coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); - break; - } - algea = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength/3, Color.kBlack)); - + private void buildBase() { + switch (coralState) { + case IN_FUNNEL: + coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); + break; + case IN_ROBOT: + coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + break; + case NO_PIECE: + coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); + break; } + algea = + LEDPattern.steps( + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); + } - public void periodic() { - switch (currState) { - case OFF: - break; - case NORMAL: - if (autoPlacing) { - base = autoplace.overlayOn(base); - } - if (isLimiting) { - base = currentLimiting.overlayOn(base); - } - io.setStrip(base); - break; - case CLIMB: - break; + public void periodic() { + switch (currState) { + case OFF: + break; + case NORMAL: + if (autoPlacing) { + base = autoplace.overlayOn(base); + } + if (isLimiting) { + base = currentLimiting.overlayOn(base); } - io.updateLEDs(); + io.setStrip(base); + break; + case CLIMB: + break; } + io.updateLEDs(); + } - @Override - public Set getMeasures() { - return Set.of(new Measure("State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), - new Measure("CoralState", "the current Coral state of the LEDSubsystem", () -> coralState.ordinal())); - } + @Override + public Set getMeasures() { + return Set.of( + new Measure( + "State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), + new Measure( + "CoralState", + "the current Coral state of the LEDSubsystem", + () -> coralState.ordinal())); + } - public enum LEDStates { - OFF, - NORMAL, - CLIMB - } + public enum LEDStates { + OFF, + NORMAL, + CLIMB + } - public enum CoralStates { - IN_FUNNEL, - IN_ROBOT, - NO_PIECE, - } + public enum CoralStates { + IN_FUNNEL, + IN_ROBOT, + NO_PIECE, + } } From e8132ea0819c19db36af7c0d6e37516289b0c48e Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Fri, 24 Jan 2025 20:19:55 -0500 Subject: [PATCH 07/48] made Leds work on normal mode --- .../frc/robot/constants/LEDConstants.java | 30 ++- .../java/frc/robot/subsystems/led/LEDIO.java | 4 +- .../robot/subsystems/led/LEDSubsystem.java | 174 ++++++++++++++++-- 3 files changed, 181 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 1183d1db..166ac277 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -3,24 +3,36 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDConstants { - public static final int stripLength = 42; + public static final int kStripLength = 42; + + public static final int kAlgeaEnd = kStripLength / 3; + public static final int kLevelStart = kStripLength / 3; + public static final int kPlaceStart = kStripLength / 9 * 7; + public static final int kGetAlgeaStart = kStripLength / 9 * 8; public static final Color kAlmostBlack = new Color(0, 0, 1); - // LED Colors + // Normal LED Colors public static final Color kHasAlgea = Color.kAquamarine; public static final Color kNotHasAlgea = kAlmostBlack; + public static final Color kCoralNotInRobot = Color.kOrange; public static final Color kCoralInFunnel = Color.kHotPink; public static final Color kCoralInRobot = Color.kWhite; - public static final Color kCurrentLimiting = Color.kRed; - public static final Color kAutoPlacing = Color.kPurple; - public static final Color kManual = kAlmostBlack; - public static final Color kRight = Color.kSaddleBrown; - public static final Color kLeft = Color.kGreenYellow; + public static final Color kL1 = Color.kBlue; public static final Color kL2 = Color.kGreen; public static final Color kL3 = Color.kYellow; public static final Color kL4 = Color.kRed; - public static final Color kGettingAlgea = Color.kAquamarine; - public static final Color kNotGettingAlgea = kAlmostBlack; + + public static final Color kManual = kAlmostBlack; + public static final Color kRight = Color.kSaddleBrown; + public static final Color kLeft = Color.kGreenYellow; + + public static final Color kGetAlgea = Color.kAquamarine; + public static final Color kNotGetAlgea = kAlmostBlack; + + public static final Color kCurrentLimiting = Color.kRed; + + public static final Color kAutoPlacing = Color.kPurple; + // Climb LED Colors } diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index bc02f8d3..70efe5a4 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDIO { - public AddressableLED ledBase; - public AddressableLEDBuffer led; + private AddressableLED ledBase; + private AddressableLEDBuffer led; public LEDIO(int port, int length) { ledBase = new AddressableLED(port); diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 426f4616..0fb29bee 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -13,27 +13,130 @@ public class LEDSubsystem extends MeasurableSubsystem { private LEDIO io; private LEDPattern base = LEDPattern.solid(Color.kBlack); - private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + public LEDStates currState = LEDStates.OFF; + + // section patterns private LEDPattern algea = LEDPattern.steps( - Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); - - private LEDPattern currentLimiting = - LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kStripLength / 3, Color.kBlack)); + private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + private LEDPattern level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); + private LEDPattern place = + LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kManual)); + private LEDPattern getAlgea = + LEDPattern.steps(Map.of(LEDConstants.kGetAlgeaStart, LEDConstants.kNotGetAlgea)); private LEDPattern autoplace = - LEDPattern.steps(Map.of(LEDConstants.stripLength / 3 * 2, LEDConstants.kAutoPlacing)) + LEDPattern.steps(Map.of(LEDConstants.kStripLength / 3 * 2, LEDConstants.kAutoPlacing)) .blink(Seconds.of(0.25)); - public LEDStates currState = LEDStates.OFF; - public CoralStates coralState = CoralStates.NO_PIECE; - public boolean autoPlacing = false; - public boolean hasAlgae = false; - public boolean isLimiting = false; + private LEDPattern currentLimiting = + LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); + + // section booleans and states + private boolean hasAlgae = false; + private CoralStates coralState = CoralStates.NO_PIECE; + private LevelStates levelState = LevelStates.L1; + private PlaceStates placeState = PlaceStates.MANUAL; + private boolean shouldGetAlgea = false; + private boolean autoPlacing = false; + private boolean isLimiting = false; public LEDSubsystem(LEDIO io) { this.io = io; } + public void setState(LEDStates state) { + currState = state; + switch (currState) { + case OFF: + io.setOff(); + break; + case NORMAL: + buildBase(); + break; + case CLIMB: + break; + default: + break; + } + } + + public LEDStates getState() { + return currState; + } + + // setters + public void setAlgeaLights(boolean on) { + hasAlgae = on; + buildBase(); + } + + public void setCoralLights(CoralStates state) { + coralState = state; + buildBase(); + } + + public void setLevelLights(LevelStates state) { + levelState = state; + buildBase(); + } + + public void setPlaceLights(PlaceStates state) { + placeState = state; + buildBase(); + } + + public void setGetAlgeaLights(boolean on) { + shouldGetAlgea = on; + buildBase(); + } + + public void setAutoPlacing(boolean isAutoPlacing) { + autoPlacing = isAutoPlacing; + buildBase(); + } + + public void setCurrentLimiting(boolean isCurrentLimiting) { + this.isLimiting = isCurrentLimiting; + buildBase(); + } + + // getters + public boolean getAlgeaLights() { + return hasAlgae; + } + + public CoralStates getCoralLights() { + return coralState; + } + + public LevelStates getLevelLights() { + return levelState; + } + + public PlaceStates getPlaceLights() { + return placeState; + } + + public boolean getGetAlgeaLights() { + return shouldGetAlgea; + } + + public boolean getAutoPlacing() { + return autoPlacing; + } + + public boolean getCurrentLimiting() { + return isLimiting; + } + private void buildBase() { + algea = + LEDPattern.steps( + Map.of( + 0, + hasAlgae ? LEDConstants.kHasAlgea : LEDConstants.kNotHasAlgea, + LEDConstants.kStripLength / 3, + Color.kBlack)); switch (coralState) { case IN_FUNNEL: coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); @@ -45,9 +148,34 @@ private void buildBase() { coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); break; } - algea = + switch (levelState) { + case L1: + level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); + break; + case L2: + level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL2)); + break; + case L3: + level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL3)); + break; + case L4: + level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL4)); + break; + } + switch (placeState) { + case MANUAL: + place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kManual)); + case LEFT: + place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kLeft)); + case RIGHT: + place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kRight)); + } + getAlgea = LEDPattern.steps( - Map.of(0, LEDConstants.kHasAlgea, LEDConstants.stripLength / 3, Color.kBlack)); + Map.of( + LEDConstants.kGetAlgeaStart, + shouldGetAlgea ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea)); + base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(algea.overlayOn(coral)))); } public void periodic() { @@ -55,11 +183,12 @@ public void periodic() { case OFF: break; case NORMAL: + LEDPattern output = base; if (autoPlacing) { - base = autoplace.overlayOn(base); + output = autoplace.overlayOn(output); } if (isLimiting) { - base = currentLimiting.overlayOn(base); + output = currentLimiting.overlayOn(output); } io.setStrip(base); break; @@ -89,6 +218,19 @@ public enum LEDStates { public enum CoralStates { IN_FUNNEL, IN_ROBOT, - NO_PIECE, + NO_PIECE + } + + public enum LevelStates { + L1, + L2, + L3, + L4 + } + + public enum PlaceStates { + MANUAL, + LEFT, + RIGHT } } From 1d4f932d2b2d0719477621ce45fe36fb4efaf8b2 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 27 Jan 2025 20:44:59 -0500 Subject: [PATCH 08/48] added tons of setpoints --- src/main/deploy/choreo/2025-project.chor | 194 ++++++++++++++++++++++- src/main/deploy/choreo/KTofetch.traj | 62 ++++++++ src/main/deploy/choreo/New Path.traj | 28 ---- src/main/deploy/choreo/constants.traj | 41 +++++ src/main/deploy/choreo/startToI.traj | 69 ++++++++ src/main/deploy/choreo/startToJ.traj | 70 ++++++++ src/main/deploy/choreo/startToK.traj | 113 +++++++++++++ 7 files changed, 547 insertions(+), 30 deletions(-) create mode 100644 src/main/deploy/choreo/KTofetch.traj delete mode 100644 src/main/deploy/choreo/New Path.traj create mode 100644 src/main/deploy/choreo/constants.traj create mode 100644 src/main/deploy/choreo/startToI.traj create mode 100644 src/main/deploy/choreo/startToJ.traj create mode 100644 src/main/deploy/choreo/startToK.traj diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 1b20df9d..fb047720 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -3,9 +3,199 @@ "version":1, "type":"Swerve", "variables":{ - "expressions":{}, + "expressions":{ + "endEffectorOffset":{ + "dimension":"Number", + "var":{ + "exp":"9.408839", + "val":9.408839 + } + } + }, "poses":{ - "startpos":{ + "A":{ + "x":{ + "exp":"126.0625 in", + "val":3.2019875 + }, + "y":{ + "exp":"174.378839 in", + "val":4.4292225106 + }, + "heading":{ + "exp":"0 rad", + "val":0.0 + } + }, + "B":{ + "x":{ + "exp":"126.0625 in", + "val":3.2019875 + }, + "y":{ + "exp":"161.438839 in", + "val":4.1005465106 + }, + "heading":{ + "exp":"0 rad", + "val":0.0 + } + }, + "C":{ + "x":{ + "exp":"137.65477204339692 in", + "val":3.4964312099022816 + }, + "y":{ + "exp":"122.54275684567627 in", + "val":3.1125860238801772 + }, + "heading":{ + "exp":"60 deg", + "val":1.0471975511965976 + } + }, + "D":{ + "x":{ + "exp":"148.86114076836753 in", + "val":3.781072975516535 + }, + "y":{ + "exp":"116.07275684567627 in", + "val":2.948248023880177 + }, + "heading":{ + "exp":"60 deg", + "val":1.0471975511965976 + } + }, + "E":{ + "x":{ + "exp":"188.34227204339692 in", + "val":4.783893709902282 + }, + "y":{ + "exp":"106.66391784567627 in", + "val":2.709263513280177 + }, + "heading":{ + "exp":"120 deg", + "val":2.0943951023931953 + } + }, + "F":{ + "x":{ + "exp":"199.54864076836753 in", + "val":5.068535475516535 + }, + "y":{ + "exp":"113.13391784567627 in", + "val":2.873601513280177 + }, + "heading":{ + "exp":"120 deg", + "val":2.0943951023931953 + } + }, + "G":{ + "x":{ + "exp":"227.4375 in", + "val":5.7769125 + }, + "y":{ + "exp":"142.621161 in", + "val":3.6225774894 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } + }, + "H":{ + "x":{ + "exp":"227.4375 in", + "val":5.7769125 + }, + "y":{ + "exp":"155.561161 in", + "val":3.9512534894 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } + }, + "I":{ + "x":{ + "exp":"215.84522795660308 in", + "val":5.482468790097718 + }, + "y":{ + "exp":"194.45724315432372 in", + "val":4.939213976119822 + }, + "heading":{ + "exp":"-120 deg", + "val":-2.0943951023931953 + } + }, + "J":{ + "x":{ + "exp":"204.63885923163247 in", + "val":5.197827024483464 + }, + "y":{ + "exp":"200.92724315432375 in", + "val":5.103551976119823 + }, + "heading":{ + "exp":"-120 deg", + "val":-2.0943951023931953 + } + }, + "K":{ + "x":{ + "exp":"165.15772795660308 in", + "val":4.1950062900977185 + }, + "y":{ + "exp":"210.33608215432375 in", + "val":5.342536486719823 + }, + "heading":{ + "exp":"-60 deg", + "val":-1.0471975511965976 + } + }, + "L":{ + "x":{ + "exp":"153.95135923163247 in", + "val":3.9103645244834646 + }, + "y":{ + "exp":"203.86608215432372 in", + "val":5.178198486719822 + }, + "heading":{ + "exp":"-60 deg", + "val":-1.0471975511965976 + } + }, + "fetch":{ + "x":{ + "exp":"61.8667 in", + "val":1.57141418 + }, + "y":{ + "exp":"291.9457 in", + "val":7.41542078 + }, + "heading":{ + "exp":"-54.011392 deg", + "val":-0.9426766239853251 + } + }, + "start":{ "x":{ "exp":"7.1008875 m", "val":7.1008875 diff --git a/src/main/deploy/choreo/KTofetch.traj b/src/main/deploy/choreo/KTofetch.traj new file mode 100644 index 00000000..a52cd88d --- /dev/null +++ b/src/main/deploy/choreo/KTofetch.traj @@ -0,0 +1,62 @@ +{ + "name":"KTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "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":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "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.19692], + "samples":[ + {"t":0.0, "x":4.19501, "y":5.34254, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.08578, "ay":6.38853, "alpha":0.62782, "fx":[-135.0894,-138.59076,-140.00364,-136.46362], "fy":[111.73935,107.3692,105.51149,110.0481]}, + {"t":0.04433, "x":4.18706, "y":5.34881, "heading":-1.0472, "vx":-0.35845, "vy":0.28321, "omega":0.02783, "ax":-8.08484, "ay":6.38779, "alpha":0.62181, "fx":[-135.09721,-138.56502,-139.96354,-136.45781], "fy":[111.6975,107.36909,105.52977,110.02143]}, + {"t":0.08866, "x":4.16323, "y":5.36764, "heading":-1.04596, "vx":-0.71685, "vy":0.56638, "omega":0.0554, "ax":-8.08364, "ay":6.38684, "alpha":0.6141, "fx":[-135.10853,-138.53482,-139.91087,-136.44722], "fy":[111.64213,107.36517,105.55471,109.99099]}, + {"t":0.13299, "x":4.12351, "y":5.39903, "heading":-1.04351, "vx":-1.0752, "vy":0.84951, "omega":0.08262, "ax":-8.08203, "ay":6.38556, "alpha":0.60386, "fx":[-135.12427,-138.49632,-139.84004,-136.43118], "fy":[111.56744,107.3576,105.58869,109.95279]}, + {"t":0.17732, "x":4.0679, "y":5.44296, "heading":-1.03985, "vx":-1.43348, "vy":1.13258, "omega":0.10939, "ax":-8.07977, "ay":6.38378, "alpha":0.58959, "fx":[-135.14607,-138.44262,-139.74102,-136.4084], "fy":[111.46301,107.3466,105.63596,109.89974]}, + {"t":0.22165, "x":3.99642, "y":5.49944, "heading":-1.035, "vx":-1.79166, "vy":1.41558, "omega":0.13553, "ax":-8.07637, "ay":6.38111, "alpha":0.56834, "fx":[-135.17699,-138.36003,-139.59394,-136.37619], "fy":[111.30821,107.33245,105.70471,109.81777]}, + {"t":0.26598, "x":3.90906, "y":5.56846, "heading":-1.02899, "vx":-2.14968, "vy":1.69845, "omega":0.16072, "ax":-8.0707, "ay":6.37663, "alpha":0.53337, "fx":[-135.22351,-138.21682,-139.35303,-136.32785], "fy":[111.05551,107.31537,105.81323,109.67463]}, + {"t":0.31031, "x":3.80583, "y":5.65002, "heading":-1.02186, "vx":-2.50746, "vy":1.98113, "omega":0.18436, "ax":-8.05931, "ay":6.36765, "alpha":0.46527, "fx":[-135.30158,-137.91889,-138.88423,-136.24149], "fy":[110.56582,107.29379,106.0117,109.37614]}, + {"t":0.35464, "x":3.68675, "y":5.7441, "heading":-1.01369, "vx":-2.86473, "vy":2.26341, "omega":0.20499, "ax":-8.0249, "ay":6.34051, "alpha":0.27639, "fx":[-135.45065,-137.00926,-137.55649,-135.98842], "fy":[109.18603,107.23609,106.49813,108.48051]}, + {"t":0.39897, "x":3.55187, "y":5.85067, "heading":-1.0046, "vx":-3.22048, "vy":2.54449, "omega":0.21724, "ax":-3.3354, "ay":2.63509, "alpha":-4.89506, "fx":[-63.51845,-40.93333,-51.0611,-71.42416], "fy":[26.8117,44.09236,62.15702,46.22775]}, + {"t":0.4433, "x":3.40583, "y":5.96606, "heading":-0.99497, "vx":-3.36834, "vy":2.6613, "omega":0.00024, "ax":-0.00064, "ay":0.00042, "alpha":-0.00053, "fx":[-0.01133,-0.00916,-0.01057,-0.01273], "fy":[0.00543,0.00684,0.00901,0.0076]}, + {"t":0.48763, "x":3.25651, "y":6.08404, "heading":-0.99496, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":-0.00001, "ay":-0.00001, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":0.53196, "x":3.10719, "y":6.20201, "heading":-0.99495, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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":0.57629, "x":2.95787, "y":6.31999, "heading":-0.99494, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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.62062, "x":2.80855, "y":6.43797, "heading":-0.99493, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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":0.66495, "x":2.65923, "y":6.55594, "heading":-0.99492, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":0.00001, "ay":0.00001, "alpha":0.0, "fx":[0.00009,0.00009,0.00009,0.00009], "fy":[0.00011,0.00011,0.00011,0.00011]}, + {"t":0.70928, "x":2.50991, "y":6.67392, "heading":-0.99491, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":0.00064, "ay":-0.00043, "alpha":0.00053, "fx":[0.01132,0.00915,0.01056,0.01273], "fy":[-0.00544,-0.00685,-0.00902,-0.00761]}, + {"t":0.75361, "x":2.36059, "y":6.7919, "heading":-0.9949, "vx":-3.36834, "vy":2.6613, "omega":0.00024, "ax":3.3354, "ay":-2.63509, "alpha":4.89515, "fx":[63.37291,40.87287,51.20925,71.48177], "fy":[-26.79028,-44.27539,-62.15698,-46.06615]}, + {"t":0.79795, "x":2.21455, "y":6.90729, "heading":-0.99489, "vx":-3.22048, "vy":2.54449, "omega":0.21725, "ax":8.0249, "ay":-6.3405, "alpha":-0.27706, "fx":[135.45815,137.02999,137.54953,135.96714], "fy":[-109.1772,-107.20948,-106.50665,-108.50736]}, + {"t":0.84228, "x":2.07967, "y":7.01385, "heading":-0.98526, "vx":-2.86473, "vy":2.26341, "omega":0.20496, "ax":8.05931, "ay":-6.36765, "alpha":-0.46544, "fx":[135.33433,137.98275,138.85442,136.17469], "fy":[-110.52644,-107.21133,-106.04999,-109.45966]}, + {"t":0.88661, "x":1.96059, "y":7.10793, "heading":-0.97618, "vx":-2.50746, "vy":1.98113, "omega":0.18433, "ax":8.0707, "ay":-6.37663, "alpha":-0.53338, "fx":[135.27906,138.32175,139.30306,136.21735], "fy":[-110.98861,-107.17971,-105.87817,-109.81224]}, + {"t":0.93094, "x":1.85737, "y":7.18949, "heading":-0.968, "vx":-2.14968, "vy":1.69845, "omega":0.16069, "ax":8.07637, "ay":-6.38111, "alpha":-0.56827, "fx":[135.25269,138.50155,139.52628,136.22667], "fy":[-111.21698,-107.1494,-105.79315,-110.00361]}, + {"t":0.97527, "x":1.77, "y":7.25852, "heading":-0.96088, "vx":-1.79166, "vy":1.41558, "omega":0.13549, "ax":8.07977, "ay":-6.38378, "alpha":-0.58948, "fx":[135.23905,138.61544,139.65823,136.22543], "fy":[-111.35092,-107.12297,-105.74455,-110.12687]}, + {"t":1.0196, "x":1.69852, "y":7.315, "heading":-0.95487, "vx":-1.43348, "vy":1.13258, "omega":0.10936, "ax":8.08203, "ay":-6.38557, "alpha":-0.60373, "fx":[135.23152,138.6948,139.74478,136.22075], "fy":[-111.43812,-107.10071,-105.71393,-110.21378]}, + {"t":1.06393, "x":1.64291, "y":7.35893, "heading":-0.95003, "vx":-1.0752, "vy":0.84951, "omega":0.0826, "ax":8.08364, "ay":-6.38684, "alpha":-0.61396, "fx":[135.22694,138.75311,139.80587,136.21556], "fy":[-111.49935,-107.08258,-105.69298,-110.27812]}, + {"t":1.10826, "x":1.60319, "y":7.39031, "heading":-0.94637, "vx":-0.71685, "vy":0.56638, "omega":0.05538, "ax":8.08485, "ay":-6.38779, "alpha":-0.62165, "fx":[135.22359,138.79714,139.85161,136.21129], "fy":[-111.5451,-107.06854,-105.67735,-110.32682]}, + {"t":1.15259, "x":1.57936, "y":7.40914, "heading":-0.94391, "vx":-0.35845, "vy":0.28321, "omega":0.02782, "ax":8.08578, "ay":-6.38853, "alpha":-0.62766, "fx":[135.22047,138.83063,139.88764,136.20872], "fy":[-111.58127,-107.05856,-105.66458,-110.36376]}, + {"t":1.19692, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/New Path.traj b/src/main/deploy/choreo/New Path.traj deleted file mode 100644 index 7b2e9ed1..00000000 --- a/src/main/deploy/choreo/New Path.traj +++ /dev/null @@ -1,28 +0,0 @@ -{ - "name":"New Path", - "version":1, - "snapshot":{ - "waypoints":[], - "constraints":[], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"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":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":null, - "waypoints":[], - "samples":[], - "splits":[] - }, - "events":[] -} diff --git a/src/main/deploy/choreo/constants.traj b/src/main/deploy/choreo/constants.traj new file mode 100644 index 00000000..a02bf762 --- /dev/null +++ b/src/main/deploy/choreo/constants.traj @@ -0,0 +1,41 @@ +{ + "name":"constants", + "version":1, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"61.8667 in", "val":1.57141418}, "y":{"exp":"291.9457 in", "val":7.41542078}, "heading":{"exp":"-54.011392 deg", "val":-0.9426766239853251}, "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":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startToI.traj b/src/main/deploy/choreo/startToI.traj new file mode 100644 index 00000000..144e1aae --- /dev/null +++ b/src/main/deploy/choreo/startToI.traj @@ -0,0 +1,69 @@ +{ + "name":"startToI", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.0711771011352536, "y":5.008563137054443, "heading":-2.0943951023931953, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "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.47831,0.81778], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.68422, "ay":-0.36994, "alpha":21.53352, "fx":[-122.28034,-135.32501,-168.38311,-164.87608], "fy":[-125.15111,110.83102,48.516,-59.36616]}, + {"t":0.02174, "x":7.09884, "y":5.07559, "heading":3.14159, "vx":-0.18881, "vy":-0.00804, "omega":0.46817, "ax":-8.7789, "ay":-0.38206, "alpha":20.8178, "fx":[-125.27686,-138.23513,-168.64407,-165.1502], "fy":[-122.10181,107.11956,47.52888,-58.54186]}, + {"t":0.04348, "x":7.09266, "y":5.07533, "heading":-3.13141, "vx":-0.37967, "vy":-0.01635, "omega":0.92078, "ax":-8.88101, "ay":-0.41033, "alpha":20.02147, "fx":[-128.63718,-141.25931,-169.11417,-165.24321], "fy":[-118.49076,103.03895,45.74272,-58.20961]}, + {"t":0.06522, "x":7.0823, "y":5.07487, "heading":-3.1114, "vx":-0.57276, "vy":-0.02527, "omega":1.35607, "ax":-8.99354, "ay":-0.44993, "alpha":19.10895, "fx":[-132.5319,-144.41829,-169.76182,-165.19813], "fy":[-114.03242,98.50092,43.17228,-58.25353]}, + {"t":0.08697, "x":7.06772, "y":5.07422, "heading":-3.08191, "vx":-0.76829, "vy":-0.03505, "omega":1.77152, "ax":-9.12016, "ay":-0.49503, "alpha":18.03048, "fx":[-137.14424,-147.76596,-170.54593,-165.06927], "fy":[-108.33096,93.33717,39.82752,-58.5149]}, + {"t":0.10871, "x":7.04886, "y":5.07334, "heading":-3.0434, "vx":-0.96657, "vy":-0.04582, "omega":2.16353, "ax":-9.26432, "ay":-0.53845, "alpha":16.72705, "fx":[-142.6162,-151.36879,-171.41604,-164.93257], "fy":[-100.87576,87.29589,35.71495,-58.77087]}, + {"t":0.13045, "x":7.02566, "y":5.07222, "heading":-2.99636, "vx":-1.16799, "vy":-0.05752, "omega":2.5272, "ax":-9.4276, "ay":-0.57195, "alpha":15.14014, "fx":[-148.94721,-155.28304,-172.31234,-164.9002], "fy":[-91.0847,80.02923,30.83798,-58.69723]}, + {"t":0.15219, "x":6.99804, "y":5.07083, "heading":-2.94141, "vx":-1.37296, "vy":-0.06996, "omega":2.85637, "ax":-9.60745, "ay":-0.58804, "alpha":13.22675, "fx":[-155.8463,-159.52782,-173.16553,-165.14006], "fy":[-78.46285,71.05898,25.19389,-57.79965]}, + {"t":0.17393, "x":6.96592, "y":5.06917, "heading":-2.87931, "vx":-1.58184, "vy":-0.08274, "omega":3.14393, "ax":-9.79519, "ay":-0.58441, "alpha":10.96972, "fx":[-162.61678,-164.04307,-173.89575,-165.8976], "fy":[-62.94649,59.70004,18.76152,-55.27735]}, + {"t":0.19567, "x":6.92921, "y":5.06723, "heading":-2.81096, "vx":-1.7948, "vy":-0.09545, "omega":3.38243, "ax":-9.97623, "ay":-0.5691, "alpha":8.34921, "fx":[-168.26781,-168.59767,-174.40708,-167.49877], "fy":[-45.3634,44.90952,11.46576,-49.73268]}, + {"t":0.21741, "x":6.88783, "y":5.06502, "heading":-2.73742, "vx":-2.0117, "vy":-0.10782, "omega":3.56395, "ax":-10.13132, "ay":-0.55932, "alpha":5.221, "fx":[-171.99219,-172.5468,-174.56501,-170.21925], "fy":[-27.63164,25.01849,3.07701,-38.51947]}, + {"t":0.23915, "x":6.8417, "y":5.06255, "heading":-2.65994, "vx":-2.23197, "vy":-0.11998, "omega":3.67747, "ax":-10.22099, "ay":-0.56388, "alpha":1.07267, "fx":[-173.71341,-174.16021,-174.10108,-173.44961], "fy":[-12.26794,-2.63369,-7.12254,-16.34188]}, + {"t":0.2609, "x":6.79076, "y":5.0598, "heading":-2.57998, "vx":-2.45418, "vy":-0.13224, "omega":3.70079, "ax":-10.09537, "ay":-0.56874, "alpha":-5.22766, "fx":[-174.10918,-168.91891,-172.05728,-171.79176], "fy":[-1.44465,-41.56803,-21.91626,26.23222]}, + {"t":0.28264, "x":6.73502, "y":5.0568, "heading":-2.49952, "vx":-2.67367, "vy":-0.14461, "omega":3.58713, "ax":-9.21816, "ay":-0.85605, "alpha":-14.97646, "fx":[-174.05392,-147.43853,-158.46903,-147.23161], "fy":[3.56823,-91.88415,-61.49257,91.56372]}, + {"t":0.30438, "x":6.67471, "y":5.05345, "heading":-2.42153, "vx":-2.87409, "vy":-0.16322, "omega":3.26152, "ax":-8.3616, "ay":-1.36264, "alpha":-19.80359, "fx":[-173.90451,-131.92003,-130.92643,-132.16281], "fy":[0.47241,-112.51295,-92.50879,111.83709]}, + {"t":0.32612, "x":6.61024, "y":5.04958, "heading":-2.35062, "vx":-3.05588, "vy":-0.19284, "omega":2.83097, "ax":-8.33726, "ay":-0.98447, "alpha":-19.38487, "fx":[-173.47389,-125.58057,-135.99081,-132.21261], "fy":[-4.86053,-118.70833,-54.51021,111.09695]}, + {"t":0.34786, "x":6.54184, "y":5.04515, "heading":-2.28907, "vx":-3.23714, "vy":-0.21425, "omega":2.40951, "ax":-8.35347, "ay":-0.28138, "alpha":-19.06643, "fx":[-172.60575,-121.76339,-138.62767,-135.36366], "fy":[-10.37518,-121.26971,6.54914,105.95083]}, + {"t":0.3696, "x":6.46948, "y":5.04043, "heading":-2.23669, "vx":-3.41876, "vy":-0.22036, "omega":1.99498, "ax":-8.30265, "ay":-0.22338, "alpha":-18.8651, "fx":[-170.92584,-119.3784,-134.96267,-139.63586], "fy":[-16.04334,-121.05974,24.47427,97.43042]}, + {"t":0.39134, "x":6.39319, "y":5.03558, "heading":-2.19331, "vx":-3.59927, "vy":-0.22522, "omega":1.58483, "ax":-8.02053, "ay":-0.83535, "alpha":-18.39026, "fx":[-166.55142,-113.74811,-122.747,-142.66141], "fy":[-23.74949,-119.80984,2.69347,84.02972]}, + {"t":0.41309, "x":6.31304, "y":5.03049, "heading":-2.15886, "vx":-3.77365, "vy":-0.24338, "omega":1.185, "ax":-3.84263, "ay":-4.62884, "alpha":-9.86128, "fx":[-94.93168,-51.11821,-28.16192,-87.23649], "fy":[-76.57492,-109.04856,-86.87386,-42.4437]}, + {"t":0.43483, "x":6.23009, "y":5.02411, "heading":-2.13309, "vx":-3.85719, "vy":-0.34402, "omega":0.9706, "ax":9.48704, "ay":-0.65437, "alpha":-7.41465, "fx":[159.12141,158.0338,167.63881,160.69329], "fy":[-29.47539,-52.48603,-0.27601,37.71481]}, + {"t":0.45657, "x":6.14847, "y":5.01647, "heading":-2.11199, "vx":-3.65093, "vy":-0.35825, "omega":0.8094, "ax":8.81026, "ay":-0.5047, "alpha":-17.73218, "fx":[146.16597,149.35233,172.29618,131.62525], "fy":[-76.71409,-86.74571,18.3563,110.76413]}, + {"t":0.47831, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-3.45938, "vy":-0.36922, "omega":0.42388, "ax":9.97262, "ay":0.5036, "alpha":-7.93764, "fx":[173.416,169.60085,173.54811,161.96046], "fy":[-7.54417,-40.18634,17.99921,63.99577]}, + {"t":0.51226, "x":5.95949, "y":4.99632, "heading":-2.08001, "vx":-3.12084, "vy":-0.35212, "omega":0.15442, "ax":10.16399, "ay":0.85565, "alpha":-4.09773, "fx":[174.28446,174.25611,173.68105,169.32472], "fy":[8.25607,-12.05422,19.32854,42.68677]}, + {"t":0.5462, "x":5.8594, "y":4.98486, "heading":-2.07476, "vx":-2.77581, "vy":-0.32308, "omega":0.01531, "ax":10.21006, "ay":1.01231, "alpha":-2.23742, "fx":[174.2243,174.8557,173.78328,171.81734], "fy":[14.20184,2.41609,19.81848,32.43971]}, + {"t":0.58015, "x":5.77105, "y":4.97447, "heading":-2.07424, "vx":-2.42921, "vy":-0.28871, "omega":-0.06064, "ax":10.22321, "ay":1.1019, "alpha":-1.14657, "fx":[174.10744,174.64275,173.85749,172.9679], "fy":[17.32573,11.08552,20.05778,26.50327]}, + {"t":0.6141, "x":5.69448, "y":4.96531, "heading":-2.0763, "vx":-2.08216, "vy":-0.25131, "omega":-0.09956, "ax":10.22631, "ay":1.15992, "alpha":-0.43183, "fx":[174.00819,174.26507,173.91042,173.60294], "fy":[19.23215,16.82931,20.21405,22.64427]}, + {"t":0.64804, "x":5.62969, "y":4.95745, "heading":-2.07968, "vx":-1.73501, "vy":-0.21193, "omega":-0.11422, "ax":10.22588, "ay":1.20052, "alpha":0.07194, "fx":[173.93146,173.88189,173.9479,173.99606], "fy":[20.4986,20.90569,20.34221,19.93584]}, + {"t":0.68199, "x":5.57668, "y":4.95094, "heading":-2.08356, "vx":-1.38787, "vy":-0.17118, "omega":-0.11178, "ax":10.22416, "ay":1.23051, "alpha":0.44583, "fx":[173.87308,173.53356,173.97435,174.25949], "fy":[21.38841,23.9451,20.45992,17.92916]}, + {"t":0.71594, "x":5.53546, "y":4.94584, "heading":-2.08735, "vx":-1.0408, "vy":-0.12941, "omega":-0.09665, "ax":10.22203, "ay":1.25355, "alpha":0.73417, "fx":[173.82833,173.22714,173.99315,174.44647], "fy":[22.04087,26.29661,20.57101,16.38191]}, + {"t":0.74988, "x":5.50602, "y":4.94217, "heading":-2.09064, "vx":-0.69379, "vy":-0.08685, "omega":-0.07172, "ax":10.21983, "ay":1.27181, "alpha":0.96324, "fx":[173.79329,172.96005,174.00685,174.58507], "fy":[22.5376,28.16863,20.67392,15.1523]}, + {"t":0.78383, "x":5.48836, "y":4.93996, "heading":-2.09307, "vx":-0.34686, "vy":-0.04368, "omega":-0.03902, "ax":10.21771, "ay":1.28663, "alpha":1.14957, "fx":[173.76492,172.72729,174.01745,174.69133], "fy":[22.93021,29.69322,20.76504,14.15204]}, + {"t":0.81778, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startToJ.traj b/src/main/deploy/choreo/startToJ.traj new file mode 100644 index 00000000..90fd306d --- /dev/null +++ b/src/main/deploy/choreo/startToJ.traj @@ -0,0 +1,70 @@ +{ + "name":"startToJ", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.123365116119384, "y":5.0878586769104, "heading":-2.0943951023931953, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.123365116119384 m", "val":6.123365116119384}, "y":{"exp":"5.0878586769104 m", "val":5.0878586769104}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "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.46112,0.8859], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.45182, "ay":0.06208, "alpha":23.24675, "fx":[-122.66991,-120.28781,-165.72569,-166.36902], "fy":[-124.71991,127.03341,56.9578,-55.04746]}, + {"t":0.02196, "x":7.09885, "y":5.07569, "heading":3.14159, "vx":-0.18558, "vy":0.00136, "omega":0.51045, "ax":-8.58524, "ay":0.06601, "alpha":22.30431, "fx":[-126.86884,-124.44934,-166.08049,-166.731], "fy":[-120.39547,122.91046,55.85584,-53.8797]}, + {"t":0.04392, "x":7.09271, "y":5.07574, "heading":-3.13038, "vx":-0.3741, "vy":0.00281, "omega":1.0002, "ax":-8.73278, "ay":0.0518, "alpha":21.22148, "fx":[-131.54948,-128.97852,-166.72246,-166.91798], "fy":[-115.19394,118.10143,53.83968,-53.22275]}, + {"t":0.06587, "x":7.08239, "y":5.07581, "heading":-3.10842, "vx":-0.56585, "vy":0.00395, "omega":1.46618, "ax":-8.89659, "ay":0.02756, "alpha":19.95925, "fx":[-136.89934,-133.82204,-167.60481,-166.98786], "fy":[-108.69082,112.53617,50.94112,-52.9114]}, + {"t":0.08783, "x":7.06782, "y":5.07591, "heading":-3.07623, "vx":-0.7612, "vy":0.00456, "omega":1.90444, "ax":-9.07861, "ay":0.00304, "alpha":18.46765, "fx":[-143.05245,-138.96515,-168.66798,-167.01281], "fy":[-100.33627,106.06968,47.19252,-52.71909]}, + {"t":0.10979, "x":7.04891, "y":5.07601, "heading":-3.03441, "vx":-0.96055, "vy":0.00462, "omega":2.30995, "ax":-9.27869, "ay":-0.0109, "alpha":16.69878, "fx":[-149.97041,-144.40899,-169.84121,-167.09054], "fy":[-89.5158,98.4725,42.63059,-52.32896]}, + {"t":0.13175, "x":7.02558, "y":5.07611, "heading":-2.98369, "vx":-1.16429, "vy":0.00438, "omega":2.67662, "ax":-9.49203, "ay":-0.00466, "alpha":14.62808, "fx":[-157.28184,-150.14316,-171.04463,-167.35696], "fy":[-75.74762,89.41452,37.29988,-51.28396]}, + {"t":0.15371, "x":6.99773, "y":5.0762, "heading":-2.92492, "vx":-1.37271, "vy":0.00428, "omega":2.99783, "ax":-9.7075, "ay":0.0251, "alpha":12.2734, "fx":[-164.18724,-156.1127,-172.19193,-167.99542], "fy":[-59.07854,78.42939,31.25404,-48.89723]}, + {"t":0.17566, "x":6.96525, "y":5.0763, "heading":-2.85909, "vx":-1.58587, "vy":0.00483, "omega":3.26732, "ax":-9.90955, "ay":0.07, "alpha":9.6779, "fx":[-169.65105,-162.16644,-173.1933,-169.22358], "fy":[-40.5476,64.84326,24.55154,-44.08464]}, + {"t":0.19762, "x":6.92804, "y":5.07642, "heading":-2.78735, "vx":-1.80346, "vy":0.00637, "omega":3.47983, "ax":-10.08333, "ay":0.11147, "alpha":6.81598, "fx":[-172.96446,-167.94729,-173.95605,-171.19048], "fy":[-22.25679,47.65087,17.24094,-35.05099]}, + {"t":0.21958, "x":6.88601, "y":5.07659, "heading":-2.71094, "vx":-2.02487, "vy":0.00882, "omega":3.62949, "ax":-10.21192, "ay":0.13534, "alpha":3.43822, "fx":[-174.2496,-172.6294,-174.37604,-173.5525], "fy":[-6.68876,25.34189,9.32337,-18.76819]}, + {"t":0.24154, "x":6.83908, "y":5.07682, "heading":-2.63124, "vx":-2.2491, "vy":0.01179, "omega":3.70499, "ax":-10.2443, "ay":0.14832, "alpha":-1.02787, "fx":[-174.3326,-174.30417,-174.29529,-174.07847], "fy":[4.26936,-4.20138,0.63762,9.38567]}, + {"t":0.26349, "x":6.78723, "y":5.07711, "heading":-2.54989, "vx":-2.47404, "vy":0.01504, "omega":3.68242, "ax":-10.02387, "ay":0.14741, "alpha":-7.21869, "fx":[-174.12736,-168.79669,-173.2753,-165.81338], "fy":[9.77426,-43.0777,-9.6761,53.00899]}, + {"t":0.28545, "x":6.73049, "y":5.07748, "heading":-2.46903, "vx":-2.69415, "vy":0.01828, "omega":3.52391, "ax":-9.27764, "ay":-0.0686, "alpha":-14.7523, "fx":[-174.13931,-149.23197,-167.97845,-139.89006], "fy":[9.84685,-89.61083,-28.27975,103.37656]}, + {"t":0.30741, "x":6.66909, "y":5.07786, "heading":-2.39165, "vx":-2.89786, "vy":0.01677, "omega":3.19998, "ax":-8.47821, "ay":-0.12723, "alpha":-19.16578, "fx":[-174.2887,-130.89343,-147.49866,-124.16694], "fy":[4.99497,-114.50693,-20.88342,121.7389]}, + {"t":0.32937, "x":6.60342, "y":5.0782, "heading":-2.32139, "vx":-3.08403, "vy":0.01398, "omega":2.77914, "ax":-8.48871, "ay":0.91996, "alpha":-19.11542, "fx":[-174.19988,-129.50327,-143.39221,-130.46678], "fy":[-0.96144,-115.65549,64.49394,114.71624]}, + {"t":0.35133, "x":6.53365, "y":5.07873, "heading":-2.26036, "vx":-3.27042, "vy":0.03418, "omega":2.35941, "ax":-8.52782, "ay":0.97636, "alpha":-19.13043, "fx":[-173.88369,-129.51581,-140.40159,-136.42194], "fy":[-6.11891,-115.09586,80.45498,107.19041]}, + {"t":0.37328, "x":6.45979, "y":5.07972, "heading":-2.20856, "vx":-3.45767, "vy":0.05562, "omega":1.93935, "ax":-8.52822, "ay":0.91523, "alpha":-19.27527, "fx":[-173.38858,-128.81294,-137.57962,-140.46912], "fy":[-10.55119,-115.17019,86.71551,101.27691]}, + {"t":0.39524, "x":6.38181, "y":5.08116, "heading":-2.16597, "vx":-3.64493, "vy":0.07572, "omega":1.5161, "ax":-8.49445, "ay":0.83247, "alpha":-19.4917, "fx":[-172.721,-127.55028,-134.55559,-143.12568], "fy":[-14.28105,-115.58531,89.86043,96.64626]}, + {"t":0.4172, "x":6.29972, "y":5.08302, "heading":-2.13268, "vx":-3.83145, "vy":0.094, "omega":1.08811, "ax":-8.43072, "ay":0.74853, "alpha":-19.70145, "fx":[-171.84275,-126.01427,-131.10421,-144.65555], "fy":[-17.18734,-115.79137,90.91953,92.98797]}, + {"t":0.43916, "x":6.21356, "y":5.08527, "heading":-2.10879, "vx":-4.01658, "vy":0.11043, "omega":0.65551, "ax":-8.29817, "ay":0.69961, "alpha":-20.02099, "fx":[-170.61102,-124.0897,-125.14642,-144.7509], "fy":[-18.97659,-115.39975,91.49278,90.48411]}, + {"t":0.46112, "x":6.12337, "y":5.08786, "heading":-2.0944, "vx":-4.19879, "vy":0.12579, "omega":0.21589, "ax":5.55257, "ay":-1.69018, "alpha":-6.88086, "fx":[75.65537,95.75004,111.69242,94.69287], "fy":[-42.6886,-54.48597,-18.73214,0.9087]}, + {"t":0.49651, "x":5.97821, "y":5.09125, "heading":-2.08675, "vx":-4.00223, "vy":0.06596, "omega":-0.02768, "ax":10.18229, "ay":-0.21329, "alpha":-0.06999, "fx":[173.1893,173.18971,173.20674,173.20531], "fy":[-3.76448,-4.0836,-3.49223,-3.17167]}, + {"t":0.53191, "x":5.84292, "y":5.09345, "heading":-2.08773, "vx":-3.6418, "vy":0.05841, "omega":-0.03016, "ax":10.24817, "ay":-0.18329, "alpha":0.02564, "fx":[174.3204,174.32104,174.31645,174.31567], "fy":[-3.06814,-2.94927,-3.16721,-3.28589]}, + {"t":0.56731, "x":5.72043, "y":5.09541, "heading":-2.0888, "vx":-3.27903, "vy":0.05193, "omega":-0.02925, "ax":10.27041, "ay":-0.17311, "alpha":0.05861, "fx":[174.70051,174.70258,174.69338,174.69055], "fy":[-2.83205,-2.55845,-3.05741,-3.33001]}, + {"t":0.60271, "x":5.61079, "y":5.09714, "heading":-2.08984, "vx":-2.91547, "vy":0.0458, "omega":-0.02718, "ax":10.28158, "ay":-0.16799, "alpha":0.07533, "fx":[174.89094,174.89399,174.883,174.8787], "fy":[-2.71364,-2.36044,-3.00208,-3.35363]}, + {"t":0.63811, "x":5.51403, "y":5.09865, "heading":-2.0908, "vx":-2.55152, "vy":0.03985, "omega":-0.02451, "ax":10.28828, "ay":-0.16491, "alpha":0.08544, "fx":[175.00526,175.00899,174.99711,174.99176], "fy":[-2.64267,-2.24077,-2.96861,-3.36839]}, + {"t":0.67351, "x":5.43016, "y":5.09996, "heading":-2.09167, "vx":-2.18733, "vy":0.03401, "omega":-0.02149, "ax":10.29276, "ay":-0.16286, "alpha":0.09221, "fx":[175.08149,175.08571,175.07331,175.0672], "fy":[-2.59548,-2.16061,-2.94611,-3.37851]}, + {"t":0.7089, "x":5.35918, "y":5.10106, "heading":-2.09243, "vx":-1.82299, "vy":0.02825, "omega":-0.01822, "ax":10.29596, "ay":-0.16139, "alpha":0.09706, "fx":[175.13594,175.14052,175.12779,175.1211], "fy":[-2.56187,-2.10317,-2.92991,-3.38588]}, + {"t":0.7443, "x":5.3011, "y":5.10196, "heading":-2.09307, "vx":-1.45853, "vy":0.02254, "omega":-0.01479, "ax":10.29836, "ay":-0.16029, "alpha":0.10071, "fx":[175.17677,175.18164,175.16868,175.16154], "fy":[-2.53673,-2.05999,-2.91769,-3.3915]}, + {"t":0.7797, "x":5.25592, "y":5.10266, "heading":-2.0936, "vx":-1.09398, "vy":0.01686, "omega":-0.01122, "ax":10.30023, "ay":-0.15943, "alpha":0.10355, "fx":[175.20852,175.21362,175.2005,175.19299], "fy":[-2.5172,-2.02635,-2.90815,-3.39591]}, + {"t":0.8151, "x":5.22365, "y":5.10316, "heading":-2.09399, "vx":-0.72937, "vy":0.01122, "omega":-0.00756, "ax":10.30172, "ay":-0.15875, "alpha":0.10583, "fx":[175.23392,175.23921,175.22596,175.21816], "fy":[-2.50157,-1.9994,-2.90053,-3.39947]}, + {"t":0.8505, "x":5.20428, "y":5.10345, "heading":-2.09426, "vx":-0.36471, "vy":0.0056, "omega":-0.00381, "ax":10.30294, "ay":-0.15819, "alpha":0.10769, "fx":[175.2547,175.26015,175.2468,175.23875], "fy":[-2.48875,-1.97735,-2.89432,-3.40239]}, + {"t":0.8859, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/startToK.traj b/src/main/deploy/choreo/startToK.traj new file mode 100644 index 00000000..1f9da404 --- /dev/null +++ b/src/main/deploy/choreo/startToK.traj @@ -0,0 +1,113 @@ +{ + "name":"startToK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.174379348754883, "y":6.211442947387695, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.2539963722229, "y":6.749329090118408, "heading":-1.0471975511965976, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.297754287719727, "y":6.199490070343018, "heading":-1.0471975511965976, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.174379348754883 m", "val":6.174379348754883}, "y":{"exp":"6.211442947387695 m", "val":6.211442947387695}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.2539963722229 m", "val":5.2539963722229}, "y":{"exp":"6.749329090118408 m", "val":6.749329090118408}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.297754287719727 m", "val":4.297754287719727}, "y":{"exp":"6.199490070343018 m", "val":6.199490070343018}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "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.60234,0.9311,1.28522,1.71887], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.87395, "ay":5.0951, "alpha":37.07106, "fx":[162.95989,5.95327,-121.202,-175.21246], "fy":[43.23402,175.1303,126.64944,1.65119]}, + {"t":0.0251, "x":7.1003, "y":5.07728, "heading":3.14159, "vx":-0.04703, "vy":0.12787, "omega":0.93039, "ax":-3.19948, "ay":6.6853, "alpha":26.88292, "fx":[76.32017,2.43903,-121.32427,-175.12386], "fy":[148.22829,175.18805,126.50624,4.93757]}, + {"t":0.05019, "x":7.09811, "y":5.0826, "heading":-3.11824, "vx":-0.12733, "vy":0.29566, "omega":1.60508, "ax":-4.31654, "ay":7.00756, "alpha":22.04484, "fx":[7.6521,-3.40404,-122.90665,-175.03392], "fy":[170.12692,175.14347,124.93754,6.57875]}, + {"t":0.07529, "x":7.09355, "y":5.09223, "heading":-3.07796, "vx":-0.23566, "vy":0.47153, "omega":2.15835, "ax":-4.721, "ay":7.00413, "alpha":20.90143, "fx":[-9.813,-11.02609,-125.42755,-174.94468], "fy":[171.78421,174.79654,122.36611,7.6063]}, + {"t":0.10039, "x":7.08615, "y":5.10627, "heading":-3.02379, "vx":-0.35415, "vy":0.64731, "omega":2.68292, "ax":-5.02228, "ay":6.96401, "alpha":20.27222, "fx":[-18.0773,-20.24986,-128.56331,-174.81978], "fy":[171.97537,173.92972,119.01404,8.90409]}, + {"t":0.12549, "x":7.07568, "y":5.1247, "heading":-2.95646, "vx":-0.48019, "vy":0.82209, "omega":3.1917, "ax":-5.36022, "ay":6.90951, "alpha":19.50234, "fx":[-26.84786,-31.18693,-132.08968,-174.57852], "fy":[171.30977,172.25386,115.0144,11.53724]}, + {"t":0.15058, "x":7.06194, "y":5.14751, "heading":-2.87635, "vx":-0.61472, "vy":0.9955, "omega":3.68116, "ax":-5.77572, "ay":6.85507, "alpha":18.23226, "fx":[-39.15196,-44.1216,-135.7167,-173.98301], "fy":[169.21482,169.33246,110.60446,17.25925]}, + {"t":0.17568, "x":7.0447, "y":5.17466, "heading":-2.78397, "vx":-0.75968, "vy":1.16755, "omega":4.13874, "ax":-6.27986, "ay":6.82283, "alpha":16.03166, "fx":[-56.45378,-59.61557,-138.97609,-172.22878], "fy":[164.43411,164.41231,106.31382,29.05732]}, + {"t":0.20078, "x":7.02365, "y":5.20611, "heading":-2.68009, "vx":-0.91729, "vy":1.33878, "omega":4.54109, "ax":-6.85021, "ay":6.83993, "alpha":12.26062, "fx":[-79.59392,-78.85882,-140.94224,-166.68565], "fy":[154.6732,155.91909,103.41143,51.37743]}, + {"t":0.22588, "x":6.99847, "y":5.24186, "heading":-2.56612, "vx":-1.08921, "vy":1.51045, "omega":4.8488, "ax":-7.39475, "ay":6.87643, "alpha":6.14976, "fx":[-108.36587,-104.73156,-139.09746,-150.93542], "fy":[136.13754,139.48964,105.35124,86.88594]}, + {"t":0.25097, "x":6.96881, "y":5.28194, "heading":-2.44443, "vx":-1.2748, "vy":1.68303, "omega":5.00314, "ax":-7.72604, "ay":6.62389, "alpha":-3.40365, "fx":[-139.6609,-144.1275,-121.68667,-120.19567], "fy":[103.85161,97.06085,123.9501,125.81906]}, + {"t":0.27607, "x":6.93438, "y":5.32626, "heading":-2.31887, "vx":-1.4687, "vy":1.84927, "omega":4.91772, "ax":-6.42132, "ay":5.16921, "alpha":-23.85069, "fx":[-162.84141,-169.92741,-13.72107,-90.40911], "fy":[61.52202,-29.84942,171.34171,148.69302]}, + {"t":0.30117, "x":6.8955, "y":5.3743, "heading":-2.19544, "vx":-1.62986, "vy":1.979, "omega":4.31913, "ax":-6.30262, "ay":5.32002, "alpha":-23.67024, "fx":[-163.02809,-170.34663,-3.74021,-91.7079], "fy":[60.19181,-17.96736,172.04646,147.69719]}, + {"t":0.32627, "x":6.85261, "y":5.42564, "heading":-2.08705, "vx":-1.78804, "vy":2.11252, "omega":3.72507, "ax":-6.2415, "ay":5.54236, "alpha":-22.47972, "fx":[-163.05499,-169.4567,0.94367,-93.0967], "fy":[59.0473,-0.57285,172.05859,146.56302]}, + {"t":0.35136, "x":6.80577, "y":5.48041, "heading":-1.99356, "vx":-1.94468, "vy":2.25162, "omega":3.16089, "ax":-6.14038, "ay":5.71681, "alpha":-21.44793, "fx":[-163.48192,-165.84011,5.82653,-94.289], "fy":[56.4247,15.34729,171.74282,145.45044]}, + {"t":0.37646, "x":6.75503, "y":5.53872, "heading":-1.91423, "vx":-2.09879, "vy":2.3951, "omega":2.6226, "ax":-5.99961, "ay":5.85545, "alpha":-20.60076, "fx":[-164.1016,-158.65487,10.09263,-95.54246], "fy":[52.52555,30.62058,171.113,144.13904]}, + {"t":0.40156, "x":6.70046, "y":5.60067, "heading":-1.84841, "vx":-2.24937, "vy":2.54206, "omega":2.10557, "ax":-5.78374, "ay":5.99446, "alpha":-19.91315, "fx":[-164.49972,-145.22616,13.06072,-96.854], "fy":[47.98934,47.20048,170.15644,142.50979]}, + {"t":0.42666, "x":6.64219, "y":5.66636, "heading":-1.79556, "vx":-2.39452, "vy":2.6925, "omega":1.60581, "ax":-5.42097, "ay":6.19025, "alpha":-19.31771, "fx":[-163.92443,-120.56809,13.64597,-97.99012], "fy":[44.24572,67.76599,168.72833,140.43727]}, + {"t":0.45175, "x":6.58038, "y":5.73589, "heading":-1.75526, "vx":-2.53057, "vy":2.84786, "omega":1.12098, "ax":-4.93632, "ay":6.39285, "alpha":-18.70739, "fx":[-160.76705,-87.11462,10.21508,-98.19494], "fy":[43.84788,87.48399,166.03221,137.59826]}, + {"t":0.47685, "x":6.51532, "y":5.80937, "heading":-1.72713, "vx":-2.65446, "vy":3.0083, "omega":0.65147, "ax":-4.38073, "ay":6.30499, "alpha":-17.60353, "fx":[-149.40143,-57.19315,2.76126,-94.22675], "fy":[49.01981,90.30153,157.48955,132.17366]}, + {"t":0.50195, "x":6.44732, "y":5.88686, "heading":-1.71078, "vx":-2.76441, "vy":3.16654, "omega":0.20967, "ax":-0.73342, "ay":3.27025, "alpha":-6.8054, "fx":[-34.49394,3.13123,7.6915,-26.22985], "fy":[42.35856,39.01522,69.45703,71.67299]}, + {"t":0.52705, "x":6.37771, "y":5.96736, "heading":-1.70551, "vx":-2.78282, "vy":3.24862, "omega":0.03887, "ax":0.97227, "ay":0.85014, "alpha":-0.04219, "fx":[16.42395,16.62758,16.65193,16.44835], "fy":[14.37428,14.34348,14.54706,14.5779]}, + {"t":0.55214, "x":6.30817, "y":6.04916, "heading":-1.70454, "vx":-2.75841, "vy":3.26995, "omega":0.03782, "ax":2.94745, "ay":-0.3281, "alpha":4.91756, "fx":[62.62375,41.0179,37.50168,59.39763], "fy":[5.2711,9.29077,-17.43762,-19.44794]}, + {"t":0.57724, "x":6.23987, "y":6.13112, "heading":-1.70359, "vx":-2.68444, "vy":3.26172, "omega":0.16123, "ax":5.96437, "ay":-4.89617, "alpha":17.26723, "fx":[154.6941,111.04268,28.11894,111.95341], "fy":[-28.65223,-37.39613,-151.31014,-115.77141]}, + {"t":0.60234, "x":6.17438, "y":6.21144, "heading":-1.69954, "vx":-2.53475, "vy":3.13884, "omega":0.5946, "ax":3.49947, "ay":-7.21468, "alpha":17.84806, "fx":[148.80184,13.12396,-12.39956,88.57343], "fy":[-61.39132,-126.923,-162.47835,-140.08585]}, + {"t":0.61728, "x":6.13689, "y":6.25754, "heading":-1.69066, "vx":-2.48246, "vy":3.03102, "omega":0.86131, "ax":1.05254, "ay":-8.56164, "alpha":16.66695, "fx":[113.2785,-59.26755,-38.02411,55.62689], "fy":[-116.63566,-144.45554,-162.87246,-158.56045]}, + {"t":0.63222, "x":6.09991, "y":6.30188, "heading":-1.67779, "vx":-2.46673, "vy":2.90308, "omega":1.11038, "ax":-0.34641, "ay":-8.99662, "alpha":15.07928, "fx":[79.14707,-84.21369,-53.01755,34.51476], "fy":[-144.57461,-140.41836,-161.00702,-166.11984]}, + {"t":0.64717, "x":6.06301, "y":6.34426, "heading":-1.66119, "vx":-2.4719, "vy":2.76864, "omega":1.33572, "ax":-1.26779, "ay":-9.17598, "alpha":13.57443, "fx":[51.29856,-96.11172,-61.81217,20.36658], "fy":[-158.64338,-136.8424,-159.2815,-169.55562]}, + {"t":0.66211, "x":6.02593, "y":6.38461, "heading":-1.64123, "vx":-2.49085, "vy":2.63152, "omega":1.53857, "ax":-1.91509, "ay":-9.26391, "alpha":12.1785, "fx":[28.54278,-101.91288,-66.95836,10.02805], "fy":[-165.8196,-135.09494,-158.13612,-171.25545]}, + {"t":0.67706, "x":5.98849, "y":6.4229, "heading":-1.61824, "vx":-2.51947, "vy":2.49308, "omega":1.72056, "ax":-2.38887, "ay":-9.31642, "alpha":10.86504, "fx":[9.6228,-104.16661,-69.80279,1.81037], "fy":[-169.20722,-135.00404,-157.56358,-172.10372]}, + {"t":0.692, "x":5.95058, "y":6.45911, "heading":-1.59253, "vx":-2.55517, "vy":2.35386, "omega":1.88292, "ax":-2.74131, "ay":-9.35566, "alpha":9.61336, "fx":[-6.19819,-104.021,-71.07533,-5.22138], "fy":[-170.35819,-136.25296,-157.47618,-172.46127]}, + {"t":0.70694, "x":5.91209, "y":6.49324, "heading":-1.56439, "vx":-2.59613, "vy":2.21405, "omega":2.02658, "ax":-3.00171, "ay":-9.39129, "alpha":8.41176, "fx":[-19.31447,-102.09391,-71.21513,-11.60973], "fy":[-170.18744,-138.52514,-157.77594,-172.4838]}, + {"t":0.72189, "x":5.87296, "y":6.52528, "heading":-1.53411, "vx":-2.64099, "vy":2.07371, "omega":2.15228, "ax":-3.18887, "ay":-9.42739, "alpha":7.25275, "fx":[-29.98399,-98.78771,-70.52218,-17.67321], "fy":[-169.30822,-141.51566,-158.36673,-172.2383]}, + {"t":0.73683, "x":5.83314, "y":6.55522, "heading":-1.50194, "vx":-2.68864, "vy":1.93283, "omega":2.26066, "ax":-3.31674, "ay":-9.46521, "alpha":6.12968, "fx":[-38.44767,-94.40548,-69.22309,-23.59079], "fy":[-168.14595,-144.94531,-159.1591,-171.7519]}, + {"t":0.75177, "x":5.79259, "y":6.58305, "heading":-1.46816, "vx":-2.7382, "vy":1.79139, "omega":2.35226, "ax":-3.39662, "ay":-9.50442, "alpha":5.03595, "fx":[-44.96464,-89.18936,-67.49785,-29.45018], "fy":[-166.98176,-148.57837,-160.07473,-171.03517]}, + {"t":0.76672, "x":5.75129, "y":6.60875, "heading":-1.43301, "vx":-2.78896, "vy":1.64936, "omega":2.42752, "ax":-3.4377, "ay":-9.544, "alpha":3.96503, "fx":[-49.80131,-83.32649,-65.48904,-35.28023], "fy":[-165.98347,-152.23498,-161.0502,-170.09396]}, + {"t":0.78166, "x":5.70923, "y":6.63234, "heading":-1.39673, "vx":-2.84033, "vy":1.50673, "omega":2.48677, "ax":-3.44608, "ay":-9.5833, "alpha":2.90455, "fx":[-53.2011,-76.89237,-63.28111,-41.09304], "fy":[-165.23974,-155.81863,-162.04774,-168.93063]}, + {"t":0.7966, "x":5.6664, "y":6.65378, "heading":-1.35957, "vx":-2.89183, "vy":1.36353, "omega":2.53018, "ax":-3.41532, "ay":-9.62633, "alpha":1.78118, "fx":[-55.2766,-69.34063,-60.67631,-47.08067], "fy":[-164.82323,-159.51524,-163.13402,-167.49207]}, + {"t":0.81155, "x":5.6228, "y":6.67308, "heading":-1.32176, "vx":-2.94287, "vy":1.21967, "omega":2.55679, "ax":-3.27538, "ay":-9.69644, "alpha":0.17909, "fx":[-55.46686,-56.84764,-55.95777,-54.58116], "fy":[-165.00894,-164.54635,-164.86316,-165.31626]}, + {"t":0.82649, "x":5.57846, "y":6.69023, "heading":-1.28355, "vx":-2.99182, "vy":1.07477, "omega":2.55947, "ax":-2.91165, "ay":-9.79367, "alpha":-2.48151, "fx":[-52.7641,-33.86933,-45.85782,-65.61414], "fy":[-166.14364,-170.91748,-167.92863,-161.36012]}, + {"t":0.84144, "x":5.53343, "y":6.70519, "heading":-1.24531, "vx":-3.03533, "vy":0.92842, "omega":2.52239, "ax":-2.45642, "ay":-9.83088, "alpha":-5.35817, "fx":[-48.70348,-8.53002,-32.5503,-77.34845], "fy":[-167.58095,-174.17717,-170.94085,-156.1832]}, + {"t":0.85638, "x":5.48779, "y":6.71797, "heading":-1.20761, "vx":-3.07203, "vy":0.78151, "omega":2.44232, "ax":-2.01031, "ay":-9.79813, "alpha":-7.89639, "fx":[-44.39978,13.56846,-18.34906,-87.59881], "fy":[-168.92172,-173.98478,-172.96095,-150.78626]}, + {"t":0.87132, "x":5.44166, "y":6.72855, "heading":-1.17112, "vx":-3.10207, "vy":0.63509, "omega":2.32432, "ax":-1.59079, "ay":-9.71964, "alpha":-10.04238, "fx":[-40.13709,31.84904,-4.00064,-95.94709], "fy":[-170.09717,-171.69083,-173.79509,-145.72983]}, + {"t":0.88627, "x":5.39513, "y":6.73696, "heading":-1.13638, "vx":-3.12585, "vy":0.48984, "omega":2.17425, "ax":-1.19665, "ay":-9.61402, "alpha":-11.8545, "fx":[-36.01742,46.87817,10.25269,-102.53225], "fy":[-171.10341,-168.30183,-173.4391,-141.28283]}, + {"t":0.90121, "x":5.34828, "y":6.74321, "heading":-1.10389, "vx":-3.14373, "vy":0.34618, "omega":1.9971, "ax":-0.82358, "ay":-9.49145, "alpha":-13.40956, "fx":[-32.10075,59.31968,24.40184,-107.65616], "fy":[-171.94976,-164.40807,-171.90981,-137.51945]}, + {"t":0.91615, "x":5.30121, "y":6.74732, "heading":-1.07405, "vx":-3.15604, "vy":0.20434, "omega":1.79671, "ax":-0.46673, "ay":-9.35574, "alpha":-14.7841, "fx":[-28.43322,69.71938,38.59945,-111.64145], "fy":[-172.64958,-160.34242,-169.16572,-134.39602]}, + {"t":0.9311, "x":5.254, "y":6.74933, "heading":-1.0472, "vx":-3.16301, "vy":0.06453, "omega":1.57578, "ax":-0.22556, "ay":-9.54189, "alpha":-13.46921, "fx":[-21.74724,69.42219,36.4,-99.42181], "fy":[-173.81579,-160.74518,-170.62071,-144.03762]}, + {"t":0.95639, "x":5.17392, "y":6.74791, "heading":-1.00734, "vx":-3.16872, "vy":-0.17683, "omega":1.23508, "ax":0.28974, "ay":-9.60405, "alpha":-12.9059, "fx":[-12.28791,74.80241,44.39735,-87.19826], "fy":[-174.71708,-158.28756,-168.73181,-151.71245]}, + {"t":0.98169, "x":5.09386, "y":6.74036, "heading":-0.9761, "vx":-3.16139, "vy":-0.41976, "omega":0.90864, "ax":0.87217, "ay":-9.6364, "alpha":-12.29359, "fx":[-2.39131,80.01333,54.51588,-72.79671], "fy":[-175.10734,-155.69249,-165.76595,-159.08388]}, + {"t":1.00698, "x":5.01417, "y":6.72666, "heading":-0.95311, "vx":-3.13933, "vy":-0.66351, "omega":0.59767, "ax":1.52065, "ay":-9.63177, "alpha":-11.56159, "fx":[8.17869,85.12605,65.76814,-55.60986], "fy":[-174.90455,-152.9307,-161.66849,-165.83067]}, + {"t":1.03228, "x":4.93525, "y":6.7068, "heading":-0.938, "vx":-3.10086, "vy":-0.90714, "omega":0.30523, "ax":2.23594, "ay":-9.57961, "alpha":-10.63277, "fx":[19.70733,90.21282,77.23917,-35.02861], "fy":[-173.95155,-149.95943,-156.55794,-171.31702]}, + {"t":1.05757, "x":4.85753, "y":6.68079, "heading":-0.93028, "vx":-3.04431, "vy":-1.14945, "omega":0.03628, "ax":3.01859, "ay":-9.46366, "alpha":-9.43771, "fx":[32.52369,95.35171,88.19905,-10.69332], "fy":[-171.98117,-146.7177,-150.70756,-174.49028]}, + {"t":1.08286, "x":4.78149, "y":6.64869, "heading":-0.92936, "vx":-2.96795, "vy":-1.38883, "omega":-0.20244, "ax":3.8629, "ay":-9.26096, "alpha":-7.94706, "fx":[46.97026,100.63129,98.16504,17.06102], "fy":[-168.56963,-143.11889,-144.46995,-173.94682]}, + {"t":1.10816, "x":4.70766, "y":6.61059, "heading":-0.93448, "vx":-2.87024, "vy":-1.62308, "omega":-0.40346, "ax":4.74908, "ay":-8.94655, "alpha":-6.20509, "fx":[63.32095,106.15172,106.89995,46.74956], "fy":[-163.08932,-139.04239,-138.18897,-168.39208]}, + {"t":1.13345, "x":4.63657, "y":6.56668, "heading":-0.94468, "vx":-2.75012, "vy":-1.84938, "omega":-0.56041, "ax":5.64085, "ay":-8.50442, "alpha":-4.32362, "fx":[81.61609,112.01873,114.36231,75.79973], "fy":[-154.70008,-134.32647,-132.13653,-157.46816]}, + {"t":1.15875, "x":4.56882, "y":6.51718, "heading":-0.95886, "vx":-2.60743, "vy":-2.06449, "omega":-0.66978, "ax":6.49445, "ay":-7.93682, "alpha":-2.41724, "fx":[101.41082,118.32905,120.63826,101.49695], "fy":[-142.46425,-128.76439,-126.48725,-142.29647]}, + {"t":1.18404, "x":4.50494, "y":6.46242, "heading":-0.9758, "vx":-2.44316, "vy":-2.26525, "omega":-0.73092, "ax":7.27171, "ay":-7.2628, "alpha":-0.54151, "fx":[121.54097,125.14797,125.87969,122.19012], "fy":[-125.69091,-122.10495,-121.32348,-125.03328]}, + {"t":1.20934, "x":4.44547, "y":6.4028, "heading":-0.99429, "vx":-2.25923, "vy":-2.44896, "omega":-0.74462, "ax":7.94558, "ay":-6.511, "alpha":1.28982, "fx":[140.17678,132.47961,130.25926,137.69263], "fy":[-104.48175,-114.0606,-116.65647,-107.80198]}, + {"t":1.23463, "x":4.39086, "y":6.33877, "heading":-1.01312, "vx":-2.05825, "vy":-2.61365, "omega":-0.71199, "ax":8.50054, "ay":-5.71538, "alpha":3.04162, "fx":[155.40404,140.22992,133.94303,148.79014], "fy":[-80.11028,-104.33087,-112.45157,-91.97501]}, + {"t":1.25993, "x":4.34152, "y":6.27083, "heading":-1.03113, "vx":-1.84323, "vy":-2.75822, "omega":-0.63506, "ax":8.93437, "ay":-4.91112, "alpha":4.64301, "fx":[166.07495,148.16826,137.07702,156.56439], "fy":[-54.73026,-92.65158,-108.64987,-78.11553]}, + {"t":1.28522, "x":4.29775, "y":6.19949, "heading":-1.0472, "vx":-1.61724, "vy":-2.88244, "omega":-0.51761, "ax":9.47119, "ay":-3.82822, "alpha":4.30181, "fx":[171.0855,160.49655,149.34604,163.48072], "fy":[-36.90311,-69.5811,-91.29186,-62.69164]}, + {"t":1.32136, "x":4.2455, "y":6.09283, "heading":-1.0659, "vx":-1.27498, "vy":-3.02078, "omega":-0.36216, "ax":10.04635, "ay":-1.79521, "alpha":4.41529, "fx":[174.93258,172.70688,164.12969,171.77296], "fy":[-1.22634,-26.97682,-60.47788,-33.46324]}, + {"t":1.35749, "x":4.20598, "y":5.98249, "heading":-1.07899, "vx":-0.91193, "vy":-3.08566, "omega":-0.2026, "ax":10.11629, "ay":1.32569, "alpha":4.32977, "fx":[168.3788,170.78065,174.66865,174.47243], "fy":[47.16392,36.75512,-5.94147,12.2211]}, + {"t":1.39363, "x":4.17963, "y":5.87185, "heading":-1.08631, "vx":-0.54636, "vy":-3.03775, "omega":-0.04614, "ax":8.83357, "ay":5.149, "alpha":3.5441, "fx":[143.11902,140.24709,159.15013,158.50995], "fy":[100.41159,104.15719,72.0167,73.74664]}, + {"t":1.42977, "x":4.16566, "y":5.76544, "heading":-1.08798, "vx":-0.22714, "vy":-2.85168, "omega":0.08194, "ax":6.24013, "ay":8.14301, "alpha":2.1768, "fx":[102.54978,94.53545,110.67783,116.80819], "fy":[141.64696,147.0381,135.25496,130.10117]}, + {"t":1.46591, "x":4.16152, "y":5.6677, "heading":-1.08502, "vx":-0.00164, "vy":-2.55742, "omega":0.1606, "ax":3.64071, "ay":9.61453, "alpha":0.9597, "fx":[61.58582,55.73997,62.34109,68.04286], "fy":[163.75684,165.81611,163.42708,161.16136]}, + {"t":1.50204, "x":4.16384, "y":5.58156, "heading":-1.07921, "vx":0.12993, "vy":-2.20998, "omega":0.19528, "ax":1.70564, "ay":10.14798, "alpha":0.09374, "fx":[29.09321,28.38271,28.9314,29.6427], "fy":[172.60291,172.71975,172.62684,172.50756]}, + {"t":1.53818, "x":4.16965, "y":5.50833, "heading":-1.07216, "vx":0.19156, "vy":-1.84326, "omega":0.19867, "ax":0.36838, "ay":10.28736, "alpha":-0.48997, "fx":[5.41462,9.54109,7.08748,3.02116], "fy":[175.02272,174.85157,174.97656,175.0891]}, + {"t":1.57432, "x":4.17681, "y":5.44843, "heading":-1.06498, "vx":0.20488, "vy":-1.4715, "omega":0.18096, "ax":-0.56506, "ay":10.27961, "alpha":-0.8908, "fx":[-11.68923,-3.76812,-7.67071,-15.31793], "fy":[174.76986,175.12687,175.01261,174.50343]}, + {"t":1.61045, "x":4.18385, "y":5.40197, "heading":-1.05844, "vx":0.18446, "vy":-1.10003, "omega":0.14877, "ax":-1.238, "ay":10.22061, "alpha":-1.1768, "fx":[-24.281,-13.50369,-18.11304,-28.33437], "fy":[173.51157,174.68831,174.28611,172.91265]}, + {"t":1.64659, "x":4.18971, "y":5.36889, "heading":-1.05306, "vx":0.13972, "vy":-0.73068, "omega":0.10624, "ax":-1.74005, "ay":10.14699, "alpha":-1.3888, "fx":[-33.80042,-20.86747,-25.81559,-37.9073], "fy":[171.94454,173.99514,173.34631,171.10383]}, + {"t":1.68273, "x":4.19362, "y":5.34911, "heading":-1.04922, "vx":0.07684, "vy":-0.364, "omega":0.05606, "ax":-2.1263, "ay":10.07277, "alpha":-1.55123, "fx":[-41.19267,-26.60419,-31.69437,-45.17964], "fy":[170.35203,173.23688,172.39461,169.35629]}, + {"t":1.71887, "x":4.19501, "y":5.34254, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} From 1376efc032457a76a2651c73666d4d643747e2eb Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 28 Jan 2025 20:36:19 -0500 Subject: [PATCH 09/48] added a ton of traj's. fixed small problems and started on flipping to processor side --- src/main/deploy/choreo/ATofetch.traj | 70 +++++++++ src/main/deploy/choreo/BTofetch.traj | 71 +++++++++ src/main/deploy/choreo/GTofetch.traj | 126 +++++++++++++++ src/main/deploy/choreo/HTofetch.traj | 123 +++++++++++++++ src/main/deploy/choreo/ITofetch.traj | 87 +++++++++++ src/main/deploy/choreo/JTofetch.traj | 85 ++++++++++ src/main/deploy/choreo/LTofetch.traj | 62 ++++++++ src/main/deploy/choreo/constants.traj | 29 ++-- src/main/deploy/choreo/fetchToA.traj | 78 ++++++++++ src/main/deploy/choreo/fetchToB.traj | 79 ++++++++++ src/main/deploy/choreo/fetchToF.traj | 146 ++++++++++++++++++ src/main/deploy/choreo/fetchToI.traj | 86 +++++++++++ src/main/deploy/choreo/fetchToJ.traj | 84 ++++++++++ src/main/deploy/choreo/fetchToK.traj | 62 ++++++++ src/main/deploy/choreo/fetchToL.traj | 62 ++++++++ src/main/deploy/choreo/startToG.traj | 53 +++++++ src/main/deploy/choreo/startToH.traj | 52 +++++++ src/main/deploy/choreo/startToK.traj | 136 +++++++--------- src/main/deploy/choreo/startToL.traj | 86 +++++++++++ .../frc/robot/constants/DriveConstants.java | 1 + .../subsystems/drive/DriveSubsystem.java | 2 +- .../subsystems/pathHandler/PathHandler.java | 21 ++- 22 files changed, 1503 insertions(+), 98 deletions(-) create mode 100644 src/main/deploy/choreo/ATofetch.traj create mode 100644 src/main/deploy/choreo/BTofetch.traj create mode 100644 src/main/deploy/choreo/GTofetch.traj create mode 100644 src/main/deploy/choreo/HTofetch.traj create mode 100644 src/main/deploy/choreo/ITofetch.traj create mode 100644 src/main/deploy/choreo/JTofetch.traj create mode 100644 src/main/deploy/choreo/LTofetch.traj create mode 100644 src/main/deploy/choreo/fetchToA.traj create mode 100644 src/main/deploy/choreo/fetchToB.traj create mode 100644 src/main/deploy/choreo/fetchToF.traj create mode 100644 src/main/deploy/choreo/fetchToI.traj create mode 100644 src/main/deploy/choreo/fetchToJ.traj create mode 100644 src/main/deploy/choreo/fetchToK.traj create mode 100644 src/main/deploy/choreo/fetchToL.traj create mode 100644 src/main/deploy/choreo/startToG.traj create mode 100644 src/main/deploy/choreo/startToH.traj create mode 100644 src/main/deploy/choreo/startToL.traj diff --git a/src/main/deploy/choreo/ATofetch.traj b/src/main/deploy/choreo/ATofetch.traj new file mode 100644 index 00000000..b41d625a --- /dev/null +++ b/src/main/deploy/choreo/ATofetch.traj @@ -0,0 +1,70 @@ +{ + "name":"ATofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "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":0, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "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":0, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.219], + "samples":[ + {"t":0.0, "x":3.20199, "y":4.42922, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.73119, "ay":8.85668, "alpha":-8.1153, "fx":[-61.82511,-38.03643,-93.76775,-128.27548], "fy":[163.91169,171.12075,148.13917,119.42691]}, + {"t":0.03694, "x":3.19876, "y":4.43527, "heading":0.0, "vx":-0.17477, "vy":0.32716, "omega":-0.29977, "ax":-4.74487, "ay":8.87046, "alpha":-7.80369, "fx":[-63.03167,-39.64934,-93.45619,-126.69852], "fy":[163.42337,170.73313,148.31417,121.06527]}, + {"t":0.07388, "x":3.18907, "y":4.4534, "heading":-0.01107, "vx":-0.35004, "vy":0.65483, "omega":-0.58804, "ax":-4.76138, "ay":8.88754, "alpha":-7.39857, "fx":[-65.14939,-41.42781,-92.68925,-124.69208], "fy":[162.55633,170.28317,148.76733,123.09151]}, + {"t":0.11082, "x":3.17289, "y":4.48365, "heading":-0.0328, "vx":-0.52592, "vy":0.98313, "omega":-0.86133, "ax":-4.78059, "ay":8.90831, "alpha":-6.87925, "fx":[-68.04962,-43.63075,-91.4958,-122.08996], "fy":[161.32124,169.69631,149.46881,125.62509]}, + {"t":0.14776, "x":3.1502, "y":4.52605, "heading":-0.06461, "vx":-0.70251, "vy":1.31219, "omega":-1.11545, "ax":-4.80235, "ay":8.9334, "alpha":-6.2068, "fx":[-71.54797,-46.67642,-89.91693,-118.60513], "fy":[159.74729,168.83532,150.37589,128.8599]}, + {"t":0.1847, "x":3.12097, "y":4.58061, "heading":-0.10582, "vx":-0.87991, "vy":1.64219, "omega":-1.34472, "ax":-4.82628, "ay":8.96371, "alpha":-5.30241, "fx":[-75.3661,-51.2713,-88.01787,-113.71904], "fy":[157.91413,167.42812,151.42639,133.11172]}, + {"t":0.22164, "x":3.08518, "y":4.64739, "heading":-0.15549, "vx":-1.05819, "vy":1.9733, "omega":-1.54059, "ax":-4.85148, "ay":8.9994, "alpha":-3.99599, "fx":[-79.07923,-58.69549,-85.91722,-106.39745], "fy":[155.99918,164.86783,152.52052,138.92116]}, + {"t":0.25857, "x":3.04278, "y":4.72642, "heading":-0.2124, "vx":-1.2374, "vy":2.30573, "omega":-1.6882, "ax":-4.87421, "ay":9.03366, "alpha":-1.89066, "fx":[-82.03267,-71.48461,-83.8767,-94.24165], "fy":[154.34597,159.56704,153.45958,147.26725]}, + {"t":0.29551, "x":2.99374, "y":4.81776, "heading":-0.27476, "vx":-1.41745, "vy":2.63943, "omega":-1.75804, "ax":-4.86921, "ay":9.01547, "alpha":2.04639, "fx":[-83.16754,-95.04219,-82.71332,-70.37236], "fy":[153.56376,146.43334,153.6423,159.76311]}, + {"t":0.33245, "x":2.93806, "y":4.92141, "heading":-0.3397, "vx":-1.59731, "vy":2.97245, "omega":-1.68245, "ax":-4.66322, "ay":8.55992, "alpha":10.69818, "fx":[-80.5656,-138.18653,-88.52894,-9.99874], "fy":[154.67159,105.85661,148.14876,173.73027]}, + {"t":0.36939, "x":2.87588, "y":5.03705, "heading":-0.40185, "vx":-1.76957, "vy":3.28865, "omega":-1.28726, "ax":-4.72223, "ay":7.07048, "alpha":19.31066, "fx":[-74.31686,-157.51151,-132.36738,42.90037], "fy":[156.84566,71.35326,85.65129,167.21746]}, + {"t":0.40633, "x":2.80729, "y":5.16335, "heading":-0.4494, "vx":-1.944, "vy":3.54983, "omega":-0.57394, "ax":-2.77672, "ay":6.01607, "alpha":15.40494, "fx":[-55.13791,-112.28147,-44.12373,22.61807], "fy":[133.94466,77.06963,66.88697,131.42508]}, + {"t":0.44327, "x":2.73358, "y":5.29859, "heading":-0.4706, "vx":-2.04657, "vy":3.77206, "omega":-0.00489, "ax":0.15486, "ay":0.09246, "alpha":0.01693, "fx":[2.61593,2.57865,2.65226,2.68947], "fy":[1.62822,1.55477,1.5173,1.59055]}, + {"t":0.48021, "x":2.65809, "y":5.43799, "heading":-0.47078, "vx":-2.04085, "vy":3.77547, "omega":-0.00427, "ax":-0.00413, "ay":-0.00218, "alpha":-0.00001, "fx":[-0.07024,-0.07022,-0.07026,-0.07028], "fy":[-0.03711,-0.03706,-0.03703,-0.03708]}, + {"t":0.51715, "x":2.5827, "y":5.57745, "heading":-0.47094, "vx":-2.04101, "vy":3.77539, "omega":-0.00427, "ax":-0.04054, "ay":-0.02189, "alpha":-0.00008, "fx":[-0.68956,-0.6894,-0.68973,-0.68989], "fy":[-0.37252,-0.3722,-0.37203,-0.37236]}, + {"t":0.55409, "x":2.50728, "y":5.71689, "heading":-0.47109, "vx":-2.0425, "vy":3.77458, "omega":-0.00427, "ax":-0.05855, "ay":-0.03166, "alpha":-0.00011, "fx":[-0.99577,-0.99552,-0.99601,-0.99627], "fy":[-0.53895,-0.53845,-0.5382,-0.5387]}, + {"t":0.59103, "x":2.43179, "y":5.8563, "heading":-0.47125, "vx":-2.04467, "vy":3.77341, "omega":-0.00428, "ax":-0.08146, "ay":-0.04414, "alpha":-0.00013, "fx":[-1.38544,-1.38515,-1.38573,-1.38602], "fy":[-0.75123,-0.75066,-0.75036,-0.75094]}, + {"t":0.62797, "x":2.35621, "y":5.99566, "heading":-0.47141, "vx":-2.04768, "vy":3.77178, "omega":-0.00428, "ax":-0.11599, "ay":-0.063, "alpha":-0.00004, "fx":[-1.97287,-1.97278,-1.97295,-1.97304], "fy":[-1.07169,-1.07153,-1.07144,-1.07161]}, + {"t":0.66491, "x":2.28049, "y":6.13494, "heading":-0.47157, "vx":-2.05196, "vy":3.76946, "omega":-0.00428, "ax":-0.15542, "ay":-0.08468, "alpha":0.00026, "fx":[-2.64397,-2.64454,-2.64343,-2.64286], "fy":[-1.43951,-1.44062,-1.44119,-1.44008]}, + {"t":0.70185, "x":2.20458, "y":6.27413, "heading":-0.47173, "vx":-2.0577, "vy":3.76633, "omega":-0.00427, "ax":-0.20169, "ay":-0.1104, "alpha":0.00038, "fx":[-3.43115,-3.43199,-3.43032,-3.42949], "fy":[-1.8766,-1.87826,-1.87913,-1.87747]}, + {"t":0.73879, "x":2.12844, "y":6.41318, "heading":-0.47188, "vx":-2.06515, "vy":3.76225, "omega":-0.00426, "ax":-0.42425, "ay":-0.2422, "alpha":-0.01584, "fx":[-7.19948,-7.16474,-7.23321,-7.2681], "fy":[-4.17185,-4.10283,-4.06765,-4.13654]}, + {"t":0.77572, "x":2.05186, "y":6.55199, "heading":-0.47204, "vx":-2.08082, "vy":3.7533, "omega":-0.00484, "ax":2.42798, "ay":-6.09913, "alpha":-15.25704, "fx":[49.39531,107.38305,36.20086,-27.78206], "fy":[-135.07474,-80.99272,-69.32289,-129.58738]}, + {"t":0.81266, "x":1.97666, "y":6.68647, "heading":-0.47222, "vx":-1.99113, "vy":3.52801, "omega":-0.56843, "ax":4.79143, "ay":-7.03497, "alpha":-19.74896, "fx":[69.90442,154.04894,144.61663,-42.56651], "fy":[-158.84246,-78.69798,-73.94321,-167.16792]}, + {"t":0.8496, "x":1.90637, "y":6.81199, "heading":-0.49322, "vx":-1.81414, "vy":3.26814, "omega":-1.29794, "ax":4.79782, "ay":-8.53825, "alpha":-10.30866, "fx":[74.56143,132.70267,105.27683,13.89737], "fy":[-157.64088,-112.7757,-137.13914,-173.37712]}, + {"t":0.88654, "x":1.84263, "y":6.92689, "heading":-0.54116, "vx":-1.63691, "vy":2.95274, "omega":-1.67874, "ax":4.95642, "ay":-8.97232, "alpha":-1.83527, "fx":[81.77851,94.65878,87.41576,73.37626], "fy":[-154.31028,-146.71446,-151.04376,-158.39772]}, + {"t":0.92348, "x":1.78555, "y":7.02984, "heading":-0.60317, "vx":-1.45383, "vy":2.62131, "omega":-1.74653, "ax":4.96524, "ay":-8.98518, "alpha":1.84246, "fx":[88.28539,73.63434,81.28272,94.62692], "fy":[-150.86902,-158.57259,-154.85211,-147.04777]}, + {"t":0.96042, "x":1.73523, "y":7.12054, "heading":-0.66769, "vx":-1.27042, "vy":2.28941, "omega":-1.67847, "ax":4.95042, "ay":-8.95097, "alpha":3.85851, "fx":[94.57647,61.27014,76.93089,104.04332], "fy":[-147.14107,-163.89023,-157.24191,-140.74054]}, + {"t":0.99736, "x":1.69168, "y":7.199, "heading":-0.72969, "vx":-1.08755, "vy":1.95876, "omega":-1.53594, "ax":4.9354, "ay":-8.91238, "alpha":5.16512, "fx":[100.42937,53.37411,73.25806,108.73715], "fy":[-143.31253,-166.71815,-159.08733,-137.27012]}, + {"t":1.0343, "x":1.65488, "y":7.26528, "heading":-0.78643, "vx":-0.90524, "vy":1.62955, "omega":-1.34514, "ax":4.92233, "ay":-8.87675, "alpha":6.10711, "fx":[105.62632,48.0466,70.04587,111.19113], "fy":[-139.60761,-168.39163,-160.59155,-135.37345]}, + {"t":1.07124, "x":1.62479, "y":7.31941, "heading":-0.83612, "vx":-0.72341, "vy":1.30165, "omega":-1.11955, "ax":4.91067, "ay":-8.84556, "alpha":6.82851, "fx":[110.06004,44.31143,67.25388,112.49122], "fy":[-136.20544,-169.45728,-161.82479,-134.35447]}, + {"t":1.10818, "x":1.60142, "y":7.36146, "heading":-0.87747, "vx":-0.54202, "vy":0.9749, "omega":-0.86731, "ax":4.90007, "ay":-8.81886, "alpha":7.39835, "fx":[113.71752,41.59335,64.88761,113.19658], "fy":[-133.22103,-170.17906,-162.82034,-133.80478]}, + {"t":1.14512, "x":1.58474, "y":7.39146, "heading":-0.90951, "vx":-0.36101, "vy":0.64914, "omega":-0.59402, "ax":4.89059, "ay":-8.79619, "alpha":7.85551, "fx":[116.64195,39.51266,62.9645,113.63116], "fy":[-130.71271,-170.70172,-163.5984,-133.46964]}, + {"t":1.18206, "x":1.57475, "y":7.40943, "heading":-0.93145, "vx":-0.18036, "vy":0.32421, "omega":-0.30384, "ax":4.88253, "ay":-8.77686, "alpha":8.22552, "fx":[118.89851,37.80072,61.504,113.99853], "fy":[-128.70033,-171.11198,-164.17273,-133.18249]}, + {"t":1.219, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/BTofetch.traj b/src/main/deploy/choreo/BTofetch.traj new file mode 100644 index 00000000..bc613b50 --- /dev/null +++ b/src/main/deploy/choreo/BTofetch.traj @@ -0,0 +1,71 @@ +{ + "name":"BTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.2019875, "y":4.1005465106, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "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":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "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":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.28721], + "samples":[ + {"t":0.0, "x":3.20199, "y":4.10055, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.33801, "ay":9.04682, "alpha":-8.26482, "fx":[-51.42029,-31.44463,-88.92083,-123.3676], "fy":[167.47731,172.46079,151.10306,124.49401]}, + {"t":0.03786, "x":3.19888, "y":4.10703, "heading":0.0, "vx":-0.16423, "vy":0.34251, "omega":-0.3129, "ax":-4.35296, "ay":9.06367, "alpha":-7.90764, "fx":[-52.96269,-33.18757,-88.5125,-121.50786], "fy":[166.96928,172.11328,151.32141,126.278]}, + {"t":0.07572, "x":3.18954, "y":4.12649, "heading":-0.01185, "vx":-0.32903, "vy":0.68565, "omega":-0.61228, "ax":-4.37069, "ay":9.08453, "alpha":-7.44323, "fx":[-55.52093,-35.11015,-87.58852,-119.15682], "fy":[166.10339,171.70477,151.83132,128.46139]}, + {"t":0.11358, "x":3.17395, "y":4.15896, "heading":-0.03503, "vx":-0.4945, "vy":1.02958, "omega":-0.89407, "ax":-4.39105, "ay":9.10966, "alpha":-6.8482, "fx":[-58.95761,-37.51454,-86.17827,-116.11171], "fy":[164.87442,171.16024,152.60124,131.17471]}, + {"t":0.15144, "x":3.15208, "y":4.20447, "heading":-0.06888, "vx":-0.66075, "vy":1.37447, "omega":-1.15334, "ax":-4.41376, "ay":9.13957, "alpha":-6.0776, "fx":[-63.07226,-40.89319,-84.32149,-112.02037], "fy":[163.29415,170.3363,153.58733,134.62785]}, + {"t":0.1893, "x":3.12391, "y":4.26305, "heading":-0.11254, "vx":-0.82785, "vy":1.72048, "omega":-1.38343, "ax":-4.43811, "ay":9.17487, "alpha":-5.03884, "fx":[-67.55561,-46.08804,-82.07789,-106.24235], "fy":[161.42699,168.93552,154.72867,139.15628]}, + {"t":0.22716, "x":3.08938, "y":4.33477, "heading":-0.16492, "vx":-0.99587, "vy":2.06783, "omega":-1.5742, "ax":-4.46244, "ay":9.21455, "alpha":-3.52842, "fx":[-71.93287,-54.64758,-79.54958,-97.48903], "fy":[159.44152,166.25832,155.93326,145.31459]}, + {"t":0.26501, "x":3.04848, "y":4.41966, "heading":-0.22451, "vx":-1.16481, "vy":2.41669, "omega":-1.70778, "ax":-4.48064, "ay":9.24607, "alpha":-1.06435, "fx":[-75.48408,-69.68031,-76.94924,-82.74367], "fy":[157.67931,160.36229,157.02825,154.02231]}, + {"t":0.30287, "x":3.00117, "y":4.51778, "heading":-0.28917, "vx":-1.33445, "vy":2.76674, "omega":-1.74808, "ax":-4.45505, "ay":9.18727, "alpha":3.60173, "fx":[-77.09335,-97.71059,-74.93098,-53.38142], "fy":[156.73917,144.65268,157.48863,166.21115]}, + {"t":0.34073, "x":2.94746, "y":4.62911, "heading":-0.35535, "vx":-1.50311, "vy":3.11456, "omega":-1.61172, "ax":-4.15854, "ay":8.46896, "alpha":13.50644, "fx":[-74.88593,-146.29681,-80.17454,18.41469], "fy":[157.55672,94.33785,151.25295,173.07114]}, + {"t":0.37859, "x":2.88757, "y":4.75309, "heading":-0.41637, "vx":-1.66055, "vy":3.43519, "omega":-1.10037, "ax":-4.33476, "ay":7.26538, "alpha":19.31468, "fx":[-69.36817,-155.99777,-120.1531,50.58664], "fy":[159.06678,74.43104,95.77531,165.05531]}, + {"t":0.41645, "x":2.8216, "y":4.88835, "heading":-0.45803, "vx":-1.82466, "vy":3.71025, "omega":-0.36914, "ax":-1.38538, "ay":3.94099, "alpha":9.6305, "fx":[-32.06436,-60.12958,-14.20937,12.14349], "fy":[92.30599,56.05123,38.9486,80.83489]}, + {"t":0.45431, "x":2.75152, "y":5.03164, "heading":-0.472, "vx":-1.87711, "vy":3.85945, "omega":-0.00453, "ax":0.07253, "ay":0.03773, "alpha":0.00475, "fx":[1.22866,1.21815,1.23873,1.24924], "fy":[0.65737,0.63679,0.62627,0.64686]}, + {"t":0.49217, "x":2.68051, "y":5.17779, "heading":-0.47217, "vx":-1.87437, "vy":3.86088, "omega":-0.00435, "ax":-0.01524, "ay":-0.00734, "alpha":0.00005, "fx":[-0.25931,-0.25943,-0.2592,-0.25908], "fy":[-0.12472,-0.12495,-0.12507,-0.12484]}, + {"t":0.53003, "x":2.60954, "y":5.32395, "heading":-0.47234, "vx":-1.87494, "vy":3.8606, "omega":-0.00435, "ax":-0.03549, "ay":-0.01719, "alpha":0.00002, "fx":[-0.60366,-0.60369,-0.60363,-0.6036], "fy":[-0.29234,-0.2924,-0.29244,-0.29237]}, + {"t":0.56789, "x":2.53853, "y":5.4701, "heading":-0.4725, "vx":-1.87629, "vy":3.85995, "omega":-0.00435, "ax":-0.04562, "ay":-0.02214, "alpha":-0.00004, "fx":[-0.7759,-0.77581,-0.77599,-0.77608], "fy":[-0.37673,-0.37655,-0.37646,-0.37664]}, + {"t":0.60575, "x":2.46746, "y":5.61622, "heading":-0.47267, "vx":-1.87801, "vy":3.85912, "omega":-0.00435, "ax":-0.05823, "ay":-0.02832, "alpha":-0.00013, "fx":[-0.99027,-0.98999,-0.99054,-0.99082], "fy":[-0.48212,-0.48156,-0.48128,-0.48183]}, + {"t":0.64361, "x":2.39632, "y":5.7623, "heading":-0.47283, "vx":-1.88022, "vy":3.85804, "omega":-0.00436, "ax":-0.08276, "ay":-0.04034, "alpha":-0.0002, "fx":[-1.40758,-1.40713,-1.40802,-1.40847], "fy":[-0.68693,-0.68604,-0.68558,-0.68647]}, + {"t":0.68147, "x":2.32508, "y":5.90833, "heading":-0.473, "vx":-1.88335, "vy":3.85652, "omega":-0.00437, "ax":-0.13453, "ay":-0.06577, "alpha":-0.00022, "fx":[-2.28816,-2.28767,-2.28863,-2.28913], "fy":[-1.11947,-1.11851,-1.11801,-1.11898]}, + {"t":0.71932, "x":2.25368, "y":6.05429, "heading":-0.47316, "vx":-1.88844, "vy":3.85403, "omega":-0.00437, "ax":-0.21776, "ay":-0.10688, "alpha":0.00022, "fx":[-3.70426,-3.70475,-3.7038,-3.7033], "fy":[-1.81734,-1.8183,-1.81879,-1.81783]}, + {"t":0.75718, "x":2.18203, "y":6.20012, "heading":-0.47333, "vx":-1.89669, "vy":3.84998, "omega":-0.00437, "ax":-0.2656, "ay":-0.13117, "alpha":0.00063, "fx":[-4.51842,-4.51982,-4.51708,-4.51568], "fy":[-2.22912,-2.23186,-2.23326,-2.23052]}, + {"t":0.79504, "x":2.11003, "y":6.34579, "heading":-0.47349, "vx":-1.90674, "vy":3.84501, "omega":-0.00434, "ax":-0.36301, "ay":-0.18316, "alpha":-0.00411, "fx":[-6.17031,-6.16122,-6.17905,-6.18815], "fy":[-3.12902,-3.11117,-3.102,-3.11984]}, + {"t":0.8329, "x":2.03758, "y":6.49122, "heading":-0.47366, "vx":-1.92049, "vy":3.83808, "omega":-0.0045, "ax":0.83555, "ay":-4.07752, "alpha":-9.36113, "fx":[22.94358,50.63804,3.75452,-20.48601], "fy":[-93.93016,-60.28167,-42.37498,-80.84297]}, + {"t":0.87076, "x":1.96547, "y":6.63361, "heading":-0.47383, "vx":-1.88885, "vy":3.68371, "omega":-0.3589, "ax":4.47798, "ay":-7.21087, "alpha":-19.60986, "fx":[65.79957,152.85587,135.86617,-49.84482], "fy":[-160.56414,-80.82648,-84.05559,-165.17316]}, + {"t":0.90862, "x":1.89717, "y":6.7679, "heading":-0.48742, "vx":-1.71932, "vy":3.41071, "omega":-1.10132, "ax":4.40336, "ay":-8.40326, "alpha":-13.2812, "fx":[68.38635,140.20094,106.76045,-15.74783], "fy":[-160.4741,-103.31441,-134.71815,-173.2414]}, + {"t":0.94648, "x":1.83524, "y":6.89101, "heading":-0.52911, "vx":-1.55261, "vy":3.09257, "omega":-1.60413, "ax":4.56601, "ay":-9.14295, "alpha":-3.2997, "fx":[74.30603,96.63901,82.66857,57.05255], "fy":[-158.08116,-145.41917,-153.60766,-164.96775]}, + {"t":0.98434, "x":1.77973, "y":7.00154, "heading":-0.58984, "vx":-1.37975, "vy":2.74643, "omega":-1.72906, "ax":4.59425, "ay":-9.19048, "alpha":1.07735, "fx":[79.99073,71.66627,76.50496,84.42588], "fy":[-155.45046,-159.4814,-157.25221,-153.12589]}, + {"t":1.0222, "x":1.73078, "y":7.09893, "heading":-0.6553, "vx":-1.20581, "vy":2.39848, "omega":-1.68827, "ax":4.58367, "ay":-9.1588, "alpha":3.41582, "fx":[86.03763,57.05683,72.168,96.60534], "fy":[-152.30824,-165.41672,-159.48393,-145.9452]}, + {"t":1.06006, "x":1.68842, "y":7.18317, "heading":-0.71922, "vx":-1.03228, "vy":2.05174, "omega":-1.55895, "ax":4.57013, "ay":-9.11728, "alpha":4.89421, "fx":[92.06112,47.74167,68.42126,102.72238], "fy":[-148.83803,-168.43051,-161.23122,-141.82927]}, + {"t":1.09792, "x":1.65261, "y":7.25431, "heading":-0.77824, "vx":-0.85926, "vy":1.70656, "omega":-1.37366, "ax":4.55832, "ay":-9.07753, "alpha":5.94569, "fx":[97.66098,41.4269,65.07887,105.97607], "fy":[-145.29971,-170.15377,-162.67384,-139.49746]}, + {"t":1.13578, "x":1.62335, "y":7.31242, "heading":-0.83025, "vx":-0.68668, "vy":1.3629, "omega":-1.14856, "ax":4.54788, "ay":-9.04196, "alpha":6.74848, "fx":[102.58704,36.963,62.13262,107.74981], "fy":[-141.92562,-171.22228,-163.86582,-138.19057]}, + {"t":1.17363, "x":1.60061, "y":7.35753, "heading":-0.87373, "vx":-0.5145, "vy":1.02057, "omega":-0.89307, "ax":4.5383, "ay":-9.01099, "alpha":7.38413, "fx":[106.73175,33.69039,59.61179,108.74663], "fy":[-138.88601,-171.92931,-164.83153,-137.45077]}, + {"t":1.21149, "x":1.58438, "y":7.38971, "heading":-0.90754, "vx":-0.34269, "vy":0.67943, "omega":-0.61351, "ax":4.52958, "ay":-8.98442, "alpha":7.89558, "fx":[110.08405,31.18262,57.55127,109.36922], "fy":[-136.28608,-172.42833,-165.58652,-136.98885]}, + {"t":1.24935, "x":1.57465, "y":7.409, "heading":-0.93077, "vx":-0.1712, "vy":0.33928, "omega":-0.31459, "ax":4.52206, "ay":-8.96171, "alpha":8.30945, "fx":[112.68219,29.14171,55.98362,109.8685], "fy":[-134.18061,-172.8071,-166.14228,-136.61451]}, + {"t":1.28721, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/GTofetch.traj b/src/main/deploy/choreo/GTofetch.traj new file mode 100644 index 00000000..cf3d10d8 --- /dev/null +++ b/src/main/deploy/choreo/GTofetch.traj @@ -0,0 +1,126 @@ +{ + "name":"GTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.068316459655762, "y":4.486484527587891, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.048557281494141, "y":5.589489936828613, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9426766239853251, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "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":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.068316459655762 m", "val":6.068316459655762}, "y":{"exp":"4.486484527587891 m", "val":4.486484527587891}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "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.49858,0.91145,1.50988,2.03588], + "samples":[ + {"t":0.0, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.90148, "ay":6.34553, "alpha":29.13269, "fx":[175.2204,122.47296,-1.79369,-98.48599], "fy":[2.39999,125.43442,175.23334,128.67472]}, + {"t":0.02624, "x":5.77791, "y":3.62476, "heading":3.14159, "vx":0.07614, "vy":0.16651, "omega":0.76448, "ax":3.6224, "ay":6.83422, "alpha":24.89369, "fx":[175.14462,121.76138,-1.42128,-49.02054], "fy":[4.76968,126.10018,175.21294,158.90993]}, + {"t":0.05248, "x":5.78116, "y":3.63148, "heading":-3.12153, "vx":0.1712, "vy":0.34585, "omega":1.41772, "ax":4.72833, "ay":7.02475, "alpha":20.2, "fx":[174.90285,119.61545,-2.29071,29.48226], "fy":[9.81293,128.10641,175.17116,164.86536]}, + {"t":0.07872, "x":5.78728, "y":3.64298, "heading":-3.08433, "vx":0.29527, "vy":0.53019, "omega":1.94779, "ax":5.67349, "ay":6.7633, "alpha":17.79488, "fx":[174.29563,116.14088,-4.17601,99.75747], "fy":[17.17589,131.22572,175.09244,136.6733]}, + {"t":0.10497, "x":5.79698, "y":3.65922, "heading":-3.03322, "vx":0.44415, "vy":0.70767, "omega":2.41475, "ax":6.02917, "ay":6.53641, "alpha":17.50219, "fx":[173.04912,111.41935,-6.66926,132.41855], "fy":[26.66662,135.20794,174.95568,107.89992]}, + {"t":0.13121, "x":5.81071, "y":3.68004, "heading":-2.96985, "vx":0.60236, "vy":0.87919, "omega":2.87403, "ax":6.016, "ay":6.60283, "alpha":17.42758, "fx":[170.73471,105.37216,-9.25714,142.47184], "fy":[38.52412,139.90616,174.7551,96.06373]}, + {"t":0.15745, "x":5.82859, "y":3.70539, "heading":-2.89443, "vx":0.76023, "vy":1.05246, "omega":3.33135, "ax":5.79087, "ay":6.94841, "alpha":16.82636, "fx":[166.53477,97.71701,-11.14493,140.89747], "fy":[53.55344,145.26383,174.52515,99.41911]}, + {"t":0.18369, "x":5.85053, "y":3.7354, "heading":-2.80701, "vx":0.91219, "vy":1.23479, "omega":3.7729, "ax":5.36265, "ay":7.55437, "alpha":15.3285, "fx":[158.59154,87.913,-10.74383,129.10762], "fy":[73.47752,151.25791,174.36821,114.88682]}, + {"t":0.20993, "x":5.87631, "y":3.7704, "heading":-2.70801, "vx":1.05291, "vy":1.43303, "omega":4.17514, "ax":4.63406, "ay":8.42184, "alpha":12.36133, "fx":[141.51532,74.71533,-4.66142,103.72691], "fy":[102.13391,157.94979,174.34421,138.58448]}, + {"t":0.23617, "x":5.90554, "y":3.8109, "heading":-2.59845, "vx":1.17452, "vy":1.65403, "omega":4.49952, "ax":3.28241, "ay":9.51976, "alpha":6.1859, "fx":[93.88326,53.77583,14.39266,61.27962], "fy":[146.47907,165.71058,173.32301,162.20063]}, + {"t":0.26241, "x":5.93749, "y":3.85759, "heading":-2.48037, "vx":1.26065, "vy":1.90384, "omega":4.66184, "ax":0.05504, "ay":9.919, "alpha":-8.02328, "fx":[-50.7398,-8.54181,56.76796,6.25858], "fy":[165.76305,171.85629,163.63635,173.62149]}, + {"t":0.28865, "x":5.97059, "y":3.91096, "heading":-2.35804, "vx":1.2621, "vy":2.16413, "omega":4.4513, "ax":-3.95203, "ay":8.79082, "alpha":-11.73115, "fx":[-117.44511,-110.89505,8.32962,-48.88125], "fy":[127.75966,130.95456,172.6474,166.75596]}, + {"t":0.3149, "x":6.00235, "y":3.97078, "heading":-2.24123, "vx":1.15839, "vy":2.39481, "omega":4.14346, "ax":-6.78747, "ay":7.24735, "alpha":-8.33007, "fx":[-142.19842,-144.60468,-76.11593,-98.89252], "fy":[99.92572,94.70426,155.46758,143.00369]}, + {"t":0.34114, "x":6.03041, "y":4.03611, "heading":-2.1325, "vx":0.98028, "vy":2.58499, "omega":3.92487, "ax":-8.50115, "ay":5.45963, "alpha":-5.49163, "fx":[-157.38283,-159.8685,-127.74994,-133.40732], "fy":[74.38874,67.68242,117.56337,111.83252]}, + {"t":0.36738, "x":6.05321, "y":4.10583, "heading":-2.02951, "vx":0.7572, "vy":2.72826, "omega":3.78076, "ax":-9.39189, "ay":3.93361, "alpha":-3.84758, "fx":[-166.10661,-167.58917,-152.15453,-153.16304], "fy":[52.89424,47.02928,84.4922,83.22272]}, + {"t":0.39362, "x":6.06984, "y":4.17877, "heading":-1.9303, "vx":0.51074, "vy":2.83148, "omega":3.6798, "ax":-9.84925, "ay":2.71378, "alpha":-3.13575, "fx":[-171.13084,-171.90995,-163.48359,-163.60728], "fy":[34.26923,29.09748,60.54354,60.73243]}, + {"t":0.41986, "x":6.07985, "y":4.25401, "heading":-1.83374, "vx":0.25229, "vy":2.90269, "omega":3.59751, "ax":-10.08255, "ay":1.72572, "alpha":-3.04038, "fx":[-173.80937,-174.10693,-169.00981,-169.07877], "fy":[17.52346,12.44798,43.56452,43.88034]}, + {"t":0.4461, "x":6.083, "y":4.33077, "heading":-1.73933, "vx":-0.01229, "vy":2.94798, "omega":3.51773, "ax":-10.1894, "ay":0.90073, "alpha":-3.35535, "fx":[-174.80666,-174.65484,-171.80566,-172.00767], "fy":[2.01997,-3.4401,31.52841,31.17624]}, + {"t":0.47234, "x":6.07917, "y":4.40844, "heading":-1.64702, "vx":-0.27967, "vy":2.97161, "omega":3.42968, "ax":-10.21438, "ay":0.1814, "alpha":-4.01779, "fx":[-174.43657,-173.71636,-173.21543,-173.60633], "fy":[-12.99351,-19.31348,23.3008,21.34877]}, + {"t":0.49858, "x":6.06832, "y":4.48648, "heading":-1.55702, "vx":-0.54771, "vy":2.97637, "omega":3.32425, "ax":-10.17665, "ay":-0.22696, "alpha":-4.65388, "fx":[-173.15627,-172.05362,-173.21542,-173.98222], "fy":[-23.14473,-28.97058,20.90924,15.76395]}, + {"t":0.51653, "x":6.05685, "y":4.53988, "heading":-1.49735, "vx":-0.73039, "vy":2.9723, "omega":3.24071, "ax":-10.12344, "ay":-0.31308, "alpha":-5.80847, "fx":[-172.10052,-170.63505,-172.20868,-173.84313], "fy":[-29.74124,-35.80757,27.38004,16.86712]}, + {"t":0.53449, "x":6.0421, "y":4.59318, "heading":-1.43918, "vx":-0.91211, "vy":2.96668, "omega":3.13644, "ax":-10.05928, "ay":-0.39737, "alpha":-6.96177, "fx":[-170.77926,-169.01052,-170.86848,-173.76353], "fy":[-36.32038,-42.21947,34.23576,17.26752]}, + {"t":0.55244, "x":6.02411, "y":4.64637, "heading":-1.38288, "vx":-1.09268, "vy":2.95955, "omega":3.01147, "ax":-9.98675, "ay":-0.47989, "alpha":-8.07087, "fx":[-169.22797,-167.27532,-169.24203,-173.74173], "fy":[-42.71851,-48.00904,41.06133,17.01504]}, + {"t":0.57039, "x":6.00289, "y":4.69942, "heading":-1.32882, "vx":-1.27195, "vy":2.95093, "omega":2.86659, "ax":-9.90773, "ay":-0.56499, "alpha":-9.11617, "fx":[-167.46175,-165.46694,-167.41629,-173.7659], "fy":[-48.92718,-53.23191,47.53011,16.18778]}, + {"t":0.58834, "x":5.97846, "y":4.7523, "heading":-1.27736, "vx":-1.4498, "vy":2.94079, "omega":2.70295, "ax":-9.82363, "ay":-0.65748, "alpha":-10.08691, "fx":[-165.48525,-163.58145,-165.50245,-173.81947], "fy":[-54.96924,-58.02147,53.3972,14.85956]}, + {"t":0.60629, "x":5.95085, "y":4.80498, "heading":-1.22884, "vx":-1.62615, "vy":2.92899, "omega":2.52188, "ax":-9.7356, "ay":-0.76137, "alpha":-10.97557, "fx":[-163.30274,-161.58959,-163.62304,-173.88358], "fy":[-60.85416,-62.51826,58.4784,13.09141]}, + {"t":0.62424, "x":5.92009, "y":4.85744, "heading":-1.18357, "vx":-1.80091, "vy":2.91532, "omega":2.32487, "ax":-9.64471, "ay":-0.87976, "alpha":-11.77533, "fx":[-160.92535,-159.44774,-161.90403,-173.93813], "fy":[-66.56593,-66.8445,62.62375,10.92889]}, + {"t":0.64219, "x":5.88621, "y":4.90963, "heading":-1.14184, "vx":-1.97404, "vy":2.89953, "omega":2.11349, "ax":-9.55209, "ay":-1.01525, "alpha":-12.47822, "fx":[-158.37491,-157.10423,-160.47227,-173.96169], "fy":[-72.06456,-71.09753,65.6877,8.3977]}, + {"t":0.66014, "x":5.84923, "y":4.96151, "heading":-1.1039, "vx":-2.1455, "vy":2.8813, "omega":1.8895, "ax":-9.45888, "ay":-1.17085, "alpha":-13.07316, "fx":[-155.68283,-154.5007,-159.45739,-173.93016], "fy":[-77.29601,-75.35467,67.49349,5.49369]}, + {"t":0.67809, "x":5.8092, "y":5.01305, "heading":-1.06998, "vx":-2.3153, "vy":2.86029, "omega":1.65482, "ax":-9.36609, "ay":-1.35149, "alpha":-13.54352, "fx":[-152.88181,-151.56603,-158.99732,-173.81292], "fy":[-82.20929,-79.68953,67.78237,2.16237]}, + {"t":0.69604, "x":5.76613, "y":5.06417, "heading":-1.04028, "vx":-2.48342, "vy":2.83603, "omega":1.41171, "ax":-9.274, "ay":-1.5668, "alpha":-13.86269, "fx":[-149.98601,-148.19706,-159.24617,-173.56335], "fy":[-86.78258,-84.2043,66.12552,-1.74187]}, + {"t":0.71399, "x":5.72005, "y":5.11483, "heading":-1.01494, "vx":-2.6499, "vy":2.8079, "omega":1.16286, "ax":-9.18057, "ay":-1.83635, "alpha":-13.98504, "fx":[-146.95043,-144.21272,-160.37711,-173.09511], "fy":[-91.06588,-89.08573,61.74737,-6.5386]}, + {"t":0.73194, "x":5.67101, "y":5.16494, "heading":-0.99406, "vx":-2.8147, "vy":2.77494, "omega":0.91182, "ax":-9.0771, "ay":-2.20067, "alpha":-13.82404, "fx":[-143.58456,-139.25442,-162.53962,-172.21669], "fy":[-95.26261,-94.69582,53.1311,-12.90395]}, + {"t":0.74989, "x":5.61902, "y":5.21439, "heading":-0.97769, "vx":-2.97764, "vy":2.73543, "omega":0.66367, "ax":-8.9347, "ay":-2.74638, "alpha":-13.19387, "fx":[-139.34512,-132.57635,-165.56184,-170.42374], "fy":[-99.90959,-101.69971,37.06149,-22.31251]}, + {"t":0.76784, "x":5.56413, "y":5.26305, "heading":-0.96578, "vx":-3.13802, "vy":2.68614, "omega":0.42683, "ax":-8.65491, "ay":-3.66225, "alpha":-11.6418, "fx":[-132.79575,-122.6986,-167.26863,-166.10738], "fy":[-106.25534,-111.08675,6.34879,-38.18189]}, + {"t":0.7858, "x":5.5064, "y":5.31068, "heading":-0.95812, "vx":-3.29338, "vy":2.6204, "omega":0.21785, "ax":-7.8976, "ay":-5.26956, "alpha":-8.11386, "fx":[-120.59784,-108.10871,-155.52453,-153.11228], "fy":[-116.31354,-122.76716,-51.28457,-68.16989]}, + {"t":0.80375, "x":5.44601, "y":5.35687, "heading":-0.95421, "vx":-3.43515, "vy":2.5258, "omega":0.0722, "ax":-6.23933, "ay":-7.12972, "alpha":-2.62363, "fx":[-100.96967,-94.34327,-112.53117,-116.6726], "fy":[-127.58708,-130.36911,-114.26264,-112.87926]}, + {"t":0.8217, "x":5.38334, "y":5.40106, "heading":-0.95291, "vx":-3.54715, "vy":2.39782, "omega":0.02511, "ax":-5.08711, "ay":-7.6067, "alpha":-0.35081, "fx":[-85.93587,-84.82449,-87.14468,-88.21602], "fy":[-130.07194,-130.39771,-128.69826,-128.3835]}, + {"t":0.83965, "x":5.31885, "y":5.44288, "heading":-0.95246, "vx":-3.63847, "vy":2.26127, "omega":0.01881, "ax":-4.40662, "ay":-7.34006, "alpha":-0.06109, "fx":[-74.87578,-74.65916,-75.03538,-75.25101], "fy":[-124.97135,-125.0022,-124.73328,-124.70266]}, + {"t":0.8576, "x":5.25283, "y":5.48229, "heading":-0.95212, "vx":-3.71757, "vy":2.12951, "omega":0.01771, "ax":-3.61336, "ay":-6.52214, "alpha":-0.02948, "fx":[-61.4394,-61.32681,-61.48506,-61.59749], "fy":[-111.00473,-110.99635,-110.87486,-110.88329]}, + {"t":0.87555, "x":5.18551, "y":5.51946, "heading":-0.95181, "vx":-3.78243, "vy":2.01244, "omega":0.01718, "ax":-2.36696, "ay":-4.55416, "alpha":-0.01692, "fx":[-40.26064,-40.19363,-40.26187,-40.32886], "fy":[-77.51235,-77.48494,-77.41765,-77.44509]}, + {"t":0.8935, "x":5.11723, "y":5.55485, "heading":-0.9515, "vx":-3.82492, "vy":1.93069, "omega":0.01688, "ax":-0.10749, "ay":-0.1353, "alpha":0.0059, "fx":[-1.82507,-1.84847,-1.83177,-1.80837], "fy":[-2.28133,-2.29802,-2.32142,-2.30474]}, + {"t":0.91145, "x":5.04856, "y":5.58949, "heading":-0.95119, "vx":-3.82685, "vy":1.92826, "omega":0.01699, "ax":0.94084, "ay":1.98327, "alpha":-0.13497, "fx":[15.94072,16.47773,16.06638,15.52918], "fy":[33.29193,33.64444,34.1772,33.82563]}, + {"t":0.93282, "x":4.96698, "y":5.63115, "heading":-0.95083, "vx":-3.80674, "vy":1.97065, "omega":0.0141, "ax":0.46541, "ay":0.89416, "alpha":0.00014, "fx":[7.91652,7.91597,7.91637,7.91692], "fy":[15.20983,15.20944,15.2089,15.20929]}, + {"t":0.9542, "x":4.88573, "y":5.67348, "heading":-0.95053, "vx":-3.7968, "vy":1.98976, "omega":0.0141, "ax":0.20883, "ay":0.39725, "alpha":0.00054, "fx":[3.5524,3.55026,3.55179,3.55394], "fy":[6.75902,6.7575,6.75535,6.75687]}, + {"t":0.97557, "x":4.80463, "y":5.71609, "heading":-0.95023, "vx":-3.79233, "vy":1.99825, "omega":0.01412, "ax":0.09265, "ay":0.17554, "alpha":0.00028, "fx":[1.57608,1.57497,1.57577,1.57687], "fy":[2.98684,2.98605,2.98494,2.98574]}, + {"t":0.99694, "x":4.7236, "y":5.75884, "heading":-0.94993, "vx":-3.79035, "vy":2.002, "omega":0.01412, "ax":0.04098, "ay":0.0775, "alpha":0.00015, "fx":[0.69716,0.69655,0.69698,0.69759], "fy":[1.31885,1.31842,1.31781,1.31824]}, + {"t":1.01831, "x":4.6426, "y":5.80165, "heading":-0.94963, "vx":-3.78948, "vy":2.00365, "omega":0.01413, "ax":0.01811, "ay":0.03422, "alpha":0.0001, "fx":[0.3081,0.30773,0.308,0.30838], "fy":[0.58238,0.58211,0.58173,0.58201]}, + {"t":1.03969, "x":4.56161, "y":5.84448, "heading":-0.94932, "vx":-3.78909, "vy":2.00439, "omega":0.01413, "ax":0.008, "ay":0.01511, "alpha":0.00007, "fx":[0.13616,0.13588,0.13608,0.13636], "fy":[0.25725,0.25705,0.25678,0.25697]}, + {"t":1.06106, "x":4.48063, "y":5.88732, "heading":-0.94902, "vx":-3.78892, "vy":2.00471, "omega":0.01413, "ax":0.00354, "ay":0.00667, "alpha":0.00006, "fx":[0.06021,0.05998,0.06014,0.06037], "fy":[0.11371,0.11354,0.11331,0.11348]}, + {"t":1.08243, "x":4.39965, "y":5.93017, "heading":-0.94872, "vx":-3.78884, "vy":2.00485, "omega":0.01413, "ax":0.00157, "ay":0.00295, "alpha":0.00005, "fx":[0.02667,0.02645,0.02661,0.02682], "fy":[0.05033,0.05018,0.04996,0.05012]}, + {"t":1.1038, "x":4.31868, "y":5.97302, "heading":-0.94842, "vx":-3.78881, "vy":2.00491, "omega":0.01413, "ax":0.0007, "ay":0.0013, "alpha":0.00005, "fx":[0.01185,0.01165,0.0118,0.012], "fy":[0.02234,0.0222,0.02199,0.02214]}, + {"t":1.12518, "x":4.2377, "y":6.01587, "heading":-0.94812, "vx":-3.78879, "vy":2.00494, "omega":0.01413, "ax":0.00031, "ay":0.00058, "alpha":0.00005, "fx":[0.00531,0.00511,0.00526,0.00546], "fy":[0.00999,0.00984,0.00964,0.00979]}, + {"t":1.14655, "x":4.15673, "y":6.05872, "heading":-0.94781, "vx":-3.78879, "vy":2.00495, "omega":0.01413, "ax":0.00014, "ay":0.00026, "alpha":0.00005, "fx":[0.00242,0.00222,0.00237,0.00256], "fy":[0.00452,0.00438,0.00418,0.00433]}, + {"t":1.16792, "x":4.07575, "y":6.10157, "heading":-0.94751, "vx":-3.78878, "vy":2.00496, "omega":0.01413, "ax":0.00007, "ay":0.00011, "alpha":0.00005, "fx":[0.00114,0.00094,0.00108,0.00128], "fy":[0.0021,0.00196,0.00176,0.0019]}, + {"t":1.18929, "x":3.99477, "y":6.14442, "heading":-0.94721, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.00003, "ay":0.00005, "alpha":0.00005, "fx":[0.00056,0.00036,0.0005,0.0007], "fy":[0.001,0.00086,0.00066,0.0008]}, + {"t":1.21067, "x":3.9138, "y":6.18727, "heading":-0.94691, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.00001, "ay":0.00002, "alpha":0.00005, "fx":[0.00026,0.00007,0.00021,0.0004], "fy":[0.00044,0.0003,0.00011,0.00025]}, + {"t":1.23204, "x":3.83282, "y":6.23012, "heading":-0.9466, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.0, "ay":-0.00001, "alpha":0.00005, "fx":[0.00005,-0.00014,0.0,0.0002], "fy":[0.00005,-0.00009,-0.00029,-0.00015]}, + {"t":1.25341, "x":3.75185, "y":6.27297, "heading":-0.9463, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00001, "ay":-0.00004, "alpha":0.00005, "fx":[-0.00022,-0.00042,-0.00028,-0.00008], "fy":[-0.00047,-0.00061,-0.00081,-0.00067]}, + {"t":1.27478, "x":3.67087, "y":6.31582, "heading":-0.946, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00005, "ay":-0.0001, "alpha":0.00005, "fx":[-0.00076,-0.00095,-0.00081,-0.00062], "fy":[-0.00148,-0.00163,-0.00182,-0.00168]}, + {"t":1.29616, "x":3.5899, "y":6.35867, "heading":-0.9457, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00012, "ay":-0.00023, "alpha":0.00005, "fx":[-0.00193,-0.00213,-0.00199,-0.00179], "fy":[-0.0037,-0.00384,-0.00404,-0.0039]}, + {"t":1.31753, "x":3.50892, "y":6.40153, "heading":-0.9454, "vx":-3.78879, "vy":2.00496, "omega":0.01414, "ax":-0.00027, "ay":-0.00052, "alpha":0.00005, "fx":[-0.00457,-0.00477,-0.00462,-0.00443], "fy":[-0.00869,-0.00883,-0.00902,-0.00888]}, + {"t":1.3389, "x":3.42794, "y":6.44438, "heading":-0.94509, "vx":-3.78879, "vy":2.00494, "omega":0.01414, "ax":-0.00062, "ay":-0.00118, "alpha":0.00005, "fx":[-0.01054,-0.01073,-0.01059,-0.0104], "fy":[-0.01996,-0.0201,-0.02029,-0.02016]}, + {"t":1.36027, "x":3.34697, "y":6.48723, "heading":-0.94479, "vx":-3.7888, "vy":2.00492, "omega":0.01414, "ax":-0.00142, "ay":-0.00268, "alpha":0.00005, "fx":[-0.02406,-0.02424,-0.02411,-0.02392], "fy":[-0.0455,-0.04564,-0.04582,-0.04569]}, + {"t":1.38165, "x":3.26599, "y":6.53008, "heading":-0.94449, "vx":-3.78884, "vy":2.00486, "omega":0.01415, "ax":-0.00322, "ay":-0.00608, "alpha":0.00004, "fx":[-0.05467,-0.05483,-0.05471,-0.05455], "fy":[-0.10334,-0.10346,-0.10363,-0.10351]}, + {"t":1.40302, "x":3.18501, "y":6.57292, "heading":-0.94419, "vx":-3.7889, "vy":2.00473, "omega":0.01415, "ax":-0.00729, "ay":-0.01378, "alpha":0.00003, "fx":[-0.12398,-0.12411,-0.12402,-0.12389], "fy":[-0.23434,-0.23443,-0.23455,-0.23447]}, + {"t":1.42439, "x":3.10403, "y":6.61577, "heading":-0.94388, "vx":-3.78906, "vy":2.00444, "omega":0.01415, "ax":-0.01652, "ay":-0.03122, "alpha":0.00001, "fx":[-0.28092,-0.28094,-0.28092,-0.28089], "fy":[-0.53106,-0.53108,-0.53111,-0.53108]}, + {"t":1.44576, "x":3.02305, "y":6.6586, "heading":-0.94358, "vx":-3.78941, "vy":2.00377, "omega":0.01415, "ax":-0.0374, "ay":-0.07072, "alpha":-0.00006, "fx":[-0.63614,-0.63589,-0.63607,-0.63632], "fy":[-1.20315,-1.20297,-1.20272,-1.2029]}, + {"t":1.46714, "x":2.94205, "y":6.70141, "heading":-0.94328, "vx":-3.79021, "vy":2.00226, "omega":0.01415, "ax":-0.08564, "ay":-0.15963, "alpha":-0.00317, "fx":[-1.45838,-1.44588,-1.45495,-1.46745], "fy":[-2.72598,-2.71692,-2.70442,-2.71348]}, + {"t":1.48851, "x":2.86102, "y":6.74417, "heading":-0.94298, "vx":-3.79204, "vy":1.99885, "omega":0.01408, "ax":-0.36107, "ay":-0.27242, "alpha":-0.65828, "fx":[-6.49392,-3.90252,-5.79103,-8.37948], "fy":[-6.87222,-4.99489,-2.39315,-4.2747]}, + {"t":1.50988, "x":2.7799, "y":6.78682, "heading":-0.94268, "vx":-3.79976, "vy":1.99302, "omega":0.00001, "ax":-0.19978, "ay":-0.30768, "alpha":-0.00026, "fx":[-3.39833,-3.39731,-3.39805,-3.39907], "fy":[-5.23448,-5.23374,-5.23272,-5.23346]}, + {"t":1.54495, "x":2.64653, "y":6.85652, "heading":-0.94268, "vx":-3.80676, "vy":1.98224, "omega":0.0, "ax":-0.03401, "ay":-0.06535, "alpha":0.0, "fx":[-0.57851,-0.5785,-0.57851,-0.57851], "fy":[-1.1116,-1.1116,-1.1116,-1.1116]}, + {"t":1.58001, "x":2.51302, "y":6.92599, "heading":-0.94268, "vx":-3.80796, "vy":1.97994, "omega":0.0, "ax":0.01012, "ay":-0.02307, "alpha":0.0, "fx":[0.17206,0.17206,0.17206,0.17206], "fy":[-0.39235,-0.39235,-0.39235,-0.39235]}, + {"t":1.61508, "x":2.37949, "y":6.99541, "heading":-0.94268, "vx":-3.8076, "vy":1.97913, "omega":0.0, "ax":8.23713, "ay":-4.28328, "alpha":-0.00004, "fx":[140.11107,140.11121,140.11125,140.11111], "fy":[-72.85752,-72.85727,-72.85713,-72.85737]}, + {"t":1.65015, "x":2.25103, "y":7.06218, "heading":-0.94268, "vx":-3.51875, "vy":1.82893, "omega":0.0, "ax":9.04507, "ay":-4.70151, "alpha":0.0, "fx":[153.85405,153.85405,153.85405,153.85405], "fy":[-79.97128,-79.97127,-79.97127,-79.97127]}, + {"t":1.68521, "x":2.1332, "y":7.12342, "heading":-0.94268, "vx":-3.20157, "vy":1.66407, "omega":0.0, "ax":9.09619, "ay":-4.72796, "alpha":0.0, "fx":[154.72364,154.72363,154.72363,154.72364], "fy":[-80.42131,-80.42133,-80.42134,-80.42132]}, + {"t":1.72028, "x":2.02653, "y":7.17887, "heading":-0.94268, "vx":-2.88259, "vy":1.49827, "omega":0.0, "ax":9.11463, "ay":-4.73751, "alpha":0.0, "fx":[155.03731,155.0373,155.03729,155.0373], "fy":[-80.58364,-80.58367,-80.58368,-80.58365]}, + {"t":1.75535, "x":1.93105, "y":7.2285, "heading":-0.94268, "vx":-2.56297, "vy":1.33214, "omega":0.0, "ax":9.12413, "ay":-4.74242, "alpha":0.0, "fx":[155.19887,155.19886,155.19885,155.19887], "fy":[-80.66726,-80.66728,-80.66729,-80.66727]}, + {"t":1.79042, "x":1.84678, "y":7.2723, "heading":-0.94268, "vx":-2.24302, "vy":1.16584, "omega":0.0, "ax":9.12992, "ay":-4.74542, "alpha":0.0, "fx":[155.29734,155.29733,155.29732,155.29733], "fy":[-80.71821,-80.71824,-80.71826,-80.71823]}, + {"t":1.82548, "x":1.77374, "y":7.31026, "heading":-0.94268, "vx":-1.92286, "vy":0.99943, "omega":0.0, "ax":9.13382, "ay":-4.74744, "alpha":0.0, "fx":[155.36362,155.36361,155.3636,155.36362], "fy":[-80.75252,-80.75255,-80.75256,-80.75253]}, + {"t":1.86055, "x":1.71193, "y":7.34239, "heading":-0.94268, "vx":-1.60256, "vy":0.83295, "omega":0.0, "ax":9.13662, "ay":-4.74889, "alpha":0.0, "fx":[155.41128,155.41126,155.41126,155.41127], "fy":[-80.77718,-80.77721,-80.77723,-80.7772]}, + {"t":1.89562, "x":1.66135, "y":7.36868, "heading":-0.94268, "vx":-1.28217, "vy":0.66643, "omega":0.0, "ax":9.13873, "ay":-4.74998, "alpha":0.0, "fx":[155.44719,155.44717,155.44717,155.44718], "fy":[-80.79576,-80.7958,-80.79581,-80.79578]}, + {"t":1.93068, "x":1.622, "y":7.38913, "heading":-0.94268, "vx":-0.9617, "vy":0.49986, "omega":0.0, "ax":9.14038, "ay":-4.75083, "alpha":0.0, "fx":[155.47522,155.4752,155.4752,155.47521], "fy":[-80.81027,-80.8103,-80.81032,-80.81029]}, + {"t":1.96575, "x":1.5939, "y":7.40373, "heading":-0.94268, "vx":-0.64118, "vy":0.33326, "omega":0.0, "ax":9.1417, "ay":-4.75152, "alpha":0.0, "fx":[155.49771,155.49769,155.49768,155.4977], "fy":[-80.82191,-80.82194,-80.82196,-80.82192]}, + {"t":2.00082, "x":1.57704, "y":7.4125, "heading":-0.94268, "vx":-0.32061, "vy":0.16664, "omega":0.0, "ax":9.14278, "ay":-4.75208, "alpha":0.0, "fx":[155.51615,155.51613,155.51612,155.51614], "fy":[-80.83145,-80.83148,-80.8315,-80.83147]}, + {"t":2.03588, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/HTofetch.traj b/src/main/deploy/choreo/HTofetch.traj new file mode 100644 index 00000000..9a3dc730 --- /dev/null +++ b/src/main/deploy/choreo/HTofetch.traj @@ -0,0 +1,123 @@ +{ + "name":"HTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.946532726287842, "y":4.711103439331055, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.048557281494141, "y":5.589489936828613, "heading":-1.773711057838377, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9426766239853251, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "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":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.946532726287842 m", "val":5.946532726287842}, "y":{"exp":"4.711103439331055 m", "val":4.711103439331055}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"-1.773711057838377 rad", "val":-1.773711057838377}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "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.43417,0.79118,1.46954,1.99555], + "samples":[ + {"t":0.0, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.27299, "ay":7.77502, "alpha":8.68726, "fx":[150.78654,114.04652,59.9091,102.06473], "fy":[89.16091,133.06217,164.63284,142.14733]}, + {"t":0.02412, "x":5.77874, "y":3.95352, "heading":3.14159, "vx":0.15131, "vy":0.18754, "omega":0.20954, "ax":6.0078, "ay":7.96277, "alpha":8.88088, "fx":[148.71413,110.96411,54.48007,94.60521], "fy":[92.51688,135.61712,166.48436,147.15926]}, + {"t":0.04824, "x":5.78413, "y":3.96036, "heading":-3.13654, "vx":0.29622, "vy":0.37961, "omega":0.42376, "ax":5.68158, "ay":8.17668, "alpha":9.11051, "fx":[146.01699,107.01919,47.91824,85.61379], "fy":[96.64948,138.721,168.46249,152.4991]}, + {"t":0.07236, "x":5.79293, "y":3.97189, "heading":-3.12632, "vx":0.43327, "vy":0.57684, "omega":0.64351, "ax":5.27293, "ay":8.42018, "alpha":9.38563, "fx":[142.43363,101.95205,39.9169,74.4615], "fy":[101.77732,142.45112,170.50327,158.1674]}, + {"t":0.09648, "x":5.80492, "y":3.98825, "heading":-3.11079, "vx":0.56045, "vy":0.77994, "omega":0.8699, "ax":4.7509, "ay":8.69488, "alpha":9.71996, "fx":[137.53913,95.38289,30.07321,60.25089], "fy":[108.20728,146.88912,172.47389,164.01934]}, + {"t":0.1206, "x":5.81982, "y":4.0096, "heading":-3.08981, "vx":0.67505, "vy":0.98967, "omega":1.10435, "ax":4.07095, "ay":8.99714, "alpha":10.1303, "fx":[130.61162,86.74377,17.86587,41.76129], "fy":[116.36447,152.10123,174.1177,169.57168]}, + {"t":0.14472, "x":5.83729, "y":4.03608, "heading":-3.06317, "vx":0.77324, "vy":1.20668, "omega":1.3487, "ax":3.17076, "ay":9.31069, "alpha":10.62556, "fx":[120.36529,75.16994,2.651,17.54882], "fy":[126.80131,158.08233,174.95989,173.64491]}, + {"t":0.16885, "x":5.85686, "y":4.0679, "heading":-3.03064, "vx":0.84973, "vy":1.43127, "omega":1.605, "ax":1.96992, "ay":9.5936, "alpha":11.16109, "fx":[104.41579,59.34076,-16.27592,-13.44975], "fy":[140.06718,164.60795,174.16152,173.9007]}, + {"t":0.19297, "x":5.87793, "y":4.10521, "heading":-2.99193, "vx":0.89724, "vy":1.66267, "omega":1.87422, "ax":0.37977, "ay":9.76026, "alpha":11.51147, "fx":[78.34541,37.31434,-39.44548,-50.37516], "fy":[155.96257,170.86748,170.34796,166.89863]}, + {"t":0.21709, "x":5.89968, "y":4.14816, "heading":-2.94672, "vx":0.9064, "vy":1.8981, "omega":2.15188, "ax":-1.66842, "ay":9.6567, "alpha":11.11248, "fx":[35.21357,6.65004,-66.65369,-88.72712], "fy":[170.74107,174.66996,161.58341,150.03609]}, + {"t":0.24121, "x":5.92106, "y":4.19675, "heading":-2.89482, "vx":0.86616, "vy":2.13102, "omega":2.41993, "ax":-4.12167, "ay":9.02355, "alpha":9.40623, "fx":[-28.43685,-34.18138,-96.12007,-121.69527], "fy":[171.84636,171.32014,145.9273,124.85795]}, + {"t":0.26533, "x":5.94075, "y":4.25078, "heading":-2.83644, "vx":0.76674, "vy":2.34868, "omega":2.64681, "ax":-6.56019, "ay":7.64165, "alpha":6.90464, "fx":[-95.30646,-81.80189,-124.1816,-145.05759], "fy":[145.83253,154.28437,122.89806,96.91403]}, + {"t":0.28945, "x":5.95734, "y":4.30965, "heading":-2.7726, "vx":0.6085, "vy":2.533, "omega":2.81336, "ax":-8.39333, "ay":5.76456, "alpha":4.52617, "fx":[-139.70708,-125.27568,-146.76738,-159.32244], "fy":[104.39031,121.66165,94.80124,71.36065]}, + {"t":0.31357, "x":5.96957, "y":4.37243, "heading":-2.70474, "vx":0.40605, "vy":2.67205, "omega":2.92253, "ax":-9.4759, "ay":3.89633, "alpha":2.47042, "fx":[-161.18736,-154.40762,-161.87367,-167.26094], "fy":[67.12302,81.71386,65.86187,50.40289]}, + {"t":0.33769, "x":5.97661, "y":4.43801, "heading":-2.63425, "vx":0.17748, "vy":2.76603, "omega":2.98212, "ax":-10.00848, "ay":2.31796, "alpha":0.80567, "fx":[-170.32714,-168.95063,-170.24339,-171.44449], "fy":[39.15195,44.80455,39.70585,34.04867]}, + {"t":0.36181, "x":5.97798, "y":4.50541, "heading":-2.56232, "vx":-0.06393, "vy":2.82194, "omega":3.00155, "ax":-10.22378, "ay":1.07713, "alpha":-0.46361, "fx":[-173.89517,-174.2219,-173.94103,-173.55603], "fy":[18.63832,15.17743,17.99656,21.47464]}, + {"t":0.38593, "x":5.97346, "y":4.57379, "heading":-2.48992, "vx":-0.31054, "vy":2.84792, "omega":2.99037, "ax":-10.27691, "ay":0.12181, "alpha":-1.41324, "fx":[-174.95278,-174.79825,-174.92219,-174.55574], "fy":[3.1938,-7.49568,0.82341,11.76629]}, + {"t":0.41005, "x":5.96298, "y":4.64252, "heading":-2.41779, "vx":-0.55842, "vy":2.85086, "omega":2.95628, "ax":-10.2526, "ay":-0.61636, "alpha":-2.1278, "fx":[-174.82613,-173.25562,-174.5309,-174.96261], "fy":[-8.86335,-24.79841,-12.4341,4.15954]}, + {"t":0.43417, "x":5.94653, "y":4.7111, "heading":-2.34648, "vx":-0.80572, "vy":2.83599, "omega":2.90496, "ax":-10.20363, "ay":-0.98367, "alpha":-2.64213, "fx":[-174.22263,-171.40923,-173.78136,-174.83022], "fy":[-15.36232,-34.50419,-18.51246,1.45136]}, + {"t":0.45118, "x":5.93136, "y":4.75917, "heading":-2.29709, "vx":-0.97919, "vy":2.81927, "omega":2.86004, "ax":-10.16841, "ay":-1.11811, "alpha":-3.41215, "fx":[-173.93995,-169.68118,-173.4866,-174.73931], "fy":[-18.06774,-42.01421,-20.4555,4.46223]}, + {"t":0.46818, "x":5.91324, "y":4.80694, "heading":-2.24847, "vx":-1.15205, "vy":2.80026, "omega":2.80203, "ax":-10.12147, "ay":-1.26703, "alpha":-4.264, "fx":[-173.54956,-167.35236,-173.16799,-174.58352], "fy":[-21.29297,-50.33093,-22.27861,7.69507]}, + {"t":0.48518, "x":5.8922, "y":4.85436, "heading":-2.20084, "vx":-1.32412, "vy":2.77872, "omega":2.72955, "ax":-10.06003, "ay":-1.43072, "alpha":-5.20016, "fx":[-173.02079,-164.26848,-172.83045,-174.35319], "fy":[-25.04057,-59.46893,-23.93336,11.09811]}, + {"t":0.50218, "x":5.86823, "y":4.9014, "heading":-2.15443, "vx":-1.49515, "vy":2.7544, "omega":2.64114, "ax":-9.98152, "ay":-1.60763, "alpha":-6.21499, "fx":[-172.32289,-160.2796,-172.48351,-174.04491], "fy":[-29.27881,-69.34681,-25.33743,14.58194]}, + {"t":0.51918, "x":5.84137, "y":4.94799, "heading":-2.10953, "vx":-1.66483, "vy":2.72707, "omega":2.53548, "ax":-9.88504, "ay":-1.7924, "alpha":-7.285, "fx":[-171.43518,-155.316,-172.14709,-173.66884], "fy":[-33.91133,-79.68695,-26.34235,17.98812]}, + {"t":0.53618, "x":5.81164, "y":4.99409, "heading":-2.06643, "vx":-1.83288, "vy":2.6966, "omega":2.41164, "ax":-9.77427, "ay":-1.97351, "alpha":-8.35496, "fx":[-170.36649,-149.54371,-171.85945,-173.26044], "fy":[-38.73501,-89.89038,-26.69615,21.0462]}, + {"t":0.55318, "x":5.77907, "y":5.03965, "heading":-2.02543, "vx":-1.99905, "vy":2.66305, "omega":2.2696, "ax":-9.6596, "ay":-2.13446, "alpha":-9.33823, "fx":[-169.17304,-143.50076,-171.66934,-172.88527], "fy":[-43.44073,-99.07807,-26.09532,23.38779]}, + {"t":0.57018, "x":5.74369, "y":5.08461, "heading":-1.98685, "vx":-2.16326, "vy":2.62676, "omega":2.11085, "ax":-9.55264, "ay":-2.26527, "alpha":-10.1683, "fx":[-167.93005,-137.83413,-171.58035,-172.60592], "fy":[-47.77485,-106.6258,-24.4781,24.7527]}, + {"t":0.58718, "x":5.70553, "y":5.12894, "heading":-1.95096, "vx":-2.32566, "vy":2.58825, "omega":1.93799, "ax":-9.4559, "ay":-2.37292, "alpha":-10.85118, "fx":[-166.66569,-132.75911,-171.50917,-172.43441], "fy":[-51.72333,-112.65743,-22.25268,25.18289]}, + {"t":0.60418, "x":5.66463, "y":5.1726, "heading":-1.91802, "vx":-2.48641, "vy":2.54791, "omega":1.75351, "ax":-9.36452, "ay":-2.47357, "alpha":-11.42525, "fx":[-165.35693,-128.10688,-171.34078,-172.3464], "fy":[-55.42147,-117.65689,-20.06803,24.84747]}, + {"t":0.62118, "x":5.62101, "y":5.21556, "heading":-1.88821, "vx":-2.64561, "vy":2.50586, "omega":1.55928, "ax":-9.2727, "ay":-2.5839, "alpha":-11.91027, "fx":[-163.95972,-123.65418,-170.97475,-172.31548], "fy":[-59.01405,-122.01158,-18.60748,23.8273]}, + {"t":0.63818, "x":5.57469, "y":5.25778, "heading":-1.8617, "vx":-2.80325, "vy":2.46193, "omega":1.3568, "ax":-9.1747, "ay":-2.72184, "alpha":-12.30062, "fx":[-162.4094,-119.20429,-170.30279,-172.31959], "fy":[-62.64705,-125.97382,-18.6346,22.06452]}, + {"t":0.65518, "x":5.52571, "y":5.29924, "heading":-1.83863, "vx":-2.95922, "vy":2.41566, "omega":1.14769, "ax":-9.06257, "ay":-2.91102, "alpha":-12.56938, "fx":[-160.59724,-114.54814,-169.12893,-172.33255], "fy":[-66.51139,-129.73009,-21.14969,19.32862]}, + {"t":0.67218, "x":5.47409, "y":5.33989, "heading":-1.81912, "vx":-3.11329, "vy":2.36617, "omega":0.93401, "ax":-8.92236, "ay":-3.18752, "alpha":-12.66127, "fx":[-158.32707,-109.45772,-166.97985,-172.30235], "fy":[-70.89515,-133.40578,-27.6515,15.0772]}, + {"t":0.68918, "x":5.41988, "y":5.37965, "heading":-1.80324, "vx":-3.26497, "vy":2.31199, "omega":0.71876, "ax":-8.72592, "ay":-3.6109, "alpha":-12.46746, "fx":[-155.22036,-103.78544,-162.62525,-172.07059], "fy":[-76.26631,-136.96538,-40.49696,8.04735]}, + {"t":0.70618, "x":5.36311, "y":5.41844, "heading":-1.79102, "vx":-3.41331, "vy":2.2506, "omega":0.50681, "ax":-8.41009, "ay":-4.27769, "alpha":-11.77557, "fx":[-150.43332,-97.74685,-153.05549,-170.97707], "fy":[-83.49781,-139.97721,-62.64064,-4.93318]}, + {"t":0.72318, "x":5.30387, "y":5.45608, "heading":-1.78241, "vx":-3.55629, "vy":2.17788, "omega":0.30662, "ax":-7.81345, "ay":-5.33295, "alpha":-10.12216, "fx":[-141.4177,-91.72247,-132.95739,-165.52045], "fy":[-94.84751,-141.64569,-94.09984,-32.25477]}, + {"t":0.74018, "x":5.24228, "y":5.49233, "heading":-1.77719, "vx":-3.68912, "vy":2.08722, "omega":0.13454, "ax":-6.3984, "ay":-6.99014, "alpha":-6.02977, "fx":[-117.78402,-82.70119,-98.93651,-135.91822], "fy":[-116.72541,-142.24913,-126.12647,-90.50006]}, + {"t":0.75718, "x":5.17864, "y":5.52681, "heading":-1.77491, "vx":-3.79789, "vy":1.96838, "omega":0.03204, "ax":-3.89522, "ay":-8.15098, "alpha":0.36864, "fx":[-65.09001,-67.93289,-67.40263,-64.60107], "fy":[-138.92518,-137.67233,-138.38159,-139.60417]}, + {"t":0.77418, "x":5.11351, "y":5.55909, "heading":-1.77436, "vx":-3.86411, "vy":1.82981, "omega":0.0383, "ax":5.0962, "ay":-4.90822, "alpha":18.79489, "fx":[147.93722,90.02279,9.17581,99.60351], "fy":[-34.75609,-30.5035,-147.50408,-121.18609]}, + {"t":0.79118, "x":5.04856, "y":5.58949, "heading":-1.77371, "vx":-3.77748, "vy":1.74637, "omega":0.35782, "ax":5.72809, "ay":-0.36717, "alpha":13.85288, "fx":[124.97881,81.89562,67.79369,115.06483], "fy":[22.18273,52.67748,-51.7097,-48.13262]}, + {"t":0.81541, "x":4.95872, "y":5.63169, "heading":-1.76504, "vx":-3.6387, "vy":1.73748, "omega":0.69344, "ax":4.25886, "ay":-1.81535, "alpha":11.65475, "fx":[103.11066,59.20872,39.81011,87.63865], "fy":[-5.15853,8.79363,-65.37568,-61.77369]}, + {"t":0.83963, "x":4.87181, "y":5.67325, "heading":-1.74824, "vx":-3.53552, "vy":1.6935, "omega":0.9758, "ax":3.30195, "ay":-2.23199, "alpha":9.611, "fx":[84.08423,42.20685,27.9809,70.38896], "fy":[-16.56985,-9.40801,-63.64966,-62.2346]}, + {"t":0.86386, "x":4.78713, "y":5.71363, "heading":-1.7246, "vx":-3.45552, "vy":1.63942, "omega":1.20865, "ax":2.77807, "ay":-2.01032, "alpha":7.8581, "fx":[70.01967,33.98063,24.7133,60.30311], "fy":[-16.64297,-11.93326,-54.12995,-54.074]}, + {"t":0.88809, "x":4.70423, "y":5.75276, "heading":-1.69532, "vx":-3.38822, "vy":1.59072, "omega":1.39903, "ax":2.4085, "ay":-1.47509, "alpha":6.1571, "fx":[58.06424,29.40638,24.06325,52.33746], "fy":[-11.17873,-7.9596,-40.28581,-40.93944]}, + {"t":0.91232, "x":4.62285, "y":5.79086, "heading":-1.66142, "vx":-3.32987, "vy":1.55498, "omega":1.5482, "ax":2.03705, "ay":-0.80314, "alpha":4.40081, "fx":[46.23178,25.59112,23.13107,43.64466], "fy":[-3.55819,-1.64892,-24.3144,-25.12305]}, + {"t":0.93654, "x":4.54277, "y":5.8283, "heading":-1.62392, "vx":-3.28051, "vy":1.53552, "omega":1.65482, "ax":1.61692, "ay":-0.11727, "alpha":2.64757, "fx":[34.13689,21.58365,20.86416,33.42884], "fy":[4.19861,5.0066,-8.34065,-8.84377]}, + {"t":0.96077, "x":4.46377, "y":5.86547, "heading":-1.58382, "vx":-3.24134, "vy":1.53268, "omega":1.71896, "ax":1.1681, "ay":0.48658, "alpha":1.04319, "fx":[22.38498,17.36443,17.34638,22.38045], "fy":[10.77459,10.91209,5.76208,5.65741]}, + {"t":0.985, "x":4.38558, "y":5.90274, "heading":-1.54218, "vx":-3.21304, "vy":1.54447, "omega":1.74423, "ax":0.73504, "ay":0.95081, "alpha":-0.28191, "fx":[11.84283,13.2204,13.16204,11.7863], "fy":[15.47785,15.49976,16.86729,16.84694]}, + {"t":1.00923, "x":4.30795, "y":5.94044, "heading":-1.49992, "vx":-3.19523, "vy":1.5675, "omega":1.73741, "ax":0.35436, "ay":1.26167, "alpha":-1.27392, "fx":[3.12801,9.4243,8.90138,2.65629], "fy":[18.21104,18.60283,24.69995,24.32884]}, + {"t":1.03345, "x":4.23065, "y":5.97878, "heading":-1.45783, "vx":-3.18665, "vy":1.59807, "omega":1.70654, "ax":0.04413, "ay":1.43568, "alpha":-1.94842, "fx":[-3.53913,6.14199,4.97015,-4.57016], "fy":[19.2691,20.32412,29.5551,28.53397]}, + {"t":1.05768, "x":4.15345, "y":6.01792, "heading":-1.41648, "vx":-3.18558, "vy":1.63285, "omega":1.65934, "ax":-0.19231, "ay":1.50176, "alpha":-2.35535, "fx":[-8.24825,3.44418,1.59524,-9.8759], "fy":[19.09592,20.90384,31.97321,30.20482]}, + {"t":1.08191, "x":4.07622, "y":6.05792, "heading":-1.37628, "vx":-3.19024, "vy":1.66924, "omega":1.60227, "ax":-0.36037, "ay":1.49, "alpha":-2.55317, "fx":[-11.26466,1.33273,-1.12709,-13.46037], "fy":[18.12597,20.62665,32.5417,30.08337]}, + {"t":1.10613, "x":3.99882, "y":6.0988, "heading":-1.33746, "vx":-3.19897, "vy":1.70534, "omega":1.54042, "ax":-0.47011, "ay":1.42698, "alpha":-2.59764, "fx":[-12.93132,-0.24119,-3.19508,-15.61806], "fy":[16.71559,19.76794,31.80756,28.79889]}, + {"t":1.13036, "x":3.92118, "y":6.14054, "heading":-1.30014, "vx":-3.21036, "vy":1.73991, "omega":1.47748, "ax":-0.53407, "ay":1.33433, "alpha":-2.53866, "fx":[-13.61198,-1.36382,-4.67813,-16.68353], "fy":[15.12524,18.56564,30.24616,26.84909]}, + {"t":1.15459, "x":3.84325, "y":6.18308, "heading":-1.26435, "vx":-3.2233, "vy":1.77223, "omega":1.41598, "ax":-0.56604, "ay":1.22862, "alpha":-2.42015, "fx":[-13.66065,-2.14672,-5.69889,-17.00653], "fy":[13.5238,17.20735,28.25244,24.61011]}, + {"t":1.17882, "x":3.76499, "y":6.22638, "heading":-1.23004, "vx":-3.23701, "vy":1.802, "omega":1.35735, "ax":-0.58026, "ay":1.12173, "alpha":-2.28072, "fx":[-13.40724,-2.71629,-6.41781,-16.93892], "fy":[12.00015,15.82723,26.14143,22.35222]}, + {"t":1.20304, "x":3.6864, "y":6.27036, "heading":-1.19716, "vx":-3.25107, "vy":1.82918, "omega":1.30209, "ax":-0.59107, "ay":1.02143, "alpha":-2.1548, "fx":[-13.1558,-3.20707,-7.02243,-16.83039], "fy":[10.57579,14.50931,24.15571,20.25623]}, + {"t":1.22727, "x":3.60746, "y":6.31498, "heading":-1.16561, "vx":-3.26539, "vy":1.85392, "omega":1.24988, "ax":-0.61314, "ay":0.9321, "alpha":-2.07471, "fx":[-13.19545,-3.76126,-7.72478,-17.03606], "fy":[9.21451,13.2942,22.47961,18.43045]}, + {"t":1.2515, "x":3.52817, "y":6.36017, "heading":-1.13533, "vx":-3.28024, "vy":1.8765, "omega":1.19962, "ax":-0.66246, "ay":0.8554, "alpha":-2.07387, "fx":[-13.82483,-4.53631,-8.77108,-17.94048], "fy":[7.82622,12.18662,21.26007,16.92742]}, + {"t":1.27573, "x":3.4485, "y":6.40588, "heading":-1.10627, "vx":-3.29629, "vy":1.89723, "omega":1.14938, "ax":-0.75812, "ay":0.79095, "alpha":-2.19166, "fx":[-15.39164,-5.72189,-10.46671,-20.00128], "fy":[6.26155,11.16149,20.6334,15.75887]}, + {"t":1.29995, "x":3.36842, "y":6.45208, "heading":-1.07842, "vx":-3.31466, "vy":1.91639, "omega":1.09628, "ax":-0.9252, "ay":0.73674, "alpha":-2.48017, "fx":[-18.34889,-7.56826,-13.21945,-23.81327], "fy":[4.29385,10.16737,20.75919,14.90682]}, + {"t":1.32418, "x":3.28784, "y":6.49872, "heading":-1.05186, "vx":-3.33707, "vy":1.93424, "omega":1.03619, "ax":-1.1984, "ay":0.68948, "alpha":-3.01267, "fx":[-23.32635,-10.42681,-17.60099,-30.1834], "fy":[1.58568,9.12802,21.86675,14.33089]}, + {"t":1.34841, "x":3.20664, "y":6.54579, "heading":-1.02676, "vx":-3.36611, "vy":1.95094, "omega":0.9632, "ax":-1.6251, "ay":0.64529, "alpha":-3.89147, "fx":[-31.1944,-14.79783,-24.4107,-40.16708], "fy":[-2.35485,7.9498,24.33072,13.9791]}, + {"t":1.37263, "x":3.12461, "y":6.59324, "heading":-1.00342, "vx":-3.40548, "vy":1.96658, "omega":0.86892, "ax":-2.26264, "ay":0.60328, "alpha":-5.2459, "fx":[-43.0239,-21.3536,-34.6632,-54.90695], "fy":[-8.18392,6.57108,28.81946,13.84018]}, + {"t":1.39686, "x":3.04145, "y":6.64107, "heading":-0.98237, "vx":-3.4603, "vy":1.98119, "omega":0.74183, "ax":-3.15707, "ay":0.5803, "alpha":-7.20156, "fx":[-59.713,-30.85005,-49.25466,-74.98601], "fy":[-16.52493,5.20906,36.62796,14.17074]}, + {"t":1.42109, "x":2.95669, "y":6.68923, "heading":-0.9644, "vx":-3.53679, "vy":1.99525, "omega":0.56735, "ax":-4.29768, "ay":0.65983, "alpha":-9.831, "fx":[-81.27488,-43.85352,-67.85557,-99.42526], "fy":[-27.2183,5.44238,50.46456,16.2055]}, + {"t":1.44532, "x":2.86974, "y":6.73777, "heading":-0.95065, "vx":-3.64091, "vy":2.01124, "omega":0.32918, "ax":-5.56054, "ay":1.12258, "alpha":-13.57212, "fx":[-107.3127,-58.67469,-86.06402,-126.28118], "fy":[-39.57391,14.86868,77.34778,23.73659]}, + {"t":1.46954, "x":2.7799, "y":6.78682, "heading":-0.94268, "vx":-3.77562, "vy":2.03844, "omega":0.00036, "ax":-0.79753, "ay":-1.42914, "alpha":-0.01033, "fx":[-13.57064,-13.53004,-13.56086,-13.60145], "fy":[-24.34378,-24.31549,-24.2747,-24.303]}, + {"t":1.50461, "x":2.64701, "y":6.85743, "heading":-0.94266, "vx":-3.80359, "vy":1.98832, "omega":0.0, "ax":-0.15619, "ay":-0.29977, "alpha":0.0, "fx":[-2.65673,-2.65673,-2.65673,-2.65674], "fy":[-5.09902,-5.09901,-5.099,-5.09901]}, + {"t":1.53968, "x":2.51353, "y":6.92697, "heading":-0.94266, "vx":-3.80907, "vy":1.97781, "omega":0.0, "ax":-0.01602, "ay":-0.07344, "alpha":0.0, "fx":[-0.27252,-0.2725,-0.27251,-0.27253], "fy":[-1.24921,-1.2492,-1.24919,-1.2492]}, + {"t":1.57474, "x":2.37995, "y":6.99628, "heading":-0.94266, "vx":-3.80963, "vy":1.97523, "omega":0.0, "ax":8.23851, "ay":-4.27954, "alpha":-0.00146, "fx":[140.13104,140.13688,140.13846,140.13262], "fy":[-72.80159,-72.79166,-72.786,-72.79593]}, + {"t":1.60981, "x":2.25142, "y":7.06291, "heading":-0.94266, "vx":-3.52073, "vy":1.82516, "omega":-0.00005, "ax":9.04986, "ay":-4.69227, "alpha":-0.00002, "fx":[153.93541,153.93547,153.9355,153.93544], "fy":[-79.81434,-79.81423,-79.81417,-79.81428]}, + {"t":1.64488, "x":2.13352, "y":7.12403, "heading":-0.94267, "vx":-3.20338, "vy":1.66062, "omega":-0.00005, "ax":9.10118, "ay":-4.71836, "alpha":0.00008, "fx":[154.80866,154.80834,154.80818,154.8085], "fy":[-80.25747,-80.2581,-80.25841,-80.25779]}, + {"t":1.67995, "x":2.02679, "y":7.17936, "heading":-0.94267, "vx":-2.88423, "vy":1.49516, "omega":-0.00005, "ax":9.11969, "ay":-4.72777, "alpha":0.00012, "fx":[155.12365,155.12318,155.12295,155.12342], "fy":[-80.41729,-80.4182,-80.41865,-80.41775]}, + {"t":1.71501, "x":1.93125, "y":7.22889, "heading":-0.94267, "vx":-2.56442, "vy":1.32937, "omega":-0.00004, "ax":9.12922, "ay":-4.73261, "alpha":0.00014, "fx":[155.28589,155.28534,155.28508,155.28562], "fy":[-80.49961,-80.50066,-80.50118,-80.50014]}, + {"t":1.75008, "x":1.84694, "y":7.27259, "heading":-0.94267, "vx":-2.24429, "vy":1.16341, "omega":-0.00004, "ax":9.13503, "ay":-4.73557, "alpha":0.00015, "fx":[155.38477,155.38418,155.38389,155.38448], "fy":[-80.54978,-80.55091,-80.55149,-80.55035]}, + {"t":1.78515, "x":1.77385, "y":7.31048, "heading":-0.94267, "vx":-1.92395, "vy":0.99735, "omega":-0.00003, "ax":9.13895, "ay":-4.73755, "alpha":0.00016, "fx":[155.45133,155.45071,155.4504,155.45102], "fy":[-80.58355,-80.58475,-80.58535,-80.58415]}, + {"t":1.82021, "x":1.71201, "y":7.34254, "heading":-0.94267, "vx":-1.60348, "vy":0.83122, "omega":-0.00003, "ax":9.14176, "ay":-4.73898, "alpha":0.00016, "fx":[155.49919,155.49854,155.49822,155.49887], "fy":[-80.60782,-80.60907,-80.60969,-80.60845]}, + {"t":1.85528, "x":1.6614, "y":7.36877, "heading":-0.94267, "vx":-1.2829, "vy":0.66503, "omega":-0.00002, "ax":9.14388, "ay":-4.74006, "alpha":0.00017, "fx":[155.53525,155.53459,155.53426,155.53492], "fy":[-80.62612,-80.6274,-80.62804,-80.62676]}, + {"t":1.89035, "x":1.62203, "y":7.38918, "heading":-0.94268, "vx":-0.96225, "vy":0.49881, "omega":-0.00002, "ax":9.14553, "ay":-4.7409, "alpha":0.00017, "fx":[155.5634,155.56272,155.56238,155.56306], "fy":[-80.6404,-80.6417,-80.64236,-80.64105]}, + {"t":1.92541, "x":1.59391, "y":7.40376, "heading":-0.94268, "vx":-0.64154, "vy":0.33257, "omega":-0.00001, "ax":9.14686, "ay":-4.74158, "alpha":0.00017, "fx":[155.58598,155.58529,155.58495,155.58563], "fy":[-80.65186,-80.65318,-80.65384,-80.65252]}, + {"t":1.96048, "x":1.57704, "y":7.41251, "heading":-0.94268, "vx":-0.32079, "vy":0.16629, "omega":-0.00001, "ax":9.14795, "ay":-4.74213, "alpha":0.00017, "fx":[155.60449,155.6038,155.60345,155.60415], "fy":[-80.66125,-80.66259,-80.66326,-80.66192]}, + {"t":1.99555, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/ITofetch.traj b/src/main/deploy/choreo/ITofetch.traj new file mode 100644 index 00000000..ee640f39 --- /dev/null +++ b/src/main/deploy/choreo/ITofetch.traj @@ -0,0 +1,87 @@ +{ + "name":"ITofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.302966594696045, "y":5.351784706115723, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":37, "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":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.302966594696045 m", "val":5.302966594696045}, "y":{"exp":"5.351784706115723 m", "val":5.351784706115723}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":37, "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.30265,1.54956], + "samples":[ + {"t":0.0, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.20905, "ay":9.71733, "alpha":8.96118, "fx":[22.63273,-40.7351,-97.10075,-35.09833], "fy":[173.66919,170.40933,145.74536,171.33207]}, + {"t":0.02018, "x":5.48202, "y":4.94119, "heading":-2.0944, "vx":-0.04457, "vy":0.19606, "omega":0.1808, "ax":-2.46304, "ay":9.66252, "alpha":8.85977, "fx":[18.10572,-43.86921,-99.77011,-42.04855], "fy":[174.17584,169.61249,143.9096,169.72892]}, + {"t":0.04035, "x":5.48062, "y":4.94711, "heading":-2.09075, "vx":-0.09427, "vy":0.39101, "omega":0.35956, "ax":-2.74523, "ay":9.59334, "alpha":8.74858, "fx":[12.82842,-47.56006,-102.74699,-49.30343], "fy":[174.61775,168.59551,141.77488,167.73152]}, + {"t":0.06053, "x":5.47816, "y":4.95696, "heading":-2.08349, "vx":-0.14965, "vy":0.58457, "omega":0.53608, "ax":-3.06019, "ay":9.50587, "alpha":8.62149, "fx":[6.67372,-51.86845,-106.06111,-56.95583], "fy":[174.93193,167.29834,139.2848,165.2534]}, + {"t":0.08071, "x":5.47452, "y":4.97069, "heading":-2.07268, "vx":-0.2114, "vy":0.77637, "omega":0.71003, "ax":-3.41321, "ay":9.39484, "alpha":8.47127, "fx":[-0.51746,-56.86746,-109.74302,-65.10297], "fy":[175.02559,165.64068,136.37033,162.17734]}, + {"t":0.10088, "x":5.46956, "y":4.98826, "heading":-2.05835, "vx":-0.28026, "vy":0.96592, "omega":0.88095, "ax":-3.8103, "ay":9.2531, "alpha":8.28885, "fx":[-8.94259,-62.64453,-113.82317,-73.8383], "fy":[174.76055,163.51425,132.94656,158.34889]}, + {"t":0.12106, "x":5.46313, "y":5.00963, "heading":-2.04058, "vx":-0.35714, "vy":1.15261, "omega":1.04818, "ax":-4.25805, "ay":9.071, "alpha":8.06241, "fx":[-18.84252,-69.30289,-118.3298,-83.23785], "fy":[173.92933,160.77147,128.90875,153.57089]}, + {"t":0.14123, "x":5.45505, "y":5.03474, "heading":-2.01943, "vx":-0.44305, "vy":1.33563, "omega":1.21085, "ax":-4.76325, "ay":8.83551, "alpha":7.77587, "fx":[-30.50061,-76.96149,-123.28522,-93.33899], "fy":[172.21904,157.20923,124.12796,147.60183]}, + {"t":0.16141, "x":5.44514, "y":5.06348, "heading":-1.995, "vx":-0.53916, "vy":1.5139, "omega":1.36774, "ax":-5.33204, "ay":8.52917, "alpha":7.40707, "fx":[-44.22592,-85.75046,-128.69928,-104.11024], "fy":[169.15909,152.54565,118.44688,140.16327]}, + {"t":0.18159, "x":5.43318, "y":5.09576, "heading":-1.9674, "vx":-0.64674, "vy":1.68599, "omega":1.51719, "ax":-5.96819, "ay":8.12893, "alpha":6.92577, "fx":[-60.29916,-95.79704,-134.55893,-115.41383], "fy":[164.05372,146.38811,111.67706,130.96398]}, + {"t":0.20176, "x":5.41892, "y":5.13144, "heading":-1.93679, "vx":-0.76716, "vy":1.85, "omega":1.65693, "ax":-6.66998, "ay":7.60536, "alpha":6.29309, "fx":[-78.84451,-107.1913,-140.81244,-126.96942], "fy":[155.91772,138.19285,103.60014,119.74917]}, + {"t":0.22194, "x":5.40208, "y":5.17031, "heading":-1.90336, "vx":-0.90173, "vy":2.00345, "omega":1.7839, "ax":-7.42486, "ay":6.92343, "alpha":5.46578, "fx":[-99.58117,-119.91331,-147.34761,-138.33695], "fy":[143.48231,127.2244,93.97769,106.37767]}, + {"t":0.24212, "x":5.38238, "y":5.21214, "heading":-1.86737, "vx":-1.05154, "vy":2.14314, "omega":1.89418, "ax":-8.20214, "ay":6.04736, "alpha":4.41038, "fx":[-121.45856,-133.69541,-153.96571,-148.94428], "fy":[125.42073,112.54514,82.57598,90.91355]}, + {"t":0.26229, "x":5.35949, "y":5.25661, "heading":-1.82915, "vx":-1.21703, "vy":2.26515, "omega":1.98316, "ax":-8.94659, "ay":4.95296, "alpha":3.12706, "fx":[-142.37856,-147.80201,-160.35812,-158.17701], "fy":[100.97438,93.11147,69.21281,73.6953]}, + {"t":0.28247, "x":5.33311, "y":5.30332, "heading":-1.78914, "vx":-1.39754, "vy":2.36509, "omega":2.04626, "ax":-9.58131, "ay":3.64701, "alpha":1.66627, "fx":[-159.5009,-160.78316,-166.10044,-165.51656], "fy":[70.8644,68.11271,53.82863,55.33287]}, + {"t":0.30265, "x":5.30297, "y":5.35178, "heading":-1.74785, "vx":-1.59085, "vy":2.43867, "omega":2.07987, "ax":-9.89947, "ay":2.75525, "alpha":-1.24901, "fx":[-170.10776,-169.89763,-166.51937,-167.02418], "fy":[40.70863,41.40562,53.40792,51.94196]}, + {"t":0.33827, "x":5.24001, "y":5.44041, "heading":-1.67375, "vx":-1.94353, "vy":2.53683, "omega":2.03538, "ax":-9.89306, "ay":2.18627, "alpha":-5.96393, "fx":[-174.45133,-174.34203,-159.31728,-165.0018], "fy":[11.10646,8.45799,71.418,57.76887]}, + {"t":0.3739, "x":5.16449, "y":5.53218, "heading":-1.60124, "vx":-2.29598, "vy":2.61472, "omega":1.82291, "ax":-9.33786, "ay":1.12746, "alpha":-15.22306, "fx":[-172.32893,-162.05614,-137.30792,-163.64466], "fy":[-28.46143,-62.89962,106.93575,61.13629]}, + {"t":0.40952, "x":5.07677, "y":5.62604, "heading":-1.5363, "vx":-2.62865, "vy":2.65488, "omega":1.28057, "ax":-9.2435, "ay":0.58062, "alpha":-16.27017, "fx":[-169.56121,-154.75525,-138.47711,-166.12369], "fy":[-40.35538,-77.34053,104.29892,52.90139]}, + {"t":0.44515, "x":4.97725, "y":5.721, "heading":-1.49068, "vx":-2.95796, "vy":2.67557, "omega":0.70093, "ax":-9.27581, "ay":-0.62357, "alpha":-15.27243, "fx":[-163.94116,-143.01908,-153.90082,-170.25424], "fy":[-56.42614,-94.51622,75.68539,32.83013]}, + {"t":0.48078, "x":4.86599, "y":5.81592, "heading":-1.4657, "vx":-3.28842, "vy":2.65335, "omega":0.15683, "ax":-7.20084, "ay":-6.62764, "alpha":-4.21327, "fx":[-121.73962,-102.61146,-125.48655,-140.09898], "fy":[-116.87152,-132.50616,-108.93565,-92.62361]}, + {"t":0.5164, "x":4.74426, "y":5.90624, "heading":-1.46012, "vx":-3.54496, "vy":2.41724, "omega":0.00673, "ax":-4.92756, "ay":-7.82322, "alpha":-0.04184, "fx":[-83.85605,-83.59958,-83.7766,-84.03307], "fy":[-133.07904,-133.21088,-133.06249,-132.93031]}, + {"t":0.55203, "x":4.61484, "y":5.98739, "heading":-1.45988, "vx":-3.72051, "vy":2.13853, "omega":0.00524, "ax":-3.34595, "ay":-6.23547, "alpha":-0.00153, "fx":[-56.91623,-56.90722,-56.91116,-56.92017], "fy":[-106.06532,-106.06761,-106.06181,-106.05952]}, + {"t":0.58765, "x":4.48017, "y":6.05962, "heading":-1.45969, "vx":-3.83971, "vy":1.91638, "omega":0.00519, "ax":-1.12831, "ay":-2.32222, "alpha":0.00001, "fx":[-19.19223,-19.19227,-19.19226,-19.19223], "fy":[-39.50024,-39.50024,-39.50027,-39.50027]}, + {"t":0.62328, "x":4.34266, "y":6.12642, "heading":-1.45951, "vx":-3.87991, "vy":1.83365, "omega":0.00519, "ax":-0.23147, "ay":-0.49265, "alpha":0.00006, "fx":[-3.93717,-3.93747,-3.93744,-3.93714], "fy":[-8.37963,-8.37966,-8.37996,-8.37993]}, + {"t":0.65891, "x":4.20429, "y":6.19144, "heading":-1.45932, "vx":-3.88815, "vy":1.8161, "omega":0.00519, "ax":-0.04565, "ay":-0.09785, "alpha":0.00001, "fx":[-0.77648,-0.77655,-0.77655,-0.77648], "fy":[-1.66442,-1.66442,-1.66449,-1.66449]}, + {"t":0.69453, "x":4.06574, "y":6.25608, "heading":-1.45914, "vx":-3.88978, "vy":1.81261, "omega":0.00519, "ax":-0.00902, "ay":-0.01936, "alpha":0.0, "fx":[-0.15345,-0.15346,-0.15346,-0.15345], "fy":[-0.32939,-0.32939,-0.3294,-0.32939]}, + {"t":0.73016, "x":3.92716, "y":6.32064, "heading":-1.45895, "vx":-3.8901, "vy":1.81192, "omega":0.00519, "ax":-0.00178, "ay":-0.00383, "alpha":0.0, "fx":[-0.03036,-0.03035,-0.03035,-0.03036], "fy":[-0.06517,-0.06517,-0.06516,-0.06516]}, + {"t":0.76578, "x":3.78857, "y":6.38519, "heading":-1.45877, "vx":-3.89017, "vy":1.81179, "omega":0.00519, "ax":-0.00035, "ay":-0.00076, "alpha":0.0, "fx":[-0.00601,-0.006,-0.006,-0.00602], "fy":[-0.0129,-0.0129,-0.01289,-0.01289]}, + {"t":0.80141, "x":3.64998, "y":6.44973, "heading":-1.45858, "vx":-3.89018, "vy":1.81176, "omega":0.00519, "ax":-0.00007, "ay":-0.00015, "alpha":0.0, "fx":[-0.00122,-0.0012,-0.0012,-0.00122], "fy":[-0.0026,-0.0026,-0.00258,-0.00258]}, + {"t":0.83704, "x":3.51139, "y":6.51428, "heading":-1.4584, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00002, "ay":-0.00004, "alpha":0.0, "fx":[-0.00035,-0.00034,-0.00034,-0.00035], "fy":[-0.00074,-0.00073,-0.00072,-0.00072]}, + {"t":0.87266, "x":3.3728, "y":6.57883, "heading":-1.45821, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00003, "ay":-0.00007, "alpha":0.0, "fx":[-0.0006,-0.00058,-0.00058,-0.0006], "fy":[-0.00127,-0.00126,-0.00125,-0.00125]}, + {"t":0.90829, "x":3.2342, "y":6.64337, "heading":-1.45803, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00016, "ay":-0.00035, "alpha":0.0, "fx":[-0.00276,-0.00275,-0.00275,-0.00276], "fy":[-0.00591,-0.00591,-0.0059,-0.0059]}, + {"t":0.94391, "x":3.09561, "y":6.70792, "heading":-1.45784, "vx":-3.89019, "vy":1.81174, "omega":0.00519, "ax":-0.00082, "ay":-0.00175, "alpha":0.0, "fx":[-0.01388,-0.01386,-0.01387,-0.01388], "fy":[-0.02978,-0.02978,-0.02977,-0.02977]}, + {"t":0.97954, "x":2.95702, "y":6.77246, "heading":-1.45766, "vx":-3.89022, "vy":1.81168, "omega":0.00519, "ax":-0.00412, "ay":-0.00885, "alpha":0.0, "fx":[-0.0701,-0.07009,-0.07009,-0.0701], "fy":[-0.15054,-0.15054,-0.15053,-0.15053]}, + {"t":1.01517, "x":2.81842, "y":6.837, "heading":-1.45747, "vx":-3.89036, "vy":1.81136, "omega":0.00519, "ax":-0.02082, "ay":-0.04474, "alpha":0.00001, "fx":[-0.3541,-0.35413,-0.35413,-0.35409], "fy":[-0.761,-0.761,-0.76104,-0.76103]}, + {"t":1.05079, "x":2.67981, "y":6.9015, "heading":-1.45729, "vx":-3.89111, "vy":1.80977, "omega":0.00519, "ax":-0.10366, "ay":-0.22673, "alpha":0.00302, "fx":[-1.75679,-1.77139,-1.76971,-1.75511], "fy":[-3.84847,-3.85013,-3.86472,-3.86306]}, + {"t":1.08642, "x":2.54112, "y":6.96583, "heading":-1.4571, "vx":-3.8948, "vy":1.80169, "omega":0.0053, "ax":1.60058, "ay":-2.01146, "alpha":5.45083, "fx":[40.35521,12.58999,14.92197,41.03472], "fy":[-19.18928,-23.42536,-49.52988,-44.71284]}, + {"t":1.12204, "x":2.40338, "y":7.02874, "heading":-1.45691, "vx":-3.83778, "vy":1.73003, "omega":0.19949, "ax":7.95189, "ay":-2.9313, "alpha":19.87865, "fx":[171.00939,158.83989,63.35958,147.82852], "fy":[21.81946,28.21315,-159.53476,-89.94043]}, + {"t":1.15767, "x":2.2717, "y":7.08852, "heading":-1.44981, "vx":-3.55448, "vy":1.6256, "omega":0.90768, "ax":8.16781, "ay":-2.90688, "alpha":19.47796, "fx":[172.45037,166.21588,67.20039,149.86179], "fy":[22.81506,28.38905,-160.0403,-88.94449]}, + {"t":1.1933, "x":2.15026, "y":7.14458, "heading":-1.41747, "vx":-3.2635, "vy":1.52204, "omega":1.60161, "ax":9.09212, "ay":-4.19564, "alpha":7.61169, "fx":[172.23898,163.36465,128.21826,154.79551], "fy":[-27.40972,-59.30985,-118.04261,-80.70457]}, + {"t":1.22892, "x":2.03976, "y":7.19615, "heading":-1.36041, "vx":-2.93958, "vy":1.37256, "omega":1.87278, "ax":9.31206, "ay":-4.32478, "alpha":0.86108, "fx":[160.80777,158.77433,155.87436,158.12577], "fy":[-68.26798,-72.79752,-78.86865,-74.319]}, + {"t":1.26455, "x":1.94094, "y":7.2423, "heading":-1.29369, "vx":-2.60783, "vy":1.21849, "omega":1.90346, "ax":9.29955, "ay":-4.32189, "alpha":-2.61043, "fx":[150.17046,158.14255,165.26456,159.15312], "fy":[-89.6424,-74.81558,-57.2502,-72.34833]}, + {"t":1.30017, "x":1.85394, "y":7.28297, "heading":-1.22588, "vx":-2.27652, "vy":1.06452, "omega":1.81046, "ax":9.25181, "ay":-4.30155, "alpha":-4.63169, "fx":[142.62858,158.70132,169.46461,158.68789], "fy":[-101.43174,-73.94679,-43.74196,-73.55179]}, + {"t":1.3358, "x":1.77871, "y":7.31816, "heading":-1.16138, "vx":-1.94692, "vy":0.91127, "omega":1.64545, "ax":9.20374, "ay":-4.28316, "alpha":-5.93642, "fx":[137.61545,159.66292,171.68775,157.24566], "fy":[-108.27603,-72.04326,-34.37259,-76.72946]}, + {"t":1.37143, "x":1.71519, "y":7.34791, "heading":-1.10276, "vx":-1.61903, "vy":0.75868, "omega":1.43396, "ax":9.16006, "ay":-4.27043, "alpha":-6.86323, "fx":[134.35111,160.71314,172.98408,155.19172], "fy":[-112.39723,-69.80235,-27.44135,-80.9143]}, + {"t":1.40705, "x":1.66332, "y":7.37223, "heading":-1.05167, "vx":-1.29269, "vy":0.60654, "omega":1.18945, "ax":9.12057, "ay":-4.26247, "alpha":-7.57661, "fx":[132.23086,161.70575,173.78413,152.83247], "fy":[-114.95345,-67.56847,-22.11603,-85.37551]}, + {"t":1.44268, "x":1.62306, "y":7.39113, "heading":-1.0093, "vx":-0.96776, "vy":0.45469, "omega":0.91952, "ax":9.08495, "ay":-4.25746, "alpha":-8.15484, "fx":[130.82588,162.56774,174.29545,150.44068], "fy":[-116.60168,-65.54223,-17.93386,-89.595]}, + {"t":1.4783, "x":1.59434, "y":7.40463, "heading":-0.97654, "vx":-0.6441, "vy":0.30301, "omega":0.629, "ax":9.05335, "ay":-4.25399, "alpha":-8.63251, "fx":[129.83585,163.26145,174.62991,148.25218], "fy":[-117.74277,-63.85414,-14.60853,-93.23094]}, + {"t":1.51393, "x":1.57714, "y":7.41272, "heading":-0.95413, "vx":-0.32156, "vy":0.15146, "omega":0.32146, "ax":9.02613, "ay":-4.25128, "alpha":-9.02315, "fx":[129.04925,163.76668,174.85236,146.45941], "fy":[-118.63545,-62.59549,-11.94972,-96.07188]}, + {"t":1.54956, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj new file mode 100644 index 00000000..0767f9be --- /dev/null +++ b/src/main/deploy/choreo/JTofetch.traj @@ -0,0 +1,85 @@ +{ + "name":"JTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.048037528991699, "y":5.385420322418213, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "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":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.048037528991699 m", "val":5.048037528991699}, "y":{"exp":"5.385420322418213 m", "val":5.385420322418213}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "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.25469,1.45785], + "samples":[ + {"t":0.0, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.10203, "ay":9.45844, "alpha":9.07882, "fx":[9.48232,-51.89092,-108.96565,-59.68401], "fy":[174.8333,167.32099,137.06875,164.31867]}, + {"t":0.01819, "x":5.19731, "y":5.10512, "heading":-2.0944, "vx":-0.05643, "vy":0.17207, "omega":0.16516, "ax":-3.37119, "ay":9.36884, "alpha":9.0322, "fx":[4.58906,-55.27875,-111.82201,-66.86011], "fy":[175.00486,166.21478,134.72628,161.49902]}, + {"t":0.03638, "x":5.19573, "y":5.1098, "heading":-2.09139, "vx":-0.11776, "vy":0.3425, "omega":0.32947, "ax":-3.66634, "ay":9.26101, "alpha":8.97702, "fx":[-1.03828,-59.18031,-114.94889,-74.28642], "fy":[175.03407,164.84615,132.04327,158.18536]}, + {"t":0.05458, "x":5.19298, "y":5.11756, "heading":-2.0854, "vx":-0.18446, "vy":0.51098, "omega":0.49278, "ax":-3.99081, "ay":9.13075, "alpha":8.90786, "fx":[-7.5136,-63.64677,-118.36346,-82.006], "fy":[174.8449,163.1514,128.9628,154.287]}, + {"t":0.07277, "x":5.18896, "y":5.12837, "heading":-2.07643, "vx":-0.25706, "vy":0.67708, "omega":0.65483, "ax":-4.34814, "ay":8.97265, "alpha":8.81807, "fx":[-14.97287,-68.73633,-122.08036,-90.0528], "fy":[174.33014,161.04809,125.41764,149.6928]}, + {"t":0.09096, "x":5.18357, "y":5.14217, "heading":-2.06452, "vx":-0.33616, "vy":0.84031, "omega":0.81525, "ax":-4.74203, "ay":8.77978, "alpha":8.69913, "fx":[-23.57527,-74.51393,-126.10976,-98.44321], "fy":[173.3379,158.42899,121.32843,144.27091]}, + {"t":0.10915, "x":5.17667, "y":5.15891, "heading":-2.04969, "vx":-0.42242, "vy":1.00003, "omega":0.9735, "ax":-5.17601, "ay":8.54336, "alpha":8.53975, "fx":[-33.50069,-81.04976,-130.45454,-107.16473], "fy":[171.65275,155.15409,116.60216,137.87163]}, + {"t":0.12734, "x":5.16813, "y":5.17852, "heading":-2.03198, "vx":-0.51659, "vy":1.15545, "omega":1.12886, "ax":-5.65298, "ay":8.25229, "alpha":8.32481, "fx":[-44.93909,-88.41518,-135.10599,-116.16213], "fy":[168.97038,151.04016,111.13099,130.33517]}, + {"t":0.14553, "x":5.15779, "y":5.2009, "heading":-2.01144, "vx":-0.61942, "vy":1.30558, "omega":1.2803, "ax":-6.1744, "ay":7.89275, "alpha":8.0343, "fx":[-58.06396,-96.67424,-140.03789,-125.32268], "fy":[164.86676,145.84776,104.79249,121.50699]}, + {"t":0.16373, "x":5.1455, "y":5.22596, "heading":-1.98815, "vx":-0.73175, "vy":1.44916, "omega":1.42646, "ax":-6.7389, "ay":7.44795, "alpha":7.64275, "fx":[-72.97702,-105.86728,-145.19855,-134.46399], "fy":[158.7685,139.26653,97.45216,111.26259]}, + {"t":0.18192, "x":5.13108, "y":5.25355, "heading":-1.9622, "vx":-0.85434, "vy":1.58465, "omega":1.5655, "ax":-7.34025, "ay":6.89837, "alpha":7.12059, "fx":[-89.60865,-115.98161,-150.50099,-143.3308], "fy":[149.94415,130.90185,88.97004,99.54134]}, + {"t":0.20011, "x":5.11432, "y":5.28352, "heading":-1.93372, "vx":-0.98787, "vy":1.71015, "omega":1.69503, "ax":-7.96442, "ay":6.22334, "alpha":6.43909, "fx":[-107.56689,-126.90309,-155.81242,-151.60791], "fy":[137.55969,120.27105,79.21362,86.3844]}, + {"t":0.2183, "x":5.09503, "y":5.31566, "heading":-1.90289, "vx":-1.13276, "vy":1.82336, "omega":1.81217, "ax":-8.58646, "ay":5.40476, "alpha":5.58034, "fx":[-125.96873,-138.34389,-160.94536,-158.95469], "fy":[120.86328,106.82627,68.07885,71.96543]}, + {"t":0.23649, "x":5.073, "y":5.34973, "heading":-1.86992, "vx":-1.28897, "vy":1.92169, "omega":1.91369, "ax":-9.16888, "ay":4.43409, "alpha":4.54971, "fx":[-143.37302,-149.75295,-165.65545,-165.05857], "fy":[99.53841,90.03311,55.51989,56.59891]}, + {"t":0.25469, "x":5.04804, "y":5.38542, "heading":-1.83511, "vx":-1.45576, "vy":2.00235, "omega":1.99646, "ax":-9.5195, "ay":3.84152, "alpha":2.35709, "fx":[-156.76286,-158.60664,-166.38923,-165.93682], "fy":[77.68883,73.99832,54.26092,55.42461]}, + {"t":0.29007, "x":4.99056, "y":5.45868, "heading":-1.76446, "vx":-1.79263, "vy":2.13829, "omega":2.07987, "ax":-9.61617, "ay":3.63177, "alpha":-0.39692, "fx":[-164.34792,-164.12868,-162.76813,-163.02849], "fy":[59.71795,60.27856,63.86774,63.23729]}, + {"t":0.32546, "x":4.92111, "y":5.53662, "heading":-1.69086, "vx":-2.13292, "vy":2.26681, "omega":2.06582, "ax":-9.6377, "ay":3.22207, "alpha":-5.16116, "fx":[-172.06522,-171.23838,-152.60449,-159.82969], "fy":[30.37331,33.45443,84.72639,70.67182]}, + {"t":0.36085, "x":4.83959, "y":5.61886, "heading":-1.61775, "vx":-2.47397, "vy":2.38083, "omega":1.88318, "ax":-9.05266, "ay":2.1335, "alpha":-16.00904, "fx":[-173.86035,-167.67192,-117.44607,-156.95415], "fy":[-15.42405,-44.22686,128.29699,76.51473]}, + {"t":0.39623, "x":4.74638, "y":5.70444, "heading":-1.55111, "vx":-2.79431, "vy":2.45632, "omega":1.31667, "ax":-8.87919, "ay":1.6189, "alpha":-17.98096, "fx":[-171.75232,-160.00624,-113.1567,-159.21472], "fy":[-28.29363,-63.25182,131.0429,70.65068]}, + {"t":0.43162, "x":4.64194, "y":5.79238, "heading":-1.50452, "vx":-3.10852, "vy":2.51361, "omega":0.68038, "ax":-8.97674, "ay":0.68806, "alpha":-17.28136, "fx":[-167.53154,-150.28714,-128.96564,-163.98256], "fy":[-41.69228,-77.60162,111.58936,54.51963]}, + {"t":0.46701, "x":4.52631, "y":5.88176, "heading":-1.48044, "vx":-3.42618, "vy":2.53796, "omega":0.06884, "ax":-5.86793, "ay":-7.22216, "alpha":-1.78497, "fx":[-100.72137,-91.08339,-99.06311,-108.37948], "fy":[-123.75387,-129.77939,-122.16632,-115.68781]}, + {"t":0.5024, "x":4.4014, "y":5.96705, "heading":-1.47801, "vx":-3.63383, "vy":2.28239, "omega":0.00568, "ax":-3.7268, "ay":-6.34541, "alpha":-0.00823, "fx":[-63.40475,-63.35659,-63.37896,-63.42713], "fy":[-107.94201,-107.95695,-107.92528,-107.91033]}, + {"t":0.53778, "x":4.27047, "y":6.04384, "heading":-1.47781, "vx":-3.76571, "vy":2.05785, "omega":0.00538, "ax":-1.36535, "ay":-2.57082, "alpha":-0.00004, "fx":[-23.22433,-23.22412,-23.22415,-23.22436], "fy":[-43.729,-43.72899,-43.72879,-43.7288]}, + {"t":0.57317, "x":4.13636, "y":6.11505, "heading":-1.47762, "vx":-3.81403, "vy":1.96687, "omega":0.00538, "ax":-0.28862, "ay":-0.56325, "alpha":0.00007, "fx":[-4.90918,-4.90952,-4.90949,-4.90915], "fy":[-9.58056,-9.58059,-9.58093,-9.5809]}, + {"t":0.60856, "x":4.00121, "y":6.1843, "heading":-1.47743, "vx":-3.82424, "vy":1.94694, "omega":0.00539, "ax":-0.05769, "ay":-0.11348, "alpha":0.00002, "fx":[-0.98134,-0.98142,-0.98141,-0.98133], "fy":[-1.93015,-1.93016,-1.93024,-1.93023]}, + {"t":0.64394, "x":3.86585, "y":6.25313, "heading":-1.47724, "vx":-3.82628, "vy":1.94292, "omega":0.00539, "ax":-0.01155, "ay":-0.02275, "alpha":0.0, "fx":[-0.19645,-0.19645,-0.19645,-0.19645], "fy":[-0.38698,-0.38698,-0.38699,-0.38699]}, + {"t":0.67933, "x":3.73044, "y":6.32187, "heading":-1.47705, "vx":-3.82669, "vy":1.94212, "omega":0.00539, "ax":-0.00231, "ay":-0.00456, "alpha":0.0, "fx":[-0.03937,-0.03936,-0.03936,-0.03937], "fy":[-0.07757,-0.07757,-0.07756,-0.07756]}, + {"t":0.71472, "x":3.59502, "y":6.39059, "heading":-1.47685, "vx":-3.82677, "vy":1.94196, "omega":0.00539, "ax":-0.00047, "ay":-0.00092, "alpha":0.0, "fx":[-0.00792,-0.00791,-0.00791,-0.00792], "fy":[-0.0156,-0.01559,-0.01558,-0.01558]}, + {"t":0.7501, "x":3.45961, "y":6.45931, "heading":-1.47666, "vx":-3.82679, "vy":1.94193, "omega":0.00539, "ax":-0.0001, "ay":-0.0002, "alpha":0.0, "fx":[-0.00172,-0.00171,-0.00171,-0.00172], "fy":[-0.00338,-0.00338,-0.00337,-0.00337]}, + {"t":0.78549, "x":3.32419, "y":6.52803, "heading":-1.47647, "vx":-3.82679, "vy":1.94192, "omega":0.00539, "ax":-0.00006, "ay":-0.00011, "alpha":0.0, "fx":[-0.001,-0.00098,-0.00098,-0.001], "fy":[-0.00195,-0.00195,-0.00194,-0.00194]}, + {"t":0.82088, "x":3.18877, "y":6.59675, "heading":-1.47628, "vx":-3.8268, "vy":1.94191, "omega":0.00539, "ax":-0.0002, "ay":-0.0004, "alpha":0.0, "fx":[-0.00343,-0.00342,-0.00342,-0.00343], "fy":[-0.00675,-0.00675,-0.00674,-0.00674]}, + {"t":0.85627, "x":3.05335, "y":6.66547, "heading":-1.47609, "vx":-3.8268, "vy":1.9419, "omega":0.00539, "ax":-0.00099, "ay":-0.00194, "alpha":0.0, "fx":[-0.01679,-0.01678,-0.01678,-0.01679], "fy":[-0.03308,-0.03308,-0.03307,-0.03307]}, + {"t":0.89165, "x":2.91793, "y":6.73418, "heading":-1.4759, "vx":-3.82684, "vy":1.94183, "omega":0.00539, "ax":-0.00492, "ay":-0.0097, "alpha":0.0, "fx":[-0.0837,-0.0837,-0.0837,-0.0837], "fy":[-0.16497,-0.16497,-0.16497,-0.16497]}, + {"t":0.92704, "x":2.78251, "y":6.80289, "heading":-1.47571, "vx":-3.82701, "vy":1.94149, "omega":0.00539, "ax":-0.02454, "ay":-0.0484, "alpha":0.00001, "fx":[-0.41739,-0.41743,-0.41743,-0.41738], "fy":[-0.8233,-0.8233,-0.82335,-0.82335]}, + {"t":0.96243, "x":2.64706, "y":6.87157, "heading":-1.47552, "vx":-3.82788, "vy":1.93978, "omega":0.00539, "ax":-0.12008, "ay":-0.24254, "alpha":0.00492, "fx":[-2.03178,-2.05566,-2.05337,-2.02949], "fy":[-4.11242,-4.11467,-4.13852,-4.13628]}, + {"t":0.99781, "x":2.51153, "y":6.94006, "heading":-1.47533, "vx":-3.83213, "vy":1.93119, "omega":0.00556, "ax":2.39421, "ay":-2.48258, "alpha":7.6893, "fx":[60.70273,21.0911,22.49153,58.61422], "fy":[-20.54414,-27.22218,-64.75857,-56.3872]}, + {"t":1.0332, "x":2.37742, "y":7.00684, "heading":-1.47513, "vx":-3.74741, "vy":1.84334, "omega":0.27766, "ax":7.85497, "ay":-3.18513, "alpha":19.7877, "fx":[171.47432,159.35323,58.24068,145.37502], "fy":[17.68893,20.98116,-161.51494,-93.86782]}, + {"t":1.06859, "x":2.24973, "y":7.07008, "heading":-1.46531, "vx":-3.46944, "vy":1.73063, "omega":0.97789, "ax":8.06752, "ay":-3.15788, "alpha":19.417, "fx":[172.88321,166.82504,61.71757,147.47915], "fy":[19.05515,21.15064,-162.23756,-92.82669]}, + {"t":1.10397, "x":2.13201, "y":7.12934, "heading":-1.4307, "vx":-3.18396, "vy":1.61888, "omega":1.665, "ax":8.93778, "ay":-4.49298, "alpha":7.74547, "fx":[171.46691,160.84835,123.45681,152.344], "fy":[-31.77408,-65.70281,-122.99797,-85.22205]}, + {"t":1.13936, "x":2.02493, "y":7.18382, "heading":-1.37178, "vx":-2.86767, "vy":1.45989, "omega":1.93909, "ax":9.16135, "ay":-4.63458, "alpha":0.81952, "fx":[158.30576,156.15648,153.2616,155.60361], "fy":[-73.85472,-78.23076,-83.80941,-79.43647]}, + {"t":1.17475, "x":1.92919, "y":7.23258, "heading":-1.30316, "vx":-2.54348, "vy":1.29588, "omega":1.96809, "ax":9.14598, "ay":-4.63033, "alpha":-2.74957, "fx":[146.55018,155.69719,163.60286,156.4318], "fy":[-95.43351,-79.7689,-61.81251,-78.02742]}, + {"t":1.21014, "x":1.84491, "y":7.27554, "heading":-1.23352, "vx":-2.21983, "vy":1.13203, "omega":1.87079, "ax":9.09493, "ay":-4.60714, "alpha":-4.82139, "fx":[138.42983,156.45149,168.30123,155.62578], "fy":[-107.08507,-78.58829,-47.99452,-79.79678]}, + {"t":1.24552, "x":1.77205, "y":7.31271, "heading":-1.16732, "vx":-1.89799, "vy":0.969, "omega":1.70018, "ax":9.04354, "ay":-4.58673, "alpha":-6.16182, "fx":[133.15736,157.59803,170.8264,153.73016], "fy":[-113.71091,-76.44997,-38.3919,-83.52301]}, + {"t":1.28091, "x":1.71055, "y":7.34413, "heading":-1.10715, "vx":-1.57797, "vy":0.80669, "omega":1.48213, "ax":8.99631, "ay":-4.57271, "alpha":-7.12282, "fx":[129.8064,158.81309,172.32322,151.15595], "fy":[-117.61524,-74.01959,-31.28658,-88.20041]}, + {"t":1.3163, "x":1.66035, "y":7.36981, "heading":-1.05471, "vx":-1.25961, "vy":0.64487, "omega":1.23007, "ax":8.95312, "ay":-4.56371, "alpha":-7.87169, "fx":[127.68779,159.94701,173.26416,148.2608], "fy":[-119.97919,-71.62898,-25.83273,-93.06865]}, + {"t":1.35168, "x":1.62138, "y":7.38977, "heading":-1.01118, "vx":-0.94279, "vy":0.48338, "omega":0.95152, "ax":8.91391, "ay":-4.55761, "alpha":-8.4849, "fx":[126.324,160.92548,173.87838,145.36427], "fy":[-121.46439,-69.47383,-21.55708,-97.5994]}, + {"t":1.38707, "x":1.5936, "y":7.40403, "heading":-0.97751, "vx":-0.62735, "vy":0.3221, "omega":0.65126, "ax":8.8791, "ay":-4.55295, "alpha":-8.99399, "fx":[125.38559,161.71038,174.29012,142.73741], "fy":[-122.47127,-67.68322,-18.16438,-101.45894]}, + {"t":1.42246, "x":1.57695, "y":7.41257, "heading":-0.95446, "vx":-0.31315, "vy":0.16098, "omega":0.33299, "ax":8.84922, "ay":-4.54913, "alpha":-9.40993, "fx":[124.64296,162.28135,174.57182,140.59441], "fy":[-123.25702,-66.34855,-15.45659,-104.45519]}, + {"t":1.45785, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/LTofetch.traj b/src/main/deploy/choreo/LTofetch.traj new file mode 100644 index 00000000..a867a04d --- /dev/null +++ b/src/main/deploy/choreo/LTofetch.traj @@ -0,0 +1,62 @@ +{ + "name":"LTofetch", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "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.17196], + "samples":[ + {"t":0.0, "x":3.91036, "y":5.1782, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.44665, "ay":7.12277, "alpha":0.64117, "fx":[-124.01568,-128.11294,-129.35809,-125.17464], "fy":[123.90932,119.67059,118.31503,122.72997]}, + {"t":0.04341, "x":3.90335, "y":5.18491, "heading":-1.0472, "vx":-0.32323, "vy":0.30917, "omega":0.02783, "ax":-7.44576, "ay":7.12191, "alpha":0.63702, "fx":[-124.018,-128.0887,-129.32487,-125.16911], "fy":[123.87714,119.6659,118.31936,122.70447]}, + {"t":0.08681, "x":3.88231, "y":5.20504, "heading":-1.04599, "vx":-0.64642, "vy":0.6183, "omega":0.05548, "ax":-7.44461, "ay":7.12082, "alpha":0.63172, "fx":[-124.0227,-128.06069,-129.28064,-125.15866], "fy":[123.83404,119.6565,118.3266,122.67512]}, + {"t":0.13022, "x":3.84723, "y":5.23858, "heading":-1.04358, "vx":-0.96956, "vy":0.92739, "omega":0.0829, "ax":-7.44308, "ay":7.11935, "alpha":0.62469, "fx":[-124.02994,-128.02535,-129.2208,-125.1426], "fy":[123.7756,119.64178,118.33717,122.6382]}, + {"t":0.17362, "x":3.79814, "y":5.28554, "heading":-1.03998, "vx":-1.29263, "vy":1.23641, "omega":0.11002, "ax":-7.44094, "ay":7.11731, "alpha":0.61493, "fx":[-124.03994,-127.97636,-129.13719,-125.11958], "fy":[123.6939,119.62063,118.3518,122.58711]}, + {"t":0.21703, "x":3.73502, "y":5.34592, "heading":-1.03521, "vx":-1.61561, "vy":1.54534, "omega":0.13671, "ax":-7.43773, "ay":7.11424, "alpha":0.60047, "fx":[-124.05296,-127.90118,-129.01368,-125.08684], "fy":[123.57329,119.59068,118.3717,122.50882]}, + {"t":0.26043, "x":3.65789, "y":5.41969, "heading":-1.02927, "vx":-1.93845, "vy":1.85414, "omega":0.16277, "ax":-7.43238, "ay":7.10912, "alpha":0.57688, "fx":[-124.06902,-127.77099,-128.81333,-125.03745], "fy":[123.37788,119.54596,118.39905,122.37348]}, + {"t":0.30384, "x":3.56675, "y":5.50687, "heading":-1.02221, "vx":-2.26106, "vy":2.16272, "omega":0.18781, "ax":-7.4217, "ay":7.0989, "alpha":0.53154, "fx":[-124.08515,-127.50118,-128.42889,-124.94881], "fy":[123.00318,119.4667,118.43702,122.0942]}, + {"t":0.34725, "x":3.46161, "y":5.60743, "heading":-1.01406, "vx":-2.5832, "vy":2.47085, "omega":0.21088, "ax":-7.38987, "ay":7.06845, "alpha":0.40914, "fx":[-124.05689,-126.68894,-127.36159,-124.69083], "fy":[121.9604,119.23609,118.46875,121.26392]}, + {"t":0.39065, "x":3.34252, "y":5.72134, "heading":-1.0049, "vx":-2.90397, "vy":2.77766, "omega":0.22864, "ax":-4.56546, "ay":4.36719, "alpha":-5.26148, "fx":[-89.08373,-60.67549,-68.20924,-92.66012], "fy":[53.9039,77.95047,92.72971,72.55455]}, + {"t":0.43406, "x":3.21217, "y":5.84602, "heading":-0.99498, "vx":-3.10213, "vy":2.96722, "omega":0.00026, "ax":-0.00131, "ay":0.001, "alpha":-0.001, "fx":[-0.02296,-0.0189,-0.02154,-0.02561], "fy":[0.01372,0.01636,0.02043,0.01779]}, + {"t":0.47746, "x":3.07752, "y":5.97482, "heading":-0.99497, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":-0.00002, "ay":-0.00002, "alpha":0.0, "fx":[-0.00027,-0.00027,-0.00027,-0.00027], "fy":[-0.00028,-0.00028,-0.00028,-0.00028]}, + {"t":0.52087, "x":2.94287, "y":6.10361, "heading":-0.99496, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[-0.00004,-0.00004,-0.00004,-0.00004]}, + {"t":0.56428, "x":2.80822, "y":6.23241, "heading":-0.99495, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "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.60768, "x":2.67356, "y":6.36121, "heading":-0.99494, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.00003,0.00003,0.00003,0.00003]}, + {"t":0.65109, "x":2.53891, "y":6.49, "heading":-0.99493, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.00002, "ay":0.00002, "alpha":0.0, "fx":[0.00026,0.00026,0.00026,0.00026], "fy":[0.00027,0.00027,0.00027,0.00027]}, + {"t":0.69449, "x":2.40426, "y":6.6188, "heading":-0.99492, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0013, "ay":-0.00101, "alpha":0.001, "fx":[0.0229,0.01883,0.02148,0.02555], "fy":[-0.01379,-0.01643,-0.0205,-0.01786]}, + {"t":0.7379, "x":2.2696, "y":6.7476, "heading":-0.99491, "vx":-3.10213, "vy":2.96722, "omega":0.00026, "ax":4.56546, "ay":-4.36719, "alpha":5.26176, "fx":[88.93536,60.56537,68.37443,92.75368], "fy":[-53.91785,-78.15434,-92.69308,-72.37313]}, + {"t":0.7813, "x":2.13925, "y":6.87228, "heading":-0.9949, "vx":-2.90397, "vy":2.77766, "omega":0.22866, "ax":7.38987, "ay":-7.06845, "alpha":-0.40926, "fx":[124.07639,126.72016,127.34317,124.6585], "fy":[-121.94112,-119.20258,-118.48794,-121.29751]}, + {"t":0.82471, "x":2.02017, "y":6.98619, "heading":-0.98497, "vx":-2.5832, "vy":2.47085, "omega":0.21089, "ax":7.4217, "ay":-7.0989, "alpha":-0.53158, "fx":[124.13582,127.57944,128.38182,124.86695], "fy":[-122.95275,-119.3827,-118.48727,-122.17838]}, + {"t":0.86812, "x":1.91503, "y":7.08675, "heading":-0.97582, "vx":-2.26106, "vy":2.16272, "omega":0.18782, "ax":7.43238, "ay":-7.10912, "alpha":-0.57689, "fx":[124.14842,127.89277,128.74001,124.90958], "fy":[-123.29871,-119.41522,-118.47795,-122.50448]}, + {"t":0.91152, "x":1.82389, "y":7.17392, "heading":-0.96767, "vx":-1.93845, "vy":1.85414, "omega":0.16278, "ax":7.43773, "ay":-7.11424, "alpha":-0.60049, "fx":[124.15771,128.06123,128.91726,124.91846], "fy":[-123.46876,-119.41884,-118.47591,-122.68099]}, + {"t":0.95493, "x":1.74676, "y":7.2477, "heading":-0.9606, "vx":-1.61561, "vy":1.54534, "omega":0.13671, "ax":7.44094, "ay":-7.11731, "alpha":-0.61495, "fx":[124.16633,128.16885,129.02107,124.91683], "fy":[-123.56772,-119.41393,-118.47761,-122.79418]}, + {"t":0.99833, "x":1.68364, "y":7.30808, "heading":-0.95467, "vx":-1.29263, "vy":1.23641, "omega":0.11002, "ax":7.44308, "ay":-7.11935, "alpha":-0.62471, "fx":[124.17407,128.24422,129.08853,124.91188], "fy":[-123.63166,-119.40673,-118.48071,-122.87363]}, + {"t":1.04174, "x":1.63454, "y":7.35504, "heading":-0.94989, "vx":-0.96956, "vy":0.92739, "omega":0.0829, "ax":7.44461, "ay":-7.12082, "alpha":-0.63174, "fx":[124.18055,128.29973,129.13588,124.90654], "fy":[-123.67636,-119.39977,-118.48388,-122.93223]}, + {"t":1.08515, "x":1.59947, "y":7.38858, "heading":-0.94629, "vx":-0.64642, "vy":0.6183, "omega":0.05548, "ax":7.44576, "ay":-7.12191, "alpha":-0.63704, "fx":[124.18547,128.34163,129.17136,124.90225], "fy":[-123.70982,-119.39425,-118.4863,-122.97649]}, + {"t":1.12855, "x":1.57843, "y":7.40871, "heading":-0.94388, "vx":-0.32323, "vy":0.30917, "omega":0.02783, "ax":7.44665, "ay":-7.12277, "alpha":-0.64119, "fx":[124.1886,128.37339,129.19961,124.89975], "fy":[-123.73652,-119.39083,-118.48749,-123.01005]}, + {"t":1.17196, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/constants.traj b/src/main/deploy/choreo/constants.traj index a02bf762..c6f351fb 100644 --- a/src/main/deploy/choreo/constants.traj +++ b/src/main/deploy/choreo/constants.traj @@ -8,24 +8,25 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":6506, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21807, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"61.8667 in", "val":1.57141418}, "y":{"exp":"291.9457 in", "val":7.41542078}, "heading":{"exp":"-54.011392 deg", "val":-0.9426766239853251}, "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":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":true}, + {"from":0, "to":13, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src/main/deploy/choreo/fetchToA.traj b/src/main/deploy/choreo/fetchToA.traj new file mode 100644 index 00000000..cf21135e --- /dev/null +++ b/src/main/deploy/choreo/fetchToA.traj @@ -0,0 +1,78 @@ +{ + "name":"fetchToA", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2019875, "y":4.4292225106, "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":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "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":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.9966,1.25852], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.03207, "ay":-8.55381, "alpha":15.13169, "fx":[136.76558,-21.71623,39.56227,119.72551], "fy":[-109.32572,-173.83389,-170.78855,-128.04357]}, + {"t":0.03215, "x":1.5735, "y":7.411, "heading":-0.94268, "vx":0.12962, "vy":-0.27499, "omega":0.48646, "ax":4.04237, "ay":-8.63623, "alpha":14.35199, "fx":[133.32416,-17.1468,40.50067,118.36041], "fy":[-113.4526,-174.31685,-170.54969,-129.28009]}, + {"t":0.0643, "x":1.57975, "y":7.3977, "heading":-0.92704, "vx":0.25958, "vy":-0.55263, "omega":0.94785, "ax":4.06225, "ay":-8.73154, "alpha":13.35159, "fx":[128.35389,-11.65886,42.51312,117.1827], "fy":[-118.9922,-174.74062,-170.03613,-130.31492]}, + {"t":0.09644, "x":1.5902, "y":7.37542, "heading":-0.89657, "vx":0.39017, "vy":-0.83334, "omega":1.37708, "ax":4.08606, "ay":-8.84036, "alpha":12.1154, "fx":[121.59573,-5.01192,45.5229,115.90423], "fy":[-125.82764,-175.02474,-169.22583,-131.40966]}, + {"t":0.12859, "x":1.60485, "y":7.34406, "heading":-0.8523, "vx":0.52153, "vy":-1.11754, "omega":1.76657, "ax":4.10875, "ay":-8.96143, "alpha":10.62628, "fx":[112.85682,3.2065,49.43313,114.05851], "fy":[-133.6538,-175.02826,-168.08621,-132.95698]}, + {"t":0.16074, "x":1.62374, "y":7.3035, "heading":-0.7955, "vx":0.65362, "vy":-1.40563, "omega":2.10819, "ax":4.12839, "ay":-9.08994, "alpha":8.85778, "fx":[102.26444,13.645,54.1255,110.85591], "fy":[-141.85347,-174.47565,-166.57976,-135.56024]}, + {"t":0.19289, "x":1.64689, "y":7.25362, "heading":-0.72773, "vx":0.78634, "vy":-1.69786, "omega":2.39295, "ax":4.148, "ay":-9.21731, "alpha":6.72017, "fx":[90.5748,27.34673,59.4603,104.8432], "fy":[-149.52207,-172.79133,-164.6683,-140.15325]}, + {"t":0.22504, "x":1.67431, "y":7.19427, "heading":-0.6508, "vx":0.9197, "vy":-1.99418, "omega":2.60899, "ax":4.17041, "ay":-9.3297, "alpha":3.91616, "fx":[79.32444,46.09029,65.27696,93.05792], "fy":[-155.72205,-168.66251,-162.3114,-148.08632]}, + {"t":0.25719, "x":1.70603, "y":7.12534, "heading":-0.56692, "vx":1.05377, "vy":-2.29411, "omega":2.73489, "ax":4.17323, "ay":-9.38413, "alpha":-0.32495, "fx":[70.59249,72.97407,71.39368,68.98147], "fy":[-159.81385,-158.7322,-159.4363,-160.50331]}, + {"t":0.28933, "x":1.74207, "y":7.04674, "heading":-0.479, "vx":1.18793, "vy":-2.5958, "omega":2.72445, "ax":4.05956, "ay":-9.17327, "alpha":-7.4088, "fx":[66.4462,112.17443,77.61777,19.9696], "fy":[-161.52513,-133.65133,-155.6836,-173.27884]}, + {"t":0.32148, "x":1.78236, "y":6.95855, "heading":-0.39142, "vx":1.31844, "vy":-2.8907, "omega":2.48626, "ax":3.7059, "ay":-7.96691, "alpha":-17.68241, "fx":[68.37079,157.11578,81.13229,-54.47345], "fy":[-160.6467,-75.31992,-140.61073,-165.48205]}, + {"t":0.35363, "x":1.82666, "y":6.8615, "heading":-0.31149, "vx":1.43758, "vy":-3.14683, "omega":1.91781, "ax":2.62075, "ay":-8.12043, "alpha":-19.27883, "fx":[74.1735,159.34352,-6.19945,-49.00488], "fy":[-157.69624,-69.19324,-158.83859,-166.77638]}, + {"t":0.38578, "x":1.87423, "y":6.75614, "heading":-0.24983, "vx":1.52183, "vy":-3.40789, "omega":1.29802, "ax":2.58608, "ay":-8.10726, "alpha":-19.39961, "fx":[77.98599,157.65851,-20.03758,-39.65305], "fy":[-155.05081,-70.25371,-157.97242,-168.33171]}, + {"t":0.41793, "x":1.92449, "y":6.64239, "heading":-0.2081, "vx":1.60497, "vy":-3.66852, "omega":0.67436, "ax":2.52379, "ay":-7.93776, "alpha":-19.26937, "fx":[77.63198,151.64758,-24.03926,-33.52418], "fy":[-152.4166,-73.87681,-147.3186,-166.46404]}, + {"t":0.45008, "x":1.97739, "y":6.52035, "heading":-0.18642, "vx":1.6861, "vy":-3.92371, "omega":0.05488, "ax":-4.50262, "ay":-2.48567, "alpha":-1.4298, "fx":[-73.3383,-73.62976,-79.86472,-79.52066], "fy":[-47.49438,-39.96494,-37.11816,-44.54466]}, + {"t":0.48222, "x":2.02927, "y":6.39293, "heading":-0.18466, "vx":1.54135, "vy":-4.00362, "omega":0.00891, "ax":-5.23191, "ay":-1.89685, "alpha":-0.01845, "fx":[-88.95562,-88.95928,-89.03098,-89.02732], "fy":[-32.33574,-32.23071,-32.19406,-32.29906]}, + {"t":0.51437, "x":2.07611, "y":6.26324, "heading":-0.18437, "vx":1.37315, "vy":-4.0646, "omega":0.00832, "ax":-2.65718, "ay":-0.86653, "alpha":0.00053, "fx":[-45.19892,-45.19926,-45.19687,-45.19654], "fy":[-14.73785,-14.7405,-14.7411,-14.73846]}, + {"t":0.54652, "x":2.11889, "y":6.13212, "heading":-0.1841, "vx":1.28773, "vy":-4.09245, "omega":0.00834, "ax":-0.82342, "ay":-0.25613, "alpha":0.00002, "fx":[-14.0061,-14.00611,-14.00604,-14.00603], "fy":[-4.35661,-4.35668,-4.35669,-4.35662]}, + {"t":0.57867, "x":2.15986, "y":6.00042, "heading":-0.18384, "vx":1.26126, "vy":-4.10069, "omega":0.00834, "ax":-0.217, "ay":-0.06647, "alpha":0.00005, "fx":[-3.69113,-3.69117,-3.69093,-3.69089], "fy":[-1.1305,-1.13074,-1.13078,-1.13054]}, + {"t":0.61082, "x":2.20029, "y":5.86856, "heading":-0.18357, "vx":1.25428, "vy":-4.10282, "omega":0.00834, "ax":0.19263, "ay":0.05914, "alpha":0.00005, "fx":[3.27653,3.27648,3.27672,3.27676], "fy":[1.00604,1.0058,1.00576,1.00599]}, + {"t":0.64297, "x":2.24072, "y":5.73669, "heading":-0.1833, "vx":1.26048, "vy":-4.10092, "omega":0.00834, "ax":1.29306, "ay":0.40478, "alpha":-0.00023, "fx":[21.99498,21.99517,21.99409,21.9939], "fy":[6.88455,6.88566,6.88587,6.88477]}, + {"t":0.67511, "x":2.28191, "y":5.60506, "heading":-0.18303, "vx":1.30204, "vy":-4.08791, "omega":0.00833, "ax":4.51015, "ay":1.52645, "alpha":-0.00028, "fx":[76.71699,76.71703,76.71588,76.71585], "fy":[25.96347,25.965,25.96546,25.96393]}, + {"t":0.70726, "x":2.3261, "y":5.47443, "heading":-0.18276, "vx":1.44704, "vy":-4.03884, "omega":0.00833, "ax":7.63868, "ay":3.00998, "alpha":0.00739, "fx":[129.91494,129.92672,129.94855,129.93677], "fy":[51.2358,51.18682,51.16196,51.21093]}, + {"t":0.73941, "x":2.37656, "y":5.34614, "heading":-0.1825, "vx":1.69261, "vy":-3.94207, "omega":0.00856, "ax":8.439, "ay":4.24903, "alpha":0.56304, "fx":[141.90118,143.50395,145.15471,143.62018], "fy":[75.4131,71.69977,69.14101,72.84513]}, + {"t":0.77156, "x":2.43534, "y":5.22161, "heading":-0.18222, "vx":1.96391, "vy":-3.80547, "omega":0.02666, "ax":3.95517, "ay":8.47585, "alpha":12.659, "fx":[22.66345,13.8333,134.61876,97.98949], "fy":[169.2782,165.60507,101.4206,140.38294]}, + {"t":0.80371, "x":2.50052, "y":5.10365, "heading":-0.18136, "vx":2.09106, "vy":-3.53299, "omega":0.43363, "ax":1.50739, "ay":9.11837, "alpha":15.82965, "fx":[-16.9609,-63.93138,110.40258,73.05091], "fy":[172.62968,159.23877,131.2225,157.31276]}, + {"t":0.83586, "x":2.56852, "y":4.99478, "heading":-0.16742, "vx":2.13952, "vy":-3.23985, "omega":0.94253, "ax":0.40707, "ay":9.85966, "alpha":9.52994, "fx":[-21.88357,-50.82332,55.21986,45.1833], "fy":[172.7617,165.79699,164.15771,168.12337]}, + {"t":0.868, "x":2.63751, "y":4.89572, "heading":-0.13712, "vx":2.15261, "vy":-2.92288, "omega":1.2489, "ax":-0.84979, "ay":10.21342, "alpha":-0.94374, "fx":[-10.89814,-9.12956,-17.8014,-19.98975], "fy":[174.01209,174.17038,173.51021,173.21681]}, + {"t":0.90015, "x":2.70628, "y":4.80703, "heading":-0.09697, "vx":2.12529, "vy":-2.59453, "omega":1.21856, "ax":-1.40909, "ay":10.04549, "alpha":-5.66362, "fx":[-0.285,4.40156,-40.74631,-59.24347], "fy":[174.51626,174.72154,169.99855,164.24703]}, + {"t":0.9323, "x":2.77387, "y":4.72881, "heading":-0.0578, "vx":2.07999, "vy":-2.27159, "omega":1.03648, "ax":-1.68945, "ay":9.85251, "alpha":-8.55686, "fx":[9.65611,9.83689,-52.41959,-82.02157], "fy":[174.38227,174.68474,166.95931,154.32737]}, + {"t":0.96445, "x":2.83987, "y":4.66088, "heading":-0.02448, "vx":2.02568, "vy":-1.95485, "omega":0.76139, "ax":-1.84431, "ay":9.68627, "alpha":-10.55848, "fx":[18.31046,12.22467,-59.79247,-96.22746], "fy":[173.79564,174.64362,164.57323,146.02995]}, + {"t":0.9966, "x":2.90404, "y":4.60304, "heading":0.0, "vx":1.96638, "vy":-1.64345, "omega":0.42196, "ax":-3.12473, "ay":9.57787, "alpha":-7.40092, "fx":[-25.72623,-16.88667,-71.24541,-98.74453], "fy":[172.97615,174.26867,159.96915,144.4532]}, + {"t":1.02934, "x":2.96674, "y":4.55436, "heading":0.01381, "vx":1.86408, "vy":-1.32987, "omega":0.17965, "ax":-5.19774, "ay":8.75801, "alpha":-5.17506, "fx":[-79.20768,-60.1407,-96.04266,-118.25751], "fy":[155.98559,164.42342,146.43311,129.04258]}, + {"t":1.06208, "x":3.02499, "y":4.51552, "heading":0.0197, "vx":1.69391, "vy":-1.04313, "omega":0.01022, "ax":-6.79339, "ay":7.6768, "alpha":-3.25562, "fx":[-114.42754,-98.58947,-117.26395,-131.93319], "fy":[132.44355,144.69877,130.07651,115.10201]}, + {"t":1.09482, "x":3.07681, "y":4.48548, "heading":0.02003, "vx":1.47149, "vy":-0.79179, "omega":-0.09637, "ax":-7.91384, "ay":6.56959, "alpha":-1.61414, "fx":[-135.27697,-127.3748,-134.27859,-141.51843], "fy":[111.1816,120.18671,112.45478,103.16416]}, + {"t":1.12756, "x":3.12074, "y":4.46308, "heading":0.01688, "vx":1.21239, "vy":-0.5767, "omega":-0.14922, "ax":-8.66546, "ay":5.56349, "alpha":-0.2652, "fx":[-147.59355,-146.42487,-147.21174,-148.35788], "fy":[94.32943,96.13827,94.93595,93.12999]}, + {"t":1.1603, "x":3.15579, "y":4.44718, "heading":0.01199, "vx":0.92868, "vy":-0.39455, "omega":-0.1579, "ax":-9.16211, "ay":4.70017, "alpha":0.80974, "fx":[-155.15741,-158.22144,-156.64242,-153.35808], "fy":[81.38777,75.2452,78.45391,84.70735]}, + {"t":1.19304, "x":3.18129, "y":4.43678, "heading":0.00682, "vx":0.62871, "vy":-0.24067, "omega":-0.13139, "ax":-9.49126, "ay":3.9767, "alpha":1.66344, "fx":[-160.03404,-165.33693,-163.29903,-157.1046], "fy":[71.40854,58.06017,63.50503,77.59641]}, + {"t":1.22578, "x":3.19678, "y":4.43103, "heading":0.00252, "vx":0.31797, "vy":-0.11047, "omega":-0.07693, "ax":-9.71178, "ay":3.37407, "alpha":2.34975, "fx":[-163.33148,-169.60297,-167.87017,-159.97406], "fy":[63.58111,44.17849,50.26139,71.547]}, + {"t":1.25852, "x":3.20199, "y":4.42922, "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/fetchToB.traj b/src/main/deploy/choreo/fetchToB.traj new file mode 100644 index 00000000..27c65882 --- /dev/null +++ b/src/main/deploy/choreo/fetchToB.traj @@ -0,0 +1,79 @@ +{ + "name":"fetchToB", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8945255279541016, "y":4.331230163574219, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2019875, "y":4.1005465106, "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":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8945255279541016 m", "val":2.8945255279541016}, "y":{"exp":"4.331230163574219 m", "val":4.331230163574219}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "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":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.04495,1.32048], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.01035, "ay":-8.45166, "alpha":16.07271, "fx":[140.65173,-27.32961,38.35413,121.18355], "fy":[-104.26569,-173.04045,-171.06693,-126.66854]}, + {"t":0.03265, "x":1.57355, "y":7.41091, "heading":-0.94268, "vx":0.13096, "vy":-0.27599, "omega":0.52485, "ax":4.02155, "ay":-8.54436, "alpha":15.24056, "fx":[137.07046,-22.56442,39.30305,119.81286], "fy":[-108.8808,-173.69739,-170.83218,-127.93837]}, + {"t":0.06531, "x":1.57997, "y":7.39735, "heading":-0.92554, "vx":0.26228, "vy":-0.555, "omega":1.02253, "ax":4.04482, "ay":-8.65308, "alpha":14.15042, "fx":[131.77992,-16.71328,41.45925,118.67879], "fy":[-115.1656,-174.32688,-170.29786,-128.95561]}, + {"t":0.09796, "x":1.59069, "y":7.37461, "heading":-0.89215, "vx":0.39436, "vy":-0.83756, "omega":1.4846, "ax":4.07257, "ay":-8.77902, "alpha":12.78343, "fx":[124.40952,-9.50449,44.73385,117.45406], "fy":[-123.01945,-174.83339,-169.43638,-130.02539]}, + {"t":0.13062, "x":1.60574, "y":7.34258, "heading":-0.84367, "vx":0.52735, "vy":-1.12424, "omega":1.90204, "ax":4.09785, "ay":-8.92061, "alpha":11.12242, "fx":[114.66519,-0.45705,49.01091,115.59381], "fy":[-132.07268,-175.04914,-168.20743,-131.6189]}, + {"t":0.16327, "x":1.62515, "y":7.30111, "heading":-0.78156, "vx":0.66116, "vy":-1.41554, "omega":2.26524, "ax":4.11827, "ay":-9.07107, "alpha":9.14377, "fx":[102.70362,11.21254,54.1445,112.14186], "fy":[-141.49819,-174.63606,-166.56604,-134.48482]}, + {"t":0.19593, "x":1.64893, "y":7.25005, "heading":-0.70759, "vx":0.79565, "vy":-1.71175, "omega":2.56383, "ax":4.13909, "ay":-9.21852, "alpha":6.7365, "fx":[89.59245,26.82443,59.95566,105.24634], "fy":[-150.07314,-172.85144,-164.4699,-139.82288]}, + {"t":0.22858, "x":1.67712, "y":7.18924, "heading":-0.62387, "vx":0.93081, "vy":-2.01278, "omega":2.78381, "ax":4.16404, "ay":-9.34397, "alpha":3.47967, "fx":[77.46988,48.75196,66.22148,90.87292], "fy":[-156.6142,-167.87298,-161.8814,-149.38442]}, + {"t":0.26124, "x":1.70974, "y":7.11853, "heading":-0.53296, "vx":1.06678, "vy":-2.31791, "omega":2.89744, "ax":4.15269, "ay":-9.37539, "alpha":-1.74725, "fx":[69.05409,81.24254,72.61503,59.63236], "fy":[-160.44802,-154.58566,-158.74043,-164.11702]}, + {"t":0.29389, "x":1.74679, "y":7.03784, "heading":-0.43835, "vx":1.20239, "vy":-2.62406, "omega":2.84038, "ax":3.95407, "ay":-8.93626, "alpha":-10.65471, "fx":[66.77369,128.55417,78.1576,-4.45505], "fy":[-161.35427,-117.81763,-154.57763,-174.2635]}, + {"t":0.32655, "x":1.78816, "y":6.94739, "heading":-0.34559, "vx":1.33151, "vy":-2.91587, "omega":2.49245, "ax":2.79739, "ay":-8.07304, "alpha":-19.063, "fx":[71.61106,159.68161,13.3555,-54.31678], "fy":[-159.10674,-69.23754,-155.5354,-165.40088]}, + {"t":0.3592, "x":1.83313, "y":6.84787, "heading":-0.2642, "vx":1.42285, "vy":-3.17949, "omega":1.86996, "ax":2.58691, "ay":-8.15952, "alpha":-19.29816, "fx":[77.29401,158.46985,-17.95889,-41.79448], "fy":[-155.89936,-70.12184,-160.73275,-168.41029]}, + {"t":0.39186, "x":1.88097, "y":6.73969, "heading":-0.20314, "vx":1.50733, "vy":-3.44594, "omega":1.23978, "ax":2.61309, "ay":-8.05874, "alpha":-19.43561, "fx":[80.44854,155.98352,-25.27019,-33.37047], "fy":[-152.91897,-70.95584,-155.6936,-168.73926]}, + {"t":0.42451, "x":1.93159, "y":6.62287, "heading":-0.16266, "vx":1.59266, "vy":-3.70909, "omega":0.60512, "ax":2.13071, "ay":-7.46212, "alpha":-18.09747, "fx":[69.79988,131.24631,-27.64446,-28.4309], "fy":[-146.32928,-81.35265,-123.2297,-156.8026]}, + {"t":0.45717, "x":1.98473, "y":6.49777, "heading":-0.1429, "vx":1.66224, "vy":-3.95277, "omega":0.01415, "ax":-5.8618, "ay":-2.37965, "alpha":-0.19334, "fx":[-99.27616,-99.4231,-100.13849,-99.99273], "fy":[-41.25962,-40.10945,-39.69664,-40.84278]}, + {"t":0.48982, "x":2.03589, "y":6.36743, "heading":-0.14243, "vx":1.47082, "vy":-4.03047, "omega":0.00784, "ax":-5.98794, "ay":-2.02498, "alpha":-0.00312, "fx":[-101.84668,-101.84861,-101.85965,-101.85773], "fy":[-34.45691,-34.43783,-34.43163,-34.45071]}, + {"t":0.52248, "x":2.08072, "y":6.23473, "heading":-0.14218, "vx":1.27529, "vy":-4.0966, "omega":0.00773, "ax":-2.47545, "ay":-0.7441, "alpha":0.00007, "fx":[-42.10685,-42.10689,-42.10661,-42.10657], "fy":[-12.65671,-12.65705,-12.6572,-12.65687]}, + {"t":0.55513, "x":2.12105, "y":6.10056, "heading":-0.14193, "vx":1.19445, "vy":-4.1209, "omega":0.00774, "ax":-0.7372, "ay":-0.21131, "alpha":0.00008, "fx":[-12.53979,-12.53985,-12.53948,-12.53943], "fy":[-3.59402,-3.59441,-3.5945,-3.59411]}, + {"t":0.58778, "x":2.15966, "y":5.96588, "heading":-0.14167, "vx":1.17038, "vy":-4.1278, "omega":0.00774, "ax":-0.25749, "ay":-0.07264, "alpha":0.00017, "fx":[-4.3801,-4.38021,-4.37941,-4.37929], "fy":[-1.23514,-1.23594,-1.23607,-1.23526]}, + {"t":0.62044, "x":2.19774, "y":5.83105, "heading":-0.14142, "vx":1.16197, "vy":-4.13017, "omega":0.00774, "ax":-0.11762, "ay":-0.03292, "alpha":0.0003, "fx":[-2.00128,-2.00148,-2.00002,-1.99981], "fy":[-0.55906,-0.56052,-0.56073,-0.55926]}, + {"t":0.65309, "x":2.23562, "y":5.69616, "heading":-0.14117, "vx":1.15813, "vy":-4.13125, "omega":0.00775, "ax":-0.01519, "ay":-0.00412, "alpha":0.00039, "fx":[-0.25917,-0.25944,-0.25757,-0.25731], "fy":[-0.06897,-0.07083,-0.07109,-0.06923]}, + {"t":0.68575, "x":2.27343, "y":5.56126, "heading":-0.14091, "vx":1.15763, "vy":-4.13138, "omega":0.00777, "ax":0.28473, "ay":0.08028, "alpha":0.0003, "fx":[4.84261,4.84242,4.84388,4.84408], "fy":[1.36631,1.36484,1.36464,1.36611]}, + {"t":0.7184, "x":2.31138, "y":5.42639, "heading":-0.14066, "vx":1.16693, "vy":-4.12876, "omega":0.00778, "ax":1.48445, "ay":0.4292, "alpha":-0.0001, "fx":[25.25032,25.25041,25.24992,25.24984], "fy":[7.30025,7.30075,7.30086,7.30036]}, + {"t":0.75106, "x":2.35028, "y":5.2918, "heading":-0.14041, "vx":1.2154, "vy":-4.11474, "omega":0.00777, "ax":4.95774, "ay":1.57261, "alpha":-0.00028, "fx":[84.33034,84.3303,84.3292,84.32924], "fy":[26.74867,26.75024,26.75071,26.74916]}, + {"t":0.78371, "x":2.39261, "y":5.15827, "heading":-0.14015, "vx":1.3773, "vy":-4.06339, "omega":0.00776, "ax":7.90991, "ay":2.97411, "alpha":0.00993, "fx":[134.52296,134.5408,134.56749,134.54967], "fy":[50.63898,50.56989,50.53856,50.60764]}, + {"t":0.81637, "x":2.44181, "y":5.02717, "heading":-0.1399, "vx":1.63559, "vy":-3.96627, "omega":0.00809, "ax":8.5174, "ay":4.2927, "alpha":0.88592, "fx":[142.25473,144.98959,147.40992,144.85995], "fy":[77.96769,71.90982,68.09207,74.10047]}, + {"t":0.84902, "x":2.49976, "y":4.89994, "heading":-0.13964, "vx":1.91373, "vy":-3.82609, "omega":0.03702, "ax":3.37829, "ay":8.64748, "alpha":13.94404, "fx":[10.03426,-5.42596,133.46189,91.78456], "fy":[171.28351,167.25169,104.47651,145.35311]}, + {"t":0.88168, "x":2.56405, "y":4.77961, "heading":-0.13843, "vx":2.02404, "vy":-3.54371, "omega":0.49236, "ax":1.39368, "ay":9.15961, "alpha":15.79525, "fx":[-20.47114,-64.4684,109.95837,69.80555], "fy":[172.4672,159.4116,132.28892,159.04192]}, + {"t":0.91433, "x":2.63089, "y":4.66877, "heading":-0.12235, "vx":2.06955, "vy":-3.24461, "omega":1.00815, "ax":-0.1924, "ay":10.20908, "alpha":2.33866, "fx":[-12.16656,-16.66784,6.99698,8.74706], "fy":[173.72588,173.17492,173.80321,173.91024]}, + {"t":0.94699, "x":2.69837, "y":4.56826, "heading":-0.08943, "vx":2.06327, "vy":-2.91123, "omega":1.08451, "ax":-1.04697, "ay":10.14015, "alpha":-4.11164, "fx":[-0.16299,3.06975,-31.51045,-42.63089], "fy":[174.39462,174.58872,171.77876,169.16196]}, + {"t":0.97964, "x":2.76518, "y":4.4786, "heading":-0.05401, "vx":2.02908, "vy":-2.58011, "omega":0.95025, "ax":-1.43823, "ay":9.95026, "alpha":-7.54565, "fx":[10.09833,9.8868,-46.87576,-70.96492], "fy":[174.28174,174.60107,168.51375,159.60756]}, + {"t":1.0123, "x":2.83068, "y":4.39966, "heading":-0.02298, "vx":1.98212, "vy":-2.25519, "omega":0.70385, "ax":-1.64366, "ay":9.7822, "alpha":-9.74536, "fx":[18.62186,12.6822,-55.52119,-87.61594], "fy":[173.71515,174.56345,166.01161,151.27979]}, + {"t":1.04495, "x":2.89453, "y":4.33123, "heading":0.0, "vx":1.92845, "vy":-1.93575, "omega":0.38562, "ax":-2.92579, "ay":9.68504, "alpha":-6.60839, "fx":[-24.71637,-17.02459,-66.8811,-90.44539], "fy":[173.12733,174.23847,161.82101,149.7723]}, + {"t":1.07939, "x":2.95921, "y":4.27031, "heading":0.01328, "vx":1.82768, "vy":-1.60219, "omega":0.15802, "ax":-4.88544, "ay":8.96782, "alpha":-4.45855, "fx":[-74.04058,-58.54812,-90.54685,-109.26415], "fy":[158.53007,164.99885,149.88722,136.74439]}, + {"t":1.11383, "x":3.01926, "y":4.22044, "heading":0.01872, "vx":1.65942, "vy":-1.29334, "omega":0.00446, "ax":-6.32605, "ay":8.08365, "alpha":-2.71661, "fx":[-105.56651,-93.00178,-109.79262,-122.05597], "fy":[139.64227,148.36865,136.44858,125.54296]}, + {"t":1.14827, "x":3.07266, "y":4.18069, "heading":0.01888, "vx":1.44155, "vy":-1.01493, "omega":-0.0891, "ax":-7.33328, "ay":7.21838, "alpha":-1.30787, "fx":[-124.73348,-118.35205,-124.89707,-130.96552], "fy":[122.92537,129.10629,122.80822,116.29077]}, + {"t":1.18271, "x":3.11795, "y":4.15002, "heading":0.01581, "vx":1.18899, "vy":-0.76633, "omega":-0.13414, "ax":-8.02986, "ay":6.44881, "alpha":-0.18544, "fx":[-136.66046,-135.78821,-136.51555,-137.37857], "fy":[109.5995,110.68144,109.78678,108.70182]}, + {"t":1.21715, "x":3.15414, "y":4.12745, "heading":0.01119, "vx":0.91244, "vy":-0.54423, "omega":-0.14053, "ax":-8.51625, "ay":5.79148, "alpha":0.70127, "fx":[-144.42543,-147.49155,-145.3695,-142.14916], "fy":[99.21677,94.58836,97.80277,102.43749]}, + {"t":1.25159, "x":3.18051, "y":4.11214, "heading":0.00635, "vx":0.61913, "vy":-0.34477, "omega":-0.11637, "ax":-8.86244, "ay":5.23795, "alpha":1.40572, "fx":[-149.72586,-155.36124,-152.09848,-145.80438], "fy":[91.08295,81.07908,87.0121,97.21012]}, + {"t":1.28603, "x":3.19658, "y":4.10338, "heading":0.00234, "vx":0.3139, "vy":-0.16437, "omega":-0.06796, "ax":-9.1144, "ay":4.77253, "alpha":1.97329, "fx":[-153.50604,-160.73381,-157.21762,-148.67603], "fy":[84.60928,69.88687,77.42305,92.79851]}, + {"t":1.32048, "x":3.20199, "y":4.10055, "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/fetchToF.traj b/src/main/deploy/choreo/fetchToF.traj new file mode 100644 index 00000000..8aefd21f --- /dev/null +++ b/src/main/deploy/choreo/fetchToF.traj @@ -0,0 +1,146 @@ +{ + "name":"fetchToF", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9735233783721924, "y":3.2965428829193115, "heading":1.0253809355466683, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.682854175567627, "y":2.118865728378296, "heading":1.9269363946215805, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.068535475516535, "y":2.873601513280177, "heading":2.0943951023931953, "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":"MaxAngularVelocity", "props":{"max":1.7453292519943295}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":1.242441799491644, "y":5.837604267057031, "r":0.3808915973251697}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9735233783721924 m", "val":2.9735233783721924}, "y":{"exp":"3.2965428829193115 m", "val":3.2965428829193115}, "heading":{"exp":"1.0253809355466685 rad", "val":1.0253809355466683}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.682854175567627 m", "val":4.682854175567627}, "y":{"exp":"2.118865728378296 m", "val":2.118865728378296}, "heading":{"exp":"1.9269363946215805 rad", "val":1.9269363946215805}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"F.x", "val":5.068535475516535}, "y":{"exp":"F.y", "val":2.873601513280177}, "heading":{"exp":"F.heading", "val":2.0943951023931953}, "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":"MaxAngularVelocity", "props":{"max":{"exp":"100 deg / s", "val":1.7453292519943295}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.242441799491644 m", "val":1.242441799491644}, "y":{"exp":"5.837604267057031 m", "val":5.837604267057031}, "r":{"exp":"0.3808915973251697 m", "val":0.3808915973251697}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.33357,2.02536,2.49779], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.25339, "ay":-7.30868, "alpha":23.15434, "fx":[168.55266,-52.48541,38.73116,134.59751], "fy":[-46.9864,-167.0472,-170.95919,-112.28118]}, + {"t":0.02837, "x":1.57313, "y":7.41248, "heading":-0.94268, "vx":0.12069, "vy":-0.20738, "omega":0.65698, "ax":4.4697, "ay":-7.9578, "alpha":18.0954, "fx":[155.52288,-24.75451,44.12909,129.21608], "fy":[-80.14294,-173.30068,-169.61194,-118.38395]}, + {"t":0.05675, "x":1.57835, "y":7.40339, "heading":-0.92404, "vx":0.24751, "vy":-0.43317, "omega":1.17041, "ax":4.66554, "ay":-8.52122, "alpha":12.44584, "fx":[134.51229,8.5208,52.62819,121.77671], "fy":[-111.87068,-174.82488,-167.12201,-125.95617]}, + {"t":0.08512, "x":1.58725, "y":7.38767, "heading":-0.89083, "vx":0.37989, "vy":-0.67495, "omega":1.52355, "ax":4.81077, "ay":-8.90435, "alpha":6.63307, "fx":[108.88492,44.54209,64.56294,109.32908], "fy":[-136.96676,-169.25521,-162.80975,-136.81003]}, + {"t":0.1135, "x":1.59997, "y":7.36494, "heading":-0.8476, "vx":0.51639, "vy":-0.9276, "omega":1.71176, "ax":4.87989, "ay":-9.05385, "alpha":1.03885, "fx":[86.44546,77.54015,79.82469,88.21216], "fy":[-152.16435,-156.89679,-155.77508,-151.17723]}, + {"t":0.14187, "x":1.61658, "y":7.33497, "heading":-0.79903, "vx":0.65485, "vy":-1.18449, "omega":1.74123, "ax":4.8787, "ay":-9.05503, "alpha":0.02106, "fx":[83.04706,82.87388,82.9236,83.09657], "fy":[-153.98972,-154.08318,-154.0571,-153.96368]}, + {"t":0.17024, "x":1.63713, "y":7.29772, "heading":-0.74962, "vx":0.79328, "vy":-1.44142, "omega":1.74183, "ax":4.87505, "ay":-9.05123, "alpha":0.00028, "fx":[82.92403,82.92174,82.92252,82.92482], "fy":[-153.95834,-153.95957,-153.95916,-153.95793]}, + {"t":0.19862, "x":1.6616, "y":7.25318, "heading":-0.7002, "vx":0.9316, "vy":-1.69824, "omega":1.74184, "ax":4.87077, "ay":-9.04547, "alpha":-0.00005, "fx":[82.85037,82.85074,82.8506,82.85023], "fy":[-153.86098,-153.86078,-153.86086,-153.86106]}, + {"t":0.22699, "x":1.68999, "y":7.20135, "heading":-0.65078, "vx":1.0698, "vy":-1.95489, "omega":1.74184, "ax":4.86544, "ay":-9.0363, "alpha":-0.00006, "fx":[82.75969,82.76016,82.75995,82.75947], "fy":[-153.70496,-153.7047,-153.70482,-153.70507]}, + {"t":0.25536, "x":1.7223, "y":7.14224, "heading":-0.60136, "vx":1.20785, "vy":-2.21128, "omega":1.74183, "ax":4.85809, "ay":-9.0203, "alpha":-0.00009, "fx":[82.63454,82.63519,82.63486,82.6342], "fy":[-153.43285,-153.43249,-153.43267,-153.43302]}, + {"t":0.28374, "x":1.75853, "y":7.07587, "heading":-0.55193, "vx":1.3457, "vy":-2.46723, "omega":1.74183, "ax":4.84581, "ay":-8.98744, "alpha":-0.00017, "fx":[82.4255,82.42676,82.42602,82.42477], "fy":[-152.87388,-152.8732,-152.87357,-152.87426]}, + {"t":0.31211, "x":1.79866, "y":7.00225, "heading":-0.50251, "vx":1.48319, "vy":-2.72223, "omega":1.74183, "ax":4.81453, "ay":-8.88945, "alpha":-0.00099, "fx":[81.89262,81.89957,81.89499,81.88804], "fy":[-151.20771,-151.20383,-151.20613,-151.21001]}, + {"t":0.34049, "x":1.84269, "y":6.92143, "heading":-0.45309, "vx":1.6198, "vy":-2.97446, "omega":1.7418, "ax":3.60643, "ay":-5.31497, "alpha":-2.21001, "fx":[61.69822,70.34643,61.21733,52.11542], "fy":[-95.59572,-85.7816,-85.03883,-95.20804]}, + {"t":0.36886, "x":1.8901, "y":6.83489, "heading":-0.40367, "vx":1.72213, "vy":-3.12527, "omega":1.67909, "ax":0.09096, "ay":-1.90397, "alpha":-4.19011, "fx":[6.91956,15.55323,-4.12032,-12.16343], "fy":[-45.16288,-27.30145,-19.31247,-37.76685]}, + {"t":0.39723, "x":1.939, "y":6.74545, "heading":-0.35602, "vx":1.72471, "vy":-3.17929, "omega":1.5602, "ax":-0.9255, "ay":-2.07545, "alpha":-3.30774, "fx":[-10.63281,-5.05649,-21.17582,-26.10456], "fy":[-45.44149,-31.14249,-25.0585,-39.56865]}, + {"t":0.42561, "x":1.98756, "y":6.65441, "heading":-0.31176, "vx":1.69845, "vy":-3.23818, "omega":1.46635, "ax":-2.03167, "ay":-3.26071, "alpha":-4.49371, "fx":[-25.87,-20.28654,-44.26824,-47.80751], "fy":[-69.11284,-50.70213,-41.5287,-60.51129]}, + {"t":0.45398, "x":2.03494, "y":6.56121, "heading":-0.27015, "vx":1.6408, "vy":-3.3307, "omega":1.33885, "ax":-4.12779, "ay":-4.73405, "alpha":-5.56499, "fx":[-55.16312,-55.12065,-87.03706,-83.52947], "fy":[-99.1361,-78.40877,-60.62139,-83.93301]}, + {"t":0.48236, "x":2.07983, "y":6.4648, "heading":-0.23216, "vx":1.52368, "vy":-3.46502, "omega":1.18095, "ax":-6.16568, "ay":-5.41719, "alpha":-5.71088, "fx":[-85.10246,-95.72467,-124.89659,-113.78209], "fy":[-115.61108,-92.86905,-66.33256,-93.76694]}, + {"t":0.51073, "x":2.12058, "y":6.36431, "heading":-0.19865, "vx":1.34874, "vy":-3.61873, "omega":1.01891, "ax":-6.84709, "ay":-4.52188, "alpha":-4.1796, "fx":[-103.167,-112.43429,-129.1124,-121.15431], "fy":[-96.32346,-74.15162,-57.0989,-80.08928]}, + {"t":0.5391, "x":2.15609, "y":6.25981, "heading":-0.16974, "vx":1.15446, "vy":-3.74703, "omega":0.90031, "ax":-5.13448, "ay":-3.02162, "alpha":-2.94527, "fx":[-80.0287,-82.08562,-94.66423,-92.56563], "fy":[-62.80753,-46.89817,-40.17362,-55.70852]}, + {"t":0.56748, "x":2.18678, "y":6.15228, "heading":-0.1442, "vx":1.00877, "vy":-3.83277, "omega":0.81675, "ax":-2.60635, "ay":-1.13651, "alpha":-0.66133, "fx":[-42.94336,-42.67441,-45.73038,-45.98459], "fy":[-21.3329,-17.99458,-17.34217,-20.65709]}, + {"t":0.59585, "x":2.21436, "y":6.04307, "heading":-0.12102, "vx":0.93482, "vy":-3.86501, "omega":0.79798, "ax":-1.24854, "ay":-0.24108, "alpha":0.52266, "fx":[-22.3404,-22.62733,-20.13599,-19.8455], "fy":[-2.65931,-5.21606,-5.54617,-2.9811]}, + {"t":0.62422, "x":2.24038, "y":5.93331, "heading":-0.09838, "vx":0.89939, "vy":-3.87185, "omega":0.81281, "ax":-0.80882, "ay":0.01259, "alpha":0.87597, "fx":[-15.6597,-16.07556,-11.8574,-11.43865], "fy":[2.5566,-1.70731,-2.13618,2.14383]}, + {"t":0.6526, "x":2.26557, "y":5.82345, "heading":-0.07532, "vx":0.87644, "vy":-3.8715, "omega":0.83767, "ax":-0.69286, "ay":0.07844, "alpha":1.00739, "fx":[-14.03444,-14.40663,-9.53655,-9.16362], "fy":[3.96992,-0.93556,-1.31072,3.61337]}, + {"t":0.68097, "x":2.29016, "y":5.71363, "heading":-0.05155, "vx":0.85679, "vy":-3.86927, "omega":0.86625, "ax":-0.6758, "ay":0.09309, "alpha":1.08857, "fx":[-13.99198,-14.27061,-8.99804,-8.72007], "fy":[4.37085,-0.93602,-1.21474,4.11338]}, + {"t":0.70935, "x":2.3142, "y":5.60389, "heading":-0.02697, "vx":0.83761, "vy":-3.86663, "omega":0.89714, "ax":-0.6773, "ay":0.08391, "alpha":1.13129, "fx":[-14.18565,-14.34026,-8.85505,-8.70197], "fy":[4.25908,-1.261,-1.41582,4.12718]}, + {"t":0.73772, "x":2.33769, "y":5.49421, "heading":-0.00152, "vx":0.81839, "vy":-3.86425, "omega":0.92924, "ax":-0.65957, "ay":0.05053, "alpha":1.10622, "fx":[-13.89602,-13.90828,-8.54156,-8.53081], "fy":[3.56149,-1.83768,-1.85314,3.56727]}, + {"t":0.76609, "x":2.36065, "y":5.38459, "heading":0.02485, "vx":0.79968, "vy":-3.86281, "omega":0.96062, "ax":-0.59759, "ay":-0.01033, "alpha":0.98187, "fx":[-12.60661,-12.48788,-7.72293,-7.84215], "fy":[2.15961,-2.62964,-2.51855,2.28584]}, + {"t":0.79447, "x":2.3831, "y":5.27498, "heading":0.05211, "vx":0.78272, "vy":-3.86311, "omega":0.98848, "ax":-0.47371, "ay":-0.08857, "alpha":0.76306, "fx":[-10.00776,-9.81138,-6.10791,-6.30372], "fy":[0.25564,-3.45986,-3.2725,0.45034]}, + {"t":0.82284, "x":2.40512, "y":5.16533, "heading":0.08015, "vx":0.76928, "vy":-3.86562, "omega":1.01013, "ax":-0.25824, "ay":-0.17547, "alpha":0.44021, "fx":[-5.54756,-5.3738,-3.23784,-3.41093], "fy":[-2.00101,-4.13832,-3.96914,-1.83039]}, + {"t":0.85122, "x":2.42684, "y":5.05558, "heading":0.10881, "vx":0.76195, "vy":-3.8706, "omega":1.02262, "ax":0.10604, "ay":-0.25391, "alpha":0.00202, "fx":[1.79834,1.7994,1.80917,1.8081], "fy":[-4.3146,-4.32436,-4.32329,-4.31354]}, + {"t":0.87959, "x":2.4485, "y":4.94565, "heading":0.13783, "vx":0.76496, "vy":-3.8778, "omega":1.02268, "ax":0.71042, "ay":-0.27604, "alpha":-0.5104, "fx":[13.47568,13.14611,10.69091,11.02355], "fy":[-5.75375,-3.28484,-3.63476,-6.10781]}, + {"t":0.90796, "x":2.47049, "y":4.83551, "heading":0.16685, "vx":0.78512, "vy":-3.88564, "omega":1.0082, "ax":1.61383, "ay":-0.12713, "alpha":-0.85311, "fx":[29.77687,29.12498,25.11894,25.78209], "fy":[-3.89121,0.28877,-0.41938,-4.62823]}, + {"t":0.93634, "x":2.49342, "y":4.72521, "heading":0.19545, "vx":0.83091, "vy":-3.88924, "omega":0.98399, "ax":2.56668, "ay":0.31987, "alpha":-0.5019, "fx":[45.02113,44.5404,42.29378,42.77871], "fy":[4.40299,6.94565,6.48776,3.92752]}, + {"t":0.96471, "x":2.51803, "y":4.61499, "heading":0.22337, "vx":0.90374, "vy":-3.88017, "omega":0.96975, "ax":3.02274, "ay":1.01925, "alpha":0.88728, "fx":[48.92938,50.0645,53.89579,52.77403], "fy":[19.25678,14.63141,15.45264,20.00799]}, + {"t":0.99308, "x":2.54489, "y":4.5053, "heading":0.25089, "vx":0.9895, "vy":-3.85125, "omega":0.99493, "ax":3.05795, "ay":1.8576, "alpha":2.98327, "fx":[43.07568,48.05082,60.88486,56.04825], "fy":[38.26892,22.6112,25.33768,40.17125]}, + {"t":1.02146, "x":2.57419, "y":4.39678, "heading":0.27912, "vx":1.07627, "vy":-3.79854, "omega":1.07957, "ax":2.93476, "ay":2.60914, "alpha":4.85396, "fx":[34.34033,44.13007,65.35204,55.8556], "fy":[55.22242,30.11918,34.56842,57.61272]}, + {"t":1.04983, "x":2.60591, "y":4.29005, "heading":0.30975, "vx":1.15954, "vy":-3.72451, "omega":1.2173, "ax":2.43521, "ay":2.86852, "alpha":5.43262, "fx":[23.34861,34.85417,59.48213,48.00365], "fy":[59.88305,33.02882,38.8083,63.45079]}, + {"t":1.07821, "x":2.63979, "y":4.18552, "heading":0.34429, "vx":1.22864, "vy":-3.64312, "omega":1.37144, "ax":1.40348, "ay":2.36232, "alpha":4.216, "fx":[10.00017,18.20304,37.89089,29.39736], "fy":[47.23088,27.61819,33.6318,52.2488]}, + {"t":1.10658, "x":2.67522, "y":4.0831, "heading":0.3832, "vx":1.26846, "vy":-3.57609, "omega":1.49107, "ax":0.33546, "ay":1.45037, "alpha":2.12877, "fx":[-1.1683,2.82005,12.62818,8.54444], "fy":[27.58239,18.04135,21.82746,31.23054]}, + {"t":1.13495, "x":2.71135, "y":3.98222, "heading":0.42551, "vx":1.27798, "vy":-3.53494, "omega":1.55147, "ax":-0.35219, "ay":0.79868, "alpha":0.76691, "fx":[-8.46668,-6.9415,-3.50857,-5.04605], "fy":[14.49932,11.11885,12.67412,16.04888]}, + {"t":1.16333, "x":2.74747, "y":3.88224, "heading":0.46953, "vx":1.26798, "vy":-3.51227, "omega":1.57323, "ax":-0.64495, "ay":0.51555, "alpha":0.27006, "fx":[-11.85096,-11.26477,-10.08893,-10.67669], "fy":[9.05324,7.88227,8.48563,9.65651]}, + {"t":1.1917, "x":2.78318, "y":3.78279, "heading":0.51417, "vx":1.24968, "vy":-3.49765, "omega":1.58089, "ax":-0.59496, "ay":0.49498, "alpha":0.27194, "fx":[-11.01976,-10.37615,-9.21975,-9.86484], "fy":[8.66571,7.51478,8.17335,9.32404]}, + {"t":1.22008, "x":2.8184, "y":3.68375, "heading":0.55903, "vx":1.2328, "vy":-3.4836, "omega":1.58861, "ax":0.02516, "ay":0.76279, "alpha":0.61476, "fx":[-1.64593,-0.04989,2.50333,0.9042], "fy":[13.44985,10.92136,12.50317,15.02488]}, + {"t":1.24845, "x":2.85339, "y":3.58522, "heading":0.6041, "vx":1.23352, "vy":-3.46196, "omega":1.60605, "ax":2.15562, "ay":1.46774, "alpha":0.91717, "fx":[33.58979,36.30386,39.72584,37.04668], "fy":[25.74601,21.8074,24.2107,28.09909]}, + {"t":1.27682, "x":2.88926, "y":3.48758, "heading":0.64967, "vx":1.29468, "vy":-3.42031, "omega":1.63208, "ax":6.09701, "ay":2.0631, "alpha":-0.88482, "fx":[105.87886,103.29245,101.51254,104.14968], "fy":[33.87295,38.91371,36.38136,31.20321]}, + {"t":1.3052, "x":2.92845, "y":3.39136, "heading":0.69598, "vx":1.46768, "vy":-3.36177, "omega":1.60697, "ax":8.52312, "ay":1.41482, "alpha":-5.6279, "fx":[152.41388,140.79694,138.68456,148.00817], "fy":[18.04056,54.1354,33.33741,-9.25085]}, + {"t":1.33357, "x":2.97352, "y":3.29654, "heading":0.74158, "vx":1.70951, "vy":-3.32163, "omega":1.44728, "ax":5.28074, "ay":6.35334, "alpha":8.28897, "fx":[52.84715,111.14529,117.74323,77.56007], "fy":[129.73111,80.18419,92.91412,129.44439]}, + {"t":1.35044, "x":3.00312, "y":3.2414, "heading":0.76599, "vx":1.79861, "vy":-3.21443, "omega":1.58714, "ax":5.2561, "ay":5.29673, "alpha":5.20544, "fx":[69.8231,99.82878,106.16342,81.80367], "fy":[101.15779,71.03402,81.80028,106.39108]}, + {"t":1.36732, "x":3.03422, "y":3.18792, "heading":0.79277, "vx":1.8873, "vy":-3.12506, "omega":1.67498, "ax":4.75339, "ay":3.6949, "alpha":1.54254, "fx":[75.9225,82.71986,85.63029,79.1426], "fy":[64.65496,56.9848,61.23035,68.52638]}, + {"t":1.38419, "x":3.06674, "y":3.13572, "heading":0.82104, "vx":1.9675, "vy":-3.06272, "omega":1.701, "ax":3.24359, "ay":2.48852, "alpha":0.29323, "fx":[54.20793,55.36204,56.13343,54.98682], "fy":[42.44639,41.27153,42.2151,43.38302]}, + {"t":1.40106, "x":3.1004, "y":3.08439, "heading":0.84974, "vx":2.02223, "vy":-3.02073, "omega":1.70595, "ax":1.62007, "ay":1.54983, "alpha":0.08421, "fx":[27.26892,27.58929,27.84479,27.52476], "fy":[26.35715,26.07114,26.36721,26.65295]}, + {"t":1.41794, "x":3.13475, "y":3.03364, "heading":0.87852, "vx":2.04957, "vy":-2.99458, "omega":1.70737, "ax":0.47726, "ay":0.87634, "alpha":0.0365, "fx":[7.99228,8.13074,8.24371,8.10527], "fy":[14.89572,14.78173,14.91701,15.03098]}, + {"t":1.43481, "x":3.1694, "y":2.98324, "heading":0.90733, "vx":2.05762, "vy":-2.97979, "omega":1.70799, "ax":-0.16919, "ay":0.46837, "alpha":0.01736, "fx":[-2.93729,-2.87063,-2.81837,-2.88503], "fy":[7.95957,7.90769,7.97426,8.02613]}, + {"t":1.45168, "x":3.20409, "y":2.93303, "heading":0.93615, "vx":2.05476, "vy":-2.97189, "omega":1.70828, "ax":-0.46958, "ay":0.25534, "alpha":0.00919, "fx":[-8.01874,-7.9828,-7.95621,-7.99215], "fy":[4.33847,4.31196,4.34813,4.37463]}, + {"t":1.46855, "x":3.23869, "y":2.88292, "heading":0.96497, "vx":2.04684, "vy":-2.96758, "omega":1.70844, "ax":-0.52258, "ay":0.18703, "alpha":0.00741, "fx":[-8.91403,-8.88445,-8.86388,-8.89346], "fy":[3.17671,3.15615,3.18594,3.20649]}, + {"t":1.48543, "x":3.27316, "y":2.83288, "heading":0.9938, "vx":2.03802, "vy":-2.96442, "omega":1.70856, "ax":-0.34814, "ay":0.25609, "alpha":0.0108, "fx":[-5.95819,-5.91419,-5.88544,-5.92944], "fy":[4.34831,4.31968,4.36382,4.39245]}, + {"t":1.5023, "x":3.30749, "y":2.7829, "heading":1.02263, "vx":2.03215, "vy":-2.9601, "omega":1.70874, "ax":0.11003, "ay":0.50286, "alpha":0.02045, "fx":[1.80298,1.88822,1.94014,1.85491], "fy":[8.537,8.48513,8.56988,8.62174]}, + {"t":1.51917, "x":3.3418, "y":2.73302, "heading":1.05146, "vx":2.03401, "vy":-2.95162, "omega":1.70909, "ax":0.97903, "ay":1.0206, "alpha":0.03978, "fx":[16.52139,16.69154,16.78457,16.61448], "fy":[17.32662,17.22738,17.39364,17.49286]}, + {"t":1.53605, "x":3.37626, "y":2.68336, "heading":1.0803, "vx":2.05052, "vy":-2.9344, "omega":1.70976, "ax":2.35745, "ay":1.90943, "alpha":0.07178, "fx":[39.87329,40.19026,40.32573,40.00912], "fy":[32.42569,32.2319,32.53196,32.72565]}, + {"t":1.55292, "x":3.41119, "y":2.63412, "heading":1.10915, "vx":2.0903, "vy":-2.90218, "omega":1.71097, "ax":4.00142, "ay":3.08163, "alpha":0.11024, "fx":[67.75278,68.26005,68.37261,67.86647], "fy":[52.37308,52.00755,52.46261,52.82745]}, + {"t":1.56979, "x":3.44703, "y":2.58559, "heading":1.13801, "vx":2.15782, "vy":-2.85018, "omega":1.71283, "ax":5.349, "ay":4.21912, "alpha":0.1409, "fx":[90.6464,91.32903,91.32252,90.64165], "fy":[71.77522,71.19275,71.75815,72.33818]}, + {"t":1.58667, "x":3.4842, "y":2.5381, "heading":1.16692, "vx":2.24807, "vy":-2.77899, "omega":1.71521, "ax":6.15504, "ay":5.13545, "alpha":0.15668, "fx":[104.36761,105.17256,105.02222,104.21924], "fy":[87.4285,86.67699,87.27881,88.02564]}, + {"t":1.60354, "x":3.52301, "y":2.49194, "heading":1.19586, "vx":2.35192, "vy":-2.69234, "omega":1.71785, "ax":6.52779, "ay":5.84783, "alpha":0.15919, "fx":[110.72927,111.59844,111.34135,110.47423], "fy":[99.59603,98.76895,99.34689,100.16777]}, + {"t":1.62041, "x":3.56362, "y":2.44735, "heading":1.22484, "vx":2.46207, "vy":-2.59367, "omega":1.72054, "ax":6.62559, "ay":6.4212, "alpha":0.1528, "fx":[112.4135,113.29891,112.98414,112.10086], "fy":[109.37564,108.55389,109.0731,109.88823]}, + {"t":1.63728, "x":3.60611, "y":2.4045, "heading":1.25387, "vx":2.57386, "vy":-2.48533, "omega":1.72311, "ax":6.55552, "ay":6.90459, "alpha":0.14226, "fx":[111.23865,112.11088,111.77523,110.90522], "fy":[117.60712,116.83757,117.28633,118.04962]}, + {"t":1.65416, "x":3.65047, "y":2.36355, "heading":1.28295, "vx":2.68447, "vy":-2.36883, "omega":1.72552, "ax":6.37805, "ay":7.32873, "alpha":0.13511, "fx":[108.2243,109.09787,108.75209,107.88094], "fy":[124.82538,124.10417,124.49678,125.21203]}, + {"t":1.67103, "x":3.69667, "y":2.32462, "heading":1.31206, "vx":2.79209, "vy":-2.24517, "omega":1.7278, "ax":6.11824, "ay":7.71798, "alpha":0.15754, "fx":[103.74397,104.81624,104.39317,103.3249], "fy":[131.4829,130.66497,131.08243,131.89201]}, + {"t":1.6879, "x":3.74465, "y":2.28784, "heading":1.34121, "vx":2.89532, "vy":-2.11495, "omega":1.73045, "ax":5.72664, "ay":8.12897, "alpha":0.30045, "fx":[96.72765,98.89871,98.08031,95.92753], "fy":[138.67288,137.18201,137.88528,139.34543]}, + {"t":1.70478, "x":3.79432, "y":2.25331, "heading":1.37041, "vx":2.99195, "vy":-1.97779, "omega":1.73552, "ax":4.56048, "ay":8.91828, "alpha":0.24919, "fx":[76.81915,78.85992,78.31554,76.29538], "fy":[152.03672,151.00976,151.36768,152.37544]}, + {"t":1.72165, "x":3.84545, "y":2.22121, "heading":1.39969, "vx":3.0689, "vy":-1.82731, "omega":1.73973, "ax":2.00541, "ay":9.88402, "alpha":0.02547, "fx":[33.99544,34.23329,34.2273,33.98978], "fy":[168.14509,168.09699,168.10381,168.15183]}, + {"t":1.73852, "x":3.89752, "y":2.19178, "heading":1.42905, "vx":3.10273, "vy":-1.66054, "omega":1.74016, "ax":0.16486, "ay":10.13626, "alpha":0.00441, "fx":[2.78065,2.82254,2.82783,2.78595], "fy":[172.41499,172.41422,172.41487,172.41564]}, + {"t":1.75539, "x":3.9499, "y":2.16521, "heading":1.45841, "vx":3.10551, "vy":-1.48951, "omega":1.74023, "ax":-1.0175, "ay":10.12237, "alpha":0.00156, "fx":[-17.31634,-17.30172,-17.29859,-17.3132], "fy":[172.17756,172.17899,172.17951,172.17809]}, + {"t":1.77227, "x":4.00215, "y":2.14152, "heading":1.48777, "vx":3.08835, "vy":-1.31871, "omega":1.74026, "ax":-1.80446, "ay":10.0373, "alpha":0.00077, "fx":[-30.69786,-30.69083,-30.68896,-30.696], "fy":[170.73079,170.73203,170.73245,170.7312]}, + {"t":1.78914, "x":4.054, "y":2.12069, "heading":1.51714, "vx":3.0579, "vy":-1.14935, "omega":1.74027, "ax":-2.35553, "ay":9.94083, "alpha":0.00045, "fx":[-40.06944,-40.06543,-40.06426,-40.06827], "fy":[169.09012,169.09106,169.09138,169.09044]}, + {"t":1.80601, "x":4.10526, "y":2.10272, "heading":1.5465, "vx":3.01815, "vy":-0.98162, "omega":1.74028, "ax":-2.75925, "ay":9.85031, "alpha":0.00028, "fx":[-46.93578,-46.93325,-46.93247,-46.93501], "fy":[167.55051,167.55121,167.55145,167.55074]}, + {"t":1.82289, "x":4.1558, "y":2.08756, "heading":1.57586, "vx":2.9716, "vy":-0.81542, "omega":1.74028, "ax":-3.06625, "ay":9.76993, "alpha":0.00019, "fx":[-52.15723,-52.15551,-52.15499,-52.1567], "fy":[166.18332,166.18385,166.18403,166.18349]}, + {"t":1.83976, "x":4.2055, "y":2.07519, "heading":1.60523, "vx":2.91986, "vy":-0.65057, "omega":1.74029, "ax":-3.30687, "ay":9.69978, "alpha":0.00014, "fx":[-56.24977,-56.24855,-56.24818,-56.2494], "fy":[164.99013,164.99055,164.99068,164.99027]}, + {"t":1.85663, "x":4.2543, "y":2.06559, "heading":1.63459, "vx":2.86406, "vy":-0.48691, "omega":1.74029, "ax":-3.50019, "ay":9.63876, "alpha":0.0001, "fx":[-59.53781,-59.53692,-59.53666,-59.53755], "fy":[163.95239,163.95272,163.95282,163.95249]}, + {"t":1.87351, "x":4.30212, "y":2.05875, "heading":1.66395, "vx":2.80501, "vy":-0.32427, "omega":1.74029, "ax":-3.65871, "ay":9.58558, "alpha":0.00008, "fx":[-62.23409,-62.23342,-62.23324,-62.23391], "fy":[163.04781,163.04806,163.04814,163.04788]}, + {"t":1.89038, "x":4.34893, "y":2.05464, "heading":1.69332, "vx":2.74327, "vy":-0.16254, "omega":1.74029, "ax":-3.79095, "ay":9.53901, "alpha":0.00006, "fx":[-64.48325,-64.48275,-64.48262,-64.48312], "fy":[162.25566,162.25586,162.25591,162.25571]}, + {"t":1.90725, "x":4.39468, "y":2.05326, "heading":1.72268, "vx":2.67931, "vy":-0.00158, "omega":1.74029, "ax":-3.90286, "ay":9.498, "alpha":0.00003, "fx":[-66.38674,-66.38645,-66.38638,-66.38667], "fy":[161.55815,161.55827,161.55831,161.55819]}, + {"t":1.92412, "x":4.43933, "y":2.05458, "heading":1.75205, "vx":2.61346, "vy":0.15868, "omega":1.74029, "ax":-3.99876, "ay":9.46168, "alpha":-0.00016, "fx":[-68.01689,-68.01827,-68.01857,-68.01719], "fy":[160.94086,160.94028,160.94014,160.94072]}, + {"t":1.941, "x":4.48286, "y":2.05861, "heading":1.78141, "vx":2.54598, "vy":0.31832, "omega":1.74029, "ax":-4.08181, "ay":9.42934, "alpha":-0.00313, "fx":[-69.41406,-69.4414,-69.44684,-69.41949], "fy":[160.39754,160.38574,160.38325,160.39506]}, + {"t":1.95787, "x":4.52524, "y":2.06532, "heading":1.81077, "vx":2.47711, "vy":0.47742, "omega":1.74024, "ax":-4.15441, "ay":9.40038, "alpha":-0.04865, "fx":[-70.41548,-70.84023,-70.91569,-70.48994], "fy":[160.00893,159.82168,159.78629,159.97423]}, + {"t":1.97474, "x":4.56644, "y":2.07471, "heading":1.84014, "vx":2.40701, "vy":0.63604, "omega":1.73942, "ax":-4.21777, "ay":9.37295, "alpha":-0.61993, "fx":[-68.659,-73.99846,-74.91051,-69.40452], "fy":[160.82139,158.43892,157.98656,160.47787]}, + {"t":1.99162, "x":4.60645, "y":2.08678, "heading":1.86949, "vx":2.33585, "vy":0.79418, "omega":1.72896, "ax":-4.25863, "ay":9.31538, "alpha":-3.14469, "fx":[-58.15135,-83.35406,-88.86788,-59.3787], "fy":[164.98463,153.8089,150.57782,164.43662]}, + {"t":2.00849, "x":4.64526, "y":2.10151, "heading":1.89866, "vx":2.26399, "vy":0.95136, "omega":1.6759, "ax":-4.25485, "ay":9.18646, "alpha":-6.40189, "fx":[-47.06113,-93.15751,-106.15324,-43.12293], "fy":[168.5539,148.1591,138.92221,169.401]}, + {"t":2.02536, "x":4.68285, "y":2.11887, "heading":1.92694, "vx":2.1922, "vy":1.10637, "omega":1.56788, "ax":-4.48089, "ay":9.11781, "alpha":-5.9568, "fx":[-52.63026,-95.73822,-107.16889,-49.33723], "fy":[167.11492,146.74381,138.4977,168.00888]}, + {"t":2.05686, "x":4.74968, "y":2.15823, "heading":1.97632, "vx":2.05108, "vy":1.39353, "omega":1.38027, "ax":-4.93961, "ay":8.85457, "alpha":-6.31413, "fx":[-59.98686,-103.9674,-115.95171,-56.17914], "fy":[164.56731,140.97648,131.14168,165.7692]}, + {"t":2.08835, "x":4.81182, "y":2.20651, "heading":2.01979, "vx":1.8955, "vy":1.6724, "omega":1.18141, "ax":-5.57275, "ay":8.43605, "alpha":-6.78872, "fx":[-69.78068,-114.45127,-128.02364,-66.90773], "fy":[160.58652,132.52484,119.249,161.61905]}, + {"t":2.11985, "x":4.86876, "y":2.26337, "heading":2.057, "vx":1.71999, "vy":1.9381, "omega":0.96759, "ax":-6.47942, "ay":7.70789, "alpha":-7.40216, "fx":[-83.75139,-128.21358,-144.25604,-84.6315], "fy":[153.64806,119.1335,98.7702,152.88404]}, + {"t":2.15134, "x":4.91972, "y":2.32823, "heading":2.08747, "vx":1.51592, "vy":2.18086, "omega":0.73446, "ax":-7.79616, "ay":6.29153, "alpha":-8.02616, "fx":[-105.29244,-146.21329,-163.3197,-115.61615], "fy":[139.56431,95.96678,61.91348,130.6239]}, + {"t":2.18284, "x":4.96359, "y":2.40004, "heading":2.1106, "vx":1.27038, "vy":2.37901, "omega":0.48168, "ax":-9.44329, "ay":3.25061, "alpha":-8.1471, "fx":[-139.53148,-166.45235,-174.50183,-162.02496], "fy":[104.87516,53.13009,-0.20342,63.36566]}, + {"t":2.21433, "x":4.99892, "y":2.47658, "heading":2.12577, "vx":0.97296, "vy":2.48139, "omega":0.22509, "ax":-9.78241, "ay":-2.14933, "alpha":-7.70176, "fx":[-173.62097,-173.44298,-157.36913,-161.15065], "fy":[14.43409,-19.85159,-75.36777,-65.45262]}, + {"t":2.24583, "x":5.02471, "y":2.55366, "heading":2.13286, "vx":0.66487, "vy":2.41369, "omega":-0.01748, "ax":-7.51062, "ay":-6.86796, "alpha":-4.55605, "fx":[-141.44925,-143.06413,-118.08337,-108.41758], "fy":[-101.92782,-100.08887,-128.65408,-136.61735]}, + {"t":2.27732, "x":5.04193, "y":2.62628, "heading":2.13231, "vx":0.42832, "vy":2.19739, "omega":-0.16097, "ax":-4.96762, "ay":-8.97594, "alpha":-1.91685, "fx":[-88.0147,-95.22035,-81.64072,-73.11561], "fy":[-150.90038,-146.54753,-154.56149,-158.703]}, + {"t":2.30882, "x":5.05295, "y":2.69103, "heading":2.12724, "vx":0.27187, "vy":1.91469, "omega":-0.22134, "ax":-3.26342, "ay":-9.75157, "alpha":-0.37319, "fx":[-55.74156,-57.92306,-55.29046,-53.0841], "fy":[-165.79491,-165.055,-165.96295,-166.67267]}, + {"t":2.34031, "x":5.0599, "y":2.7465, "heading":2.12027, "vx":0.16908, "vy":1.60757, "omega":-0.2331, "ax":-2.17407, "ay":-10.05681, "alpha":0.56749, "fx":[-37.07274,-33.16775,-36.89301,-40.78759], "fy":[-171.07551,-171.86585,-171.09426,-170.21805]}, + {"t":2.37181, "x":5.06414, "y":2.79214, "heading":2.11293, "vx":0.10061, "vy":1.29083, "omega":-0.21522, "ax":-1.44553, "ay":-10.18739, "alpha":1.17876, "fx":[-25.3874,-16.63393,-23.73898,-32.59191], "fy":[-173.28043,-174.3246,-173.48008,-172.05325]}, + {"t":2.4033, "x":5.0666, "y":2.82774, "heading":2.10615, "vx":0.05509, "vy":0.96998, "omega":-0.1781, "ax":-0.93199, "ay":-10.24571, "alpha":1.60163, "fx":[-17.51072,-5.12233,-14.0086,-26.76986], "fy":[-174.31037,-175.09837,-174.58814,-173.10953]}, + {"t":2.4348, "x":5.06787, "y":2.85321, "heading":2.10054, "vx":0.02573, "vy":0.64729, "omega":-0.12766, "ax":-0.55332, "ay":-10.27124, "alpha":1.9097, "fx":[-11.87947,3.24513,-6.58204,-22.43088], "fy":[-174.826,-175.18541,-175.06596,-173.7657]}, + {"t":2.46629, "x":5.0684, "y":2.8685, "heading":2.09652, "vx":0.00831, "vy":0.32379, "omega":-0.06751, "ax":-0.26371, "ay":-10.28084, "alpha":2.1435, "fx":[-7.6597,9.56096,-0.76594,-19.07773], "fy":[-175.09292,-174.98676,-175.21987,-174.19708]}, + {"t":2.49779, "x":5.06854, "y":2.8736, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/fetchToI.traj b/src/main/deploy/choreo/fetchToI.traj new file mode 100644 index 00000000..007b22e8 --- /dev/null +++ b/src/main/deploy/choreo/fetchToI.traj @@ -0,0 +1,86 @@ +{ + "name":"fetchToI", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.334167003631592, "y":5.231322765350342, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "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":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.334167003631592 m", "val":5.334167003631592}, "y":{"exp":"5.231322765350342 m", "val":5.231322765350342}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "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.27899,1.53726], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.79753, "ay":-4.55168, "alpha":-10.00183, "fx":[122.94141,162.68044,174.8774,138.07467], "fy":[-124.96508,-65.38182,-11.57945,-107.76502]}, + {"t":0.03654, "x":1.57729, "y":7.41238, "heading":-0.94268, "vx":0.32149, "vy":-0.16633, "omega":-0.36549, "ax":8.83195, "ay":-4.55621, "alpha":-9.56698, "fx":[124.39301,162.48082,174.61885,139.42302], "fy":[-123.4916,-65.82875,-14.70735,-105.97188]}, + {"t":0.07309, "x":1.59493, "y":7.40326, "heading":-0.95603, "vx":0.64423, "vy":-0.33283, "omega":-0.7151, "ax":8.8742, "ay":-4.56271, "alpha":-8.98981, "fx":[125.93572,161.99242,174.21155,141.65056], "fy":[-121.88044,-66.96259,-18.67652,-102.92192]}, + {"t":0.10963, "x":1.6244, "y":7.38805, "heading":-0.98216, "vx":0.96852, "vy":-0.49956, "omega":-1.04361, "ax":8.92351, "ay":-4.57161, "alpha":-8.25435, "fx":[127.82336,161.22543,173.56321,144.53374], "fy":[-119.84882,-68.71361,-23.71913,-98.76539]}, + {"t":0.14617, "x":1.66575, "y":7.36674, "heading":-1.0203, "vx":1.29461, "vy":-0.66662, "omega":-1.34525, "ax":8.97908, "ay":-4.58399, "alpha":-7.32184, "fx":[130.44836,160.19601,172.50413,147.77785], "fy":[-116.91595,-70.98103,-30.23446,-93.75828]}, + {"t":0.18271, "x":1.71905, "y":7.33932, "heading":-1.06946, "vx":1.62272, "vy":-0.83413, "omega":-1.61281, "ax":9.03994, "ay":-4.60181, "alpha":-6.09877, "fx":[134.42428,158.93928,170.69646,151.00708], "fy":[-112.21895,-73.61272,-38.91241,-88.35805]}, + {"t":0.21926, "x":1.78439, "y":7.30577, "heading":-1.1284, "vx":1.95307, "vy":-1.00229, "omega":-1.83567, "ax":9.10368, "ay":-4.62625, "alpha":-4.36498, "fx":[140.70695,157.53936,167.38666,153.77123], "fy":[-104.06972,-76.34936,-51.02492,-83.32098]}, + {"t":0.2558, "x":1.86184, "y":7.26605, "heading":-1.19548, "vx":2.28574, "vy":-1.17135, "omega":-1.99518, "ax":9.15656, "ay":-4.64991, "alpha":-1.60822, "fx":[150.63227,156.21991,160.60075,155.54863], "fy":[-88.80485,-78.63427,-69.1531,-79.78201]}, + {"t":0.29234, "x":1.95148, "y":7.22015, "heading":-1.26839, "vx":2.62035, "vy":-1.34127, "omega":-2.05395, "ax":9.11396, "ay":-4.62529, "alpha":3.35969, "fx":[164.76314,155.75234,143.9132,155.67499], "fy":[-57.84492,-78.64352,-98.89831,-79.31243]}, + {"t":0.32888, "x":2.05332, "y":7.16804, "heading":-1.34344, "vx":2.9534, "vy":-1.51029, "omega":-1.93118, "ax":8.54666, "ay":-4.19891, "alpha":12.95969, "fx":[174.21763,161.71782,92.49453,153.07491], "fy":[5.02969,-59.1798,-147.61068,-83.9285]}, + {"t":0.36543, "x":2.16695, "y":7.11005, "heading":-1.41401, "vx":3.26571, "vy":-1.66373, "omega":-1.45759, "ax":7.9939, "ay":-3.11314, "alpha":19.51706, "fx":[171.80766,164.04861,59.18033,148.85924], "fy":[25.44372,16.02034,-162.98246,-90.29578]}, + {"t":0.40197, "x":2.29162, "y":7.04717, "heading":-1.46728, "vx":3.55783, "vy":-1.77749, "omega":-0.74439, "ax":7.68511, "ay":-3.3142, "alpha":19.62622, "fx":[170.19763,152.97921,55.75756,143.95177], "fy":[16.96379,12.51447,-160.78762,-94.18498]}, + {"t":0.43851, "x":2.42677, "y":6.98001, "heading":-1.49448, "vx":3.83867, "vy":-1.8986, "omega":-0.02719, "ax":-0.26032, "ay":-1.06338, "alpha":0.58926, "fx":[-3.09909,-5.99283,-5.75232,-2.86789], "fy":[-16.56888,-16.77243,-19.60488,-19.40493]}, + {"t":0.47506, "x":2.56687, "y":6.90992, "heading":-1.49547, "vx":3.82915, "vy":-1.93746, "omega":-0.00566, "ax":-0.09131, "ay":-0.18038, "alpha":0.00033, "fx":[-1.55243,-1.55402,-1.5539,-1.55231], "fy":[-3.06728,-3.0674,-3.06899,-3.06887]}, + {"t":0.5116, "x":2.70674, "y":6.839, "heading":-1.49568, "vx":3.82582, "vy":-1.94405, "omega":-0.00565, "ax":-0.01718, "ay":-0.03379, "alpha":0.0, "fx":[-0.29215,-0.29218,-0.29218,-0.29215], "fy":[-0.57476,-0.57476,-0.57478,-0.57478]}, + {"t":0.54814, "x":2.84653, "y":6.76793, "heading":-1.49589, "vx":3.82519, "vy":-1.94529, "omega":-0.00565, "ax":-0.00322, "ay":-0.00633, "alpha":0.0, "fx":[-0.05479,-0.05478,-0.05478,-0.05479], "fy":[-0.10772,-0.10771,-0.1077,-0.1077]}, + {"t":0.58468, "x":2.98631, "y":6.69684, "heading":-1.49609, "vx":3.82507, "vy":-1.94552, "omega":-0.00565, "ax":-0.0006, "ay":-0.00119, "alpha":0.0, "fx":[-0.01028,-0.01026,-0.01026,-0.01028], "fy":[-0.02019,-0.02019,-0.02017,-0.02017]}, + {"t":0.62123, "x":3.12609, "y":6.62575, "heading":-1.4963, "vx":3.82505, "vy":-1.94556, "omega":-0.00565, "ax":-0.00011, "ay":-0.00022, "alpha":0.0, "fx":[-0.00194,-0.00192,-0.00192,-0.00194], "fy":[-0.00379,-0.00379,-0.00377,-0.00377]}, + {"t":0.65777, "x":3.26587, "y":6.55465, "heading":-1.4965, "vx":3.82505, "vy":-1.94557, "omega":-0.00565, "ax":-0.00002, "ay":-0.00004, "alpha":0.0, "fx":[-0.00039,-0.00037,-0.00037,-0.00039], "fy":[-0.00075,-0.00075,-0.00073,-0.00073]}, + {"t":0.69431, "x":3.40564, "y":6.48356, "heading":-1.49671, "vx":3.82505, "vy":-1.94557, "omega":-0.00565, "ax":-0.00001, "ay":-0.00002, "alpha":0.0, "fx":[-0.00019,-0.00018,-0.00018,-0.0002], "fy":[-0.00037,-0.00036,-0.00035,-0.00035]}, + {"t":0.73085, "x":3.54542, "y":6.41246, "heading":-1.49692, "vx":3.82504, "vy":-1.94557, "omega":-0.00565, "ax":-0.00004, "ay":-0.00007, "alpha":0.0, "fx":[-0.00065,-0.00063,-0.00063,-0.00065], "fy":[-0.00126,-0.00126,-0.00124,-0.00125]}, + {"t":0.7674, "x":3.6852, "y":6.34136, "heading":-1.49712, "vx":3.82504, "vy":-1.94557, "omega":-0.00565, "ax":-0.0002, "ay":-0.00039, "alpha":0.0, "fx":[-0.00337,-0.00336,-0.00336,-0.00338], "fy":[-0.00662,-0.00662,-0.0066,-0.0066]}, + {"t":0.80394, "x":3.82498, "y":6.27027, "heading":-1.49733, "vx":3.82504, "vy":-1.94559, "omega":-0.00565, "ax":-0.00105, "ay":-0.00207, "alpha":0.0, "fx":[-0.01795,-0.01793,-0.01794,-0.01795], "fy":[-0.03528,-0.03528,-0.03526,-0.03526]}, + {"t":0.84048, "x":3.96475, "y":6.19917, "heading":-1.49754, "vx":3.825, "vy":-1.94566, "omega":-0.00565, "ax":-0.00563, "ay":-0.01106, "alpha":0.0, "fx":[-0.09575,-0.09574,-0.09574,-0.09575], "fy":[-0.18821,-0.18821,-0.1882,-0.1882]}, + {"t":0.87702, "x":4.10453, "y":6.12806, "heading":-1.49774, "vx":3.82479, "vy":-1.94607, "omega":-0.00565, "ax":-0.03005, "ay":-0.05902, "alpha":0.00001, "fx":[-0.51115,-0.5112,-0.5112,-0.51114], "fy":[-1.00397,-1.00397,-1.00402,-1.00402]}, + {"t":0.91357, "x":4.24427, "y":6.05691, "heading":-1.49795, "vx":3.82369, "vy":-1.94823, "omega":-0.00565, "ax":-0.16072, "ay":-0.31427, "alpha":0.00006, "fx":[-2.73369,-2.73399,-2.73397,-2.73367], "fy":[-5.34555,-5.34557,-5.34587,-5.34585]}, + {"t":0.95011, "x":4.38389, "y":5.9855, "heading":-1.49816, "vx":3.81782, "vy":-1.95971, "omega":-0.00565, "ax":-0.84617, "ay":-1.61718, "alpha":0.00013, "fx":[-14.39279,-14.39342,-14.39336,-14.39273], "fy":[-27.50743,-27.50746,-27.50807,-27.50804]}, + {"t":0.98665, "x":4.52284, "y":5.91281, "heading":-1.49836, "vx":3.7869, "vy":-2.01881, "omega":-0.00564, "ax":-3.08306, "ay":-5.42775, "alpha":-0.00163, "fx":[-52.44507,-52.43582,-52.439,-52.44825], "fy":[-92.32681,-92.32884,-92.3222,-92.32018]}, + {"t":1.0232, "x":4.65917, "y":5.83541, "heading":-1.49857, "vx":3.67424, "vy":-2.21715, "omega":-0.0057, "ax":-5.0948, "ay":-7.49912, "alpha":-0.31979, "fx":[-86.98865,-85.07369,-86.3336,-88.24822], "fy":[-127.64104,-128.66698,-127.48389,-126.43977]}, + {"t":1.05974, "x":4.79003, "y":5.74939, "heading":-1.49878, "vx":3.48806, "vy":-2.49119, "omega":-0.01739, "ax":-9.07212, "ay":-0.81109, "alpha":-15.5178, "fx":[-161.1027,-135.83391,-151.66566,-168.65428], "fy":[-58.64473,-98.49972,71.30528,30.65339]}, + {"t":1.09628, "x":4.91144, "y":5.65781, "heading":-1.49941, "vx":3.15654, "vy":-2.52083, "omega":-0.58445, "ax":-8.98008, "ay":1.18607, "alpha":-17.55451, "fx":[-170.02472,-157.35042,-121.0452,-162.57371], "fy":[-36.18374,-68.32165,123.27189,61.93253]}, + {"t":1.13282, "x":5.02079, "y":5.56648, "heading":-1.52077, "vx":2.82838, "vy":-2.47749, "omega":-1.22594, "ax":-8.98859, "ay":1.66195, "alpha":-17.23725, "fx":[-172.0886,-163.38325,-115.69643,-160.40533], "fy":[-28.67138,-56.86951,129.76556,68.8524]}, + {"t":1.16937, "x":5.11815, "y":5.47706, "heading":-1.56557, "vx":2.49991, "vy":-2.41675, "omega":-1.85583, "ax":-9.6713, "ay":2.92175, "alpha":-6.38693, "fx":[-173.77678,-172.40883,-150.10955,-161.72892], "fy":[17.78566,25.87885,88.96024,66.16748]}, + {"t":1.20591, "x":5.20304, "y":5.39069, "heading":-1.63339, "vx":2.1465, "vy":-2.30999, "omega":-2.08923, "ax":-9.67262, "ay":3.47425, "alpha":-0.49944, "fx":[-165.53166,-165.09465,-163.48966,-163.99835], "fy":[56.29633,57.51293,61.94278,60.63176]}, + {"t":1.24245, "x":5.27502, "y":5.3086, "heading":-1.70973, "vx":1.79304, "vy":-2.18303, "omega":-2.10748, "ax":-9.55272, "ay":3.73705, "alpha":2.72586, "fx":[-156.09236,-159.52479,-167.83622,-166.50281], "fy":[79.01985,72.00596,49.58983,53.64922]}, + {"t":1.27899, "x":5.33417, "y":5.23132, "heading":-1.78674, "vx":1.44395, "vy":-2.04647, "omega":-2.00787, "ax":-9.21452, "ay":4.39156, "alpha":3.82092, "fx":[-145.93061,-152.10216,-165.33218,-163.58013], "fy":[95.75739,85.95035,56.36626,60.7225]}, + {"t":1.29744, "x":5.35924, "y":5.19432, "heading":-1.82378, "vx":1.27397, "vy":-1.96545, "omega":-1.93738, "ax":-8.60346, "ay":5.41894, "alpha":5.01762, "fx":[-127.71939,-140.35258,-160.58456,-156.71292], "fy":[119.01672,104.13886,68.8464,76.69657]}, + {"t":1.31589, "x":5.38127, "y":5.15898, "heading":-1.85952, "vx":1.11526, "vy":-1.86549, "omega":-1.84482, "ax":-7.94456, "ay":6.27877, "alpha":6.02308, "fx":[-108.16438,-128.45724,-155.42214,-148.49527], "fy":[137.09592,118.58816,79.91745,91.59887]}, + {"t":1.33434, "x":5.4005, "y":5.12564, "heading":-1.89356, "vx":0.9687, "vy":-1.74966, "omega":-1.73371, "ax":-7.28216, "ay":6.97996, "alpha":6.82867, "fx":[-89.0061,-117.05964,-150.08687,-139.3173], "fy":[150.30976,129.9256,89.62204,105.05109]}, + {"t":1.35278, "x":5.41713, "y":5.09455, "heading":-1.92554, "vx":0.83437, "vy":-1.6209, "omega":-1.60774, "ax":-6.64554, "ay":7.54333, "alpha":7.455, "fx":[-71.29923,-106.48739,-144.76457,-129.6038], "fy":[159.53767,138.78556,98.0616,116.8548]}, + {"t":1.37123, "x":5.43139, "y":5.06593, "heading":-1.9552, "vx":0.71178, "vy":-1.48175, "omega":-1.47022, "ax":-6.05076, "ay":7.99284, "alpha":7.93539, "fx":[-55.49331,-96.86658,-139.58693,-119.74019], "fy":[165.75942,145.71666,105.36669,126.9808]}, + {"t":1.38968, "x":5.44349, "y":5.03996, "heading":-1.98232, "vx":0.60016, "vy":-1.3343, "omega":-1.32383, "ax":-5.50463, "ay":8.35085, "alpha":8.30332, "fx":[-41.652,-88.20986,-134.64016,-110.02652], "fy":[169.82082,151.15952,111.6757,135.52617]}, + {"t":1.40813, "x":5.45362, "y":5.01676, "heading":-2.00674, "vx":0.49861, "vy":-1.18025, "omega":-1.17066, "ax":-5.00825, "ay":8.63639, "alpha":8.58684, "fx":[-29.6437,-80.47305,-129.97559,-100.66307], "fy":[172.37115,155.45534,117.1218,142.66203]}, + {"t":1.42657, "x":5.46197, "y":4.99646, "heading":-2.02833, "vx":0.40622, "vy":-1.02093, "omega":-1.01225, "ax":-4.55957, "ay":8.86492, "alpha":8.8075, "fx":[-19.26176,-73.5879,-125.61946,-91.75876], "fy":[173.88039,158.86401,121.82621,148.58867]}, + {"t":1.44502, "x":5.46869, "y":4.97914, "heading":-2.04701, "vx":0.32211, "vy":-0.8574, "omega":-0.84978, "ax":-4.15496, "ay":9.04868, "alpha":8.98128, "fx":[-10.28761,-67.47968,-121.58091,-83.35063], "fy":[174.67922,161.58245,125.8955,153.50472]}, + {"t":1.46347, "x":5.47392, "y":4.96486, "heading":-2.06268, "vx":0.24546, "vy":-0.69047, "omega":-0.6841, "ax":-3.79016, "ay":9.19723, "alpha":9.12, "fx":[-2.51908,-62.07597,-117.85795,-75.42541], "fy":[174.99763,163.76005,129.4214,157.58975]}, + {"t":1.48191, "x":5.4778, "y":4.95369, "heading":-2.0753, "vx":0.17554, "vy":-0.52081, "omega":-0.51586, "ax":-3.46083, "ay":9.31798, "alpha":9.23254, "fx":[4.21963,-57.31052,-114.44163,-67.93826], "fy":[174.99523,165.51057,132.48193,160.99677]}, + {"t":1.50036, "x":5.48045, "y":4.94566, "heading":-2.08482, "vx":0.1117, "vy":-0.34892, "omega":-0.34554, "ax":-3.16281, "ay":9.41665, "alpha":9.32583, "fx":[10.07669,-53.12453,-111.31891,-60.8271], "fy":[174.78315,166.92126,135.14299,163.85094]}, + {"t":1.51881, "x":5.48198, "y":4.94083, "heading":-2.09119, "vx":0.05336, "vy":-0.17521, "omega":-0.17351, "ax":-2.8923, "ay":9.49766, "alpha":9.40553, "fx":[15.17487,-49.46671,-108.47449,-54.0227], "fy":[174.43924,168.05939,137.46013,166.25138]}, + {"t":1.53726, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/fetchToJ.traj b/src/main/deploy/choreo/fetchToJ.traj new file mode 100644 index 00000000..afedb9c2 --- /dev/null +++ b/src/main/deploy/choreo/fetchToJ.traj @@ -0,0 +1,84 @@ +{ + "name":"fetchToJ", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.066040992736816, "y":5.357499599456787, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "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":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.066040992736816 m", "val":5.066040992736816}, "y":{"exp":"5.357499599456787 m", "val":5.357499599456787}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "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.21683,1.45822], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.76941, "ay":-4.58984, "alpha":-10.09461, "fx":[122.23365,162.49911,174.85969,137.06798], "fy":[-125.65353,-65.82443,-11.778,-109.03122]}, + {"t":0.03579, "x":1.57703, "y":7.41248, "heading":-0.94268, "vx":0.31385, "vy":-0.16427, "omega":-0.36128, "ax":8.80402, "ay":-4.59424, "alpha":-9.66251, "fx":[123.66873,162.29944,174.60053,138.44698], "fy":[-124.21319,-66.26837,-14.87282,-107.23271]}, + {"t":0.07158, "x":1.5939, "y":7.40366, "heading":-0.95561, "vx":0.62894, "vy":-0.32869, "omega":-0.70709, "ax":8.84633, "ay":-4.6006, "alpha":-9.09182, "fx":[125.18444,161.81681,174.19665,140.69609], "fy":[-122.64874,-67.3805,-18.77822,-104.21205]}, + {"t":0.10737, "x":1.62208, "y":7.38895, "heading":-0.98091, "vx":0.94554, "vy":-0.49334, "omega":-1.03248, "ax":8.89554, "ay":-4.60925, "alpha":-8.36872, "fx":[127.01995,161.06108,173.56143,143.60022], "fy":[-120.6978,-69.0944,-23.70789,-100.10767]}, + {"t":0.14316, "x":1.66161, "y":7.36834, "heading":-1.01786, "vx":1.2639, "vy":-0.6583, "omega":-1.33199, "ax":8.95083, "ay":-4.62111, "alpha":-7.45943, "fx":[129.54098,160.04752,172.53859,146.87716], "fy":[-117.92043,-71.31502,-30.02541,-95.15425]}, + {"t":0.17895, "x":1.71258, "y":7.34182, "heading":-1.06553, "vx":1.58425, "vy":-0.82369, "omega":-1.59896, "ax":9.01122, "ay":-4.63797, "alpha":-6.28264, "fx":[133.31252,158.80821,170.82446,150.16785], "fy":[-113.54157,-73.90133,-38.34858,-89.77077]}, + {"t":0.21473, "x":1.77505, "y":7.30937, "heading":-1.12276, "vx":1.90675, "vy":-0.98968, "omega":-1.82381, "ax":9.07469, "ay":-4.66117, "alpha":-4.64886, "fx":[139.20619,157.41784,167.76401,153.0436], "fy":[-106.08208,-76.6201,-49.78931,-84.64888]}, + {"t":0.25052, "x":1.8491, "y":7.27097, "heading":-1.18803, "vx":2.23153, "vy":-1.1565, "omega":-1.99019, "ax":9.13095, "ay":-4.68534, "alpha":-2.12509, "fx":[148.45708,156.06649,161.72084,155.01476], "fy":[-92.43101,-78.99699,-66.53476,-80.82241]}, + {"t":0.28631, "x":1.93481, "y":7.22658, "heading":-1.25926, "vx":2.55832, "vy":-1.32418, "omega":-2.06624, "ax":9.1192, "ay":-4.67484, "alpha":2.27048, "fx":[161.91076,155.35542,147.70939,155.48444], "fy":[-65.51047,-79.63802,-93.21289,-79.70965]}, + {"t":0.3221, "x":2.03221, "y":7.17619, "heading":-1.33321, "vx":2.88468, "vy":-1.49149, "omega":-1.98498, "ax":8.72388, "ay":-4.42214, "alpha":10.54327, "fx":[173.99515,158.68856,107.33343,153.546], "fy":[-11.09336,-69.3562,-137.31095,-83.11683]}, + {"t":0.35789, "x":2.14104, "y":7.11998, "heading":-1.40425, "vx":3.19691, "vy":-1.64975, "omega":-1.60765, "ax":8.02096, "ay":-3.12321, "alpha":19.38589, "fx":[171.84343,165.09634,59.41183,149.3851], "fy":[26.57224,13.83431,-163.16836,-89.73791]}, + {"t":0.39368, "x":2.26059, "y":7.05894, "heading":-1.46179, "vx":3.48397, "vy":-1.76153, "omega":-0.91384, "ax":7.82208, "ay":-3.23064, "alpha":19.71071, "fx":[171.27677,158.43048,57.05558,145.44267], "fy":[18.69129,17.07614,-161.90091,-93.67589]}, + {"t":0.42947, "x":2.39029, "y":6.99382, "heading":-1.49449, "vx":3.76391, "vy":-1.87715, "omega":-0.20841, "ax":1.59335, "ay":-2.12546, "alpha":5.65719, "fx":[41.39022,12.36603,13.70895,40.94461], "fy":[-21.0421,-24.44894,-51.62072,-47.50208]}, + {"t":0.46526, "x":2.52602, "y":6.92528, "heading":-1.50195, "vx":3.82094, "vy":-1.95322, "omega":-0.00595, "ax":-0.11321, "ay":-0.22419, "alpha":0.00341, "fx":[-1.91789,-1.93448,-1.93332,-1.91672], "fy":[-3.80459,-3.80573,-3.82231,-3.82117]}, + {"t":0.50105, "x":2.66269, "y":6.85523, "heading":-1.50216, "vx":3.81689, "vy":-1.96125, "omega":-0.00582, "ax":-0.02248, "ay":-0.04373, "alpha":0.00001, "fx":[-0.38236,-0.38241,-0.3824,-0.38236], "fy":[-0.74385,-0.74386,-0.7439,-0.7439]}, + {"t":0.53684, "x":2.79928, "y":6.78501, "heading":-1.50237, "vx":3.81608, "vy":-1.96281, "omega":-0.00582, "ax":-0.0044, "ay":-0.00856, "alpha":0.0, "fx":[-0.07487,-0.07486,-0.07486,-0.07487], "fy":[-0.14554,-0.14554,-0.14553,-0.14553]}, + {"t":0.57263, "x":2.93586, "y":6.71476, "heading":-1.50258, "vx":3.81593, "vy":-1.96312, "omega":-0.00582, "ax":-0.00086, "ay":-0.00167, "alpha":0.0, "fx":[-0.01467,-0.01465,-0.01465,-0.01467], "fy":[-0.0285,-0.0285,-0.02848,-0.02848]}, + {"t":0.60842, "x":3.07242, "y":6.6445, "heading":-1.50279, "vx":3.81589, "vy":-1.96318, "omega":-0.00582, "ax":-0.00017, "ay":-0.00033, "alpha":0.0, "fx":[-0.00293,-0.00291,-0.00291,-0.00293], "fy":[-0.00568,-0.00567,-0.00565,-0.00566]}, + {"t":0.6442, "x":3.20899, "y":6.57424, "heading":-1.503, "vx":3.81589, "vy":-1.96319, "omega":-0.00582, "ax":-0.00005, "ay":-0.00009, "alpha":0.0, "fx":[-0.00084,-0.00082,-0.00082,-0.00084], "fy":[-0.00162,-0.00162,-0.0016,-0.0016]}, + {"t":0.67999, "x":3.34556, "y":6.50398, "heading":-1.50321, "vx":3.81589, "vy":-1.96319, "omega":-0.00582, "ax":-0.00009, "ay":-0.00017, "alpha":0.0, "fx":[-0.00151,-0.00149,-0.00149,-0.00151], "fy":[-0.00291,-0.00291,-0.00289,-0.00289]}, + {"t":0.71578, "x":3.48213, "y":6.43372, "heading":-1.50341, "vx":3.81588, "vy":-1.9632, "omega":-0.00582, "ax":-0.00042, "ay":-0.00081, "alpha":0.0, "fx":[-0.00712,-0.0071,-0.0071,-0.00712], "fy":[-0.01383,-0.01382,-0.0138,-0.01381]}, + {"t":0.75157, "x":3.61869, "y":6.36346, "heading":-1.50362, "vx":3.81587, "vy":-1.96323, "omega":-0.00582, "ax":-0.00213, "ay":-0.00414, "alpha":0.0, "fx":[-0.03625,-0.03624,-0.03624,-0.03625], "fy":[-0.07045,-0.07045,-0.07043,-0.07043]}, + {"t":0.78736, "x":3.75526, "y":6.29319, "heading":-1.50383, "vx":3.81579, "vy":-1.96338, "omega":-0.00583, "ax":-0.01089, "ay":-0.02116, "alpha":0.0, "fx":[-0.18525,-0.18525,-0.18525,-0.18525], "fy":[-0.35995,-0.35995,-0.35996,-0.35996]}, + {"t":0.82315, "x":3.89181, "y":6.22291, "heading":-1.50404, "vx":3.8154, "vy":-1.96413, "omega":-0.00582, "ax":-0.05572, "ay":-0.1081, "alpha":0.00002, "fx":[-0.94766,-0.94777,-0.94776,-0.94765], "fy":[-1.83868,-1.83869,-1.8388,-1.83879]}, + {"t":0.85894, "x":4.02833, "y":6.15255, "heading":-1.50425, "vx":3.81341, "vy":-1.968, "omega":-0.00582, "ax":-0.28544, "ay":-0.54959, "alpha":0.0001, "fx":[-4.85497,-4.85545,-4.85542,-4.85493], "fy":[-9.34811,-9.34814,-9.34863,-9.3486]}, + {"t":0.89473, "x":4.16463, "y":6.08176, "heading":-1.50446, "vx":3.80319, "vy":-1.98767, "omega":-0.00582, "ax":-1.38218, "ay":-2.56754, "alpha":0.00004, "fx":[-23.51034,-23.51055,-23.51053,-23.51031], "fy":[-43.67299,-43.67299,-43.6732,-43.6732]}, + {"t":0.93052, "x":4.29985, "y":6.00898, "heading":-1.50466, "vx":3.75373, "vy":-2.07956, "omega":-0.00582, "ax":-3.80608, "ay":-6.36785, "alpha":-0.03172, "fx":[-64.79243,-64.60521,-64.68809,-64.87538], "fy":[-108.34459,-108.40704,-108.28604,-108.2235]}, + {"t":0.96631, "x":4.43176, "y":5.93048, "heading":-1.50487, "vx":3.61751, "vy":-2.30746, "omega":-0.00695, "ax":-7.399, "ay":-5.49768, "alpha":-6.83211, "fx":[-126.30463,-97.27831,-130.93232,-148.90394], "fy":[-103.22182,-126.60447,-82.49768,-61.73219]}, + {"t":1.0021, "x":4.55649, "y":5.84437, "heading":-1.50512, "vx":3.35271, "vy":-2.50422, "omega":-0.25147, "ax":-8.88543, "ay":1.18344, "alpha":-17.7753, "fx":[-169.21962,-154.79315,-118.94526,-161.59679], "fy":[-36.01814,-69.22156,123.39241,62.36742]}, + {"t":1.03789, "x":4.67079, "y":5.75551, "heading":-1.51412, "vx":3.0347, "vy":-2.46186, "omega":-0.88763, "ax":-8.83529, "ay":1.76617, "alpha":-18.04183, "fx":[-171.77682,-161.99813,-108.42867,-158.93934], "fy":[-28.7121,-57.99339,135.32516,71.54849]}, + {"t":1.07367, "x":4.77374, "y":5.66853, "heading":-1.54589, "vx":2.7185, "vy":-2.39865, "omega":-1.53333, "ax":-9.2919, "ay":2.55238, "alpha":-12.63255, "fx":[-174.37308,-173.20416,-125.96618,-158.66684], "fy":[-7.94108,-11.62732,120.24525,72.98432]}, + {"t":1.10946, "x":4.86508, "y":5.58432, "heading":-1.60077, "vx":2.38595, "vy":-2.30731, "omega":-1.98544, "ax":-9.62388, "ay":3.4612, "alpha":-3.28526, "fx":[-169.82865,-167.80283,-156.01389,-161.15212], "fy":[41.13522,48.2165,78.49977,67.64458]}, + {"t":1.14525, "x":4.94431, "y":5.50396, "heading":-1.67182, "vx":2.04152, "vy":-2.18343, "omega":-2.10302, "ax":-9.55162, "ay":3.79349, "alpha":1.0669, "fx":[-160.01059,-161.2611,-164.7602,-163.84932], "fy":[70.56647,67.74396,58.6977,61.09669]}, + {"t":1.18104, "x":5.01125, "y":5.42825, "heading":-1.74709, "vx":1.69967, "vy":-2.04767, "omega":-2.06484, "ax":-9.43685, "ay":3.96234, "alpha":3.69388, "fx":[-151.12953,-156.40125,-167.96179,-166.57961], "fy":[88.18524,78.64782,49.29761,53.46257]}, + {"t":1.21683, "x":5.06604, "y":5.3575, "heading":-1.82099, "vx":1.36194, "vy":-1.90586, "omega":-1.93264, "ax":-9.10076, "ay":4.57055, "alpha":4.64621, "fx":[-140.94411,-149.04647,-165.38834,-163.8261], "fy":[103.05245,91.28888,56.43298,60.20071]}, + {"t":1.2354, "x":5.08976, "y":5.3229, "heading":-1.85687, "vx":1.19295, "vy":-1.82099, "omega":-1.84636, "ax":-8.47812, "ay":5.56374, "alpha":5.74815, "fx":[-122.10371,-137.19911,-160.57211,-156.9665], "fy":[124.85043,108.36468,69.04571,76.28998]}, + {"t":1.25397, "x":5.11045, "y":5.29004, "heading":-1.89116, "vx":1.03552, "vy":-1.71768, "omega":-1.73963, "ax":-7.81288, "ay":6.39766, "alpha":6.656, "fx":[-102.31961,-125.29622,-155.2904,-148.6735], "fy":[141.57834,122.00428,80.30423,91.40264]}, + {"t":1.27254, "x":5.12833, "y":5.25925, "heading":-1.92346, "vx":0.89045, "vy":-1.59889, "omega":-1.61604, "ax":-7.14613, "ay":7.08082, "alpha":7.36681, "fx":[-83.1943,-113.91619,-149.79022,-139.31407], "fy":[153.66035,132.75504,90.22123,105.13453]}, + {"t":1.29111, "x":5.14363, "y":5.23078, "heading":-1.95347, "vx":0.75776, "vy":-1.46741, "omega":-1.47925, "ax":-6.50507, "ay":7.63241, "alpha":7.90511, "fx":[-65.66081,-103.36271,-144.26803,-129.30564], "fy":[161.99022,141.1812,98.87567,117.25355]}, + {"t":1.30967, "x":5.15658, "y":5.20485, "heading":-1.98093, "vx":0.63697, "vy":-1.32568, "omega":-1.33246, "ax":-5.90481, "ay":8.07448, "alpha":8.30615, "fx":[-50.0898,-93.75778,-138.8678,-119.04095], "fy":[167.51628,147.78064,106.38338,127.69786]}, + {"t":1.32824, "x":5.16739, "y":5.18163, "heading":-2.00568, "vx":0.52732, "vy":-1.17575, "omega":-1.17823, "ax":-5.35199, "ay":8.42777, "alpha":8.60395, "fx":[-36.50228,-85.11712,-133.68713,-108.83704], "fy":[171.042,152.96045,112.87458,136.53882]}, + {"t":1.34681, "x":5.17626, "y":5.16125, "heading":-2.02755, "vx":0.42794, "vy":-1.01926, "omega":-1.01846, "ax":-4.84795, "ay":8.71016, "alpha":8.82636, "fx":[-24.74651,-77.40084,-128.78666,-98.91503], "fy":[173.17873,157.04064,118.47908,143.93069]}, + {"t":1.36538, "x":5.18337, "y":5.14383, "heading":-2.04647, "vx":0.33793, "vy":-0.85753, "omega":-0.85457, "ax":-4.39095, "ay":8.93633, "alpha":8.99438, "fx":[-14.6076,-70.54406,-124.19951,-89.40428], "fy":[174.3668,160.26793,123.31786,150.06478]}, + {"t":1.38395, "x":5.18889, "y":5.12944, "heading":-2.06233, "vx":0.25639, "vy":-0.69159, "omega":-0.68756, "ax":-3.97769, "ay":9.11805, "alpha":9.12333, "fx":[-5.86445,-64.47408,-119.93938,-80.35958], "fy":[174.91486,162.83097,127.49917,155.13635]}, + {"t":1.40252, "x":5.19297, "y":5.11817, "heading":-2.0751, "vx":0.18253, "vy":-0.52229, "omega":-0.51815, "ax":-3.60416, "ay":9.2646, "alpha":9.22441, "fx":[1.68511,-59.1193,-116.00693,-71.78205], "fy":[175.03685,164.87379,131.1174,159.32495]}, + {"t":1.42108, "x":5.19573, "y":5.11007, "heading":-2.08472, "vx":0.11561, "vy":-0.35026, "omega":-0.34687, "ax":-3.26622, "ay":9.38326, "alpha":9.306, "fx":[8.21586,-54.41345,-112.39434,-63.63815], "fy":[174.88062,166.50662,134.25364,162.78522]}, + {"t":1.43965, "x":5.19732, "y":5.10519, "heading":-2.09116, "vx":0.05496, "vy":-0.17602, "omega":-0.17407, "ax":-2.95987, "ay":9.47964, "alpha":9.37463, "fx":[13.87455,-50.29716,-109.08855,-55.87476], "fy":[174.54865,167.81416,136.97695,165.64443]}, + {"t":1.45822, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj new file mode 100644 index 00000000..936fb870 --- /dev/null +++ b/src/main/deploy/choreo/fetchToK.traj @@ -0,0 +1,62 @@ +{ + "name":"fetchToK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":27, "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":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":27, "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.19692], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.08578, "ay":-6.38853, "alpha":-0.62757, "fx":[135.22249,138.83326,139.88579,136.20595], "fy":[-111.57882,-107.05515,-105.66702,-110.36719]}, + {"t":0.04433, "x":1.57936, "y":7.40914, "heading":-0.94268, "vx":0.35845, "vy":-0.28321, "omega":-0.02782, "ax":8.08485, "ay":-6.38779, "alpha":-0.62158, "fx":[135.22889,138.80527,139.84682,136.20267], "fy":[-111.53869,-107.05798,-105.68367,-110.33748]}, + {"t":0.08866, "x":1.60319, "y":7.39031, "heading":-0.94391, "vx":0.71685, "vy":-0.56638, "omega":-0.05538, "ax":8.08364, "ay":-6.38684, "alpha":-0.6139, "fx":[135.23535,138.76658,139.79828,136.20129], "fy":[-111.48918,-107.0651,-105.70297,-110.29578]}, + {"t":0.13299, "x":1.64291, "y":7.35893, "heading":-0.94636, "vx":1.0752, "vy":-0.84951, "omega":-0.08259, "ax":8.08203, "ay":-6.38557, "alpha":-0.6037, "fx":[135.2428,138.71335,139.73461,136.2011], "fy":[-111.4245,-107.07664,-105.7273,-110.2381]}, + {"t":0.17732, "x":1.69852, "y":7.315, "heading":-0.95003, "vx":1.43348, "vy":-1.13258, "omega":-0.10935, "ax":8.07977, "ay":-6.38378, "alpha":-0.58948, "fx":[135.25282,138.63869,139.64581,136.20083], "fy":[-111.33429,-107.09283,-105.76084,-110.15735]}, + {"t":0.22165, "x":1.77, "y":7.25852, "heading":-0.95487, "vx":1.79166, "vy":-1.41558, "omega":-0.13548, "ax":8.07637, "ay":-6.38111, "alpha":-0.56832, "fx":[135.26839,138.52885,139.5121,136.19783], "fy":[-111.19803,-107.11402,-105.81169,-110.03939]}, + {"t":0.26598, "x":1.85737, "y":7.18949, "heading":-0.96088, "vx":2.14968, "vy":-1.69845, "omega":-0.16068, "ax":8.0707, "ay":-6.37663, "alpha":-0.53351, "fx":[135.29572,138.35187,139.28798,136.18564], "fy":[-110.96852,-107.14072,-105.89777,-109.85169]}, + {"t":0.31031, "x":1.96059, "y":7.10793, "heading":-0.968, "vx":2.50746, "vy":-1.98113, "omega":-0.18433, "ax":8.05931, "ay":-6.36765, "alpha":-0.46572, "fx":[135.34983,138.01286,138.84027,136.14319], "fy":[-110.50779,-107.17241,-106.06816,-109.49901]}, + {"t":0.35464, "x":2.07967, "y":7.01385, "heading":-0.97617, "vx":2.86473, "vy":-2.26341, "omega":-0.20497, "ax":8.0249, "ay":-6.3405, "alpha":-0.27781, "fx":[135.46577,137.05066,137.54244,135.94591], "fy":[-109.16822,-107.1829,-106.51534,-108.53415]}, + {"t":0.39897, "x":2.21455, "y":6.90729, "heading":-0.98526, "vx":3.22048, "vy":-2.54449, "omega":-0.21729, "ax":3.3354, "ay":-2.6351, "alpha":4.89611, "fx":[63.22878,40.81151,51.3561,71.54043], "fy":[-26.76748,-44.45733,-62.15853,-45.9056]}, + {"t":0.4433, "x":2.36059, "y":6.7919, "heading":-0.99489, "vx":3.36834, "vy":-2.6613, "omega":-0.00024, "ax":0.00064, "ay":-0.00043, "alpha":0.00053, "fx":[0.01131,0.00915,0.01055,0.01272], "fy":[-0.00545,-0.00686,-0.00903,-0.00762]}, + {"t":0.48763, "x":2.50991, "y":6.67392, "heading":-0.9949, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.00009,0.00008,0.00008,0.00009], "fy":[0.0001,0.0001,0.0001,0.0001]}, + {"t":0.53196, "x":2.65923, "y":6.55594, "heading":-0.99491, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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":0.57629, "x":2.80855, "y":6.43797, "heading":-0.99492, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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.62062, "x":2.95787, "y":6.31999, "heading":-0.99493, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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":0.66495, "x":3.10719, "y":6.20201, "heading":-0.99494, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":-0.00001, "ay":-0.00001, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, + {"t":0.70928, "x":3.25651, "y":6.08404, "heading":-0.99495, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":-0.00064, "ay":0.00042, "alpha":-0.00053, "fx":[-0.01132,-0.00915,-0.01056,-0.01273], "fy":[0.00544,0.00685,0.00901,0.00761]}, + {"t":0.75361, "x":3.40583, "y":5.96606, "heading":-0.99496, "vx":3.36834, "vy":-2.6613, "omega":-0.00024, "ax":-3.3354, "ay":2.6351, "alpha":-4.89603, "fx":[-63.37516,-40.87032,-51.20753,-71.48407], "fy":[26.78713,44.27431,62.16009,46.06744]}, + {"t":0.79795, "x":3.55187, "y":5.85067, "heading":-0.99497, "vx":3.22048, "vy":-2.54449, "omega":-0.21729, "ax":-8.0249, "ay":6.3405, "alpha":0.27714, "fx":[-135.45782,-137.03004,-137.54985,-135.96708], "fy":[109.17761,107.20941,106.50624,108.50743]}, + {"t":0.84228, "x":3.68675, "y":5.7441, "heading":-1.00461, "vx":2.86473, "vy":-2.26341, "omega":-0.205, "ax":-8.05931, "ay":6.36765, "alpha":0.46556, "fx":[-135.31595,-137.9495,-138.87125,-136.20945], "fy":[110.54858,107.25429,106.02835,109.41619]}, + {"t":0.88661, "x":3.80583, "y":5.65002, "heading":-1.01369, "vx":2.50746, "vy":-1.98113, "omega":-0.18436, "ax":-8.0707, "ay":6.37663, "alpha":0.5335, "fx":[-135.23855,-138.2477,-139.33963,-136.29531], "fy":[111.03743,107.27548,105.83063,109.71517]}, + {"t":0.93094, "x":3.90906, "y":5.56846, "heading":-1.02187, "vx":2.14968, "vy":-1.69845, "omega":-0.16071, "ax":-8.07637, "ay":6.38111, "alpha":0.56839, "fx":[-135.19083,-138.38824,-139.5817,-136.34637], "fy":[111.29155,107.29601,105.72071,109.85486]}, + {"t":0.97527, "x":3.99642, "y":5.49944, "heading":-1.02899, "vx":1.79166, "vy":-1.41558, "omega":-0.13551, "ax":-8.07977, "ay":6.38378, "alpha":0.58959, "fx":[-135.15797,-138.4668,-139.73055,-136.38279], "fy":[111.44868,107.31536,105.64969,109.93157]}, + {"t":1.0196, "x":4.0679, "y":5.44296, "heading":-1.035, "vx":1.43348, "vy":-1.13258, "omega":-0.10938, "ax":-8.08203, "ay":6.38556, "alpha":0.60383, "fx":[-135.13387,-138.51571,-139.83162,-136.41061], "fy":[111.55588,107.33254,105.59976,109.97835]}, + {"t":1.06393, "x":4.12351, "y":5.39903, "heading":-1.03985, "vx":1.0752, "vy":-0.84951, "omega":-0.08261, "ax":-8.08364, "ay":6.38684, "alpha":0.61405, "fx":[-135.11562,-138.54897,-139.90466,-136.4322], "fy":[111.63359,107.34689,105.56289,110.00964]}, + {"t":1.10826, "x":4.16323, "y":5.36764, "heading":-1.04351, "vx":0.71685, "vy":-0.56638, "omega":-0.05539, "ax":-8.08484, "ay":6.38779, "alpha":0.62174, "fx":[-135.10168,-138.5736,-139.95962,-136.44869], "fy":[111.69212,107.358,105.53494,110.03274]}, + {"t":1.15259, "x":4.18706, "y":5.34881, "heading":-1.04596, "vx":0.35845, "vy":-0.28321, "omega":-0.02783, "ax":-8.08578, "ay":6.38853, "alpha":0.62774, "fx":[-135.09115,-138.59355,-140.00208,-136.46065], "fy":[111.73724,107.36558,105.51355,110.05179]}, + {"t":1.19692, "x":4.19501, "y":5.34254, "heading":-1.0472, "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/fetchToL.traj b/src/main/deploy/choreo/fetchToL.traj new file mode 100644 index 00000000..1f7afbd9 --- /dev/null +++ b/src/main/deploy/choreo/fetchToL.traj @@ -0,0 +1,62 @@ +{ + "name":"fetchToL", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "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":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "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.17196], + "samples":[ + {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.44665, "ay":-7.12277, "alpha":-0.64117, "fx":[124.19086,128.37629,129.19751,124.89671], "fy":[-123.73426,-119.38771,-118.48977,-123.01315]}, + {"t":0.04341, "x":1.57843, "y":7.40871, "heading":-0.94268, "vx":0.32323, "vy":-0.30917, "omega":-0.02783, "ax":7.44576, "ay":-7.12191, "alpha":-0.63703, "fx":[124.19199,128.35036,129.16529,124.89306], "fy":[-123.70329,-119.38485,-118.4929,-122.98583]}, + {"t":0.08681, "x":1.59947, "y":7.38858, "heading":-0.94388, "vx":0.64642, "vy":-0.6183, "omega":-0.05548, "ax":7.44461, "ay":-7.12082, "alpha":-0.63173, "fx":[124.1912,128.31417,129.12598,124.89135], "fy":[-123.6657,-119.38422,-118.49463,-122.94769]}, + {"t":0.13022, "x":1.63454, "y":7.35504, "heading":-0.94629, "vx":0.96956, "vy":-0.92739, "omega":-0.0829, "ax":7.44308, "ay":-7.11935, "alpha":-0.6247, "fx":[124.18864,128.2642,129.07499,124.89086], "fy":[-123.61708,-119.38523,-118.4954,-122.89503]}, + {"t":0.17362, "x":1.68364, "y":7.30808, "heading":-0.94989, "vx":1.29263, "vy":-1.23641, "omega":-0.11002, "ax":7.44094, "ay":-7.11731, "alpha":-0.61495, "fx":[124.18451,128.19411,129.00417,124.89028], "fy":[-123.54953,-119.38676,-118.4959,-122.82125]}, + {"t":0.21703, "x":1.74676, "y":7.2477, "heading":-0.95467, "vx":1.61561, "vy":-1.54534, "omega":-0.13671, "ax":7.43773, "ay":-7.11424, "alpha":-0.6005, "fx":[124.17904,128.09131,128.89744,124.88687], "fy":[-123.44744,-119.38649,-118.49732,-122.71324]}, + {"t":0.26043, "x":1.82389, "y":7.17392, "heading":-0.9606, "vx":1.93845, "vy":-1.85414, "omega":-0.16278, "ax":7.43238, "ay":-7.10912, "alpha":-0.57692, "fx":[124.17209,127.92678,128.71799,124.87391], "fy":[-123.27508,-119.37865,-118.50166,-122.54098]}, + {"t":0.30384, "x":1.91503, "y":7.08675, "heading":-0.96767, "vx":2.26106, "vy":-2.16272, "omega":-0.18782, "ax":7.4217, "ay":-7.0989, "alpha":-0.53164, "fx":[124.16017,127.61532,128.35907,124.82945], "fy":[-122.92848,-119.34414,-118.51156,-122.21692]}, + {"t":0.34725, "x":2.02017, "y":6.98619, "heading":-0.97582, "vx":2.5832, "vy":-2.47085, "omega":-0.21089, "ax":7.38987, "ay":-7.06845, "alpha":-0.40941, "fx":[124.09628,126.75098,127.32433,124.6266], "fy":[-121.92144,-119.16946,-118.50759,-121.33066]}, + {"t":0.39065, "x":2.13925, "y":6.87228, "heading":-0.98497, "vx":2.90397, "vy":-2.77766, "omega":-0.22866, "ax":4.56547, "ay":-4.36718, "alpha":5.26197, "fx":[88.78635,60.45819,68.53937,92.84528], "fy":[-53.93424,-78.35616,-92.65493,-72.19277]}, + {"t":0.43406, "x":2.2696, "y":6.7476, "heading":-0.9949, "vx":3.10213, "vy":-2.96722, "omega":-0.00026, "ax":0.0013, "ay":-0.00101, "alpha":0.001, "fx":[0.02284,0.01877,0.02141,0.02548], "fy":[-0.01386,-0.01649,-0.02056,-0.01793]}, + {"t":0.47746, "x":2.40426, "y":6.6188, "heading":-0.99491, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.00001, "ay":0.00002, "alpha":0.0, "fx":[0.00026,0.00025,0.00026,0.00026], "fy":[0.00026,0.00026,0.00026,0.00026]}, + {"t":0.52087, "x":2.53891, "y":6.49, "heading":-0.99492, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.00003,0.00003,0.00003,0.00003]}, + {"t":0.56428, "x":2.67356, "y":6.36121, "heading":-0.99493, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "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.60768, "x":2.80822, "y":6.23241, "heading":-0.99494, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":0.65109, "x":2.94287, "y":6.10361, "heading":-0.99495, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":-0.00002, "ay":-0.00002, "alpha":0.0, "fx":[-0.00026,-0.00026,-0.00026,-0.00026], "fy":[-0.00027,-0.00027,-0.00027,-0.00027]}, + {"t":0.69449, "x":3.07752, "y":5.97482, "heading":-0.99496, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":-0.0013, "ay":0.00101, "alpha":-0.001, "fx":[-0.0229,-0.01884,-0.02148,-0.02555], "fy":[0.01379,0.01642,0.0205,0.01786]}, + {"t":0.7379, "x":3.21217, "y":5.84602, "heading":-0.99497, "vx":3.10213, "vy":-2.96722, "omega":-0.00026, "ax":-4.56546, "ay":4.36719, "alpha":-5.26169, "fx":[-88.93608,-60.56625,-68.3736,-92.75297], "fy":[53.91803,78.15309,92.69306,72.37418]}, + {"t":0.7813, "x":3.34252, "y":5.72134, "heading":-0.99498, "vx":2.90397, "vy":-2.77766, "omega":-0.22865, "ax":-7.38987, "ay":7.06845, "alpha":0.4093, "fx":[-124.07616,-126.72012,-127.34339,-124.65854], "fy":[121.94135,119.20262,118.4877,121.29747]}, + {"t":0.82471, "x":3.46161, "y":5.60743, "heading":-1.0049, "vx":2.5832, "vy":-2.47085, "omega":-0.21089, "ax":-7.4217, "ay":7.0989, "alpha":0.53161, "fx":[-124.10816,-127.53787,-128.40758,-124.91041], "fy":[122.9803,119.42734,118.45976,122.1337]}, + {"t":0.86812, "x":3.56675, "y":5.50687, "heading":-1.01406, "vx":2.26106, "vy":-2.16272, "omega":-0.18781, "ax":-7.43238, "ay":7.10912, "alpha":0.57691, "fx":[-124.09086,-127.8061,-128.79326,-125.00054], "fy":[123.35612,119.50829,118.42065,122.41131]}, + {"t":0.91152, "x":3.65789, "y":5.41969, "heading":-1.02221, "vx":1.93845, "vy":-1.85414, "omega":-0.16277, "ax":-7.43773, "ay":7.11424, "alpha":0.60049, "fx":[-124.07225,-127.9325,-128.99606,-125.05385], "fy":[123.55407,119.55709,118.39075,122.54258]}, + {"t":0.95493, "x":3.73502, "y":5.34592, "heading":-1.02927, "vx":1.61561, "vy":-1.54534, "omega":-0.13671, "ax":-7.44094, "ay":7.11731, "alpha":0.61493, "fx":[-124.0561,-128.00285,-129.12248,-125.09164], "fy":[123.67779,119.59223,118.36775,122.61568]}, + {"t":0.99833, "x":3.79814, "y":5.28554, "heading":-1.03521, "vx":1.29263, "vy":-1.23641, "omega":-0.11001, "ax":-7.44308, "ay":7.11935, "alpha":0.62468, "fx":[-124.04272,-128.04643,-129.2092,-125.12034], "fy":[123.76286,119.61918,118.34977,122.66095]}, + {"t":1.04174, "x":3.84723, "y":5.23858, "heading":-1.03998, "vx":0.96956, "vy":-0.92739, "omega":-0.0829, "ax":-7.44461, "ay":7.12082, "alpha":0.63171, "fx":[-124.03195,-128.07599,-129.27225,-125.14249], "fy":[123.82482,119.64009,118.33571,122.69164]}, + {"t":1.08515, "x":3.88231, "y":5.20504, "heading":-1.04358, "vx":0.64642, "vy":-0.6183, "omega":-0.05548, "ax":-7.44576, "ay":7.12191, "alpha":0.63701, "fx":[-124.02364,-128.09799,-129.31977,-125.15929], "fy":[123.87152,119.65595,118.32492,122.7145]}, + {"t":1.12855, "x":3.90335, "y":5.18491, "heading":-1.04599, "vx":0.32323, "vy":-0.30917, "omega":-0.02783, "ax":-7.44665, "ay":7.12277, "alpha":0.64115, "fx":[-124.01763,-128.11603,-129.35631,-125.17137], "fy":[123.90737,119.66728,118.31697,122.7333]}, + {"t":1.17196, "x":3.91036, "y":5.1782, "heading":-1.0472, "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/startToG.traj b/src/main/deploy/choreo/startToG.traj new file mode 100644 index 00000000..313008ea --- /dev/null +++ b/src/main/deploy/choreo/startToG.traj @@ -0,0 +1,53 @@ +{ + "name":"startToG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7769125, "y":3.6225774894, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.87609], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.94202, "ay":-7.61907, "alpha":0.0, "fx":[-118.08182,-118.08182,-118.08182,-118.08182], "fy":[-129.59826,-129.59826,-129.59826,-129.59826]}, + {"t":0.04867, "x":7.09266, "y":5.06665, "heading":3.14159, "vx":-0.33788, "vy":-0.37083, "omega":0.0, "ax":-6.94116, "ay":-7.61813, "alpha":0.0, "fx":[-118.06713,-118.06713,-118.06713,-118.06713], "fy":[-129.58214,-129.58214,-129.58214,-129.58214]}, + {"t":0.09734, "x":7.068, "y":5.03958, "heading":3.14159, "vx":-0.67572, "vy":-0.74162, "omega":0.0, "ax":-6.94001, "ay":-7.61686, "alpha":0.0, "fx":[-118.04755,-118.04755,-118.04755,-118.04755], "fy":[-129.56064,-129.56064,-129.56064,-129.56064]}, + {"t":0.14601, "x":7.02689, "y":4.99446, "heading":3.14159, "vx":-1.0135, "vy":-1.11234, "omega":0.0, "ax":-6.9384, "ay":-7.61509, "alpha":0.0, "fx":[-118.02013,-118.02013,-118.02013,-118.02013], "fy":[-129.53055,-129.53055,-129.53055,-129.53055]}, + {"t":0.19469, "x":6.96934, "y":4.93131, "heading":3.14159, "vx":-1.3512, "vy":-1.48298, "omega":0.0, "ax":-6.93598, "ay":-7.61244, "alpha":0.0, "fx":[-117.97901,-117.97901,-117.97901,-117.97901], "fy":[-129.48542,-129.48542,-129.48542,-129.48542]}, + {"t":0.24336, "x":6.89536, "y":4.85011, "heading":3.14159, "vx":-1.68878, "vy":-1.85349, "omega":0.0, "ax":-6.93195, "ay":-7.60802, "alpha":0.0, "fx":[-117.91053,-117.91053,-117.91053,-117.91053], "fy":[-129.41025,-129.41025,-129.41025,-129.41025]}, + {"t":0.29203, "x":6.80496, "y":4.75089, "heading":3.14159, "vx":-2.02617, "vy":-2.22378, "omega":0.0, "ax":-6.92391, "ay":-7.5992, "alpha":0.0, "fx":[-117.77377,-117.77377,-117.77377,-117.77377], "fy":[-129.26016,-129.26016,-129.26016,-129.26016]}, + {"t":0.3407, "x":6.69814, "y":4.63365, "heading":3.14159, "vx":-2.36317, "vy":-2.59365, "omega":0.0, "ax":-6.89994, "ay":-7.57289, "alpha":0.0, "fx":[-117.36604,-117.36604,-117.36604,-117.36604], "fy":[-128.81266,-128.81266,-128.81266,-128.81266]}, + {"t":0.38937, "x":6.57495, "y":4.49844, "heading":3.14159, "vx":-2.699, "vy":-2.96223, "omega":0.0, "ax":-3.954, "ay":-4.33963, "alpha":0.0, "fx":[-67.25641,-67.25641,-67.25641,-67.25641], "fy":[-73.81588,-73.81588,-73.81588,-73.81588]}, + {"t":0.43804, "x":6.4389, "y":4.34913, "heading":3.14159, "vx":-2.89145, "vy":-3.17345, "omega":0.0, "ax":3.954, "ay":4.33963, "alpha":0.0, "fx":[67.25641,67.25641,67.25641,67.25641], "fy":[73.81588,73.81588,73.81588,73.81588]}, + {"t":0.48672, "x":6.30285, "y":4.19981, "heading":3.14159, "vx":-2.699, "vy":-2.96223, "omega":0.0, "ax":6.89994, "ay":7.57289, "alpha":0.0, "fx":[117.36604,117.36604,117.36604,117.36604], "fy":[128.81266,128.81266,128.81266,128.81266]}, + {"t":0.53539, "x":6.17966, "y":4.06461, "heading":3.14159, "vx":-2.36317, "vy":-2.59365, "omega":0.0, "ax":6.92391, "ay":7.5992, "alpha":0.0, "fx":[117.77377,117.77377,117.77377,117.77377], "fy":[129.26016,129.26016,129.26016,129.26016]}, + {"t":0.58406, "x":6.07284, "y":3.94737, "heading":3.14159, "vx":-2.02617, "vy":-2.22378, "omega":0.0, "ax":6.93195, "ay":7.60802, "alpha":0.0, "fx":[117.91053,117.91053,117.91053,117.91053], "fy":[129.41025,129.41025,129.41025,129.41025]}, + {"t":0.63273, "x":5.98244, "y":3.84815, "heading":3.14159, "vx":-1.68878, "vy":-1.85349, "omega":0.0, "ax":6.93598, "ay":7.61244, "alpha":0.0, "fx":[117.97901,117.97901,117.97901,117.97901], "fy":[129.48542,129.48542,129.48542,129.48542]}, + {"t":0.6814, "x":5.90846, "y":3.76695, "heading":3.14159, "vx":-1.3512, "vy":-1.48298, "omega":0.0, "ax":6.9384, "ay":7.61509, "alpha":0.0, "fx":[118.02013,118.02013,118.02013,118.02013], "fy":[129.53055,129.53055,129.53055,129.53055]}, + {"t":0.73007, "x":5.85091, "y":3.70379, "heading":3.14159, "vx":-1.0135, "vy":-1.11234, "omega":0.0, "ax":6.94001, "ay":7.61686, "alpha":0.0, "fx":[118.04755,118.04755,118.04755,118.04755], "fy":[129.56064,129.56064,129.56064,129.56064]}, + {"t":0.77874, "x":5.8098, "y":3.65867, "heading":3.14159, "vx":-0.67572, "vy":-0.74162, "omega":0.0, "ax":6.94116, "ay":7.61813, "alpha":0.0, "fx":[118.06713,118.06713,118.06713,118.06713], "fy":[129.58214,129.58214,129.58214,129.58214]}, + {"t":0.82742, "x":5.78514, "y":3.6316, "heading":3.14159, "vx":-0.33788, "vy":-0.37083, "omega":0.0, "ax":6.94202, "ay":7.61907, "alpha":0.0, "fx":[118.08182,118.08182,118.08182,118.08182], "fy":[129.59826,129.59826,129.59826,129.59826]}, + {"t":0.87609, "x":5.77691, "y":3.62258, "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/startToH.traj b/src/main/deploy/choreo/startToH.traj new file mode 100644 index 00000000..e04c8948 --- /dev/null +++ b/src/main/deploy/choreo/startToH.traj @@ -0,0 +1,52 @@ +{ + "name":"startToH", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7769125, "y":3.9512534894, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.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,0.82274], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.85635, "ay":-6.67224, "alpha":0.0, "fx":[-133.63427,-133.63427,-133.63427,-133.63427], "fy":[-113.49289,-113.49289,-113.49289,-113.49289]}, + {"t":0.0484, "x":7.09169, "y":5.06786, "heading":3.14159, "vx":-0.38022, "vy":-0.32291, "omega":0.0, "ax":-7.85537, "ay":-6.6714, "alpha":0.0, "fx":[-133.61753,-133.61753,-133.61753,-133.61753], "fy":[-113.47868,-113.47868,-113.47868,-113.47868]}, + {"t":0.09679, "x":7.06409, "y":5.04442, "heading":3.14159, "vx":-0.76039, "vy":-0.64578, "omega":0.0, "ax":-7.85405, "ay":-6.67029, "alpha":0.0, "fx":[-133.59521,-133.59521,-133.59521,-133.59521], "fy":[-113.45972,-113.45972,-113.45972,-113.45972]}, + {"t":0.14519, "x":7.01809, "y":5.00536, "heading":3.14159, "vx":-1.14049, "vy":-0.9686, "omega":0.0, "ax":-7.85222, "ay":-6.66873, "alpha":0.0, "fx":[-133.56394,-133.56394,-133.56394,-133.56394], "fy":[-113.43317,-113.43317,-113.43317,-113.43317]}, + {"t":0.19358, "x":6.9537, "y":4.95067, "heading":3.14159, "vx":-1.52051, "vy":-1.29134, "omega":0.0, "ax":-7.84946, "ay":-6.66639, "alpha":0.0, "fx":[-133.51701,-133.51701,-133.51701,-133.51701], "fy":[-113.39331,-113.39331,-113.39331,-113.39331]}, + {"t":0.24198, "x":6.87092, "y":4.88037, "heading":3.14159, "vx":-1.9004, "vy":-1.61397, "omega":0.0, "ax":-7.84485, "ay":-6.66248, "alpha":0.0, "fx":[-133.43873,-133.43873,-133.43873,-133.43873], "fy":[-113.32683,-113.32683,-113.32683,-113.32683]}, + {"t":0.29038, "x":6.76976, "y":4.79446, "heading":3.14159, "vx":-2.28006, "vy":-1.93641, "omega":0.0, "ax":-7.83564, "ay":-6.65465, "alpha":0.0, "fx":[-133.28199,-133.28199,-133.28199,-133.28199], "fy":[-113.19371,-113.19371,-113.19371,-113.19371]}, + {"t":0.33877, "x":6.65024, "y":4.69295, "heading":3.14159, "vx":-2.65927, "vy":-2.25847, "omega":0.0, "ax":-7.80796, "ay":-6.63115, "alpha":0.0, "fx":[-132.81122,-132.81122,-132.81122,-132.81122], "fy":[-112.7939,-112.7939,-112.7939,-112.7939]}, + {"t":0.38717, "x":6.51239, "y":4.57588, "heading":3.14159, "vx":-3.03715, "vy":-2.57939, "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.43557, "x":6.36541, "y":4.45105, "heading":3.14159, "vx":-3.03715, "vy":-2.57939, "omega":0.0, "ax":7.80796, "ay":6.63115, "alpha":0.0, "fx":[132.81122,132.81122,132.81122,132.81122], "fy":[112.7939,112.7939,112.7939,112.7939]}, + {"t":0.48396, "x":6.22756, "y":4.33398, "heading":3.14159, "vx":-2.65927, "vy":-2.25847, "omega":0.0, "ax":7.83564, "ay":6.65465, "alpha":0.0, "fx":[133.28199,133.28199,133.28199,133.28199], "fy":[113.19371,113.19371,113.19371,113.19371]}, + {"t":0.53236, "x":6.10804, "y":4.23247, "heading":3.14159, "vx":-2.28006, "vy":-1.93641, "omega":0.0, "ax":7.84485, "ay":6.66248, "alpha":0.0, "fx":[133.43873,133.43873,133.43873,133.43873], "fy":[113.32683,113.32683,113.32683,113.32683]}, + {"t":0.58075, "x":6.00688, "y":4.14656, "heading":3.14159, "vx":-1.9004, "vy":-1.61397, "omega":0.0, "ax":7.84946, "ay":6.66639, "alpha":0.0, "fx":[133.51701,133.51701,133.51701,133.51701], "fy":[113.39331,113.39331,113.39331,113.39331]}, + {"t":0.62915, "x":5.9241, "y":4.07626, "heading":3.14159, "vx":-1.52051, "vy":-1.29134, "omega":0.0, "ax":7.85222, "ay":6.66873, "alpha":0.0, "fx":[133.56394,133.56394,133.56394,133.56394], "fy":[113.43317,113.43317,113.43317,113.43317]}, + {"t":0.67755, "x":5.85971, "y":4.02157, "heading":3.14159, "vx":-1.14049, "vy":-0.9686, "omega":0.0, "ax":7.85405, "ay":6.67029, "alpha":0.0, "fx":[133.59521,133.59521,133.59521,133.59521], "fy":[113.45972,113.45972,113.45972,113.45972]}, + {"t":0.72594, "x":5.81371, "y":3.98251, "heading":3.14159, "vx":-0.76039, "vy":-0.64578, "omega":0.0, "ax":7.85537, "ay":6.6714, "alpha":0.0, "fx":[133.61753,133.61753,133.61753,133.61753], "fy":[113.47868,113.47868,113.47868,113.47868]}, + {"t":0.77434, "x":5.78611, "y":3.95907, "heading":3.14159, "vx":-0.38022, "vy":-0.32291, "omega":0.0, "ax":7.85635, "ay":6.67224, "alpha":0.0, "fx":[133.63427,133.63427,133.63427,133.63427], "fy":[113.49289,113.49289,113.49289,113.49289]}, + {"t":0.82274, "x":5.77691, "y":3.95125, "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/startToK.traj b/src/main/deploy/choreo/startToK.traj index 1f9da404..3a148a02 100644 --- a/src/main/deploy/choreo/startToK.traj +++ b/src/main/deploy/choreo/startToK.traj @@ -3,10 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.174379348754883, "y":6.211442947387695, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":5.2539963722229, "y":6.749329090118408, "heading":-1.0471975511965976, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.297754287719727, "y":6.199490070343018, "heading":-1.0471975511965976, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.455554008483887, "y":5.959114074707031, "heading":-1.325818250323842, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -16,10 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.174379348754883 m", "val":6.174379348754883}, "y":{"exp":"6.211442947387695 m", "val":6.211442947387695}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"5.2539963722229 m", "val":5.2539963722229}, "y":{"exp":"6.749329090118408 m", "val":6.749329090118408}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.297754287719727 m", "val":4.297754287719727}, "y":{"exp":"6.199490070343018 m", "val":6.199490070343018}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.455554008483887 m", "val":4.455554008483887}, "y":{"exp":"5.959114074707031 m", "val":5.959114074707031}, "heading":{"exp":"-1.325818250323842 rad", "val":-1.325818250323842}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -32,81 +28,57 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.60234,0.9311,1.28522,1.71887], + "waypoints":[0.0,1.01258,1.41325], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.87395, "ay":5.0951, "alpha":37.07106, "fx":[162.95989,5.95327,-121.202,-175.21246], "fy":[43.23402,175.1303,126.64944,1.65119]}, - {"t":0.0251, "x":7.1003, "y":5.07728, "heading":3.14159, "vx":-0.04703, "vy":0.12787, "omega":0.93039, "ax":-3.19948, "ay":6.6853, "alpha":26.88292, "fx":[76.32017,2.43903,-121.32427,-175.12386], "fy":[148.22829,175.18805,126.50624,4.93757]}, - {"t":0.05019, "x":7.09811, "y":5.0826, "heading":-3.11824, "vx":-0.12733, "vy":0.29566, "omega":1.60508, "ax":-4.31654, "ay":7.00756, "alpha":22.04484, "fx":[7.6521,-3.40404,-122.90665,-175.03392], "fy":[170.12692,175.14347,124.93754,6.57875]}, - {"t":0.07529, "x":7.09355, "y":5.09223, "heading":-3.07796, "vx":-0.23566, "vy":0.47153, "omega":2.15835, "ax":-4.721, "ay":7.00413, "alpha":20.90143, "fx":[-9.813,-11.02609,-125.42755,-174.94468], "fy":[171.78421,174.79654,122.36611,7.6063]}, - {"t":0.10039, "x":7.08615, "y":5.10627, "heading":-3.02379, "vx":-0.35415, "vy":0.64731, "omega":2.68292, "ax":-5.02228, "ay":6.96401, "alpha":20.27222, "fx":[-18.0773,-20.24986,-128.56331,-174.81978], "fy":[171.97537,173.92972,119.01404,8.90409]}, - {"t":0.12549, "x":7.07568, "y":5.1247, "heading":-2.95646, "vx":-0.48019, "vy":0.82209, "omega":3.1917, "ax":-5.36022, "ay":6.90951, "alpha":19.50234, "fx":[-26.84786,-31.18693,-132.08968,-174.57852], "fy":[171.30977,172.25386,115.0144,11.53724]}, - {"t":0.15058, "x":7.06194, "y":5.14751, "heading":-2.87635, "vx":-0.61472, "vy":0.9955, "omega":3.68116, "ax":-5.77572, "ay":6.85507, "alpha":18.23226, "fx":[-39.15196,-44.1216,-135.7167,-173.98301], "fy":[169.21482,169.33246,110.60446,17.25925]}, - {"t":0.17568, "x":7.0447, "y":5.17466, "heading":-2.78397, "vx":-0.75968, "vy":1.16755, "omega":4.13874, "ax":-6.27986, "ay":6.82283, "alpha":16.03166, "fx":[-56.45378,-59.61557,-138.97609,-172.22878], "fy":[164.43411,164.41231,106.31382,29.05732]}, - {"t":0.20078, "x":7.02365, "y":5.20611, "heading":-2.68009, "vx":-0.91729, "vy":1.33878, "omega":4.54109, "ax":-6.85021, "ay":6.83993, "alpha":12.26062, "fx":[-79.59392,-78.85882,-140.94224,-166.68565], "fy":[154.6732,155.91909,103.41143,51.37743]}, - {"t":0.22588, "x":6.99847, "y":5.24186, "heading":-2.56612, "vx":-1.08921, "vy":1.51045, "omega":4.8488, "ax":-7.39475, "ay":6.87643, "alpha":6.14976, "fx":[-108.36587,-104.73156,-139.09746,-150.93542], "fy":[136.13754,139.48964,105.35124,86.88594]}, - {"t":0.25097, "x":6.96881, "y":5.28194, "heading":-2.44443, "vx":-1.2748, "vy":1.68303, "omega":5.00314, "ax":-7.72604, "ay":6.62389, "alpha":-3.40365, "fx":[-139.6609,-144.1275,-121.68667,-120.19567], "fy":[103.85161,97.06085,123.9501,125.81906]}, - {"t":0.27607, "x":6.93438, "y":5.32626, "heading":-2.31887, "vx":-1.4687, "vy":1.84927, "omega":4.91772, "ax":-6.42132, "ay":5.16921, "alpha":-23.85069, "fx":[-162.84141,-169.92741,-13.72107,-90.40911], "fy":[61.52202,-29.84942,171.34171,148.69302]}, - {"t":0.30117, "x":6.8955, "y":5.3743, "heading":-2.19544, "vx":-1.62986, "vy":1.979, "omega":4.31913, "ax":-6.30262, "ay":5.32002, "alpha":-23.67024, "fx":[-163.02809,-170.34663,-3.74021,-91.7079], "fy":[60.19181,-17.96736,172.04646,147.69719]}, - {"t":0.32627, "x":6.85261, "y":5.42564, "heading":-2.08705, "vx":-1.78804, "vy":2.11252, "omega":3.72507, "ax":-6.2415, "ay":5.54236, "alpha":-22.47972, "fx":[-163.05499,-169.4567,0.94367,-93.0967], "fy":[59.0473,-0.57285,172.05859,146.56302]}, - {"t":0.35136, "x":6.80577, "y":5.48041, "heading":-1.99356, "vx":-1.94468, "vy":2.25162, "omega":3.16089, "ax":-6.14038, "ay":5.71681, "alpha":-21.44793, "fx":[-163.48192,-165.84011,5.82653,-94.289], "fy":[56.4247,15.34729,171.74282,145.45044]}, - {"t":0.37646, "x":6.75503, "y":5.53872, "heading":-1.91423, "vx":-2.09879, "vy":2.3951, "omega":2.6226, "ax":-5.99961, "ay":5.85545, "alpha":-20.60076, "fx":[-164.1016,-158.65487,10.09263,-95.54246], "fy":[52.52555,30.62058,171.113,144.13904]}, - {"t":0.40156, "x":6.70046, "y":5.60067, "heading":-1.84841, "vx":-2.24937, "vy":2.54206, "omega":2.10557, "ax":-5.78374, "ay":5.99446, "alpha":-19.91315, "fx":[-164.49972,-145.22616,13.06072,-96.854], "fy":[47.98934,47.20048,170.15644,142.50979]}, - {"t":0.42666, "x":6.64219, "y":5.66636, "heading":-1.79556, "vx":-2.39452, "vy":2.6925, "omega":1.60581, "ax":-5.42097, "ay":6.19025, "alpha":-19.31771, "fx":[-163.92443,-120.56809,13.64597,-97.99012], "fy":[44.24572,67.76599,168.72833,140.43727]}, - {"t":0.45175, "x":6.58038, "y":5.73589, "heading":-1.75526, "vx":-2.53057, "vy":2.84786, "omega":1.12098, "ax":-4.93632, "ay":6.39285, "alpha":-18.70739, "fx":[-160.76705,-87.11462,10.21508,-98.19494], "fy":[43.84788,87.48399,166.03221,137.59826]}, - {"t":0.47685, "x":6.51532, "y":5.80937, "heading":-1.72713, "vx":-2.65446, "vy":3.0083, "omega":0.65147, "ax":-4.38073, "ay":6.30499, "alpha":-17.60353, "fx":[-149.40143,-57.19315,2.76126,-94.22675], "fy":[49.01981,90.30153,157.48955,132.17366]}, - {"t":0.50195, "x":6.44732, "y":5.88686, "heading":-1.71078, "vx":-2.76441, "vy":3.16654, "omega":0.20967, "ax":-0.73342, "ay":3.27025, "alpha":-6.8054, "fx":[-34.49394,3.13123,7.6915,-26.22985], "fy":[42.35856,39.01522,69.45703,71.67299]}, - {"t":0.52705, "x":6.37771, "y":5.96736, "heading":-1.70551, "vx":-2.78282, "vy":3.24862, "omega":0.03887, "ax":0.97227, "ay":0.85014, "alpha":-0.04219, "fx":[16.42395,16.62758,16.65193,16.44835], "fy":[14.37428,14.34348,14.54706,14.5779]}, - {"t":0.55214, "x":6.30817, "y":6.04916, "heading":-1.70454, "vx":-2.75841, "vy":3.26995, "omega":0.03782, "ax":2.94745, "ay":-0.3281, "alpha":4.91756, "fx":[62.62375,41.0179,37.50168,59.39763], "fy":[5.2711,9.29077,-17.43762,-19.44794]}, - {"t":0.57724, "x":6.23987, "y":6.13112, "heading":-1.70359, "vx":-2.68444, "vy":3.26172, "omega":0.16123, "ax":5.96437, "ay":-4.89617, "alpha":17.26723, "fx":[154.6941,111.04268,28.11894,111.95341], "fy":[-28.65223,-37.39613,-151.31014,-115.77141]}, - {"t":0.60234, "x":6.17438, "y":6.21144, "heading":-1.69954, "vx":-2.53475, "vy":3.13884, "omega":0.5946, "ax":3.49947, "ay":-7.21468, "alpha":17.84806, "fx":[148.80184,13.12396,-12.39956,88.57343], "fy":[-61.39132,-126.923,-162.47835,-140.08585]}, - {"t":0.61728, "x":6.13689, "y":6.25754, "heading":-1.69066, "vx":-2.48246, "vy":3.03102, "omega":0.86131, "ax":1.05254, "ay":-8.56164, "alpha":16.66695, "fx":[113.2785,-59.26755,-38.02411,55.62689], "fy":[-116.63566,-144.45554,-162.87246,-158.56045]}, - {"t":0.63222, "x":6.09991, "y":6.30188, "heading":-1.67779, "vx":-2.46673, "vy":2.90308, "omega":1.11038, "ax":-0.34641, "ay":-8.99662, "alpha":15.07928, "fx":[79.14707,-84.21369,-53.01755,34.51476], "fy":[-144.57461,-140.41836,-161.00702,-166.11984]}, - {"t":0.64717, "x":6.06301, "y":6.34426, "heading":-1.66119, "vx":-2.4719, "vy":2.76864, "omega":1.33572, "ax":-1.26779, "ay":-9.17598, "alpha":13.57443, "fx":[51.29856,-96.11172,-61.81217,20.36658], "fy":[-158.64338,-136.8424,-159.2815,-169.55562]}, - {"t":0.66211, "x":6.02593, "y":6.38461, "heading":-1.64123, "vx":-2.49085, "vy":2.63152, "omega":1.53857, "ax":-1.91509, "ay":-9.26391, "alpha":12.1785, "fx":[28.54278,-101.91288,-66.95836,10.02805], "fy":[-165.8196,-135.09494,-158.13612,-171.25545]}, - {"t":0.67706, "x":5.98849, "y":6.4229, "heading":-1.61824, "vx":-2.51947, "vy":2.49308, "omega":1.72056, "ax":-2.38887, "ay":-9.31642, "alpha":10.86504, "fx":[9.6228,-104.16661,-69.80279,1.81037], "fy":[-169.20722,-135.00404,-157.56358,-172.10372]}, - {"t":0.692, "x":5.95058, "y":6.45911, "heading":-1.59253, "vx":-2.55517, "vy":2.35386, "omega":1.88292, "ax":-2.74131, "ay":-9.35566, "alpha":9.61336, "fx":[-6.19819,-104.021,-71.07533,-5.22138], "fy":[-170.35819,-136.25296,-157.47618,-172.46127]}, - {"t":0.70694, "x":5.91209, "y":6.49324, "heading":-1.56439, "vx":-2.59613, "vy":2.21405, "omega":2.02658, "ax":-3.00171, "ay":-9.39129, "alpha":8.41176, "fx":[-19.31447,-102.09391,-71.21513,-11.60973], "fy":[-170.18744,-138.52514,-157.77594,-172.4838]}, - {"t":0.72189, "x":5.87296, "y":6.52528, "heading":-1.53411, "vx":-2.64099, "vy":2.07371, "omega":2.15228, "ax":-3.18887, "ay":-9.42739, "alpha":7.25275, "fx":[-29.98399,-98.78771,-70.52218,-17.67321], "fy":[-169.30822,-141.51566,-158.36673,-172.2383]}, - {"t":0.73683, "x":5.83314, "y":6.55522, "heading":-1.50194, "vx":-2.68864, "vy":1.93283, "omega":2.26066, "ax":-3.31674, "ay":-9.46521, "alpha":6.12968, "fx":[-38.44767,-94.40548,-69.22309,-23.59079], "fy":[-168.14595,-144.94531,-159.1591,-171.7519]}, - {"t":0.75177, "x":5.79259, "y":6.58305, "heading":-1.46816, "vx":-2.7382, "vy":1.79139, "omega":2.35226, "ax":-3.39662, "ay":-9.50442, "alpha":5.03595, "fx":[-44.96464,-89.18936,-67.49785,-29.45018], "fy":[-166.98176,-148.57837,-160.07473,-171.03517]}, - {"t":0.76672, "x":5.75129, "y":6.60875, "heading":-1.43301, "vx":-2.78896, "vy":1.64936, "omega":2.42752, "ax":-3.4377, "ay":-9.544, "alpha":3.96503, "fx":[-49.80131,-83.32649,-65.48904,-35.28023], "fy":[-165.98347,-152.23498,-161.0502,-170.09396]}, - {"t":0.78166, "x":5.70923, "y":6.63234, "heading":-1.39673, "vx":-2.84033, "vy":1.50673, "omega":2.48677, "ax":-3.44608, "ay":-9.5833, "alpha":2.90455, "fx":[-53.2011,-76.89237,-63.28111,-41.09304], "fy":[-165.23974,-155.81863,-162.04774,-168.93063]}, - {"t":0.7966, "x":5.6664, "y":6.65378, "heading":-1.35957, "vx":-2.89183, "vy":1.36353, "omega":2.53018, "ax":-3.41532, "ay":-9.62633, "alpha":1.78118, "fx":[-55.2766,-69.34063,-60.67631,-47.08067], "fy":[-164.82323,-159.51524,-163.13402,-167.49207]}, - {"t":0.81155, "x":5.6228, "y":6.67308, "heading":-1.32176, "vx":-2.94287, "vy":1.21967, "omega":2.55679, "ax":-3.27538, "ay":-9.69644, "alpha":0.17909, "fx":[-55.46686,-56.84764,-55.95777,-54.58116], "fy":[-165.00894,-164.54635,-164.86316,-165.31626]}, - {"t":0.82649, "x":5.57846, "y":6.69023, "heading":-1.28355, "vx":-2.99182, "vy":1.07477, "omega":2.55947, "ax":-2.91165, "ay":-9.79367, "alpha":-2.48151, "fx":[-52.7641,-33.86933,-45.85782,-65.61414], "fy":[-166.14364,-170.91748,-167.92863,-161.36012]}, - {"t":0.84144, "x":5.53343, "y":6.70519, "heading":-1.24531, "vx":-3.03533, "vy":0.92842, "omega":2.52239, "ax":-2.45642, "ay":-9.83088, "alpha":-5.35817, "fx":[-48.70348,-8.53002,-32.5503,-77.34845], "fy":[-167.58095,-174.17717,-170.94085,-156.1832]}, - {"t":0.85638, "x":5.48779, "y":6.71797, "heading":-1.20761, "vx":-3.07203, "vy":0.78151, "omega":2.44232, "ax":-2.01031, "ay":-9.79813, "alpha":-7.89639, "fx":[-44.39978,13.56846,-18.34906,-87.59881], "fy":[-168.92172,-173.98478,-172.96095,-150.78626]}, - {"t":0.87132, "x":5.44166, "y":6.72855, "heading":-1.17112, "vx":-3.10207, "vy":0.63509, "omega":2.32432, "ax":-1.59079, "ay":-9.71964, "alpha":-10.04238, "fx":[-40.13709,31.84904,-4.00064,-95.94709], "fy":[-170.09717,-171.69083,-173.79509,-145.72983]}, - {"t":0.88627, "x":5.39513, "y":6.73696, "heading":-1.13638, "vx":-3.12585, "vy":0.48984, "omega":2.17425, "ax":-1.19665, "ay":-9.61402, "alpha":-11.8545, "fx":[-36.01742,46.87817,10.25269,-102.53225], "fy":[-171.10341,-168.30183,-173.4391,-141.28283]}, - {"t":0.90121, "x":5.34828, "y":6.74321, "heading":-1.10389, "vx":-3.14373, "vy":0.34618, "omega":1.9971, "ax":-0.82358, "ay":-9.49145, "alpha":-13.40956, "fx":[-32.10075,59.31968,24.40184,-107.65616], "fy":[-171.94976,-164.40807,-171.90981,-137.51945]}, - {"t":0.91615, "x":5.30121, "y":6.74732, "heading":-1.07405, "vx":-3.15604, "vy":0.20434, "omega":1.79671, "ax":-0.46673, "ay":-9.35574, "alpha":-14.7841, "fx":[-28.43322,69.71938,38.59945,-111.64145], "fy":[-172.64958,-160.34242,-169.16572,-134.39602]}, - {"t":0.9311, "x":5.254, "y":6.74933, "heading":-1.0472, "vx":-3.16301, "vy":0.06453, "omega":1.57578, "ax":-0.22556, "ay":-9.54189, "alpha":-13.46921, "fx":[-21.74724,69.42219,36.4,-99.42181], "fy":[-173.81579,-160.74518,-170.62071,-144.03762]}, - {"t":0.95639, "x":5.17392, "y":6.74791, "heading":-1.00734, "vx":-3.16872, "vy":-0.17683, "omega":1.23508, "ax":0.28974, "ay":-9.60405, "alpha":-12.9059, "fx":[-12.28791,74.80241,44.39735,-87.19826], "fy":[-174.71708,-158.28756,-168.73181,-151.71245]}, - {"t":0.98169, "x":5.09386, "y":6.74036, "heading":-0.9761, "vx":-3.16139, "vy":-0.41976, "omega":0.90864, "ax":0.87217, "ay":-9.6364, "alpha":-12.29359, "fx":[-2.39131,80.01333,54.51588,-72.79671], "fy":[-175.10734,-155.69249,-165.76595,-159.08388]}, - {"t":1.00698, "x":5.01417, "y":6.72666, "heading":-0.95311, "vx":-3.13933, "vy":-0.66351, "omega":0.59767, "ax":1.52065, "ay":-9.63177, "alpha":-11.56159, "fx":[8.17869,85.12605,65.76814,-55.60986], "fy":[-174.90455,-152.9307,-161.66849,-165.83067]}, - {"t":1.03228, "x":4.93525, "y":6.7068, "heading":-0.938, "vx":-3.10086, "vy":-0.90714, "omega":0.30523, "ax":2.23594, "ay":-9.57961, "alpha":-10.63277, "fx":[19.70733,90.21282,77.23917,-35.02861], "fy":[-173.95155,-149.95943,-156.55794,-171.31702]}, - {"t":1.05757, "x":4.85753, "y":6.68079, "heading":-0.93028, "vx":-3.04431, "vy":-1.14945, "omega":0.03628, "ax":3.01859, "ay":-9.46366, "alpha":-9.43771, "fx":[32.52369,95.35171,88.19905,-10.69332], "fy":[-171.98117,-146.7177,-150.70756,-174.49028]}, - {"t":1.08286, "x":4.78149, "y":6.64869, "heading":-0.92936, "vx":-2.96795, "vy":-1.38883, "omega":-0.20244, "ax":3.8629, "ay":-9.26096, "alpha":-7.94706, "fx":[46.97026,100.63129,98.16504,17.06102], "fy":[-168.56963,-143.11889,-144.46995,-173.94682]}, - {"t":1.10816, "x":4.70766, "y":6.61059, "heading":-0.93448, "vx":-2.87024, "vy":-1.62308, "omega":-0.40346, "ax":4.74908, "ay":-8.94655, "alpha":-6.20509, "fx":[63.32095,106.15172,106.89995,46.74956], "fy":[-163.08932,-139.04239,-138.18897,-168.39208]}, - {"t":1.13345, "x":4.63657, "y":6.56668, "heading":-0.94468, "vx":-2.75012, "vy":-1.84938, "omega":-0.56041, "ax":5.64085, "ay":-8.50442, "alpha":-4.32362, "fx":[81.61609,112.01873,114.36231,75.79973], "fy":[-154.70008,-134.32647,-132.13653,-157.46816]}, - {"t":1.15875, "x":4.56882, "y":6.51718, "heading":-0.95886, "vx":-2.60743, "vy":-2.06449, "omega":-0.66978, "ax":6.49445, "ay":-7.93682, "alpha":-2.41724, "fx":[101.41082,118.32905,120.63826,101.49695], "fy":[-142.46425,-128.76439,-126.48725,-142.29647]}, - {"t":1.18404, "x":4.50494, "y":6.46242, "heading":-0.9758, "vx":-2.44316, "vy":-2.26525, "omega":-0.73092, "ax":7.27171, "ay":-7.2628, "alpha":-0.54151, "fx":[121.54097,125.14797,125.87969,122.19012], "fy":[-125.69091,-122.10495,-121.32348,-125.03328]}, - {"t":1.20934, "x":4.44547, "y":6.4028, "heading":-0.99429, "vx":-2.25923, "vy":-2.44896, "omega":-0.74462, "ax":7.94558, "ay":-6.511, "alpha":1.28982, "fx":[140.17678,132.47961,130.25926,137.69263], "fy":[-104.48175,-114.0606,-116.65647,-107.80198]}, - {"t":1.23463, "x":4.39086, "y":6.33877, "heading":-1.01312, "vx":-2.05825, "vy":-2.61365, "omega":-0.71199, "ax":8.50054, "ay":-5.71538, "alpha":3.04162, "fx":[155.40404,140.22992,133.94303,148.79014], "fy":[-80.11028,-104.33087,-112.45157,-91.97501]}, - {"t":1.25993, "x":4.34152, "y":6.27083, "heading":-1.03113, "vx":-1.84323, "vy":-2.75822, "omega":-0.63506, "ax":8.93437, "ay":-4.91112, "alpha":4.64301, "fx":[166.07495,148.16826,137.07702,156.56439], "fy":[-54.73026,-92.65158,-108.64987,-78.11553]}, - {"t":1.28522, "x":4.29775, "y":6.19949, "heading":-1.0472, "vx":-1.61724, "vy":-2.88244, "omega":-0.51761, "ax":9.47119, "ay":-3.82822, "alpha":4.30181, "fx":[171.0855,160.49655,149.34604,163.48072], "fy":[-36.90311,-69.5811,-91.29186,-62.69164]}, - {"t":1.32136, "x":4.2455, "y":6.09283, "heading":-1.0659, "vx":-1.27498, "vy":-3.02078, "omega":-0.36216, "ax":10.04635, "ay":-1.79521, "alpha":4.41529, "fx":[174.93258,172.70688,164.12969,171.77296], "fy":[-1.22634,-26.97682,-60.47788,-33.46324]}, - {"t":1.35749, "x":4.20598, "y":5.98249, "heading":-1.07899, "vx":-0.91193, "vy":-3.08566, "omega":-0.2026, "ax":10.11629, "ay":1.32569, "alpha":4.32977, "fx":[168.3788,170.78065,174.66865,174.47243], "fy":[47.16392,36.75512,-5.94147,12.2211]}, - {"t":1.39363, "x":4.17963, "y":5.87185, "heading":-1.08631, "vx":-0.54636, "vy":-3.03775, "omega":-0.04614, "ax":8.83357, "ay":5.149, "alpha":3.5441, "fx":[143.11902,140.24709,159.15013,158.50995], "fy":[100.41159,104.15719,72.0167,73.74664]}, - {"t":1.42977, "x":4.16566, "y":5.76544, "heading":-1.08798, "vx":-0.22714, "vy":-2.85168, "omega":0.08194, "ax":6.24013, "ay":8.14301, "alpha":2.1768, "fx":[102.54978,94.53545,110.67783,116.80819], "fy":[141.64696,147.0381,135.25496,130.10117]}, - {"t":1.46591, "x":4.16152, "y":5.6677, "heading":-1.08502, "vx":-0.00164, "vy":-2.55742, "omega":0.1606, "ax":3.64071, "ay":9.61453, "alpha":0.9597, "fx":[61.58582,55.73997,62.34109,68.04286], "fy":[163.75684,165.81611,163.42708,161.16136]}, - {"t":1.50204, "x":4.16384, "y":5.58156, "heading":-1.07921, "vx":0.12993, "vy":-2.20998, "omega":0.19528, "ax":1.70564, "ay":10.14798, "alpha":0.09374, "fx":[29.09321,28.38271,28.9314,29.6427], "fy":[172.60291,172.71975,172.62684,172.50756]}, - {"t":1.53818, "x":4.16965, "y":5.50833, "heading":-1.07216, "vx":0.19156, "vy":-1.84326, "omega":0.19867, "ax":0.36838, "ay":10.28736, "alpha":-0.48997, "fx":[5.41462,9.54109,7.08748,3.02116], "fy":[175.02272,174.85157,174.97656,175.0891]}, - {"t":1.57432, "x":4.17681, "y":5.44843, "heading":-1.06498, "vx":0.20488, "vy":-1.4715, "omega":0.18096, "ax":-0.56506, "ay":10.27961, "alpha":-0.8908, "fx":[-11.68923,-3.76812,-7.67071,-15.31793], "fy":[174.76986,175.12687,175.01261,174.50343]}, - {"t":1.61045, "x":4.18385, "y":5.40197, "heading":-1.05844, "vx":0.18446, "vy":-1.10003, "omega":0.14877, "ax":-1.238, "ay":10.22061, "alpha":-1.1768, "fx":[-24.281,-13.50369,-18.11304,-28.33437], "fy":[173.51157,174.68831,174.28611,172.91265]}, - {"t":1.64659, "x":4.18971, "y":5.36889, "heading":-1.05306, "vx":0.13972, "vy":-0.73068, "omega":0.10624, "ax":-1.74005, "ay":10.14699, "alpha":-1.3888, "fx":[-33.80042,-20.86747,-25.81559,-37.9073], "fy":[171.94454,173.99514,173.34631,171.10383]}, - {"t":1.68273, "x":4.19362, "y":5.34911, "heading":-1.04922, "vx":0.07684, "vy":-0.364, "omega":0.05606, "ax":-2.1263, "ay":10.07277, "alpha":-1.55123, "fx":[-41.19267,-26.60419,-31.69437,-45.17964], "fy":[170.35203,173.23688,172.39461,169.35629]}, - {"t":1.71887, "x":4.19501, "y":5.34254, "heading":-1.0472, "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.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.66326, "ay":4.08466, "alpha":19.89713, "fx":[-174.29945,-36.35714,-135.46412,-175.27903], "fy":[-4.09958,171.42994,111.30349,-0.71839]}, + {"t":0.03068, "x":7.09728, "y":5.0776, "heading":3.14159, "vx":-0.23514, "vy":0.12533, "omega":0.61053, "ax":-7.70099, "ay":4.2955, "alpha":18.95747, "fx":[-174.1029,-39.12919,-135.48363,-175.25079], "fy":[8.5894,170.79226,111.25268,1.62657]}, + {"t":0.06137, "x":7.08644, "y":5.08347, "heading":-3.12286, "vx":-0.47144, "vy":0.25714, "omega":1.19222, "ax":-7.74975, "ay":4.60664, "alpha":17.46219, "fx":[-171.62353,-44.04808,-136.4213,-175.1914], "fy":[30.20677,169.5602,110.06629,3.59754]}, + {"t":0.09205, "x":7.06833, "y":5.09353, "heading":-3.08628, "vx":-0.70924, "vy":0.39849, "omega":1.72804, "ax":-7.7708, "ay":4.97682, "alpha":15.77532, "fx":[-164.48135,-51.01819,-138.12354,-175.09298], "fy":[57.4363,167.55884,107.87644,5.74589]}, + {"t":0.12274, "x":7.0429, "y":5.1081, "heading":-3.03325, "vx":-0.94768, "vy":0.5512, "omega":2.21209, "ax":-7.75642, "ay":5.33016, "alpha":14.2765, "fx":[-152.5142,-59.97692,-140.3495,-174.89728], "fy":[84.2194,164.51878,104.90004,9.01975]}, + {"t":0.15342, "x":7.01017, "y":5.12752, "heading":-2.96538, "vx":-1.18568, "vy":0.71475, "omega":2.65016, "ax":-7.75501, "ay":5.60108, "alpha":12.99142, "fx":[-139.42959,-71.04844,-142.76819,-174.39603], "fy":[104.53705,159.98538,101.48894,15.0795]}, + {"t":0.18411, "x":6.97014, "y":5.15209, "heading":-2.88406, "vx":-1.42363, "vy":0.88662, "omega":3.04879, "ax":-7.83002, "ay":5.79308, "alpha":11.40375, "fx":[-130.27417,-84.75468,-144.88281,-172.83415], "fy":[115.8378,153.06788,98.29513,26.95338]}, + {"t":0.21479, "x":6.92277, "y":5.18202, "heading":-2.79051, "vx":-1.66389, "vy":1.06437, "omega":3.39871, "ax":-7.99156, "ay":5.97829, "alpha":8.51767, "fx":[-128.22413,-102.4144,-145.68103,-167.4169], "fy":[118.18071,141.68691,96.83974,50.04877]}, + {"t":0.24547, "x":6.86796, "y":5.2175, "heading":-2.68622, "vx":-1.90911, "vy":1.24781, "omega":3.66006, "ax":-8.116, "ay":6.22578, "alpha":2.80116, "fx":[-134.13234,-127.1594,-141.85,-149.0614], "fy":[111.4786,119.58615,101.82173,90.70856]}, + {"t":0.27616, "x":6.80556, "y":5.25871, "heading":-2.57392, "vx":-2.15814, "vy":1.43885, "omega":3.74602, "ax":-7.69964, "ay":6.29959, "alpha":-8.56929, "fx":[-146.86479,-163.19968,-109.8091,-104.00111], "fy":[94.08684,60.21179,134.4293,139.88881]}, + {"t":0.30684, "x":6.73571, "y":5.30583, "heading":-2.45897, "vx":-2.3944, "vy":1.63215, "omega":3.48307, "ax":-6.98366, "ay":5.87854, "alpha":-17.21122, "fx":[-155.58922,-173.00265,-57.31237,-89.25617], "fy":[78.45244,9.41339,162.56996,149.53315]}, + {"t":0.33753, "x":6.65895, "y":5.35868, "heading":-2.3521, "vx":-2.60869, "vy":1.81252, "omega":2.95496, "ax":-7.12261, "ay":5.40271, "alpha":-18.59448, "fx":[-160.80393,-172.37799,-54.10612,-97.32596], "fy":[66.17717,-5.38792,162.82088,143.98404]}, + {"t":0.36821, "x":6.57555, "y":5.41684, "heading":-2.26143, "vx":-2.82724, "vy":1.9783, "omega":2.3844, "ax":-7.23516, "ay":4.71176, "alpha":-20.54389, "fx":[-165.7071,-168.69792,-52.20463,-105.66267], "fy":[50.69862,-29.02806,161.71721,137.19528]}, + {"t":0.3989, "x":6.48539, "y":5.47976, "heading":-2.18826, "vx":-3.04925, "vy":2.12288, "omega":1.75403, "ax":-7.02484, "ay":3.83963, "alpha":-24.07171, "fx":[-169.06341,-156.61588,-40.49934,-111.78313], "fy":[32.41845,-62.93254,161.13375,130.62451]}, + {"t":0.42958, "x":6.38852, "y":5.54671, "heading":-2.13444, "vx":-3.2648, "vy":2.2407, "omega":1.0154, "ax":-6.72832, "ay":3.88352, "alpha":-23.5052, "fx":[-165.1354,-151.4919,-32.36395,-108.79594], "fy":[33.2275,-52.83235,155.4312,128.40415]}, + {"t":0.46026, "x":6.28518, "y":5.61729, "heading":-2.10328, "vx":-3.47125, "vy":2.35986, "omega":0.29416, "ax":-3.07182, "ay":0.66139, "alpha":-8.91899, "fx":[-78.85365,-47.42189,-23.61247,-59.11494], "fy":[2.89791,-21.83227,22.0839,41.85047]}, + {"t":0.49095, "x":6.17722, "y":5.69001, "heading":-2.09426, "vx":-3.56551, "vy":2.38015, "omega":0.02049, "ax":-0.87052, "ay":-1.29965, "alpha":-0.03315, "fx":[-14.91786,-14.77509,-14.69661,-14.83943], "fy":[-22.13364,-22.21662,-22.07971,-21.99671]}, + {"t":0.52163, "x":6.0674, "y":5.76243, "heading":-2.09363, "vx":-3.59222, "vy":2.34027, "omega":0.01947, "ax":-2.26188, "ay":-3.59622, "alpha":0.01253, "fx":[-38.43087,-38.49229,-38.51691,-38.45551], "fy":[-61.16672,-61.13041,-61.17474,-61.21105]}, + {"t":0.55232, "x":5.95612, "y":5.83255, "heading":-2.09303, "vx":-3.66162, "vy":2.22993, "omega":0.01986, "ax":-3.81248, "ay":-6.71073, "alpha":0.06542, "fx":[-64.59718,-65.02952,-65.10064,-64.66949], "fy":[-114.2106,-113.96408,-114.08503,-114.33088]}, + {"t":0.583, "x":5.84197, "y":5.89781, "heading":-2.09242, "vx":-3.77861, "vy":2.02401, "omega":0.02187, "ax":-3.49684, "ay":-8.52685, "alpha":1.94111, "fx":[-49.96476,-66.21137,-68.23758,-53.50733], "fy":[-148.00046,-141.16066,-142.30108,-148.69476]}, + {"t":0.61369, "x":5.72438, "y":5.9559, "heading":-2.09175, "vx":-3.8859, "vy":1.76237, "omega":0.08143, "ax":0.63224, "ay":-9.26119, "alpha":11.60606, "fx":[88.09956,-14.00488,-54.4922,23.41469], "fy":[-142.43851,-159.94672,-159.48804,-168.24782]}, + {"t":0.64437, "x":5.60544, "y":6.00562, "heading":-2.08925, "vx":-3.8665, "vy":1.4782, "omega":0.43755, "ax":2.40583, "ay":-8.94231, "alpha":13.65364, "fx":[125.02887,41.9609,-48.51929,45.21918], "fy":[-117.80013,-159.05939,-164.79945,-166.76527]}, + {"t":0.67505, "x":5.48793, "y":6.04677, "heading":-2.07583, "vx":-3.79268, "vy":1.20381, "omega":0.8565, "ax":3.16439, "ay":-8.80155, "alpha":13.68574, "fx":[132.65142,68.9556,-39.96922,53.66352], "fy":[-111.38338,-153.74405,-168.42325,-165.29639]}, + {"t":0.70574, "x":5.37304, "y":6.07956, "heading":-2.04955, "vx":-3.69559, "vy":0.93374, "omega":1.27644, "ax":3.53568, "ay":-8.79806, "alpha":13.03893, "fx":[134.2319,76.36349,-29.48886,59.45679], "fy":[-110.52379,-153.00458,-171.26475,-163.81705]}, + {"t":0.73642, "x":5.26131, "y":6.10407, "heading":-2.01038, "vx":-3.5871, "vy":0.66378, "omega":1.67653, "ax":3.77251, "ay":-8.84457, "alpha":12.10267, "fx":[133.82545,76.08707,-17.93322,64.69797], "fy":[-111.60224,-154.77802,-173.27426,-162.11962]}, + {"t":0.76711, "x":5.15302, "y":6.12028, "heading":-1.95894, "vx":-3.47134, "vy":0.39239, "omega":2.04789, "ax":3.98636, "ay":-8.89239, "alpha":11.01112, "fx":[132.3216,73.94449,-5.1065,70.06811], "fy":[-113.74586,-156.81724,-174.39881,-160.0664]}, + {"t":0.79779, "x":5.04838, "y":6.12813, "heading":-1.8961, "vx":-3.34902, "vy":0.11953, "omega":2.38576, "ax":4.2696, "ay":-8.91622, "alpha":9.57994, "fx":[129.303,73.81799,11.19992,76.17805], "fy":[-117.40838,-157.55864,-174.3019,-157.38051]}, + {"t":0.82848, "x":4.94763, "y":6.1276, "heading":-1.82289, "vx":-3.21801, "vy":-0.15405, "omega":2.67971, "ax":5.095, "ay":-8.78742, "alpha":5.38612, "fx":[117.68797,87.23676,53.79621,87.93711], "fy":[-129.21499,-151.20398,-166.29411,-151.17285]}, + {"t":0.85916, "x":4.85129, "y":6.11874, "heading":-1.74067, "vx":-3.06167, "vy":-0.42369, "omega":2.84498, "ax":5.8498, "ay":-8.45274, "alpha":0.70895, "fx":[103.48705,99.47138,95.484,99.57145], "fy":[-140.98086,-143.82168,-146.51994,-143.79247]}, + {"t":0.88984, "x":4.76009, "y":6.10176, "heading":-1.65337, "vx":-2.88218, "vy":-0.68305, "omega":2.86674, "ax":6.3159, "ay":-8.08132, "alpha":-2.75299, "fx":[92.30566,108.31135,122.06187,107.04772], "fy":[-148.65409,-137.4862,-125.37063,-138.33305]}, + {"t":0.92053, "x":4.67463, "y":6.07699, "heading":-1.56541, "vx":-2.68838, "vy":-0.93102, "omega":2.78226, "ax":6.60718, "ay":-7.74543, "alpha":-5.23735, "fx":[84.53579,115.61045,138.84363,110.55528], "fy":[-153.29562,-131.53204,-106.59857,-135.56404]}, + {"t":0.95121, "x":4.59525, "y":6.04478, "heading":-1.48004, "vx":-2.48564, "vy":-1.16869, "omega":2.62156, "ax":6.79252, "ay":-7.46335, "alpha":-7.07763, "fx":[79.71605,121.93483,149.82986,110.67487], "fy":[-155.92716,-125.77725,-90.6069,-135.48645]}, + {"t":0.9819, "x":4.52218, "y":6.00541, "heading":-1.3996, "vx":-2.27722, "vy":-1.39769, "omega":2.40439, "ax":6.90739, "ay":-7.23303, "alpha":-8.53649, "fx":[77.14547,127.4702,157.28205,108.07286], "fy":[-157.27057,-120.22726,-77.03504,-137.59402]}, + {"t":1.01258, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-2.06527, "vy":-1.61963, "omega":2.14245, "ax":7.24385, "ay":-6.93702, "alpha":-8.08334, "fx":[87.23039,133.18243,159.0245,113.4259], "fy":[-151.8483,-113.79229,-73.21839,-133.1276]}, + {"t":1.03929, "x":4.40297, "y":5.91338, "heading":-1.26859, "vx":-1.87178, "vy":-1.80493, "omega":1.92654, "ax":7.76481, "ay":-6.29081, "alpha":-8.5487, "fx":[96.51442,141.7468,165.70724,124.34052], "fy":[-146.05269,-102.85139,-56.29285,-122.8224]}, + {"t":1.066, "x":4.35575, "y":5.86292, "heading":-1.21713, "vx":-1.66437, "vy":-1.97296, "omega":1.69819, "ax":8.40442, "ay":-5.32833, "alpha":-9.02793, "fx":[108.5581,151.3525,171.75897,140.15781], "fy":[-137.23316,-87.99533,-33.18332,-104.1216]}, + {"t":1.09271, "x":4.31429, "y":5.80832, "heading":-1.17177, "vx":-1.43988, "vy":-2.11529, "omega":1.45704, "ax":9.13437, "ay":-3.82651, "alpha":-9.52044, "fx":[124.63209,161.62721,174.84382,160.38899], "fy":[-122.65209,-67.10112,-2.19871,-68.39948]}, + {"t":1.11943, "x":4.27908, "y":5.75045, "heading":-1.13285, "vx":-1.19589, "vy":-2.2175, "omega":1.20274, "ax":9.7265, "ay":-1.47567, "alpha":-10.29159, "fx":[145.83771,170.9269,170.92351,174.09174], "fy":[-96.16818,-37.15055,36.56734,-3.65182]}, + {"t":1.14614, "x":4.25061, "y":5.6907, "heading":-1.10072, "vx":-0.93609, "vy":-2.25692, "omega":0.92784, "ax":9.62861, "ay":1.70184, "alpha":-11.2268, "fx":[168.49805,174.7683,156.08706,155.7662], "fy":[-45.25193,4.62688,78.59996,77.81611]}, + {"t":1.17285, "x":4.22904, "y":5.63102, "heading":-1.07594, "vx":-0.6789, "vy":-2.21146, "omega":0.62796, "ax":8.51973, "ay":5.03535, "alpha":-9.8329, "fx":[170.14207,165.48807,131.01371,113.02863], "fy":[37.95471,56.19719,115.68423,132.763]}, + {"t":1.19956, "x":4.21395, "y":5.57374, "heading":-1.05917, "vx":-0.45132, "vy":-2.07696, "omega":0.36531, "ax":6.58679, "ay":7.64388, "alpha":-6.73979, "fx":[131.38838,138.98863,101.74012,76.0402], "fy":[114.75106,105.963,142.18281,157.18412]}, + {"t":1.22627, "x":4.20424, "y":5.52099, "heading":-1.04941, "vx":-0.27538, "vy":-1.87278, "omega":0.18529, "ax":4.60717, "ay":9.10195, "alpha":-4.34654, "fx":[85.32557,102.97456,74.26101,50.90569], "fy":[152.41486,141.29724,158.35709,167.21723]}, + {"t":1.25298, "x":4.19853, "y":5.47422, "heading":-1.04446, "vx":-0.15232, "vy":-1.62966, "omega":0.06919, "ax":3.03867, "ay":9.79508, "alpha":-2.65021, "fx":[52.62082,68.83792,51.18174,34.10683], "fy":[166.75009,160.81192,167.32036,171.56394]}, + {"t":1.27969, "x":4.19554, "y":5.43418, "heading":-1.04261, "vx":-0.07115, "vy":-1.36802, "omega":-0.0016, "ax":1.87907, "ay":10.10762, "alpha":-1.41432, "fx":[31.2394,41.502,32.65639,22.45205], "fy":[172.1702,170.0155,171.96012,173.56533]}, + {"t":1.3064, "x":4.19431, "y":5.40124, "heading":-1.04266, "vx":-0.02096, "vy":-1.09804, "omega":-0.03938, "ax":1.02363, "ay":10.24068, "alpha":-0.50546, "fx":[16.85905,20.83402,17.94637,14.00737], "fy":[174.25472,173.83117,174.16191,174.51647]}, + {"t":1.33311, "x":4.19412, "y":5.37557, "heading":-1.04371, "vx":0.00638, "vy":-0.8245, "omega":-0.05288, "ax":0.38056, "ay":10.28864, "alpha":0.17504, "fx":[6.73556,5.30176,6.20739,7.64798], "fy":[175.0013,175.04881,175.01614,174.96095]}, + {"t":1.35983, "x":4.19443, "y":5.35721, "heading":-1.04512, "vx":0.01654, "vy":-0.54968, "omega":-0.04821, "ax":-0.11458, "ay":10.29564, "alpha":0.69712, "fx":[-0.69975,-6.53241,-3.26632,2.7026], "fy":[175.17607,175.05004,175.13075,175.14653]}, + {"t":1.38654, "x":4.19483, "y":5.3462, "heading":-1.04641, "vx":0.01348, "vy":-0.27467, "omega":-0.02959, "ax":-0.50482, "ay":10.28288, "alpha":1.10768, "fx":[-6.36118,-15.72988,-11.01114,-1.24548], "fy":[175.09785,174.49868,174.84413,175.1945]}, + {"t":1.41325, "x":4.19501, "y":5.34254, "heading":-1.0472, "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/startToL.traj b/src/main/deploy/choreo/startToL.traj new file mode 100644 index 00000000..0c7aa6fe --- /dev/null +++ b/src/main/deploy/choreo/startToL.traj @@ -0,0 +1,86 @@ +{ + "name":"startToL", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "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":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "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.05456,1.44688], + "samples":[ + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.64413, "ay":2.82147, "alpha":23.98294, "fx":[-160.82196,-43.21148,-141.31688,-174.7473], "fy":[-67.84429,169.81674,103.77169,-13.77469]}, + {"t":0.03102, "x":7.09721, "y":5.07704, "heading":3.14159, "vx":-0.23709, "vy":0.08751, "omega":0.74386, "ax":-7.76433, "ay":3.03317, "alpha":22.74526, "fx":[-165.70153,-46.29545,-141.39475,-174.88417], "fy":[-54.65195,168.97294,103.63552,-11.58314]}, + {"t":0.06203, "x":7.08612, "y":5.08121, "heading":-3.11852, "vx":-0.47791, "vy":0.18159, "omega":1.44934, "ax":-7.9525, "ay":3.34407, "alpha":20.64828, "fx":[-171.48302,-52.07415,-142.57478,-174.94674], "fy":[-31.60985,167.24981,101.9671,-10.0807]}, + {"t":0.09305, "x":7.06747, "y":5.08845, "heading":-3.07357, "vx":-0.72457, "vy":0.28531, "omega":2.08977, "ax":-8.14654, "ay":3.78224, "alpha":17.90866, "fx":[-174.23429,-60.40271,-144.65957,-174.98487], "fy":[2.5824,164.38672,98.93428,-8.56393]}, + {"t":0.12407, "x":7.04108, "y":5.09912, "heading":-3.00875, "vx":-0.97725, "vy":0.40262, "omega":2.64524, "ax":-8.26473, "ay":4.2893, "alpha":15.19585, "fx":[-168.84798,-71.10826,-147.32972,-175.03671], "fy":[42.81475,159.99702,94.83596,-5.80858]}, + {"t":0.15508, "x":7.0068, "y":5.11367, "heading":-2.9267, "vx":-1.23359, "vy":0.53566, "omega":3.11656, "ax":-8.31652, "ay":4.71341, "alpha":13.05545, "fx":[-156.44657,-84.20845,-150.14964,-175.04153], "fy":[76.66999,153.43473,90.18677,0.4032]}, + {"t":0.1861, "x":6.96453, "y":5.13255, "heading":-2.83004, "vx":-1.49154, "vy":0.68185, "omega":3.52149, "ax":-8.41507, "ay":4.99139, "alpha":10.90916, "fx":[-145.50472,-100.23226,-152.49963,-174.31494], "fy":[95.97688,143.36288,85.95292,14.31568]}, + {"t":0.21711, "x":6.91422, "y":5.1561, "heading":-2.72082, "vx":-1.75254, "vy":0.83667, "omega":3.85985, "ax":-8.60109, "ay":5.22453, "alpha":7.16574, "fx":[-142.28598,-120.70363,-153.10515,-169.11357], "fy":[100.82773,126.37894,84.48675,43.77744]}, + {"t":0.24813, "x":6.85573, "y":5.18456, "heading":-2.6011, "vx":-2.01932, "vy":0.99871, "omega":4.08211, "ax":-8.65633, "ay":5.49631, "alpha":-0.42033, "fx":[-147.74245,-148.70035,-146.74147,-145.78233], "fy":[92.75007,91.16527,94.26504,95.78213]}, + {"t":0.27915, "x":6.78893, "y":5.21818, "heading":-2.47449, "vx":-2.28781, "vy":1.16919, "omega":4.06907, "ax":-7.55649, "ay":5.45555, "alpha":-15.52247, "fx":[-158.55358,-173.70793,-83.62116,-98.25215], "fy":[72.75896,3.23907,151.18977,144.00146]}, + {"t":0.31016, "x":6.71434, "y":5.25707, "heading":-2.34828, "vx":-2.52218, "vy":1.3384, "omega":3.58762, "ax":-7.62067, "ay":5.05061, "alpha":-17.05583, "fx":[-163.34837,-172.92744,-76.4881,-105.73789], "fy":[60.54455,-9.58446,154.36602,138.31162]}, + {"t":0.34118, "x":6.63244, "y":5.30101, "heading":-2.237, "vx":-2.75855, "vy":1.49505, "omega":3.05861, "ax":-7.76789, "ay":4.46673, "alpha":-18.36542, "fx":[-167.61875,-170.3519,-76.04522,-114.50272], "fy":[46.05689,-26.39666,153.60422,130.64703]}, + {"t":0.3722, "x":6.54315, "y":5.34953, "heading":-2.14214, "vx":-2.99948, "vy":1.63359, "omega":2.48898, "ax":-7.85641, "ay":3.5891, "alpha":-20.40125, "fx":[-171.11021,-162.24673,-77.38394,-123.80022], "fy":[26.89183,-54.39428,150.78618,120.91427]}, + {"t":0.40321, "x":6.44634, "y":5.40193, "heading":-2.06494, "vx":-3.24316, "vy":1.74492, "omega":1.85621, "ax":-7.35128, "ay":3.00614, "alpha":-24.43011, "fx":[-171.57416,-147.47259,-55.44407,-125.68195], "fy":[12.40133,-81.42507,156.40663,117.15172]}, + {"t":0.43423, "x":6.34221, "y":5.4575, "heading":-2.00736, "vx":-3.47117, "vy":1.83816, "omega":1.09847, "ax":-7.10095, "ay":3.19695, "alpha":-23.31839, "fx":[-167.53309,-145.55687,-48.06949,-121.98108], "fy":[15.65174,-65.6322,151.66981,115.8277]}, + {"t":0.46525, "x":6.23113, "y":5.51605, "heading":-1.97329, "vx":-3.69141, "vy":1.93731, "omega":0.37522, "ax":-3.39846, "ay":1.63877, "alpha":-11.31774, "fx":[-91.01414,-49.76212,-21.89174,-68.55938], "fy":[11.44978,-13.14631,50.8023,62.39394]}, + {"t":0.49626, "x":6.115, "y":5.57692, "heading":-1.96165, "vx":-3.79682, "vy":1.98814, "omega":0.02419, "ax":-0.08369, "ay":-0.11534, "alpha":-0.06316, "fx":[-1.62425,-1.33984,-1.22267,-1.50713], "fy":[-2.04538,-2.16269,-1.8783,-1.761]}, + {"t":0.52728, "x":5.9972, "y":5.63853, "heading":-1.9609, "vx":-3.79942, "vy":1.98456, "omega":0.02223, "ax":-0.20044, "ay":-0.386, "alpha":0.0017, "fx":[-3.40405,-3.4117,-3.41484,-3.40719], "fy":[-6.56345,-6.5603,-6.56793,-6.57108]}, + {"t":0.55829, "x":5.87926, "y":5.6999, "heading":-1.96022, "vx":-3.80563, "vy":1.97259, "omega":0.02228, "ax":-0.73926, "ay":-1.4515, "alpha":0.00873, "fx":[-12.54664,-12.58687,-12.60246,-12.56222], "fy":[-24.67878,-24.66208,-24.7005,-24.7172]}, + {"t":0.58931, "x":5.76086, "y":5.76039, "heading":-1.95952, "vx":-3.82856, "vy":1.92757, "omega":0.02255, "ax":-2.13867, "ay":-4.46287, "alpha":0.03114, "fx":[-36.27245,-36.44343,-36.48395,-36.31312], "fy":[-75.89444,-75.82003,-75.92988,-76.00422]}, + {"t":0.62033, "x":5.64109, "y":5.81802, "heading":-1.95882, "vx":-3.8949, "vy":1.78915, "omega":0.02352, "ax":-3.11273, "ay":-7.55202, "alpha":0.26504, "fx":[-51.88094,-53.83448,-54.00102,-52.07009], "fy":[-128.6358,-127.83211,-128.2844,-129.07872]}, + {"t":0.65134, "x":5.51878, "y":5.86989, "heading":-1.9581, "vx":-3.99144, "vy":1.55491, "omega":0.03174, "ax":-1.42995, "ay":-9.20031, "alpha":5.14418, "fx":[5.36199,-43.37589,-48.79522,-10.48306], "fy":[-158.63234,-151.21336,-154.14559,-161.98758]}, + {"t":0.68236, "x":5.3943, "y":5.91369, "heading":-1.95711, "vx":-4.03579, "vy":1.26955, "omega":0.19129, "ax":1.96896, "ay":-9.0953, "alpha":12.09475, "fx":[111.57017,8.78088,-34.1168,47.73183], "fy":[-126.66652,-161.96641,-166.18831,-164.01252]}, + {"t":0.71338, "x":5.27007, "y":5.94869, "heading":-1.95118, "vx":-3.97472, "vy":0.98745, "omega":0.56643, "ax":3.15655, "ay":-8.8378, "alpha":12.87639, "fx":[132.59736,45.81508,-26.21913,62.57451], "fy":[-109.59353,-160.26046,-170.17481,-161.28505]}, + {"t":0.74439, "x":5.14831, "y":5.97507, "heading":-1.93361, "vx":-3.87682, "vy":0.71333, "omega":0.96581, "ax":3.74537, "ay":-8.73981, "alpha":12.47705, "fx":[137.44168,64.1586,-16.4573,69.6874], "fy":[-105.50855,-157.33904,-172.51535,-159.28396]}, + {"t":0.77541, "x":5.02986, "y":5.99299, "heading":-1.90365, "vx":-3.76065, "vy":0.44226, "omega":1.3528, "ax":4.11545, "ay":-8.71588, "alpha":11.67612, "fx":[138.03758,71.81682,-4.96971,75.12576], "fy":[-105.75548,-156.14546,-173.83481,-157.28254]}, + {"t":0.80643, "x":4.9152, "y":6.00251, "heading":-1.86169, "vx":-3.633, "vy":0.17192, "omega":1.71495, "ax":4.40685, "ay":-8.71237, "alpha":10.6775, "fx":[136.9781,75.0444,7.59261,80.22166], "fy":[-107.71619,-155.90755,-174.115,-155.04084]}, + {"t":0.83744, "x":4.80464, "y":6.00365, "heading":-1.8085, "vx":-3.49632, "vy":-0.09831, "omega":2.04613, "ax":4.68216, "ay":-8.70129, "alpha":9.53197, "fx":[134.93333,77.18089,21.1017,85.35293], "fy":[-110.64158,-155.66387,-173.24966,-152.47076]}, + {"t":0.86846, "x":4.69845, "y":5.99642, "heading":-1.74504, "vx":-3.3511, "vy":-0.36819, "omega":2.34177, "ax":5.061, "ay":-8.64822, "alpha":7.79496, "fx":[130.84684,82.41997,39.68354,91.39397], "fy":[-115.69805,-153.52852,-170.13274,-149.05538]}, + {"t":0.89948, "x":4.59694, "y":5.98084, "heading":-1.67241, "vx":-3.19412, "vy":-0.63642, "omega":2.58355, "ax":5.99502, "ay":-8.30946, "alpha":2.74975, "fx":[117.03869,101.21071,86.51684,103.12775], "fy":[-129.83368,-142.41702,-151.89682,-141.21888]}, + {"t":0.93049, "x":4.50075, "y":5.9571, "heading":-1.59227, "vx":-3.00818, "vy":-0.89415, "omega":2.66883, "ax":6.65556, "ay":-7.82259, "alpha":-1.7646, "fx":[103.90591,113.87853,122.32009,112.73239], "fy":[-140.71171,-132.81343,-125.02968,-133.68509]}, + {"t":0.96151, "x":4.41065, "y":5.92561, "heading":-1.5095, "vx":-2.80175, "vy":-1.13678, "omega":2.6141, "ax":7.04043, "ay":-7.36615, "alpha":-4.95685, "fx":[94.62115,122.72709,143.45682,118.21796], "fy":[-147.23042,-124.85974,-100.22397,-128.87011]}, + {"t":0.99252, "x":4.32714, "y":5.8868, "heading":-1.42842, "vx":-2.58338, "vy":-1.36525, "omega":2.46036, "ax":7.26758, "ay":-6.98619, "alpha":-7.22194, "fx":[88.81652,129.69643,155.98253,119.98238], "fy":[-150.8912,-117.7203,-79.47814,-127.24306]}, + {"t":1.02354, "x":4.25051, "y":5.8411, "heading":-1.3521, "vx":-2.35796, "vy":-1.58194, "omega":2.23636, "ax":7.39863, "ay":-6.68399, "alpha":-8.9277, "fx":[85.6409,135.4397,163.63011,118.68342], "fy":[-152.78355,-111.14761,-62.36555,-128.47414]}, + {"t":1.05456, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-2.12849, "vy":-1.78925, "omega":1.95946, "ax":7.78396, "ay":-6.28799, "alpha":-8.37466, "fx":[97.0894,141.39826,165.37952,125.74447], "fy":[-145.69425,-103.35875,-57.32977,-121.44482]}, + {"t":1.08071, "x":4.12792, "y":5.73987, "heading":-1.23149, "vx":-1.9249, "vy":-1.95372, "omega":1.74042, "ax":8.35372, "ay":-5.45045, "alpha":-8.74523, "fx":[108.12702,150.0745,170.88598,139.28991], "fy":[-137.61704,-90.20927,-37.60221,-105.4138]}, + {"t":1.10687, "x":4.08043, "y":5.6869, "heading":-1.18597, "vx":-1.7064, "vy":-2.09627, "omega":1.51168, "ax":8.99951, "ay":-4.22167, "alpha":-9.10519, "fx":[122.19128,159.29699,174.50021,156.32814], "fy":[-125.16976,-72.55668,-11.95853,-77.55288]}, + {"t":1.13302, "x":4.03888, "y":5.63063, "heading":-1.14643, "vx":-1.47102, "vy":-2.20669, "omega":1.27354, "ax":9.60576, "ay":-2.39934, "alpha":-9.55174, "fx":[140.02812,168.14236,173.7058,171.68839], "fy":[-104.6263,-48.41859,19.96372,-30.16733]}, + {"t":1.15918, "x":4.00369, "y":5.5721, "heading":-1.11312, "vx":-1.21978, "vy":-2.26945, "omega":1.02371, "ax":9.85479, "ay":0.12313, "alpha":-10.20631, "fx":[160.4787,174.1988,165.50757,170.32383], "fy":[-68.87837,-15.65439,56.25877,36.65191]}, + {"t":1.18533, "x":3.97516, "y":5.51278, "heading":-1.08635, "vx":-0.96203, "vy":-2.26623, "omega":0.75676, "ax":9.39224, "ay":3.06073, "alpha":-10.08482, "fx":[174.23235,172.85964,148.57024,143.37483], "fy":[-9.20955,26.20931,92.09635,99.15233]}, + {"t":1.21149, "x":3.95321, "y":5.45455, "heading":-1.06655, "vx":-0.71637, "vy":-2.18617, "omega":0.49299, "ax":8.13434, "ay":5.84711, "alpha":-8.06267, "fx":[161.51639,159.01812,125.17399,107.74266], "fy":[65.90197,72.59189,122.04751,137.28961]}, + {"t":1.23764, "x":3.93726, "y":5.39937, "heading":-1.05366, "vx":-0.50362, "vy":-2.03324, "omega":0.28211, "ax":6.38891, "ay":7.88701, "alpha":-5.59214, "fx":[123.84147,132.67515,99.83888,78.3386], "fy":[123.04976,113.84352,143.57491,156.15476]}, + {"t":1.2638, "x":3.92627, "y":5.34889, "heading":-1.04628, "vx":-0.33652, "vy":-1.82696, "omega":0.13585, "ax":4.70607, "ay":9.07855, "alpha":-3.68163, "fx":[86.01569,100.8828,76.20148,57.09587], "fy":[152.12028,142.85014,157.46529,165.25848]}, + {"t":1.28995, "x":3.91908, "y":5.30421, "heading":-1.04273, "vx":-0.21343, "vy":-1.58951, "omega":0.03956, "ax":3.33454, "ay":9.70832, "alpha":-2.25089, "fx":[57.99386,71.09676,55.8528,41.93508], "fy":[165.00195,159.85629,165.83845,169.84654]}, + {"t":1.31611, "x":3.91464, "y":5.26596, "heading":-1.04169, "vx":-0.12622, "vy":-1.33558, "omega":-0.01931, "ax":2.27472, "ay":10.0294, "alpha":-1.16862, "fx":[38.43479,46.50908,38.96647,30.85921], "fy":[170.72776,168.72964,170.65012,172.28169]}, + {"t":1.34226, "x":3.91211, "y":5.23446, "heading":-1.0422, "vx":-0.06672, "vy":-1.07326, "omega":-0.04988, "ax":1.4611, "ay":10.18832, "alpha":-0.34592, "fx":[24.58105,27.1932,25.11978,22.51765], "fy":[173.34187,172.95622,173.27542,173.62808]}, + {"t":1.36842, "x":3.91087, "y":5.20987, "heading":-1.0435, "vx":-0.0285, "vy":-0.80679, "omega":-0.05893, "ax":0.82946, "ay":10.262, "alpha":0.28743, "fx":[14.45349,12.17387,13.75744,16.05079], "fy":[174.53493,174.7056,174.58339,174.39076]}, + {"t":1.39457, "x":3.91041, "y":5.19228, "heading":-1.04504, "vx":-0.00681, "vy":-0.53838, "omega":-0.05141, "ax":0.33068, "ay":10.29027, "alpha":0.78411, "fx":[6.80676,0.41128,4.3725,10.90879], "fy":[175.04321,175.16823,175.10222,174.82438]}, + {"t":1.42073, "x":3.91034, "y":5.18172, "heading":-1.04639, "vx":0.00184, "vy":-0.26924, "omega":-0.0309, "ax":-0.07031, "ay":10.29406, "alpha":1.18142, "fx":[0.86228,-8.93043,-3.4473,6.73166], "fy":[175.2073,174.97332,175.15037,175.06482]}, + {"t":1.44688, "x":3.91036, "y":5.1782, "heading":-1.0472, "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/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index f917539f..33f3fdc9 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -24,6 +24,7 @@ public class DriveConstants { public static final double kRobotLength = 22.0; public static final double kRobotWidth = 22.0; public static final double kFieldMaxX = 690.0; + public static final double kFieldMaxY public static final double kPOmega = 4.5; public static final double kIOmega = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 79282da4..12a6bd66 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -96,7 +96,7 @@ public void calculateController(SwerveSample desiredState) { holoContInput = desiredState; double xFF = desiredState.vx; double yFF = desiredState.vy; - double rotationFF = desiredState.heading; + double rotationFF = desiredState.omega; Pose2d pose = inputs.poseMeters; double xFeedback = xController.calculate(pose.getX(), desiredState.x); diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 1721e1ac..22eddc6b 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -7,6 +7,8 @@ import frc.robot.constants.RobotStateConstants; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.drive.Swerve; + import java.util.List; import java.util.Optional; import java.util.Set; @@ -30,6 +32,7 @@ public class PathHandler extends MeasurableSubsystem { private boolean runningPath = false; private boolean isServoing = false; private boolean mirrorTrajectory = false; + private boolean mirrorToProcessor = false; private Character startNode = 'a'; PathHandler(DriveSubsystem driveSubsystem) { @@ -42,12 +45,14 @@ public class PathHandler extends MeasurableSubsystem { String[][] pathNames, List NodeNames, List NodeLevels, - Character startNode) { + Character startNode, + boolean mirrorToProcessor) { this.driveSubsystem = driveSubsystem; this.pathNames = pathNames; this.NodeNames = NodeNames; this.NodeLevels = NodeLevels; this.startNode = startNode; + this.mirrorToProcessor = mirrorToProcessor; } public void setPathNames(String[][] pathNames) { @@ -74,6 +79,12 @@ public void setStartNode(Character startNode) { } } + public void setMirrorToProcessor(boolean mirrorToProcessor) { + if (!isHandling) { + this.mirrorToProcessor = mirrorToProcessor; + } + } + public void startPathHandler() { NodeNames.add(0, startNode); isHandling = true; @@ -204,9 +215,17 @@ private boolean shouldTransitionToServoing() { } return currState == PathStates.DRIVE_PLACE && timer.hasElapsed(currPath.getTotalTime() - 1.0) + // && TagAlignSubsystem.canSeeTag(desiredTag) && isCloseEnough; } + private SwerveSample mirrorToProcessor(SwerveSample sample) { + if (mirrorToProcessor) { + sample = new SwerveSample(sample.t, sample.x, sample.y, sample.heading * -1, sample.vx, sample.vy, sample.omega * -1, sample.ax, , getDeviceId(), null, null) + } + return sample; + } + public void periodic() { switch (currState) { case DRIVE_FETCH: From cdc2512fcfacae39cfcb1270012a0874cd5a7a29 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 30 Jan 2025 21:05:22 -0500 Subject: [PATCH 10/48] started making our first full auton command --- src/main/deploy/choreo/ConfusingTestPath.traj | 156 ++++++++++++++++++ .../PathHandler/KillPathHandlerCommand.java | 18 ++ .../PathHandler/StartPathHandlerCommand.java | 23 +++ .../NonProcessorShallowAutonCommand.java | 62 +++++++ .../commands/drive/DriveAutonCommand.java | 52 +++++- .../frc/robot/constants/DriveConstants.java | 4 +- .../subsystems/pathHandler/PathHandler.java | 40 ++++- 7 files changed, 340 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/choreo/ConfusingTestPath.traj create mode 100644 src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java diff --git a/src/main/deploy/choreo/ConfusingTestPath.traj b/src/main/deploy/choreo/ConfusingTestPath.traj new file mode 100644 index 00000000..d0b1a84c --- /dev/null +++ b/src/main/deploy/choreo/ConfusingTestPath.traj @@ -0,0 +1,156 @@ +{ + "name":"ConfusingTestPath", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":0.44632652401924133, "y":4.026000499725342, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.881347179412842, "y":4.290971755981445, "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":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0.44632652401924133 m", "val":0.44632652401924133}, "y":{"exp":"4.026000499725342 m", "val":4.026000499725342}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.881347179412842 m", "val":6.881347179412842}, "y":{"exp":"4.290971755981445 m", "val":4.290971755981445}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"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":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.59078,0.97347,1.38425,1.83631,2.41666], + "samples":[ + {"t":0.0, "x":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.8809, "ay":3.14666, "alpha":-35.43679, "fx":[170.53812,122.70139,-1.88378,-27.30358], "fy":[-4.88774,120.5167,170.50905,-72.04268]}, + {"t":0.02813, "x":0.44786, "y":4.02725, "heading":0.0, "vx":0.10918, "vy":0.08852, "omega":-0.99692, "ax":7.98031, "ay":6.50906, "alpha":-0.83108, "fx":[139.29099,135.43093,132.10036,136.1487], "fy":[106.29731,111.18497,115.1086,110.27786]}, + {"t":0.05626, "x":0.45409, "y":4.03231, "heading":-0.02805, "vx":0.33368, "vy":0.27164, "omega":-1.0203, "ax":7.97974, "ay":6.50862, "alpha":-0.75224, "fx":[138.95862,135.53654,132.43538,136.00213], "fy":[106.69721,111.02347,114.69193,110.42626]}, + {"t":0.0844, "x":0.46664, "y":4.04253, "heading":-0.05675, "vx":0.55817, "vy":0.45474, "omega":-1.04146, "ax":7.97904, "ay":6.50806, "alpha":-0.66, "fx":[138.56114,135.62558,132.82938,135.8684], "fy":[107.17306,110.87607,114.19878,110.55326]}, + {"t":0.11253, "x":0.4855, "y":4.0579, "heading":-0.08605, "vx":0.78264, "vy":0.63783, "omega":-1.06003, "ax":7.97813, "ay":6.50735, "alpha":-0.55061, "fx":[138.08156,135.69158,133.29674,135.75286], "fy":[107.74335,110.74907,113.6092,110.65104]}, + {"t":0.14066, "x":0.51067, "y":4.07842, "heading":-0.11587, "vx":1.00708, "vy":0.82089, "omega":-1.07552, "ax":7.97694, "ay":6.50641, "alpha":-0.41885, "fx":[137.49613,135.72551,133.85734,135.66264], "fy":[108.43345,110.65121,112.89497,110.70908]}, + {"t":0.16879, "x":0.54216, "y":4.10408, "heading":-0.14613, "vx":1.23149, "vy":1.00393, "omega":-1.0873, "ax":7.97532, "ay":6.50514, "alpha":-0.25707, "fx":[136.77038,135.71429,134.53977,135.60751], "fy":[109.27941,110.59501,112.01476,110.71279]}, + {"t":0.19693, "x":0.57996, "y":4.1349, "heading":-0.17671, "vx":1.45586, "vy":1.18694, "omega":-1.09453, "ax":7.97307, "ay":6.50334, "alpha":-0.05378, "fx":[135.85227,135.63808,135.38669,135.60132], "fy":[110.33434,110.5992,110.90538,110.64118]}, + {"t":0.22506, "x":0.62407, "y":4.17087, "heading":-0.20751, "vx":1.68016, "vy":1.36989, "omega":-1.09605, "ax":7.96976, "ay":6.50071, "alpha":0.20928, "fx":[134.65906,135.46501,136.46483,135.6646], "fy":[111.68017,110.69349,109.46482,110.46258]}, + {"t":0.25319, "x":0.67449, "y":4.21198, "heading":-0.23834, "vx":1.90436, "vy":1.55277, "omega":-1.09016, "ax":7.96464, "ay":6.49662, "alpha":0.56285, "fx":[133.05064,135.13987,137.88479,135.82941], "fy":[113.45049,110.92831,107.51721,110.12647]}, + {"t":0.28132, "x":0.73122, "y":4.25823, "heading":-0.26901, "vx":2.12843, "vy":1.73554, "omega":-1.07432, "ax":7.95604, "ay":6.48973, "alpha":1.063, "fx":[130.76974,134.55721,139.84343,136.14938], "fy":[115.87855,111.39795,104.73152,109.54604]}, + {"t":0.30946, "x":0.79424, "y":4.30962, "heading":-0.29923, "vx":2.35225, "vy":1.91811, "omega":-1.04442, "ax":7.9399, "ay":6.47682, "alpha":1.82398, "fx":[127.28874,133.48527,142.7254,136.72251], "fy":[119.40977,112.30343,100.40513,108.55718]}, + {"t":0.33759, "x":0.86356, "y":4.36615, "heading":-0.32861, "vx":2.57562, "vy":2.10031, "omega":-0.99311, "ax":7.90374, "ay":6.44827, "alpha":3.11923, "fx":[121.34116,131.29186,147.37785,137.75088], "fy":[125.00117,114.16886,92.75778,106.80477]}, + {"t":0.36572, "x":0.93915, "y":4.42778, "heading":-0.35655, "vx":2.79797, "vy":2.28172, "omega":-0.90536, "ax":7.79127, "ay":6.36563, "alpha":5.80023, "fx":[109.05196,125.41989,155.90901,139.72845], "fy":[135.04049,118.93371,75.78766,103.34826]}, + {"t":0.39385, "x":1.02094, "y":4.49449, "heading":-0.38202, "vx":3.01715, "vy":2.4608, "omega":-0.74218, "ax":6.95096, "ay":6.01786, "alpha":14.54572, "fx":[73.31148,85.53153,169.98006,144.11216], "fy":[155.42203,142.02633,17.2258,94.77385]}, + {"t":0.42198, "x":1.10857, "y":4.5661, "heading":-0.4029, "vx":3.2127, "vy":2.6301, "omega":-0.33298, "ax":3.93479, "ay":2.97269, "alpha":11.79458, "fx":[47.54079,31.46021,91.94868,96.76863], "fy":[92.25735,43.75194,6.6639,59.58494]}, + {"t":0.45012, "x":1.20051, "y":4.64127, "heading":-0.41227, "vx":3.32339, "vy":2.71372, "omega":-0.00117, "ax":0.16522, "ay":-0.19216, "alpha":0.00989, "fx":[2.80365,2.89396,2.88939,2.65414], "fy":[-3.08565,-3.35371,-3.39413,-3.24094]}, + {"t":0.47825, "x":1.29407, "y":4.71754, "heading":-0.4123, "vx":3.32804, "vy":2.70832, "omega":-0.00089, "ax":0.40738, "ay":-0.50279, "alpha":0.0, "fx":[6.93047,6.92926,6.92812,6.92996], "fy":[-8.55561,-8.56174,-8.54854,-8.54302]}, + {"t":0.50638, "x":1.38786, "y":4.79353, "heading":-0.41233, "vx":3.3395, "vy":2.69417, "omega":-0.00089, "ax":1.22951, "ay":-1.54456, "alpha":-0.00004, "fx":[20.90672,20.92902,20.91978,20.89904], "fy":[-26.25537,-26.26409,-26.28724,-26.28358]}, + {"t":0.53451, "x":1.48229, "y":4.86871, "heading":-0.41235, "vx":3.37409, "vy":2.65072, "omega":-0.00089, "ax":2.98669, "ay":-3.94193, "alpha":-0.01143, "fx":[51.01036,51.10239,50.45754,50.64065], "fy":[-66.79105,-67.10557,-67.27246,-67.03523]}, + {"t":0.56265, "x":1.57839, "y":4.94172, "heading":-0.41238, "vx":3.45811, "vy":2.53983, "omega":-0.00121, "ax":2.20716, "ay":-7.56513, "alpha":-6.19223, "fx":[44.73442,70.80412,26.96516,7.66887], "fy":[-136.37986,-117.39919,-122.93228,-138.01146]}, + {"t":0.59078, "x":1.67655, "y":5.01018, "heading":-0.41241, "vx":3.52021, "vy":2.327, "omega":-0.17541, "ax":-1.91443, "ay":-7.4438, "alpha":-13.12628, "fx":[-6.19702,34.98789,-81.66628,-77.3805], "fy":[-150.71955,-130.59442,-97.34976,-127.80408]}, + {"t":0.609, "x":1.74038, "y":5.05135, "heading":-0.41561, "vx":3.48532, "vy":2.19135, "omega":-0.41462, "ax":-2.54991, "ay":-6.42087, "alpha":-12.85895, "fx":[-18.05738,14.62928,-86.49877,-83.56581], "fy":[-138.65212,-114.61071,-72.16244,-111.44317]}, + {"t":0.62723, "x":1.80348, "y":5.09022, "heading":-0.42316, "vx":3.43885, "vy":2.07434, "omega":-0.64896, "ax":-3.13784, "ay":-5.1062, "alpha":-12.34609, "fx":[-31.10036,-5.74901,-87.45565,-89.19024], "fy":[-121.8606,-88.38191,-45.8808,-91.29638]}, + {"t":0.64545, "x":1.86562, "y":5.12717, "heading":-0.43499, "vx":3.38167, "vy":1.98128, "omega":-0.87395, "ax":-3.46691, "ay":-3.72004, "alpha":-11.40829, "fx":[-41.52019,-19.92546,-83.47447,-90.96421], "fy":[-100.5158,-59.06728,-23.54786,-69.9764]}, + {"t":0.66367, "x":1.92667, "y":5.16266, "heading":-0.45092, "vx":3.31849, "vy":1.91349, "omega":-1.08185, "ax":-3.48151, "ay":-2.44876, "alpha":-10.08136, "fx":[-46.49432,-26.52062,-75.98515,-87.87813], "fy":[-76.52644,-34.37081,-6.32429,-49.38908]}, + {"t":0.6819, "x":1.98657, "y":5.19713, "heading":-0.47063, "vx":3.25504, "vy":1.86887, "omega":-1.26557, "ax":-3.24826, "ay":-1.38826, "alpha":-8.55158, "fx":[-46.16492,-27.78958,-66.54826,-80.50524], "fy":[-53.53144,-15.96759,6.09731,-31.0539]}, + {"t":0.70012, "x":2.04535, "y":5.23095, "heading":-0.49369, "vx":3.19585, "vy":1.84357, "omega":-1.42141, "ax":-2.86715, "ay":-0.55895, "alpha":-7.00899, "fx":[-42.33794,-26.05359,-56.3431,-70.34272], "fy":[-33.88006,-2.90948,14.56684,-15.80733]}, + {"t":0.71834, "x":2.10311, "y":5.26446, "heading":-0.5196, "vx":3.1436, "vy":1.83338, "omega":-1.54914, "ax":-2.4157, "ay":0.06599, "alpha":-5.56134, "fx":[-36.62206,-22.7878,-46.11082,-58.84075], "fy":[-18.02266,6.1999,20.03957,-3.72688]}, + {"t":0.73657, "x":2.16, "y":5.29788, "heading":-0.54783, "vx":3.09957, "vy":1.83458, "omega":-1.65049, "ax":-1.94158, "ay":0.5249, "alpha":-4.25088, "fx":[-30.03281,-18.81134,-36.26105,-46.99765], "fy":[-5.57194,12.4962,23.2912,5.49794]}, + {"t":0.75479, "x":2.21616, "y":5.3314, "heading":-0.57791, "vx":3.06419, "vy":1.84415, "omega":-1.72795, "ax":-1.47006, "ay":0.84935, "alpha":-3.08508, "fx":[-23.12488,-14.54102,-26.97641,-35.37908], "fy":[3.99528,16.72564,24.83258,12.23551]}, + {"t":0.77301, "x":2.27176, "y":5.36515, "heading":-0.6094, "vx":3.0374, "vy":1.85963, "omega":-1.78418, "ax":-1.01138, "ay":1.05748, "alpha":-2.05698, "fx":[-16.14895,-10.14299,-18.28157,-24.23939], "fy":[11.04905,19.26724,24.90353,16.72988]}, + {"t":0.79124, "x":2.32694, "y":5.39921, "heading":-0.64191, "vx":3.01897, "vy":1.8789, "omega":-1.82166, "ax":-0.56543, "ay":1.15021, "alpha":-1.15856, "fx":[-9.1517,-5.60227,-10.08497,-13.63261], "fy":[15.66751,20.14342,23.4611,18.98661]}, + {"t":0.80946, "x":2.38187, "y":5.43364, "heading":-0.67511, "vx":3.00867, "vy":1.89986, "omega":-1.84277, "ax":-0.12456, "ay":1.10543, "alpha":-0.39314, "fx":[-2.03089,-0.73737,-2.2034,-3.50309], "fy":[17.49039,18.95505,20.12036,18.64642]}, + {"t":0.82769, "x":2.43668, "y":5.46845, "heading":-0.70869, "vx":3.0064, "vy":1.92, "omega":-1.84994, "ax":0.32462, "ay":0.86678, "alpha":0.20736, "fx":[5.4272,4.82213,5.61775,6.21978], "fy":[15.46767,14.69927,14.02163,14.78599]}, + {"t":0.84591, "x":2.49152, "y":5.50358, "heading":-0.7424, "vx":3.01231, "vy":1.9358, "omega":-1.84616, "ax":0.79771, "ay":0.32495, "alpha":0.55448, "fx":[13.47638,11.6644,13.66213,15.47202], "fy":[7.43875,5.44536,3.61579,5.609]}, + {"t":0.86413, "x":2.54654, "y":5.53891, "heading":-0.77605, "vx":3.02685, "vy":1.94172, "omega":-1.83606, "ax":1.30136, "ay":-0.70172, "alpha":0.465, "fx":[22.16755,20.52465,22.10693,23.74405], "fy":[-10.34157,-11.99917,-13.52773,-11.87583]}, + {"t":0.88236, "x":2.60192, "y":5.57418, "heading":-0.80951, "vx":3.05057, "vy":1.92893, "omega":-1.82758, "ax":1.79878, "ay":-2.43423, "alpha":-0.34366, "fx":[30.57824,31.7792,30.62848,29.40068], "fy":[-42.57533,-41.44075,-40.22082,-41.3852]}, + {"t":0.90058, "x":2.65781, "y":5.60893, "heading":-0.84281, "vx":3.08335, "vy":1.88457, "omega":-1.83384, "ax":2.13315, "ay":-4.82202, "alpha":-2.01359, "fx":[35.05291,43.91666,37.66183,28.50584], "fy":[-88.07592,-81.57139,-75.78998,-82.64712]}, + {"t":0.9188, "x":2.71436, "y":5.64247, "heading":-0.87623, "vx":3.12222, "vy":1.7967, "omega":-1.87054, "ax":2.06959, "ay":-6.92852, "alpha":-3.88623, "fx":[30.67974,52.57444,40.84004,16.71835], "fy":[-126.34344,-115.5237,-108.90087,-120.64026]}, + {"t":0.93703, "x":2.7716, "y":5.67406, "heading":-0.91032, "vx":3.15993, "vy":1.67044, "omega":-1.94136, "ax":1.72072, "ay":-8.19001, "alpha":-5.1744, "fx":[22.06307,55.22142,39.41146,0.38018], "fy":[-147.31706,-135.56394,-131.59017,-142.76768]}, + {"t":0.95525, "x":2.82947, "y":5.70315, "heading":-0.9457, "vx":3.19129, "vy":1.52118, "omega":-2.03566, "ax":1.34433, "ay":-8.90304, "alpha":-5.79542, "fx":[14.43111,53.87246,35.60105,-12.43761], "fy":[-158.2164,-147.47839,-146.01362,-154.04449]}, + {"t":0.97347, "x":2.88785, "y":5.72939, "heading":-0.98279, "vx":3.21579, "vy":1.35894, "omega":-2.14127, "ax":2.5922, "ay":-8.78621, "alpha":-1.54169, "fx":[40.33909,52.06789,48.25967,35.70338], "fy":[-151.67009,-147.60414,-147.21852,-151.31096]}, + {"t":0.99215, "x":2.94835, "y":5.75323, "heading":-1.02277, "vx":3.26419, "vy":1.19489, "omega":-2.17006, "ax":3.36586, "ay":-7.95222, "alpha":1.92676, "fx":[63.25421,48.10011,51.95567,65.69908], "fy":[-131.06513,-137.84694,-139.23505,-132.91287]}, + {"t":1.01082, "x":3.00988, "y":5.77415, "heading":-1.06329, "vx":3.32704, "vy":1.0464, "omega":-2.13408, "ax":4.17979, "ay":-6.6265, "alpha":5.50772, "fx":[88.74894,48.99539,57.25415,89.38951], "fy":[-95.13783,-119.51285,-127.83867,-108.37022]}, + {"t":1.02949, "x":3.07273, "y":5.79254, "heading":-1.10314, "vx":3.40508, "vy":0.92268, "omega":-2.03124, "ax":4.70253, "ay":-5.05275, "alpha":8.17884, "fx":[102.45304,52.79103,62.58878,102.12213], "fy":[-54.94026,-90.98088,-113.17738,-84.6848]}, + {"t":1.04816, "x":3.13713, "y":5.80888, "heading":-1.14107, "vx":3.49289, "vy":0.82833, "omega":-1.87853, "ax":4.79262, "ay":-3.65392, "alpha":9.46598, "fx":[102.75067,53.13018,64.99979,105.20396], "fy":[-25.96599,-59.88772,-96.67962,-66.07493]}, + {"t":1.06683, "x":3.20318, "y":5.82371, "heading":-1.17614, "vx":3.58237, "vy":0.76011, "omega":-1.70178, "ax":4.52613, "ay":-2.63566, "alpha":9.57639, "fx":[95.40029,48.97061,62.39946,101.18204], "fy":[-9.81459,-36.72489,-80.11951,-52.66853]}, + {"t":1.0855, "x":3.27086, "y":5.83745, "heading":-1.20792, "vx":3.66688, "vy":0.7109, "omega":-1.52298, "ax":4.02272, "ay":-1.95149, "alpha":8.87096, "fx":[84.02533,42.34463,55.46689,91.86412], "fy":[-2.36947,-22.61676,-64.87045,-42.92059]}, + {"t":1.10418, "x":3.34003, "y":5.85038, "heading":-1.23635, "vx":3.74199, "vy":0.67446, "omega":-1.35734, "ax":3.39313, "ay":-1.4945, "alpha":7.67165, "fx":[70.68494,34.95724,46.31392,78.90853], "fy":[0.08615,-14.80437,-51.65336,-35.31224]}, + {"t":1.12285, "x":3.41049, "y":5.86271, "heading":-1.2617, "vx":3.80535, "vy":0.64655, "omega":-1.2141, "ax":2.73499, "ay":-1.17766, "alpha":6.25854, "fx":[57.01342,27.8519,36.85822,64.36223], "fy":[0.07092,-10.64864,-40.58812,-28.96048]}, + {"t":1.14152, "x":3.48202, "y":5.87458, "heading":-1.28437, "vx":3.85641, "vy":0.62457, "omega":-1.09724, "ax":2.12311, "ay":-0.94657, "alpha":4.85486, "fx":[44.32253,21.61437,28.30975,50.20723], "fy":[-0.91044,-8.43429,-31.53438,-23.52432]}, + {"t":1.16019, "x":3.55439, "y":5.88608, "heading":-1.30485, "vx":3.89606, "vy":0.60689, "omega":-1.00659, "ax":1.6014, "ay":-0.77408, "alpha":3.60458, "fx":[33.43785,16.49944,21.2289,37.79127], "fy":[-2.11287,-7.24733,-24.33866,-18.96879]}, + {"t":1.17886, "x":3.62742, "y":5.89727, "heading":-1.32365, "vx":3.92596, "vy":0.59244, "omega":-0.93929, "ax":1.18515, "ay":-0.65167, "alpha":2.57287, "fx":[24.67518,12.53078,15.72719,27.70294], "fy":[-3.31907,-6.71878,-18.90295,-15.39796]}, + {"t":1.19753, "x":3.70093, "y":5.90822, "heading":-1.34119, "vx":3.94809, "vy":0.58027, "omega":-0.89125, "ax":0.86885, "ay":-0.5824, "alpha":1.76323, "fx":[17.943,9.58806,11.65209,19.93273], "fy":[-4.65376,-6.82449,-15.18066,-12.96707]}, + {"t":1.21621, "x":3.7748, "y":5.91895, "heading":-1.35783, "vx":3.96431, "vy":0.5694, "omega":-0.85833, "ax":0.63349, "ay":-0.57709, "alpha":1.13534, "fx":[12.8585,7.46537,8.70895,14.06937], "fy":[-6.47001,-7.77257,-13.16934,-11.85257]}, + {"t":1.23488, "x":3.84893, "y":5.92949, "heading":-1.37385, "vx":3.97614, "vy":0.55862, "omega":-0.83713, "ax":0.44993, "ay":-0.65351, "alpha":0.61357, "fx":[8.79956,5.89225,6.5147,9.40643], "fy":[-9.31723,-9.97345,-12.91561,-12.25787]}, + {"t":1.25355, "x":3.92325, "y":5.9398, "heading":-1.38948, "vx":3.98454, "vy":0.54642, "omega":-0.82567, "ax":0.27766, "ay":-0.83787, "alpha":0.0827, "fx":[4.86108,4.51576,4.59116,4.92346], "fy":[-13.97811,-14.07569,-14.52375,-14.43004]}, + {"t":1.27222, "x":3.99769, "y":5.94986, "heading":-1.4049, "vx":3.98972, "vy":0.53077, "omega":-0.82413, "ax":0.05927, "ay":-1.16697, "alpha":-0.62764, "fx":[-0.27881,2.84876,2.31164,-0.84871], "fy":[-21.53709,-21.0409,-18.15748,-18.6636]}, + {"t":1.29089, "x":4.0722, "y":5.95957, "heading":-1.42029, "vx":3.99083, "vy":0.50899, "omega":-0.83585, "ax":-0.28604, "ay":-1.69024, "alpha":-1.7586, "fx":[-8.50083,0.21719,-1.16504,-10.01314], "fy":[-33.41005,-32.23262,-24.06672,-25.29254]}, + {"t":1.30956, "x":4.14666, "y":5.96877, "heading":-1.4359, "vx":3.98549, "vy":0.47743, "omega":-0.86869, "ax":-0.85782, "ay":-2.47698, "alpha":-3.59884, "fx":[-22.05437,-4.14588,-6.85633,-25.30849], "fy":[-51.23463,-49.57991,-32.89062,-34.8255]}, + {"t":1.32824, "x":4.22093, "y":5.97726, "heading":-1.45212, "vx":3.96947, "vy":0.43118, "omega":-0.93588, "ax":-1.7148, "ay":-3.63575, "alpha":-6.27301, "fx":[-41.80693,-10.33095,-15.42641,-49.10907], "fy":[-76.04683,-75.74538,-46.98485,-48.59516]}, + {"t":1.34691, "x":4.29475, "y":5.98467, "heading":-1.46959, "vx":3.93745, "vy":0.36329, "omega":-1.05301, "ax":-2.66143, "ay":-5.1915, "alpha":-8.97251, "fx":[-61.82453,-15.25038,-25.22367,-78.78202], "fy":[-103.58396,-109.98086,-71.9048,-67.75378]}, + {"t":1.36558, "x":4.3678, "y":5.99055, "heading":-1.48925, "vx":3.88776, "vy":0.26636, "omega":-1.22054, "ax":-3.25225, "ay":-6.69115, "alpha":-10.13525, "fx":[-73.18074,-15.25446,-31.46564,-101.37852], "fy":[-124.41537,-137.94193,-106.88365,-86.01711]}, + {"t":1.38425, "x":4.43982, "y":5.99436, "heading":-1.51204, "vx":3.82704, "vy":0.14142, "omega":-1.40978, "ax":-1.38672, "ay":-7.60048, "alpha":-3.76798, "fx":[-33.88269,-8.08295,-11.44207,-40.94314], "fy":[-132.47222,-135.08648,-126.60445,-122.96501]}, + {"t":1.40309, "x":4.51166, "y":5.99568, "heading":-1.5386, "vx":3.80092, "vy":-0.00174, "omega":-1.48075, "ax":-0.19573, "ay":-6.39241, "alpha":-0.03412, "fx":[-3.53949,-2.88258,-3.02918,-3.86576], "fy":[-108.47649,-108.57111,-108.99505,-108.88967]}, + {"t":1.42192, "x":4.58322, "y":5.99451, "heading":-1.56649, "vx":3.79723, "vy":-0.12214, "omega":-1.4814, "ax":0.79428, "ay":-4.96501, "alpha":2.70652, "fx":[21.33277,6.28782,6.02922,20.3919], "fy":[-78.29841,-78.94779,-90.6184,-89.94886]}, + {"t":1.44076, "x":4.65489, "y":5.99133, "heading":-1.59439, "vx":3.81219, "vy":-0.21566, "omega":-1.43042, "ax":1.19998, "ay":-4.01782, "alpha":3.80456, "fx":[31.24413,11.02316,10.04493,29.33278], "fy":[-59.44,-59.94733,-77.34539,-76.63503]}, + {"t":1.45959, "x":4.7269, "y":5.98655, "heading":-1.62133, "vx":3.83479, "vy":-0.29134, "omega":-1.35876, "ax":1.11343, "ay":-3.65067, "alpha":3.69887, "fx":[29.46327,10.16877,8.80808,27.31628], "fy":[-53.59668,-53.50892,-70.70828,-70.57347]}, + {"t":1.47843, "x":4.79933, "y":5.98042, "heading":-1.64692, "vx":3.85576, "vy":-0.3601, "omega":-1.28909, "ax":0.68709, "ay":-3.66952, "alpha":2.78035, "fx":[19.64594,5.22164,3.97087,17.91067], "fy":[-56.26343,-55.74121,-68.62238,-69.04284]}, + {"t":1.49726, "x":4.87208, "y":5.97298, "heading":-1.67121, "vx":3.86871, "vy":-0.42922, "omega":-1.23672, "ax":0.02761, "ay":-3.82872, "alpha":1.24459, "fx":[3.98814,-2.32128,-2.96084,3.17245], "fy":[-62.4071,-61.99465,-67.84855,-68.25174]}, + {"t":1.5161, "x":4.94495, "y":5.96422, "heading":-1.6945, "vx":3.86923, "vy":-0.50134, "omega":-1.21327, "ax":-0.78585, "ay":-3.94077, "alpha":-0.77931, "fx":[-15.70614,-11.25262,-10.93767,-15.57198], "fy":[-68.13295,-68.9096,-65.94388,-65.13938]}, + {"t":1.53494, "x":5.01769, "y":5.95408, "heading":-1.71735, "vx":3.85442, "vy":-0.57556, "omega":-1.22795, "ax":-1.69781, "ay":-3.99188, "alpha":-3.12207, "fx":[-37.59542,-21.04239,-19.66934,-37.21023], "fy":[-72.57035,-76.24789,-63.26686,-59.51764]}, + {"t":1.55377, "x":5.08999, "y":5.94253, "heading":-1.74048, "vx":3.82244, "vy":-0.65075, "omega":-1.28676, "ax":-2.6355, "ay":-4.04215, "alpha":-5.48771, "fx":[-59.1775,-31.27072,-29.0395,-59.82864], "fy":[-76.11565,-84.68082,-61.39202,-52.8348]}, + {"t":1.57261, "x":5.16152, "y":5.92955, "heading":-1.76472, "vx":3.7728, "vy":-0.72689, "omega":-1.39012, "ax":-3.49432, "ay":-4.07746, "alpha":-7.51358, "fx":[-77.62929,-41.12754,-38.53713,-80.45543], "fy":[-78.19075,-93.02339,-60.45681,-45.75482]}, + {"t":1.59144, "x":5.23197, "y":5.91514, "heading":-1.7909, "vx":3.70699, "vy":-0.80369, "omega":-1.53165, "ax":-4.19188, "ay":-4.03513, "alpha":-8.9543, "fx":[-91.502,-49.93211,-47.41225,-96.36406], "fy":[-77.98439,-98.98019,-59.25448,-38.32624]}, + {"t":1.61028, "x":5.30105, "y":5.89929, "heading":-1.81975, "vx":3.62803, "vy":-0.87969, "omega":-1.70031, "ax":-4.69521, "ay":-3.94856, "alpha":-9.72663, "fx":[-100.84442,-57.25466,-54.81275,-106.54496], "fy":[-76.26048,-101.99818,-58.22739,-32.16961]}, + {"t":1.62911, "x":5.36855, "y":5.88202, "heading":-1.85178, "vx":3.53959, "vy":-0.95407, "omega":-1.88351, "ax":-4.9797, "ay":-4.0252, "alpha":-9.73266, "fx":[-105.53259,-62.21847,-59.80061,-111.2613], "fy":[-76.06453,-104.25865,-61.70498,-31.84169]}, + {"t":1.64795, "x":5.43434, "y":5.86333, "heading":-1.88726, "vx":3.4458, "vy":-1.02988, "omega":-2.06683, "ax":-4.98359, "ay":-4.54424, "alpha":-8.71521, "fx":[-104.29786,-63.55722,-61.34222,-109.88015], "fy":[-81.56466,-108.70035,-75.11863,-43.80128]}, + {"t":1.66678, "x":5.49836, "y":5.84313, "heading":-1.92619, "vx":3.35193, "vy":-1.11548, "omega":-2.23099, "ax":-4.56431, "ay":-5.68868, "alpha":-6.16443, "fx":[-93.70859,-60.61316,-58.55446,-97.67452], "fy":[-96.63042,-116.83541,-99.20537,-74.38011]}, + {"t":1.68562, "x":5.56068, "y":5.82111, "heading":-1.96821, "vx":3.26596, "vy":-1.22263, "omega":-2.3471, "ax":-3.60915, "ay":-7.19368, "alpha":-1.79344, "fx":[-67.80018,-55.59074,-54.48868,-67.68274], "fy":[-121.13748,-127.00123,-123.85848,-117.45264]}, + {"t":1.70446, "x":5.62156, "y":5.7968, "heading":-2.01242, "vx":3.19797, "vy":-1.35813, "omega":-2.38088, "ax":-2.44436, "ay":-8.34793, "alpha":3.15359, "fx":[-25.9439,-52.21233,-55.55411,-32.60133], "fy":[-144.49956,-136.24114,-139.8105,-147.43259]}, + {"t":1.72329, "x":5.68136, "y":5.76974, "heading":-2.05726, "vx":3.15193, "vy":-1.51536, "omega":-2.32148, "ax":-1.57702, "ay":-8.92499, "alpha":7.02392, "fx":[14.86927,-50.25235,-60.10829,-11.80712], "fy":[-155.0377,-143.82975,-147.93327,-160.4454]}, + {"t":1.74213, "x":5.74045, "y":5.73961, "heading":-2.10099, "vx":3.12223, "vy":-1.68347, "omega":-2.18918, "ax":-1.13802, "ay":-9.16654, "alpha":9.50396, "fx":[41.58333,-49.74058,-66.17762,-3.09494], "fy":[-156.87274,-149.38126,-151.18387,-166.24315]}, + {"t":1.76096, "x":5.79906, "y":5.70628, "heading":-2.14222, "vx":3.10079, "vy":-1.85613, "omega":-2.01017, "ax":-0.99548, "ay":-9.26287, "alpha":11.0657, "fx":[56.81343,-51.02334,-72.90946,-0.61211], "fy":[-156.41262,-152.92728,-151.6017,-169.29335]}, + {"t":1.7798, "x":5.85729, "y":5.66967, "heading":-2.18009, "vx":3.08204, "vy":-2.0306, "omega":-1.80174, "ax":-1.02391, "ay":-9.29123, "alpha":12.12887, "fx":[65.30433,-53.96887,-79.74738,-1.25403], "fy":[-155.85491,-154.83888,-150.37864,-171.09227]}, + {"t":1.79863, "x":5.91516, "y":5.62978, "heading":-2.21402, "vx":3.06276, "vy":-2.20561, "omega":-1.57328, "ax":-1.16272, "ay":-9.27816, "alpha":12.94176, "fx":[69.82212,-58.78106,-86.54269,-3.60835], "fy":[-155.70407,-155.27381,-148.09518,-172.20264]}, + {"t":1.81747, "x":5.97264, "y":5.58659, "heading":-2.24366, "vx":3.04086, "vy":-2.38037, "omega":-1.32952, "ax":-1.4751, "ay":-9.18461, "alpha":13.94954, "fx":[72.16324,-69.84802,-94.94836,-7.73122], "fy":[-155.85529,-152.24336,-143.96549,-172.84655]}, + {"t":1.83631, "x":6.02965, "y":5.54012, "heading":-2.2687, "vx":3.01307, "vy":-2.55337, "omega":-1.06677, "ax":-2.45366, "ay":-9.10962, "alpha":13.36188, "fx":[49.49958,-90.50739,-103.44648,-22.48996], "fy":[-165.13066,-143.84484,-138.81292,-172.01998]}, + {"t":1.86154, "x":6.1049, "y":5.47279, "heading":-2.29562, "vx":2.95116, "vy":-2.78323, "omega":-0.72961, "ax":-3.1113, "ay":-8.91061, "alpha":12.83546, "fx":[31.77162,-100.70858,-109.20124,-33.55108], "fy":[-168.25539,-135.24282,-133.30777,-169.46165]}, + {"t":1.88677, "x":6.17837, "y":5.39973, "heading":-2.31403, "vx":2.87265, "vy":-3.00806, "omega":-0.40574, "ax":-4.35475, "ay":-8.51546, "alpha":10.42798, "fx":[-12.00648,-113.52216,-115.8097,-54.95375], "fy":[-168.61969,-122.89357,-125.65389,-162.21497]}, + {"t":1.912, "x":6.24947, "y":5.32112, "heading":-2.32426, "vx":2.76277, "vy":-3.22293, "omega":-0.14262, "ax":-6.66597, "ay":-7.11689, "alpha":4.04608, "fx":[-97.63638,-127.56448,-126.31687,-102.02722], "fy":[-134.25274,-106.22296,-110.49661,-133.25255]}, + {"t":1.93724, "x":6.31706, "y":5.23753, "heading":-2.32786, "vx":2.59457, "vy":-3.40251, "omega":-0.04052, "ax":-8.50847, "ay":-4.31058, "alpha":-3.98424, "fx":[-151.08395,-135.43705,-137.96188,-154.42387], "fy":[-65.7737,-92.72043,-83.83244,-50.9602]}, + {"t":1.96247, "x":6.37982, "y":5.1503, "heading":-2.32889, "vx":2.37988, "vy":-3.51128, "omega":-0.14106, "ax":-7.76322, "ay":3.26923, "alpha":-18.45892, "fx":[-165.97588,-155.2463,-91.53446,-115.44412], "fy":[33.10339,-56.30363,123.37511,122.25985]}, + {"t":1.9877, "x":6.4374, "y":5.06274, "heading":-2.33245, "vx":2.18399, "vy":-3.42878, "omega":-0.60682, "ax":-6.21224, "ay":5.68436, "alpha":-21.21455, "fx":[-155.93513,-167.56065,-15.17424,-84.00385], "fy":[72.02066,-1.53461,166.49396,149.77767]}, + {"t":2.01293, "x":6.49053, "y":4.97804, "heading":-2.34776, "vx":2.02724, "vy":-3.28535, "omega":-1.14212, "ax":-5.61628, "ay":6.28812, "alpha":-21.92083, "fx":[-151.12975,-168.81868,9.3651,-71.54168], "fy":[84.12236,15.97296,170.17763,157.56327]}, + {"t":2.03817, "x":6.5399, "y":4.89714, "heading":-2.37658, "vx":1.88553, "vy":-3.12669, "omega":-1.69524, "ax":-5.33112, "ay":6.48637, "alpha":-22.56796, "fx":[-148.72365,-169.80966,20.70744,-64.89725], "fy":[89.58995,20.07642,170.54299,161.11602]}, + {"t":2.0634, "x":6.58578, "y":4.82031, "heading":-2.41935, "vx":1.75101, "vy":-2.96302, "omega":-2.26469, "ax":-5.27105, "ay":6.76998, "alpha":-21.54256, "fx":[-145.53975,-169.31742,17.80347,-61.58223], "fy":[95.40428,30.78229,171.62749,162.80775]}, + {"t":2.08863, "x":6.62828, "y":4.7477, "heading":-2.4765, "vx":1.61801, "vy":-2.79219, "omega":-2.80827, "ax":-5.66546, "ay":8.28546, "alpha":-6.97161, "fx":[-120.68745,-125.80795,-62.20002,-76.77585], "fy":[125.59111,119.62628,162.14602,156.37008]}, + {"t":2.11386, "x":6.6673, "y":4.67988, "heading":-2.54736, "vx":1.47505, "vy":-2.58313, "omega":-2.98418, "ax":-5.54541, "ay":8.61761, "alpha":-0.31006, "fx":[-95.60968,-95.56402,-93.02014,-93.1095], "fy":[145.76828,145.77439,147.41122,147.37834]}, + {"t":2.1391, "x":6.70276, "y":4.61745, "heading":-2.62265, "vx":1.33513, "vy":-2.36569, "omega":-2.992, "ax":-5.3805, "ay":8.67735, "alpha":3.61477, "fx":[-75.76767,-77.81059,-104.4713,-108.03368], "fy":[157.13446,156.33905,139.94234,136.98104]}, + {"t":2.16433, "x":6.73473, "y":4.56052, "heading":-2.69815, "vx":1.19937, "vy":-2.14673, "omega":-2.90079, "ax":-5.22853, "ay":8.65416, "alpha":6.40572, "fx":[-61.12234,-65.30155,-108.84876,-120.47029], "fy":[163.48867,162.15933,136.81192,126.35913]}, + {"t":2.18956, "x":6.76333, "y":4.50911, "heading":-2.77134, "vx":1.06744, "vy":-1.92837, "omega":-2.73916, "ax":-5.08997, "ay":8.60675, "alpha":8.49277, "fx":[-50.71543,-55.43754,-110.07706,-130.08579], "fy":[167.08334,165.91192,135.98559,116.61283]}, + {"t":2.21479, "x":6.78865, "y":4.46319, "heading":-2.84046, "vx":0.939, "vy":-1.71119, "omega":-2.52486, "ax":-4.96311, "ay":8.5598, "alpha":10.04709, "fx":[-43.60568,-47.18198,-109.7338,-137.16261], "fy":[169.13263,168.52744,136.37297,108.3662]}, + {"t":2.24003, "x":6.81076, "y":4.42274, "heading":-2.90417, "vx":0.81377, "vy":-1.49521, "omega":-2.27135, "ax":-4.84882, "ay":8.52212, "alpha":11.18408, "fx":[-39.01379,-40.0666,-108.58636,-142.24169], "fy":[170.29199,170.41828,137.368,101.75677]}, + {"t":2.26526, "x":6.82975, "y":4.38772, "heading":-2.96148, "vx":0.69142, "vy":-1.28017, "omega":-1.98915, "ax":-4.74842, "ay":8.49463, "alpha":12.00512, "fx":[-36.28863,-33.85622,-107.06473,-145.86745], "fy":[170.92619,171.80461,138.61694,96.61727]}, + {"t":2.29049, "x":6.84569, "y":4.35812, "heading":-3.01167, "vx":0.57161, "vy":-1.06583, "omega":-1.68623, "ax":-4.66203, "ay":8.47506, "alpha":12.60051, "fx":[-34.85731,-28.42069,-105.43553,-148.48562], "fy":[171.25075,172.8222,139.90634,92.65409]}, + {"t":2.31573, "x":6.85862, "y":4.33393, "heading":-3.05422, "vx":0.45397, "vy":-0.85198, "omega":-1.36828, "ax":-4.58826, "ay":8.46036, "alpha":13.04675, "fx":[-34.1919,-23.68156,-103.87631,-150.43014], "fy":[171.40811,173.5639,141.10456,89.55638]}, + {"t":2.34096, "x":6.86862, "y":4.31512, "heading":-3.08875, "vx":0.3382, "vy":-0.63851, "omega":-1.03908, "ax":-4.52442, "ay":8.44782, "alpha":13.40577, "fx":[-33.79329,-19.58873,-102.51163,-151.94238], "fy":[171.50824,174.09755,142.12886,87.04515]}, + {"t":2.36619, "x":6.87571, "y":4.3017, "heading":-3.11497, "vx":0.22403, "vy":-0.42535, "omega":-0.70081, "ax":-4.46693, "ay":8.43539, "alpha":13.72642, "fx":[-33.18714,-16.11004,-101.43269,-153.19518], "fy":[171.64658,174.47474,142.92553,84.88756]}, + {"t":2.39142, "x":6.87994, "y":4.29365, "heading":-3.13265, "vx":0.11132, "vy":-0.2125, "omega":-0.35446, "ax":-4.41181, "ay":8.42156, "alpha":14.04771, "fx":[-31.92685,-13.22648,-100.70878,-154.31265], "fy":[171.90522,174.73544,143.45733,82.89506]}, + {"t":2.41666, "x":6.88135, "y":4.29097, "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/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java b/src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java new file mode 100644 index 00000000..8d9ba365 --- /dev/null +++ b/src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.PathHandler; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pathHandler.PathHandler; + +public class KillPathHandlerCommand extends Command { + + PathHandler pathHandler; + + public KillPathHandlerCommand (PathHandler pathHandler) { + this.pathHandler = pathHandler; + } + + @Override + public void execute() { + pathHandler.killPathHandler(); + } +} diff --git a/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java new file mode 100644 index 00000000..bd004a8c --- /dev/null +++ b/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java @@ -0,0 +1,23 @@ +package frc.robot.commands.PathHandler; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pathHandler.PathHandler; + +public class StartPathHandlerCommand extends Command { + + PathHandler pathHandler; + + public StartPathHandlerCommand (PathHandler pathHandler) { + this.pathHandler = pathHandler; + } + + @Override + public void execute() { + pathHandler.startPathHandler(); + } + + @Override + public boolean isFinished() { + return pathHandler.isFinished(); + } +} diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java new file mode 100644 index 00000000..bad4148c --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java @@ -0,0 +1,62 @@ +package frc.robot.commands.auton; + +import java.util.List; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.PathHandler.KillPathHandlerCommand; +import frc.robot.commands.PathHandler.StartPathHandlerCommand; +import frc.robot.commands.drive.DriveAutonCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.pathHandler.PathHandler; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class NonProcessorShallowAutonCommand extends SequentialCommandGroup { + + private PathHandler pathHandler; + private DriveSubsystem driveSubsystem; + private DriveAutonCommand startPath; + + public NonProcessorShallowAutonCommand ( + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + PathHandler pathHandler, + String startPathName, + String[][] pathNames, + List NodeNames, + List NodeLevels, + boolean mirrorToProcessor + ) { + // addRequirements(driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.pathHandler = pathHandler; + this.driveSubsystem = driveSubsystem; + this.pathHandler.setPathNames(pathNames); + this.pathHandler.setStartNode(NodeNames.remove(0)); + this.pathHandler.setNodeNames(NodeNames); + this.pathHandler.setNodeLevels(NodeLevels); + this.pathHandler.setMirrorToProcessor(mirrorToProcessor); + + startPath = new DriveAutonCommand(driveSubsystem, startPathName, false, true, mirrorToProcessor); + + addCommands( + new SequentialCommandGroup( + // TODO fill in all zeroing and such + startPath, + new StartPathHandlerCommand(pathHandler), + new KillPathHandlerCommand(pathHandler) + ) + ); + } + + public void reassignAlliance() { + startPath.reassignAlliance(); + } + +} diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java index 114e18d8..ca8834cf 100644 --- a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java @@ -4,9 +4,11 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.auton.AutoCommandInterface; +import frc.robot.constants.DriveConstants; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.Optional; import org.slf4j.Logger; @@ -20,6 +22,7 @@ public class DriveAutonCommand extends Command implements AutoCommandInterface { private boolean isTherePath = false; private String trajectoryName; private boolean mirrorTrajectory = false; + private boolean mirrorToProcessor = false; private boolean resetOdometry; private boolean lastPath; @@ -28,13 +31,15 @@ public DriveAutonCommand( DriveSubsystem driveSubsystem, String trajectoryName, boolean lastPath, - boolean resetOdometry) { + boolean resetOdometry, + boolean mirrorToProcessor) { addRequirements(driveSubsystem); this.driveSubsystem = driveSubsystem; this.resetOdometry = resetOdometry; this.lastPath = lastPath; this.trajectoryName = trajectoryName; + this.mirrorToProcessor = mirrorToProcessor; Optional> tempTrajectory = Choreo.loadTrajectory(trajectoryName); if (tempTrajectory.isPresent()) { trajectory = tempTrajectory.get(); @@ -51,12 +56,46 @@ public void reassignAlliance() { mirrorTrajectory = driveSubsystem.shouldFlip(); } + private SwerveSample mirrorToProcessor(SwerveSample sample) { + if (mirrorToProcessor) { + sample = + new SwerveSample( + sample.t, + sample.x, + DriveConstants.kFieldMaxY - sample.y, + sample.heading * -1, + sample.vx, + DriveConstants.kFieldMaxY - sample.vy, + sample.omega * -1, + sample.ax, + DriveConstants.kFieldMaxY - sample.ay, + sample.alpha * -1, + sample.moduleForcesX(), + new double[] { + sample.moduleForcesY()[0] * -1, + sample.moduleForcesY()[1] * -1, + sample.moduleForcesY()[2] * -1, + sample.moduleForcesY()[3] * -1 + }); + } + return sample; + } + + private Pose2d mirrorToProcessor(Pose2d pose) { + pose = + new Pose2d( + pose.getX(), + DriveConstants.kFieldMaxY - pose.getY(), + Rotation2d.fromDegrees(pose.getRotation().getDegrees() * -1)); + return pose; + } + @Override public void initialize() { if (isTherePath) { driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName); Pose2d initialPose = new Pose2d(); - initialPose = trajectory.getInitialPose(mirrorTrajectory).get(); + initialPose = mirrorToProcessor(trajectory.getInitialPose(mirrorTrajectory).get()); if (resetOdometry) { driveSubsystem.resetOdometry(initialPose); } @@ -66,7 +105,8 @@ public void initialize() { driveSubsystem.grapherTrajectoryActive(true); timer.reset(); logger.info("Begin Trajectory: {}", trajectoryName); - SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + SwerveSample desiredState = + mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get()); driveSubsystem.calculateController(desiredState); } } @@ -74,7 +114,8 @@ public void initialize() { @Override public void execute() { if (isTherePath) { - SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get(); + SwerveSample desiredState = + mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get()); driveSubsystem.calculateController(desiredState); } } @@ -94,7 +135,8 @@ public void end(boolean interrupted) { if (!interrupted && !lastPath) { driveSubsystem.calculateController( - trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get()); + mirrorToProcessor( + trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get())); } else { driveSubsystem.drive(0, 0, 0); } diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 33f3fdc9..a8379ca9 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -23,8 +23,8 @@ public class DriveConstants { public static final double kRobotLength = 22.0; public static final double kRobotWidth = 22.0; - public static final double kFieldMaxX = 690.0; - public static final double kFieldMaxY + public static final double kFieldMaxX = 17.548225; + public static final double kFieldMaxY = 8.0518; public static final double kPOmega = 4.5; public static final double kIOmega = 0.0; diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 22eddc6b..4e8098e5 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -4,11 +4,10 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.DriveConstants; import frc.robot.constants.RobotStateConstants; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; -import frc.robot.subsystems.drive.Swerve; - import java.util.List; import java.util.Optional; import java.util.Set; @@ -109,7 +108,8 @@ private void startPath(Trajectory path) { timer.stop(); timer.reset(); timer.start(); - driveSubsystem.calculateController(currPath.getInitialSample(mirrorTrajectory).get()); + driveSubsystem.calculateController( + mirrorToProcessor(currPath.getInitialSample(mirrorTrajectory).get())); if (currState == PathStates.PLACE) { currState = PathStates.DRIVE_FETCH; } else if (currState == PathStates.FETCH) { @@ -120,13 +120,15 @@ private void startPath(Trajectory path) { private void drivePath() { if (isHandling && runningPath && currPath != null) { - driveSubsystem.calculateController(currPath.sampleAt(timer.get(), mirrorTrajectory).get()); + driveSubsystem.calculateController( + mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get())); if (timer.hasElapsed(currPath.getTotalTime())) { driveSubsystem.setAutoDebugMsg("End " + currPathString); runningPath = false; timer.stop(); timer.reset(); - driveSubsystem.calculateController(currPath.getFinalSample(mirrorTrajectory).get()); + driveSubsystem.calculateController( + mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get())); if (currState == PathStates.DRIVE_FETCH) { currState = PathStates.FETCH; } else if (currState == PathStates.DRIVE_PLACE) { @@ -142,7 +144,7 @@ private void drivePath() { private void drivePathServo() { if (isHandling && runningPath && currPath != null && isServoing) { driveSubsystem.calculateControllerServo( - currPath.sampleAt(timer.get(), mirrorTrajectory).get(), + mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()), 0.0); // TODO: use tag servoing here when ready if (timer.hasElapsed(currPath.getTotalTime())) { driveSubsystem.setAutoDebugMsg("End " + currPathString); @@ -150,7 +152,7 @@ private void drivePathServo() { timer.stop(); timer.reset(); driveSubsystem.calculateControllerServo( - currPath.getFinalSample(mirrorTrajectory).get(), 0.0); + mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get()), 0.0); if (currState == PathStates.DRIVE_FETCH) { currState = PathStates.FETCH; } else if (currState == PathStates.DRIVE_PLACE_SERVO) { @@ -221,11 +223,33 @@ private boolean shouldTransitionToServoing() { private SwerveSample mirrorToProcessor(SwerveSample sample) { if (mirrorToProcessor) { - sample = new SwerveSample(sample.t, sample.x, sample.y, sample.heading * -1, sample.vx, sample.vy, sample.omega * -1, sample.ax, , getDeviceId(), null, null) + sample = + new SwerveSample( + sample.t, + sample.x, + DriveConstants.kFieldMaxY - sample.y, + sample.heading * -1, + sample.vx, + DriveConstants.kFieldMaxY - sample.vy, + sample.omega * -1, + sample.ax, + DriveConstants.kFieldMaxY - sample.ay, + sample.alpha * -1, + sample.moduleForcesX(), + new double[] { + sample.moduleForcesY()[0] * -1, + sample.moduleForcesY()[1] * -1, + sample.moduleForcesY()[2] * -1, + sample.moduleForcesY()[3] * -1 + }); } return sample; } + public boolean isFinished() { + return !isHandling; + } + public void periodic() { switch (currState) { case DRIVE_FETCH: From 97a6220410fcfcbf7d0a000ce0eb3f6539007b82 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 3 Feb 2025 17:56:53 -0500 Subject: [PATCH 11/48] added a bunch of measures to LEDSubsystem --- .vscode/settings.json | 3 ++- .../robot/subsystems/led/LEDSubsystem.java | 24 ++++++++++++++++++- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0d..1745ba0d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 0fb29bee..879a30dd 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -203,10 +203,32 @@ public Set getMeasures() { return Set.of( new Measure( "State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()), + new Measure( + "HasAlgea", "the current algea state of the LEDSubsystem", () -> hasAlgae ? 0 : 1), new Measure( "CoralState", "the current Coral state of the LEDSubsystem", - () -> coralState.ordinal())); + () -> coralState.ordinal()), + new Measure( + "Level", + "the current assumed Level of the LEDSubsystem", + () -> (levelState.ordinal() + 1)), + new Measure( + "PlaceState", + "the current Placement state of the LEDSubsystem", + () -> placeState.ordinal()), + new Measure( + "GetAlgea", + "the current get algea state of the LEDSubsystem", + () -> shouldGetAlgea ? 0 : 1), + new Measure( + "autoPlacing", + "the current autoPlacing state of the LEDSubsystem", + () -> autoPlacing ? 0 : 1), + new Measure( + "currentLimiting", + "the current currentLimiting state of the LEDSubsystem", + () -> isLimiting ? 0 : 1)); } public enum LEDStates { From 494b88b926f638c4a71c27523c9362e6c35517cc Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 3 Feb 2025 18:14:04 -0500 Subject: [PATCH 12/48] added a new constant to choreo, the deep startpoint --- src/main/deploy/choreo/2025-project.chor | 14 ++++++++++++++ src/main/deploy/choreo/constants.traj | 3 ++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index fb047720..68b577eb 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -208,6 +208,20 @@ "exp":"180 deg", "val":3.141592653589793 } + }, + "startDeep":{ + "x":{ + "exp":"7.1008875 m", + "val":7.1008875 + }, + "y":{ + "exp":"7.2570308 m", + "val":7.2570308 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } } } }, diff --git a/src/main/deploy/choreo/constants.traj b/src/main/deploy/choreo/constants.traj index c6f351fb..5abf3bbb 100644 --- a/src/main/deploy/choreo/constants.traj +++ b/src/main/deploy/choreo/constants.traj @@ -8,6 +8,7 @@ }, "params":{ "waypoints":[ + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"7.2570308 m", "val":7.2570308}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":6506, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, @@ -26,7 +27,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":true}, - {"from":0, "to":13, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], + {"from":1, "to":14, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 From 82e4acd6dedd4498e697d8332ca4529957caf79b Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 4 Feb 2025 19:42:51 -0500 Subject: [PATCH 13/48] added some more setpoints and related paths --- src/main/deploy/choreo/2025-project.chor | 42 +++ src/main/deploy/choreo/5MeterTestPath.traj | 65 +++++ src/main/deploy/choreo/ConfusingTestPath.traj | 252 +++++++++--------- src/main/deploy/choreo/GToBarge.traj | 54 ++++ src/main/deploy/choreo/HToBarge.traj | 53 ++++ src/main/deploy/choreo/constants.traj | 6 +- src/main/deploy/choreo/startDeepToA.traj | 145 ++++++++++ src/main/deploy/choreo/startDeepToB.traj | 145 ++++++++++ src/main/deploy/choreo/startDeepToK.traj | 104 ++++++++ src/main/deploy/choreo/startDeepToL.traj | 104 ++++++++ src/main/deploy/choreo/startGToG.traj | 50 ++++ src/main/deploy/choreo/startHToH.traj | 50 ++++ 12 files changed, 944 insertions(+), 126 deletions(-) create mode 100644 src/main/deploy/choreo/5MeterTestPath.traj create mode 100644 src/main/deploy/choreo/GToBarge.traj create mode 100644 src/main/deploy/choreo/HToBarge.traj create mode 100644 src/main/deploy/choreo/startDeepToA.traj create mode 100644 src/main/deploy/choreo/startDeepToB.traj create mode 100644 src/main/deploy/choreo/startDeepToK.traj create mode 100644 src/main/deploy/choreo/startDeepToL.traj create mode 100644 src/main/deploy/choreo/startGToG.traj create mode 100644 src/main/deploy/choreo/startHToH.traj diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 68b577eb..604756e8 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -181,6 +181,20 @@ "val":-1.0471975511965976 } }, + "barge":{ + "x":{ + "exp":"305.5 in", + "val":7.7597 + }, + "y":{ + "exp":"4.3597 m", + "val":4.3597 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } + }, "fetch":{ "x":{ "exp":"61.8667 in", @@ -222,6 +236,34 @@ "exp":"180 deg", "val":3.141592653589793 } + }, + "startG":{ + "x":{ + "exp":"7.1008875 m", + "val":7.1008875 + }, + "y":{ + "exp":"142.621161 in", + "val":3.6225774894 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } + }, + "startH":{ + "x":{ + "exp":"7.1008875 m", + "val":7.1008875 + }, + "y":{ + "exp":"155.561161 in", + "val":3.9512534894 + }, + "heading":{ + "exp":"180 deg", + "val":3.141592653589793 + } } } }, diff --git a/src/main/deploy/choreo/5MeterTestPath.traj b/src/main/deploy/choreo/5MeterTestPath.traj new file mode 100644 index 00000000..1e8c21e6 --- /dev/null +++ b/src/main/deploy/choreo/5MeterTestPath.traj @@ -0,0 +1,65 @@ +{ + "name":"5MeterTestPath", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":0.0, "y":0.0, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.0, "y":0.0, "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 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5 m", "val":5.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.58285], + "samples":[ + {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":10.30754, "ay":0.0, "alpha":0.0, "fx":[175.32826,175.32826,175.32826,175.32826], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.04946, "x":0.01261, "y":0.0, "heading":0.0, "vx":0.50985, "vy":0.0, "omega":0.0, "ax":10.30628, "ay":0.0, "alpha":0.0, "fx":[175.30682,175.30682,175.30682,175.30682], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.09893, "x":0.05044, "y":0.0, "heading":0.0, "vx":1.01964, "vy":0.0, "omega":0.0, "ax":10.3046, "ay":0.0, "alpha":0.0, "fx":[175.27823,175.27823,175.27823,175.27823], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.14839, "x":0.11348, "y":0.0, "heading":0.0, "vx":1.52935, "vy":0.0, "omega":0.0, "ax":10.30224, "ay":0.0, "alpha":0.0, "fx":[175.23818,175.23818,175.23818,175.23818], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.19786, "x":0.20173, "y":0.0, "heading":0.0, "vx":2.03894, "vy":0.0, "omega":0.0, "ax":10.29871, "ay":0.0, "alpha":0.0, "fx":[175.17807,175.17807,175.17807,175.17807], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.24732, "x":0.31518, "y":0.0, "heading":0.0, "vx":2.54835, "vy":0.0, "omega":0.0, "ax":10.29282, "ay":0.0, "alpha":0.0, "fx":[175.07785,175.07785,175.07785,175.07785], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.29678, "x":0.45383, "y":0.0, "heading":0.0, "vx":3.05748, "vy":0.0, "omega":0.0, "ax":10.28103, "ay":0.0, "alpha":0.0, "fx":[174.87733,174.87733,174.87733,174.87733], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.34625, "x":0.61764, "y":0.0, "heading":0.0, "vx":3.56602, "vy":0.0, "omega":0.0, "ax":10.24571, "ay":0.0, "alpha":0.0, "fx":[174.27652,174.27652,174.27652,174.27652], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.39571, "x":0.80656, "y":0.0, "heading":0.0, "vx":4.07281, "vy":0.0, "omega":0.0, "ax":4.45615, "ay":0.0, "alpha":0.0, "fx":[75.7979,75.7979,75.7979,75.7979], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.44518, "x":1.01347, "y":0.0, "heading":0.0, "vx":4.29323, "vy":0.0, "omega":0.0, "ax":0.00051, "ay":0.0, "alpha":0.0, "fx":[0.00871,0.00871,0.00871,0.00871], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.49464, "x":1.22583, "y":0.0, "heading":0.0, "vx":4.29326, "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.5441, "x":1.43819, "y":0.0, "heading":0.0, "vx":4.29326, "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.59357, "x":1.65055, "y":0.0, "heading":0.0, "vx":4.29326, "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.64303, "x":1.86292, "y":0.0, "heading":0.0, "vx":4.29326, "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.6925, "x":2.07528, "y":0.0, "heading":0.0, "vx":4.29326, "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.74196, "x":2.28764, "y":0.0, "heading":0.0, "vx":4.29326, "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.79142, "x":2.5, "y":0.0, "heading":0.0, "vx":4.29326, "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.84089, "x":2.71236, "y":0.0, "heading":0.0, "vx":4.29326, "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.89035, "x":2.92472, "y":0.0, "heading":0.0, "vx":4.29326, "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.93982, "x":3.13708, "y":0.0, "heading":0.0, "vx":4.29326, "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.98928, "x":3.34945, "y":0.0, "heading":0.0, "vx":4.29326, "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.03874, "x":3.56181, "y":0.0, "heading":0.0, "vx":4.29326, "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.08821, "x":3.77417, "y":0.0, "heading":0.0, "vx":4.29326, "vy":0.0, "omega":0.0, "ax":-0.00051, "ay":0.0, "alpha":0.0, "fx":[-0.00871,-0.00871,-0.00871,-0.00871], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.13767, "x":3.98653, "y":0.0, "heading":0.0, "vx":4.29323, "vy":0.0, "omega":0.0, "ax":-4.45615, "ay":0.0, "alpha":0.0, "fx":[-75.7979,-75.7979,-75.7979,-75.7979], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.18714, "x":4.19344, "y":0.0, "heading":0.0, "vx":4.07281, "vy":0.0, "omega":0.0, "ax":-10.24571, "ay":0.0, "alpha":0.0, "fx":[-174.27652,-174.27652,-174.27652,-174.27652], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.2366, "x":4.38236, "y":0.0, "heading":0.0, "vx":3.56602, "vy":0.0, "omega":0.0, "ax":-10.28103, "ay":0.0, "alpha":0.0, "fx":[-174.87733,-174.87733,-174.87733,-174.87733], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.28606, "x":4.54617, "y":0.0, "heading":0.0, "vx":3.05748, "vy":0.0, "omega":0.0, "ax":-10.29282, "ay":0.0, "alpha":0.0, "fx":[-175.07785,-175.07785,-175.07785,-175.07785], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.33553, "x":4.68482, "y":0.0, "heading":0.0, "vx":2.54835, "vy":0.0, "omega":0.0, "ax":-10.29871, "ay":0.0, "alpha":0.0, "fx":[-175.17807,-175.17807,-175.17807,-175.17807], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.38499, "x":4.79827, "y":0.0, "heading":0.0, "vx":2.03894, "vy":0.0, "omega":0.0, "ax":-10.30224, "ay":0.0, "alpha":0.0, "fx":[-175.23818,-175.23818,-175.23818,-175.23818], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.43446, "x":4.88652, "y":0.0, "heading":0.0, "vx":1.52935, "vy":0.0, "omega":0.0, "ax":-10.3046, "ay":0.0, "alpha":0.0, "fx":[-175.27823,-175.27823,-175.27823,-175.27823], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.48392, "x":4.94956, "y":0.0, "heading":0.0, "vx":1.01964, "vy":0.0, "omega":0.0, "ax":-10.30628, "ay":0.0, "alpha":0.0, "fx":[-175.30682,-175.30682,-175.30682,-175.30682], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.53338, "x":4.98739, "y":0.0, "heading":0.0, "vx":0.50985, "vy":0.0, "omega":0.0, "ax":-10.30754, "ay":0.0, "alpha":0.0, "fx":[-175.32826,-175.32826,-175.32826,-175.32826], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.58285, "x":5.0, "y":0.0, "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/ConfusingTestPath.traj b/src/main/deploy/choreo/ConfusingTestPath.traj index d0b1a84c..28b1fe11 100644 --- a/src/main/deploy/choreo/ConfusingTestPath.traj +++ b/src/main/deploy/choreo/ConfusingTestPath.traj @@ -4,31 +4,33 @@ "snapshot":{ "waypoints":[ {"x":0.44632652401924133, "y":4.026000499725342, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.881347179412842, "y":4.290971755981445, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.881347179412842, "y":4.290971755981445, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "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":0, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}], + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}, + {"from":1, "to":5, "data":{"type":"PointAt", "props":{"x":4.4726667404174805, "y":3.991537094116211, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"0.44632652401924133 m", "val":0.44632652401924133}, "y":{"exp":"4.026000499725342 m", "val":4.026000499725342}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.881347179412842 m", "val":6.881347179412842}, "y":{"exp":"4.290971755981445 m", "val":4.290971755981445}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.881347179412842 m", "val":6.881347179412842}, "y":{"exp":"4.290971755981445 m", "val":4.290971755981445}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "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":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}], + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}, + {"from":1, "to":5, "data":{"type":"PointAt", "props":{"x":{"exp":"4.4726667404174805 m", "val":4.4726667404174805}, "y":{"exp":"3.991537094116211 m", "val":3.991537094116211}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,120 +38,120 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.59078,0.97347,1.38425,1.83631,2.41666], + "waypoints":[0.0,0.59086,0.96585,1.39567,1.85025,2.44024], "samples":[ - {"t":0.0, "x":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.8809, "ay":3.14666, "alpha":-35.43679, "fx":[170.53812,122.70139,-1.88378,-27.30358], "fy":[-4.88774,120.5167,170.50905,-72.04268]}, - {"t":0.02813, "x":0.44786, "y":4.02725, "heading":0.0, "vx":0.10918, "vy":0.08852, "omega":-0.99692, "ax":7.98031, "ay":6.50906, "alpha":-0.83108, "fx":[139.29099,135.43093,132.10036,136.1487], "fy":[106.29731,111.18497,115.1086,110.27786]}, - {"t":0.05626, "x":0.45409, "y":4.03231, "heading":-0.02805, "vx":0.33368, "vy":0.27164, "omega":-1.0203, "ax":7.97974, "ay":6.50862, "alpha":-0.75224, "fx":[138.95862,135.53654,132.43538,136.00213], "fy":[106.69721,111.02347,114.69193,110.42626]}, - {"t":0.0844, "x":0.46664, "y":4.04253, "heading":-0.05675, "vx":0.55817, "vy":0.45474, "omega":-1.04146, "ax":7.97904, "ay":6.50806, "alpha":-0.66, "fx":[138.56114,135.62558,132.82938,135.8684], "fy":[107.17306,110.87607,114.19878,110.55326]}, - {"t":0.11253, "x":0.4855, "y":4.0579, "heading":-0.08605, "vx":0.78264, "vy":0.63783, "omega":-1.06003, "ax":7.97813, "ay":6.50735, "alpha":-0.55061, "fx":[138.08156,135.69158,133.29674,135.75286], "fy":[107.74335,110.74907,113.6092,110.65104]}, - {"t":0.14066, "x":0.51067, "y":4.07842, "heading":-0.11587, "vx":1.00708, "vy":0.82089, "omega":-1.07552, "ax":7.97694, "ay":6.50641, "alpha":-0.41885, "fx":[137.49613,135.72551,133.85734,135.66264], "fy":[108.43345,110.65121,112.89497,110.70908]}, - {"t":0.16879, "x":0.54216, "y":4.10408, "heading":-0.14613, "vx":1.23149, "vy":1.00393, "omega":-1.0873, "ax":7.97532, "ay":6.50514, "alpha":-0.25707, "fx":[136.77038,135.71429,134.53977,135.60751], "fy":[109.27941,110.59501,112.01476,110.71279]}, - {"t":0.19693, "x":0.57996, "y":4.1349, "heading":-0.17671, "vx":1.45586, "vy":1.18694, "omega":-1.09453, "ax":7.97307, "ay":6.50334, "alpha":-0.05378, "fx":[135.85227,135.63808,135.38669,135.60132], "fy":[110.33434,110.5992,110.90538,110.64118]}, - {"t":0.22506, "x":0.62407, "y":4.17087, "heading":-0.20751, "vx":1.68016, "vy":1.36989, "omega":-1.09605, "ax":7.96976, "ay":6.50071, "alpha":0.20928, "fx":[134.65906,135.46501,136.46483,135.6646], "fy":[111.68017,110.69349,109.46482,110.46258]}, - {"t":0.25319, "x":0.67449, "y":4.21198, "heading":-0.23834, "vx":1.90436, "vy":1.55277, "omega":-1.09016, "ax":7.96464, "ay":6.49662, "alpha":0.56285, "fx":[133.05064,135.13987,137.88479,135.82941], "fy":[113.45049,110.92831,107.51721,110.12647]}, - {"t":0.28132, "x":0.73122, "y":4.25823, "heading":-0.26901, "vx":2.12843, "vy":1.73554, "omega":-1.07432, "ax":7.95604, "ay":6.48973, "alpha":1.063, "fx":[130.76974,134.55721,139.84343,136.14938], "fy":[115.87855,111.39795,104.73152,109.54604]}, - {"t":0.30946, "x":0.79424, "y":4.30962, "heading":-0.29923, "vx":2.35225, "vy":1.91811, "omega":-1.04442, "ax":7.9399, "ay":6.47682, "alpha":1.82398, "fx":[127.28874,133.48527,142.7254,136.72251], "fy":[119.40977,112.30343,100.40513,108.55718]}, - {"t":0.33759, "x":0.86356, "y":4.36615, "heading":-0.32861, "vx":2.57562, "vy":2.10031, "omega":-0.99311, "ax":7.90374, "ay":6.44827, "alpha":3.11923, "fx":[121.34116,131.29186,147.37785,137.75088], "fy":[125.00117,114.16886,92.75778,106.80477]}, - {"t":0.36572, "x":0.93915, "y":4.42778, "heading":-0.35655, "vx":2.79797, "vy":2.28172, "omega":-0.90536, "ax":7.79127, "ay":6.36563, "alpha":5.80023, "fx":[109.05196,125.41989,155.90901,139.72845], "fy":[135.04049,118.93371,75.78766,103.34826]}, - {"t":0.39385, "x":1.02094, "y":4.49449, "heading":-0.38202, "vx":3.01715, "vy":2.4608, "omega":-0.74218, "ax":6.95096, "ay":6.01786, "alpha":14.54572, "fx":[73.31148,85.53153,169.98006,144.11216], "fy":[155.42203,142.02633,17.2258,94.77385]}, - {"t":0.42198, "x":1.10857, "y":4.5661, "heading":-0.4029, "vx":3.2127, "vy":2.6301, "omega":-0.33298, "ax":3.93479, "ay":2.97269, "alpha":11.79458, "fx":[47.54079,31.46021,91.94868,96.76863], "fy":[92.25735,43.75194,6.6639,59.58494]}, - {"t":0.45012, "x":1.20051, "y":4.64127, "heading":-0.41227, "vx":3.32339, "vy":2.71372, "omega":-0.00117, "ax":0.16522, "ay":-0.19216, "alpha":0.00989, "fx":[2.80365,2.89396,2.88939,2.65414], "fy":[-3.08565,-3.35371,-3.39413,-3.24094]}, - {"t":0.47825, "x":1.29407, "y":4.71754, "heading":-0.4123, "vx":3.32804, "vy":2.70832, "omega":-0.00089, "ax":0.40738, "ay":-0.50279, "alpha":0.0, "fx":[6.93047,6.92926,6.92812,6.92996], "fy":[-8.55561,-8.56174,-8.54854,-8.54302]}, - {"t":0.50638, "x":1.38786, "y":4.79353, "heading":-0.41233, "vx":3.3395, "vy":2.69417, "omega":-0.00089, "ax":1.22951, "ay":-1.54456, "alpha":-0.00004, "fx":[20.90672,20.92902,20.91978,20.89904], "fy":[-26.25537,-26.26409,-26.28724,-26.28358]}, - {"t":0.53451, "x":1.48229, "y":4.86871, "heading":-0.41235, "vx":3.37409, "vy":2.65072, "omega":-0.00089, "ax":2.98669, "ay":-3.94193, "alpha":-0.01143, "fx":[51.01036,51.10239,50.45754,50.64065], "fy":[-66.79105,-67.10557,-67.27246,-67.03523]}, - {"t":0.56265, "x":1.57839, "y":4.94172, "heading":-0.41238, "vx":3.45811, "vy":2.53983, "omega":-0.00121, "ax":2.20716, "ay":-7.56513, "alpha":-6.19223, "fx":[44.73442,70.80412,26.96516,7.66887], "fy":[-136.37986,-117.39919,-122.93228,-138.01146]}, - {"t":0.59078, "x":1.67655, "y":5.01018, "heading":-0.41241, "vx":3.52021, "vy":2.327, "omega":-0.17541, "ax":-1.91443, "ay":-7.4438, "alpha":-13.12628, "fx":[-6.19702,34.98789,-81.66628,-77.3805], "fy":[-150.71955,-130.59442,-97.34976,-127.80408]}, - {"t":0.609, "x":1.74038, "y":5.05135, "heading":-0.41561, "vx":3.48532, "vy":2.19135, "omega":-0.41462, "ax":-2.54991, "ay":-6.42087, "alpha":-12.85895, "fx":[-18.05738,14.62928,-86.49877,-83.56581], "fy":[-138.65212,-114.61071,-72.16244,-111.44317]}, - {"t":0.62723, "x":1.80348, "y":5.09022, "heading":-0.42316, "vx":3.43885, "vy":2.07434, "omega":-0.64896, "ax":-3.13784, "ay":-5.1062, "alpha":-12.34609, "fx":[-31.10036,-5.74901,-87.45565,-89.19024], "fy":[-121.8606,-88.38191,-45.8808,-91.29638]}, - {"t":0.64545, "x":1.86562, "y":5.12717, "heading":-0.43499, "vx":3.38167, "vy":1.98128, "omega":-0.87395, "ax":-3.46691, "ay":-3.72004, "alpha":-11.40829, "fx":[-41.52019,-19.92546,-83.47447,-90.96421], "fy":[-100.5158,-59.06728,-23.54786,-69.9764]}, - {"t":0.66367, "x":1.92667, "y":5.16266, "heading":-0.45092, "vx":3.31849, "vy":1.91349, "omega":-1.08185, "ax":-3.48151, "ay":-2.44876, "alpha":-10.08136, "fx":[-46.49432,-26.52062,-75.98515,-87.87813], "fy":[-76.52644,-34.37081,-6.32429,-49.38908]}, - {"t":0.6819, "x":1.98657, "y":5.19713, "heading":-0.47063, "vx":3.25504, "vy":1.86887, "omega":-1.26557, "ax":-3.24826, "ay":-1.38826, "alpha":-8.55158, "fx":[-46.16492,-27.78958,-66.54826,-80.50524], "fy":[-53.53144,-15.96759,6.09731,-31.0539]}, - {"t":0.70012, "x":2.04535, "y":5.23095, "heading":-0.49369, "vx":3.19585, "vy":1.84357, "omega":-1.42141, "ax":-2.86715, "ay":-0.55895, "alpha":-7.00899, "fx":[-42.33794,-26.05359,-56.3431,-70.34272], "fy":[-33.88006,-2.90948,14.56684,-15.80733]}, - {"t":0.71834, "x":2.10311, "y":5.26446, "heading":-0.5196, "vx":3.1436, "vy":1.83338, "omega":-1.54914, "ax":-2.4157, "ay":0.06599, "alpha":-5.56134, "fx":[-36.62206,-22.7878,-46.11082,-58.84075], "fy":[-18.02266,6.1999,20.03957,-3.72688]}, - {"t":0.73657, "x":2.16, "y":5.29788, "heading":-0.54783, "vx":3.09957, "vy":1.83458, "omega":-1.65049, "ax":-1.94158, "ay":0.5249, "alpha":-4.25088, "fx":[-30.03281,-18.81134,-36.26105,-46.99765], "fy":[-5.57194,12.4962,23.2912,5.49794]}, - {"t":0.75479, "x":2.21616, "y":5.3314, "heading":-0.57791, "vx":3.06419, "vy":1.84415, "omega":-1.72795, "ax":-1.47006, "ay":0.84935, "alpha":-3.08508, "fx":[-23.12488,-14.54102,-26.97641,-35.37908], "fy":[3.99528,16.72564,24.83258,12.23551]}, - {"t":0.77301, "x":2.27176, "y":5.36515, "heading":-0.6094, "vx":3.0374, "vy":1.85963, "omega":-1.78418, "ax":-1.01138, "ay":1.05748, "alpha":-2.05698, "fx":[-16.14895,-10.14299,-18.28157,-24.23939], "fy":[11.04905,19.26724,24.90353,16.72988]}, - {"t":0.79124, "x":2.32694, "y":5.39921, "heading":-0.64191, "vx":3.01897, "vy":1.8789, "omega":-1.82166, "ax":-0.56543, "ay":1.15021, "alpha":-1.15856, "fx":[-9.1517,-5.60227,-10.08497,-13.63261], "fy":[15.66751,20.14342,23.4611,18.98661]}, - {"t":0.80946, "x":2.38187, "y":5.43364, "heading":-0.67511, "vx":3.00867, "vy":1.89986, "omega":-1.84277, "ax":-0.12456, "ay":1.10543, "alpha":-0.39314, "fx":[-2.03089,-0.73737,-2.2034,-3.50309], "fy":[17.49039,18.95505,20.12036,18.64642]}, - {"t":0.82769, "x":2.43668, "y":5.46845, "heading":-0.70869, "vx":3.0064, "vy":1.92, "omega":-1.84994, "ax":0.32462, "ay":0.86678, "alpha":0.20736, "fx":[5.4272,4.82213,5.61775,6.21978], "fy":[15.46767,14.69927,14.02163,14.78599]}, - {"t":0.84591, "x":2.49152, "y":5.50358, "heading":-0.7424, "vx":3.01231, "vy":1.9358, "omega":-1.84616, "ax":0.79771, "ay":0.32495, "alpha":0.55448, "fx":[13.47638,11.6644,13.66213,15.47202], "fy":[7.43875,5.44536,3.61579,5.609]}, - {"t":0.86413, "x":2.54654, "y":5.53891, "heading":-0.77605, "vx":3.02685, "vy":1.94172, "omega":-1.83606, "ax":1.30136, "ay":-0.70172, "alpha":0.465, "fx":[22.16755,20.52465,22.10693,23.74405], "fy":[-10.34157,-11.99917,-13.52773,-11.87583]}, - {"t":0.88236, "x":2.60192, "y":5.57418, "heading":-0.80951, "vx":3.05057, "vy":1.92893, "omega":-1.82758, "ax":1.79878, "ay":-2.43423, "alpha":-0.34366, "fx":[30.57824,31.7792,30.62848,29.40068], "fy":[-42.57533,-41.44075,-40.22082,-41.3852]}, - {"t":0.90058, "x":2.65781, "y":5.60893, "heading":-0.84281, "vx":3.08335, "vy":1.88457, "omega":-1.83384, "ax":2.13315, "ay":-4.82202, "alpha":-2.01359, "fx":[35.05291,43.91666,37.66183,28.50584], "fy":[-88.07592,-81.57139,-75.78998,-82.64712]}, - {"t":0.9188, "x":2.71436, "y":5.64247, "heading":-0.87623, "vx":3.12222, "vy":1.7967, "omega":-1.87054, "ax":2.06959, "ay":-6.92852, "alpha":-3.88623, "fx":[30.67974,52.57444,40.84004,16.71835], "fy":[-126.34344,-115.5237,-108.90087,-120.64026]}, - {"t":0.93703, "x":2.7716, "y":5.67406, "heading":-0.91032, "vx":3.15993, "vy":1.67044, "omega":-1.94136, "ax":1.72072, "ay":-8.19001, "alpha":-5.1744, "fx":[22.06307,55.22142,39.41146,0.38018], "fy":[-147.31706,-135.56394,-131.59017,-142.76768]}, - {"t":0.95525, "x":2.82947, "y":5.70315, "heading":-0.9457, "vx":3.19129, "vy":1.52118, "omega":-2.03566, "ax":1.34433, "ay":-8.90304, "alpha":-5.79542, "fx":[14.43111,53.87246,35.60105,-12.43761], "fy":[-158.2164,-147.47839,-146.01362,-154.04449]}, - {"t":0.97347, "x":2.88785, "y":5.72939, "heading":-0.98279, "vx":3.21579, "vy":1.35894, "omega":-2.14127, "ax":2.5922, "ay":-8.78621, "alpha":-1.54169, "fx":[40.33909,52.06789,48.25967,35.70338], "fy":[-151.67009,-147.60414,-147.21852,-151.31096]}, - {"t":0.99215, "x":2.94835, "y":5.75323, "heading":-1.02277, "vx":3.26419, "vy":1.19489, "omega":-2.17006, "ax":3.36586, "ay":-7.95222, "alpha":1.92676, "fx":[63.25421,48.10011,51.95567,65.69908], "fy":[-131.06513,-137.84694,-139.23505,-132.91287]}, - {"t":1.01082, "x":3.00988, "y":5.77415, "heading":-1.06329, "vx":3.32704, "vy":1.0464, "omega":-2.13408, "ax":4.17979, "ay":-6.6265, "alpha":5.50772, "fx":[88.74894,48.99539,57.25415,89.38951], "fy":[-95.13783,-119.51285,-127.83867,-108.37022]}, - {"t":1.02949, "x":3.07273, "y":5.79254, "heading":-1.10314, "vx":3.40508, "vy":0.92268, "omega":-2.03124, "ax":4.70253, "ay":-5.05275, "alpha":8.17884, "fx":[102.45304,52.79103,62.58878,102.12213], "fy":[-54.94026,-90.98088,-113.17738,-84.6848]}, - {"t":1.04816, "x":3.13713, "y":5.80888, "heading":-1.14107, "vx":3.49289, "vy":0.82833, "omega":-1.87853, "ax":4.79262, "ay":-3.65392, "alpha":9.46598, "fx":[102.75067,53.13018,64.99979,105.20396], "fy":[-25.96599,-59.88772,-96.67962,-66.07493]}, - {"t":1.06683, "x":3.20318, "y":5.82371, "heading":-1.17614, "vx":3.58237, "vy":0.76011, "omega":-1.70178, "ax":4.52613, "ay":-2.63566, "alpha":9.57639, "fx":[95.40029,48.97061,62.39946,101.18204], "fy":[-9.81459,-36.72489,-80.11951,-52.66853]}, - {"t":1.0855, "x":3.27086, "y":5.83745, "heading":-1.20792, "vx":3.66688, "vy":0.7109, "omega":-1.52298, "ax":4.02272, "ay":-1.95149, "alpha":8.87096, "fx":[84.02533,42.34463,55.46689,91.86412], "fy":[-2.36947,-22.61676,-64.87045,-42.92059]}, - {"t":1.10418, "x":3.34003, "y":5.85038, "heading":-1.23635, "vx":3.74199, "vy":0.67446, "omega":-1.35734, "ax":3.39313, "ay":-1.4945, "alpha":7.67165, "fx":[70.68494,34.95724,46.31392,78.90853], "fy":[0.08615,-14.80437,-51.65336,-35.31224]}, - {"t":1.12285, "x":3.41049, "y":5.86271, "heading":-1.2617, "vx":3.80535, "vy":0.64655, "omega":-1.2141, "ax":2.73499, "ay":-1.17766, "alpha":6.25854, "fx":[57.01342,27.8519,36.85822,64.36223], "fy":[0.07092,-10.64864,-40.58812,-28.96048]}, - {"t":1.14152, "x":3.48202, "y":5.87458, "heading":-1.28437, "vx":3.85641, "vy":0.62457, "omega":-1.09724, "ax":2.12311, "ay":-0.94657, "alpha":4.85486, "fx":[44.32253,21.61437,28.30975,50.20723], "fy":[-0.91044,-8.43429,-31.53438,-23.52432]}, - {"t":1.16019, "x":3.55439, "y":5.88608, "heading":-1.30485, "vx":3.89606, "vy":0.60689, "omega":-1.00659, "ax":1.6014, "ay":-0.77408, "alpha":3.60458, "fx":[33.43785,16.49944,21.2289,37.79127], "fy":[-2.11287,-7.24733,-24.33866,-18.96879]}, - {"t":1.17886, "x":3.62742, "y":5.89727, "heading":-1.32365, "vx":3.92596, "vy":0.59244, "omega":-0.93929, "ax":1.18515, "ay":-0.65167, "alpha":2.57287, "fx":[24.67518,12.53078,15.72719,27.70294], "fy":[-3.31907,-6.71878,-18.90295,-15.39796]}, - {"t":1.19753, "x":3.70093, "y":5.90822, "heading":-1.34119, "vx":3.94809, "vy":0.58027, "omega":-0.89125, "ax":0.86885, "ay":-0.5824, "alpha":1.76323, "fx":[17.943,9.58806,11.65209,19.93273], "fy":[-4.65376,-6.82449,-15.18066,-12.96707]}, - {"t":1.21621, "x":3.7748, "y":5.91895, "heading":-1.35783, "vx":3.96431, "vy":0.5694, "omega":-0.85833, "ax":0.63349, "ay":-0.57709, "alpha":1.13534, "fx":[12.8585,7.46537,8.70895,14.06937], "fy":[-6.47001,-7.77257,-13.16934,-11.85257]}, - {"t":1.23488, "x":3.84893, "y":5.92949, "heading":-1.37385, "vx":3.97614, "vy":0.55862, "omega":-0.83713, "ax":0.44993, "ay":-0.65351, "alpha":0.61357, "fx":[8.79956,5.89225,6.5147,9.40643], "fy":[-9.31723,-9.97345,-12.91561,-12.25787]}, - {"t":1.25355, "x":3.92325, "y":5.9398, "heading":-1.38948, "vx":3.98454, "vy":0.54642, "omega":-0.82567, "ax":0.27766, "ay":-0.83787, "alpha":0.0827, "fx":[4.86108,4.51576,4.59116,4.92346], "fy":[-13.97811,-14.07569,-14.52375,-14.43004]}, - {"t":1.27222, "x":3.99769, "y":5.94986, "heading":-1.4049, "vx":3.98972, "vy":0.53077, "omega":-0.82413, "ax":0.05927, "ay":-1.16697, "alpha":-0.62764, "fx":[-0.27881,2.84876,2.31164,-0.84871], "fy":[-21.53709,-21.0409,-18.15748,-18.6636]}, - {"t":1.29089, "x":4.0722, "y":5.95957, "heading":-1.42029, "vx":3.99083, "vy":0.50899, "omega":-0.83585, "ax":-0.28604, "ay":-1.69024, "alpha":-1.7586, "fx":[-8.50083,0.21719,-1.16504,-10.01314], "fy":[-33.41005,-32.23262,-24.06672,-25.29254]}, - {"t":1.30956, "x":4.14666, "y":5.96877, "heading":-1.4359, "vx":3.98549, "vy":0.47743, "omega":-0.86869, "ax":-0.85782, "ay":-2.47698, "alpha":-3.59884, "fx":[-22.05437,-4.14588,-6.85633,-25.30849], "fy":[-51.23463,-49.57991,-32.89062,-34.8255]}, - {"t":1.32824, "x":4.22093, "y":5.97726, "heading":-1.45212, "vx":3.96947, "vy":0.43118, "omega":-0.93588, "ax":-1.7148, "ay":-3.63575, "alpha":-6.27301, "fx":[-41.80693,-10.33095,-15.42641,-49.10907], "fy":[-76.04683,-75.74538,-46.98485,-48.59516]}, - {"t":1.34691, "x":4.29475, "y":5.98467, "heading":-1.46959, "vx":3.93745, "vy":0.36329, "omega":-1.05301, "ax":-2.66143, "ay":-5.1915, "alpha":-8.97251, "fx":[-61.82453,-15.25038,-25.22367,-78.78202], "fy":[-103.58396,-109.98086,-71.9048,-67.75378]}, - {"t":1.36558, "x":4.3678, "y":5.99055, "heading":-1.48925, "vx":3.88776, "vy":0.26636, "omega":-1.22054, "ax":-3.25225, "ay":-6.69115, "alpha":-10.13525, "fx":[-73.18074,-15.25446,-31.46564,-101.37852], "fy":[-124.41537,-137.94193,-106.88365,-86.01711]}, - {"t":1.38425, "x":4.43982, "y":5.99436, "heading":-1.51204, "vx":3.82704, "vy":0.14142, "omega":-1.40978, "ax":-1.38672, "ay":-7.60048, "alpha":-3.76798, "fx":[-33.88269,-8.08295,-11.44207,-40.94314], "fy":[-132.47222,-135.08648,-126.60445,-122.96501]}, - {"t":1.40309, "x":4.51166, "y":5.99568, "heading":-1.5386, "vx":3.80092, "vy":-0.00174, "omega":-1.48075, "ax":-0.19573, "ay":-6.39241, "alpha":-0.03412, "fx":[-3.53949,-2.88258,-3.02918,-3.86576], "fy":[-108.47649,-108.57111,-108.99505,-108.88967]}, - {"t":1.42192, "x":4.58322, "y":5.99451, "heading":-1.56649, "vx":3.79723, "vy":-0.12214, "omega":-1.4814, "ax":0.79428, "ay":-4.96501, "alpha":2.70652, "fx":[21.33277,6.28782,6.02922,20.3919], "fy":[-78.29841,-78.94779,-90.6184,-89.94886]}, - {"t":1.44076, "x":4.65489, "y":5.99133, "heading":-1.59439, "vx":3.81219, "vy":-0.21566, "omega":-1.43042, "ax":1.19998, "ay":-4.01782, "alpha":3.80456, "fx":[31.24413,11.02316,10.04493,29.33278], "fy":[-59.44,-59.94733,-77.34539,-76.63503]}, - {"t":1.45959, "x":4.7269, "y":5.98655, "heading":-1.62133, "vx":3.83479, "vy":-0.29134, "omega":-1.35876, "ax":1.11343, "ay":-3.65067, "alpha":3.69887, "fx":[29.46327,10.16877,8.80808,27.31628], "fy":[-53.59668,-53.50892,-70.70828,-70.57347]}, - {"t":1.47843, "x":4.79933, "y":5.98042, "heading":-1.64692, "vx":3.85576, "vy":-0.3601, "omega":-1.28909, "ax":0.68709, "ay":-3.66952, "alpha":2.78035, "fx":[19.64594,5.22164,3.97087,17.91067], "fy":[-56.26343,-55.74121,-68.62238,-69.04284]}, - {"t":1.49726, "x":4.87208, "y":5.97298, "heading":-1.67121, "vx":3.86871, "vy":-0.42922, "omega":-1.23672, "ax":0.02761, "ay":-3.82872, "alpha":1.24459, "fx":[3.98814,-2.32128,-2.96084,3.17245], "fy":[-62.4071,-61.99465,-67.84855,-68.25174]}, - {"t":1.5161, "x":4.94495, "y":5.96422, "heading":-1.6945, "vx":3.86923, "vy":-0.50134, "omega":-1.21327, "ax":-0.78585, "ay":-3.94077, "alpha":-0.77931, "fx":[-15.70614,-11.25262,-10.93767,-15.57198], "fy":[-68.13295,-68.9096,-65.94388,-65.13938]}, - {"t":1.53494, "x":5.01769, "y":5.95408, "heading":-1.71735, "vx":3.85442, "vy":-0.57556, "omega":-1.22795, "ax":-1.69781, "ay":-3.99188, "alpha":-3.12207, "fx":[-37.59542,-21.04239,-19.66934,-37.21023], "fy":[-72.57035,-76.24789,-63.26686,-59.51764]}, - {"t":1.55377, "x":5.08999, "y":5.94253, "heading":-1.74048, "vx":3.82244, "vy":-0.65075, "omega":-1.28676, "ax":-2.6355, "ay":-4.04215, "alpha":-5.48771, "fx":[-59.1775,-31.27072,-29.0395,-59.82864], "fy":[-76.11565,-84.68082,-61.39202,-52.8348]}, - {"t":1.57261, "x":5.16152, "y":5.92955, "heading":-1.76472, "vx":3.7728, "vy":-0.72689, "omega":-1.39012, "ax":-3.49432, "ay":-4.07746, "alpha":-7.51358, "fx":[-77.62929,-41.12754,-38.53713,-80.45543], "fy":[-78.19075,-93.02339,-60.45681,-45.75482]}, - {"t":1.59144, "x":5.23197, "y":5.91514, "heading":-1.7909, "vx":3.70699, "vy":-0.80369, "omega":-1.53165, "ax":-4.19188, "ay":-4.03513, "alpha":-8.9543, "fx":[-91.502,-49.93211,-47.41225,-96.36406], "fy":[-77.98439,-98.98019,-59.25448,-38.32624]}, - {"t":1.61028, "x":5.30105, "y":5.89929, "heading":-1.81975, "vx":3.62803, "vy":-0.87969, "omega":-1.70031, "ax":-4.69521, "ay":-3.94856, "alpha":-9.72663, "fx":[-100.84442,-57.25466,-54.81275,-106.54496], "fy":[-76.26048,-101.99818,-58.22739,-32.16961]}, - {"t":1.62911, "x":5.36855, "y":5.88202, "heading":-1.85178, "vx":3.53959, "vy":-0.95407, "omega":-1.88351, "ax":-4.9797, "ay":-4.0252, "alpha":-9.73266, "fx":[-105.53259,-62.21847,-59.80061,-111.2613], "fy":[-76.06453,-104.25865,-61.70498,-31.84169]}, - {"t":1.64795, "x":5.43434, "y":5.86333, "heading":-1.88726, "vx":3.4458, "vy":-1.02988, "omega":-2.06683, "ax":-4.98359, "ay":-4.54424, "alpha":-8.71521, "fx":[-104.29786,-63.55722,-61.34222,-109.88015], "fy":[-81.56466,-108.70035,-75.11863,-43.80128]}, - {"t":1.66678, "x":5.49836, "y":5.84313, "heading":-1.92619, "vx":3.35193, "vy":-1.11548, "omega":-2.23099, "ax":-4.56431, "ay":-5.68868, "alpha":-6.16443, "fx":[-93.70859,-60.61316,-58.55446,-97.67452], "fy":[-96.63042,-116.83541,-99.20537,-74.38011]}, - {"t":1.68562, "x":5.56068, "y":5.82111, "heading":-1.96821, "vx":3.26596, "vy":-1.22263, "omega":-2.3471, "ax":-3.60915, "ay":-7.19368, "alpha":-1.79344, "fx":[-67.80018,-55.59074,-54.48868,-67.68274], "fy":[-121.13748,-127.00123,-123.85848,-117.45264]}, - {"t":1.70446, "x":5.62156, "y":5.7968, "heading":-2.01242, "vx":3.19797, "vy":-1.35813, "omega":-2.38088, "ax":-2.44436, "ay":-8.34793, "alpha":3.15359, "fx":[-25.9439,-52.21233,-55.55411,-32.60133], "fy":[-144.49956,-136.24114,-139.8105,-147.43259]}, - {"t":1.72329, "x":5.68136, "y":5.76974, "heading":-2.05726, "vx":3.15193, "vy":-1.51536, "omega":-2.32148, "ax":-1.57702, "ay":-8.92499, "alpha":7.02392, "fx":[14.86927,-50.25235,-60.10829,-11.80712], "fy":[-155.0377,-143.82975,-147.93327,-160.4454]}, - {"t":1.74213, "x":5.74045, "y":5.73961, "heading":-2.10099, "vx":3.12223, "vy":-1.68347, "omega":-2.18918, "ax":-1.13802, "ay":-9.16654, "alpha":9.50396, "fx":[41.58333,-49.74058,-66.17762,-3.09494], "fy":[-156.87274,-149.38126,-151.18387,-166.24315]}, - {"t":1.76096, "x":5.79906, "y":5.70628, "heading":-2.14222, "vx":3.10079, "vy":-1.85613, "omega":-2.01017, "ax":-0.99548, "ay":-9.26287, "alpha":11.0657, "fx":[56.81343,-51.02334,-72.90946,-0.61211], "fy":[-156.41262,-152.92728,-151.6017,-169.29335]}, - {"t":1.7798, "x":5.85729, "y":5.66967, "heading":-2.18009, "vx":3.08204, "vy":-2.0306, "omega":-1.80174, "ax":-1.02391, "ay":-9.29123, "alpha":12.12887, "fx":[65.30433,-53.96887,-79.74738,-1.25403], "fy":[-155.85491,-154.83888,-150.37864,-171.09227]}, - {"t":1.79863, "x":5.91516, "y":5.62978, "heading":-2.21402, "vx":3.06276, "vy":-2.20561, "omega":-1.57328, "ax":-1.16272, "ay":-9.27816, "alpha":12.94176, "fx":[69.82212,-58.78106,-86.54269,-3.60835], "fy":[-155.70407,-155.27381,-148.09518,-172.20264]}, - {"t":1.81747, "x":5.97264, "y":5.58659, "heading":-2.24366, "vx":3.04086, "vy":-2.38037, "omega":-1.32952, "ax":-1.4751, "ay":-9.18461, "alpha":13.94954, "fx":[72.16324,-69.84802,-94.94836,-7.73122], "fy":[-155.85529,-152.24336,-143.96549,-172.84655]}, - {"t":1.83631, "x":6.02965, "y":5.54012, "heading":-2.2687, "vx":3.01307, "vy":-2.55337, "omega":-1.06677, "ax":-2.45366, "ay":-9.10962, "alpha":13.36188, "fx":[49.49958,-90.50739,-103.44648,-22.48996], "fy":[-165.13066,-143.84484,-138.81292,-172.01998]}, - {"t":1.86154, "x":6.1049, "y":5.47279, "heading":-2.29562, "vx":2.95116, "vy":-2.78323, "omega":-0.72961, "ax":-3.1113, "ay":-8.91061, "alpha":12.83546, "fx":[31.77162,-100.70858,-109.20124,-33.55108], "fy":[-168.25539,-135.24282,-133.30777,-169.46165]}, - {"t":1.88677, "x":6.17837, "y":5.39973, "heading":-2.31403, "vx":2.87265, "vy":-3.00806, "omega":-0.40574, "ax":-4.35475, "ay":-8.51546, "alpha":10.42798, "fx":[-12.00648,-113.52216,-115.8097,-54.95375], "fy":[-168.61969,-122.89357,-125.65389,-162.21497]}, - {"t":1.912, "x":6.24947, "y":5.32112, "heading":-2.32426, "vx":2.76277, "vy":-3.22293, "omega":-0.14262, "ax":-6.66597, "ay":-7.11689, "alpha":4.04608, "fx":[-97.63638,-127.56448,-126.31687,-102.02722], "fy":[-134.25274,-106.22296,-110.49661,-133.25255]}, - {"t":1.93724, "x":6.31706, "y":5.23753, "heading":-2.32786, "vx":2.59457, "vy":-3.40251, "omega":-0.04052, "ax":-8.50847, "ay":-4.31058, "alpha":-3.98424, "fx":[-151.08395,-135.43705,-137.96188,-154.42387], "fy":[-65.7737,-92.72043,-83.83244,-50.9602]}, - {"t":1.96247, "x":6.37982, "y":5.1503, "heading":-2.32889, "vx":2.37988, "vy":-3.51128, "omega":-0.14106, "ax":-7.76322, "ay":3.26923, "alpha":-18.45892, "fx":[-165.97588,-155.2463,-91.53446,-115.44412], "fy":[33.10339,-56.30363,123.37511,122.25985]}, - {"t":1.9877, "x":6.4374, "y":5.06274, "heading":-2.33245, "vx":2.18399, "vy":-3.42878, "omega":-0.60682, "ax":-6.21224, "ay":5.68436, "alpha":-21.21455, "fx":[-155.93513,-167.56065,-15.17424,-84.00385], "fy":[72.02066,-1.53461,166.49396,149.77767]}, - {"t":2.01293, "x":6.49053, "y":4.97804, "heading":-2.34776, "vx":2.02724, "vy":-3.28535, "omega":-1.14212, "ax":-5.61628, "ay":6.28812, "alpha":-21.92083, "fx":[-151.12975,-168.81868,9.3651,-71.54168], "fy":[84.12236,15.97296,170.17763,157.56327]}, - {"t":2.03817, "x":6.5399, "y":4.89714, "heading":-2.37658, "vx":1.88553, "vy":-3.12669, "omega":-1.69524, "ax":-5.33112, "ay":6.48637, "alpha":-22.56796, "fx":[-148.72365,-169.80966,20.70744,-64.89725], "fy":[89.58995,20.07642,170.54299,161.11602]}, - {"t":2.0634, "x":6.58578, "y":4.82031, "heading":-2.41935, "vx":1.75101, "vy":-2.96302, "omega":-2.26469, "ax":-5.27105, "ay":6.76998, "alpha":-21.54256, "fx":[-145.53975,-169.31742,17.80347,-61.58223], "fy":[95.40428,30.78229,171.62749,162.80775]}, - {"t":2.08863, "x":6.62828, "y":4.7477, "heading":-2.4765, "vx":1.61801, "vy":-2.79219, "omega":-2.80827, "ax":-5.66546, "ay":8.28546, "alpha":-6.97161, "fx":[-120.68745,-125.80795,-62.20002,-76.77585], "fy":[125.59111,119.62628,162.14602,156.37008]}, - {"t":2.11386, "x":6.6673, "y":4.67988, "heading":-2.54736, "vx":1.47505, "vy":-2.58313, "omega":-2.98418, "ax":-5.54541, "ay":8.61761, "alpha":-0.31006, "fx":[-95.60968,-95.56402,-93.02014,-93.1095], "fy":[145.76828,145.77439,147.41122,147.37834]}, - {"t":2.1391, "x":6.70276, "y":4.61745, "heading":-2.62265, "vx":1.33513, "vy":-2.36569, "omega":-2.992, "ax":-5.3805, "ay":8.67735, "alpha":3.61477, "fx":[-75.76767,-77.81059,-104.4713,-108.03368], "fy":[157.13446,156.33905,139.94234,136.98104]}, - {"t":2.16433, "x":6.73473, "y":4.56052, "heading":-2.69815, "vx":1.19937, "vy":-2.14673, "omega":-2.90079, "ax":-5.22853, "ay":8.65416, "alpha":6.40572, "fx":[-61.12234,-65.30155,-108.84876,-120.47029], "fy":[163.48867,162.15933,136.81192,126.35913]}, - {"t":2.18956, "x":6.76333, "y":4.50911, "heading":-2.77134, "vx":1.06744, "vy":-1.92837, "omega":-2.73916, "ax":-5.08997, "ay":8.60675, "alpha":8.49277, "fx":[-50.71543,-55.43754,-110.07706,-130.08579], "fy":[167.08334,165.91192,135.98559,116.61283]}, - {"t":2.21479, "x":6.78865, "y":4.46319, "heading":-2.84046, "vx":0.939, "vy":-1.71119, "omega":-2.52486, "ax":-4.96311, "ay":8.5598, "alpha":10.04709, "fx":[-43.60568,-47.18198,-109.7338,-137.16261], "fy":[169.13263,168.52744,136.37297,108.3662]}, - {"t":2.24003, "x":6.81076, "y":4.42274, "heading":-2.90417, "vx":0.81377, "vy":-1.49521, "omega":-2.27135, "ax":-4.84882, "ay":8.52212, "alpha":11.18408, "fx":[-39.01379,-40.0666,-108.58636,-142.24169], "fy":[170.29199,170.41828,137.368,101.75677]}, - {"t":2.26526, "x":6.82975, "y":4.38772, "heading":-2.96148, "vx":0.69142, "vy":-1.28017, "omega":-1.98915, "ax":-4.74842, "ay":8.49463, "alpha":12.00512, "fx":[-36.28863,-33.85622,-107.06473,-145.86745], "fy":[170.92619,171.80461,138.61694,96.61727]}, - {"t":2.29049, "x":6.84569, "y":4.35812, "heading":-3.01167, "vx":0.57161, "vy":-1.06583, "omega":-1.68623, "ax":-4.66203, "ay":8.47506, "alpha":12.60051, "fx":[-34.85731,-28.42069,-105.43553,-148.48562], "fy":[171.25075,172.8222,139.90634,92.65409]}, - {"t":2.31573, "x":6.85862, "y":4.33393, "heading":-3.05422, "vx":0.45397, "vy":-0.85198, "omega":-1.36828, "ax":-4.58826, "ay":8.46036, "alpha":13.04675, "fx":[-34.1919,-23.68156,-103.87631,-150.43014], "fy":[171.40811,173.5639,141.10456,89.55638]}, - {"t":2.34096, "x":6.86862, "y":4.31512, "heading":-3.08875, "vx":0.3382, "vy":-0.63851, "omega":-1.03908, "ax":-4.52442, "ay":8.44782, "alpha":13.40577, "fx":[-33.79329,-19.58873,-102.51163,-151.94238], "fy":[171.50824,174.09755,142.12886,87.04515]}, - {"t":2.36619, "x":6.87571, "y":4.3017, "heading":-3.11497, "vx":0.22403, "vy":-0.42535, "omega":-0.70081, "ax":-4.46693, "ay":8.43539, "alpha":13.72642, "fx":[-33.18714,-16.11004,-101.43269,-153.19518], "fy":[171.64658,174.47474,142.92553,84.88756]}, - {"t":2.39142, "x":6.87994, "y":4.29365, "heading":-3.13265, "vx":0.11132, "vy":-0.2125, "omega":-0.35446, "ax":-4.41181, "ay":8.42156, "alpha":14.04771, "fx":[-31.92685,-13.22648,-100.70878,-154.31265], "fy":[171.90522,174.73544,143.45733,82.89506]}, - {"t":2.41666, "x":6.88135, "y":4.29097, "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":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.87948, "ay":3.14855, "alpha":-33.63363, "fx":[164.77106,120.72737,-2.70988,-18.83316], "fy":[-8.36323,116.62028,164.59757,-58.63118]}, + {"t":0.02814, "x":0.44786, "y":4.02725, "heading":0.0, "vx":0.10915, "vy":0.08859, "omega":-0.94633, "ax":7.99122, "ay":6.49929, "alpha":-0.35492, "fx":[137.45223,135.78204,134.38737,136.092], "fy":[108.66497,110.74989,112.43204,110.3573]}, + {"t":0.05627, "x":0.4541, "y":4.03231, "heading":-0.02663, "vx":0.334, "vy":0.27146, "omega":-0.95632, "ax":7.99041, "ay":6.49864, "alpha":-0.2905, "fx":[137.16623,135.82682,134.65208,136.01323], "fy":[108.99267,110.66196,112.08311,110.42262]}, + {"t":0.08441, "x":0.46666, "y":4.04252, "heading":-0.05353, "vx":0.55882, "vy":0.4543, "omega":-0.96449, "ax":7.98943, "ay":6.49786, "alpha":-0.21516, "fx":[136.82757,135.85687,134.96261,135.94444], "fy":[109.37902,110.58621,111.67165,110.47037]}, + {"t":0.11255, "x":0.48554, "y":4.05788, "heading":-0.08067, "vx":0.78361, "vy":0.63713, "omega":-0.97054, "ax":7.98822, "ay":6.4969, "alpha":-0.12591, "fx":[136.42246,135.86747,135.33033,135.88886], "fy":[109.8385,110.5268,111.18119,110.49528]}, + {"t":0.14068, "x":0.51075, "y":4.07837, "heading":-0.10798, "vx":1.00837, "vy":0.81993, "omega":-0.97409, "ax":7.98669, "ay":6.49569, "alpha":-0.01849, "fx":[135.93154,135.85202,135.77115,135.85074], "fy":[110.39121,110.48948,110.58842,110.49021]}, + {"t":0.16882, "x":0.54229, "y":4.10402, "heading":-0.13539, "vx":1.23309, "vy":1.00269, "omega":-0.97461, "ax":7.98473, "ay":6.49412, "alpha":0.11322, "fx":[135.3267,135.80098,136.30791,135.836], "fy":[111.06585,110.48245,109.85924,110.44515]}, + {"t":0.19695, "x":0.58014, "y":4.1348, "heading":-0.16281, "vx":1.45775, "vy":1.18542, "omega":-0.97142, "ax":7.9821, "ay":6.49203, "alpha":0.27848, "fx":[134.56534,135.69988,136.97476,135.85303], "fy":[111.90507,110.51796,108.94163,110.34563]}, + {"t":0.22509, "x":0.62432, "y":4.17072, "heading":-0.19014, "vx":1.68234, "vy":1.36808, "omega":-0.96358, "ax":7.97845, "ay":6.48912, "alpha":0.49195, "fx":[133.5798,135.52557,137.82511,135.91442], "fy":[112.97503,110.61535,107.75219,110.16962]}, + {"t":0.25323, "x":0.67481, "y":4.21178, "heading":-0.21725, "vx":1.90682, "vy":1.55066, "omega":-0.94974, "ax":7.97311, "ay":6.48484, "alpha":0.7782, "fx":[132.25564,135.23827,138.9473,136.03996], "fy":[114.38422,110.80729,106.14817,109.88168]}, + {"t":0.28136, "x":0.73162, "y":4.25798, "heading":-0.24397, "vx":2.13116, "vy":1.73312, "omega":-0.92785, "ax":7.96468, "ay":6.4781, "alpha":1.18197, "fx":[130.3831,134.76288,140.49836,136.26314], "fy":[116.32357,111.15428,103.86391,109.42051]}, + {"t":0.3095, "x":0.79473, "y":4.30931, "heading":-0.27008, "vx":2.35526, "vy":1.91539, "omega":-0.89459, "ax":7.94989, "ay":6.46628, "alpha":1.79388, "fx":[127.53299,133.93759,142.785,136.64595], "fy":[119.16167,111.78349,100.34378,108.66919]}, + {"t":0.33764, "x":0.86415, "y":4.36576, "heading":-0.29525, "vx":2.57894, "vy":2.09733, "omega":-0.84412, "ax":7.91926, "ay":6.44199, "alpha":2.82911, "fx":[122.67559,132.33545,146.48784,137.31868], "fy":[123.70688,113.01881,94.20821,107.37192]}, + {"t":0.36577, "x":0.93984, "y":4.42732, "heading":-0.319, "vx":2.80176, "vy":2.27858, "omega":-0.76452, "ax":7.83323, "ay":6.37654, "alpha":4.94849, "fx":[112.6295,128.3394,153.38438,138.61094], "fy":[132.08124,116.00997,80.92391,104.8371]}, + {"t":0.39391, "x":1.02178, "y":4.49396, "heading":-0.34051, "vx":3.02215, "vy":2.45799, "omega":-0.62528, "ax":7.30577, "ay":6.0886, "alpha":11.59121, "fx":[81.96683,106.14043,167.34416,141.6246], "fy":[150.95227,129.63123,35.32825,98.34975]}, + {"t":0.42205, "x":1.1097, "y":4.56552, "heading":-0.3581, "vx":3.22771, "vy":2.6293, "omega":-0.29915, "ax":3.55729, "ay":2.81037, "alpha":10.60415, "fx":[41.80946,29.7427,83.12136,87.3605], "fy":[84.88368,38.38662,9.79704,58.14715]}, + {"t":0.45018, "x":1.20192, "y":4.64062, "heading":-0.36652, "vx":3.3278, "vy":2.70838, "omega":-0.00079, "ax":0.1164, "ay":-0.13444, "alpha":0.00745, "fx":[1.95463,2.08188,2.06185,1.82111], "fy":[-2.13407,-2.40124,-2.38279,-2.229]}, + {"t":0.47832, "x":1.2956, "y":4.71677, "heading":-0.36654, "vx":3.33108, "vy":2.7046, "omega":-0.00058, "ax":0.3533, "ay":-0.43679, "alpha":0.0, "fx":[5.99452,6.01524,6.02437,6.00425], "fy":[-7.44094,-7.45252,-7.4183,-7.40674]}, + {"t":0.50646, "x":1.38947, "y":4.79269, "heading":-0.36656, "vx":3.34102, "vy":2.69231, "omega":-0.00058, "ax":1.07188, "ay":-1.34578, "alpha":-0.00003, "fx":[18.21509,18.2563,18.2482,18.20987], "fy":[-22.87249,-22.8907,-22.90602,-22.89586]}, + {"t":0.53459, "x":1.4839, "y":4.86791, "heading":-0.36658, "vx":3.37118, "vy":2.65444, "omega":-0.00058, "ax":2.62271, "ay":-3.4503, "alpha":-0.02079, "fx":[44.92858,45.12694,43.99986,44.39095], "fy":[-58.3018,-58.9146,-58.97923,-58.55866]}, + {"t":0.56273, "x":1.57979, "y":4.94123, "heading":-0.36659, "vx":3.44497, "vy":2.55736, "omega":-0.00116, "ax":-0.41591, "ay":-7.59516, "alpha":-11.36915, "fx":[15.73999,52.04338,-47.11196,-48.9692], "fy":[-146.88238,-123.91482,-109.86267,-136.10593]}, + {"t":0.59086, "x":1.67655, "y":5.01018, "heading":-0.36663, "vx":3.43327, "vy":2.34366, "omega":-0.32105, "ax":-3.09108, "ay":-7.02992, "alpha":-15.01405, "fx":[-19.20667,17.33978,-113.03743,-95.40926], "fy":[-151.77788,-131.65734,-74.75773,-120.11448]}, + {"t":0.60872, "x":1.73737, "y":5.05091, "heading":-0.37236, "vx":3.37807, "vy":2.21813, "omega":-0.58915, "ax":-3.21903, "ay":-6.13773, "alpha":-13.98731, "fx":[-25.08431,3.50315,-103.30174,-94.13643], "fy":[-139.54834,-110.39547,-59.92221,-107.73832]}, + {"t":0.62658, "x":1.79717, "y":5.08954, "heading":-0.38288, "vx":3.32059, "vy":2.10853, "omega":-0.83892, "ax":-2.99875, "ay":-4.84463, "alpha":-11.89672, "fx":[-29.53648,-7.77401,-81.30428,-85.41651], "fy":[-117.47151,-77.9985,-44.1441,-90.00896]}, + {"t":0.64443, "x":1.85599, "y":5.12642, "heading":-0.39786, "vx":3.26704, "vy":2.02202, "omega":-1.05135, "ax":-2.26177, "ay":-3.10199, "alpha":-8.35968, "fx":[-26.25297,-10.56544,-53.01124,-64.05842], "fy":[-79.13492,-44.84375,-25.85062,-61.2266]}, + {"t":0.66229, "x":1.91397, "y":5.16203, "heading":-0.41663, "vx":3.22666, "vy":1.96663, "omega":-1.20063, "ax":-1.38975, "ay":-1.44402, "alpha":-4.72512, "fx":[-17.60743,-8.21406,-30.09787,-38.63767], "fy":[-39.71192,-19.24294,-9.32387,-29.9708]}, + {"t":0.68015, "x":1.97136, "y":5.19692, "heading":-0.43807, "vx":3.20184, "vy":1.94085, "omega":-1.285, "ax":-0.77551, "ay":-0.32983, "alpha":-2.26072, "fx":[-10.56312,-5.80052,-15.88473,-20.51663], "fy":[-12.91019,-3.04581,1.70587,-8.19124]}, + {"t":0.698, "x":2.02841, "y":5.23152, "heading":-0.46102, "vx":3.18799, "vy":1.93496, "omega":-1.32537, "ax":-0.44824, "ay":0.29177, "alpha":-0.9394, "fx":[-6.62335,-4.52149,-8.63497,-10.71767], "fy":[1.91892,5.9818,8.01239,3.93844]}, + {"t":0.71586, "x":2.08527, "y":5.26612, "heading":-0.48468, "vx":3.17999, "vy":1.94017, "omega":-1.34215, "ax":-0.32746, "ay":0.58535, "alpha":-0.40706, "fx":[-5.17708,-4.20567,-5.96459,-6.9324], "fy":[8.63783,10.37247,11.27894,9.53722]}, + {"t":0.73372, "x":2.142, "y":5.30086, "heading":-0.50865, "vx":3.17414, "vy":1.95062, "omega":-1.34941, "ax":-0.32833, "ay":0.68141, "alpha":-0.34795, "fx":[-5.27663,-4.41033,-5.89345,-6.75875], "fy":[10.45753,11.92381,12.72669,11.25453]}, + {"t":0.75157, "x":2.19863, "y":5.3358, "heading":-0.53275, "vx":3.16828, "vy":1.96279, "omega":-1.35563, "ax":-0.37841, "ay":0.66018, "alpha":-0.52885, "fx":[-6.00195,-4.64982,-6.87137,-8.22314], "fy":[9.48839,11.69565,12.97336,10.76035]}, + {"t":0.76943, "x":2.25514, "y":5.37095, "heading":-0.55695, "vx":3.16152, "vy":1.97458, "omega":-1.36507, "ax":-0.41618, "ay":0.5587, "alpha":-0.7851, "fx":[-6.49185,-4.41784,-7.66666,-9.74031], "fy":[6.89748,10.13255,12.11237,8.87065]}, + {"t":0.78729, "x":2.31153, "y":5.4063, "heading":-0.58133, "vx":3.15409, "vy":1.98455, "omega":-1.37909, "ax":-0.38754, "ay":0.37615, "alpha":-1.00634, "fx":[-5.92017,-3.16348,-7.26393,-10.02026], "fy":[3.04034,7.12375,9.76117,5.66783]}, + {"t":0.80514, "x":2.36779, "y":5.4418, "heading":-0.60595, "vx":3.14717, "vy":1.99127, "omega":-1.39706, "ax":-0.24237, "ay":0.07383, "alpha":-1.12878, "fx":[-3.46672,-0.2517,-4.77813,-7.99409], "fy":[-2.51937,1.98031,5.03968,0.5227]}, + {"t":0.823, "x":2.42395, "y":5.47737, "heading":-0.6309, "vx":3.14284, "vy":1.99259, "omega":-1.41722, "ax":0.06782, "ay":-0.42897, "alpha":-1.13179, "fx":[1.69996,5.07198,0.60976,-2.76746], "fy":[-11.07269,-6.64991,-3.50623,-7.95777]}, + {"t":0.84086, "x":2.48008, "y":5.51288, "heading":-0.65621, "vx":3.14405, "vy":1.98493, "omega":-1.43743, "ax":0.5872, "ay":-1.25715, "alpha":-1.03669, "fx":[10.35534,13.6375,9.62755,6.33205], "fy":[-24.80111,-20.83508,-17.94503,-21.95353]}, + {"t":0.85871, "x":2.53632, "y":5.54812, "heading":-0.68188, "vx":3.15454, "vy":1.96248, "omega":-1.45594, "ax":1.33557, "ay":-2.5563, "alpha":-0.89993, "fx":[22.85195,25.99538,22.59656,19.42645], "fy":[-46.3479,-42.97461,-40.58761,-44.01783]}, + {"t":0.87657, "x":2.59286, "y":5.58276, "heading":-0.70787, "vx":3.17839, "vy":1.91683, "omega":-1.47201, "ax":2.23803, "ay":-4.35942, "alpha":-0.78316, "fx":[37.88205,41.13998,38.27984,34.97107], "fy":[-76.42717,-73.5352,-71.84254,-74.80481]}, + {"t":0.89443, "x":2.64997, "y":5.61629, "heading":-0.73416, "vx":3.21835, "vy":1.83899, "omega":-1.48599, "ax":2.98522, "ay":-6.21807, "alpha":-0.70462, "fx":[50.13655,53.9318,51.46809,47.57476], "fy":[-107.42026,-104.83681,-104.08636,-106.7267]}, + {"t":0.91228, "x":2.70792, "y":5.64814, "heading":-0.76069, "vx":3.27166, "vy":1.72795, "omega":-1.49857, "ax":3.27148, "ay":-7.47629, "alpha":-0.64837, "fx":[54.7235,58.85566,56.62992,52.37869], "fy":[-128.39042,-126.08928,-125.93923,-128.259]}, + {"t":0.93014, "x":2.76686, "y":5.6778, "heading":-0.78745, "vx":3.33007, "vy":1.59445, "omega":-1.51015, "ax":3.26645, "ay":-8.27208, "alpha":-0.53311, "fx":[54.65643,58.36121,56.50782,52.72035], "fy":[-141.55641,-139.78565,-139.85319,-141.62739]}, + {"t":0.948, "x":2.82684, "y":5.70496, "heading":-0.81442, "vx":3.3884, "vy":1.44674, "omega":-1.51967, "ax":3.14526, "ay":-8.78793, "alpha":-0.2827, "fx":[52.96089,55.05937,54.0514,51.92854], "fy":[-149.86152,-148.99209,-149.09965,-149.9676]}, + {"t":0.96585, "x":2.88785, "y":5.72939, "heading":-0.84156, "vx":3.44456, "vy":1.28982, "omega":-1.52472, "ax":3.22836, "ay":-8.79319, "alpha":0.94723, "fx":[57.04004,49.62282,52.94619,60.04487], "fy":[-148.26728,-151.24276,-150.87281,-147.89573]}, + {"t":0.98454, "x":2.95278, "y":5.75196, "heading":-0.87005, "vx":3.5049, "vy":1.12549, "omega":-1.50702, "ax":2.89966, "ay":-8.43562, "alpha":1.3926, "fx":[52.32616,41.75345,46.62105,56.58866], "fy":[-141.34527,-145.52457,-145.62459,-141.4555]}, + {"t":1.00323, "x":3.01879, "y":5.77152, "heading":-0.89821, "vx":3.55908, "vy":0.96785, "omega":-1.48099, "ax":2.42308, "ay":-7.79742, "alpha":1.45968, "fx":[43.91938,33.76853,38.75896,48.41653], "fy":[-129.95183,-134.09688,-135.28518,-131.19344]}, + {"t":1.02192, "x":3.08572, "y":5.78824, "heading":-0.92589, "vx":3.60436, "vy":0.82214, "omega":-1.45372, "ax":1.73285, "ay":-6.60003, "alpha":0.9942, "fx":[30.60283,25.24754,28.39419,33.6569], "fy":[-109.66536,-112.42839,-114.81551,-112.14896]}, + {"t":1.0406, "x":3.15338, "y":5.80245, "heading":-0.95305, "vx":3.63675, "vy":0.6988, "omega":-1.43514, "ax":0.81144, "ay":-4.56505, "alpha":-0.10924, "fx":[13.63467,14.3746,13.98206,13.21832], "fy":[-77.81232,-77.59345,-77.48058,-77.71467]}, + {"t":1.05929, "x":3.22149, "y":5.81471, "heading":-0.97987, "vx":3.65191, "vy":0.61349, "omega":-1.43718, "ax":0.00946, "ay":-2.49763, "alpha":-1.30297, "fx":[-0.79335,4.83092,1.13986,-4.53359], "fy":[-46.59112,-43.25903,-38.34001,-41.74567]}, + {"t":1.07798, "x":3.28973, "y":5.82574, "heading":-1.00673, "vx":3.65209, "vy":0.56681, "omega":-1.46153, "ax":-0.49826, "ay":-0.96538, "alpha":-2.16848, "fx":[-10.14489,-1.03149,-6.79113,-15.93377], "fy":[-23.51874,-18.04386,-9.27568,-14.84526]}, + {"t":1.09667, "x":3.35789, "y":5.83617, "heading":-1.03404, "vx":3.64278, "vy":0.54877, "omega":-1.50205, "ax":-0.75256, "ay":0.04109, "alpha":-2.66254, "fx":[-15.10758,-3.8242,-10.50543,-21.76604], "fy":[-8.08586,-1.54047,9.51795,2.90441]}, + {"t":1.11535, "x":3.42584, "y":5.84643, "heading":-1.06211, "vx":3.62871, "vy":0.54954, "omega":-1.55181, "ax":-0.82723, "ay":0.66301, "alpha":-2.86112, "fx":[-16.86397,-4.52138,-11.31352,-23.58516], "fy":[1.86511,8.63932,20.70348,13.9026]}, + {"t":1.13404, "x":3.49351, "y":5.85681, "heading":-1.09111, "vx":3.61325, "vy":0.56193, "omega":-1.60527, "ax":-0.78121, "ay":1.02338, "alpha":-2.84879, "fx":[-16.37779,-3.85502,-10.24551,-22.67408], "fy":[8.11261,14.53842,26.69906,20.27978]}, + {"t":1.15273, "x":3.56089, "y":5.86749, "heading":-1.12111, "vx":3.59866, "vy":0.58106, "omega":-1.65851, "ax":-0.65414, "ay":1.20161, "alpha":-2.68929, "fx":[-14.31872,-2.29717,-7.97954,-19.91155], "fy":[11.76223,17.47866,29.10394,23.41107]}, + {"t":1.17142, "x":3.62803, "y":5.87856, "heading":-1.15211, "vx":3.58643, "vy":0.60351, "omega":-1.70877, "ax":-0.47001, "ay":1.23779, "alpha":-2.42168, "fx":[-11.10216,-0.12578,-4.9199,-15.83138], "fy":[13.34085,18.14105,28.75485,23.98073]}, + {"t":1.19011, "x":3.69497, "y":5.89006, "heading":-1.18404, "vx":3.57765, "vy":0.62664, "omega":-1.75402, "ax":-0.24163, "ay":1.14014, "alpha":-2.06452, "fx":[-6.94998,2.51296,-1.28604,-10.71697], "fy":[12.90818,16.68693,25.86905,22.10967]}, + {"t":1.20879, "x":3.76178, "y":5.90197, "heading":-1.21682, "vx":3.57313, "vy":0.64795, "omega":-1.79261, "ax":0.02475, "ay":0.8893, "alpha":-1.62322, "fx":[-1.96355,5.5493,2.80433,-4.70606], "fy":[10.10265,12.82283,20.14611,17.43517]}, + {"t":1.22748, "x":3.82856, "y":5.91423, "heading":-1.25032, "vx":3.57359, "vy":0.66457, "omega":-1.82294, "ax":0.32289, "ay":0.43977, "alpha":-1.10337, "fx":[3.75686,8.92124,7.23515,2.05564], "fy":[4.12285,5.80612,10.83764,9.15497]}, + {"t":1.24617, "x":3.8954, "y":5.92673, "heading":-1.28438, "vx":3.57963, "vy":0.67279, "omega":-1.84356, "ax":0.63699, "ay":-0.27719, "alpha":-0.53149, "fx":[9.92223,12.46913,11.7569,9.19159], "fy":[-6.29023,-5.54956,-3.13783,-3.88218]}, + {"t":1.26486, "x":3.96241, "y":5.93925, "heading":-1.31883, "vx":3.59153, "vy":0.66761, "omega":-1.85349, "ax":0.92758, "ay":-1.33725, "alpha":0.01613, "fx":[15.76338,15.76376,15.79928,15.78478], "fy":[-22.6632,-22.66756,-22.82735,-22.82686]}, + {"t":1.28354, "x":4.02969, "y":5.95149, "heading":-1.35347, "vx":3.60887, "vy":0.64262, "omega":-1.85319, "ax":1.11679, "ay":-2.76117, "alpha":0.39168, "fx":[19.71261,17.8877,18.28654,20.09822], "fy":[-45.80162,-46.22195,-48.13036,-47.7129]}, + {"t":1.30223, "x":4.09732, "y":5.96302, "heading":-1.3881, "vx":3.62974, "vy":0.59102, "omega":-1.84587, "ax":1.09807, "ay":-4.35563, "alpha":0.41356, "fx":[19.548,17.48763,17.81487,19.86103], "fy":[-72.93218,-73.33351,-75.24259,-74.84375]}, + {"t":1.32092, "x":4.16534, "y":5.9733, "heading":-1.4226, "vx":3.65026, "vy":0.50962, "omega":-1.83814, "ax":0.83662, "ay":-5.5931, "alpha":0.05318, "fx":[14.35503,14.08142,14.10895,14.37721], "fy":[-94.99433,-95.03715,-95.27946,-95.23752]}, + {"t":1.33961, "x":4.2337, "y":5.98185, "heading":-1.45695, "vx":3.66589, "vy":0.4051, "omega":-1.83715, "ax":0.48735, "ay":-6.20897, "alpha":-0.37671, "fx":[7.28327,9.49203,9.30405,7.07942], "fy":[-106.44017,-106.21373,-104.78435,-105.01276]}, + {"t":1.35829, "x":4.3023, "y":5.98834, "heading":-1.49128, "vx":3.675, "vy":0.28907, "omega":-1.84419, "ax":0.26052, "ay":-6.64717, "alpha":-0.43665, "fx":[3.20193,5.83327,5.67244,3.01791], "fy":[-113.94976,-113.77985,-112.18217,-112.35425]}, + {"t":1.37698, "x":4.37102, "y":5.99258, "heading":-1.52574, "vx":3.67987, "vy":0.16485, "omega":-1.85235, "ax":0.21363, "ay":-7.44512, "alpha":0.1378, "fx":[4.21934,3.03798,3.05825,4.21979], "fy":[-126.53455,-126.5741,-126.74378,-126.70494]}, + {"t":1.39567, "x":4.43982, "y":5.99436, "heading":-1.56036, "vx":3.68386, "vy":0.02571, "omega":-1.84977, "ax":0.49994, "ay":-7.51898, "alpha":1.88427, "fx":[15.78219,1.62763,1.65319,14.95255], "fy":[-125.25007,-125.91495,-130.51656,-129.90115]}, + {"t":1.41461, "x":4.50969, "y":5.9935, "heading":-1.5954, "vx":3.69333, "vy":-0.1167, "omega":-1.81408, "ax":0.51883, "ay":-6.61801, "alpha":2.36444, "fx":[17.17572,1.33814,0.9174,15.86903], "fy":[-108.57531,-108.9333,-116.5749,-116.19818]}, + {"t":1.43355, "x":4.57974, "y":5.9901, "heading":-1.62976, "vx":3.70316, "vy":-0.24205, "omega":-1.7693, "ax":0.18966, "ay":-6.13054, "alpha":1.84266, "fx":[9.40074,-2.20341,-2.73108,8.43786], "fy":[-101.15884,-100.86477,-107.41723,-107.67414]}, + {"t":1.45249, "x":4.64991, "y":5.98442, "heading":-1.66327, "vx":3.70675, "vy":-0.35817, "omega":-1.7344, "ax":-0.37038, "ay":-5.94137, "alpha":0.76158, "fx":[-3.81632,-8.4847,-8.74795,-4.15125], "fy":[-99.87482,-99.49214,-102.25091,-102.62614]}, + {"t":1.47143, "x":4.72006, "y":5.97657, "heading":-1.69612, "vx":3.69973, "vy":-0.4707, "omega":-1.71997, "ax":-1.00034, "ay":-5.80663, "alpha":-0.49183, "fx":[-18.55229,-15.66122,-15.46304,-18.38522], "fy":[-99.48619,-99.89896,-98.05378,-97.6375]}, + {"t":1.49037, "x":4.78995, "y":5.96661, "heading":-1.7287, "vx":3.68079, "vy":-0.58069, "omega":-1.72929, "ax":-1.5509, "ay":-5.53292, "alpha":-1.55281, "fx":[-31.07524,-22.31054,-21.56036,-30.57565], "fy":[-96.26457,-98.00968,-91.97375,-90.20579]}, + {"t":1.50931, "x":4.85939, "y":5.95462, "heading":-1.76145, "vx":3.65141, "vy":-0.68549, "omega":-1.7587, "ax":-1.84709, "ay":-4.79737, "alpha":-2.02199, "fx":[-37.31915,-26.75181,-25.37508,-36.22759], "fy":[-84.66988,-87.16341,-78.53847,-76.03569]}, + {"t":1.52826, "x":4.92822, "y":5.94077, "heading":-1.79476, "vx":3.61642, "vy":-0.77635, "omega":-1.797, "ax":-1.7162, "ay":-3.44506, "alpha":-1.58757, "fx":[-33.78497,-26.06433,-24.54945,-32.3696], "fy":[-61.27175,-63.23626,-55.92763,-53.96235]}, + {"t":1.5472, "x":4.99641, "y":5.92545, "heading":-1.8288, "vx":3.58392, "vy":-0.8416, "omega":-1.82707, "ax":-1.23971, "ay":-1.97757, "alpha":-0.4892, "fx":[-22.48364,-20.28584,-19.68211,-21.8968], "fy":[-34.53636,-35.14378,-32.74144,-32.13009]}, + {"t":1.56614, "x":5.06407, "y":5.90916, "heading":-1.86341, "vx":3.56044, "vy":-0.87906, "omega":-1.83633, "ax":-0.65449, "ay":-0.82806, "alpha":0.75503, "fx":[-8.77392,-12.46166,-13.4795,-9.81584], "fy":[-12.95814,-11.84977,-15.21447,-16.31785]}, + {"t":1.58508, "x":5.13139, "y":5.89236, "heading":-1.89819, "vx":3.54804, "vy":-0.89475, "omega":-1.82203, "ax":-0.14133, "ay":-0.12663, "alpha":1.77627, "fx":[3.16553,-5.21514,-7.95789,0.39147], "fy":[0.44558,3.26251,-4.75678,-7.56714]}, + {"t":1.60402, "x":5.19857, "y":5.87539, "heading":-1.9327, "vx":3.54536, "vy":-0.89714, "omega":-1.78839, "ax":0.19843, "ay":0.11721, "alpha":2.39719, "fx":[10.99718,-0.13932,-4.23401,6.87741], "fy":[5.27,9.4336,-1.28984,-5.43886]}, + {"t":1.62296, "x":5.26576, "y":5.85841, "heading":-1.96657, "vx":3.54912, "vy":-0.89492, "omega":-1.74298, "ax":0.30632, "ay":-0.10968, "alpha":2.5535, "fx":[13.46379,1.72524,-3.02576,8.67866], "fy":[1.34002,6.16409,-5.08742,-9.87891]}, + {"t":1.6419, "x":5.33304, "y":5.84144, "heading":-1.99959, "vx":3.55492, "vy":-0.897, "omega":-1.69462, "ax":0.13479, "ay":-0.8751, "alpha":2.23167, "fx":[9.66929,-0.57698,-5.05145,5.13019], "fy":[-12.37433,-7.8319,-17.42269,-21.91173]}, + {"t":1.66084, "x":5.40039, "y":5.8243, "heading":-2.03168, "vx":3.55748, "vy":-0.91358, "omega":-1.65235, "ax":-0.36365, "ay":-2.29263, "alpha":1.46379, "fx":[-1.14384,-8.06546,-11.18759,-4.34559], "fy":[-37.64728,-34.45158,-40.37427,-43.51456]}, + {"t":1.67978, "x":5.46771, "y":5.80658, "heading":-2.06298, "vx":3.55059, "vy":-0.957, "omega":-1.62462, "ax":-1.19465, "ay":-4.37166, "alpha":0.41547, "fx":[-18.66399,-21.05631,-21.95445,-19.60821], "fy":[-74.25911,-73.26195,-74.47396,-75.44804]}, + {"t":1.69872, "x":5.53475, "y":5.78767, "heading":-2.09375, "vx":3.52796, "vy":-1.0398, "omega":-1.61675, "ax":-2.14001, "ay":-6.51024, "alpha":-0.43342, "fx":[-37.98719,-35.83818,-34.80377,-36.97454], "fy":[-110.94478,-112.02512,-110.53882,-109.44046]}, + {"t":1.71766, "x":5.60119, "y":5.76681, "heading":-2.12438, "vx":3.48743, "vy":-1.16311, "omega":-1.62496, "ax":-2.82518, "ay":-7.74154, "alpha":-0.65397, "fx":[-51.03848,-46.4788,-45.01828,-49.68664], "fy":[-131.11206,-133.04968,-132.26596,-130.29809]}, + {"t":1.7366, "x":5.66673, "y":5.74339, "heading":-2.15515, "vx":3.43392, "vy":-1.30975, "omega":-1.63735, "ax":-3.37149, "ay":-8.33226, "alpha":-0.61998, "fx":[-60.32928,-55.60586,-54.29426,-59.16324], "fy":[-140.81298,-142.88755,-142.66292,-140.55421]}, + {"t":1.75555, "x":5.73117, "y":5.71709, "heading":-2.18617, "vx":3.37006, "vy":-1.46757, "omega":-1.64909, "ax":-3.92381, "ay":-8.5626, "alpha":-0.68069, "fx":[-70.01304,-64.71118,-63.37455,-68.87279], "fy":[-144.37799,-146.93436,-146.9445,-144.33244]}, + {"t":1.77449, "x":5.7943, "y":5.68775, "heading":-2.2174, "vx":3.29574, "vy":-1.62975, "omega":-1.66198, "ax":-4.51444, "ay":-8.56015, "alpha":-0.92651, "fx":[-81.10529,-73.92375,-72.28724,-79.84078], "fy":[-143.56727,-147.48326,-147.71028,-143.66172]}, + {"t":1.79343, "x":5.85591, "y":5.65535, "heading":-2.24888, "vx":3.21023, "vy":-1.79189, "omega":-1.67953, "ax":-5.10016, "ay":-8.41586, "alpha":-1.23752, "fx":[-92.23666,-82.85228,-80.93385,-90.98615], "fy":[-140.07818,-145.89923,-146.37451,-140.25336]}, + {"t":1.81237, "x":5.9158, "y":5.6199, "heading":-2.28069, "vx":3.11363, "vy":-1.95129, "omega":-1.70297, "ax":-5.61407, "ay":-8.21401, "alpha":-1.4002, "fx":[-101.39666,-91.05363,-89.1664,-100.35814], "fy":[-135.91497,-143.10458,-143.75067,-136.10177]}, + {"t":1.83131, "x":5.97377, "y":5.58147, "heading":-2.31295, "vx":3.00729, "vy":-2.10687, "omega":-1.72949, "ax":-6.01209, "ay":-8.02378, "alpha":-1.33089, "fx":[-107.68711,-98.05337,-96.46173,-106.85374], "fy":[-132.59968,-139.91191,-140.59643,-132.82065]}, + {"t":1.85025, "x":6.02965, "y":5.54012, "heading":-2.34571, "vx":2.89342, "vy":-2.25885, "omega":-1.7547, "ax":-5.99615, "ay":-8.12819, "alpha":0.62686, "fx":[-99.20083,-104.04234,-104.69804,-100.02994], "fy":[-140.24185,-136.66554,-136.32167,-139.80336]}, + {"t":1.87707, "x":6.10509, "y":5.47662, "heading":-2.39276, "vx":2.73261, "vy":-2.47683, "omega":-1.73789, "ax":-6.43153, "ay":-7.65315, "alpha":0.89715, "fx":[-105.62439,-112.20626,-113.00607,-106.75715], "fy":[-133.18099,-127.63852,-127.28437,-132.60786]}, + {"t":1.90389, "x":6.17606, "y":5.40745, "heading":-2.43937, "vx":2.56013, "vy":-2.68207, "omega":-1.71383, "ax":-6.81601, "ay":-7.03561, "alpha":0.92478, "fx":[-112.35686,-118.60507,-119.3613,-113.43037], "fy":[-122.85172,-116.76377,-116.624,-122.45545]}, + {"t":1.9307, "x":6.24227, "y":5.33299, "heading":-2.48533, "vx":2.37734, "vy":-2.87075, "omega":-1.68903, "ax":-6.95424, "ay":-6.19493, "alpha":0.84539, "fx":[-115.31422,-120.32947,-121.15594,-116.35859], "fy":[-108.16642,-102.46604,-102.69068,-108.17271]}, + {"t":1.95752, "x":6.30352, "y":5.25377, "heading":-2.53063, "vx":2.19085, "vy":-3.03689, "omega":-1.66636, "ax":-6.05328, "ay":-4.90329, "alpha":1.41821, "fx":[-98.10816,-104.93763,-107.60106,-101.21116], "fy":[-87.17021,-78.41872,-79.88771,-88.13735]}, + {"t":1.98434, "x":6.3601, "y":5.17057, "heading":-2.57532, "vx":2.02851, "vy":-3.16838, "omega":-1.62833, "ax":-4.73218, "ay":-3.38842, "alpha":1.30745, "fx":[-76.11387,-81.15802,-84.73137,-79.96868], "fy":[-60.3551,-53.07572,-55.10637,-62.00681]}, + {"t":2.01116, "x":6.4128, "y":5.08438, "heading":-2.61899, "vx":1.9016, "vy":-3.25925, "omega":-1.59326, "ax":-2.70479, "ay":-1.87572, "alpha":1.69559, "fx":[-40.24408,-44.58973,-51.71647,-47.48049], "fy":[-33.58136,-26.31993,-30.3354,-37.38545]}, + {"t":2.03798, "x":6.46282, "y":4.9963, "heading":-2.66171, "vx":1.82907, "vy":-3.30955, "omega":-1.54779, "ax":0.59205, "ay":1.50122, "alpha":-0.74784, "fx":[8.80923,8.95152,11.50078,11.0206], "fy":[26.08034,22.00824,24.94448,29.10828]}, + {"t":2.06479, "x":6.51209, "y":4.90808, "heading":-2.70322, "vx":1.84494, "vy":-3.26929, "omega":-1.56785, "ax":-0.94672, "ay":8.31378, "alpha":-1.231, "fx":[-22.42067,-18.87627,-9.53044,-13.58662], "fy":[141.33642,139.78856,141.46884,143.06632]}, + {"t":2.09161, "x":6.56123, "y":4.8234, "heading":-2.74527, "vx":1.81955, "vy":-3.04634, "omega":-1.60086, "ax":-4.90909, "ay":8.65161, "alpha":6.50402, "fx":[-55.57898,-59.17181,-103.50173,-115.75636], "fy":[161.98237,162.03175,138.10447,126.52701]}, + {"t":2.11843, "x":6.60826, "y":4.74481, "heading":-2.7882, "vx":1.6879, "vy":-2.81432, "omega":-1.42643, "ax":-5.10148, "ay":8.67865, "alpha":6.09509, "fx":[-62.65521,-62.11329,-104.28884,-118.04183], "fy":[161.60039,162.46663,139.28204,127.1363]}, + {"t":2.14525, "x":6.65169, "y":4.67246, "heading":-2.82645, "vx":1.55109, "vy":-2.58158, "omega":-1.26298, "ax":-5.17165, "ay":8.6983, "alpha":5.6893, "fx":[-66.98056,-63.61214,-103.49961,-117.78108], "fy":[160.63514,162.41726,140.46924,128.30082]}, + {"t":2.17206, "x":6.69143, "y":4.60636, "heading":-2.86032, "vx":1.4124, "vy":-2.34831, "omega":-1.1104, "ax":-5.20913, "ay":8.71356, "alpha":5.33656, "fx":[-70.16683,-64.72676,-102.50783,-117.02151], "fy":[159.66692,162.24683,141.49522,129.45151]}, + {"t":2.19888, "x":6.72743, "y":4.54651, "heading":-2.8901, "vx":1.2727, "vy":-2.11463, "omega":-0.96729, "ax":-5.23285, "ay":8.72608, "alpha":5.02518, "fx":[-72.68564,-65.70807,-101.53837,-116.10483], "fy":[158.77798,162.01635,142.37266,130.54533]}, + {"t":2.2257, "x":6.75968, "y":4.49294, "heading":-2.91604, "vx":1.13237, "vy":-1.88061, "omega":-0.83252, "ax":-5.24938, "ay":8.73681, "alpha":4.74287, "fx":[-74.74997,-66.65008,-100.64289,-115.11865], "fy":[157.9795,161.74195,143.12687,131.59396]}, + {"t":2.25252, "x":6.78816, "y":4.44565, "heading":-2.93837, "vx":0.99159, "vy":-1.64631, "omega":-0.70533, "ax":-5.26161, "ay":8.74632, "alpha":4.48028, "fx":[-76.47914,-67.59659,-99.83035,-114.08813], "fy":[157.26699,161.42865,143.78005,132.61386]}, + {"t":2.27934, "x":6.81286, "y":4.40464, "heading":-2.95728, "vx":0.85048, "vy":-1.41175, "omega":-0.58518, "ax":-5.27105, "ay":8.75498, "alpha":4.23044, "fx":[-77.94949,-68.5706,-99.09674,-113.01917], "fy":[156.63208,161.07788,144.35035,133.61884]}, + {"t":2.30615, "x":6.83377, "y":4.36993, "heading":-2.97298, "vx":0.70913, "vy":-1.17696, "omega":-0.47173, "ax":-5.27852, "ay":8.76304, "alpha":3.98831, "fx":[-79.21419,-69.58432,-98.43385,-111.91179], "fy":[156.06572,160.6901,144.85251,134.61888]}, + {"t":2.33297, "x":6.85089, "y":4.34152, "heading":-2.98563, "vx":0.56757, "vy":-0.94195, "omega":-0.36477, "ax":-5.28453, "ay":8.77063, "alpha":3.75045, "fx":[-80.31212,-70.64243,-97.83261,-110.76594], "fy":[155.55946,160.26611,145.29851,135.61947]}, + {"t":2.35979, "x":6.86421, "y":4.31941, "heading":-2.99541, "vx":0.42585, "vy":-0.70674, "omega":-0.26419, "ax":-5.2894, "ay":8.77782, "alpha":3.5151, "fx":[-81.27208,-71.74252,-97.28477,-109.58517], "fy":[155.10607,159.80835,145.69785,136.62076]}, + {"t":2.38661, "x":6.87373, "y":4.30361, "heading":-3.0025, "vx":0.284, "vy":-0.47134, "omega":-0.16992, "ax":-5.29334, "ay":8.78463, "alpha":3.28227, "fx":[-82.11484,-72.87419,-96.78396,-108.37954], "fy":[154.70002,159.3221,146.05752,137.61652]}, + {"t":2.41343, "x":6.87944, "y":4.29413, "heading":-3.00705, "vx":0.14204, "vy":-0.23576, "omega":-0.0819, "ax":-5.29649, "ay":8.79101, "alpha":3.05382, "fx":[-82.85456,-74.01885,-96.32644,-107.16705], "fy":[154.33771,158.81626,146.38202,138.59409]}, + {"t":2.44024, "x":6.88135, "y":4.29097, "heading":-3.00925, "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/GToBarge.traj b/src/main/deploy/choreo/GToBarge.traj new file mode 100644 index 00000000..219d771d --- /dev/null +++ b/src/main/deploy/choreo/GToBarge.traj @@ -0,0 +1,54 @@ +{ + "name":"GToBarge", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.7597, "y":4.3597, "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":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.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,0.9107], + "samples":[ + {"t":0.0, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.6612, "ay":3.59165, "alpha":0.0, "fx":[164.33422,164.33422,164.33422,164.33422], "fy":[61.09301,61.09301,61.09301,61.09301]}, + {"t":0.04793, "x":5.78801, "y":3.6267, "heading":3.14159, "vx":0.46308, "vy":0.17215, "omega":0.0, "ax":9.65998, "ay":3.5912, "alpha":0.0, "fx":[164.31345,164.31345,164.31345,164.31345], "fy":[61.08529,61.08529,61.08529,61.08529]}, + {"t":0.09586, "x":5.8213, "y":3.63908, "heading":3.14159, "vx":0.9261, "vy":0.34429, "omega":0.0, "ax":9.65835, "ay":3.5906, "alpha":0.0, "fx":[164.28577,164.28577,164.28577,164.28577], "fy":[61.075,61.075,61.075,61.075]}, + {"t":0.1438, "x":5.87679, "y":3.65971, "heading":3.14159, "vx":1.38904, "vy":0.51639, "omega":0.0, "ax":9.65607, "ay":3.58975, "alpha":0.0, "fx":[164.24704,164.24704,164.24704,164.24704], "fy":[61.0606,61.0606,61.0606,61.0606]}, + {"t":0.19173, "x":5.95446, "y":3.68858, "heading":3.14159, "vx":1.85187, "vy":0.68845, "omega":0.0, "ax":9.65266, "ay":3.58848, "alpha":0.0, "fx":[164.18901,164.18901,164.18901,164.18901], "fy":[61.03903,61.03903,61.03903,61.03903]}, + {"t":0.23966, "x":6.05431, "y":3.7257, "heading":3.14159, "vx":2.31454, "vy":0.86045, "omega":0.0, "ax":9.64699, "ay":3.58637, "alpha":0.0, "fx":[164.09249,164.09249,164.09249,164.09249], "fy":[61.00314,61.00314,61.00314,61.00314]}, + {"t":0.28759, "x":6.17633, "y":3.77107, "heading":3.14159, "vx":2.77693, "vy":1.03236, "omega":0.0, "ax":9.63568, "ay":3.58217, "alpha":0.0, "fx":[163.90022,163.90022,163.90022,163.90022], "fy":[60.93166,60.93166,60.93166,60.93166]}, + {"t":0.33552, "x":6.3205, "y":3.82466, "heading":3.14159, "vx":3.23879, "vy":1.20405, "omega":0.0, "ax":9.60219, "ay":3.56972, "alpha":0.0, "fx":[163.33057,163.33057,163.33057,163.33057], "fy":[60.71989,60.71989,60.71989,60.71989]}, + {"t":0.38345, "x":6.48677, "y":3.88648, "heading":3.14159, "vx":3.69904, "vy":1.37516, "omega":0.0, "ax":6.78138, "ay":2.52105, "alpha":0.0, "fx":[115.3494,115.3494,115.3494,115.3494], "fy":[42.88238,42.88238,42.88238,42.88238]}, + {"t":0.43139, "x":6.67187, "y":3.95529, "heading":3.14159, "vx":4.02408, "vy":1.496, "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.47932, "x":6.86475, "y":4.02699, "heading":3.14159, "vx":4.02408, "vy":1.496, "omega":0.0, "ax":-6.78138, "ay":-2.52105, "alpha":0.0, "fx":[-115.3494,-115.3494,-115.3494,-115.3494], "fy":[-42.88238,-42.88238,-42.88238,-42.88238]}, + {"t":0.52725, "x":7.04984, "y":4.0958, "heading":3.14159, "vx":3.69904, "vy":1.37516, "omega":0.0, "ax":-9.60219, "ay":-3.56972, "alpha":0.0, "fx":[-163.33057,-163.33057,-163.33057,-163.33057], "fy":[-60.71989,-60.71989,-60.71989,-60.71989]}, + {"t":0.57518, "x":7.21611, "y":4.15761, "heading":3.14159, "vx":3.23879, "vy":1.20405, "omega":0.0, "ax":-9.63568, "ay":-3.58217, "alpha":0.0, "fx":[-163.90022,-163.90022,-163.90022,-163.90022], "fy":[-60.93166,-60.93166,-60.93166,-60.93166]}, + {"t":0.62311, "x":7.36028, "y":4.21121, "heading":3.14159, "vx":2.77693, "vy":1.03236, "omega":0.0, "ax":-9.64699, "ay":-3.58637, "alpha":0.0, "fx":[-164.09249,-164.09249,-164.09249,-164.09249], "fy":[-61.00314,-61.00314,-61.00314,-61.00314]}, + {"t":0.67104, "x":7.4823, "y":4.25657, "heading":3.14159, "vx":2.31454, "vy":0.86045, "omega":0.0, "ax":-9.65266, "ay":-3.58848, "alpha":0.0, "fx":[-164.18901,-164.18901,-164.18901,-164.18901], "fy":[-61.03903,-61.03903,-61.03903,-61.03903]}, + {"t":0.71898, "x":7.58215, "y":4.2937, "heading":3.14159, "vx":1.85187, "vy":0.68845, "omega":0.0, "ax":-9.65607, "ay":-3.58975, "alpha":0.0, "fx":[-164.24704,-164.24704,-164.24704,-164.24704], "fy":[-61.0606,-61.0606,-61.0606,-61.0606]}, + {"t":0.76691, "x":7.65983, "y":4.32257, "heading":3.14159, "vx":1.38904, "vy":0.51639, "omega":0.0, "ax":-9.65835, "ay":-3.5906, "alpha":0.0, "fx":[-164.28577,-164.28577,-164.28577,-164.28577], "fy":[-61.075,-61.075,-61.075,-61.075]}, + {"t":0.81484, "x":7.71531, "y":4.3432, "heading":3.14159, "vx":0.9261, "vy":0.34429, "omega":0.0, "ax":-9.65998, "ay":-3.5912, "alpha":0.0, "fx":[-164.31345,-164.31345,-164.31345,-164.31345], "fy":[-61.08529,-61.08529,-61.08529,-61.08529]}, + {"t":0.86277, "x":7.7486, "y":4.35557, "heading":3.14159, "vx":0.46308, "vy":0.17215, "omega":0.0, "ax":-9.6612, "ay":-3.59165, "alpha":0.0, "fx":[-164.33422,-164.33422,-164.33422,-164.33422], "fy":[-61.09301,-61.09301,-61.09301,-61.09301]}, + {"t":0.9107, "x":7.7597, "y":4.3597, "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/HToBarge.traj b/src/main/deploy/choreo/HToBarge.traj new file mode 100644 index 00000000..103c4f33 --- /dev/null +++ b/src/main/deploy/choreo/HToBarge.traj @@ -0,0 +1,53 @@ +{ + "name":"HToBarge", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.7597, "y":4.3597, "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":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.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,0.88977], + "samples":[ + {"t":0.0, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":10.09554, "ay":2.07964, "alpha":0.0, "fx":[171.72232,171.72232,171.72232,171.72232], "fy":[35.37413,35.37413,35.37413,35.37413]}, + {"t":0.04943, "x":5.78925, "y":3.95379, "heading":3.14159, "vx":0.49904, "vy":0.1028, "omega":0.0, "ax":10.09431, "ay":2.07939, "alpha":0.0, "fx":[171.70128,171.70128,171.70128,171.70128], "fy":[35.3698,35.3698,35.3698,35.3698]}, + {"t":0.09886, "x":5.82625, "y":3.96142, "heading":3.14159, "vx":0.99802, "vy":0.20559, "omega":0.0, "ax":10.09266, "ay":2.07905, "alpha":0.0, "fx":[171.67321,171.67321,171.67321,171.67321], "fy":[35.36401,35.36401,35.36401,35.36401]}, + {"t":0.14829, "x":5.88791, "y":3.97412, "heading":3.14159, "vx":1.49691, "vy":0.30836, "omega":0.0, "ax":10.09035, "ay":2.07857, "alpha":0.0, "fx":[171.6339,171.6339,171.6339,171.6339], "fy":[35.35592,35.35592,35.35592,35.35592]}, + {"t":0.19773, "x":5.97423, "y":3.9919, "heading":3.14159, "vx":1.9957, "vy":0.41111, "omega":0.0, "ax":10.08688, "ay":2.07786, "alpha":0.0, "fx":[171.57491,171.57491,171.57491,171.57491], "fy":[35.34376,35.34376,35.34376,35.34376]}, + {"t":0.24716, "x":6.08521, "y":4.01476, "heading":3.14159, "vx":2.49431, "vy":0.51382, "omega":0.0, "ax":10.0811, "ay":2.07667, "alpha":0.0, "fx":[171.47656,171.47656,171.47656,171.47656], "fy":[35.3235,35.3235,35.3235,35.3235]}, + {"t":0.29659, "x":6.22082, "y":4.0427, "heading":3.14159, "vx":2.99263, "vy":0.61647, "omega":0.0, "ax":10.06953, "ay":2.07428, "alpha":0.0, "fx":[171.27978,171.27978,171.27978,171.27978], "fy":[35.28297,35.28297,35.28297,35.28297]}, + {"t":0.34602, "x":6.38106, "y":4.0757, "heading":3.14159, "vx":3.49038, "vy":0.71901, "omega":0.0, "ax":10.03487, "ay":2.06714, "alpha":0.0, "fx":[170.6903,170.6903,170.6903,170.6903], "fy":[35.16154,35.16154,35.16154,35.16154]}, + {"t":0.39545, "x":6.56585, "y":4.11377, "heading":3.14159, "vx":3.98642, "vy":0.82119, "omega":0.0, "ax":4.41992, "ay":0.91049, "alpha":0.0, "fx":[75.18164,75.18164,75.18164,75.18164], "fy":[15.48712,15.48712,15.48712,15.48712]}, + {"t":0.44488, "x":6.76831, "y":4.15548, "heading":3.14159, "vx":4.20491, "vy":0.86619, "omega":0.0, "ax":-4.41992, "ay":-0.91049, "alpha":0.0, "fx":[-75.18164,-75.18164,-75.18164,-75.18164], "fy":[-15.48712,-15.48712,-15.48712,-15.48712]}, + {"t":0.49432, "x":6.97076, "y":4.19718, "heading":3.14159, "vx":3.98642, "vy":0.82119, "omega":0.0, "ax":-10.03487, "ay":-2.06714, "alpha":0.0, "fx":[-170.6903,-170.6903,-170.6903,-170.6903], "fy":[-35.16154,-35.16154,-35.16154,-35.16154]}, + {"t":0.54375, "x":7.15556, "y":4.23525, "heading":3.14159, "vx":3.49038, "vy":0.71901, "omega":0.0, "ax":-10.06953, "ay":-2.07428, "alpha":0.0, "fx":[-171.27978,-171.27978,-171.27978,-171.27978], "fy":[-35.28297,-35.28297,-35.28297,-35.28297]}, + {"t":0.59318, "x":7.31579, "y":4.26826, "heading":3.14159, "vx":2.99263, "vy":0.61647, "omega":0.0, "ax":-10.0811, "ay":-2.07667, "alpha":0.0, "fx":[-171.47656,-171.47656,-171.47656,-171.47656], "fy":[-35.3235,-35.3235,-35.3235,-35.3235]}, + {"t":0.64261, "x":7.4514, "y":4.29619, "heading":3.14159, "vx":2.49431, "vy":0.51382, "omega":0.0, "ax":-10.08688, "ay":-2.07786, "alpha":0.0, "fx":[-171.57491,-171.57491,-171.57491,-171.57491], "fy":[-35.34376,-35.34376,-35.34376,-35.34376]}, + {"t":0.69204, "x":7.56238, "y":4.31905, "heading":3.14159, "vx":1.9957, "vy":0.41111, "omega":0.0, "ax":-10.09035, "ay":-2.07857, "alpha":0.0, "fx":[-171.6339,-171.6339,-171.6339,-171.6339], "fy":[-35.35592,-35.35592,-35.35592,-35.35592]}, + {"t":0.74147, "x":7.6487, "y":4.33683, "heading":3.14159, "vx":1.49691, "vy":0.30836, "omega":0.0, "ax":-10.09266, "ay":-2.07905, "alpha":0.0, "fx":[-171.67321,-171.67321,-171.67321,-171.67321], "fy":[-35.36401,-35.36401,-35.36401,-35.36401]}, + {"t":0.79091, "x":7.71036, "y":4.34954, "heading":3.14159, "vx":0.99802, "vy":0.20559, "omega":0.0, "ax":-10.09431, "ay":-2.07939, "alpha":0.0, "fx":[-171.70128,-171.70128,-171.70128,-171.70128], "fy":[-35.3698,-35.3698,-35.3698,-35.3698]}, + {"t":0.84034, "x":7.74737, "y":4.35716, "heading":3.14159, "vx":0.49904, "vy":0.1028, "omega":0.0, "ax":-10.09554, "ay":-2.07964, "alpha":0.0, "fx":[-171.72232,-171.72232,-171.72232,-171.72232], "fy":[-35.37413,-35.37413,-35.37413,-35.37413]}, + {"t":0.88977, "x":7.7597, "y":4.3597, "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/constants.traj b/src/main/deploy/choreo/constants.traj index 5abf3bbb..5e811376 100644 --- a/src/main/deploy/choreo/constants.traj +++ b/src/main/deploy/choreo/constants.traj @@ -8,8 +8,12 @@ }, "params":{ "waypoints":[ + {"x":{"exp":"305.5 in", "val":7.7597}, "y":{"exp":"4.359700000000012 m", "val":4.359700000000012}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"7.2570308 m", "val":7.2570308}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":6506, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, @@ -27,7 +31,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":true}, - {"from":1, "to":14, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], + {"from":2, "to":18, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src/main/deploy/choreo/startDeepToA.traj b/src/main/deploy/choreo/startDeepToA.traj new file mode 100644 index 00000000..1a6c10b4 --- /dev/null +++ b/src/main/deploy/choreo/startDeepToA.traj @@ -0,0 +1,145 @@ +{ + "name":"startDeepToA", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.5624847412109375, "y":6.526900768280029, "heading":-1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4915122985839844, "y":6.0326056480407715, "heading":-1.1801894971536726, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.7500696182250977, "y":5.027539253234863, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2019875, "y":4.4292225106, "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":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.5624847412109375 m", "val":4.5624847412109375}, "y":{"exp":"6.526900768280029 m", "val":6.526900768280029}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4915122985839844 m", "val":3.4915122985839844}, "y":{"exp":"6.0326056480407715 m", "val":6.0326056480407715}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.7500696182250977 m", "val":2.7500696182250977}, "y":{"exp":"5.027539253234863 m", "val":5.027539253234863}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "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,0.59235,0.88055,1.15702,1.58136,2.00844], + "samples":[ + {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.47751, "ay":-0.76283, "alpha":34.00404, "fx":[-42.33677,-77.67458,-168.48115,-152.22984], "fy":[-169.92267,156.61175,48.32264,-86.91351]}, + {"t":0.02692, "x":7.09854, "y":7.25675, "heading":3.14159, "vx":-0.17441, "vy":-0.02054, "omega":0.91555, "ax":-6.78067, "ay":-0.85566, "alpha":32.43865, "fx":[-49.20726,-90.30722,-169.06842,-152.76633], "fy":[-168.01509,149.58716,46.13005,-85.92042]}, + {"t":0.05385, "x":7.09139, "y":7.25589, "heading":-3.11694, "vx":-0.35697, "vy":-0.04358, "omega":1.78896, "ax":-7.11559, "ay":-1.05709, "alpha":30.5905, "fx":[-55.75541,-105.73531,-170.2472,-152.399], "fy":[-165.88735,139.03258,41.44156,-86.5099]}, + {"t":0.08077, "x":7.0792, "y":7.25433, "heading":-3.06877, "vx":-0.54856, "vy":-0.07204, "omega":2.6126, "ax":-7.48296, "ay":-1.35255, "alpha":28.37289, "fx":[-63.80432,-122.18296,-171.78823,-151.3562], "fy":[-162.85662,124.76428,34.30783,-88.24175]}, + {"t":0.1077, "x":7.06171, "y":7.2519, "heading":-2.99843, "vx":-0.75004, "vy":-0.10846, "omega":3.37654, "ax":-7.90817, "ay":-1.71255, "alpha":25.50162, "fx":[-76.25217,-138.42686,-173.37388,-150.00988], "fy":[-157.25428,106.39739,24.73818,-90.40123]}, + {"t":0.13462, "x":7.03865, "y":7.24836, "heading":-2.90752, "vx":-0.96296, "vy":-0.15457, "omega":4.06317, "ax":-8.44315, "ay":-2.07664, "alpha":21.40907, "fx":[-97.1058,-153.65537,-174.59651,-149.10456], "fy":[-145.06375,82.81114,12.68374,-91.72329]}, + {"t":0.16155, "x":7.00966, "y":7.24345, "heading":-2.79812, "vx":-1.1903, "vy":-0.21048, "omega":4.6396, "ax":-9.10619, "ay":-2.33119, "alpha":15.47279, "fx":[-127.74682,-166.66364,-174.9266,-150.2378], "fy":[-118.63458,51.65475,-2.04908,-89.58235]}, + {"t":0.18847, "x":6.97431, "y":7.23694, "heading":-2.6732, "vx":-1.43548, "vy":-0.27325, "omega":5.05621, "ax":-9.71879, "ay":-2.4173, "alpha":7.93845, "fx":[-156.8174,-174.16256,-173.53669,-156.73902], "fy":[-75.89542,8.76109,-20.25854,-77.07724]}, + {"t":0.2154, "x":6.93214, "y":7.2287, "heading":-2.53706, "vx":-1.69716, "vy":-0.33833, "omega":5.26995, "ax":-9.93668, "ay":-2.43951, "alpha":-1.6122, "fx":[-170.22357,-166.62367,-167.91304,-171.32011], "fy":[-37.40061,-50.98141,-46.14497,-31.45429]}, + {"t":0.24232, "x":6.88284, "y":7.21871, "heading":-2.39517, "vx":-1.9647, "vy":-0.40402, "omega":5.22654, "ax":-8.42043, "ay":-2.70964, "alpha":-18.58613, "fx":[-173.1259,-126.18291,-118.90006,-154.70781], "fy":[-21.14235,-120.04682,-122.30264,79.13093]}, + {"t":0.26925, "x":6.82689, "y":7.20685, "heading":-2.25444, "vx":-2.19142, "vy":-0.47697, "omega":4.72611, "ax":-8.04395, "ay":-2.70736, "alpha":-19.93338, "fx":[-172.18687,-111.54481,-115.69392,-147.87564], "fy":[-27.28163,-133.47219,-114.73472,91.28317]}, + {"t":0.29617, "x":6.76497, "y":7.19303, "heading":-2.12719, "vx":-2.408, "vy":-0.54987, "omega":4.18941, "ax":-8.49138, "ay":-1.04779, "alpha":-18.85782, "fx":[-170.40526,-102.79592,-153.9391,-150.60361], "fy":[-36.12199,-139.9295,18.21864,86.54222]}, + {"t":0.3231, "x":6.69706, "y":7.17784, "heading":-2.01439, "vx":-2.63663, "vy":-0.57808, "omega":3.68166, "ax":-8.67216, "ay":-0.95344, "alpha":-18.42764, "fx":[-168.13395,-105.27294,-157.93083,-158.7061], "fy":[-44.61885,-137.50593,47.19041,70.06324]}, + {"t":0.35002, "x":6.62293, "y":7.16193, "heading":-1.91526, "vx":-2.87013, "vy":-0.60375, "omega":3.1855, "ax":-8.68379, "ay":-1.15836, "alpha":-18.4197, "fx":[-165.515,-104.67924,-157.38465,-163.25614], "fy":[-52.48164,-137.22297,53.15445,57.7367]}, + {"t":0.37695, "x":6.5425, "y":7.14526, "heading":-1.8295, "vx":-3.10394, "vy":-0.63494, "omega":2.68955, "ax":-8.59439, "ay":-1.40038, "alpha":-18.77454, "fx":[-162.38457,-100.81668,-155.70262,-165.84835], "fy":[-60.20089,-139.08197,55.9211,48.08171]}, + {"t":0.40387, "x":6.45581, "y":7.12765, "heading":-1.75708, "vx":-3.33534, "vy":-0.67264, "omega":2.18405, "ax":-8.41438, "ay":-1.67332, "alpha":-19.36878, "fx":[-158.52668,-93.78843,-152.99491,-167.19507], "fy":[-67.96866,-142.44614,56.703,39.86129]}, + {"t":0.4308, "x":6.36296, "y":7.10893, "heading":-1.69827, "vx":-3.56189, "vy":-0.7177, "omega":1.66255, "ax":-8.13209, "ay":-1.99967, "alpha":-19.99036, "fx":[-153.55746,-83.68541,-148.67096,-167.38404], "fy":[-75.7925,-146.20564,53.92984,32.01287]}, + {"t":0.45772, "x":6.26411, "y":7.08889, "heading":-1.65351, "vx":-3.78085, "vy":-0.77154, "omega":1.12431, "ax":-7.66335, "ay":-2.46093, "alpha":-20.21129, "fx":[-146.24476,-70.11094,-139.56929,-165.4806], "fy":[-83.67606,-148.35565,41.76302,22.82983]}, + {"t":0.48465, "x":6.15953, "y":7.06722, "heading":-1.62324, "vx":-3.98718, "vy":-0.8378, "omega":0.58013, "ax":-6.33151, "ay":-3.54729, "alpha":-18.25084, "fx":[-128.01922,-48.92433,-100.65809,-153.1872], "fy":[-92.31698,-140.97746,-10.48368,2.42437]}, + {"t":0.51157, "x":6.04988, "y":7.04338, "heading":-1.60762, "vx":-4.15766, "vy":-0.93331, "omega":0.08873, "ax":0.12945, "ay":-3.72683, "alpha":-1.97357, "fx":[-3.36249,7.28017,7.97963,-3.08978], "fy":[-67.4646,-67.67571,-59.33004,-59.09905]}, + {"t":0.5385, "x":5.93798, "y":7.0169, "heading":-1.60523, "vx":-4.15417, "vy":-1.03365, "omega":0.03559, "ax":0.89634, "ay":-3.52382, "alpha":-0.07443, "fx":[15.02839,15.42921,15.46482,15.06347], "fy":[-60.10617,-60.09576,-59.77229,-59.78265]}, + {"t":0.56542, "x":5.82646, "y":6.98779, "heading":-1.60427, "vx":-4.13004, "vy":-1.12853, "omega":0.03358, "ax":1.60578, "ay":-5.82908, "alpha":-0.45863, "fx":[25.63915,28.49907,29.00658,26.11041], "fy":[-100.11274,-99.77631,-98.18577,-98.52934]}, + {"t":0.59235, "x":5.71584, "y":6.95529, "heading":-1.60337, "vx":-4.08681, "vy":-1.28548, "omega":0.02124, "ax":2.48153, "ay":-4.3992, "alpha":2.45781, "fx":[50.46575,36.8313,34.21863,47.32462], "fy":[-68.49787,-70.60143,-81.16443,-79.05303]}, + {"t":0.60607, "x":5.65999, "y":6.93723, "heading":-1.60308, "vx":-4.05275, "vy":-1.34585, "omega":0.05497, "ax":1.31689, "ay":-3.53702, "alpha":0.26563, "fx":[23.19121,21.76768,21.61171,23.02888], "fy":[-59.54328,-59.61848,-60.78473,-60.70861]}, + {"t":0.61979, "x":5.60449, "y":6.91843, "heading":-1.60232, "vx":-4.03468, "vy":-1.3944, "omega":0.05861, "ax":0.88418, "ay":-2.52208, "alpha":-0.01334, "fx":[15.00312,15.07135,15.07623,15.00799], "fy":[-42.93108,-42.9303,-42.8687,-42.86948]}, + {"t":0.63352, "x":5.5492, "y":6.89906, "heading":-1.60152, "vx":-4.02254, "vy":-1.42901, "omega":0.05843, "ax":0.60548, "ay":-1.71255, "alpha":-0.02945, "fx":[10.22199,10.36871,10.37603,10.22928], "fy":[-29.19921,-29.20071,-29.06072,-29.05921]}, + {"t":0.64724, "x":5.49405, "y":6.87928, "heading":-1.60072, "vx":-4.01423, "vy":-1.45251, "omega":0.05802, "ax":0.40774, "ay":-1.13694, "alpha":-0.02013, "fx":[6.88421,6.98319,6.98702,6.88802], "fy":[-19.38651,-19.38855,-19.29158,-19.28953]}, + {"t":0.66097, "x":5.439, "y":6.85924, "heading":-1.59992, "vx":-4.00864, "vy":-1.46811, "omega":0.05775, "ax":0.27144, "ay":-0.74865, "alpha":-0.01227, "fx":[4.58615,4.64614,4.64812,4.58813], "fy":[-12.76329,-12.76479,-12.70533,-12.70383]}, + {"t":0.67469, "x":5.38401, "y":6.83902, "heading":-1.59913, "vx":-4.00491, "vy":-1.47839, "omega":0.05758, "ax":0.18035, "ay":-0.49361, "alpha":-0.0072, "fx":[3.04968,3.08479,3.08584,3.05073], "fy":[-8.41322,-8.41415,-8.37917,-8.37824]}, + {"t":0.68841, "x":5.32907, "y":6.81869, "heading":-1.59834, "vx":-4.00244, "vy":-1.48516, "omega":0.05748, "ax":0.121, "ay":-0.32931, "alpha":-0.00404, "fx":[2.04797,2.06767,2.06822,2.04852], "fy":[-5.61108,-5.61161,-5.59194,-5.59141]}, + {"t":0.70214, "x":5.27415, "y":6.79827, "heading":-1.59755, "vx":-4.00078, "vy":-1.48968, "omega":0.05743, "ax":0.08354, "ay":-0.22637, "alpha":-0.00211, "fx":[1.41562,1.42592,1.4262,1.4159], "fy":[-3.85552,-3.85579,-3.8455,-3.84523]}, + {"t":0.71586, "x":5.21925, "y":6.77781, "heading":-1.59676, "vx":-3.99963, "vy":-1.49279, "omega":0.0574, "ax":0.06142, "ay":-0.16585, "alpha":-0.001, "fx":[1.04231,1.04717,1.04729,1.04244], "fy":[-2.82348,-2.82361,-2.81876,-2.81863]}, + {"t":0.72959, "x":5.16437, "y":6.75731, "heading":-1.59597, "vx":-3.99879, "vy":-1.49507, "omega":0.05738, "ax":0.0507, "ay":-0.1365, "alpha":-0.00046, "fx":[0.86118,0.8634,0.86345,0.86124], "fy":[-2.32292,-2.32298,-2.32076,-2.32071]}, + {"t":0.74331, "x":5.10949, "y":6.73677, "heading":-1.59518, "vx":-3.99809, "vy":-1.49694, "omega":0.05738, "ax":0.04941, "ay":-0.13283, "alpha":-0.00038, "fx":[0.83956,0.84141,0.84145,0.83961], "fy":[-2.26022,-2.26027,-2.25842,-2.25838]}, + {"t":0.75703, "x":5.05463, "y":6.71622, "heading":-1.5944, "vx":-3.99741, "vy":-1.49876, "omega":0.05737, "ax":0.05738, "ay":-0.15413, "alpha":-0.00075, "fx":[0.97413,0.97777,0.97786,0.97422], "fy":[-2.62343,-2.62352,-2.61988,-2.61979]}, + {"t":0.77076, "x":4.99977, "y":6.69564, "heading":-1.59361, "vx":-3.99662, "vy":-1.50088, "omega":0.05736, "ax":0.07613, "ay":-0.20436, "alpha":-0.00162, "fx":[1.29085,1.29875,1.29893,1.29103], "fy":[-3.4799,-3.48007,-3.47218,-3.472]}, + {"t":0.78448, "x":4.94493, "y":6.67502, "heading":-1.59282, "vx":-3.99558, "vy":-1.50368, "omega":0.05734, "ax":0.10924, "ay":-0.2928, "alpha":-0.00314, "fx":[1.8503,1.86561,1.86596,1.85064], "fy":[-4.98791,-4.98824,-4.97295,-4.97262]}, + {"t":0.79821, "x":4.89011, "y":6.65435, "heading":-1.59203, "vx":-3.99408, "vy":-1.5077, "omega":0.0573, "ax":0.16304, "ay":-0.43563, "alpha":-0.00554, "fx":[2.75938,2.78641,2.78702,2.75999], "fy":[-7.42307,-7.42361,-7.39666,-7.39612]}, + {"t":0.81193, "x":4.83531, "y":6.63362, "heading":-1.59125, "vx":-3.99184, "vy":-1.51368, "omega":0.05722, "ax":0.24771, "ay":-0.65827, "alpha":-0.00914, "fx":[4.19069,4.23536,4.23641,4.19174], "fy":[-11.21871,-11.21948,-11.17513,-11.17435]}, + {"t":0.82565, "x":4.78055, "y":6.61279, "heading":-1.59046, "vx":-3.98844, "vy":-1.52271, "omega":0.05709, "ax":0.37887, "ay":-0.99799, "alpha":-0.01433, "fx":[6.40832,6.47864,6.48052,6.4102], "fy":[-17.00972,-17.01059,-16.94137,-16.94051]}, + {"t":0.83938, "x":4.72584, "y":6.59179, "heading":-1.58968, "vx":-3.98324, "vy":-1.53641, "omega":0.0569, "ax":0.57851, "ay":-1.50386, "alpha":-0.02172, "fx":[9.78462,9.89228,9.89605,9.78838], "fy":[-25.63214,-25.63237,-25.52847,-25.52824]}, + {"t":0.8531, "x":4.67123, "y":6.57057, "heading":-1.5889, "vx":-3.9753, "vy":-1.55705, "omega":0.0566, "ax":0.87172, "ay":-2.22592, "alpha":-0.03501, "fx":[14.73449,14.91155,14.92101,14.74388], "fy":[-37.94576,-37.9425,-37.77862,-37.78187]}, + {"t":0.86683, "x":4.61676, "y":6.54899, "heading":-1.58812, "vx":-3.96334, "vy":-1.5876, "omega":0.05612, "ax":1.26693, "ay":-3.18157, "alpha":-0.11195, "fx":[21.23064,21.81821,21.87021,21.28163], "fy":[-54.38502,-54.35232,-53.85017,-53.88268]}, + {"t":0.88055, "x":4.56248, "y":6.5269, "heading":-1.58735, "vx":-3.94595, "vy":-1.63126, "omega":0.05458, "ax":1.366, "ay":-3.05788, "alpha":0.16937, "fx":[23.7163,22.83349,22.75522,23.63585], "fy":[-51.60387,-51.65566,-52.42377,-52.37146]}, + {"t":0.89371, "x":4.51065, "y":6.50516, "heading":-1.58663, "vx":-3.92797, "vy":-1.67152, "omega":0.05681, "ax":0.94213, "ay":-2.19187, "alpha":-0.01102, "fx":[15.99609,16.05171,16.05468,15.99905], "fy":[-37.30958,-37.30832,-37.25658,-37.25784]}, + {"t":0.90688, "x":4.45902, "y":6.48296, "heading":-1.58589, "vx":-3.91557, "vy":-1.70038, "omega":0.05667, "ax":0.6556, "ay":-1.51402, "alpha":-0.02158, "fx":[11.09629,11.20317,11.20675,11.09985], "fy":[-25.80493,-25.80453,-25.70126,-25.70166]}, + {"t":0.92005, "x":4.40753, "y":6.46045, "heading":-1.58514, "vx":-3.90693, "vy":-1.72031, "omega":0.05638, "ax":0.4513, "ay":-1.0309, "alpha":-0.01509, "fx":[7.63866,7.71271,7.71442,7.64036], "fy":[-17.57151,-17.57191,-17.49903,-17.49863]}, + {"t":0.93321, "x":4.35613, "y":6.43771, "heading":-1.5844, "vx":-3.90099, "vy":-1.73388, "omega":0.05619, "ax":0.30967, "ay":-0.7014, "alpha":-0.00953, "fx":[5.24377,5.29033,5.29116,5.24459], "fy":[-11.9535,-11.95394,-11.90772,-11.90728]}, + {"t":0.94638, "x":4.3048, "y":6.41482, "heading":-1.58366, "vx":-3.89692, "vy":-1.74311, "omega":0.05606, "ax":0.21461, "ay":-0.48312, "alpha":-0.00589, "fx":[3.63588,3.66464,3.66506,3.63631], "fy":[-8.23195,-8.23226,-8.20361,-8.2033]}, + {"t":0.95954, "x":4.25351, "y":6.39183, "heading":-1.58292, "vx":-3.89409, "vy":-1.74948, "omega":0.05598, "ax":0.15329, "ay":-0.34353, "alpha":-0.00364, "fx":[2.5984,2.61616,2.61639,2.59863], "fy":[-5.85211,-5.85231,-5.83458,-5.83438]}, + {"t":0.97271, "x":4.20226, "y":6.36877, "heading":-1.58218, "vx":-3.89207, "vy":-1.754, "omega":0.05593, "ax":0.11665, "ay":-0.26053, "alpha":-0.00233, "fx":[1.97847,1.98985,1.98998,1.97861], "fy":[-4.43712,-4.43724,-4.42588,-4.42575]}, + {"t":0.98587, "x":4.15103, "y":6.34565, "heading":-1.58145, "vx":-3.89054, "vy":-1.75743, "omega":0.0559, "ax":0.09906, "ay":-0.22064, "alpha":-0.0017, "fx":[1.68081,1.68912,1.68921,1.6809], "fy":[-3.75716,-3.75724,-3.74894,-3.74886]}, + {"t":0.99904, "x":4.09982, "y":6.3225, "heading":-1.58071, "vx":-3.88923, "vy":-1.76033, "omega":0.05588, "ax":0.09778, "ay":-0.21732, "alpha":-0.00163, "fx":[1.6592,1.66713,1.66722,1.65928], "fy":[-3.70047,-3.70055,-3.69262,-3.69254]}, + {"t":1.0122, "x":4.04862, "y":6.2993, "heading":-1.57997, "vx":-3.88795, "vy":-1.76319, "omega":0.05586, "ax":0.11269, "ay":-0.24998, "alpha":-0.00208, "fx":[1.9117,1.92183,1.92193,1.9118], "fy":[-4.25707,-4.25716,-4.24704,-4.24695]}, + {"t":1.02537, "x":3.99744, "y":6.27607, "heading":-1.57924, "vx":-3.88646, "vy":-1.76648, "omega":0.05583, "ax":0.14634, "ay":-0.32389, "alpha":-0.0031, "fx":[2.48153,2.49666,2.49681,2.48167], "fy":[-5.51675,-5.51686,-5.50175,-5.50164]}, + {"t":1.03853, "x":3.94629, "y":6.25278, "heading":-1.5785, "vx":-3.88454, "vy":-1.77075, "omega":0.05579, "ax":0.20438, "ay":-0.45088, "alpha":-0.00483, "fx":[3.46456,3.48813,3.48835,3.46479], "fy":[-7.68108,-7.68122,-7.65773,-7.65759]}, + {"t":1.0517, "x":3.89517, "y":6.22943, "heading":-1.57777, "vx":-3.88184, "vy":-1.77669, "omega":0.05573, "ax":0.29645, "ay":-0.65077, "alpha":-0.00743, "fx":[5.02411,5.06042,5.0608,5.02449], "fy":[-11.08737,-11.08749,-11.05141,-11.05129]}, + {"t":1.06487, "x":3.84408, "y":6.20598, "heading":-1.57704, "vx":-3.87794, "vy":-1.78525, "omega":0.05563, "ax":0.43732, "ay":-0.95288, "alpha":-0.01107, "fx":[7.41124,7.4655,7.46626,7.412], "fy":[-16.23511,-16.23502,-16.18148,-16.18157]}, + {"t":1.07803, "x":3.79307, "y":6.1824, "heading":-1.5763, "vx":-3.87218, "vy":-1.7978, "omega":0.05548, "ax":0.64785, "ay":-1.39581, "alpha":-0.01553, "fx":[10.98062,11.05732,11.05904,10.98232], "fy":[-23.78005,-23.77917,-23.70462,-23.70549]}, + {"t":1.0912, "x":3.74214, "y":6.15861, "heading":-1.57557, "vx":-3.86365, "vy":-1.81617, "omega":0.05528, "ax":0.95378, "ay":-2.01941, "alpha":-0.01801, "fx":[16.17657,16.26685,16.27046,16.18016], "fy":[-34.3935,-34.39073,-34.30563,-34.3084]}, + {"t":1.10436, "x":3.69136, "y":6.13452, "heading":-1.57484, "vx":-3.8511, "vy":-1.84276, "omega":0.05504, "ax":1.38244, "ay":-2.83748, "alpha":0.00377, "fx":[23.52531,23.50587,23.50443,23.52387], "fy":[-48.25549,-48.25677,-48.2741,-48.27282]}, + {"t":1.11753, "x":3.64078, "y":6.11001, "heading":-1.57412, "vx":-3.8329, "vy":-1.88012, "omega":0.05509, "ax":1.98807, "ay":-3.77466, "alpha":0.21233, "fx":[34.45909,33.32341,33.1758,34.30751], "fy":[-63.66742,-63.80993,-64.74473,-64.60178]}, + {"t":1.13069, "x":3.59049, "y":6.08493, "heading":-1.57339, "vx":-3.80672, "vy":-1.92981, "omega":0.05789, "ax":3.07363, "ay":-4.50118, "alpha":1.57641, "fx":[57.56215,49.0147,47.09499,55.45446], "fy":[-72.12077,-74.10445,-80.99982,-79.03009]}, + {"t":1.14386, "x":3.54064, "y":6.05914, "heading":-1.57263, "vx":-3.76626, "vy":-1.98907, "omega":0.07864, "ax":5.2881, "ay":-3.97262, "alpha":6.85095, "fx":[110.8079,82.28532,68.1651,98.5382], "fy":[-43.26507,-54.59454,-93.23684,-79.19627]}, + {"t":1.15702, "x":3.49151, "y":6.03261, "heading":-1.5716, "vx":-3.69664, "vy":-2.04137, "omega":0.16884, "ax":7.42079, "ay":-3.88507, "alpha":10.5922, "fx":[152.22553,130.48717,90.58159,131.6078], "fy":[-22.18218,-38.19342,-118.47939,-85.48086]}, + {"t":1.17723, "x":3.41833, "y":5.99056, "heading":-1.56819, "vx":-3.54669, "vy":-2.11988, "omega":0.38287, "ax":8.56626, "ay":-1.82992, "alpha":14.75973, "fx":[164.95963,150.95055,114.35206,152.57607], "fy":[18.36641,37.06898,-112.72962,-67.21133]}, + {"t":1.19744, "x":3.34841, "y":5.94735, "heading":-1.56045, "vx":-3.3736, "vy":-2.15685, "omega":0.68111, "ax":8.8683, "ay":-1.24159, "alpha":15.58618, "fx":[167.20003,152.87631,124.66637,158.64617], "fy":[28.55327,56.97911,-109.04225,-60.96674]}, + {"t":1.21764, "x":3.28206, "y":5.90352, "heading":-1.54669, "vx":-3.1944, "vy":-2.18194, "omega":0.99605, "ax":9.0228, "ay":-1.05974, "alpha":15.63609, "fx":[168.27625,154.81601,129.49288,161.31589], "fy":[32.03413,61.78196,-107.62183,-58.29772]}, + {"t":1.23785, "x":3.21935, "y":5.85921, "heading":-1.52656, "vx":-3.01208, "vy":-2.20336, "omega":1.312, "ax":9.13165, "ay":-1.03293, "alpha":15.37171, "fx":[169.02688,157.41578,132.05213,162.81248], "fy":[33.16749,60.58116,-107.20015,-56.8278]}, + {"t":1.25806, "x":3.16035, "y":5.81448, "heading":-1.50005, "vx":-2.82756, "vy":-2.22423, "omega":1.62261, "ax":9.2226, "ay":-1.08131, "alpha":14.91253, "fx":[169.62665,160.43966,133.60274,163.82614], "fy":[33.27533,56.00284,-107.12064,-55.72842]}, + {"t":1.27826, "x":3.1051, "y":5.76932, "heading":-1.46726, "vx":-2.64121, "vy":-2.24608, "omega":1.92394, "ax":9.30667, "ay":-1.17248, "alpha":14.29225, "fx":[170.1394,163.6457,134.78878,164.64104], "fy":[32.84632,48.96792,-106.96248,-54.62627]}, + {"t":1.29847, "x":3.05363, "y":5.72369, "heading":-1.42839, "vx":-2.45315, "vy":-2.26977, "omega":2.21273, "ax":9.38951, "ay":-1.28969, "alpha":13.51755, "fx":[170.60467,166.78795,136.05616,165.40292], "fy":[32.04594,39.85178,-106.35087,-53.29565]}, + {"t":1.31868, "x":3.00598, "y":5.67756, "heading":-1.38367, "vx":-2.26342, "vy":-2.29583, "omega":2.48588, "ax":9.47455, "ay":-1.41977, "alpha":12.58985, "fx":[171.05912,169.57616,137.79977,166.20251], "fy":[30.86343,28.95085,-104.86372,-51.55039]}, + {"t":1.33888, "x":2.96218, "y":5.63088, "heading":-1.33344, "vx":-2.07198, "vy":-2.32452, "omega":2.74027, "ax":9.56378, "ay":-1.54676, "alpha":11.51945, "fx":[171.54175,171.68808,140.37925,167.09951], "fy":[29.15977,16.82824,-102.01218,-49.21576]}, + {"t":1.35909, "x":2.92226, "y":5.5836, "heading":-1.27807, "vx":-1.87873, "vy":-2.35577, "omega":2.97304, "ax":9.65776, "ay":-1.64962, "alpha":10.33416, "fx":[172.08601,172.89308,144.00845,168.11567], "fy":[26.71423,4.55505,-97.3406,-46.16666]}, + {"t":1.37929, "x":2.88627, "y":5.53566, "heading":-1.218, "vx":-1.68358, "vy":-2.3891, "omega":3.18186, "ax":9.7554, "ay":-1.70748, "alpha":9.07284, "fx":[172.69874,173.23814,148.59092,169.21816], "fy":[23.31688,-6.44854,-90.63449,-42.40862]}, + {"t":1.3995, "x":2.85424, "y":5.48704, "heading":-1.1537, "vx":-1.48645, "vy":-2.42361, "omega":3.36519, "ax":9.85405, "ay":-1.70902, "alpha":7.76329, "fx":[173.34249,173.0646,153.72242,170.32878], "fy":[18.86477,-15.0158,-82.02237,-38.10654]}, + {"t":1.41971, "x":2.82622, "y":5.43771, "heading":-1.08571, "vx":-1.28734, "vy":-2.45814, "omega":3.52206, "ax":9.95159, "ay":-1.6519, "alpha":6.39113, "fx":[173.94467,172.79255,158.98449,171.37326], "fy":[13.3223,-20.60596,-71.65336,-33.4567]}, + {"t":1.43991, "x":2.80224, "y":5.38771, "heading":-1.01454, "vx":-1.08625, "vy":-2.49152, "omega":3.6512, "ax":10.05677, "ay":-1.50655, "alpha":4.74091, "fx":[174.42841,172.80429,164.63599,172.38229], "fy":[6.0663,-22.61482,-57.85576,-28.09997]}, + {"t":1.46012, "x":2.78234, "y":5.33705, "heading":-0.94076, "vx":-0.88304, "vy":-2.52196, "omega":3.74699, "ax":10.18178, "ay":-1.15524, "alpha":2.11636, "fx":[174.53005,173.48615,171.24824,173.49189], "fy":[-5.27037,-19.00794,-34.065,-20.25823]}, + {"t":1.48033, "x":2.76658, "y":5.28586, "heading":-0.86505, "vx":-0.6773, "vy":-2.5453, "omega":3.78976, "ax":10.24287, "ay":-0.68988, "alpha":-0.98777, "fx":[173.70782,174.31799,174.61906,174.26825], "fy":[-18.49084,-11.82182,-4.9763,-11.64979]}, + {"t":1.50053, "x":2.75498, "y":5.23429, "heading":-0.78847, "vx":-0.47033, "vy":-2.55924, "omega":3.7698, "ax":10.21525, "ay":-0.26071, "alpha":-3.73095, "fx":[172.19421,174.80859,173.46981,174.56133], "fy":[-29.9009,-3.95325,21.24914,-5.13352]}, + {"t":1.52074, "x":2.74756, "y":5.18252, "heading":-0.71229, "vx":-0.26392, "vy":-2.56451, "omega":3.69441, "ax":10.13303, "ay":0.10333, "alpha":-5.98397, "fx":[170.54169,174.90442,169.38647,174.60695], "fy":[-38.57179,3.95858,43.29107,-1.64727]}, + {"t":1.54095, "x":2.7443, "y":5.13072, "heading":-0.63764, "vx":-0.05916, "vy":-2.56243, "omega":3.57349, "ax":10.02419, "ay":0.39996, "alpha":-7.82656, "fx":[169.14259,174.62664,163.67824,174.58689], "fy":[-44.58444,11.72933,61.59529,-1.52716]}, + {"t":1.56115, "x":2.74515, "y":5.07902, "heading":-0.56544, "vx":0.14339, "vy":-2.55434, "omega":3.41535, "ax":9.90365, "ay":0.63108, "alpha":-9.39024, "fx":[168.16804,174.01202,157.14315,174.50973], "fy":[-48.35404,19.25717,76.8497,-4.81467]}, + {"t":1.58136, "x":2.75007, "y":5.02754, "heading":-0.49642, "vx":0.34351, "vy":-2.54159, "omega":3.2256, "ax":9.89468, "ay":1.02134, "alpha":-9.17397, "fx":[170.60893,172.99601,154.8717,174.74598], "fy":[-39.25597,27.31495,81.5163,-0.0845]}, + {"t":1.60648, "x":2.76182, "y":4.96401, "heading":-0.41539, "vx":0.59208, "vy":-2.51593, "omega":2.99513, "ax":9.74156, "ay":1.66748, "alpha":-9.9329, "fx":[172.18892,170.40886,145.79744,174.40944], "fy":[-31.28655,40.22682,96.68022,7.83289]}, + {"t":1.6316, "x":2.77977, "y":4.90133, "heading":-0.34015, "vx":0.83681, "vy":-2.47404, "omega":2.7456, "ax":9.46681, "ay":2.54138, "alpha":-10.7859, "fx":[173.69141,165.89343,131.68555,172.84067], "fy":[-20.76005,55.82326,115.02939,22.82002]}, + {"t":1.65673, "x":2.80378, "y":4.83998, "heading":-0.27117, "vx":1.07464, "vy":-2.4102, "omega":2.47463, "ax":8.94573, "ay":3.76799, "alpha":-11.63142, "fx":[174.70579,158.08279,109.72045,166.14819], "fy":[-6.10589,74.97325,136.01464,51.48802]}, + {"t":1.68185, "x":2.8336, "y":4.78062, "heading":-0.209, "vx":1.29937, "vy":-2.31554, "omega":2.18243, "ax":7.83727, "ay":5.52882, "alpha":-12.55915, "fx":[173.93061,144.53641,77.09326,137.67832], "fy":[15.71997,98.4326,156.72862,105.29352]}, + {"t":1.70697, "x":2.86872, "y":4.72419, "heading":-0.15417, "vx":1.49626, "vy":-2.17664, "omega":1.86692, "ax":5.44309, "ay":7.55188, "alpha":-15.32148, "fx":[166.85603,121.36227,34.22056,47.90257], "fy":[50.64048,125.7609,171.22741,166.19251]}, + {"t":1.73209, "x":2.90802, "y":4.6719, "heading":-0.10727, "vx":1.633, "vy":-1.98692, "omega":1.48201, "ax":2.31577, "ay":8.80534, "alpha":-17.80236, "fx":[137.56429,84.38661,-11.92635,-52.4624], "fy":[106.53796,152.94539,174.21932,165.40243]}, + {"t":1.75721, "x":2.94978, "y":4.62476, "heading":-0.07004, "vx":1.69118, "vy":-1.76571, "omega":1.03478, "ax":-0.89845, "ay":9.4738, "alpha":-14.07151, "fx":[56.76384,34.42211,-52.0675,-100.24799], "fy":[164.25395,171.2159,166.75281,142.36376]}, + {"t":1.78234, "x":2.99198, "y":4.58339, "heading":-0.04405, "vx":1.66861, "vy":-1.52771, "omega":0.68127, "ax":-3.80517, "ay":9.16469, "alpha":-9.2268, "fx":[-37.44799,-18.16711,-82.08911,-121.19505], "fy":[170.00407,173.72858,154.30221,125.52039]}, + {"t":1.80746, "x":3.0327, "y":4.5479, "heading":-0.02693, "vx":1.57302, "vy":-1.29748, "omega":0.44947, "ax":-5.68615, "ay":8.35769, "alpha":-6.35859, "fx":[-90.25355,-61.64882,-103.12472,-131.85215], "fy":[149.29117,163.52506,141.21868,114.61273]}, + {"t":1.83258, "x":3.07042, "y":4.51794, "heading":-0.01564, "vx":1.43017, "vy":-1.08751, "omega":0.28973, "ax":-6.82159, "ay":7.58173, "alpha":-4.47216, "fx":[-115.81186,-92.5236,-117.74141,-138.05642], "fy":[130.8023,148.37087,129.38412,107.29472]}, + {"t":1.8577, "x":3.1042, "y":4.49302, "heading":-0.00836, "vx":1.25879, "vy":-0.89704, "omega":0.17738, "ax":-7.5363, "ay":6.94226, "alpha":-3.09884, "fx":[-129.42569,-113.22156,-128.07038,-142.04338], "fy":[117.58984,133.36287,119.25993,102.13065]}, + {"t":1.88282, "x":3.13344, "y":4.47267, "heading":-0.00391, "vx":1.06947, "vy":-0.72264, "omega":0.09953, "ax":-8.00978, "ay":6.43126, "alpha":-2.06897, "fx":[-137.52332,-127.09427,-135.56202,-144.79637], "fy":[108.18475,120.31996,110.75117,98.31981]}, + {"t":1.90795, "x":3.15778, "y":4.45655, "heading":-0.0014, "vx":0.86824, "vy":-0.56107, "omega":0.04756, "ax":-8.33854, "ay":6.02266, "alpha":-1.28091, "fx":[-142.77542,-136.61923,-141.14796,-146.80211], "fy":[101.28612,109.47733,103.60989,95.40185]}, + {"t":1.93307, "x":3.17696, "y":4.44435, "heading":-0.00021, "vx":0.65876, "vy":-0.40977, "omega":0.01538, "ax":-8.57634, "ay":5.69241, "alpha":-0.66499, "fx":[-146.41058,-143.366,-145.42256,-148.3249], "fy":[96.06099,100.56161,97.5831,93.09922]}, + {"t":1.95819, "x":3.19081, "y":4.43585, "heading":0.00018, "vx":0.44331, "vy":-0.26676, "omega":-0.00133, "ax":-8.7544, "ay":5.42179, "alpha":-0.17347, "fx":[-149.05404,-148.29551,-148.77045,-149.51903], "fy":[91.98949,93.21048,92.4555,91.23683]}, + {"t":1.98331, "x":3.19918, "y":4.43086, "heading":0.00014, "vx":0.22338, "vy":-0.13056, "omega":-0.00569, "ax":-8.89167, "ay":5.19694, "alpha":0.22641, "fx":[-151.05204,-152.00101,-151.44611,-150.47994], "fy":[88.73907,87.09993,88.0549,89.69969]}, + {"t":2.00844, "x":3.20199, "y":4.42922, "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/startDeepToB.traj b/src/main/deploy/choreo/startDeepToB.traj new file mode 100644 index 00000000..9ae000d8 --- /dev/null +++ b/src/main/deploy/choreo/startDeepToB.traj @@ -0,0 +1,145 @@ +{ + "name":"startDeepToB", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.595438003540039, "y":6.493947982788086, "heading":-1.5707963267948966, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4256062507629395, "y":5.90079402923584, "heading":-1.1801894971536726, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.7665462493896484, "y":4.763915061950684, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2019875, "y":4.1005465106, "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":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.595438003540039 m", "val":4.595438003540039}, "y":{"exp":"6.493947982788086 m", "val":6.493947982788086}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4256062507629395 m", "val":3.4256062507629395}, "y":{"exp":"5.90079402923584 m", "val":5.90079402923584}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.7665462493896484 m", "val":2.7665462493896484}, "y":{"exp":"4.763915061950684 m", "val":4.763915061950684}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.60983,0.89425,1.20214,1.60477,2.03261], + "samples":[ + {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.39219, "ay":-0.57766, "alpha":38.87343, "fx":[-17.84615,-32.81466,-166.58675,-149.63111], "fy":[-174.22044,171.7344,54.50336,-91.32086]}, + {"t":0.02772, "x":7.09882, "y":7.25681, "heading":3.14159, "vx":-0.14947, "vy":-0.01601, "omega":1.07755, "ax":-5.70391, "ay":-0.62751, "alpha":37.54287, "fx":[-24.64305,-46.0,-167.2345,-150.20982], "fy":[-173.33778,168.5721,52.38678,-90.31605]}, + {"t":0.05544, "x":7.09248, "y":7.25612, "heading":-3.11172, "vx":-0.30758, "vy":-0.03341, "omega":2.11821, "ax":-6.10519, "ay":-0.81304, "alpha":35.71071, "fx":[-30.30237,-66.78098,-168.7792,-149.52781], "fy":[-172.35756,161.37117,47.04227,-91.37418]}, + {"t":0.08316, "x":7.08161, "y":7.25489, "heading":-3.05301, "vx":-0.47681, "vy":-0.05594, "omega":3.10808, "ax":-6.59125, "ay":-1.14817, "alpha":33.24879, "fx":[-37.37866,-92.32472,-170.88443,-147.87344], "fy":[-170.83296,148.15566,38.4929,-93.93606]}, + {"t":0.11088, "x":7.06586, "y":7.25289, "heading":-2.96685, "vx":-0.65951, "vy":-0.08777, "omega":4.02972, "ax":-7.19205, "ay":-1.61896, "alpha":29.75011, "fx":[-50.99084,-119.48833,-173.051,-145.80872], "fy":[-167.05696,127.16966,26.71043,-96.97507]}, + {"t":0.1386, "x":7.04482, "y":7.24984, "heading":-2.85515, "vx":-0.85887, "vy":-0.13265, "omega":4.85437, "ax":-8.02449, "ay":-2.13454, "alpha":24.08436, "fx":[-81.00333,-145.64686,-174.61231,-144.71494], "fy":[-154.36571,95.94893,11.56323,-98.37833]}, + {"t":0.16632, "x":7.01793, "y":7.24534, "heading":-2.72059, "vx":-1.08131, "vy":-0.19181, "omega":5.52197, "ax":-9.14266, "ay":-2.4112, "alpha":14.75551, "fx":[-132.00222,-167.17469,-174.63508,-148.24442], "fy":[-113.30948,49.2455,-7.46533,-92.52567]}, + {"t":0.19404, "x":6.98444, "y":7.2391, "heading":-2.56753, "vx":-1.33473, "vy":-0.25865, "omega":5.93099, "ax":-9.92018, "ay":-2.39568, "alpha":2.86701, "fx":[-166.68538,-172.59211,-171.03951,-164.64095], "fy":[-49.91191,-22.74852,-33.38914,-56.94986]}, + {"t":0.22175, "x":6.94363, "y":7.23101, "heading":-2.40313, "vx":-1.60971, "vy":-0.32506, "omega":6.01046, "ax":-8.64505, "ay":-2.46787, "alpha":-17.373, "fx":[-173.2103,-129.79478,-129.76941,-155.4246], "fy":[-19.1182,-115.84925,-110.1739,77.23012]}, + {"t":0.24947, "x":6.89569, "y":7.22105, "heading":-2.23652, "vx":-1.84935, "vy":-0.39347, "omega":5.52889, "ax":-8.19574, "ay":-2.01719, "alpha":-19.52615, "fx":[-172.51875,-110.71522,-130.94396,-143.45058], "fy":[-24.25081,-133.88058,-76.99516,97.87934]}, + {"t":0.27719, "x":6.84128, "y":7.20937, "heading":-2.08326, "vx":-2.07653, "vy":-0.44938, "omega":4.98764, "ax":-8.7673, "ay":-0.48107, "alpha":-18.06358, "fx":[-170.73358,-114.38397,-157.1709,-154.22868], "fy":[-33.71076,-130.19213,51.53351,79.63821]}, + {"t":0.30491, "x":6.78035, "y":7.19673, "heading":-1.94501, "vx":-2.31955, "vy":-0.46272, "omega":4.48693, "ax":-8.94177, "ay":-0.71092, "alpha":-17.21691, "fx":[-168.51194,-119.41115,-158.53634,-161.92841], "fy":[-42.45798,-124.87966,56.97149,61.99607]}, + {"t":0.33263, "x":6.71262, "y":7.18363, "heading":-1.82063, "vx":-2.56741, "vy":-0.48242, "omega":4.00969, "ax":-8.96757, "ay":-1.01476, "alpha":-16.95381, "fx":[-165.64352,-119.64766,-158.7012,-166.15108], "fy":[-51.46291,-123.7511,57.59663,48.57419]}, + {"t":0.36035, "x":6.63801, "y":7.16987, "heading":-1.70949, "vx":-2.81599, "vy":-0.51055, "omega":3.53974, "ax":-8.86494, "ay":-1.38194, "alpha":-17.28785, "fx":[-161.71235,-114.73701,-158.10473,-168.60635], "fy":[-61.49828,-127.13857,57.34592,37.2656]}, + {"t":0.38807, "x":6.55654, "y":7.15518, "heading":-1.61137, "vx":-3.06172, "vy":-0.54886, "omega":3.06053, "ax":-8.62543, "ay":-1.83695, "alpha":-18.15794, "fx":[-156.15504,-103.91911,-156.83893,-169.95143], "fy":[-72.99352,-134.55126,56.04288,26.51802]}, + {"t":0.41579, "x":6.46836, "y":7.13926, "heading":-1.52653, "vx":-3.30081, "vy":-0.59978, "omega":2.55721, "ax":-8.22053, "ay":-2.40646, "alpha":-19.38706, "fx":[-148.1852,-86.08347,-154.79374,-170.25308], "fy":[-86.10941,-144.47553,51.90702,14.94512]}, + {"t":0.44351, "x":6.37371, "y":7.12171, "heading":-1.45565, "vx":-3.52868, "vy":-0.66648, "omega":2.01981, "ax":-7.59554, "ay":-3.12149, "alpha":-20.58348, "fx":[-136.64377,-60.56693,-150.67292,-168.9079], "fy":[-100.66862,-153.8996,41.06582,1.12013]}, + {"t":0.47123, "x":6.27298, "y":7.10204, "heading":-1.39966, "vx":-3.73922, "vy":-0.75301, "omega":1.44925, "ax":-6.63353, "ay":-4.00423, "alpha":-20.97251, "fx":[-120.27809,-29.96472,-137.15754,-163.93747], "fy":[-115.07475,-157.8074,16.26307,-15.82417]}, + {"t":0.49895, "x":6.16678, "y":7.07963, "heading":-1.35949, "vx":-3.9231, "vy":-0.864, "omega":0.86791, "ax":-5.39634, "ay":-4.53278, "alpha":-19.49133, "fx":[-103.40312,-13.58926,-99.92831,-150.23991], "fy":[-118.52612,-146.8548,-13.59309,-29.43103]}, + {"t":0.52667, "x":6.05596, "y":7.05394, "heading":-1.33543, "vx":-4.07268, "vy":-0.98965, "omega":0.32762, "ax":-1.69472, "ay":-4.76065, "alpha":-9.05052, "fx":[-42.91476,3.1751,-11.51238,-64.05446], "fy":[-99.86126,-98.61765,-61.14724,-64.28314]}, + {"t":0.55439, "x":5.94242, "y":7.02468, "heading":-1.32635, "vx":-4.11966, "vy":-1.12161, "omega":0.07675, "ax":0.99621, "ay":-4.14785, "alpha":-0.61892, "fx":[15.5506,18.95336,18.36414,14.91272], "fy":[-72.18486,-71.34536,-68.91671,-69.76831]}, + {"t":0.58211, "x":5.82861, "y":6.99199, "heading":-1.32422, "vx":-4.09204, "vy":-1.23658, "omega":0.05959, "ax":1.72245, "ay":-6.31301, "alpha":-1.24583, "fx":[25.67109,33.71778,33.10537,24.69907], "fy":[-110.21416,-108.09288,-104.50188,-106.72101]}, + {"t":0.60983, "x":5.71584, "y":6.95529, "heading":-1.32257, "vx":-4.0443, "vy":-1.41158, "omega":0.02506, "ax":3.1594, "ay":-4.46181, "alpha":4.23182, "fx":[65.74608,41.40843,42.86899,64.93866], "fy":[-62.33444,-72.7192,-88.95178,-79.57092]}, + {"t":0.62337, "x":5.66135, "y":6.93576, "heading":-1.32223, "vx":-4.00151, "vy":-1.47201, "omega":0.08237, "ax":1.75539, "ay":-3.80796, "alpha":0.8699, "fx":[31.90089,27.17805,27.86108,32.4949], "fy":[-62.27603,-63.7338,-67.25722,-65.82238]}, + {"t":0.63691, "x":5.60732, "y":6.91548, "heading":-1.32112, "vx":-3.97773, "vy":-1.52358, "omega":0.09415, "ax":1.12886, "ay":-2.82248, "alpha":0.05368, "fx":[19.31154,19.03827,19.09189,19.36492], "fy":[-47.85497,-47.93071,-48.16425,-48.08856]}, + {"t":0.65046, "x":5.55355, "y":6.89458, "heading":-1.31984, "vx":-3.96244, "vy":-1.56181, "omega":0.09488, "ax":0.76096, "ay":-1.96373, "alpha":-0.07961, "fx":[12.79329,13.183,13.09447,12.70441], "fy":[-33.6351,-33.53126,-33.16989,-33.27379]}, + {"t":0.664, "x":5.49995, "y":6.87325, "heading":-1.31856, "vx":-3.95214, "vy":-1.5884, "omega":0.0938, "ax":0.51452, "ay":-1.3236, "alpha":-0.07081, "fx":[8.62357,8.96322,8.88031,8.54047], "fy":[-22.72273,-22.6336,-22.30537,-22.39454]}, + {"t":0.67754, "x":5.44647, "y":6.85162, "heading":-1.31729, "vx":-3.94517, "vy":-1.60633, "omega":0.09284, "ax":0.3458, "ay":-0.88022, "alpha":-0.04592, "fx":[5.80052,6.01864,5.96347,5.7453], "fy":[-15.10818,-15.0512,-14.83636,-14.89334]}, + {"t":0.69109, "x":5.39307, "y":6.82978, "heading":-1.31603, "vx":-3.94048, "vy":-1.61825, "omega":0.09222, "ax":0.23185, "ay":-0.58381, "alpha":-0.02664, "fx":[3.89701,4.02299,3.99053,3.86453], "fy":[-10.0094,-9.97647,-9.85133,-9.88426]}, + {"t":0.70463, "x":5.33972, "y":6.80781, "heading":-1.31478, "vx":-3.93734, "vy":-1.62616, "omega":0.09186, "ax":0.15635, "ay":-0.38962, "alpha":-0.01388, "fx":[2.63517,2.70067,2.6836,2.6181], "fy":[-6.6685,-6.65133,-6.58602,-6.60319]}, + {"t":0.71818, "x":5.28641, "y":6.78575, "heading":-1.31353, "vx":-3.93523, "vy":-1.63144, "omega":0.09167, "ax":0.10746, "ay":-0.26497, "alpha":-0.00579, "fx":[1.81786,1.84517,1.838,1.81068], "fy":[-4.52432,-4.51712,-4.48985,-4.49704]}, + {"t":0.73172, "x":5.23312, "y":6.76363, "heading":-1.31229, "vx":-3.93377, "vy":-1.63503, "omega":0.09159, "ax":0.07711, "ay":-0.18798, "alpha":-0.00085, "fx":[1.31006,1.31408,1.31302,1.30899], "fy":[-3.19997,-3.19891,-3.19489,-3.19595]}, + {"t":0.74526, "x":5.17985, "y":6.74147, "heading":-1.31105, "vx":-3.93273, "vy":-1.63757, "omega":0.09158, "ax":0.06007, "ay":-0.14488, "alpha":0.0019, "fx":[1.02505,1.01609,1.01847,1.02743], "fy":[-2.45863,-2.46101,-2.46996,-2.46758]}, + {"t":0.75881, "x":5.12659, "y":6.71927, "heading":-1.30981, "vx":-3.93191, "vy":-1.63953, "omega":0.09161, "ax":0.05339, "ay":-0.12791, "alpha":0.003, "fx":[0.91335,0.89921,0.90298,0.91712], "fy":[-2.16678,-2.17056,-2.1847,-2.18092]}, + {"t":0.77235, "x":5.07335, "y":6.69706, "heading":-1.30857, "vx":-3.93119, "vy":-1.64127, "omega":0.09165, "ax":0.05591, "ay":-0.13402, "alpha":0.00267, "fx":[0.95569,0.94315,0.94651,0.95905], "fy":[-2.27162,-2.27498,-2.28752,-2.28415]}, + {"t":0.78589, "x":5.02011, "y":6.67481, "heading":-1.30733, "vx":-3.93043, "vy":-1.64308, "omega":0.09168, "ax":0.06813, "ay":-0.16427, "alpha":0.00083, "fx":[1.16034,1.15643,1.15748,1.1614], "fy":[-2.79165,-2.79271,-2.79662,-2.79557]}, + {"t":0.79944, "x":4.96688, "y":6.65255, "heading":-1.30609, "vx":-3.92951, "vy":-1.64531, "omega":0.0917, "ax":0.0923, "ay":-0.22407, "alpha":-0.00279, "fx":[1.5652,1.57834,1.57478,1.56165], "fy":[-3.81969,-3.81613,-3.80301,-3.80657]}, + {"t":0.81298, "x":4.91367, "y":6.63024, "heading":-1.30485, "vx":-3.92826, "vy":-1.64834, "omega":0.09166, "ax":0.13288, "ay":-0.32404, "alpha":-0.00876, "fx":[2.24525,2.28644,2.27526,2.23406], "fy":[-5.53802,-5.52678,-5.48568,-5.49691]}, + {"t":0.82653, "x":4.86048, "y":6.60789, "heading":-1.30361, "vx":-3.92646, "vy":-1.65273, "omega":0.09154, "ax":0.1974, "ay":-0.48167, "alpha":-0.01775, "fx":[3.32732,3.4109,3.38818,3.3046], "fy":[-8.24605,-8.22312,-8.13992,-8.16285]}, + {"t":0.84007, "x":4.80732, "y":6.58546, "heading":-1.30237, "vx":-3.92379, "vy":-1.65925, "omega":0.0913, "ax":0.29796, "ay":-0.7235, "alpha":-0.02986, "fx":[5.01685,5.15781,5.11965,4.97867], "fy":[-12.39572,-12.35674,-12.21723,-12.25622]}, + {"t":0.85361, "x":4.7542, "y":6.56292, "heading":-1.30113, "vx":-3.91975, "vy":-1.66905, "omega":0.09089, "ax":0.4541, "ay":-1.08668, "alpha":-0.04022, "fx":[7.65403,7.84503,7.79414,7.60309], "fy":[-18.60416,-18.5507,-18.3641,-18.41757]}, + {"t":0.86716, "x":4.70115, "y":6.54021, "heading":-1.2999, "vx":-3.9136, "vy":-1.68377, "omega":0.09035, "ax":0.70075, "ay":-1.61426, "alpha":-0.01549, "fx":[11.89174,11.96633,11.94728,11.87269], "fy":[-27.50415,-27.48281,-27.41197,-27.43331]}, + {"t":0.8807, "x":4.64821, "y":6.51726, "heading":-1.29867, "vx":-3.90411, "vy":-1.70563, "omega":0.09014, "ax":1.11546, "ay":-2.32712, "alpha":0.27775, "fx":[19.50377,18.12928,18.44623,19.81539], "fy":[-38.75992,-39.17329,-40.40703,-39.99459]}, + {"t":0.89425, "x":4.59544, "y":6.49395, "heading":-1.29745, "vx":-3.889, "vy":-1.73715, "omega":0.0939, "ax":0.85376, "ay":-2.58565, "alpha":-0.84844, "fx":[12.91794,17.13933,16.15299,11.87846], "fy":[-46.44181,-45.23045,-41.51378,-42.73874]}, + {"t":0.90891, "x":4.53851, "y":6.4682, "heading":-1.29608, "vx":-3.87648, "vy":-1.77506, "omega":0.08146, "ax":0.68993, "ay":-1.63534, "alpha":-0.18626, "fx":[11.4041,12.29973,12.06787,11.17061], "fy":[-28.3714,-28.11202,-27.26194,-27.52162]}, + {"t":0.92357, "x":4.48175, "y":6.442, "heading":-1.29488, "vx":-3.86637, "vy":-1.79904, "omega":0.07873, "ax":0.46366, "ay":-1.03406, "alpha":-0.06095, "fx":[7.78191,8.07049,7.9916,7.70291], "fy":[-17.77178,-17.689,-17.40639,-17.48919]}, + {"t":0.93823, "x":4.42511, "y":6.41551, "heading":-1.29373, "vx":-3.85957, "vy":-1.8142, "omega":0.07784, "ax":0.30002, "ay":-0.65541, "alpha":-0.02796, "fx":[5.056,5.18756,5.15067,5.0191], "fy":[-11.23239,-11.19477,-11.06432,-11.10194]}, + {"t":0.95289, "x":4.36856, "y":6.38884, "heading":-1.29259, "vx":-3.85517, "vy":-1.82381, "omega":0.07743, "ax":0.19541, "ay":-0.42233, "alpha":-0.01415, "fx":[3.30006,3.36646,3.3476,3.2812], "fy":[-7.22622,-7.20721,-7.14104,-7.16005]}, + {"t":0.96755, "x":4.31205, "y":6.36206, "heading":-1.29145, "vx":-3.85231, "vy":-1.83, "omega":0.07722, "ax":0.13282, "ay":-0.28478, "alpha":-0.00695, "fx":[2.24763,2.28019,2.27087,2.23831], "fy":[-4.86502,-4.85567,-4.82316,-4.83251]}, + {"t":0.98221, "x":4.25559, "y":6.3352, "heading":-1.29032, "vx":-3.85036, "vy":-1.83418, "omega":0.07712, "ax":0.09962, "ay":-0.21219, "alpha":-0.00328, "fx":[1.68902,1.70439,1.69996,1.6846], "fy":[-3.61924,-3.61481,-3.59945,-3.60388]}, + {"t":0.99688, "x":4.19915, "y":6.30828, "heading":-1.28919, "vx":-3.8489, "vy":-1.83729, "omega":0.07707, "ax":0.08869, "ay":-0.18814, "alpha":-0.00206, "fx":[1.50512,1.51474,1.51196,1.50234], "fy":[-3.20633,-3.20354,-3.19393,-3.19671]}, + {"t":1.01154, "x":4.14272, "y":6.28132, "heading":-1.28806, "vx":-3.8476, "vy":-1.84004, "omega":0.07704, "ax":0.0977, "ay":-0.20711, "alpha":-0.00292, "fx":[1.65699,1.67065,1.66669,1.65303], "fy":[-3.53169,-3.52772,-3.51407,-3.51804]}, + {"t":1.0262, "x":4.08632, "y":6.25432, "heading":-1.28693, "vx":-3.84617, "vy":-1.84308, "omega":0.077, "ax":0.12882, "ay":-0.27334, "alpha":-0.00602, "fx":[2.18127,2.20942,2.20123,2.17307], "fy":[-4.66768,-4.65946,-4.63135,-4.63957]}, + {"t":1.04086, "x":4.02995, "y":6.22727, "heading":-1.2858, "vx":-3.84428, "vy":-1.84709, "omega":0.07691, "ax":0.1893, "ay":-0.40159, "alpha":-0.01194, "fx":[3.20009,3.25598,3.23969,3.18379], "fy":[-6.86702,-6.85061,-6.79489,-6.8113]}, + {"t":1.05552, "x":3.9736, "y":6.20015, "heading":-1.28467, "vx":-3.8415, "vy":-1.85298, "omega":0.07673, "ax":0.29303, "ay":-0.61976, "alpha":-0.02172, "fx":[4.94816,5.05002,5.02044,4.91857], "fy":[-10.60758,-10.57746,-10.47637,-10.50649]}, + {"t":1.07018, "x":3.91731, "y":6.17291, "heading":-1.28355, "vx":-3.83721, "vy":-1.86206, "omega":0.07641, "ax":0.46311, "ay":-0.97252, "alpha":-0.03672, "fx":[7.81565,7.98877,7.93921,7.76606], "fy":[-16.65318,-16.60141,-16.43148,-16.48326]}, + {"t":1.08485, "x":3.8611, "y":6.14551, "heading":-1.28243, "vx":-3.83042, "vy":-1.87632, "omega":0.07588, "ax":0.73379, "ay":-1.52141, "alpha":-0.05814, "fx":[12.3809,12.65852,12.58242,12.30465], "fy":[-26.05387,-25.969,-25.70373,-25.78863]}, + {"t":1.09951, "x":3.80502, "y":6.11783, "heading":-1.28132, "vx":-3.81966, "vy":-1.89863, "omega":0.07502, "ax":1.14537, "ay":-2.32676, "alpha":-0.08531, "fx":[19.32433,19.74357,19.64059,19.22084], "fy":[-39.83287,-39.69895,-39.32198,-39.45601]}, + {"t":1.11417, "x":3.74914, "y":6.08975, "heading":-1.28022, "vx":-3.80286, "vy":-1.93274, "omega":0.07377, "ax":1.71884, "ay":-3.38695, "alpha":-0.11203, "fx":[29.00228,29.58331,29.47232,28.88998], "fy":[-57.94341,-57.74258,-57.27859,-57.47978]}, + {"t":1.12883, "x":3.69357, "y":6.06105, "heading":-1.27913, "vx":-3.77766, "vy":-1.9824, "omega":0.07213, "ax":2.41653, "ay":-4.5619, "alpha":-0.12009, "fx":[40.80599,41.47876,41.40403,40.72914], "fy":[-77.94992,-77.68976,-77.24281,-77.50379]}, + {"t":1.14349, "x":3.63844, "y":6.03149, "heading":-1.27808, "vx":-3.74223, "vy":-2.04929, "omega":0.07037, "ax":3.15576, "ay":-5.61638, "alpha":-0.06113, "fx":[53.49713,53.86827,53.86023,53.48839], "fy":[-95.71389,-95.55064,-95.35182,-95.51544]}, + {"t":1.15815, "x":3.58392, "y":6.00084, "heading":-1.27704, "vx":-3.69597, "vy":-2.13163, "omega":0.06947, "ax":3.90373, "ay":-6.36996, "alpha":0.21351, "fx":[67.13647,65.7586,65.67109,67.03933], "fy":[-107.69324,-108.3913,-109.0055,-108.31443]}, + {"t":1.17282, "x":3.53015, "y":5.9689, "heading":-1.27603, "vx":-3.63873, "vy":-2.22502, "omega":0.0726, "ax":4.7555, "ay":-6.70987, "alpha":1.09608, "fx":[85.09717,77.86406,76.80192,83.79577], "fy":[-110.37571,-114.78885,-117.76611,-113.60106]}, + {"t":1.18748, "x":3.47731, "y":5.93556, "heading":-1.27496, "vx":-3.56901, "vy":-2.3234, "omega":0.08867, "ax":5.82332, "ay":-6.51164, "alpha":3.16845, "fx":[111.72361,91.94816,87.01228,105.52798], "fy":[-97.6269,-113.56809,-122.6832,-109.1664]}, + {"t":1.20214, "x":3.42561, "y":5.90079, "heading":-1.27366, "vx":-3.48363, "vy":-2.41887, "omega":0.13513, "ax":6.87173, "ay":-6.24303, "alpha":4.68657, "fx":[135.42821,109.55866,98.68198,123.87571], "fy":[-83.21214,-111.22588,-126.30644,-104.02417]}, + {"t":1.22131, "x":3.36008, "y":5.85327, "heading":-1.27107, "vx":-3.35188, "vy":-2.53857, "omega":0.22498, "ax":8.18907, "ay":-4.33364, "alpha":9.12658, "fx":[162.82602,138.30882,108.75441,147.28545], "fy":[-20.72088,-73.29995,-122.94961,-77.88555]}, + {"t":1.24048, "x":3.29732, "y":5.8038, "heading":-1.26676, "vx":-3.19487, "vy":-2.62166, "omega":0.39997, "ax":8.77545, "ay":-2.93645, "alpha":11.44089, "fx":[167.26877,156.12642,116.34202,157.3346], "fy":[13.84807,-31.2142,-119.69413,-62.73213]}, + {"t":1.25966, "x":3.23768, "y":5.753, "heading":-1.25909, "vx":-3.02662, "vy":-2.67796, "omega":0.61932, "ax":9.05624, "ay":-2.06557, "alpha":12.47734, "fx":[167.33656,162.80269,123.64797,162.38866], "fy":[29.75807,-1.86477,-114.96147,-53.47056]}, + {"t":1.27883, "x":3.18131, "y":5.70128, "heading":-1.24721, "vx":-2.85299, "vy":-2.71756, "omega":0.85854, "ax":9.21968, "ay":-1.57559, "alpha":12.76259, "fx":[167.10954,165.18379,129.7062,165.29698], "fy":[37.38841,13.0168,-110.19598,-47.41077]}, + {"t":1.298, "x":3.12831, "y":5.64888, "heading":-1.23075, "vx":-2.67622, "vy":-2.74777, "omega":1.10324, "ax":9.339, "ay":-1.31139, "alpha":12.62055, "fx":[167.08033,166.70231,134.47413,167.1581], "fy":[41.15748,18.71068,-105.94687,-43.14684]}, + {"t":1.31718, "x":3.07871, "y":5.59596, "heading":-1.2096, "vx":-2.49717, "vy":-2.77291, "omega":1.34521, "ax":9.44112, "ay":-1.17413, "alpha":12.21752, "fx":[167.25334,168.1639,138.46266,168.48291], "fy":[42.84404,19.01473,-101.95122,-39.79372]}, + {"t":1.33635, "x":3.03257, "y":5.54258, "heading":-1.18381, "vx":-2.31616, "vy":-2.79542, "omega":1.57945, "ax":9.53642, "ay":-1.10584, "alpha":11.63734, "fx":[167.59072,169.59503,142.13515,169.52632], "fy":[43.22397,16.12762,-97.78805,-36.80393]}, + {"t":1.35552, "x":2.98992, "y":5.48878, "heading":-1.15353, "vx":-2.13332, "vy":-2.81662, "omega":1.80257, "ax":9.62896, "ay":-1.06997, "alpha":10.92844, "fx":[168.06696,170.86156,145.79623,170.41876], "fy":[42.64703,11.48136,-93.08293,-33.84475]}, + {"t":1.37469, "x":2.95079, "y":5.43458, "heading":-1.11897, "vx":-1.9487, "vy":-2.83714, "omega":2.0121, "ax":9.7199, "ay":-1.04165, "alpha":10.12394, "fx":[168.66602,171.85549,149.58704,171.22231], "fy":[41.26907,6.16069,-87.57216,-30.72996]}, + {"t":1.39387, "x":2.91521, "y":5.37999, "heading":-1.08039, "vx":-1.76235, "vy":-2.85711, "omega":2.2062, "ax":9.8089, "ay":-1.00428, "alpha":9.24777, "fx":[169.37102,172.55315,153.50391,171.95803], "fy":[39.16399,1.0173,-81.12798,-27.38355]}, + {"t":1.41304, "x":2.88322, "y":5.32503, "heading":-1.03809, "vx":-1.57428, "vy":-2.87636, "omega":2.38351, "ax":9.8948, "ay":-0.9483, "alpha":8.31566, "fx":[170.15719,173.00885,157.44143,172.62364], "fy":[36.38122,-3.34235,-73.74855,-23.81127]}, + {"t":1.43221, "x":2.85486, "y":5.26971, "heading":-0.99239, "vx":-1.38457, "vy":-2.89454, "omega":2.54294, "ax":9.97605, "ay":-0.8698, "alpha":7.33613, "fx":[170.99002,173.31359,161.24838,173.20701], "fy":[32.97278,-6.56351,-65.5176,-20.07193]}, + {"t":1.45138, "x":2.83015, "y":5.21405, "heading":-0.94364, "vx":-1.19331, "vy":-2.91122, "omega":2.68359, "ax":10.0509, "ay":-0.76867, "alpha":6.31255, "fx":[171.82759,173.55293,164.77517,173.69581], "fy":[29.00327,-8.49855,-56.55273,-16.25134]}, + {"t":1.47056, "x":2.80912, "y":5.1581, "heading":-0.89219, "vx":-1.0006, "vy":-2.92596, "omega":2.80462, "ax":10.11784, "ay":-0.64569, "alpha":5.24108, "fx":[172.62697,173.78484,167.91056,174.0836], "fy":[24.53606,-9.11813,-46.91881,-12.43089]}, + {"t":1.48973, "x":2.79179, "y":5.10188, "heading":-0.83841, "vx":-0.80662, "vy":-2.93834, "omega":2.90511, "ax":10.17717, "ay":-0.49318, "alpha":4.07029, "fx":[173.36453,174.04459,170.65899,174.3749], "fy":[19.47471,-8.28014,-36.18727,-8.56248]}, + {"t":1.5089, "x":2.7782, "y":5.04545, "heading":-0.78272, "vx":-0.61149, "vy":-2.94779, "omega":2.98315, "ax":10.23301, "ay":-0.25631, "alpha":2.49876, "fx":[174.06999,174.3671,173.22732,174.57788], "fy":[12.80295,-4.89567,-21.40876,-3.93788]}, + {"t":1.52808, "x":2.76835, "y":4.98889, "heading":-0.72552, "vx":-0.4153, "vy":-2.95271, "omega":3.03105, "ax":10.26506, "ay":0.1383, "alpha":0.12479, "fx":[174.59298,174.6031,174.61639,174.61039], "fy":[3.20707,2.31261,1.49833,2.39208]}, + {"t":1.54725, "x":2.76228, "y":4.9323, "heading":-0.66741, "vx":-0.21849, "vy":-2.95006, "omega":3.03345, "ax":10.23097, "ay":0.57336, "alpha":-2.41945, "fx":[174.5749,174.43413,172.69444,174.39976], "fy":[-6.70072,10.72126,26.35797,8.63256]}, + {"t":1.56642, "x":2.75997, "y":4.87585, "heading":-0.60925, "vx":-0.02233, "vy":-2.93906, "omega":2.98706, "ax":10.14103, "ay":0.96364, "alpha":-4.66894, "fx":[174.13561,173.85921,167.90232,174.08677], "fy":[-14.99929,18.78125,48.47458,13.30875]}, + {"t":1.58559, "x":2.7614, "y":4.81967, "heading":-0.55198, "vx":0.1721, "vy":-2.92059, "omega":2.89754, "ax":10.02037, "ay":1.29333, "alpha":-6.56242, "fx":[173.54267,172.9642,161.42169,173.84622], "fy":[-21.3478,26.29426,67.11334,15.93708]}, + {"t":1.60477, "x":2.76655, "y":4.76392, "heading":-0.49642, "vx":0.36422, "vy":-2.89579, "omega":2.77172, "ax":9.94934, "ay":1.81928, "alpha":-6.51688, "fx":[174.58164,171.43778,157.94158,172.98055], "fy":[-11.86959,35.44303,75.26438,24.94392]}, + {"t":1.62993, "x":2.77886, "y":4.69161, "heading":-0.42667, "vx":0.61461, "vy":-2.85, "omega":2.60771, "ax":9.69604, "ay":2.71818, "alpha":-7.21456, "fx":[174.90673,167.53379,147.60739,169.6594], "fy":[-0.77148,50.60021,93.78537,41.32773]}, + {"t":1.6551, "x":2.7974, "y":4.62075, "heading":-0.36104, "vx":0.85864, "vy":-2.7816, "omega":2.42614, "ax":9.2192, "ay":3.90082, "alpha":-8.04584, "fx":[174.21692,160.68322,130.97096,161.39305], "fy":[14.346,69.1806,115.76095,66.12005]}, + {"t":1.68027, "x":2.82193, "y":4.55198, "heading":-0.29998, "vx":1.09066, "vy":-2.68342, "omega":2.22365, "ax":8.31476, "ay":5.4308, "alpha":-9.05592, "fx":[170.9936,148.94513,105.19621,140.59148], "fy":[35.63836,91.61903,139.49593,102.7523]}, + {"t":1.70544, "x":2.85201, "y":4.48616, "heading":-0.24401, "vx":1.29992, "vy":-2.54674, "omega":1.99573, "ax":6.65701, "ay":7.21517, "alpha":-10.49398, "fx":[161.44277,129.56447,68.95093,92.97693], "fy":[66.18181,117.32079,160.47305,146.93612]}, + {"t":1.7306, "x":2.88684, "y":4.42435, "heading":-0.19379, "vx":1.46746, "vy":-2.36516, "omega":1.73163, "ax":4.12559, "ay":8.77282, "alpha":-12.13169, "fx":[136.83628,99.93325,25.75086,18.17984], "fy":[107.89884,143.32231,172.74458,172.92713]}, + {"t":1.75577, "x":2.92508, "y":4.36761, "heading":-0.15021, "vx":1.57129, "vy":-2.14437, "omega":1.42631, "ax":1.19474, "ay":9.65711, "alpha":-11.85171, "fx":[85.22231,60.4345,-16.41859,-47.9496], "fy":[151.81837,163.90537,173.92611,167.40915]}, + {"t":1.78094, "x":2.965, "y":4.3167, "heading":-0.11431, "vx":1.60136, "vy":-1.90133, "omega":1.12803, "ax":-1.58657, "ay":9.77609, "alpha":-9.60914, "fx":[14.4751,16.78327,-51.16244,-88.04418], "fy":[173.55975,173.90311,167.11868,150.57222]}, + {"t":1.8061, "x":3.0048, "y":4.27194, "heading":-0.08592, "vx":1.56143, "vy":-1.65529, "omega":0.88619, "ax":-3.75385, "ay":9.31802, "alpha":-7.49341, "fx":[-44.63866,-23.21644,-77.05724,-110.4956], "fy":[168.5673,173.22053,156.96123,135.2384]}, + {"t":1.83127, "x":3.04291, "y":4.23323, "heading":-0.06362, "vx":1.46695, "vy":-1.42078, "omega":0.69761, "ax":-5.24027, "ay":8.67162, "alpha":-6.00161, "fx":[-81.9801,-55.21733,-95.65054,-123.69429], "fy":[154.14922,165.89625,146.4636,123.49793]}, + {"t":1.85644, "x":3.07817, "y":4.20022, "heading":-0.04606, "vx":1.33507, "vy":-1.20254, "omega":0.54656, "ax":-6.23796, "ay":8.0519, "alpha":-4.91297, "fx":[-104.33315,-79.06562,-108.99815,-132.02703], "fy":[140.19712,156.02902,136.90225,114.71396]}, + {"t":1.88161, "x":3.10979, "y":4.17251, "heading":-0.03231, "vx":1.17808, "vy":-0.99989, "omega":0.42292, "ax":-6.92352, "ay":7.51801, "alpha":-4.07835, "fx":[-118.24912,-96.44279,-118.74051,-137.63624], "fy":[128.84173,146.00756,128.61592,108.05184]}, + {"t":1.90677, "x":3.13725, "y":4.14972, "heading":-0.02166, "vx":1.00383, "vy":-0.81068, "omega":0.32027, "ax":-7.41073, "ay":7.07219, "alpha":-3.42168, "fx":[-127.42608,-109.16168,-126.01398,-141.61551], "fy":[119.90404,136.83069,121.56171,102.88723]}, + {"t":1.93194, "x":3.16016, "y":4.13156, "heading":-0.0136, "vx":0.81732, "vy":-0.6327, "omega":0.23416, "ax":-7.76864, "ay":6.70186, "alpha":-2.89603, "fx":[-133.80911,-118.62693,-131.57335,-144.56007], "fy":[112.84162,128.77437,115.5772,98.79394]}, + {"t":1.95711, "x":3.17827, "y":4.11776, "heading":-0.00771, "vx":0.62181, "vy":-0.46403, "omega":0.16127, "ax":-8.03956, "ay":6.39285, "alpha":-2.46885, "fx":[-138.45146,-125.8183,-135.91816,-146.81451], "fy":[107.18084,121.81347,110.48395,95.48359]}, + {"t":1.98228, "x":3.19138, "y":4.10811, "heading":-0.00365, "vx":0.41947, "vy":-0.30314, "omega":0.09914, "ax":-8.25005, "ay":6.13284, "alpha":-2.11671, "fx":[-141.95325,-131.39826,-139.3832,-148.58931], "fy":[102.56933,115.82163,106.12236,92.75833]}, + {"t":2.00744, "x":3.19932, "y":4.10242, "heading":-0.00115, "vx":0.21184, "vy":-0.14879, "omega":0.04587, "ax":-8.41733, "ay":5.91202, "alpha":-1.82255, "fx":[-144.6746,-135.8144,-142.19708,-150.01912], "fy":[98.75398,110.65363,102.3596,90.47954]}, + {"t":2.03261, "x":3.20199, "y":4.10055, "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/startDeepToK.traj b/src/main/deploy/choreo/startDeepToK.traj new file mode 100644 index 00000000..869fb520 --- /dev/null +++ b/src/main/deploy/choreo/startDeepToK.traj @@ -0,0 +1,104 @@ +{ + "name":"startDeepToK", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.25449800491333, "y":6.856431007385254, "heading":-1.348714396235503, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.315337657928467, "y":6.098512172698975, "heading":-1.231504335673354, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.25449800491333 m", "val":5.25449800491333}, "y":{"exp":"6.856431007385254 m", "val":6.856431007385254}, "heading":{"exp":"-1.348714396235503 rad", "val":-1.348714396235503}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.315337657928467 m", "val":4.315337657928467}, "y":{"exp":"6.098512172698975 m", "val":6.098512172698975}, "heading":{"exp":"-1.231504335673354 rad", "val":-1.231504335673354}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.73383,1.04227,1.44583], + "samples":[ + {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.67919, "ay":-0.64602, "alpha":37.62249, "fx":[-23.7765,-45.4132,-167.41749,-149.79852], "fy":[-173.45579,168.70739,51.78483,-90.99091]}, + {"t":0.02367, "x":7.0993, "y":7.25685, "heading":3.14159, "vx":-0.13444, "vy":-0.01529, "omega":0.8906, "ax":-6.25804, "ay":-0.78162, "alpha":34.95301, "fx":[-36.02302,-70.41092,-168.59917,-150.75706], "fy":[-171.27185,159.75125,47.67643,-89.33623]}, + {"t":0.04734, "x":7.09436, "y":7.25627, "heading":-3.12051, "vx":-0.28258, "vy":-0.03379, "omega":1.718, "ax":-6.9638, "ay":-1.09327, "alpha":31.27313, "fx":[-50.10129,-102.23548,-170.40524,-151.06715], "fy":[-167.60745,141.38094,40.57494,-88.73329]}, + {"t":0.07102, "x":7.08572, "y":7.25516, "heading":-3.07984, "vx":-0.44742, "vy":-0.05967, "omega":2.4583, "ax":-7.74375, "ay":-1.57568, "alpha":26.49393, "fx":[-69.08414,-134.12975,-172.47214,-151.19005], "fy":[-160.58893,111.48799,30.30841,-88.415]}, + {"t":0.09469, "x":7.07296, "y":7.25331, "heading":-3.02165, "vx":-0.63073, "vy":-0.09697, "omega":3.08546, "ax":-8.54757, "ay":-2.1231, "alpha":20.45828, "fx":[-96.41233,-158.97426,-174.26273,-151.91784], "fy":[-145.64882,71.73287,16.45776,-86.99521]}, + {"t":0.11836, "x":7.05563, "y":7.25042, "heading":-2.94861, "vx":-0.83307, "vy":-0.14723, "omega":3.56975, "ax":-9.30402, "ay":-2.54366, "alpha":13.00669, "fx":[-130.92972,-172.49402,-174.90792,-154.70299], "fy":[-115.39961,25.98556,-1.99018,-81.66338]}, + {"t":0.14203, "x":7.03331, "y":7.24622, "heading":-2.86411, "vx":-1.05332, "vy":-0.20745, "omega":3.87764, "ax":-9.80214, "ay":-2.727, "alpha":4.80846, "fx":[-159.37872,-173.1933,-172.5531,-161.80155], "fy":[-71.02164,-21.22364,-27.38467,-65.91219]}, + {"t":0.1657, "x":7.00563, "y":7.24055, "heading":-2.77232, "vx":-1.28535, "vy":-0.272, "omega":3.99147, "ax":-9.87663, "ay":-2.75936, "alpha":0.17838, "fx":[-167.78359,-168.24861,-168.21213,-167.75004], "fy":[-47.68596,-46.0222,-46.19406,-47.84131]}, + {"t":0.18938, "x":6.97243, "y":7.23333, "heading":-2.67783, "vx":-1.51915, "vy":-0.33732, "omega":3.99569, "ax":-9.85759, "ay":-2.7715, "alpha":0.00283, "fx":[-167.67178,-167.67908,-167.67788,-167.67058], "fy":[-47.15289,-47.12708,-47.13209,-47.1579]}, + {"t":0.21305, "x":6.93371, "y":7.22457, "heading":-2.58324, "vx":-1.7525, "vy":-0.40293, "omega":3.99576, "ax":-9.82588, "ay":-2.79135, "alpha":-0.00015, "fx":[-167.13556,-167.13518,-167.13528,-167.13566], "fy":[-47.47958,-47.4809,-47.48052,-47.4792]}, + {"t":0.23672, "x":6.88947, "y":7.21425, "heading":-2.48866, "vx":-1.9851, "vy":-0.469, "omega":3.99576, "ax":-9.76329, "ay":-2.82981, "alpha":-0.00051, "fx":[-166.07118,-166.0699,-166.07033,-166.07161], "fy":[-48.1329,-48.13724,-48.13553,-48.1312]}, + {"t":0.26039, "x":6.83974, "y":7.20236, "heading":-2.39407, "vx":-2.21621, "vy":-0.53599, "omega":3.99574, "ax":-9.58237, "ay":-2.93651, "alpha":-0.00743, "fx":[-162.99951,-162.98012,-162.98733,-163.00672], "fy":[-49.93403,-49.99427,-49.9645,-49.90423]}, + {"t":0.28406, "x":6.7846, "y":7.18885, "heading":-2.29948, "vx":-2.44305, "vy":-0.6055, "omega":3.99557, "ax":-5.16255, "ay":-4.18101, "alpha":-9.57157, "fx":[-114.16821,-76.37471,-54.94828,-105.76265], "fy":[-63.35535,-103.3954,-86.83979,-30.88089]}, + {"t":0.30774, "x":6.72532, "y":7.17334, "heading":-2.2049, "vx":-2.56525, "vy":-0.70447, "omega":3.76899, "ax":-5.43791, "ay":-0.26394, "alpha":-12.86776, "fx":[-123.19867,-88.36109,-58.01343,-100.41597], "fy":[-10.84764,-61.07096,5.18552,48.77521]}, + {"t":0.33141, "x":6.66307, "y":7.15659, "heading":-2.11568, "vx":-2.69398, "vy":-0.71072, "omega":3.46439, "ax":-5.51193, "ay":1.23377, "alpha":-11.25596, "fx":[-120.97794,-93.40428,-62.97155,-97.67173], "fy":[7.95345,-29.49315,42.95919,62.52509]}, + {"t":0.35508, "x":6.59775, "y":7.14011, "heading":-2.03367, "vx":-2.82446, "vy":-0.68152, "omega":3.19793, "ax":-5.38115, "ay":1.51934, "alpha":-10.13142, "fx":[-116.60616,-89.82915,-63.34035,-96.35189], "fy":[10.55887,-17.65707,49.39675,61.07534]}, + {"t":0.37875, "x":6.52939, "y":7.12441, "heading":-1.95797, "vx":-2.95184, "vy":-0.64555, "omega":2.9581, "ax":-5.12807, "ay":1.3341, "alpha":-9.29135, "fx":[-110.40529,-82.65371,-61.73925,-94.10962], "fy":[6.73519,-15.24707,45.43225,53.85034]}, + {"t":0.40242, "x":6.45807, "y":7.1095, "heading":-1.88795, "vx":-3.07323, "vy":-0.61397, "omega":2.73816, "ax":-4.76183, "ay":0.97481, "alpha":-8.5195, "fx":[-102.23192,-73.56289,-58.23428,-89.96063], "fy":[0.77572,-16.3276,37.59833,44.27865]}, + {"t":0.4261, "x":6.38399, "y":7.09524, "heading":-1.82313, "vx":-3.18595, "vy":-0.59089, "omega":2.53649, "ax":-4.31281, "ay":0.58172, "alpha":-7.72836, "fx":[-92.51681,-63.75666,-53.26659,-83.89855], "fy":[-5.26322,-18.17484,28.82278,34.19487]}, + {"t":0.44977, "x":6.30736, "y":7.08141, "heading":-1.76308, "vx":-3.28805, "vy":-0.57712, "omega":2.35354, "ax":-3.8345, "ay":0.21747, "alpha":-6.9396, "fx":[-82.24231,-54.33363,-47.64972,-76.66956], "fy":[-10.5736,-19.92709,20.5874,24.71001]}, + {"t":0.47344, "x":6.22846, "y":7.06781, "heading":-1.70737, "vx":-3.37882, "vy":-0.57198, "omega":2.18927, "ax":-3.38672, "ay":-0.10986, "alpha":-6.24459, "fx":[-72.6359,-46.09307,-42.2334,-69.4665], "fy":[-15.28102,-21.83472,13.35915,16.28199]}, + {"t":0.49711, "x":6.14752, "y":7.05424, "heading":-1.65555, "vx":-3.45899, "vy":-0.57458, "omega":2.04145, "ax":-3.01734, "ay":-0.43102, "alpha":-5.75856, "fx":[-64.7794,-39.38793,-37.61701,-63.51215], "fy":[-20.18183,-24.73258,6.85969,8.72883]}, + {"t":0.52078, "x":6.0648, "y":7.04052, "heading":-1.60722, "vx":-3.53041, "vy":-0.58478, "omega":1.90513, "ax":-2.75298, "ay":-0.79867, "alpha":-5.5871, "fx":[-59.34553,-34.14263,-34.06618,-59.75499], "fy":[-26.42817,-29.68515,0.38326,1.38927]}, + {"t":0.54445, "x":5.98045, "y":7.02645, "heading":-1.56212, "vx":-3.59558, "vy":-0.60369, "omega":1.77287, "ax":-2.59951, "ay":-1.27215, "alpha":-5.81132, "fx":[-56.53724,-29.95119,-31.57766,-58.8013], "fy":[-35.21289,-37.76614,-6.94572,-6.63092]}, + {"t":0.56813, "x":5.89461, "y":7.01181, "heading":-1.52016, "vx":-3.65712, "vy":-0.6338, "omega":1.63531, "ax":-2.54552, "ay":-1.90232, "alpha":-6.47657, "fx":[-56.11811,-26.15258,-29.95486,-60.96857], "fy":[-47.41674,-49.81562,-16.0042,-16.19485]}, + {"t":0.5918, "x":5.80733, "y":6.99627, "heading":-1.48145, "vx":-3.71738, "vy":-0.67883, "omega":1.48199, "ax":-2.56229, "ay":-2.71216, "alpha":-7.56379, "fx":[-57.41239,-21.87209,-28.79302,-66.25759], "fy":[-63.14445,-66.00193,-27.54371,-27.84203]}, + {"t":0.61547, "x":5.71861, "y":6.97944, "heading":-1.44636, "vx":-3.77803, "vy":-0.74303, "omega":1.30294, "ax":-2.59785, "ay":-3.68327, "alpha":-8.94127, "fx":[-59.27119,-16.1158,-27.28974,-74.07828], "fy":[-81.4222,-85.34173,-42.21602,-41.62529]}, + {"t":0.63914, "x":5.62845, "y":6.96082, "heading":-1.41552, "vx":-3.83953, "vy":-0.83022, "omega":1.09129, "ax":-2.56382, "ay":-4.77386, "alpha":-10.31834, "fx":[-60.03108,-7.99583,-23.74199,-82.6708], "fy":[-100.60034,-105.72253,-60.96059,-57.52463]}, + {"t":0.66281, "x":5.53684, "y":6.93983, "heading":-1.38969, "vx":-3.90022, "vy":-0.94323, "omega":0.84703, "ax":-2.34698, "ay":-5.93212, "alpha":-11.28461, "fx":[-57.90599,2.58623,-15.47321,-88.89318], "fy":[-118.97622,-124.41014,-84.35479,-75.8734]}, + {"t":0.68649, "x":5.44386, "y":6.91584, "heading":-1.36964, "vx":-3.95577, "vy":-1.08366, "omega":0.5799, "ax":-1.93839, "ay":-7.03313, "alpha":-11.65616, "fx":[-52.89519,13.82782,-2.25121,-90.56705], "fy":[-134.80774,-139.13809,-108.965,-95.61541]}, + {"t":0.71016, "x":5.34968, "y":6.88822, "heading":-1.35591, "vx":-4.00166, "vy":-1.25014, "omega":0.30398, "ax":-1.60638, "ay":-7.8198, "alpha":-12.56622, "fx":[-50.24979,24.33089,12.10033,-95.47753], "fy":[-146.54051,-149.76611,-126.9864,-108.7572]}, + {"t":0.73383, "x":5.2545, "y":6.85643, "heading":-1.34871, "vx":-4.03969, "vy":-1.43525, "omega":0.00651, "ax":3.18631, "ay":-8.39195, "alpha":-0.08425, "fx":[53.83291,54.50404,54.56487,53.8912], "fy":[-142.92868,-142.68897,-142.56016,-142.80114]}, + {"t":0.75197, "x":5.18173, "y":6.82901, "heading":-1.3486, "vx":-3.98187, "vy":-1.58751, "omega":0.00498, "ax":3.73801, "ay":-8.80409, "alpha":0.01737, "fx":[63.66483,63.52147,63.50027,63.64351], "fy":[-149.71492,-149.7735,-149.79518,-149.73668]}, + {"t":0.77012, "x":5.1101, "y":6.79876, "heading":-1.34851, "vx":-3.91405, "vy":-1.74725, "omega":0.0053, "ax":4.21581, "ay":-8.86259, "alpha":0.05321, "fx":[71.97239,71.53462,71.4474,71.88415], "fy":[-150.61478,-150.81718,-150.88511,-150.68349]}, + {"t":0.78826, "x":5.03978, "y":6.7656, "heading":-1.34841, "vx":-3.83756, "vy":-1.90805, "omega":0.00626, "ax":4.74064, "ay":-8.74766, "alpha":0.34116, "fx":[82.36594,79.618,78.92638,81.63712], "fy":[-147.80844,-149.27005,-149.76396,-148.3386]}, + {"t":0.8064, "x":4.97093, "y":6.72954, "heading":-1.3483, "vx":-3.75155, "vy":-2.06677, "omega":0.01245, "ax":5.61845, "ay":-8.29875, "alpha":1.85, "fx":[105.05208,91.06832,86.45947,99.69296], "fy":[-134.37942,-143.98317,-147.34881,-138.92606]}, + {"t":0.82455, "x":4.90379, "y":6.69067, "heading":-1.34807, "vx":-3.64961, "vy":-2.21733, "omega":0.04602, "ax":6.8122, "ay":-7.33988, "alpha":4.53419, "fx":[137.33472,109.20217,94.8755,122.08185], "fy":[-102.90735,-131.53989,-143.39346,-121.5564]}, + {"t":0.84269, "x":4.83869, "y":6.64923, "heading":-1.34724, "vx":-3.52601, "vy":-2.35051, "omega":0.12828, "ax":7.71392, "ay":-6.31802, "alpha":6.48757, "fx":[157.2393,127.86862,102.98685,136.75172], "fy":[-70.92417,-114.22626,-138.59318,-106.12729]}, + {"t":0.86084, "x":4.77599, "y":6.60555, "heading":-1.34491, "vx":-3.38606, "vy":-2.46514, "omega":0.24599, "ax":8.34048, "ay":-5.4379, "alpha":7.30977, "fx":[166.10854,143.38953,112.14464,145.83411], "fy":[-48.83078,-95.03441,-131.93946,-94.18407]}, + {"t":0.87898, "x":4.71592, "y":6.55993, "heading":-1.34044, "vx":-3.23473, "vy":-2.5638, "omega":0.37862, "ax":8.88122, "ay":-4.62309, "alpha":6.78227, "fx":[169.47547,155.35801,126.53465,152.90023], "fy":[-37.51802,-75.47275,-118.71412,-82.84514]}, + {"t":0.89712, "x":4.6587, "y":6.51265, "heading":-1.33357, "vx":-3.07359, "vy":-2.64768, "omega":0.50167, "ax":9.32494, "ay":-3.87797, "alpha":5.43312, "fx":[170.75679,162.69817,142.17157,158.83163], "fy":[-32.83058,-59.80795,-99.93407,-71.27985]}, + {"t":0.91527, "x":4.60447, "y":6.46397, "heading":-1.32447, "vx":-2.9044, "vy":-2.71804, "omega":0.60025, "ax":9.62981, "ay":-3.25917, "alpha":4.05649, "fx":[171.41526,166.70102,153.75601,163.32892], "fy":[-30.49083,-49.09304,-81.48307,-60.68336]}, + {"t":0.93341, "x":4.55335, "y":6.41412, "heading":-1.31358, "vx":-2.72968, "vy":-2.77717, "omega":0.67385, "ax":9.82969, "ay":-2.76102, "alpha":2.88219, "fx":[171.83368,169.01514,161.36142,166.59041], "fy":[-29.06329,-41.68916,-65.64851,-51.45555]}, + {"t":0.95155, "x":4.50545, "y":6.36328, "heading":-1.30136, "vx":-2.55134, "vy":-2.82727, "omega":0.72614, "ax":9.96132, "ay":-2.36036, "alpha":1.92256, "fx":[172.12906,170.4698,166.22847,168.92981], "fy":[-28.0916,-36.31904,-52.62758,-43.55817]}, + {"t":0.9697, "x":4.46079, "y":6.31159, "heading":-1.28818, "vx":-2.3706, "vy":-2.87009, "omega":0.76102, "ax":10.04963, "ay":-2.03507, "alpha":1.14151, "fx":[172.34964,171.45194,169.35507,170.60872], "fy":[-27.39224,-32.23332,-42.00604,-36.83219]}, + {"t":0.98784, "x":4.41944, "y":6.25918, "heading":-1.27437, "vx":-2.18827, "vy":-2.90702, "omega":0.78174, "ax":10.11009, "ay":-1.76758, "alpha":0.50032, "fx":[172.52009,172.15356,171.38629,171.81915], "fy":[-26.87401,-28.99628,-33.29237,-31.10136]}, + {"t":1.00598, "x":4.3814, "y":6.20615, "heading":-1.26019, "vx":-2.00483, "vy":-2.93909, "omega":0.79081, "ax":10.15226, "ay":-1.5447, "alpha":-0.03254, "fx":[172.65488,172.67743,172.71915,172.69687], "fy":[-26.48403,-26.34516,-26.06595,-26.20452]}, + {"t":1.02413, "x":4.34669, "y":6.15257, "heading":-1.24584, "vx":-1.82064, "vy":-2.96711, "omega":0.79022, "ax":10.18213, "ay":-1.35666, "alpha":-0.48096, "fx":[172.76333,173.08242,173.59772,173.33693], "fy":[-26.18826,-24.11469,-19.99869,-22.00371]}, + {"t":1.04227, "x":4.31534, "y":6.09851, "heading":-1.2315, "vx":-1.6359, "vy":-2.99173, "omega":0.7815, "ax":10.26394, "ay":-0.65499, "alpha":-0.69593, "fx":[174.27188,174.51232,174.84905,174.71323], "fy":[-15.60862,-12.87935,-6.76079,-9.31614]}, + {"t":1.06918, "x":4.27504, "y":6.01779, "heading":-1.21048, "vx":-1.35976, "vy":-3.00935, "omega":0.76277, "ax":10.24808, "ay":0.80363, "alpha":-1.006, "fx":[174.748,174.62574,173.83176,174.06182], "fy":[7.52422,10.46553,19.58062,17.10797]}, + {"t":1.09608, "x":4.24217, "y":5.93712, "heading":-1.18996, "vx":-1.08405, "vy":-2.98773, "omega":0.73571, "ax":9.95366, "ay":2.54264, "alpha":-1.35752, "fx":[171.13475,170.71628,167.53965,167.84491], "fy":[35.8544,38.00238,50.16805,48.97346]}, + {"t":1.12298, "x":4.2166, "y":5.85766, "heading":-1.17016, "vx":-0.81626, "vy":-2.91932, "omega":0.69919, "ax":9.2496, "ay":4.45559, "alpha":-1.71684, "fx":[160.90522,160.96662,154.13358,153.32648], "fy":[68.30125,68.31352,82.58767,83.95075]}, + {"t":1.14989, "x":4.19799, "y":5.78073, "heading":-1.15135, "vx":-0.56741, "vy":-2.79945, "omega":0.653, "ax":8.08772, "ay":6.31581, "alpha":-2.02676, "fx":[142.33454,144.52705,133.59978,129.81779], "fy":[101.42978,98.41183,112.8242,117.05437]}, + {"t":1.17679, "x":4.18565, "y":5.7077, "heading":-1.13379, "vx":-0.34983, "vy":-2.62954, "omega":0.59847, "ax":6.58396, "ay":7.86777, "alpha":-2.23716, "fx":[116.48684,122.57156,108.48151,100.42496], "fy":[130.31231,124.71414,137.18236,143.10512]}, + {"t":1.20369, "x":4.17862, "y":5.6398, "heading":-1.11769, "vx":-0.17269, "vy":-2.41786, "omega":0.53828, "ax":4.97059, "ay":8.97521, "alpha":-2.34097, "fx":[87.35962,98.01277,82.50931,70.31169], "fy":[151.43502,144.85315,154.25418,160.12089]}, + {"t":1.2306, "x":4.17578, "y":5.578, "heading":-1.1032, "vx":-0.03897, "vy":-2.1764, "omega":0.4753, "ax":3.45565, "ay":9.66299, "alpha":-2.36565, "fx":[59.32217,73.90875,58.562,43.32581], "fy":[164.51372,158.56239,164.89349,169.489]}, + {"t":1.2575, "x":4.17598, "y":5.52294, "heading":-1.09042, "vx":0.054, "vy":-1.91643, "omega":0.41166, "ax":2.14466, "ay":10.03936, "alpha":-2.34332, "fx":[34.90501,52.20343,37.93299,20.8788], "fy":[171.42499,167.02132,170.87273,173.74787]}, + {"t":1.2844, "x":4.17821, "y":5.47502, "heading":-1.07934, "vx":0.1117, "vy":-1.64634, "omega":0.34861, "ax":1.05686, "ay":10.21515, "alpha":-2.29722, "fx":[14.73328,33.60339,20.76012,2.81065], "fy":[174.37768,171.7797,173.8418,175.0278]}, + {"t":1.31131, "x":4.1816, "y":5.43442, "heading":-1.06996, "vx":0.14013, "vy":-1.37151, "omega":0.28681, "ax":0.16909, "ay":10.27188, "alpha":-2.2415, "fx":[-1.57918,18.04331,6.64981,-11.60913], "fy":[175.04181,174.14511,174.98973,174.71021]}, + {"t":1.33821, "x":4.18543, "y":5.40124, "heading":-1.06225, "vx":0.14468, "vy":-1.09516, "omega":0.22651, "ax":-0.5539, "ay":10.26158, "alpha":-2.18397, "fx":[-14.72601,5.13288,-4.93129,-23.16259], "fy":[174.47156,175.0389,175.08032,173.59525]}, + {"t":1.36511, "x":4.18912, "y":5.37549, "heading":-1.05615, "vx":0.12978, "vy":-0.81909, "omega":0.16775, "ax":-1.1459, "ay":10.21531, "alpha":-2.12853, "fx":[-25.3835,-5.58808,-14.48508,-32.50891], "fy":[173.27922,175.05694,174.57882,172.12295]}, + {"t":1.39202, "x":4.1922, "y":5.35715, "heading":-1.05164, "vx":0.09895, "vy":-0.54426, "omega":0.11049, "ax":-1.63495, "ay":10.15105, "alpha":-2.07694, "fx":[-34.11042,-14.54137,-22.42962,-40.15879], "fy":[171.80634,174.56918,173.76196,170.52864]}, + {"t":1.41892, "x":4.19427, "y":5.34618, "heading":-1.04867, "vx":0.05497, "vy":-0.27116, "omega":0.05461, "ax":-2.04311, "ay":10.07906, "alpha":-2.02977, "fx":[-41.34028,-22.07844,-29.09609,-46.49592], "fy":[170.23884,173.801,172.7925,168.93514]}, + {"t":1.44583, "x":4.19501, "y":5.34254, "heading":-1.0472, "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/startDeepToL.traj b/src/main/deploy/choreo/startDeepToL.traj new file mode 100644 index 00000000..10ed5821 --- /dev/null +++ b/src/main/deploy/choreo/startDeepToL.traj @@ -0,0 +1,104 @@ +{ + "name":"startDeepToL", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.336880683898926, "y":6.839954376220703, "heading":-1.865030672844723, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.101142883300781, "y":5.999652862548828, "heading":-1.1659041662799314, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "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":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.336880683898926 m", "val":5.336880683898926}, "y":{"exp":"6.839954376220703 m", "val":6.839954376220703}, "heading":{"exp":"-1.865030672844723 rad", "val":-1.865030672844723}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.101142883300781 m", "val":4.101142883300781}, "y":{"exp":"5.999652862548828 m", "val":5.999652862548828}, "heading":{"exp":"-1.1659041662799314 rad", "val":-1.1659041662799314}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "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.66911,1.09796,1.51059], + "samples":[ + {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.59698, "ay":-1.14619, "alpha":27.82044, "fx":[-69.47717,-122.18867,-170.57535,-154.64832], "fy":[-160.7136,124.98887,40.23992,-82.50042]}, + {"t":0.02478, "x":7.09855, "y":7.25668, "heading":3.14159, "vx":-0.18827, "vy":-0.0284, "omega":0.68945, "ax":-7.80661, "ay":-1.24697, "alpha":26.47518, "fx":[-74.99925,-130.07267,-171.00054,-155.08014], "fy":[-158.17082,116.67948,38.29086,-81.64223]}, + {"t":0.04956, "x":7.09149, "y":7.25559, "heading":-3.12451, "vx":-0.38173, "vy":-0.05931, "omega":1.34555, "ax":-8.01373, "ay":-1.39884, "alpha":25.06947, "fx":[-80.59668,-137.94576,-171.72727,-154.97529], "fy":[-155.33586,107.18985,34.7573,-81.78701]}, + {"t":0.07435, "x":7.07957, "y":7.25369, "heading":-3.09116, "vx":-0.58033, "vy":-0.09397, "omega":1.96682, "ax":-8.22654, "ay":-1.58933, "alpha":23.52579, "fx":[-87.0842,-145.53228,-172.64621,-154.46182], "fy":[-151.71489,96.58478,29.67861,-82.68468]}, + {"t":0.09913, "x":7.06266, "y":7.25088, "heading":-3.04242, "vx":-0.7842, "vy":-0.13336, "omega":2.54984, "ax":-8.46063, "ay":-1.80449, "alpha":21.68972, "fx":[-95.52581,-152.79662,-173.61473,-153.71441], "fy":[-146.4378,84.57455,23.06643,-83.97862]}, + {"t":0.12391, "x":7.04063, "y":7.24702, "heading":-2.97923, "vx":-0.99387, "vy":-0.17808, "omega":3.08735, "ax":-8.73572, "ay":-2.02432, "alpha":19.33431, "fx":[-107.14827,-159.74871,-174.45511,-153.01649], "fy":[-138.01191,70.50382,14.90258,-85.12706]}, + {"t":0.14869, "x":7.01332, "y":7.24198, "heading":-2.90272, "vx":-1.21036, "vy":-0.22825, "omega":3.5665, "ax":-9.06522, "ay":-2.21792, "alpha":16.20435, "fx":[-122.74529,-166.22865,-174.94653,-152.86701], "fy":[-124.1455,53.34805,5.12373,-85.23078]}, + {"t":0.17347, "x":6.98054, "y":7.23564, "heading":-2.81433, "vx":-1.43501, "vy":-0.28321, "omega":3.96807, "ax":-9.43145, "ay":-2.35138, "alpha":12.15582, "fx":[-141.10641,-171.63703,-174.80007,-154.16147], "fy":[-102.55447,31.62677,-6.44378,-82.61388]}, + {"t":0.19826, "x":6.94208, "y":7.2279, "heading":-2.716, "vx":-1.66874, "vy":-0.34148, "omega":4.26932, "ax":-9.76129, "ay":-2.42534, "alpha":7.2671, "fx":[-157.73925,-174.41906,-173.56255,-158.42617], "fy":[-74.24463,3.21109,-20.32518,-73.65856]}, + {"t":0.22304, "x":6.89773, "y":7.2187, "heading":-2.61019, "vx":-1.91065, "vy":-0.40159, "omega":4.44941, "ax":-9.9405, "ay":-2.47823, "alpha":1.25928, "fx":[-168.04909,-170.86931,-170.14491,-167.27681], "fy":[-46.32684,-34.62353,-38.35241,-49.31288]}, + {"t":0.24782, "x":6.84733, "y":7.20798, "heading":-2.49993, "vx":-2.15699, "vy":-0.463, "omega":4.48062, "ax":-9.67218, "ay":-2.46894, "alpha":-7.9411, "fx":[-172.23636,-153.17038,-159.1224,-173.5546], "fy":[-27.10818,-83.02269,-69.12181,11.26877]}, + {"t":0.2726, "x":6.7909, "y":7.19575, "heading":-2.38889, "vx":-2.39669, "vy":-0.52419, "omega":4.28382, "ax":-8.03531, "ay":-2.89468, "alpha":-20.68597, "fx":[-173.06204,-121.90319,-100.52212,-151.2262], "fy":[-21.36318,-124.32222,-136.71036,85.44488]}, + {"t":0.29738, "x":6.72904, "y":7.18187, "heading":-2.28273, "vx":-2.59582, "vy":-0.59592, "omega":3.77118, "ax":-7.98471, "ay":-2.86633, "alpha":-20.2341, "fx":[-172.14467,-113.61948,-108.10361,-149.40284], "fy":[-26.98327,-131.60437,-124.85571,88.42154]}, + {"t":0.32217, "x":6.66226, "y":7.16622, "heading":-2.18927, "vx":-2.79369, "vy":-0.66696, "omega":3.26974, "ax":-8.05772, "ay":-2.51223, "alpha":-19.51942, "fx":[-170.87922,-104.2521,-124.36984,-148.7368], "fy":[-33.18119,-138.73515,-88.23776,89.22497]}, + {"t":0.34695, "x":6.59055, "y":7.14892, "heading":-2.10824, "vx":-2.99338, "vy":-0.72921, "omega":2.78601, "ax":-8.35988, "ay":-1.29962, "alpha":-19.10899, "fx":[-169.21757,-99.34506,-148.36273,-151.87159], "fy":[-39.64437,-141.72022,9.7752,83.1649]}, + {"t":0.37173, "x":6.5138, "y":7.13045, "heading":-2.0392, "vx":-3.20056, "vy":-0.76142, "omega":2.31245, "ax":-8.43258, "ay":-1.13451, "alpha":-19.04817, "fx":[-167.31977,-98.93031,-151.11059,-156.38225], "fy":[-45.41577,-141.16033,36.21738,73.16822]}, + {"t":0.39651, "x":6.4319, "y":7.11124, "heading":-1.98189, "vx":-3.40953, "vy":-0.78954, "omega":1.8404, "ax":-8.38399, "ay":-1.22903, "alpha":-19.12661, "fx":[-165.16596,-97.31507,-148.95931,-158.99698], "fy":[-50.42616,-140.95902,42.62483,65.13883]}, + {"t":0.42129, "x":6.34483, "y":7.09129, "heading":-1.93628, "vx":-3.6173, "vy":-0.81999, "omega":1.36641, "ax":-8.24036, "ay":-1.40332, "alpha":-19.19632, "fx":[-162.44403,-94.41293,-143.8638,-159.94393], "fy":[-54.72907,-140.53111,41.50857,58.27103]}, + {"t":0.44608, "x":6.25265, "y":7.07054, "heading":-1.90242, "vx":-3.82152, "vy":-0.85477, "omega":0.89068, "ax":-7.90498, "ay":-1.70117, "alpha":-18.92321, "fx":[-157.78554,-90.00294,-131.80805,-158.24949], "fy":[-58.15443,-137.56585,29.53281,50.44155]}, + {"t":0.47086, "x":6.15552, "y":7.04883, "heading":-1.88035, "vx":-4.01742, "vy":-0.89693, "omega":0.42173, "ax":-6.18299, "ay":-2.3984, "alpha":-15.55814, "fx":[-132.82425,-74.4432,-78.54897,-134.86748], "fy":[-58.74096,-111.03614,-16.0924,22.68507]}, + {"t":0.49564, "x":6.05406, "y":7.02587, "heading":-1.8699, "vx":-4.17064, "vy":-0.95637, "omega":0.03617, "ax":0.03375, "ay":-0.98753, "alpha":-0.45733, "fx":[-0.8304,1.31501,1.98088,-0.16893], "fy":[-17.52867,-18.17611,-16.06819,-15.41764]}, + {"t":0.52042, "x":5.95072, "y":7.00187, "heading":-1.869, "vx":-4.16981, "vy":-0.98084, "omega":0.02483, "ax":0.11087, "ay":-0.47691, "alpha":-0.00528, "fx":[1.86976,1.89438,1.90197,1.87735], "fy":[-8.12062,-8.12813,-8.1036,-8.09608]}, + {"t":0.5452, "x":5.84741, "y":6.97741, "heading":-1.86838, "vx":-4.16706, "vy":-0.99266, "omega":0.0247, "ax":0.12589, "ay":-0.52677, "alpha":-0.00174, "fx":[2.13609,2.1442,2.1467,2.13859], "fy":[-8.96296,-8.96542,-8.95734,-8.95487]}, + {"t":0.56999, "x":5.74418, "y":6.95265, "heading":-1.86777, "vx":-4.16394, "vy":-1.00571, "omega":0.02466, "ax":0.27955, "ay":-1.14363, "alpha":-0.00343, "fx":[4.7444,4.76055,4.7656,4.74945], "fy":[-19.45837,-19.46311,-19.44727,-19.44253]}, + {"t":0.59477, "x":5.64108, "y":6.92738, "heading":-1.86716, "vx":-4.15701, "vy":-1.03406, "omega":0.02457, "ax":0.72341, "ay":-2.80203, "alpha":-0.00062, "fx":[12.30287,12.30595,12.30701,12.30393], "fy":[-47.66282,-47.66353,-47.66079,-47.66007]}, + {"t":0.61955, "x":5.53828, "y":6.90089, "heading":-1.86655, "vx":-4.13908, "vy":-1.10349, "omega":0.02456, "ax":1.77185, "ay":-5.45068, "alpha":0.49502, "fx":[32.16871,29.40507,28.11753,30.86308], "fy":[-91.84043,-91.73063,-93.58977,-93.69746]}, + {"t":0.64433, "x":5.43625, "y":6.87187, "heading":-1.86594, "vx":-4.09517, "vy":-1.23857, "omega":0.03683, "ax":6.88677, "ay":-3.97417, "alpha":12.45183, "fx":[148.03672,129.8205,73.74712,116.96328], "fy":[-32.99933,-18.50706,-119.58754,-99.30381]}, + {"t":0.66911, "x":5.33688, "y":6.83995, "heading":-1.86503, "vx":-3.92451, "vy":-1.33706, "omega":0.34541, "ax":7.32463, "ay":-2.80149, "alpha":13.78679, "fx":[152.32346,132.96789,86.64327,126.42472], "fy":[-13.45203,16.40062,-106.54687,-87.01206]}, + {"t":0.68776, "x":5.26498, "y":6.81454, "heading":-1.85859, "vx":-3.78794, "vy":-1.3893, "omega":0.60247, "ax":7.03374, "ay":-1.61012, "alpha":13.88569, "fx":[145.61033,117.75228,88.52987,126.67542], "fy":[3.09498,39.31282,-81.41929,-70.53937]}, + {"t":0.7064, "x":5.19557, "y":6.78835, "heading":-1.84736, "vx":-3.65679, "vy":-1.41932, "omega":0.86138, "ax":6.47845, "ay":-0.3786, "alpha":13.63696, "fx":[135.38719,98.49702,83.41478,123.48701], "fy":[19.76855,57.67875,-51.36375,-51.84284]}, + {"t":0.72505, "x":5.12852, "y":6.76182, "heading":-1.8313, "vx":-3.53599, "vy":-1.42638, "omega":1.11564, "ax":5.67453, "ay":0.67769, "alpha":12.87124, "fx":[121.58479,78.55978,70.58495,115.3589], "fy":[33.39258,67.62618,-22.05846,-32.851]}, + {"t":0.7437, "x":5.06357, "y":6.73535, "heading":-1.81049, "vx":-3.43019, "vy":-1.41374, "omega":1.35564, "ax":4.71055, "ay":1.37058, "alpha":11.55865, "fx":[104.65773,60.34878,53.90525,101.58865], "fy":[41.65111,68.59778,-0.99657,-15.99948]}, + {"t":0.76234, "x":5.00043, "y":6.70922, "heading":-1.78522, "vx":-3.34236, "vy":-1.38819, "omega":1.57115, "ax":3.6799, "ay":1.5651, "alpha":9.71854, "fx":[85.1628,44.46984,38.13379,82.60985], "fy":[42.42887,60.77541,7.96513,-4.68179]}, + {"t":0.78099, "x":4.93875, "y":6.68361, "heading":-1.75592, "vx":-3.27374, "vy":-1.359, "omega":1.75236, "ax":2.68081, "ay":1.11392, "alpha":7.36005, "fx":[64.09888,31.4764,26.07912,60.74501], "fy":[32.27329,42.4528,4.23252,-3.16875]}, + {"t":0.79963, "x":4.87818, "y":6.65847, "heading":-1.72325, "vx":-3.22376, "vy":-1.33823, "omega":1.88959, "ax":1.88989, "ay":-0.24234, "alpha":4.51407, "fx":[44.30011,23.40996,19.9033,40.97235], "fy":[5.22991,9.00819,-13.99201,-16.7343]}, + {"t":0.81828, "x":4.8184, "y":6.63347, "heading":-1.68802, "vx":-3.18852, "vy":-1.34275, "omega":1.97376, "ax":1.63913, "ay":-2.63839, "alpha":1.57453, "fx":[32.61043,24.70189,23.21116,31.00088], "fy":[-41.33289,-41.0403,-48.47276,-48.66695]}, + {"t":0.83692, "x":4.75923, "y":6.60798, "heading":-1.65121, "vx":-3.15796, "vy":-1.39195, "omega":2.00312, "ax":2.17874, "ay":-5.1908, "alpha":-0.26053, "fx":[36.11228,37.61539,38.01122,36.50042], "fy":[-88.8991,-88.7115,-87.68829,-87.87696]}, + {"t":0.85557, "x":4.70073, "y":6.58112, "heading":-1.61387, "vx":-3.11733, "vy":-1.48873, "omega":1.99826, "ax":3.12801, "ay":-6.82555, "alpha":-0.72355, "fx":[50.0787,54.71831,56.37232,51.65659], "fy":[-117.92568,-116.63964,-114.24799,-115.58932]}, + {"t":0.87421, "x":4.64315, "y":6.55218, "heading":-1.57661, "vx":-3.05901, "vy":-1.616, "omega":1.98477, "ax":4.01654, "ay":-7.59625, "alpha":-0.6961, "fx":[65.00033,69.66957,71.67424,66.93636], "fy":[-131.19257,-129.30452,-127.18289,-129.15997]}, + {"t":0.89286, "x":4.58681, "y":6.52073, "heading":-1.5396, "vx":-2.98412, "vy":-1.75763, "omega":1.97179, "ax":4.7251, "ay":-7.88964, "alpha":-0.68492, "fx":[76.96208,81.59123,83.81212,79.12498], "fy":[-136.37896,-134.0326,-131.96673,-134.42408]}, + {"t":0.91151, "x":4.53199, "y":6.48658, "heading":-1.50283, "vx":-2.89602, "vy":-1.90474, "omega":1.95902, "ax":5.28262, "ay":-7.94977, "alpha":-0.80677, "fx":[85.78854,91.18614,93.95711,88.49182], "fy":[-138.02082,-134.83467,-132.33767,-135.69986]}, + {"t":0.93015, "x":4.47891, "y":6.44968, "heading":-1.46631, "vx":-2.79752, "vy":-2.05297, "omega":1.94398, "ax":5.73334, "ay":-7.89434, "alpha":-1.05541, "fx":[92.2242,99.15987,102.86904,95.83659], "fy":[-138.18936,-133.61145,-130.2085,-135.1126]}, + {"t":0.9488, "x":4.42775, "y":6.41003, "heading":-1.43006, "vx":-2.69062, "vy":-2.20016, "omega":1.9243, "ax":6.1096, "ay":-7.77844, "alpha":-1.4021, "fx":[96.9802,106.00002,110.93658,101.77343], "fy":[-137.77176,-131.27492,-126.543,-133.64629]}, + {"t":0.96744, "x":4.37864, "y":6.36766, "heading":-1.39418, "vx":-2.5767, "vy":-2.3452, "omega":1.89816, "ax":6.43229, "ay":-7.62883, "alpha":-1.82018, "fx":[100.5746,112.01939,118.3505,106.70127], "fy":[-137.1427,-128.28545,-121.85524,-131.77322]}, + {"t":0.98609, "x":4.33171, "y":6.3226, "heading":-1.35879, "vx":-2.45677, "vy":-2.48744, "omega":1.86422, "ax":6.71462, "ay":-7.45937, "alpha":-2.28884, "fx":[103.36177,117.41633,125.20361,110.87309], "fy":[-136.45734,-124.89101,-116.44315,-129.73523]}, + {"t":1.00473, "x":4.28707, "y":6.27493, "heading":-1.32403, "vx":-2.33157, "vy":-2.62652, "omega":1.82154, "ax":6.96503, "ay":-7.2778, "alpha":-2.79246, "fx":[105.58235,122.31729,131.5404,114.4524], "fy":[-135.77582,-121.23579,-110.49667,-127.66513]}, + {"t":1.02338, "x":4.24481, "y":6.22469, "heading":-1.29007, "vx":-2.20171, "vy":-2.76222, "omega":1.76947, "ax":7.18908, "ay":-7.08885, "alpha":-3.31934, "fx":[107.40091,126.80416,137.38249,117.54903], "fy":[-135.11734,-117.41101,-104.1485,-125.64037]}, + {"t":1.04202, "x":4.20501, "y":6.17196, "heading":-1.25707, "vx":-2.06766, "vy":-2.8944, "omega":1.70758, "ax":7.39123, "ay":-6.89459, "alpha":-3.86751, "fx":[108.91531,130.94987,142.78054,120.24513], "fy":[-134.49725,-113.45891,-97.44188,-123.70203]}, + {"t":1.06067, "x":4.16774, "y":6.11679, "heading":-1.22523, "vx":-1.92985, "vy":-3.02295, "omega":1.63547, "ax":7.60479, "ay":-6.64309, "alpha":-4.76867, "fx":[109.43197,135.66474,149.52683,122.79787], "fy":[-134.55361,-108.36827,-87.45173,-121.61477]}, + {"t":1.07932, "x":4.13308, "y":6.05927, "heading":-1.19474, "vx":-1.78805, "vy":-3.14682, "omega":1.54656, "ax":8.06947, "ay":-5.42331, "alpha":-10.83264, "fx":[97.70077,150.29478,172.01858,129.02314], "fy":[-143.77594,-87.97179,-23.39115,-113.85693]}, + {"t":1.09796, "x":4.10114, "y":5.99965, "heading":-1.1659, "vx":-1.63759, "vy":-3.24794, "omega":1.34458, "ax":9.28646, "ay":-3.26605, "alpha":-9.76006, "fx":[129.0325,163.84845,174.07082,164.88845], "fy":[-117.05485,-60.10432,7.93838,-52.99766]}, + {"t":1.12547, "x":4.05961, "y":5.90907, "heading":-1.12892, "vx":-1.38214, "vy":-3.33778, "omega":1.07609, "ax":9.74962, "ay":0.65126, "alpha":-10.80226, "fx":[162.2707,174.10401,162.20416,164.77412], "fy":[-62.39984,-10.16531,63.6715,53.20444]}, + {"t":1.15298, "x":4.02528, "y":5.8175, "heading":-1.09932, "vx":-1.11394, "vy":-3.31987, "omega":0.77894, "ax":8.79279, "ay":4.51999, "alpha":-9.55794, "fx":[171.43972,167.5875,136.85486,122.36948], "fy":[28.00713,48.16699,108.05914,123.30167]}, + {"t":1.18049, "x":3.99796, "y":5.72789, "heading":-1.07789, "vx":-0.87206, "vy":-3.19553, "omega":0.51602, "ax":7.06011, "ay":7.18744, "alpha":-6.63543, "fx":[139.15436,144.34455,109.61246,87.25074], "fy":[104.45102,97.98073,135.81897,150.77477]}, + {"t":1.20799, "x":3.97664, "y":5.6427, "heading":-1.06369, "vx":-0.67785, "vy":-2.99781, "omega":0.33349, "ax":5.43347, "ay":8.60705, "alpha":-4.55428, "fx":[101.81362,115.83158,86.77284,65.26879], "fy":[141.53097,130.64081,151.60135,161.84062]}, + {"t":1.2355, "x":3.96005, "y":5.56349, "heading":-1.05452, "vx":-0.52839, "vy":-2.76105, "omega":0.2082, "ax":4.2053, "ay":9.32675, "alpha":-3.17993, "fx":[75.25242,90.53383,69.09245,51.24524], "fy":[157.55452,149.44805,160.56118,167.01774]}, + {"t":1.26301, "x":3.94711, "y":5.49107, "heading":-1.04879, "vx":-0.4127, "vy":-2.50448, "omega":0.12073, "ax":3.31126, "ay":9.71002, "alpha":-2.21788, "fx":[57.44186,70.5139,55.57873,41.75975], "fy":[165.06711,159.99361,165.82224,169.77564]}, + {"t":1.29052, "x":3.93701, "y":5.42585, "heading":-1.04547, "vx":-0.32162, "vy":-2.23737, "omega":0.05972, "ax":2.65099, "ay":9.92714, "alpha":-1.51684, "fx":[45.13332,55.11507,45.13234,34.98986], "fy":[168.97214,166.02231,169.04013,171.39652]}, + {"t":1.31803, "x":3.92916, "y":5.36806, "heading":-1.04383, "vx":-0.24869, "vy":-1.96429, "omega":0.01799, "ax":2.15069, "ay":10.05706, "alpha":-0.98952, "fx":[36.26214,43.22037,36.90591,29.94231], "fy":[171.1839,169.5801,171.08411,172.4224]}, + {"t":1.34554, "x":3.92314, "y":5.31783, "heading":-1.04333, "vx":-0.18953, "vy":-1.68764, "omega":-0.00923, "ax":1.7616, "ay":10.13835, "alpha":-0.58166, "fx":[29.62065,33.8912,30.30011,26.04495], "fy":[172.52305,171.74443,172.42445,173.10979]}, + {"t":1.37305, "x":3.91859, "y":5.27524, "heading":-1.04359, "vx":-0.14107, "vy":-1.40875, "omega":-0.02523, "ax":1.45178, "ay":10.19105, "alpha":-0.25839, "fx":[24.48766,26.44202,24.89818,22.9496], "fy":[173.37688,173.09303,173.32606,173.59149]}, + {"t":1.40055, "x":3.91526, "y":5.24035, "heading":-1.04428, "vx":-0.10114, "vy":-1.12841, "omega":-0.03234, "ax":1.19999, "ay":10.22616, "alpha":0.00332, "fx":[20.41475,20.38912,20.40832,20.43395], "fy":[173.94375,173.94672,173.94441,173.94144]}, + {"t":1.42806, "x":3.91293, "y":5.21317, "heading":-1.04517, "vx":-0.06813, "vy":-0.84711, "omega":-0.03224, "ax":0.99173, "ay":10.25003, "alpha":0.21909, "fx":[17.11128,15.39131,16.62312,18.35017], "fy":[174.33239,174.49047,174.37432,174.20329]}, + {"t":1.45557, "x":3.91143, "y":5.19375, "heading":-1.04606, "vx":-0.04084, "vy":-0.56514, "omega":-0.02622, "ax":0.81682, "ay":10.26649, "alpha":0.39982, "fx":[14.38168,11.20499,13.39248,16.59645], "fy":[174.60577,174.83487,174.6756,174.40406]}, + {"t":1.48308, "x":3.91062, "y":5.18209, "heading":-1.04678, "vx":-0.01838, "vy":-0.28273, "omega":-0.01522, "ax":0.66799, "ay":10.27793, "alpha":0.55325, "fx":[12.08992,7.65329,10.60582,15.10058], "fy":[174.80211,175.04814,174.88717,174.5612]}, + {"t":1.51059, "x":3.91036, "y":5.1782, "heading":-1.0472, "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/startGToG.traj b/src/main/deploy/choreo/startGToG.traj new file mode 100644 index 00000000..d134f654 --- /dev/null +++ b/src/main/deploy/choreo/startGToG.traj @@ -0,0 +1,50 @@ +{ + "name":"startGToG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":3.6225774894, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7769125, "y":3.6225774894, "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":"startG.x", "val":7.1008875}, "y":{"exp":"startG.y", "val":3.6225774894}, "heading":{"exp":"startG.heading", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.71864], + "samples":[ + {"t":0.0, "x":7.10089, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.30717, "ay":0.0, "alpha":0.0, "fx":[-175.32205,-175.32205,-175.32205,-175.32205], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.04791, "x":7.08906, "y":3.62258, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":-10.30564, "ay":0.0, "alpha":0.0, "fx":[-175.29602,-175.29602,-175.29602,-175.29602], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.09582, "x":7.05357, "y":3.62258, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":-10.3035, "ay":0.0, "alpha":0.0, "fx":[-175.2596,-175.2596,-175.2596,-175.2596], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.14373, "x":6.99444, "y":3.62258, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":-10.30029, "ay":0.0, "alpha":0.0, "fx":[-175.20495,-175.20495,-175.20495,-175.20495], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.19164, "x":6.91165, "y":3.62258, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":-10.29493, "ay":0.0, "alpha":0.0, "fx":[-175.11389,-175.11389,-175.11389,-175.11389], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.23955, "x":6.80523, "y":3.62258, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":-10.28423, "ay":0.0, "alpha":0.0, "fx":[-174.93185,-174.93185,-174.93185,-174.93185], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.28746, "x":6.67519, "y":3.62258, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":-10.25218, "ay":0.0, "alpha":0.0, "fx":[-174.3866,-174.3866,-174.3866,-174.3866], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.33537, "x":6.52159, "y":3.62258, "heading":3.14159, "vx":-3.45178, "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.38328, "x":6.35621, "y":3.62258, "heading":3.14159, "vx":-3.45178, "vy":0.0, "omega":0.0, "ax":10.25218, "ay":0.0, "alpha":0.0, "fx":[174.3866,174.3866,174.3866,174.3866], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.43118, "x":6.20261, "y":3.62258, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":10.28423, "ay":0.0, "alpha":0.0, "fx":[174.93185,174.93185,174.93185,174.93185], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.47909, "x":6.07257, "y":3.62258, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":10.29493, "ay":0.0, "alpha":0.0, "fx":[175.11389,175.11389,175.11389,175.11389], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.527, "x":5.96615, "y":3.62258, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":10.30029, "ay":0.0, "alpha":0.0, "fx":[175.20495,175.20495,175.20495,175.20495], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.57491, "x":5.88336, "y":3.62258, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":10.3035, "ay":0.0, "alpha":0.0, "fx":[175.2596,175.2596,175.2596,175.2596], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.62282, "x":5.82423, "y":3.62258, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":10.30564, "ay":0.0, "alpha":0.0, "fx":[175.29602,175.29602,175.29602,175.29602], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.67073, "x":5.78874, "y":3.62258, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":10.30717, "ay":0.0, "alpha":0.0, "fx":[175.32205,175.32205,175.32205,175.32205], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.71864, "x":5.77691, "y":3.62258, "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/startHToH.traj b/src/main/deploy/choreo/startHToH.traj new file mode 100644 index 00000000..265ae220 --- /dev/null +++ b/src/main/deploy/choreo/startHToH.traj @@ -0,0 +1,50 @@ +{ + "name":"startHToH", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.1008875, "y":3.9512534894, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7769125, "y":3.9512534894, "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":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":3.9512534894}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.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,0.71864], + "samples":[ + {"t":0.0, "x":7.10089, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.30717, "ay":0.0, "alpha":0.0, "fx":[-175.32205,-175.32205,-175.32205,-175.32205], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.04791, "x":7.08906, "y":3.95125, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":-10.30564, "ay":0.0, "alpha":0.0, "fx":[-175.29602,-175.29602,-175.29602,-175.29602], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.09582, "x":7.05357, "y":3.95125, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":-10.3035, "ay":0.0, "alpha":0.0, "fx":[-175.2596,-175.2596,-175.2596,-175.2596], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.14373, "x":6.99444, "y":3.95125, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":-10.30029, "ay":0.0, "alpha":0.0, "fx":[-175.20495,-175.20495,-175.20495,-175.20495], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.19164, "x":6.91165, "y":3.95125, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":-10.29493, "ay":0.0, "alpha":0.0, "fx":[-175.11389,-175.11389,-175.11389,-175.11389], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.23955, "x":6.80523, "y":3.95125, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":-10.28423, "ay":0.0, "alpha":0.0, "fx":[-174.93185,-174.93185,-174.93185,-174.93185], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.28746, "x":6.67519, "y":3.95125, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":-10.25218, "ay":0.0, "alpha":0.0, "fx":[-174.3866,-174.3866,-174.3866,-174.3866], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.33537, "x":6.52159, "y":3.95125, "heading":3.14159, "vx":-3.45178, "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.38328, "x":6.35621, "y":3.95125, "heading":3.14159, "vx":-3.45178, "vy":0.0, "omega":0.0, "ax":10.25218, "ay":0.0, "alpha":0.0, "fx":[174.3866,174.3866,174.3866,174.3866], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.43118, "x":6.20261, "y":3.95125, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":10.28423, "ay":0.0, "alpha":0.0, "fx":[174.93185,174.93185,174.93185,174.93185], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.47909, "x":6.07257, "y":3.95125, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":10.29493, "ay":0.0, "alpha":0.0, "fx":[175.11389,175.11389,175.11389,175.11389], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.527, "x":5.96615, "y":3.95125, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":10.30029, "ay":0.0, "alpha":0.0, "fx":[175.20495,175.20495,175.20495,175.20495], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.57491, "x":5.88336, "y":3.95125, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":10.3035, "ay":0.0, "alpha":0.0, "fx":[175.2596,175.2596,175.2596,175.2596], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.62282, "x":5.82423, "y":3.95125, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":10.30564, "ay":0.0, "alpha":0.0, "fx":[175.29602,175.29602,175.29602,175.29602], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.67073, "x":5.78874, "y":3.95125, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":10.30717, "ay":0.0, "alpha":0.0, "fx":[175.32205,175.32205,175.32205,175.32205], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.71864, "x":5.77691, "y":3.95125, "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":[] +} From 0f3cc2a18fbabeae0ffbcb08a5e39a180cf80fa1 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 4 Feb 2025 20:49:01 -0500 Subject: [PATCH 14/48] added climbing states. --- .../frc/robot/constants/LEDConstants.java | 12 ++++++++++++ .../robot/subsystems/led/LEDSubsystem.java | 19 ++++++++++++++++--- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 166ac277..5ecd9436 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -11,6 +11,7 @@ public class LEDConstants { public static final int kGetAlgeaStart = kStripLength / 9 * 8; public static final Color kAlmostBlack = new Color(0, 0, 1); + // Normal LED Colors public static final Color kHasAlgea = Color.kAquamarine; public static final Color kNotHasAlgea = kAlmostBlack; @@ -34,5 +35,16 @@ public class LEDConstants { public static final Color kCurrentLimiting = Color.kRed; public static final Color kAutoPlacing = Color.kPurple; + // Climb LED Colors + public static final Color kWaitingForCage = Color.kRed; + public static final Color kHasCage = Color.kGreen; + public static final Color[] gameColors = { + Color.kDarkRed, + Color.kDarkGreen, + Color.kDarkBlue, + Color.kPink, + Color.kLightGreen, + Color.kLightBlue + }; } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 879a30dd..44414651 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.led; +import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.util.Color; import frc.robot.constants.LEDConstants; import java.util.Map; @@ -53,7 +55,16 @@ public void setState(LEDStates state) { case NORMAL: buildBase(); break; - case CLIMB: + case CLIMB_EMPTY: + io.setStrip(LEDConstants.kWaitingForCage); + break; + case CLIMB_FULL: + io.setStrip(LEDConstants.kHasCage); + break; + case CLIMB_UP: + base = + LEDPattern.gradient(GradientType.kContinuous, Color.kLightGoldenrodYellow, Color.kBlack) + .scrollAtRelativeSpeed(Percent.per(Seconds).of(1)); break; default: break; @@ -192,7 +203,7 @@ public void periodic() { } io.setStrip(base); break; - case CLIMB: + case CLIMB_EMPTY: break; } io.updateLEDs(); @@ -234,7 +245,9 @@ public Set getMeasures() { public enum LEDStates { OFF, NORMAL, - CLIMB + CLIMB_EMPTY, + CLIMB_FULL, + CLIMB_UP } public enum CoralStates { From 4af74e27a25f79310911673d84dc9767b86917cc Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 10 Feb 2025 16:54:07 -0500 Subject: [PATCH 15/48] fixed two constants --- src/main/java/frc/robot/constants/DriveConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a8379ca9..3923eb8d 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -21,8 +21,8 @@ public class DriveConstants { public static final double kGyroDifferentThreshold = 5.0; // 5 degrees public static final int kGyroDifferentCount = 3; - public static final double kRobotLength = 22.0; - public static final double kRobotWidth = 22.0; + public static final double kRobotLength = 0.4556125; + public static final double kRobotWidth = 0.4556125; public static final double kFieldMaxX = 17.548225; public static final double kFieldMaxY = 8.0518; From eb0c5fcd2a6e82752d07396601af14bc209ac9b7 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 10 Feb 2025 18:15:33 -0500 Subject: [PATCH 16/48] added the game in LEDSubsystem --- .../frc/robot/constants/LEDConstants.java | 3 +- .../robot/subsystems/led/LEDSubsystem.java | 68 ++++++++++++++++++- 2 files changed, 67 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 5ecd9436..482bc9fe 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -39,7 +39,8 @@ public class LEDConstants { // Climb LED Colors public static final Color kWaitingForCage = Color.kRed; public static final Color kHasCage = Color.kGreen; - public static final Color[] gameColors = { + public static final Color[] kGameColors = { + Color.kBlack, // a dummy color Color.kDarkRed, Color.kDarkGreen, Color.kDarkBlue, diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 44414651..f1b54624 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -42,6 +42,10 @@ public class LEDSubsystem extends MeasurableSubsystem { private boolean autoPlacing = false; private boolean isLimiting = false; + // game stuff + private int[] BottomUnits = new int[LEDConstants.kStripLength]; + private int[] TopUnits = new int[LEDConstants.kStripLength]; + public LEDSubsystem(LEDIO io) { this.io = io; } @@ -62,9 +66,10 @@ public void setState(LEDStates state) { io.setStrip(LEDConstants.kHasCage); break; case CLIMB_UP: - base = - LEDPattern.gradient(GradientType.kContinuous, Color.kLightGoldenrodYellow, Color.kBlack) - .scrollAtRelativeSpeed(Percent.per(Seconds).of(1)); + // base = + // LEDPattern.gradient(GradientType.kContinuous, Color.kLightGoldenrodYellow, Color.kBlack) + // .scrollAtRelativeSpeed(Percent.per(Seconds).of(1)); + io.setOff(); break; default: break; @@ -189,6 +194,58 @@ private void buildBase() { base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(algea.overlayOn(coral)))); } + //game stuff + private void advanceUnits() { + for (int i = 0; i <= LEDConstants.kStripLength-1; i++) { + BottomUnits[i + 1] = BottomUnits[i]; + TopUnits[i] = TopUnits[i + 1]; + if (i == 0) + BottomUnits[i] = 0; + if (i == LEDConstants.kStripLength) + TopUnits[i] = 0; + if (TopUnits[i] != 0 && BottomUnits[i] != 0) { + if (BottomUnits[i] == TopUnits[i]) { + BottomUnits[i] = 0; + TopUnits[i] = 0; + } else if (BottomUnits[i] > TopUnits[i] || (BottomUnits[i] == 1 && TopUnits[i] == 3)) { + TopUnits[i] = 0; + } else { + BottomUnits[i] = 0; + } + } + if (TopUnits[i + 1] != 0 && BottomUnits[i] != 0) { + if (BottomUnits[i] == TopUnits[i + 1]) { + BottomUnits[i] = 0; + TopUnits[i + 1] = 0; + } else if (BottomUnits[i] > TopUnits[i + 1] || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) { + TopUnits[i + 1] = 0; + } else { + BottomUnits[i + 1] = 0; + } + } + } + } + + private void displayUnits() { + for (int i = 0; i <= LEDConstants.kStripLength-1; i++) { + io.setLED(i, LEDConstants.kGameColors[BottomUnits[i]]); + if (TopUnits[i] != 0){ + io.setLED(i, LEDConstants.kGameColors[TopUnits[i] + 3]); + } + } + } + + public void addGameUnit(boolean isBottom, int unitNum) { + // the unitNum starts at 1 and ends at 3. if you use anything else, it will be ignored + if (unitNum >= 1 && unitNum <= 3) { + if (isBottom){ + BottomUnits[0] = unitNum; + } else { + TopUnits[LEDConstants.kStripLength - 1] = unitNum; + } + } + } + public void periodic() { switch (currState) { case OFF: @@ -205,6 +262,11 @@ public void periodic() { break; case CLIMB_EMPTY: break; + case CLIMB_FULL: + break; + case CLIMB_UP: + advanceUnits(); + displayUnits(); } io.updateLEDs(); } From daec791f89103c0bddbcd3496f1f79f0e3fccf0a Mon Sep 17 00:00:00 2001 From: BB7209 Date: Thu, 20 Feb 2025 19:33:40 -0700 Subject: [PATCH 17/48] changed some constants --- src/main/java/frc/robot/constants/LEDConstants.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 482bc9fe..f083fd3d 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -3,7 +3,9 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDConstants { - public static final int kStripLength = 42; + // Auto/Operator = 27 + // other = 21.5 + public static final int kStripLength = 70; public static final int kAlgeaEnd = kStripLength / 3; public static final int kLevelStart = kStripLength / 3; From c6192e15014ba8c87d18f7876a6f8d1d69580938 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Thu, 20 Feb 2025 20:55:23 -0500 Subject: [PATCH 18/48] made all the nessecary changes --- .../frc/robot/constants/LEDConstants.java | 15 +++-- .../java/frc/robot/subsystems/led/LEDIO.java | 17 +++++ .../robot/subsystems/led/LEDSubsystem.java | 63 ++++++++----------- 3 files changed, 53 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index f083fd3d..6e193f21 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -5,12 +5,16 @@ public class LEDConstants { // Auto/Operator = 27 // other = 21.5 - public static final int kStripLength = 70; + public static final int kTotalStripLength = 70; + public static final int kBottomStripLength = 43; + public static final int kTopStripLength = 27; + public static final int kTopFirstIndex = kBottomStripLength + 1; - public static final int kAlgeaEnd = kStripLength / 3; - public static final int kLevelStart = kStripLength / 3; - public static final int kPlaceStart = kStripLength / 9 * 7; - public static final int kGetAlgeaStart = kStripLength / 9 * 8; + public static final int kAlgeaEnd = kBottomStripLength / 3 + 1; + public static final int kLevelStart = kTopFirstIndex; + public static final int kPlaceStart = kTopFirstIndex + kTopStripLength / 3; + public static final int kGetAlgeaStart = kTopFirstIndex + kTopStripLength / 3 * 2; + public static final int kAutoPlacingStart = kTopFirstIndex; public static final Color kAlmostBlack = new Color(0, 0, 1); @@ -41,6 +45,7 @@ public class LEDConstants { // Climb LED Colors public static final Color kWaitingForCage = Color.kRed; public static final Color kHasCage = Color.kGreen; + public static final Color kClimbed = Color.kGoldenrod; public static final Color[] kGameColors = { Color.kBlack, // a dummy color Color.kDarkRed, diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index 70efe5a4..5323b66c 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -2,18 +2,22 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.AddressableLEDBufferView; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; +import frc.robot.constants.LEDConstants; public class LEDIO { private AddressableLED ledBase; private AddressableLEDBuffer led; + private AddressableLEDBufferView ledTop; public LEDIO(int port, int length) { ledBase = new AddressableLED(port); ledBase.setLength(length); ledBase.start(); led = new AddressableLEDBuffer(length); + ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1); ledBase.setData(led); } @@ -28,6 +32,7 @@ public void setPort(int port) { public void setLength(int length) { ledBase.setLength(length); led = new AddressableLEDBuffer(length); + ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1); ledBase.setData(led); } @@ -47,6 +52,18 @@ public Color getLED(int index) { return led.getLED(index); } + public void setLEDTop(int index, Color color) { + ledTop.setLED(index, color); + } + + public void setLEDTop(int index, int r, int g, int b) { + ledTop.setRGB(index, r, g, b); + } + + public Color getLEDTop(int index) { + return ledTop.getLED(index); + } + public void setOff() { for (int i = 0; i < led.getLength(); i++) { setLED(i, Color.kBlack); diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index f1b54624..e0e80c8b 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -7,6 +7,9 @@ import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.util.Color; import frc.robot.constants.LEDConstants; +import frc.robot.subsystems.robotState.RobotStateSubsystem.CoralLoc; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; + import java.util.Map; import java.util.Set; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; @@ -20,7 +23,7 @@ public class LEDSubsystem extends MeasurableSubsystem { // section patterns private LEDPattern algea = LEDPattern.steps( - Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kStripLength / 3, Color.kBlack)); + Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack)); private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); private LEDPattern level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); private LEDPattern place = @@ -28,23 +31,23 @@ public class LEDSubsystem extends MeasurableSubsystem { private LEDPattern getAlgea = LEDPattern.steps(Map.of(LEDConstants.kGetAlgeaStart, LEDConstants.kNotGetAlgea)); private LEDPattern autoplace = - LEDPattern.steps(Map.of(LEDConstants.kStripLength / 3 * 2, LEDConstants.kAutoPlacing)) + LEDPattern.steps(Map.of(LEDConstants.kAutoPlacingStart, LEDConstants.kAutoPlacing)) .blink(Seconds.of(0.25)); private LEDPattern currentLimiting = LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); // section booleans and states private boolean hasAlgae = false; - private CoralStates coralState = CoralStates.NO_PIECE; - private LevelStates levelState = LevelStates.L1; + private CoralLoc coralState = CoralLoc.NONE; + private ScoringLevel levelState = ScoringLevel.L1; private PlaceStates placeState = PlaceStates.MANUAL; private boolean shouldGetAlgea = false; private boolean autoPlacing = false; private boolean isLimiting = false; // game stuff - private int[] BottomUnits = new int[LEDConstants.kStripLength]; - private int[] TopUnits = new int[LEDConstants.kStripLength]; + private int[] BottomUnits = new int[LEDConstants.kTopStripLength]; + private int[] TopUnits = new int[LEDConstants.kTopStripLength]; public LEDSubsystem(LEDIO io) { this.io = io; @@ -66,10 +69,7 @@ public void setState(LEDStates state) { io.setStrip(LEDConstants.kHasCage); break; case CLIMB_UP: - // base = - // LEDPattern.gradient(GradientType.kContinuous, Color.kLightGoldenrodYellow, Color.kBlack) - // .scrollAtRelativeSpeed(Percent.per(Seconds).of(1)); - io.setOff(); + io.setStrip(LEDConstants.kClimbed); break; default: break; @@ -86,12 +86,12 @@ public void setAlgeaLights(boolean on) { buildBase(); } - public void setCoralLights(CoralStates state) { + public void setCoralLights(CoralLoc state) { coralState = state; buildBase(); } - public void setLevelLights(LevelStates state) { + public void setLevelLights(ScoringLevel state) { levelState = state; buildBase(); } @@ -121,11 +121,11 @@ public boolean getAlgeaLights() { return hasAlgae; } - public CoralStates getCoralLights() { + public CoralLoc getCoralLights() { return coralState; } - public LevelStates getLevelLights() { + public ScoringLevel getLevelLights() { return levelState; } @@ -151,16 +151,18 @@ private void buildBase() { Map.of( 0, hasAlgae ? LEDConstants.kHasAlgea : LEDConstants.kNotHasAlgea, - LEDConstants.kStripLength / 3, + LEDConstants.kAlgeaEnd, Color.kBlack)); switch (coralState) { - case IN_FUNNEL: + case FUNNEL: + case TRANSFER: coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); break; - case IN_ROBOT: + case CORAL: + case SCORING: coral = LEDPattern.solid(LEDConstants.kCoralInRobot); break; - case NO_PIECE: + case NONE: coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); break; } @@ -196,12 +198,12 @@ private void buildBase() { //game stuff private void advanceUnits() { - for (int i = 0; i <= LEDConstants.kStripLength-1; i++) { + for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) { BottomUnits[i + 1] = BottomUnits[i]; TopUnits[i] = TopUnits[i + 1]; if (i == 0) BottomUnits[i] = 0; - if (i == LEDConstants.kStripLength) + if (i == LEDConstants.kTopStripLength) TopUnits[i] = 0; if (TopUnits[i] != 0 && BottomUnits[i] != 0) { if (BottomUnits[i] == TopUnits[i]) { @@ -227,10 +229,10 @@ private void advanceUnits() { } private void displayUnits() { - for (int i = 0; i <= LEDConstants.kStripLength-1; i++) { - io.setLED(i, LEDConstants.kGameColors[BottomUnits[i]]); + for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) { + io.setLEDTop(i, LEDConstants.kGameColors[BottomUnits[i]]); if (TopUnits[i] != 0){ - io.setLED(i, LEDConstants.kGameColors[TopUnits[i] + 3]); + io.setLEDTop(i, LEDConstants.kGameColors[TopUnits[i] + 3]); } } } @@ -241,7 +243,7 @@ public void addGameUnit(boolean isBottom, int unitNum) { if (isBottom){ BottomUnits[0] = unitNum; } else { - TopUnits[LEDConstants.kStripLength - 1] = unitNum; + TopUnits[LEDConstants.kTopStripLength - 1] = unitNum; } } } @@ -312,19 +314,6 @@ public enum LEDStates { CLIMB_UP } - public enum CoralStates { - IN_FUNNEL, - IN_ROBOT, - NO_PIECE - } - - public enum LevelStates { - L1, - L2, - L3, - L4 - } - public enum PlaceStates { MANUAL, LEFT, From 88599f9ce622a17fb614ebae17f3dbda42e9907e Mon Sep 17 00:00:00 2001 From: David Shen Date: Fri, 21 Feb 2025 21:14:53 -0500 Subject: [PATCH 19/48] edge cases --- src/main/java/frc/robot/RobotContainer.java | 34 +++-------------- .../robotState/AutoReefCycleCommand.java | 2 +- .../commands/tagAlign/TagAlignCommand.java | 2 +- .../robot/constants/RobotStateConstants.java | 3 +- .../robot/constants/TagServoingConstants.java | 12 +++--- .../robot/subsystems/biscuit/BiscuitIOFX.java | 2 +- .../subsystems/pathHandler/PathHandler.java | 3 +- .../robotState/RobotStateSubsystem.java | 37 +++++++++---------- .../tagAlign/TagAlignSubsystem.java | 29 +++++++++++---- 9 files changed, 59 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16769b67..9d22bc72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc.robot.commands.robotState.InterruptAutoCommand; import frc.robot.commands.robotState.ReefCycleCommand; import frc.robot.commands.robotState.ScoreAlgaeCommand; +import frc.robot.commands.robotState.SetScoreSideCommand; import frc.robot.commands.robotState.SetScoreSideRightCommand; import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; @@ -66,6 +67,7 @@ import frc.robot.subsystems.led.LEDIO; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -259,34 +261,10 @@ private void configureOperatorBindings() { .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4)); // Set scoring side - // new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) - // .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT)); - // new JoystickButton(xboxController, XboxController.Button.kRightStick.value) - // .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT)); - - // Move biscuit - new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - - // Move elevator - new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); - new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) + .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT)); + new JoystickButton(xboxController, XboxController.Button.kRightStick.value) + .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT)); // Stow new JoystickButton(xboxController, XboxController.Button.kBack.value) diff --git a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java index 3cc2fe40..b42837bc 100644 --- a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java @@ -39,8 +39,8 @@ public void initialize() { @Override public void end(boolean interrupted) { + tagAlignSubsystem.terminate(); - driveSubsystem.setIgnoreSticks(false); } @Override diff --git a/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java b/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java index 354426c1..8acae4b1 100644 --- a/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java +++ b/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java @@ -16,7 +16,7 @@ public TagAlignCommand(TagAlignSubsystem tagAlignSubsystem, DriveSubsystem drive @Override public void initialize() { - tagAlignSubsystem.start(Alliance.Blue, true); + tagAlignSubsystem.start(Alliance.Blue, true, false); } @Override diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 1ddc8768..cd9acb71 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -7,5 +7,6 @@ public class RobotStateConstants { public static final double kBlueBargeSafeX = 7.6; public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; - public static final double kCoralEjectTimer = 0.5; + public static final double kCoralEjectTimer = 0.75; + public static final double kAlgaeEjectTimer = 0.5; } diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 2a56807e..acd41b1c 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -32,15 +32,17 @@ public class TagServoingConstants { // Tag align public static final double kHorizontalCloseEnough = 20; - public static final double kAngleCloseEnough = Units.degreesToRadians(3.0); + public static final double kAngleCloseEnough = Units.degreesToRadians(1.0); public static final double kDiagCloseEnough = 20; public static final double kNoUpdateMicrosec = 500_000; // Drive - public static final double kInitialDriveRadius = 1.7; // 1.5 - public static final double kStopXDriveRadius = - kInitialDriveRadius; // Should be closer to reef than target pose - public static final double kMinStopXDriveRadius = 1.55; // 1.35 + public static final double kCoralInitialDriveRadius = 1.7; // 1.5 + public static final double kCoralStopXDriveRadius = + kCoralInitialDriveRadius; // Should be closer to reef than target pose + public static final double kAlgaeInitialDriveRadius = 2.0; // 1.5 + public static final double kAlgaeStopXDriveRadius = + kAlgaeInitialDriveRadius; // Should be closer to reef than target pose public static final double kDriveCloseEnough = 0.1; public static final double kMinVelX = 0.85; diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index c8acc2dd..7891e05d 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -85,7 +85,7 @@ public void zero() { double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1); double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos); talon.setPosition(setPos); - logger.info("set Biscuit position to " + setPos); + logger.info("set Biscuit position to " + setPos + " abs pos: " + pos); didZero = true; } } diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 338954fa..6f6573bd 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -247,7 +247,8 @@ public void periodic() { if (runningPath && isServoing) { tagAlignSubsystem.setup( robotStateSubsystem.getAllianceColor(), - robotStateSubsystem.getScoreSide() == RobotStateSubsystem.ScoreSide.LEFT); + robotStateSubsystem.getScoreSide() == RobotStateSubsystem.ScoreSide.LEFT, + false); drivePathServo(); } } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index d0313e81..04a45386 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -296,11 +296,13 @@ public void toReefAlign() { } private void toReefAlign(boolean getAlgae, boolean drive) { + boolean wantAlgae = getAlgae || !coralSubsystem.hasCoral(); if (drive) { - tagAlignSubsystem.start(allianceColor, scoreSide == ScoreSide.LEFT); + tagAlignSubsystem.start( + allianceColor, scoreSide == ScoreSide.LEFT || !coralSubsystem.hasCoral(), wantAlgae); setState(RobotStates.REEF_ALIGN); } - if ((getAlgae || !coralSubsystem.hasCoral()) + if (wantAlgae && (scoreSide == ScoreSide.LEFT || !isAutoPlacing) && !algaeSubsystem.hasAlgae()) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { @@ -575,23 +577,20 @@ public void periodic() { } } case REEF_ALIGN_ALGAE -> { - if (!isAutoPlacing - || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { - if (algaeSubsystem.hasAlgae()) { - switch (getAlgaeLevel()) { - case L2 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint); - } - case L3 -> { - biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint); - } - default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); + if (algaeSubsystem.hasAlgae()) { + switch (getAlgaeLevel()) { + case L2 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint); } - - setState(RobotStates.REMOVE_ALGAE); + case L3 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint); + } + default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } + + setState(RobotStates.REMOVE_ALGAE); } } case REEF_ALIGN_CORAL -> { @@ -651,7 +650,7 @@ public void periodic() { } case PROCESSOR_ALGAE -> { if (!algaeSubsystem.hasAlgae() - && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) && isEjectingAlgae) { isEjectingAlgae = false; toStowSafe(); @@ -680,7 +679,7 @@ public void periodic() { } case BARGE_ALGAE -> { if (!algaeSubsystem.hasAlgae() - && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) && isEjectingAlgae) { isEjectingAlgae = false; toStowSafe(); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index fd66a6de..c3e4589a 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -41,6 +41,9 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private int fieldRelHexant; private Alliance alliance = Alliance.Blue; private double goalTargetDiag; + private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius; + private double driveRadius = TagServoingConstants.kCoralInitialDriveRadius; + private boolean algae = false; private long startServoTime; @@ -50,7 +53,7 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu this.driveX = new ProfiledPIDController(5, 0, 0, new Constraints(2, 2.0)); this.driveY = new ProfiledPIDController(5.5, 0, 0, new Constraints(2, 3)); - this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0)); + this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 2.0)); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); this.alignX = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); // 0.0015 @@ -104,8 +107,7 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) { Translation2d offset = new Translation2d( - TagServoingConstants.kInitialDriveRadius, - Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180)); + driveRadius, Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180)); Translation2d sideOffset = new Translation2d( @@ -131,7 +133,7 @@ public TagAlignStates getState() { return curState; } - public void setup(Alliance alliance, boolean scoreLeft) { + public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { targetPose = getTargetDrivePose(alliance, scoreLeft); this.alliance = alliance; this.goalTargetDiag = @@ -139,6 +141,16 @@ public void setup(Alliance alliance, boolean scoreLeft) { ? TagServoingConstants.kRightCamDiagTarget : TagServoingConstants.kLeftCamDiagTarget; + this.stopXRadius = + algae + ? TagServoingConstants.kAlgaeStopXDriveRadius + : TagServoingConstants.kCoralStopXDriveRadius; + this.driveRadius = + algae + ? TagServoingConstants.kAlgaeInitialDriveRadius + : TagServoingConstants.kCoralInitialDriveRadius; + this.algae = algae; + // Inverted, scoring left coral means aligning right camera targetCamId = !scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam; @@ -198,13 +210,14 @@ public double calculateAlignY() { return vY; } - public void start(Alliance alliance, boolean scoreLeft) { - setup(alliance, scoreLeft); + public void start(Alliance alliance, boolean scoreLeft, boolean algae) { + setup(alliance, scoreLeft, algae); curState = TagAlignStates.DRIVE; } public void terminate() { + driveSubsystem.setIgnoreSticks(false); driveSubsystem.move(0, 0, 0, false); driveSubsystem.drive(0, 0, 0); curState = TagAlignStates.DONE; @@ -238,11 +251,11 @@ public void periodic() { double tagRelY = tagRelVel.getY(); double radius = getCurRadius(alliance); - boolean ignoreX = radius < TagServoingConstants.kStopXDriveRadius; + boolean ignoreX = radius < stopXRadius; tagRelX = tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; - if (radius < TagServoingConstants.kStopXDriveRadius) { + if (radius < stopXRadius && !algae) { tagRelX = 0; } From 3a962d5cb97380a670715e2b69f0f51b953582d8 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sat, 22 Feb 2025 17:41:02 -0500 Subject: [PATCH 20/48] auto algae tuning --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../robotState/AutoReefCycleCommand.java | 7 +- ...and.java => ToggleAutoPlacingCommand.java} | 4 +- .../frc/robot/constants/AlgaeConstants.java | 2 +- .../frc/robot/constants/BiscuitConstants.java | 5 +- .../robot/constants/TagServoingConstants.java | 15 ++- .../subsystems/drive/DriveSubsystem.java | 6 ++ .../robotState/RobotStateSubsystem.java | 76 ++++++++++--- .../tagAlign/TagAlignSubsystem.java | 101 +++++++++++++----- 9 files changed, 169 insertions(+), 51 deletions(-) rename src/main/java/frc/robot/commands/robotState/{ToggleAutoCommand.java => ToggleAutoPlacingCommand.java} (74%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d22bc72..ea2a5e27 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; import frc.robot.commands.robotState.ToggleAlgaeHeightCommand; -import frc.robot.commands.robotState.ToggleAutoCommand; +import frc.robot.commands.robotState.ToggleAutoPlacingCommand; import frc.robot.commands.robotState.ToggleGetAlgaeCommand; import frc.robot.commands.robotState.setScoreSideLeftCommand; import frc.robot.constants.BiscuitConstants; @@ -288,7 +288,7 @@ private void configureOperatorBindings() { // Scoring new JoystickButton(xboxController, XboxController.Button.kA.value) - .onTrue(new ToggleAutoCommand(robotStateSubsystem)); + .onTrue(new ToggleAutoPlacingCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kRightStick.value) .onTrue(new SetScoreSideRightCommand(robotStateSubsystem)); diff --git a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java index b42837bc..f49e46d3 100644 --- a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java +++ b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java @@ -7,6 +7,7 @@ import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; public class AutoReefCycleCommand extends Command { @@ -34,12 +35,14 @@ public AutoReefCycleCommand( public void initialize() { driveSubsystem.setIgnoreSticks(true); robotStateSubsystem.toReefAlign(); - scoringCoral = robotStateSubsystem.hasCoral(); + scoringCoral = + robotStateSubsystem.hasCoral() + && !(robotStateSubsystem.getGetAlgaeOnCycle() + && robotStateSubsystem.getScoreSide() == ScoreSide.RIGHT); } @Override public void end(boolean interrupted) { - tagAlignSubsystem.terminate(); } diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java similarity index 74% rename from src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java rename to src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java index 2d0a90e1..9f8cae1f 100644 --- a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java @@ -3,10 +3,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; -public class ToggleAutoCommand extends InstantCommand { +public class ToggleAutoPlacingCommand extends InstantCommand { private RobotStateSubsystem robotStateSubsystem; - public ToggleAutoCommand(RobotStateSubsystem robotState) { + public ToggleAutoPlacingCommand(RobotStateSubsystem robotState) { this.robotStateSubsystem = robotState; } diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 45b3c0f5..c4182deb 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -32,7 +32,7 @@ public class AlgaeConstants { public static final double kIntakingSpeed = -1; public static final double kHasAlgaeVelThreshold = 10; - public static final double kHasAlgaeCounts = 1; + public static final double kHasAlgaeCounts = 3; // Example Talon FX Config public static TalonFXSConfiguration getFXConfig() { diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 33f6c0d1..72b592f8 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -36,6 +36,7 @@ public class BiscuitConstants { public static final Angle kStowSetpoint = Rotations.of(1.862); public static final Angle kFunnelSetpoint = kStowSetpoint; public static final Angle kPrestageSetpoint = kStowSetpoint; + public static final Angle kPrestageAlgaeSetpoint = Rotations.of(9.089); // Algae removal public static final Angle kL2AlgaeSetpoint = Rotations.of(24.104); @@ -44,6 +45,8 @@ public class BiscuitConstants { public static final Angle kL2AlgaeRemovalSetpoint = kStowSetpoint; public static final Angle kL3AlgaeRemovalSetpoint = kStowSetpoint; + public static final double kTagAlignThreshold = 20.0; + // Coral score public static final Angle kL1CoralSetpoint = kStowSetpoint; public static final Angle kL2CoralSetpoint = kStowSetpoint; @@ -58,7 +61,7 @@ public class BiscuitConstants { // Algae scoring public static final Angle kProcessorSetpoint = Rotations.of(41.193); public static final Angle kBargeSetpoint = Rotations.of(12.3489); - public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); + public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089 // jogging public static final double kJogAmountUp = 10; diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index acd41b1c..5c3a09e1 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -1,6 +1,7 @@ package frc.robot.constants; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; public class TagServoingConstants { @@ -17,6 +18,15 @@ public class TagServoingConstants { public static final double kLeftCamDiagTarget = 1180; public static final double kRightCamDiagTarget = 985; + // Constraints + public static final Constraints driveXConstraints = new Constraints(2, 2.0); + public static final Constraints driveYConstraints = new Constraints(2, 3.0); + public static final Constraints driveOmegaConstraints = new Constraints(1, 2.0); + + public static final Constraints alignXConstraints = new Constraints(1, 1.0); + public static final Constraints alignYConstraints = new Constraints(1, 1.5); + public static final Constraints alignOmegaConstraints = new Constraints(1, 1); + public static final double[] kAngleTarget = { Units.degreesToRadians(0), Units.degreesToRadians(60), @@ -40,10 +50,11 @@ public class TagServoingConstants { public static final double kCoralInitialDriveRadius = 1.7; // 1.5 public static final double kCoralStopXDriveRadius = kCoralInitialDriveRadius; // Should be closer to reef than target pose - public static final double kAlgaeInitialDriveRadius = 2.0; // 1.5 + public static final double kAlgaeInitialDriveRadius = 1.6; public static final double kAlgaeStopXDriveRadius = kAlgaeInitialDriveRadius; // Should be closer to reef than target pose - public static final double kDriveCloseEnough = 0.1; + public static final double kCoralDriveCloseEnough = 0.1; + public static final double kAlgaeDriveCloseEnough = 0.2; public static final double kMinVelX = 0.85; // Reef diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index f5bdf66e..9485d4bd 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -90,6 +90,11 @@ public void drive(double vXmps, double vYmps, double vOmegaRadps) { } } + public void stopDriving() { + this.move(0, 0, 0, false); + io.drive(0, 0, 0, false); + } + public void setAzimuthVel(double vel) { io.setAzimuthVel(vel); } @@ -167,6 +172,7 @@ public void setRobotStateSubsystem(RobotStateSubsystem robotStateSubsystem) { } public void setIgnoreSticks(boolean ignore) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Ignoring Sticks", ignore); this.ignoreSticks = ignore; } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 04a45386..17458519 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.robotState; +import static edu.wpi.first.units.Units.Rotations; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -8,6 +10,7 @@ import frc.robot.constants.DriveConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotStateConstants; +import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.algae.AlgaeSubsystem; import frc.robot.subsystems.battMon.BattMonSubsystem; import frc.robot.subsystems.biscuit.BiscuitSubsystem; @@ -18,6 +21,7 @@ import frc.robot.subsystems.funnel.FunnelSubsystem; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Set; import org.littletonrobotics.junction.Logger; @@ -61,6 +65,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private boolean isAuto = false; private boolean isEjectingAlgae = false; private boolean isBargeSafe = true; + private boolean prestagingForAlgae = false; private Timer scoringTimer = new Timer(); @@ -281,6 +286,22 @@ private void toFunnelLoad() { setState(RobotStates.FUNNEL_LOAD, true); } + private void toPrestage() { + biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); + + prestagingForAlgae = getAlgaeOnCycle; + + if (getAlgaeOnCycle) { + elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint); + } else { + elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint); + } + + funnelSubsystem.stopMotor(); + + setState(RobotStates.PRESTAGE, true); + } + public void toPrepCoral() { if (curState == RobotStates.REEF_ALIGN_CORAL) { if (elevatorSubsystem.isFinished()) { @@ -299,11 +320,38 @@ private void toReefAlign(boolean getAlgae, boolean drive) { boolean wantAlgae = getAlgae || !coralSubsystem.hasCoral(); if (drive) { tagAlignSubsystem.start( - allianceColor, scoreSide == ScoreSide.LEFT || !coralSubsystem.hasCoral(), wantAlgae); + allianceColor, + scoreSide == ScoreSide.LEFT + || !coralSubsystem.hasCoral() + || (wantAlgae && getAlgaeOnCycle), + wantAlgae); setState(RobotStates.REEF_ALIGN); } + + boolean algaeSafe = + tagAlignSubsystem.getCurRadius(allianceColor) > TagServoingConstants.kAlgaeStopXDriveRadius + || tagAlignSubsystem.getState() != TagAlignStates.DRIVE; + + boolean ignoreCoralScoring = isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT; + + if (wantAlgae && !algaeSafe) { + algaeSubsystem.intake(); + biscuitSubsystem.setPosition(BiscuitConstants.kPrestageAlgaeSetpoint); + + switch (getAlgaeLevel()) { + case L2 -> { + elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint); + } + case L3 -> { + elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeSetpoint); + } + default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); + } + } + if (wantAlgae - && (scoreSide == ScoreSide.LEFT || !isAutoPlacing) + && algaeSafe + && (scoreSide == ScoreSide.LEFT || !isAutoPlacing || ignoreCoralScoring) && !algaeSubsystem.hasAlgae()) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { return; @@ -325,7 +373,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) { default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } } else if (!drive) { - if (!coralSubsystem.hasCoral()) { + if (!coralSubsystem.hasCoral() || ignoreCoralScoring) { toStow(); return; } else { @@ -571,12 +619,15 @@ public void periodic() { } case REEF_ALIGN -> { if (!isAutoPlacing - || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE - || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.TAG_ALIGN) { + || tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DRIVE + || tagAlignSubsystem.getIsWaitingToAlign()) { toReefAlign(getAlgaeOnCycle, false); } } case REEF_ALIGN_ALGAE -> { + if (biscuitSubsystem.getPosition().in(Rotations) > BiscuitConstants.kTagAlignThreshold) { + tagAlignSubsystem.setProceedToAlign(true); + } if (algaeSubsystem.hasAlgae()) { switch (getAlgaeLevel()) { case L2 -> { @@ -605,7 +656,8 @@ public void periodic() { } case REMOVE_ALGAE -> { if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - if (coralSubsystem.hasCoral()) { + if (coralSubsystem.hasCoral() + && !(isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT)) { toReefAlign(false, false); } else { toStowSequential(); @@ -635,14 +687,14 @@ public void periodic() { case LOADING_CORAL -> { if (coralSubsystem.hasCoral()) { coralLoc = CoralLoc.CORAL; - biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint); - funnelSubsystem.stopMotor(); - - setState(RobotStates.PRESTAGE, true); + toPrestage(); + } + } + case PRESTAGE -> { + if (getAlgaeOnCycle != prestagingForAlgae) { + toPrestage(); } } - case PRESTAGE -> {} case HP_ALGAE -> { if (algaeSubsystem.hasAlgae()) { toProcessor(); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index c3e4589a..436975e4 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; @@ -44,6 +43,9 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius; private double driveRadius = TagServoingConstants.kCoralInitialDriveRadius; private boolean algae = false; + private double driveCloseEnough = TagServoingConstants.kCoralDriveCloseEnough; + private boolean proceedToAlign = false; + private boolean waitingAlign = false; private long startServoTime; @@ -51,14 +53,17 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; - this.driveX = new ProfiledPIDController(5, 0, 0, new Constraints(2, 2.0)); - this.driveY = new ProfiledPIDController(5.5, 0, 0, new Constraints(2, 3)); - this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 2.0)); + this.driveX = new ProfiledPIDController(5, 0, 0, TagServoingConstants.driveXConstraints); + this.driveY = new ProfiledPIDController(5.5, 0, 0, TagServoingConstants.driveYConstraints); + this.driveOmega = + new ProfiledPIDController(5.0, 0, 0, TagServoingConstants.driveOmegaConstraints); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - this.alignX = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); // 0.0015 - this.alignY = new ProfiledPIDController(0.00195, 0, 0, new Constraints(1.0, 1.5)); - this.alignOmega = new ProfiledPIDController(6, 0, 0, new Constraints(1.0, 1.0)); + this.alignX = + new ProfiledPIDController(0.0017, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 + this.alignY = new ProfiledPIDController(0.00195, 0, 0, TagServoingConstants.alignYConstraints); + this.alignOmega = + new ProfiledPIDController(6, 0, 0, TagServoingConstants.alignOmegaConstraints); this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); @@ -68,6 +73,14 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", -1); } + public void setProceedToAlign(boolean proceed) { + this.proceedToAlign = proceed; + } + + public boolean getIsWaitingToAlign() { + return waitingAlign; + } + public int computeHexant(Alliance color) { Translation2d reefT = color == Alliance.Blue @@ -119,13 +132,14 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) { Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); } - private double getCurRadius(Alliance color) { + public double getCurRadius(Alliance color) { Translation2d reefT = color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : TagServoingConstants.kRedReefPose; Translation2d reefRelative = driveSubsystem.getPoseMeters().getTranslation().minus(reefT); + return FastMath.hypot(reefRelative.getX(), reefRelative.getY()); } @@ -150,6 +164,10 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { ? TagServoingConstants.kAlgaeInitialDriveRadius : TagServoingConstants.kCoralInitialDriveRadius; this.algae = algae; + this.driveCloseEnough = + algae + ? TagServoingConstants.kAlgaeDriveCloseEnough + : TagServoingConstants.kCoralDriveCloseEnough; // Inverted, scoring left coral means aligning right camera targetCamId = @@ -160,10 +178,13 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { : TagServoingConstants.kRedTargetTag[computeHexant(alliance)]; fieldRelHexant = computeFieldRelHexant(alliance); + proceedToAlign = false; + waitingAlign = false; Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); + Logger.recordOutput("TagAlignSubsystem/GettingAlgae", algae); Pose2d current = driveSubsystem.getPoseMeters(); @@ -171,8 +192,8 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { driveY.reset(current.getY()); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(0); - alignY.reset(0); + alignX.reset(this.goalTargetDiag); + alignY.reset(TagServoingConstants.kHorizontalTarget); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -216,6 +237,15 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { curState = TagAlignStates.DRIVE; } + private void tagAlign() { + alignX.reset(0); + alignY.reset(0); + alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); + + curState = TagAlignStates.TAG_ALIGN; + startServoTime = RobotController.getFPGATime(); + } + public void terminate() { driveSubsystem.setIgnoreSticks(false); driveSubsystem.move(0, 0, 0, false); @@ -227,6 +257,7 @@ public void terminate() { public void periodic() { Logger.recordOutput("TagAlignSubsystem/State", curState.toString()); // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); + Logger.recordOutput("TagAlignSubsystem/waitingAlign", waitingAlign); switch (curState) { case DRIVE -> { @@ -251,11 +282,14 @@ public void periodic() { double tagRelY = tagRelVel.getY(); double radius = getCurRadius(alliance); - boolean ignoreX = radius < stopXRadius; + boolean ignoreX = radius < stopXRadius && !algae; - tagRelX = tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; + if (!algae || tagRelX > 0) { + tagRelX = + tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; + } - if (radius < stopXRadius && !algae) { + if (ignoreX) { tagRelX = 0; } @@ -266,26 +300,28 @@ public void periodic() { .rotateBy( Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); - Logger.recordOutput("TagAlignSubsystem/Tag Rel Y Error", tagRelError.getY()); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive Y Error", tagRelError.getY()); Transform2d poseError = targetPose.minus(current); if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough - && (FastMath.hypot(poseError.getX(), poseError.getY()) - < TagServoingConstants.kDriveCloseEnough - || ignoreX - && FastMath.abs(tagRelError.getY()) < TagServoingConstants.kDriveCloseEnough)) { - alignX.reset(0); - alignY.reset(0); - alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - - curState = TagAlignStates.TAG_ALIGN; - startServoTime = RobotController.getFPGATime(); - break; + && (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough + || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough)) { + + if (proceedToAlign || !algae) { + tagAlign(); + break; + } else { + tagRelX = 0; + waitingAlign = true; + driveSubsystem.stopDriving(); + curState = TagAlignStates.WAITING; + break; + } } - Logger.recordOutput("TagAlignSubsystem/Tag Rel vX", tagRelX); - Logger.recordOutput("TagAlignSubsystem/Tag Rel vY", tagRelY); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", tagRelY); Translation2d adjusted = new Translation2d(tagRelX, tagRelY) @@ -295,6 +331,12 @@ public void periodic() { driveSubsystem.move(adjusted.getX(), adjusted.getY(), vOmega, true); } + case WAITING -> { + if (proceedToAlign || !algae) { + tagAlign(); + } + } + case TAG_ALIGN -> { WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); @@ -325,8 +367,8 @@ public void periodic() { double vX = -alignX.calculate(goalTargetDiag - diag, 0); double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); - Logger.recordOutput("TagAlignSubsystem/X Error", alignX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/Y Error", alignY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Tag Align X Error", alignX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Tag Align Y Error", alignY.getPositionError()); Logger.recordOutput( "TagAlignSubsystem/Last result delay", (RobotController.getFPGATime() - result.getTimeStamp()) / 1000); @@ -370,6 +412,7 @@ public Set getMeasures() { public enum TagAlignStates { DRIVE, + WAITING, TAG_ALIGN, DONE } From 98a9de0ca940e7fe97fe24698d4485f648f9b2f0 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 23 Feb 2025 14:31:51 -0500 Subject: [PATCH 21/48] add LED subsystem calls to robotState --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/LEDConstants.java | 7 +++--- .../java/frc/robot/subsystems/led/LEDIO.java | 8 +++--- .../robot/subsystems/led/LEDSubsystem.java | 25 ++++++++----------- .../robotState/RobotStateSubsystem.java | 17 +++++++++++++ 5 files changed, 36 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16769b67..cace61b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -135,7 +135,7 @@ public RobotContainer() { funnelSubsystem = new FunnelSubsystem(funnelIO); ledIO = new LEDIO(); - ledSubsystem = new LEDSubsystem(); + ledSubsystem = new LEDSubsystem(ledIO); visionSubsystem = new VisionSubsystem(driveSubsystem); diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 6e193f21..11b806f6 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -3,11 +3,12 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDConstants { + public static final int kLEDPort = 0; // Auto/Operator = 27 // other = 21.5 - public static final int kTotalStripLength = 70; - public static final int kBottomStripLength = 43; - public static final int kTopStripLength = 27; + public static final int kTotalStripLength = 51; + public static final int kBottomStripLength = 34; + public static final int kTopStripLength = 17; public static final int kTopFirstIndex = kBottomStripLength + 1; public static final int kAlgeaEnd = kBottomStripLength / 3 + 1; diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index 5323b66c..2607f782 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -12,11 +12,11 @@ public class LEDIO { private AddressableLEDBuffer led; private AddressableLEDBufferView ledTop; - public LEDIO(int port, int length) { - ledBase = new AddressableLED(port); - ledBase.setLength(length); + public LEDIO() { + ledBase = new AddressableLED(LEDConstants.kLEDPort); + ledBase.setLength(LEDConstants.kTotalStripLength); ledBase.start(); - led = new AddressableLEDBuffer(length); + led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength); ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1); ledBase.setData(led); } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index e0e80c8b..80504b4d 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -1,15 +1,12 @@ package frc.robot.subsystems.led; -import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.util.Color; import frc.robot.constants.LEDConstants; import frc.robot.subsystems.robotState.RobotStateSubsystem.CoralLoc; import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; - import java.util.Map; import java.util.Set; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; @@ -22,8 +19,7 @@ public class LEDSubsystem extends MeasurableSubsystem { // section patterns private LEDPattern algea = - LEDPattern.steps( - Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack)); + LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack)); private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); private LEDPattern level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); private LEDPattern place = @@ -196,15 +192,13 @@ private void buildBase() { base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(algea.overlayOn(coral)))); } - //game stuff + // game stuff private void advanceUnits() { - for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) { + for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) { BottomUnits[i + 1] = BottomUnits[i]; TopUnits[i] = TopUnits[i + 1]; - if (i == 0) - BottomUnits[i] = 0; - if (i == LEDConstants.kTopStripLength) - TopUnits[i] = 0; + if (i == 0) BottomUnits[i] = 0; + if (i == LEDConstants.kTopStripLength) TopUnits[i] = 0; if (TopUnits[i] != 0 && BottomUnits[i] != 0) { if (BottomUnits[i] == TopUnits[i]) { BottomUnits[i] = 0; @@ -219,7 +213,8 @@ private void advanceUnits() { if (BottomUnits[i] == TopUnits[i + 1]) { BottomUnits[i] = 0; TopUnits[i + 1] = 0; - } else if (BottomUnits[i] > TopUnits[i + 1] || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) { + } else if (BottomUnits[i] > TopUnits[i + 1] + || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) { TopUnits[i + 1] = 0; } else { BottomUnits[i + 1] = 0; @@ -229,9 +224,9 @@ private void advanceUnits() { } private void displayUnits() { - for (int i = 0; i <= LEDConstants.kTopStripLength+1; i++) { + for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) { io.setLEDTop(i, LEDConstants.kGameColors[BottomUnits[i]]); - if (TopUnits[i] != 0){ + if (TopUnits[i] != 0) { io.setLEDTop(i, LEDConstants.kGameColors[TopUnits[i] + 3]); } } @@ -240,7 +235,7 @@ private void displayUnits() { public void addGameUnit(boolean isBottom, int unitNum) { // the unitNum starts at 1 and ends at 3. if you use anything else, it will be ignored if (unitNum >= 1 && unitNum <= 3) { - if (isBottom){ + if (isBottom) { BottomUnits[0] = unitNum; } else { TopUnits[LEDConstants.kTopStripLength - 1] = unitNum; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index d0313e81..29a0c458 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.funnel.FunnelSubsystem; import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.led.LEDSubsystem.PlaceStates; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Set; @@ -163,10 +164,14 @@ public void setAllianceColor(Alliance alliance) { public void setScoringLevel(ScoringLevel scoringLevel) { this.scoringLevel = scoringLevel; + ledSubsystem.setLevelLights(scoringLevel); } public void setScoreSide(ScoreSide scoreSide) { this.scoreSide = scoreSide; + if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL); + else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT); + else ledSubsystem.setPlaceLights(PlaceStates.RIGHT); } public void setAlgaeHeight(AlgaeHeight algaeHeight) { @@ -179,18 +184,24 @@ public void toggleAlgaeHeight() { public void setIsAutoPlacing(boolean isAutoPlacing) { this.isAutoPlacing = isAutoPlacing; + if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL); + else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT); + else ledSubsystem.setPlaceLights(PlaceStates.RIGHT); } public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) { this.getAlgaeOnCycle = getAlgaeOnCycle; + ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle); } public void toggleGetAlgaeOnCycle() { getAlgaeOnCycle = !getAlgaeOnCycle; + ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle); } public void setCurrentLimiting(boolean isCurrentLimiting) { this.isCurrentLimiting = isCurrentLimiting; + ledSubsystem.setCurrentLimiting(isCurrentLimiting); } public void setIsAuto(boolean isAuto) { @@ -298,6 +309,7 @@ public void toReefAlign() { private void toReefAlign(boolean getAlgae, boolean drive) { if (drive) { tagAlignSubsystem.start(allianceColor, scoreSide == ScoreSide.LEFT); + setAutoPlacingLed(true); setState(RobotStates.REEF_ALIGN); } if ((getAlgae || !coralSubsystem.hasCoral()) @@ -473,6 +485,7 @@ private void toProcessor() { public void toInterrupted() { if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) { tagAlignSubsystem.terminate(); + setAutoPlacingLed(false); driveSubsystem.setIgnoreSticks(false); } @@ -609,6 +622,7 @@ public void periodic() { if (coralSubsystem.hasCoral()) { toReefAlign(false, false); } else { + setAutoPlacingLed(false); toStowSequential(); } } @@ -618,6 +632,7 @@ public void periodic() { if (!coralSubsystem.hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { coralLoc = CoralLoc.NONE; + setAutoPlacingLed(false); toFunnelLoad(); } else { coralLoc = CoralLoc.SCORING; @@ -708,6 +723,8 @@ public void periodic() { case IDLE -> {} default -> logger.error("Unhandled state: {}", curState); } + ledSubsystem.setCoralLights(coralLoc); + ledSubsystem.setAlgeaLights(hasAlgae()); } @Override From 732740fbad8e5d95967b91ec1ffb01a500a7ad55 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sun, 23 Feb 2025 14:32:03 -0500 Subject: [PATCH 22/48] clean up and vision testing --- .../java/frc/robot/constants/VisionConstants.java | 12 ++++++------ .../subsystems/robotState/RobotStateSubsystem.java | 3 +-- .../robot/subsystems/tagAlign/TagAlignSubsystem.java | 12 +----------- 3 files changed, 8 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 1e6a8065..86cff6d6 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -98,18 +98,18 @@ public final class VisionConstants { public static final Pose3d kCam2Pose = new Pose3d( - new Translation3d(-0.21, -0.31, 0.44), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0))); + new Translation3d(0.236, -0.108, 0.932), + new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(22.5))); public static final Pose3d kCam3Pose = new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( - new Translation3d(-0.22, -0.335, 0.50), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); + new Translation3d(0.20, 0.035, 0.94), + new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(-22.5))); public static final Pose3d kCam5Pose = new Pose3d( - new Translation3d(-0.22, -0.335, 0.50), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); + new Translation3d(-0.229, -0.073, 0.934), + new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(180.0))); // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is // in the form [theta], with units in radians. public static Matrix kLocalMeasurementStdDevs = diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 17458519..04363657 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -619,8 +619,7 @@ public void periodic() { } case REEF_ALIGN -> { if (!isAutoPlacing - || tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DRIVE - || tagAlignSubsystem.getIsWaitingToAlign()) { + || tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DRIVE) { toReefAlign(getAlgaeOnCycle, false); } } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 436975e4..3129baba 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -45,7 +45,6 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private boolean algae = false; private double driveCloseEnough = TagServoingConstants.kCoralDriveCloseEnough; private boolean proceedToAlign = false; - private boolean waitingAlign = false; private long startServoTime; @@ -77,10 +76,6 @@ public void setProceedToAlign(boolean proceed) { this.proceedToAlign = proceed; } - public boolean getIsWaitingToAlign() { - return waitingAlign; - } - public int computeHexant(Alliance color) { Translation2d reefT = color == Alliance.Blue @@ -179,7 +174,6 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { fieldRelHexant = computeFieldRelHexant(alliance); proceedToAlign = false; - waitingAlign = false; Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); @@ -248,8 +242,7 @@ private void tagAlign() { public void terminate() { driveSubsystem.setIgnoreSticks(false); - driveSubsystem.move(0, 0, 0, false); - driveSubsystem.drive(0, 0, 0); + driveSubsystem.stopDriving(); curState = TagAlignStates.DONE; } @@ -257,7 +250,6 @@ public void terminate() { public void periodic() { Logger.recordOutput("TagAlignSubsystem/State", curState.toString()); // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); - Logger.recordOutput("TagAlignSubsystem/waitingAlign", waitingAlign); switch (curState) { case DRIVE -> { @@ -312,8 +304,6 @@ public void periodic() { tagAlign(); break; } else { - tagRelX = 0; - waitingAlign = true; driveSubsystem.stopDriving(); curState = TagAlignStates.WAITING; break; From 5af022c15c6e12b0adee11b6ad57efb0addd7da2 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sun, 23 Feb 2025 16:45:41 -0500 Subject: [PATCH 23/48] light testing, color picking --- src/main/java/frc/robot/RobotContainer.java | 16 ++ .../frc/robot/constants/LEDConstants.java | 56 +++-- .../java/frc/robot/subsystems/led/LEDIO.java | 4 +- .../robot/subsystems/led/LEDSubsystem.java | 231 ++++++++++-------- .../robotState/RobotStateSubsystem.java | 13 +- 5 files changed, 183 insertions(+), 137 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cace61b1..209a55a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.algae.IntakeAlgaeCommand; @@ -171,6 +172,7 @@ private void configureTelemetry() { elevatorSubsystem.registerWith(telemetryService); funnelSubsystem.registerWith(telemetryService); biscuitSubsystem.registerWith(telemetryService); + ledSubsystem.registerWith(telemetryService); telemetryService.start(); } @@ -427,6 +429,20 @@ public void configurePitDashboard() { .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem)) .withPosition(2, 1) .withSize(1, 1); + + Shuffleboard.getTab("Debug") + .add( + "Toggle LED autoplacing", + new InstantCommand(() -> ledSubsystem.setAutoPlacing(!ledSubsystem.getAutoPlacing()))) + .withPosition(1, 1) + .withSize(1, 1); + Shuffleboard.getTab("Debug") + .add( + "Toggle LED Current Limiting", + new InstantCommand( + () -> ledSubsystem.setCurrentLimiting(!ledSubsystem.getCurrentLimiting()))) + .withPosition(2, 1) + .withSize(1, 1); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 11b806f6..3a07509a 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -3,57 +3,61 @@ import edu.wpi.first.wpilibj.util.Color; public class LEDConstants { + public static final Color invertRedGreen(Color color) { + return new Color(color.green, color.red, color.blue); + } + public static final int kLEDPort = 0; // Auto/Operator = 27 // other = 21.5 public static final int kTotalStripLength = 51; public static final int kBottomStripLength = 34; public static final int kTopStripLength = 17; - public static final int kTopFirstIndex = kBottomStripLength + 1; - + public static final int kTopFirstIndex = kBottomStripLength; public static final int kAlgeaEnd = kBottomStripLength / 3 + 1; + public static final int kLevelStart = kTopFirstIndex; public static final int kPlaceStart = kTopFirstIndex + kTopStripLength / 3; public static final int kGetAlgeaStart = kTopFirstIndex + kTopStripLength / 3 * 2; - public static final int kAutoPlacingStart = kTopFirstIndex; + public static final int kAutoPlacingStart = kBottomStripLength / 3 * 2 + 1; public static final Color kAlmostBlack = new Color(0, 0, 1); // Normal LED Colors - public static final Color kHasAlgea = Color.kAquamarine; + public static final Color kHasAlgea = invertRedGreen(Color.kTeal); public static final Color kNotHasAlgea = kAlmostBlack; - public static final Color kCoralNotInRobot = Color.kOrange; - public static final Color kCoralInFunnel = Color.kHotPink; - public static final Color kCoralInRobot = Color.kWhite; + public static final Color kCoralNotInRobot = invertRedGreen(Color.kOrange); + public static final Color kCoralInFunnel = invertRedGreen(Color.kPurple); + public static final Color kCoralInRobot = invertRedGreen(Color.kAntiqueWhite); - public static final Color kL1 = Color.kBlue; - public static final Color kL2 = Color.kGreen; - public static final Color kL3 = Color.kYellow; - public static final Color kL4 = Color.kRed; + public static final Color kL1 = invertRedGreen(Color.kRed); + public static final Color kL2 = invertRedGreen(Color.kLightYellow); + public static final Color kL3 = invertRedGreen(Color.kGreen); + public static final Color kL4 = invertRedGreen(Color.kBlue); public static final Color kManual = kAlmostBlack; - public static final Color kRight = Color.kSaddleBrown; - public static final Color kLeft = Color.kGreenYellow; + public static final Color kRight = invertRedGreen(Color.kOrangeRed); + public static final Color kLeft = invertRedGreen(Color.kPurple); - public static final Color kGetAlgea = Color.kAquamarine; + public static final Color kGetAlgea = invertRedGreen(Color.kTeal); public static final Color kNotGetAlgea = kAlmostBlack; - public static final Color kCurrentLimiting = Color.kRed; + public static final Color kCurrentLimiting = invertRedGreen(Color.kRed); - public static final Color kAutoPlacing = Color.kPurple; + public static final Color kAutoPlacing = invertRedGreen(Color.kBlue); // Climb LED Colors - public static final Color kWaitingForCage = Color.kRed; - public static final Color kHasCage = Color.kGreen; - public static final Color kClimbed = Color.kGoldenrod; + public static final Color kWaitingForCage = invertRedGreen(Color.kRed); + public static final Color kHasCage = invertRedGreen(Color.kGreen); + public static final Color kClimbed = invertRedGreen(Color.kGoldenrod); public static final Color[] kGameColors = { - Color.kBlack, // a dummy color - Color.kDarkRed, - Color.kDarkGreen, - Color.kDarkBlue, - Color.kPink, - Color.kLightGreen, - Color.kLightBlue + invertRedGreen(Color.kBlack), // a dummy color + invertRedGreen(Color.kDarkRed), + invertRedGreen(Color.kDarkGreen), + invertRedGreen(Color.kDarkBlue), + invertRedGreen(Color.kPink), + invertRedGreen(Color.kLightGreen), + invertRedGreen(Color.kLightBlue) }; } diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java index 2607f782..bffd4833 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDIO.java +++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java @@ -17,7 +17,7 @@ public LEDIO() { ledBase.setLength(LEDConstants.kTotalStripLength); ledBase.start(); led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength); - ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1); + ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1); ledBase.setData(led); } @@ -32,7 +32,7 @@ public void setPort(int port) { public void setLength(int length) { ledBase.setLength(length); led = new AddressableLEDBuffer(length); - ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength + 1); + ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1); ledBase.setData(led); } diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java index 80504b4d..26fbfb3e 100644 --- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java @@ -9,6 +9,7 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import java.util.Map; import java.util.Set; +import org.littletonrobotics.junction.Logger; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; @@ -18,32 +19,45 @@ public class LEDSubsystem extends MeasurableSubsystem { public LEDStates currState = LEDStates.OFF; // section patterns - private LEDPattern algea = + private LEDPattern algae = LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack)); private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot); - private LEDPattern level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); + private LEDPattern level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL1)); private LEDPattern place = - LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kManual)); + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kManual)); private LEDPattern getAlgea = - LEDPattern.steps(Map.of(LEDConstants.kGetAlgeaStart, LEDConstants.kNotGetAlgea)); + LEDPattern.steps( + Map.of( + LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kNotGetAlgea)); private LEDPattern autoplace = - LEDPattern.steps(Map.of(LEDConstants.kAutoPlacingStart, LEDConstants.kAutoPlacing)) + LEDPattern.steps( + Map.of( + LEDConstants.kAutoPlacingStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kAutoPlacing)) .blink(Seconds.of(0.25)); private LEDPattern currentLimiting = - LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(1), Seconds.of(2)); + LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(0.5), Seconds.of(1)); // section booleans and states private boolean hasAlgae = false; private CoralLoc coralState = CoralLoc.NONE; - private ScoringLevel levelState = ScoringLevel.L1; + private ScoringLevel levelState = ScoringLevel.L4; private PlaceStates placeState = PlaceStates.MANUAL; - private boolean shouldGetAlgea = false; + private boolean shouldGetAlgae = false; private boolean autoPlacing = false; private boolean isLimiting = false; // game stuff - private int[] BottomUnits = new int[LEDConstants.kTopStripLength]; - private int[] TopUnits = new int[LEDConstants.kTopStripLength]; + private int[] bottomUnits = new int[LEDConstants.kTopStripLength]; + private int[] topUnits = new int[LEDConstants.kTopStripLength]; public LEDSubsystem(LEDIO io) { this.io = io; @@ -52,23 +66,12 @@ public LEDSubsystem(LEDIO io) { public void setState(LEDStates state) { currState = state; switch (currState) { - case OFF: - io.setOff(); - break; - case NORMAL: - buildBase(); - break; - case CLIMB_EMPTY: - io.setStrip(LEDConstants.kWaitingForCage); - break; - case CLIMB_FULL: - io.setStrip(LEDConstants.kHasCage); - break; - case CLIMB_UP: - io.setStrip(LEDConstants.kClimbed); - break; - default: - break; + case OFF -> io.setOff(); + case NORMAL -> buildBase(); + case CLIMB_EMPTY -> io.setStrip(LEDConstants.kWaitingForCage); + case CLIMB_FULL -> io.setStrip(LEDConstants.kHasCage); + case CLIMB_UP -> io.setStrip(LEDConstants.kClimbed); + default -> {} } } @@ -98,7 +101,7 @@ public void setPlaceLights(PlaceStates state) { } public void setGetAlgeaLights(boolean on) { - shouldGetAlgea = on; + shouldGetAlgae = on; buildBase(); } @@ -130,7 +133,7 @@ public PlaceStates getPlaceLights() { } public boolean getGetAlgeaLights() { - return shouldGetAlgea; + return shouldGetAlgae; } public boolean getAutoPlacing() { @@ -142,82 +145,101 @@ public boolean getCurrentLimiting() { } private void buildBase() { - algea = + algae = LEDPattern.steps( Map.of( 0, hasAlgae ? LEDConstants.kHasAlgea : LEDConstants.kNotHasAlgea, - LEDConstants.kAlgeaEnd, + LEDConstants.kAlgeaEnd / (double) LEDConstants.kTotalStripLength, Color.kBlack)); switch (coralState) { - case FUNNEL: - case TRANSFER: - coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); - break; - case CORAL: - case SCORING: - coral = LEDPattern.solid(LEDConstants.kCoralInRobot); - break; - case NONE: - coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); - break; + case FUNNEL, TRANSFER -> coral = LEDPattern.solid(LEDConstants.kCoralInFunnel); + case CORAL, SCORING -> coral = LEDPattern.solid(LEDConstants.kCoralInRobot); + case NONE -> coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot); } switch (levelState) { - case L1: - level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL1)); - break; - case L2: - level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL2)); - break; - case L3: - level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL3)); - break; - case L4: - level = LEDPattern.steps(Map.of(LEDConstants.kLevelStart, LEDConstants.kL4)); - break; + case L1 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL1)); + case L2 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL2)); + case L3 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL3)); + case L4 -> level = + LEDPattern.steps( + Map.of( + LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kL4)); } switch (placeState) { - case MANUAL: - place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kManual)); - case LEFT: - place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kLeft)); - case RIGHT: - place = LEDPattern.steps(Map.of(LEDConstants.kPlaceStart, LEDConstants.kRight)); + case MANUAL -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kManual)); + case LEFT -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kLeft)); + case RIGHT -> place = + LEDPattern.steps( + Map.of( + LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength, + LEDConstants.kRight)); } getAlgea = LEDPattern.steps( Map.of( - LEDConstants.kGetAlgeaStart, - shouldGetAlgea ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea)); - base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(algea.overlayOn(coral)))); + LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength, + shouldGetAlgae ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea)); + base = algae.overlayOn(coral); + + if (isLimiting) { + base = currentLimiting.overlayOn(base); + } + + if (autoPlacing) { + base = autoplace.overlayOn(base); + } + + base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base))); } // game stuff private void advanceUnits() { for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) { - BottomUnits[i + 1] = BottomUnits[i]; - TopUnits[i] = TopUnits[i + 1]; - if (i == 0) BottomUnits[i] = 0; - if (i == LEDConstants.kTopStripLength) TopUnits[i] = 0; - if (TopUnits[i] != 0 && BottomUnits[i] != 0) { - if (BottomUnits[i] == TopUnits[i]) { - BottomUnits[i] = 0; - TopUnits[i] = 0; - } else if (BottomUnits[i] > TopUnits[i] || (BottomUnits[i] == 1 && TopUnits[i] == 3)) { - TopUnits[i] = 0; + bottomUnits[i + 1] = bottomUnits[i]; + topUnits[i] = topUnits[i + 1]; + if (i == 0) bottomUnits[i] = 0; + if (i == LEDConstants.kTopStripLength) topUnits[i] = 0; + if (topUnits[i] != 0 && bottomUnits[i] != 0) { + if (bottomUnits[i] == topUnits[i]) { + bottomUnits[i] = 0; + topUnits[i] = 0; + } else if (bottomUnits[i] > topUnits[i] || (bottomUnits[i] == 1 && topUnits[i] == 3)) { + topUnits[i] = 0; } else { - BottomUnits[i] = 0; + bottomUnits[i] = 0; } } - if (TopUnits[i + 1] != 0 && BottomUnits[i] != 0) { - if (BottomUnits[i] == TopUnits[i + 1]) { - BottomUnits[i] = 0; - TopUnits[i + 1] = 0; - } else if (BottomUnits[i] > TopUnits[i + 1] - || (BottomUnits[i] == 1 && TopUnits[i + 1] == 3)) { - TopUnits[i + 1] = 0; + if (topUnits[i + 1] != 0 && bottomUnits[i] != 0) { + if (bottomUnits[i] == topUnits[i + 1]) { + bottomUnits[i] = 0; + topUnits[i + 1] = 0; + } else if (bottomUnits[i] > topUnits[i + 1] + || (bottomUnits[i] == 1 && topUnits[i + 1] == 3)) { + topUnits[i + 1] = 0; } else { - BottomUnits[i + 1] = 0; + bottomUnits[i + 1] = 0; } } } @@ -225,9 +247,9 @@ private void advanceUnits() { private void displayUnits() { for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) { - io.setLEDTop(i, LEDConstants.kGameColors[BottomUnits[i]]); - if (TopUnits[i] != 0) { - io.setLEDTop(i, LEDConstants.kGameColors[TopUnits[i] + 3]); + io.setLEDTop(i, LEDConstants.kGameColors[bottomUnits[i]]); + if (topUnits[i] != 0) { + io.setLEDTop(i, LEDConstants.kGameColors[topUnits[i] + 3]); } } } @@ -236,34 +258,35 @@ public void addGameUnit(boolean isBottom, int unitNum) { // the unitNum starts at 1 and ends at 3. if you use anything else, it will be ignored if (unitNum >= 1 && unitNum <= 3) { if (isBottom) { - BottomUnits[0] = unitNum; + bottomUnits[0] = unitNum; } else { - TopUnits[LEDConstants.kTopStripLength - 1] = unitNum; + topUnits[LEDConstants.kTopStripLength - 1] = unitNum; } } } + @Override public void periodic() { + Logger.recordOutput("LedSubsystem/state", currState); + Logger.recordOutput("LedSubsystem/hasAlgae", hasAlgae); + Logger.recordOutput("LedSubsystem/coralState", coralState); + Logger.recordOutput("LedSubsystem/levelState", levelState); + Logger.recordOutput("LedSubsystem/placeState", placeState); + Logger.recordOutput("LedSubsystem/shouldGetAlgae", shouldGetAlgae); + Logger.recordOutput("LedSubsystem/autoPlacing", autoPlacing); + Logger.recordOutput("LedSubsystem/currentLimiting", isLimiting); + switch (currState) { - case OFF: - break; - case NORMAL: - LEDPattern output = base; - if (autoPlacing) { - output = autoplace.overlayOn(output); - } - if (isLimiting) { - output = currentLimiting.overlayOn(output); - } + case OFF -> {} + case NORMAL -> { io.setStrip(base); - break; - case CLIMB_EMPTY: - break; - case CLIMB_FULL: - break; - case CLIMB_UP: + } + case CLIMB_EMPTY -> {} + case CLIMB_FULL -> {} + case CLIMB_UP -> { advanceUnits(); displayUnits(); + } } io.updateLEDs(); } @@ -290,7 +313,7 @@ public Set getMeasures() { new Measure( "GetAlgea", "the current get algea state of the LEDSubsystem", - () -> shouldGetAlgea ? 0 : 1), + () -> shouldGetAlgae ? 0 : 1), new Measure( "autoPlacing", "the current autoPlacing state of the LEDSubsystem", diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 29a0c458..2394ea78 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.funnel.FunnelSubsystem; import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.led.LEDSubsystem.LEDStates; import frc.robot.subsystems.led.LEDSubsystem.PlaceStates; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -53,7 +54,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private ScoreSide scoreSide = ScoreSide.LEFT; private AlgaeHeight algaeHeight = AlgaeHeight.LOW; private AlgaeHeight currentAlgaeHeight = AlgaeHeight.LOW; - private CoralLoc coralLoc = CoralLoc.CORAL; + private CoralLoc coralLoc = CoralLoc.NONE; private Pose2d algaeRemovalPose; private boolean isAutoPlacing = false; @@ -88,6 +89,8 @@ public RobotStateSubsystem( this.ledSubsystem = ledSubsystem; this.tagAlignSubsystem = tagAlignSubsystem; this.visionSubsystem = visionSubsystem; + + ledSubsystem.setState(LEDStates.NORMAL); } public RobotStates getState() { @@ -516,10 +519,6 @@ public void toClimb() { @Override public void periodic() { - if (funnelSubsystem.hasCoral()) { - coralLoc = CoralLoc.FUNNEL; - } - Logger.recordOutput("RobotState/state", curState); Logger.recordOutput("RobotState/hasCoral", hasCoral()); Logger.recordOutput("RobotState/hasAlgae", hasAlgae()); @@ -532,6 +531,7 @@ public void periodic() { Logger.recordOutput("RobotState/isAuto", isAuto); Logger.recordOutput("RobotState/currentLimiting", isCurrentLimiting); Logger.recordOutput("RobotState/isEjectingAlgae", isEjectingAlgae); + Logger.recordOutput("RobotState/coralLoc", coralLoc); switch (curState) { case TRANSFER -> { @@ -640,6 +640,9 @@ public void periodic() { } case FUNNEL_LOAD -> { + if (funnelSubsystem.hasCoral()) { + coralLoc = CoralLoc.FUNNEL; + } if (elevatorSubsystem.isFinished()) { funnelSubsystem.startMotor(); } From 773923aafa0fa482fde7d7637eec64cddf01fdaa Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 24 Feb 2025 17:38:45 -0500 Subject: [PATCH 24/48] post merge --- src/main/java/frc/robot/constants/DriveConstants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 9c54826b..7aa4e9a8 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -53,6 +53,7 @@ public class DriveConstants { public static final double kPHolonomic = 3.0; // was 3 public static final double kIHolonomic = 0.0000; public static final double kDHolonomic = 0.00; // kPHolonomic/100 + public static double kFieldMaxY; public static Translation2d[] getWheelLocationMeters() { final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH From a35af847b372b77b06c0c1b5b0781fc0f565fcf3 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 24 Feb 2025 19:48:28 -0500 Subject: [PATCH 25/48] sort of did stuff --- ...erTestPath.traj => FiveMeterTestPath.traj} | 2 +- src/main/java/frc/robot/RobotContainer.java | 47 ++++++++++ .../PathHandler/KillPathHandlerCommand.java | 18 ---- .../PathHandler/StartPathHandlerCommand.java | 23 ----- .../NonProcessorShallowAutonCommand.java | 88 +++++++++---------- .../commands/drive/SetGyroOffsetCommand.java | 21 +++++ .../pathhHandler/KillPathHandlerCommand.java | 18 ++++ .../pathhHandler/SetPathHandlerCommand.java | 38 ++++++++ .../pathhHandler/StartPathHandlerCommand.java | 50 +++++++++++ .../vision/SetVisionUpdatesCommand.java | 19 ++++ .../frc/robot/constants/DriveConstants.java | 12 ++- .../robot/constants/PathHandlerConstants.java | 8 ++ 12 files changed, 254 insertions(+), 90 deletions(-) rename src/main/deploy/choreo/{5MeterTestPath.traj => FiveMeterTestPath.traj} (99%) delete mode 100644 src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java delete mode 100644 src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java create mode 100644 src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java create mode 100644 src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java create mode 100644 src/main/java/frc/robot/constants/PathHandlerConstants.java diff --git a/src/main/deploy/choreo/5MeterTestPath.traj b/src/main/deploy/choreo/FiveMeterTestPath.traj similarity index 99% rename from src/main/deploy/choreo/5MeterTestPath.traj rename to src/main/deploy/choreo/FiveMeterTestPath.traj index 1e8c21e6..c7979702 100644 --- a/src/main/deploy/choreo/5MeterTestPath.traj +++ b/src/main/deploy/choreo/FiveMeterTestPath.traj @@ -1,5 +1,5 @@ { - "name":"5MeterTestPath", + "name":"FiveMeterTestPath", "version":1, "snapshot":{ "waypoints":[ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16769b67..3a02348f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,10 +19,12 @@ import frc.robot.commands.algae.OpenLoopAlgaeCommand; import frc.robot.commands.algae.ProcessorAlgaeCommand; import frc.robot.commands.algae.ToggleHasAlgaeCommand; +import frc.robot.commands.auton.NonProcessorShallowAutonCommand; import frc.robot.commands.biscuit.HoldBiscuitCommand; import frc.robot.commands.biscuit.JogBiscuitCommand; import frc.robot.commands.coral.EnableEjectBeamCommand; import frc.robot.commands.coral.OpenLoopCoralCommand; +import frc.robot.commands.drive.DriveAutonCommand; import frc.robot.commands.drive.DriveTeleopCommand; import frc.robot.commands.drive.ResetGyroCommand; import frc.robot.commands.elevator.HoldElevatorCommand; @@ -42,8 +44,10 @@ import frc.robot.commands.robotState.ToggleAutoCommand; import frc.robot.commands.robotState.ToggleGetAlgaeCommand; import frc.robot.commands.robotState.setScoreSideLeftCommand; +import frc.robot.commands.vision.SetVisionUpdatesCommand; import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.PathHandlerConstants; import frc.robot.constants.RobotConstants; import frc.robot.controllers.FlyskyJoystick; import frc.robot.controllers.FlyskyJoystick.Button; @@ -65,10 +69,12 @@ import frc.robot.subsystems.funnel.FunnelSubsystem; import frc.robot.subsystems.led.LEDIO; import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.pathHandler.PathHandler; import frc.robot.subsystems.robotState.RobotStateSubsystem; import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.List; import org.strykeforce.telemetry.TelemetryController; import org.strykeforce.telemetry.TelemetryService; @@ -104,6 +110,8 @@ public class RobotContainer { private final VisionSubsystem visionSubsystem; + private final PathHandler pathHandler; + private final XboxController xboxController = new XboxController(1); private final Joystick driveJoystick = new Joystick(0); private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick); @@ -155,6 +163,8 @@ public RobotContainer() { tagAlignSubsystem, visionSubsystem); + pathHandler = new PathHandler(driveSubsystem, tagAlignSubsystem, robotStateSubsystem); + driveSubsystem.setRobotStateSubsystem(robotStateSubsystem); configureTelemetry(); @@ -427,6 +437,43 @@ public void configurePitDashboard() { .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem)) .withPosition(2, 1) .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add( + "Five Meter Path", + new DriveAutonCommand(driveSubsystem, "FiveMeterTestPath", true, true, false)) + .withPosition(2, 0) + .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add("Turn Off Vision Updates", new SetVisionUpdatesCommand(visionSubsystem, false)) + .withPosition(0, 0) + .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add("Turn On Vision Updates", new SetVisionUpdatesCommand(visionSubsystem, true)) + .withPosition(1, 0) + .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add( + "Start Auton", + new NonProcessorShallowAutonCommand( + driveSubsystem, + pathHandler, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + "startToJ", + PathHandlerConstants.pathNames, + List.of('K', 'L', 'M'), + List.of(4, 4, 4), + 'J')) + .withPosition(3, 0) + .withSize(1, 1); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java b/src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java deleted file mode 100644 index 8d9ba365..00000000 --- a/src/main/java/frc/robot/commands/PathHandler/KillPathHandlerCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.commands.PathHandler; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pathHandler.PathHandler; - -public class KillPathHandlerCommand extends Command { - - PathHandler pathHandler; - - public KillPathHandlerCommand (PathHandler pathHandler) { - this.pathHandler = pathHandler; - } - - @Override - public void execute() { - pathHandler.killPathHandler(); - } -} diff --git a/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java deleted file mode 100644 index bd004a8c..00000000 --- a/src/main/java/frc/robot/commands/PathHandler/StartPathHandlerCommand.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands.PathHandler; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.pathHandler.PathHandler; - -public class StartPathHandlerCommand extends Command { - - PathHandler pathHandler; - - public StartPathHandlerCommand (PathHandler pathHandler) { - this.pathHandler = pathHandler; - } - - @Override - public void execute() { - pathHandler.startPathHandler(); - } - - @Override - public boolean isFinished() { - return pathHandler.isFinished(); - } -} diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java index bad4148c..66355e26 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java @@ -1,11 +1,12 @@ package frc.robot.commands.auton; -import java.util.List; - +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.PathHandler.KillPathHandlerCommand; -import frc.robot.commands.PathHandler.StartPathHandlerCommand; import frc.robot.commands.drive.DriveAutonCommand; +import frc.robot.commands.drive.ResetGyroCommand; +import frc.robot.commands.drive.SetGyroOffsetCommand; +import frc.robot.commands.elevator.ZeroElevatorCommand; import frc.robot.subsystems.algae.AlgaeSubsystem; import frc.robot.subsystems.biscuit.BiscuitSubsystem; import frc.robot.subsystems.coral.CoralSubsystem; @@ -13,50 +14,49 @@ import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.pathHandler.PathHandler; import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import java.util.List; public class NonProcessorShallowAutonCommand extends SequentialCommandGroup { - private PathHandler pathHandler; - private DriveSubsystem driveSubsystem; - private DriveAutonCommand startPath; + private PathHandler pathHandler; + private DriveSubsystem driveSubsystem; + private DriveAutonCommand startPath; + + public NonProcessorShallowAutonCommand( + DriveSubsystem driveSubsystem, + PathHandler pathHandler, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + TagAlignSubsystem tagAlignSubsystem, + String startPathName, + String[][] pathNames, + List NodeNames, + List NodeLevels, + char startNode) { + addRequirements( + driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.pathHandler = pathHandler; + this.driveSubsystem = driveSubsystem; - public NonProcessorShallowAutonCommand ( - DriveSubsystem driveSubsystem, - RobotStateSubsystem robotStateSubsystem, - AlgaeSubsystem algaeSubsystem, - BiscuitSubsystem biscuitSubsystem, - CoralSubsystem coralSubsystem, - ElevatorSubsystem elevatorSubsystem, - PathHandler pathHandler, - String startPathName, - String[][] pathNames, - List NodeNames, - List NodeLevels, - boolean mirrorToProcessor - ) { - // addRequirements(driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); - this.pathHandler = pathHandler; - this.driveSubsystem = driveSubsystem; - this.pathHandler.setPathNames(pathNames); - this.pathHandler.setStartNode(NodeNames.remove(0)); - this.pathHandler.setNodeNames(NodeNames); - this.pathHandler.setNodeLevels(NodeLevels); - this.pathHandler.setMirrorToProcessor(mirrorToProcessor); - - startPath = new DriveAutonCommand(driveSubsystem, startPathName, false, true, mirrorToProcessor); + startPath = new DriveAutonCommand(driveSubsystem, startPathName, false, true, false); - addCommands( - new SequentialCommandGroup( - // TODO fill in all zeroing and such - startPath, - new StartPathHandlerCommand(pathHandler), - new KillPathHandlerCommand(pathHandler) - ) - ); - } + addCommands( + new SequentialCommandGroup( + new ParallelCommandGroup( + new ZeroElevatorCommand(elevatorSubsystem), + new SequentialCommandGroup( + new ResetGyroCommand(driveSubsystem), + new SetGyroOffsetCommand(driveSubsystem, new Rotation2d(180)))), + startPath, + new frc.robot.commands.pathhHandler.StartPathHandlerCommand( + pathHandler, pathNames, NodeNames, NodeLevels, startNode, false))); + } - public void reassignAlliance() { - startPath.reassignAlliance(); - } - + public void reassignAlliance() { + startPath.reassignAlliance(); + } } diff --git a/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java b/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java new file mode 100644 index 00000000..9725cac4 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java @@ -0,0 +1,21 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.drive.DriveSubsystem; + +public class SetGyroOffsetCommand extends InstantCommand { + private DriveSubsystem driveSubsystem; + private Rotation2d offset; + + public SetGyroOffsetCommand(DriveSubsystem driveSubsystem, Rotation2d offset) { + this.driveSubsystem = driveSubsystem; + this.offset = offset; + addRequirements(driveSubsystem); + } + + @Override + public void initialize() { + driveSubsystem.setGyroOffset(offset); + } +} diff --git a/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java new file mode 100644 index 00000000..254f2a36 --- /dev/null +++ b/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.pathhHandler; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.pathHandler.PathHandler; + +public class KillPathHandlerCommand extends InstantCommand { + + PathHandler pathHandler; + + public KillPathHandlerCommand(PathHandler pathHandler) { + this.pathHandler = pathHandler; + } + + @Override + public void initialize() { + pathHandler.killPathHandler(); + } +} diff --git a/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java new file mode 100644 index 00000000..a60903c9 --- /dev/null +++ b/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java @@ -0,0 +1,38 @@ +package frc.robot.commands.pathhHandler; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.pathHandler.PathHandler; +import java.util.List; + +public class SetPathHandlerCommand extends InstantCommand { + private PathHandler pathHandler; + private String[][] pathNames; + private List NodeNames; + private List NodeLevels; + private Character startNode; + private boolean mirrorToProcessor; + + public SetPathHandlerCommand( + PathHandler pathHandler, + String[][] pathNames, + List NodeNames, + List NodeLevels, + Character startNode, + boolean mirrorToProcessor) { + this.pathHandler = pathHandler; + this.pathNames = pathNames; + this.NodeNames = NodeNames; + this.NodeLevels = NodeLevels; + this.startNode = startNode; + this.mirrorToProcessor = mirrorToProcessor; + } + + @Override + public void initialize() { + pathHandler.setPathNames(pathNames); + pathHandler.setNodeNames(NodeNames); + pathHandler.setNodeLevels(NodeLevels); + pathHandler.setStartNode(startNode); + pathHandler.setMirrorToProcessor(mirrorToProcessor); + } +} diff --git a/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java new file mode 100644 index 00000000..3465ca72 --- /dev/null +++ b/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java @@ -0,0 +1,50 @@ +package frc.robot.commands.pathhHandler; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.pathHandler.PathHandler; +import java.util.List; + +public class StartPathHandlerCommand extends Command { + private PathHandler pathHandler; + private String[][] pathNames; + private List NodeNames; + private List NodeLevels; + private Character startNode; + private boolean mirrorToProcessor; + + public StartPathHandlerCommand( + PathHandler pathHandler, + String[][] pathNames, + List NodeNames, + List NodeLevels, + Character startNode, + boolean mirrorToProcessor) { + this.pathHandler = pathHandler; + this.pathNames = pathNames; + this.NodeNames = NodeNames; + this.NodeLevels = NodeLevels; + this.startNode = startNode; + this.mirrorToProcessor = mirrorToProcessor; + } + + public StartPathHandlerCommand(PathHandler pathHandler) { + this.pathHandler = pathHandler; + } + + @Override + public void initialize() { + if (pathNames != null) { + pathHandler.setPathNames(pathNames); + pathHandler.setNodeNames(NodeNames); + pathHandler.setNodeLevels(NodeLevels); + pathHandler.setStartNode(startNode); + pathHandler.setMirrorToProcessor(mirrorToProcessor); + } + pathHandler.startPathHandler(); + } + + @Override + public boolean isFinished() { + return pathHandler.isFinished(); + } +} diff --git a/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java b/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java new file mode 100644 index 00000000..efd85558 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.vision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.vision.VisionSubsystem; + +public class SetVisionUpdatesCommand extends InstantCommand { + VisionSubsystem visionSubsystem; + private boolean enable; + + public SetVisionUpdatesCommand(VisionSubsystem visionSubsystem, boolean enable) { + this.visionSubsystem = visionSubsystem; + this.enable = enable; + } + + @Override + public void initialize() { + visionSubsystem.setVisionUpdating(enable); + } +} diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 7aa4e9a8..dc3a14e8 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -167,6 +167,7 @@ public static TalonFXConfiguration getDriveTalonConfig() { CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); currentConfig.SupplyCurrentLimit = 60; + currentConfig.SupplyCurrentLowerLimit = 60; currentConfig.StatorCurrentLimit = 140; @@ -176,10 +177,13 @@ public static TalonFXConfiguration getDriveTalonConfig() { driveConfig.CurrentLimits = currentConfig; Slot0Configs slot0Config = new Slot0Configs(); - slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate - slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate - slot0Config.kD = 0.0; - slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate + slot0Config.kP = 0.200; // 0.16 using phoenix 6 migrate + slot0Config.kI = 0.000; // 0.0002 using phoenix 6 migrate + slot0Config.kD = 0.000; + slot0Config.kV = 0.117; // 0.047 using phoenix 6 migrate + slot0Config.kS = 0.000; + slot0Config.kA = 0.000; + slot0Config.kG = 0.000; driveConfig.Slot0 = slot0Config; MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); diff --git a/src/main/java/frc/robot/constants/PathHandlerConstants.java b/src/main/java/frc/robot/constants/PathHandlerConstants.java new file mode 100644 index 00000000..c4c178ed --- /dev/null +++ b/src/main/java/frc/robot/constants/PathHandlerConstants.java @@ -0,0 +1,8 @@ +package frc.robot.constants; + +public class PathHandlerConstants { + public static final String[][] pathNames = { + {"", "", "", "", "", "", "", "", "", "", "", ""}, + {"", "", "", "", "", "", "", "", "", "", "", ""} + }; +} From 8f516bc5c87369c0202b03342210b04aa9504650 Mon Sep 17 00:00:00 2001 From: David Shen Date: Mon, 24 Feb 2025 20:16:37 -0500 Subject: [PATCH 26/48] drive tuning --- .../java/frc/robot/constants/AlgaeConstants.java | 2 +- .../java/frc/robot/constants/DriveConstants.java | 12 ++++++++---- .../frc/robot/constants/TagServoingConstants.java | 2 +- .../java/frc/robot/subsystems/drive/Swerve.java | 13 +++---------- .../java/frc/robot/subsystems/drive/SwerveIO.java | 1 + .../subsystems/tagAlign/TagAlignSubsystem.java | 6 +++--- 6 files changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index c4182deb..05c76dfe 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -32,7 +32,7 @@ public class AlgaeConstants { public static final double kIntakingSpeed = -1; public static final double kHasAlgaeVelThreshold = 10; - public static final double kHasAlgaeCounts = 3; + public static final double kHasAlgaeCounts = 2; // Example Talon FX Config public static TalonFXSConfiguration getFXConfig() { diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 9c54826b..69b1652c 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -166,7 +166,8 @@ public static TalonFXConfiguration getDriveTalonConfig() { CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); currentConfig.SupplyCurrentLimit = 60; - + currentConfig.SupplyCurrentLowerLimit = 60; + currentConfig.SupplyCurrentLowerTime = 1.0; currentConfig.StatorCurrentLimit = 140; currentConfig.SupplyCurrentLimitEnable = true; @@ -175,10 +176,13 @@ public static TalonFXConfiguration getDriveTalonConfig() { driveConfig.CurrentLimits = currentConfig; Slot0Configs slot0Config = new Slot0Configs(); - slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate - slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate + slot0Config.kP = 0.2; + slot0Config.kI = 0.0; slot0Config.kD = 0.0; - slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate + slot0Config.kV = 0.117; + slot0Config.kS = 0.0; + slot0Config.kA = 0.0; + slot0Config.kG = 0.0; driveConfig.Slot0 = slot0Config; MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 5c3a09e1..1db91bf7 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -54,7 +54,7 @@ public class TagServoingConstants { public static final double kAlgaeStopXDriveRadius = kAlgaeInitialDriveRadius; // Should be closer to reef than target pose public static final double kCoralDriveCloseEnough = 0.1; - public static final double kAlgaeDriveCloseEnough = 0.2; + public static final double kAlgaeDriveCloseEnough = 0.1; public static final double kMinVelX = 0.85; // Reef diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index afb82abb..a849f4f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -161,15 +161,7 @@ private ChassisSpeeds getRobotRelSpeed() { return kinematics.toChassisSpeeds(swerveModuleStates); } - private ChassisSpeeds getFieldRelSpeed() { - // SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); - // SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); - SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; ++i) { - swerveModuleStates[i] = swerveModules[i].getState(); - } - ChassisSpeeds roboRelSpeed = kinematics.toChassisSpeeds(swerveModuleStates); - + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { Rotation2d heading = swerveDrive.getHeading().unaryMinus(); fieldX = roboRelSpeed.vxMetersPerSecond * heading.getCos() @@ -268,7 +260,8 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity(); inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); } - inputs.fieldRelSpeed = getFieldRelSpeed(); + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; inputs.fieldX = fieldX; } diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index f8cd0b5c..7f674d4d 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -35,6 +35,7 @@ public static class SwerveIOInputs { public double pigeonTemp = 0; public double fieldX = 0; public double fieldY = 0; + public ChassisSpeeds robotRelSpeed = new ChassisSpeeds(); public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds(); public double[] azimuthVels = {0, 0, 0, 0}; public double[] azimuthCurrent = {0, 0, 0, 0}; diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 3129baba..9391d3b0 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -62,7 +62,7 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu new ProfiledPIDController(0.0017, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 this.alignY = new ProfiledPIDController(0.00195, 0, 0, TagServoingConstants.alignYConstraints); this.alignOmega = - new ProfiledPIDController(6, 0, 0, TagServoingConstants.alignOmegaConstraints); + new ProfiledPIDController(5.5, 0, 0, TagServoingConstants.alignOmegaConstraints); this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); @@ -357,8 +357,8 @@ public void periodic() { double vX = -alignX.calculate(goalTargetDiag - diag, 0); double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); - Logger.recordOutput("TagAlignSubsystem/Tag Align X Error", alignX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/Tag Align Y Error", alignY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Diag Error", alignX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Horizontal Error", alignY.getPositionError()); Logger.recordOutput( "TagAlignSubsystem/Last result delay", (RobotController.getFPGATime() - result.getTimeStamp()) / 1000); From 867578bc84a83d315dbe1e91aa57c779f25b46da Mon Sep 17 00:00:00 2001 From: David Shen Date: Mon, 24 Feb 2025 21:02:34 -0500 Subject: [PATCH 27/48] defer processor stowing --- .../robot/constants/RobotStateConstants.java | 2 ++ .../robotState/RobotStateSubsystem.java | 26 ++++++++++++++----- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index cd9acb71..fa3cd957 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -9,4 +9,6 @@ public class RobotStateConstants { public static final double kCoralEjectTimer = 0.75; public static final double kAlgaeEjectTimer = 0.5; + + public static final double kProcessorStowRadius = 0.5; } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 5c366f64..7e3c3ce1 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -26,6 +26,7 @@ import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; import frc.robot.subsystems.vision.VisionSubsystem; import java.util.Set; +import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; @@ -59,7 +60,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private AlgaeHeight algaeHeight = AlgaeHeight.LOW; private AlgaeHeight currentAlgaeHeight = AlgaeHeight.LOW; private CoralLoc coralLoc = CoralLoc.NONE; - private Pose2d algaeRemovalPose; + private Pose2d processorReleasePose; private boolean isAutoPlacing = false; private boolean getAlgaeOnCycle = false; @@ -497,6 +498,10 @@ public void releaseAlgae() { scoringTimer.reset(); scoringTimer.start(); isEjectingAlgae = true; + + processorReleasePose = driveSubsystem.getPoseMeters(); + Logger.recordOutput("RobotState/Processor Release Pose", processorReleasePose); + switch (algaeHeight) { case LOW -> { algaeSubsystem.scoreProcessor(); @@ -718,11 +723,20 @@ public void periodic() { } } case PROCESSOR_ALGAE -> { - if (!algaeSubsystem.hasAlgae() - && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer) - && isEjectingAlgae) { - isEjectingAlgae = false; - toStowSafe(); + if (isEjectingAlgae + && !algaeSubsystem.hasAlgae() + && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)) { + Pose2d currentPose = driveSubsystem.getPoseMeters(); + double distanceFromRelease = + FastMath.hypot( + currentPose.getX() - processorReleasePose.getX(), + currentPose.getY() - processorReleasePose.getY()); + Logger.recordOutput("RobotState/Processor Release Distance", distanceFromRelease); + + if (distanceFromRelease > RobotStateConstants.kProcessorStowRadius) { + isEjectingAlgae = false; + toStowSafe(); + } } else if (algaeHeight != currentAlgaeHeight) { toScoreAlgae(); } From e5d43077da7db02e4850eaca121cfdb56c737418 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Mon, 24 Feb 2025 21:17:53 -0500 Subject: [PATCH 28/48] fixed everything except pathHandler --- src/main/deploy/choreo/2025-project.chor | 24 +- src/main/deploy/choreo/ATofetch.traj | 73 +++--- src/main/deploy/choreo/BTofetch.traj | 72 +++--- src/main/deploy/choreo/ConfusingTestPath.traj | 240 +++++++++--------- src/main/deploy/choreo/FiveMeterTestPath.traj | 168 +++++++++--- src/main/deploy/choreo/constants.traj | 36 +-- src/main/deploy/choreo/fetchToA.traj | 85 +++---- src/main/java/frc/robot/RobotContainer.java | 45 ++-- .../frc/robot/constants/DriveConstants.java | 8 +- .../robot/constants/PathHandlerConstants.java | 32 ++- .../subsystems/pathHandler/PathHandler.java | 8 +- 11 files changed, 451 insertions(+), 340 deletions(-) diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 604756e8..687a5b84 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -289,20 +289,20 @@ } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"147 lbs", + "val":66.67807839000001 }, "inertia":{ "exp":"6 kg m ^ 2", "val":6.0 }, "gearing":{ - "exp":"6.5", - "val":6.5 + "exp":"7.09", + "val":7.09 }, "radius":{ - "exp":"1.75 in", - "val":0.04445 + "exp":"1.68 in", + "val":0.042671999999999995 }, "vmax":{ "exp":"6000 RPM", @@ -318,16 +318,16 @@ }, "bumper":{ "front":{ - "exp":"17.9375 in", - "val":0.4556125 + "exp":"18.5 in", + "val":0.4699 }, "side":{ - "exp":"17.9375 in", - "val":0.4556125 + "exp":"18.5 in", + "val":0.4699 }, "back":{ - "exp":"17.9375 in", - "val":0.4556125 + "exp":"18.5 in", + "val":0.4699 } }, "differentialTrackWidth":{ diff --git a/src/main/deploy/choreo/ATofetch.traj b/src/main/deploy/choreo/ATofetch.traj index b41d625a..6e452342 100644 --- a/src/main/deploy/choreo/ATofetch.traj +++ b/src/main/deploy/choreo/ATofetch.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,42 +28,41 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.219], + "waypoints":[0.0,1.23474], "samples":[ - {"t":0.0, "x":3.20199, "y":4.42922, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.73119, "ay":8.85668, "alpha":-8.1153, "fx":[-61.82511,-38.03643,-93.76775,-128.27548], "fy":[163.91169,171.12075,148.13917,119.42691]}, - {"t":0.03694, "x":3.19876, "y":4.43527, "heading":0.0, "vx":-0.17477, "vy":0.32716, "omega":-0.29977, "ax":-4.74487, "ay":8.87046, "alpha":-7.80369, "fx":[-63.03167,-39.64934,-93.45619,-126.69852], "fy":[163.42337,170.73313,148.31417,121.06527]}, - {"t":0.07388, "x":3.18907, "y":4.4534, "heading":-0.01107, "vx":-0.35004, "vy":0.65483, "omega":-0.58804, "ax":-4.76138, "ay":8.88754, "alpha":-7.39857, "fx":[-65.14939,-41.42781,-92.68925,-124.69208], "fy":[162.55633,170.28317,148.76733,123.09151]}, - {"t":0.11082, "x":3.17289, "y":4.48365, "heading":-0.0328, "vx":-0.52592, "vy":0.98313, "omega":-0.86133, "ax":-4.78059, "ay":8.90831, "alpha":-6.87925, "fx":[-68.04962,-43.63075,-91.4958,-122.08996], "fy":[161.32124,169.69631,149.46881,125.62509]}, - {"t":0.14776, "x":3.1502, "y":4.52605, "heading":-0.06461, "vx":-0.70251, "vy":1.31219, "omega":-1.11545, "ax":-4.80235, "ay":8.9334, "alpha":-6.2068, "fx":[-71.54797,-46.67642,-89.91693,-118.60513], "fy":[159.74729,168.83532,150.37589,128.8599]}, - {"t":0.1847, "x":3.12097, "y":4.58061, "heading":-0.10582, "vx":-0.87991, "vy":1.64219, "omega":-1.34472, "ax":-4.82628, "ay":8.96371, "alpha":-5.30241, "fx":[-75.3661,-51.2713,-88.01787,-113.71904], "fy":[157.91413,167.42812,151.42639,133.11172]}, - {"t":0.22164, "x":3.08518, "y":4.64739, "heading":-0.15549, "vx":-1.05819, "vy":1.9733, "omega":-1.54059, "ax":-4.85148, "ay":8.9994, "alpha":-3.99599, "fx":[-79.07923,-58.69549,-85.91722,-106.39745], "fy":[155.99918,164.86783,152.52052,138.92116]}, - {"t":0.25857, "x":3.04278, "y":4.72642, "heading":-0.2124, "vx":-1.2374, "vy":2.30573, "omega":-1.6882, "ax":-4.87421, "ay":9.03366, "alpha":-1.89066, "fx":[-82.03267,-71.48461,-83.8767,-94.24165], "fy":[154.34597,159.56704,153.45958,147.26725]}, - {"t":0.29551, "x":2.99374, "y":4.81776, "heading":-0.27476, "vx":-1.41745, "vy":2.63943, "omega":-1.75804, "ax":-4.86921, "ay":9.01547, "alpha":2.04639, "fx":[-83.16754,-95.04219,-82.71332,-70.37236], "fy":[153.56376,146.43334,153.6423,159.76311]}, - {"t":0.33245, "x":2.93806, "y":4.92141, "heading":-0.3397, "vx":-1.59731, "vy":2.97245, "omega":-1.68245, "ax":-4.66322, "ay":8.55992, "alpha":10.69818, "fx":[-80.5656,-138.18653,-88.52894,-9.99874], "fy":[154.67159,105.85661,148.14876,173.73027]}, - {"t":0.36939, "x":2.87588, "y":5.03705, "heading":-0.40185, "vx":-1.76957, "vy":3.28865, "omega":-1.28726, "ax":-4.72223, "ay":7.07048, "alpha":19.31066, "fx":[-74.31686,-157.51151,-132.36738,42.90037], "fy":[156.84566,71.35326,85.65129,167.21746]}, - {"t":0.40633, "x":2.80729, "y":5.16335, "heading":-0.4494, "vx":-1.944, "vy":3.54983, "omega":-0.57394, "ax":-2.77672, "ay":6.01607, "alpha":15.40494, "fx":[-55.13791,-112.28147,-44.12373,22.61807], "fy":[133.94466,77.06963,66.88697,131.42508]}, - {"t":0.44327, "x":2.73358, "y":5.29859, "heading":-0.4706, "vx":-2.04657, "vy":3.77206, "omega":-0.00489, "ax":0.15486, "ay":0.09246, "alpha":0.01693, "fx":[2.61593,2.57865,2.65226,2.68947], "fy":[1.62822,1.55477,1.5173,1.59055]}, - {"t":0.48021, "x":2.65809, "y":5.43799, "heading":-0.47078, "vx":-2.04085, "vy":3.77547, "omega":-0.00427, "ax":-0.00413, "ay":-0.00218, "alpha":-0.00001, "fx":[-0.07024,-0.07022,-0.07026,-0.07028], "fy":[-0.03711,-0.03706,-0.03703,-0.03708]}, - {"t":0.51715, "x":2.5827, "y":5.57745, "heading":-0.47094, "vx":-2.04101, "vy":3.77539, "omega":-0.00427, "ax":-0.04054, "ay":-0.02189, "alpha":-0.00008, "fx":[-0.68956,-0.6894,-0.68973,-0.68989], "fy":[-0.37252,-0.3722,-0.37203,-0.37236]}, - {"t":0.55409, "x":2.50728, "y":5.71689, "heading":-0.47109, "vx":-2.0425, "vy":3.77458, "omega":-0.00427, "ax":-0.05855, "ay":-0.03166, "alpha":-0.00011, "fx":[-0.99577,-0.99552,-0.99601,-0.99627], "fy":[-0.53895,-0.53845,-0.5382,-0.5387]}, - {"t":0.59103, "x":2.43179, "y":5.8563, "heading":-0.47125, "vx":-2.04467, "vy":3.77341, "omega":-0.00428, "ax":-0.08146, "ay":-0.04414, "alpha":-0.00013, "fx":[-1.38544,-1.38515,-1.38573,-1.38602], "fy":[-0.75123,-0.75066,-0.75036,-0.75094]}, - {"t":0.62797, "x":2.35621, "y":5.99566, "heading":-0.47141, "vx":-2.04768, "vy":3.77178, "omega":-0.00428, "ax":-0.11599, "ay":-0.063, "alpha":-0.00004, "fx":[-1.97287,-1.97278,-1.97295,-1.97304], "fy":[-1.07169,-1.07153,-1.07144,-1.07161]}, - {"t":0.66491, "x":2.28049, "y":6.13494, "heading":-0.47157, "vx":-2.05196, "vy":3.76946, "omega":-0.00428, "ax":-0.15542, "ay":-0.08468, "alpha":0.00026, "fx":[-2.64397,-2.64454,-2.64343,-2.64286], "fy":[-1.43951,-1.44062,-1.44119,-1.44008]}, - {"t":0.70185, "x":2.20458, "y":6.27413, "heading":-0.47173, "vx":-2.0577, "vy":3.76633, "omega":-0.00427, "ax":-0.20169, "ay":-0.1104, "alpha":0.00038, "fx":[-3.43115,-3.43199,-3.43032,-3.42949], "fy":[-1.8766,-1.87826,-1.87913,-1.87747]}, - {"t":0.73879, "x":2.12844, "y":6.41318, "heading":-0.47188, "vx":-2.06515, "vy":3.76225, "omega":-0.00426, "ax":-0.42425, "ay":-0.2422, "alpha":-0.01584, "fx":[-7.19948,-7.16474,-7.23321,-7.2681], "fy":[-4.17185,-4.10283,-4.06765,-4.13654]}, - {"t":0.77572, "x":2.05186, "y":6.55199, "heading":-0.47204, "vx":-2.08082, "vy":3.7533, "omega":-0.00484, "ax":2.42798, "ay":-6.09913, "alpha":-15.25704, "fx":[49.39531,107.38305,36.20086,-27.78206], "fy":[-135.07474,-80.99272,-69.32289,-129.58738]}, - {"t":0.81266, "x":1.97666, "y":6.68647, "heading":-0.47222, "vx":-1.99113, "vy":3.52801, "omega":-0.56843, "ax":4.79143, "ay":-7.03497, "alpha":-19.74896, "fx":[69.90442,154.04894,144.61663,-42.56651], "fy":[-158.84246,-78.69798,-73.94321,-167.16792]}, - {"t":0.8496, "x":1.90637, "y":6.81199, "heading":-0.49322, "vx":-1.81414, "vy":3.26814, "omega":-1.29794, "ax":4.79782, "ay":-8.53825, "alpha":-10.30866, "fx":[74.56143,132.70267,105.27683,13.89737], "fy":[-157.64088,-112.7757,-137.13914,-173.37712]}, - {"t":0.88654, "x":1.84263, "y":6.92689, "heading":-0.54116, "vx":-1.63691, "vy":2.95274, "omega":-1.67874, "ax":4.95642, "ay":-8.97232, "alpha":-1.83527, "fx":[81.77851,94.65878,87.41576,73.37626], "fy":[-154.31028,-146.71446,-151.04376,-158.39772]}, - {"t":0.92348, "x":1.78555, "y":7.02984, "heading":-0.60317, "vx":-1.45383, "vy":2.62131, "omega":-1.74653, "ax":4.96524, "ay":-8.98518, "alpha":1.84246, "fx":[88.28539,73.63434,81.28272,94.62692], "fy":[-150.86902,-158.57259,-154.85211,-147.04777]}, - {"t":0.96042, "x":1.73523, "y":7.12054, "heading":-0.66769, "vx":-1.27042, "vy":2.28941, "omega":-1.67847, "ax":4.95042, "ay":-8.95097, "alpha":3.85851, "fx":[94.57647,61.27014,76.93089,104.04332], "fy":[-147.14107,-163.89023,-157.24191,-140.74054]}, - {"t":0.99736, "x":1.69168, "y":7.199, "heading":-0.72969, "vx":-1.08755, "vy":1.95876, "omega":-1.53594, "ax":4.9354, "ay":-8.91238, "alpha":5.16512, "fx":[100.42937,53.37411,73.25806,108.73715], "fy":[-143.31253,-166.71815,-159.08733,-137.27012]}, - {"t":1.0343, "x":1.65488, "y":7.26528, "heading":-0.78643, "vx":-0.90524, "vy":1.62955, "omega":-1.34514, "ax":4.92233, "ay":-8.87675, "alpha":6.10711, "fx":[105.62632,48.0466,70.04587,111.19113], "fy":[-139.60761,-168.39163,-160.59155,-135.37345]}, - {"t":1.07124, "x":1.62479, "y":7.31941, "heading":-0.83612, "vx":-0.72341, "vy":1.30165, "omega":-1.11955, "ax":4.91067, "ay":-8.84556, "alpha":6.82851, "fx":[110.06004,44.31143,67.25388,112.49122], "fy":[-136.20544,-169.45728,-161.82479,-134.35447]}, - {"t":1.10818, "x":1.60142, "y":7.36146, "heading":-0.87747, "vx":-0.54202, "vy":0.9749, "omega":-0.86731, "ax":4.90007, "ay":-8.81886, "alpha":7.39835, "fx":[113.71752,41.59335,64.88761,113.19658], "fy":[-133.22103,-170.17906,-162.82034,-133.80478]}, - {"t":1.14512, "x":1.58474, "y":7.39146, "heading":-0.90951, "vx":-0.36101, "vy":0.64914, "omega":-0.59402, "ax":4.89059, "ay":-8.79619, "alpha":7.85551, "fx":[116.64195,39.51266,62.9645,113.63116], "fy":[-130.71271,-170.70172,-163.5984,-133.46964]}, - {"t":1.18206, "x":1.57475, "y":7.40943, "heading":-0.93145, "vx":-0.18036, "vy":0.32421, "omega":-0.30384, "ax":4.88253, "ay":-8.77686, "alpha":8.22552, "fx":[118.89851,37.80072,61.504,113.99853], "fy":[-128.70033,-171.11198,-164.17273,-133.18249]}, - {"t":1.219, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.20199, "y":4.42922, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.1104, "ay":9.94264, "alpha":-14.60769, "fx":[-42.24185,-17.99333,-111.2651,-169.25169], "fy":[194.39532,198.37567,165.25578,104.92931]}, + {"t":0.03859, "x":3.19818, "y":4.43662, "heading":0.0, "vx":-0.19719, "vy":0.38364, "omega":-0.56365, "ax":-5.20371, "ay":10.00203, "alpha":-13.64056, "fx":[-48.58569,-22.18043,-110.55426,-165.65302], "fy":[192.85818,197.91544,165.69541,110.44709]}, + {"t":0.07717, "x":3.1867, "y":4.45887, "heading":-0.02175, "vx":-0.39798, "vy":0.76958, "omega":-1.08997, "ax":-5.32263, "ay":10.08618, "alpha":-12.18478, "fx":[-59.10002,-27.42791,-108.4034,-159.97171], "fy":[189.83515,197.20269,167.05696,118.43218]}, + {"t":0.11576, "x":3.16738, "y":4.49608, "heading":-0.06381, "vx":-0.60335, "vy":1.15876, "omega":-1.56013, "ax":-5.45727, "ay":10.19722, "alpha":-10.10839, "fx":[-72.23535,-35.99472,-104.91713,-150.73334], "fy":[185.15409,195.7329,169.18255,129.86122]}, + {"t":0.15434, "x":3.14004, "y":4.54838, "heading":-0.124, "vx":-0.81392, "vy":1.55222, "omega":-1.95017, "ax":-5.59167, "ay":10.34375, "alpha":-6.85109, "fx":[-85.35885,-53.22437,-100.27529,-133.98298], "fy":[179.3829,191.60773,171.81349,146.89736]}, + {"t":0.19293, "x":3.10447, "y":4.61597, "heading":-0.19925, "vx":-1.02968, "vy":1.95134, "omega":-2.21452, "ax":-5.68624, "ay":10.46624, "alpha":-0.2355, "fx":[-94.65877,-93.37274,-94.91599,-96.19998], "fy":[174.53289,175.23259,174.40834,173.69521]}, + {"t":0.23151, "x":3.06051, "y":4.69906, "heading":-0.2847, "vx":-1.24909, "vy":2.35519, "omega":-2.22361, "ax":-5.2182, "ay":9.50653, "alpha":16.16849, "fx":[-95.3511,-175.32318,-93.94638,16.68121], "fy":[173.99706,92.09107,170.45618,197.33277]}, + {"t":0.2701, "x":3.00843, "y":4.79701, "heading":-0.3705, "vx":-1.45044, "vy":2.722, "omega":-1.59974, "ax":-5.50687, "ay":8.10975, "alpha":22.27664, "fx":[-87.22729,-181.93966,-148.24416,50.22338], "fy":[177.28819,75.40381,97.82061,190.2299]}, + {"t":0.30868, "x":2.94836, "y":4.90808, "heading":-0.43223, "vx":-1.66292, "vy":3.03492, "omega":-0.74018, "ax":-3.49709, "ay":7.41616, "alpha":18.99973, "fx":[-67.5423,-140.51181,-57.21851,32.09361], "fy":[160.41845,88.79629,84.43353,160.84733]}, + {"t":0.34727, "x":2.88159, "y":5.0307, "heading":-0.46079, "vx":-1.79786, "vy":3.32108, "omega":-0.00707, "ax":0.14496, "ay":0.08841, "alpha":0.02105, "fx":[2.39327,2.34771,2.43954,2.4851], "fy":[1.54242,1.45059,1.40498,1.4968]}, + {"t":0.38586, "x":2.81233, "y":5.15891, "heading":-0.46106, "vx":-1.79226, "vy":3.32449, "omega":-0.00625, "ax":-0.00294, "ay":-0.00153, "alpha":0.00008, "fx":[-0.04904,-0.04921,-0.04886,-0.04868], "fy":[-0.02519,-0.02555,-0.02572,-0.02537]}, + {"t":0.42444, "x":2.74317, "y":5.28719, "heading":-0.4613, "vx":-1.79238, "vy":3.32443, "omega":-0.00625, "ax":-0.02216, "ay":-0.01191, "alpha":0.00005, "fx":[-0.36952,-0.36963,-0.36941,-0.3693], "fy":[-0.19829,-0.19852,-0.19863,-0.1984]}, + {"t":0.46303, "x":2.674, "y":5.41545, "heading":-0.46154, "vx":-1.79323, "vy":3.32397, "omega":-0.00625, "ax":-0.02747, "ay":-0.01478, "alpha":0.00002, "fx":[-0.45798,-0.45803,-0.45792,-0.45787], "fy":[-0.24637,-0.24648,-0.24654,-0.24643]}, + {"t":0.50161, "x":2.60478, "y":5.5437, "heading":-0.46178, "vx":-1.79429, "vy":3.3234, "omega":-0.00625, "ax":-0.03225, "ay":-0.01739, "alpha":-0.00001, "fx":[-0.53754,-0.53751,-0.53756,-0.53759], "fy":[-0.28985,-0.28979,-0.28976,-0.28982]}, + {"t":0.5402, "x":2.53552, "y":5.67192, "heading":-0.46202, "vx":-1.79554, "vy":3.32273, "omega":-0.00625, "ax":-0.0391, "ay":-0.02112, "alpha":-0.00006, "fx":[-0.65172,-0.65159,-0.65185,-0.65197], "fy":[-0.35222,-0.35196,-0.35184,-0.35209]}, + {"t":0.57878, "x":2.46621, "y":5.80012, "heading":-0.46227, "vx":-1.79705, "vy":3.32191, "omega":-0.00625, "ax":-0.05097, "ay":-0.02757, "alpha":-0.00009, "fx":[-0.8495,-0.8493,-0.84969,-0.84989], "fy":[-0.45993,-0.45954,-0.45935,-0.45974]}, + {"t":0.61737, "x":2.39684, "y":5.92827, "heading":-0.46251, "vx":-1.79901, "vy":3.32085, "omega":-0.00625, "ax":-0.07217, "ay":-0.03912, "alpha":-0.00009, "fx":[-1.20302,-1.20283,-1.20322,-1.20341], "fy":[-0.65236,-0.65197,-0.65177,-0.65216]}, + {"t":0.65595, "x":2.32737, "y":6.05638, "heading":-0.46275, "vx":-1.8018, "vy":3.31934, "omega":-0.00626, "ax":-0.1044, "ay":-0.0567, "alpha":0.00004, "fx":[-1.74043,-1.74052,-1.74034,-1.74025], "fy":[-0.94498,-0.94516,-0.94525,-0.94507]}, + {"t":0.69454, "x":2.25776, "y":6.18442, "heading":-0.46299, "vx":-1.80583, "vy":3.31715, "omega":-0.00626, "ax":-0.12847, "ay":-0.06995, "alpha":0.00029, "fx":[-2.14191,-2.14255,-2.14126,-2.14062], "fy":[-1.16514,-1.16643,-1.16707,-1.16578]}, + {"t":0.73313, "x":2.18799, "y":6.31236, "heading":-0.46323, "vx":-1.81078, "vy":3.31445, "omega":-0.00624, "ax":-0.118, "ay":-0.0645, "alpha":0.0003, "fx":[-1.96741,-1.96806,-1.96676,-1.96611], "fy":[-1.07418,-1.07548,-1.07613,-1.07483]}, + {"t":0.77171, "x":2.11803, "y":6.4402, "heading":-0.46347, "vx":-1.81534, "vy":3.31196, "omega":-0.00623, "ax":-0.09147, "ay":-0.05018, "alpha":0.00015, "fx":[-1.52487,-1.5252,-1.52454,-1.52421], "fy":[-0.83599,-0.83666,-0.83699,-0.83633]}, + {"t":0.8103, "x":2.04792, "y":6.56796, "heading":-0.46371, "vx":-1.81887, "vy":3.31003, "omega":-0.00623, "ax":-0.11297, "ay":-0.0622, "alpha":0.00001, "fx":[-1.88319,-1.88321,-1.88318,-1.88317], "fy":[-1.03686,-1.03689,-1.0369,-1.03688]}, + {"t":0.84888, "x":1.97765, "y":6.69563, "heading":-0.46395, "vx":-1.82322, "vy":3.30763, "omega":-0.00623, "ax":-0.50308, "ay":-0.29048, "alpha":-0.02349, "fx":[-8.36043,-8.30952,-8.41179,-8.46271], "fy":[-4.91906,-4.81674,-4.76528,-4.8676]}, + {"t":0.88747, "x":1.90693, "y":6.82304, "heading":-0.46419, "vx":-1.84264, "vy":3.29642, "omega":-0.00713, "ax":2.81579, "ay":-7.86084, "alpha":-19.26499, "fx":[57.64456,136.69488,39.35405,-45.94221], "fy":[-166.86368,-98.73185,-96.8424,-161.70804]}, + {"t":0.92605, "x":1.83792, "y":6.94438, "heading":-0.46447, "vx":-1.73399, "vy":2.9931, "omega":-0.75048, "ax":5.55519, "ay":-8.09391, "alpha":-22.93539, "fx":[79.14007,176.34983,167.02885,-52.10911], "fy":[-181.04207,-87.90772,-81.13423,-189.60213]}, + {"t":0.96464, "x":1.77515, "y":7.05385, "heading":-0.49343, "vx":-1.51964, "vy":2.6808, "omega":-1.63546, "ax":5.60395, "ay":-9.25449, "alpha":-16.86485, "fx":[82.0384,166.46701,144.4783,-19.32281], "fy":[-180.65175,-107.50149,-131.96447,-196.95423]}, + {"t":1.00322, "x":1.72069, "y":7.1504, "heading":-0.55653, "vx":-1.30341, "vy":2.32371, "omega":-2.2862, "ax":5.78598, "ay":-10.41253, "alpha":0.1264, "fx":[96.65738,95.719,96.24418,97.17754], "fy":[-173.45341,-173.9758,-173.69079,-173.16756]}, + {"t":1.04181, "x":1.6747, "y":7.23231, "heading":-0.64475, "vx":-1.08015, "vy":1.92193, "omega":-2.28132, "ax":5.7227, "ay":-10.29298, "alpha":6.51348, "fx":[113.47222,55.46401,84.79935,127.84293], "fy":[-163.10487,-190.90811,-179.95081,-152.35226]}, + {"t":1.0804, "x":1.63729, "y":7.29881, "heading":-0.73277, "vx":-0.85934, "vy":1.52477, "omega":-2.02999, "ax":5.65696, "ay":-10.10726, "alpha":10.03161, "fx":[130.2671,32.8705,76.37848,137.67895], "fy":[-150.17796,-196.19118,-183.83113,-143.73236]}, + {"t":1.11898, "x":1.60834, "y":7.35012, "heading":-0.8111, "vx":-0.64106, "vy":1.13478, "omega":-1.64292, "ax":5.59851, "ay":-9.93438, "alpha":12.50397, "fx":[144.10396,18.77424,69.35483,141.06515], "fy":[-137.09263,-198.11273,-186.67108,-140.52897]}, + {"t":1.15757, "x":1.58777, "y":7.38651, "heading":-0.87449, "vx":-0.42504, "vy":0.75146, "omega":-1.16045, "ax":5.53715, "ay":-9.79247, "alpha":14.35249, "fx":[154.14279,9.3132,63.64191,142.10828], "fy":[-125.81969,-198.83282,-188.74217,-139.54849]}, + {"t":1.19615, "x":1.57549, "y":7.40821, "heading":-0.91927, "vx":-0.21139, "vy":0.37361, "omega":-0.60665, "ax":5.47836, "ay":-9.68262, "alpha":15.72215, "fx":[160.91625,2.55465,59.36821,142.44739], "fy":[-117.13166,-199.07247,-190.16165,-139.25245]}, + {"t":1.23474, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/BTofetch.traj b/src/main/deploy/choreo/BTofetch.traj index bc613b50..159372cf 100644 --- a/src/main/deploy/choreo/BTofetch.traj +++ b/src/main/deploy/choreo/BTofetch.traj @@ -28,43 +28,43 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.28721], + "waypoints":[0.0,1.31206], "samples":[ - {"t":0.0, "x":3.20199, "y":4.10055, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.33801, "ay":9.04682, "alpha":-8.26482, "fx":[-51.42029,-31.44463,-88.92083,-123.3676], "fy":[167.47731,172.46079,151.10306,124.49401]}, - {"t":0.03786, "x":3.19888, "y":4.10703, "heading":0.0, "vx":-0.16423, "vy":0.34251, "omega":-0.3129, "ax":-4.35296, "ay":9.06367, "alpha":-7.90764, "fx":[-52.96269,-33.18757,-88.5125,-121.50786], "fy":[166.96928,172.11328,151.32141,126.278]}, - {"t":0.07572, "x":3.18954, "y":4.12649, "heading":-0.01185, "vx":-0.32903, "vy":0.68565, "omega":-0.61228, "ax":-4.37069, "ay":9.08453, "alpha":-7.44323, "fx":[-55.52093,-35.11015,-87.58852,-119.15682], "fy":[166.10339,171.70477,151.83132,128.46139]}, - {"t":0.11358, "x":3.17395, "y":4.15896, "heading":-0.03503, "vx":-0.4945, "vy":1.02958, "omega":-0.89407, "ax":-4.39105, "ay":9.10966, "alpha":-6.8482, "fx":[-58.95761,-37.51454,-86.17827,-116.11171], "fy":[164.87442,171.16024,152.60124,131.17471]}, - {"t":0.15144, "x":3.15208, "y":4.20447, "heading":-0.06888, "vx":-0.66075, "vy":1.37447, "omega":-1.15334, "ax":-4.41376, "ay":9.13957, "alpha":-6.0776, "fx":[-63.07226,-40.89319,-84.32149,-112.02037], "fy":[163.29415,170.3363,153.58733,134.62785]}, - {"t":0.1893, "x":3.12391, "y":4.26305, "heading":-0.11254, "vx":-0.82785, "vy":1.72048, "omega":-1.38343, "ax":-4.43811, "ay":9.17487, "alpha":-5.03884, "fx":[-67.55561,-46.08804,-82.07789,-106.24235], "fy":[161.42699,168.93552,154.72867,139.15628]}, - {"t":0.22716, "x":3.08938, "y":4.33477, "heading":-0.16492, "vx":-0.99587, "vy":2.06783, "omega":-1.5742, "ax":-4.46244, "ay":9.21455, "alpha":-3.52842, "fx":[-71.93287,-54.64758,-79.54958,-97.48903], "fy":[159.44152,166.25832,155.93326,145.31459]}, - {"t":0.26501, "x":3.04848, "y":4.41966, "heading":-0.22451, "vx":-1.16481, "vy":2.41669, "omega":-1.70778, "ax":-4.48064, "ay":9.24607, "alpha":-1.06435, "fx":[-75.48408,-69.68031,-76.94924,-82.74367], "fy":[157.67931,160.36229,157.02825,154.02231]}, - {"t":0.30287, "x":3.00117, "y":4.51778, "heading":-0.28917, "vx":-1.33445, "vy":2.76674, "omega":-1.74808, "ax":-4.45505, "ay":9.18727, "alpha":3.60173, "fx":[-77.09335,-97.71059,-74.93098,-53.38142], "fy":[156.73917,144.65268,157.48863,166.21115]}, - {"t":0.34073, "x":2.94746, "y":4.62911, "heading":-0.35535, "vx":-1.50311, "vy":3.11456, "omega":-1.61172, "ax":-4.15854, "ay":8.46896, "alpha":13.50644, "fx":[-74.88593,-146.29681,-80.17454,18.41469], "fy":[157.55672,94.33785,151.25295,173.07114]}, - {"t":0.37859, "x":2.88757, "y":4.75309, "heading":-0.41637, "vx":-1.66055, "vy":3.43519, "omega":-1.10037, "ax":-4.33476, "ay":7.26538, "alpha":19.31468, "fx":[-69.36817,-155.99777,-120.1531,50.58664], "fy":[159.06678,74.43104,95.77531,165.05531]}, - {"t":0.41645, "x":2.8216, "y":4.88835, "heading":-0.45803, "vx":-1.82466, "vy":3.71025, "omega":-0.36914, "ax":-1.38538, "ay":3.94099, "alpha":9.6305, "fx":[-32.06436,-60.12958,-14.20937,12.14349], "fy":[92.30599,56.05123,38.9486,80.83489]}, - {"t":0.45431, "x":2.75152, "y":5.03164, "heading":-0.472, "vx":-1.87711, "vy":3.85945, "omega":-0.00453, "ax":0.07253, "ay":0.03773, "alpha":0.00475, "fx":[1.22866,1.21815,1.23873,1.24924], "fy":[0.65737,0.63679,0.62627,0.64686]}, - {"t":0.49217, "x":2.68051, "y":5.17779, "heading":-0.47217, "vx":-1.87437, "vy":3.86088, "omega":-0.00435, "ax":-0.01524, "ay":-0.00734, "alpha":0.00005, "fx":[-0.25931,-0.25943,-0.2592,-0.25908], "fy":[-0.12472,-0.12495,-0.12507,-0.12484]}, - {"t":0.53003, "x":2.60954, "y":5.32395, "heading":-0.47234, "vx":-1.87494, "vy":3.8606, "omega":-0.00435, "ax":-0.03549, "ay":-0.01719, "alpha":0.00002, "fx":[-0.60366,-0.60369,-0.60363,-0.6036], "fy":[-0.29234,-0.2924,-0.29244,-0.29237]}, - {"t":0.56789, "x":2.53853, "y":5.4701, "heading":-0.4725, "vx":-1.87629, "vy":3.85995, "omega":-0.00435, "ax":-0.04562, "ay":-0.02214, "alpha":-0.00004, "fx":[-0.7759,-0.77581,-0.77599,-0.77608], "fy":[-0.37673,-0.37655,-0.37646,-0.37664]}, - {"t":0.60575, "x":2.46746, "y":5.61622, "heading":-0.47267, "vx":-1.87801, "vy":3.85912, "omega":-0.00435, "ax":-0.05823, "ay":-0.02832, "alpha":-0.00013, "fx":[-0.99027,-0.98999,-0.99054,-0.99082], "fy":[-0.48212,-0.48156,-0.48128,-0.48183]}, - {"t":0.64361, "x":2.39632, "y":5.7623, "heading":-0.47283, "vx":-1.88022, "vy":3.85804, "omega":-0.00436, "ax":-0.08276, "ay":-0.04034, "alpha":-0.0002, "fx":[-1.40758,-1.40713,-1.40802,-1.40847], "fy":[-0.68693,-0.68604,-0.68558,-0.68647]}, - {"t":0.68147, "x":2.32508, "y":5.90833, "heading":-0.473, "vx":-1.88335, "vy":3.85652, "omega":-0.00437, "ax":-0.13453, "ay":-0.06577, "alpha":-0.00022, "fx":[-2.28816,-2.28767,-2.28863,-2.28913], "fy":[-1.11947,-1.11851,-1.11801,-1.11898]}, - {"t":0.71932, "x":2.25368, "y":6.05429, "heading":-0.47316, "vx":-1.88844, "vy":3.85403, "omega":-0.00437, "ax":-0.21776, "ay":-0.10688, "alpha":0.00022, "fx":[-3.70426,-3.70475,-3.7038,-3.7033], "fy":[-1.81734,-1.8183,-1.81879,-1.81783]}, - {"t":0.75718, "x":2.18203, "y":6.20012, "heading":-0.47333, "vx":-1.89669, "vy":3.84998, "omega":-0.00437, "ax":-0.2656, "ay":-0.13117, "alpha":0.00063, "fx":[-4.51842,-4.51982,-4.51708,-4.51568], "fy":[-2.22912,-2.23186,-2.23326,-2.23052]}, - {"t":0.79504, "x":2.11003, "y":6.34579, "heading":-0.47349, "vx":-1.90674, "vy":3.84501, "omega":-0.00434, "ax":-0.36301, "ay":-0.18316, "alpha":-0.00411, "fx":[-6.17031,-6.16122,-6.17905,-6.18815], "fy":[-3.12902,-3.11117,-3.102,-3.11984]}, - {"t":0.8329, "x":2.03758, "y":6.49122, "heading":-0.47366, "vx":-1.92049, "vy":3.83808, "omega":-0.0045, "ax":0.83555, "ay":-4.07752, "alpha":-9.36113, "fx":[22.94358,50.63804,3.75452,-20.48601], "fy":[-93.93016,-60.28167,-42.37498,-80.84297]}, - {"t":0.87076, "x":1.96547, "y":6.63361, "heading":-0.47383, "vx":-1.88885, "vy":3.68371, "omega":-0.3589, "ax":4.47798, "ay":-7.21087, "alpha":-19.60986, "fx":[65.79957,152.85587,135.86617,-49.84482], "fy":[-160.56414,-80.82648,-84.05559,-165.17316]}, - {"t":0.90862, "x":1.89717, "y":6.7679, "heading":-0.48742, "vx":-1.71932, "vy":3.41071, "omega":-1.10132, "ax":4.40336, "ay":-8.40326, "alpha":-13.2812, "fx":[68.38635,140.20094,106.76045,-15.74783], "fy":[-160.4741,-103.31441,-134.71815,-173.2414]}, - {"t":0.94648, "x":1.83524, "y":6.89101, "heading":-0.52911, "vx":-1.55261, "vy":3.09257, "omega":-1.60413, "ax":4.56601, "ay":-9.14295, "alpha":-3.2997, "fx":[74.30603,96.63901,82.66857,57.05255], "fy":[-158.08116,-145.41917,-153.60766,-164.96775]}, - {"t":0.98434, "x":1.77973, "y":7.00154, "heading":-0.58984, "vx":-1.37975, "vy":2.74643, "omega":-1.72906, "ax":4.59425, "ay":-9.19048, "alpha":1.07735, "fx":[79.99073,71.66627,76.50496,84.42588], "fy":[-155.45046,-159.4814,-157.25221,-153.12589]}, - {"t":1.0222, "x":1.73078, "y":7.09893, "heading":-0.6553, "vx":-1.20581, "vy":2.39848, "omega":-1.68827, "ax":4.58367, "ay":-9.1588, "alpha":3.41582, "fx":[86.03763,57.05683,72.168,96.60534], "fy":[-152.30824,-165.41672,-159.48393,-145.9452]}, - {"t":1.06006, "x":1.68842, "y":7.18317, "heading":-0.71922, "vx":-1.03228, "vy":2.05174, "omega":-1.55895, "ax":4.57013, "ay":-9.11728, "alpha":4.89421, "fx":[92.06112,47.74167,68.42126,102.72238], "fy":[-148.83803,-168.43051,-161.23122,-141.82927]}, - {"t":1.09792, "x":1.65261, "y":7.25431, "heading":-0.77824, "vx":-0.85926, "vy":1.70656, "omega":-1.37366, "ax":4.55832, "ay":-9.07753, "alpha":5.94569, "fx":[97.66098,41.4269,65.07887,105.97607], "fy":[-145.29971,-170.15377,-162.67384,-139.49746]}, - {"t":1.13578, "x":1.62335, "y":7.31242, "heading":-0.83025, "vx":-0.68668, "vy":1.3629, "omega":-1.14856, "ax":4.54788, "ay":-9.04196, "alpha":6.74848, "fx":[102.58704,36.963,62.13262,107.74981], "fy":[-141.92562,-171.22228,-163.86582,-138.19057]}, - {"t":1.17363, "x":1.60061, "y":7.35753, "heading":-0.87373, "vx":-0.5145, "vy":1.02057, "omega":-0.89307, "ax":4.5383, "ay":-9.01099, "alpha":7.38413, "fx":[106.73175,33.69039,59.61179,108.74663], "fy":[-138.88601,-171.92931,-164.83153,-137.45077]}, - {"t":1.21149, "x":1.58438, "y":7.38971, "heading":-0.90754, "vx":-0.34269, "vy":0.67943, "omega":-0.61351, "ax":4.52958, "ay":-8.98442, "alpha":7.89558, "fx":[110.08405,31.18262,57.55127,109.36922], "fy":[-136.28608,-172.42833,-165.58652,-136.98885]}, - {"t":1.24935, "x":1.57465, "y":7.409, "heading":-0.93077, "vx":-0.1712, "vy":0.33928, "omega":-0.31459, "ax":4.52206, "ay":-8.96171, "alpha":8.30945, "fx":[112.68219,29.14171,55.98362,109.8685], "fy":[-134.18061,-172.8071,-166.14228,-136.61451]}, - {"t":1.28721, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.20199, "y":4.10055, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.6648, "ay":10.14345, "alpha":-14.83674, "fx":[-27.97393,-12.13773,-106.33687,-164.59155], "fy":[196.96825,198.82189,168.46797,112.08741]}, + {"t":0.03859, "x":3.19851, "y":4.1081, "heading":0.0, "vx":-0.18001, "vy":0.39143, "omega":-0.57255, "ax":-4.75765, "ay":10.21164, "alpha":-13.8093, "fx":[-34.92516,-16.1874,-105.49114,-160.62711], "fy":[195.80609,198.49779,168.96255,117.62581]}, + {"t":0.07718, "x":3.18802, "y":4.13081, "heading":-0.02209, "vx":-0.36361, "vy":0.7855, "omega":-1.10545, "ax":-4.87394, "ay":10.30706, "alpha":-12.27357, "fx":[-46.19984,-21.21101,-103.10999,-154.46403], "fy":[193.39132,197.97059,170.37332,125.5198]}, + {"t":0.11577, "x":3.17036, "y":4.16879, "heading":-0.06475, "vx":-0.5517, "vy":1.18325, "omega":-1.57908, "ax":-5.0056, "ay":10.42937, "alpha":-10.1, "fx":[-60.50185,-29.45503,-99.25587,-144.55118], "fy":[189.3226,196.82643,172.56269,136.69892]}, + {"t":0.15436, "x":3.14535, "y":4.22222, "heading":-0.12569, "vx":-0.74486, "vy":1.58572, "omega":-1.96884, "ax":-5.13533, "ay":10.58366, "alpha":-6.7496, "fx":[-75.36457,-46.29136,-93.97576,-126.78195], "fy":[183.81229,193.40307,175.33558,153.14739]}, + {"t":0.19295, "x":3.11278, "y":4.29129, "heading":-0.20167, "vx":-0.94303, "vy":1.99414, "omega":-2.22931, "ax":-5.2207, "ay":10.70622, "alpha":-0.08326, "fx":[-86.95903,-86.51777,-87.09422,-87.53555], "fy":[178.49838,178.71564,178.43769,178.21865]}, + {"t":0.23154, "x":3.0725, "y":4.37622, "heading":-0.2877, "vx":-1.1445, "vy":2.40729, "omega":-2.23252, "ax":-4.65831, "ay":9.76243, "alpha":16.39189, "fx":[-90.47134,-172.79447,-71.25339,23.9121], "fy":[176.58779,96.70698,181.0281,196.61693]}, + {"t":0.27013, "x":3.02487, "y":4.47639, "heading":-0.37385, "vx":-1.32426, "vy":2.78402, "omega":-1.59996, "ax":-4.95961, "ay":8.39275, "alpha":22.20583, "fx":[-83.03667,-181.25139,-124.49758,58.0881], "fy":[179.31037,76.99386,115.25325,188.05529]}, + {"t":0.30872, "x":2.97007, "y":4.59007, "heading":-0.43559, "vx":-1.51565, "vy":3.1079, "omega":-0.74304, "ax":-3.43616, "ay":7.51218, "alpha":19.0715, "fx":[-66.5983,-140.79904,-55.83711,34.11811], "fy":[161.78949,90.28018,87.05082,161.77699]}, + {"t":0.34731, "x":2.90902, "y":4.7156, "heading":-0.46426, "vx":-1.64825, "vy":3.39779, "omega":-0.00707, "ax":0.06948, "ay":0.04394, "alpha":0.02175, "fx":[1.13454,1.08711,1.18184,1.22927], "fy":[0.80358,0.70884,0.66139,0.75612]}, + {"t":0.3859, "x":2.84547, "y":4.84675, "heading":-0.46454, "vx":-1.64557, "vy":3.39949, "omega":-0.00623, "ax":-0.00813, "ay":-0.00387, "alpha":0.00011, "fx":[-0.13561,-0.13585,-0.13536,-0.13511], "fy":[-0.06418,-0.06467,-0.06492,-0.06442]}, + {"t":0.42449, "x":2.78196, "y":4.97793, "heading":-0.46478, "vx":-1.64589, "vy":3.39934, "omega":-0.00623, "ax":-0.01914, "ay":-0.00921, "alpha":0.0001, "fx":[-0.31908,-0.31929,-0.31887,-0.31865], "fy":[-0.15319,-0.15361,-0.15383,-0.1534]}, + {"t":0.46308, "x":2.71843, "y":5.10911, "heading":-0.46502, "vx":-1.64663, "vy":3.39898, "omega":-0.00622, "ax":-0.0228, "ay":-0.01099, "alpha":0.00009, "fx":[-0.3801,-0.38029,-0.37991,-0.37973], "fy":[-0.18293,-0.1833,-0.18349,-0.18312]}, + {"t":0.50167, "x":2.65487, "y":5.24026, "heading":-0.46526, "vx":-1.6475, "vy":3.39856, "omega":-0.00622, "ax":-0.0261, "ay":-0.0126, "alpha":0.00007, "fx":[-0.43513,-0.43528,-0.43499,-0.43484], "fy":[-0.20989,-0.21018,-0.21032,-0.21003]}, + {"t":0.54026, "x":2.59127, "y":5.3714, "heading":-0.4655, "vx":-1.64851, "vy":3.39807, "omega":-0.00622, "ax":-0.03, "ay":-0.01451, "alpha":0.00004, "fx":[-0.50007,-0.50015,-0.49999,-0.49991], "fy":[-0.2418,-0.24196,-0.24205,-0.24188]}, + {"t":0.57885, "x":2.52764, "y":5.50252, "heading":-0.46574, "vx":-1.64967, "vy":3.39751, "omega":-0.00622, "ax":-0.0351, "ay":-0.01702, "alpha":-0.00001, "fx":[-0.58516,-0.58514,-0.58518,-0.58519], "fy":[-0.28371,-0.28368,-0.28367,-0.2837]}, + {"t":0.61744, "x":2.46395, "y":5.63362, "heading":-0.46598, "vx":-1.65102, "vy":3.39685, "omega":-0.00622, "ax":-0.04298, "ay":-0.02088, "alpha":-0.00008, "fx":[-0.71639,-0.71623,-0.71655,-0.71672], "fy":[-0.34838,-0.34805,-0.34789,-0.34821]}, + {"t":0.65603, "x":2.4002, "y":5.76469, "heading":-0.46622, "vx":-1.65268, "vy":3.39605, "omega":-0.00622, "ax":-0.05877, "ay":-0.02862, "alpha":-0.00015, "fx":[-0.97948,-0.97916,-0.9798,-0.98012], "fy":[-0.47755,-0.47691,-0.47658,-0.47723]}, + {"t":0.69462, "x":2.33638, "y":5.89572, "heading":-0.46646, "vx":-1.65495, "vy":3.39494, "omega":-0.00623, "ax":-0.09597, "ay":-0.04685, "alpha":-0.0002, "fx":[-1.59963,-1.59918,-1.60007,-1.60051], "fy":[-0.78164,-0.78075,-0.78031,-0.78119]}, + {"t":0.73321, "x":2.27245, "y":6.0267, "heading":-0.4667, "vx":-1.65865, "vy":3.39314, "omega":-0.00623, "ax":-0.17689, "ay":-0.08663, "alpha":-0.00001, "fx":[-2.94868,-2.94867,-2.9487,-2.94871], "fy":[-1.44402,-1.44399,-1.44398,-1.444]}, + {"t":0.7718, "x":2.20831, "y":6.15757, "heading":-0.46694, "vx":-1.66548, "vy":3.38979, "omega":-0.00623, "ax":-0.24011, "ay":-0.11816, "alpha":0.00065, "fx":[-4.00316,-4.00459,-4.00174,-4.00031], "fy":[-1.96752,-1.97037,-1.9718,-1.96896]}, + {"t":0.81039, "x":2.14386, "y":6.2883, "heading":-0.46718, "vx":-1.67475, "vy":3.38523, "omega":-0.00621, "ax":-0.19255, "ay":-0.09544, "alpha":0.0004, "fx":[-3.21012,-3.21099,-3.20926,-3.20839], "fy":[-1.58964,-1.59137,-1.59225,-1.59052]}, + {"t":0.84898, "x":2.07909, "y":6.41886, "heading":-0.46742, "vx":-1.68218, "vy":3.38155, "omega":-0.00619, "ax":-0.11227, "ay":-0.05594, "alpha":0.00014, "fx":[-1.87169,-1.872,-1.87139,-1.87108], "fy":[-0.93211,-0.93272,-0.93303,-0.93242]}, + {"t":0.88757, "x":2.01409, "y":6.54931, "heading":-0.46766, "vx":-1.68651, "vy":3.37939, "omega":-0.00619, "ax":-0.11498, "ay":-0.05752, "alpha":0.0, "fx":[-1.91674,-1.91674,-1.91674,-1.91675], "fy":[-0.95875,-0.95875,-0.95875,-0.95875]}, + {"t":0.92616, "x":1.94892, "y":6.67968, "heading":-0.4679, "vx":-1.69095, "vy":3.37717, "omega":-0.00619, "ax":-0.47686, "ay":-0.25093, "alpha":-0.02228, "fx":[-7.92505,-7.87634,-7.97314,-8.02185], "fy":[-4.25595,-4.15908,-4.10994,-4.2068]}, + {"t":0.96475, "x":1.88331, "y":6.80982, "heading":-0.46814, "vx":-1.70935, "vy":3.36749, "omega":-0.00705, "ax":2.51816, "ay":-7.9237, "alpha":-19.07622, "fx":[54.00885,132.86196,29.70436,-48.66929], "fy":[-167.22021,-101.51389,-99.40457,-160.19858]}, + {"t":1.00334, "x":1.81923, "y":6.93387, "heading":-0.46841, "vx":-1.61217, "vy":3.06171, "omega":-0.7432, "ax":5.21723, "ay":-8.2958, "alpha":-22.75464, "fx":[74.58173,175.04104,158.09757,-59.84514], "fy":[-182.97918,-90.42646,-92.36886,-187.37358]}, + {"t":1.04193, "x":1.7609, "y":7.04584, "heading":-0.49709, "vx":-1.41084, "vy":2.74158, "omega":-1.62129, "ax":5.18794, "ay":-9.5036, "alpha":-16.67175, "fx":[76.84977,163.799,132.96933,-27.69631], "fy":[-182.92454,-111.49712,-143.27642,-195.98381]}, + {"t":1.08052, "x":1.71031, "y":7.14457, "heading":-0.55965, "vx":-1.21064, "vy":2.37484, "omega":-2.26466, "ax":5.35242, "ay":-10.64186, "alpha":0.04239, "fx":[89.28373,88.96969,89.16112,89.47464], "fy":[-177.36252,-177.52124,-177.42678,-177.2679]}, + {"t":1.11911, "x":1.66758, "y":7.22829, "heading":-0.64705, "vx":-1.00409, "vy":1.96417, "omega":-2.26302, "ax":5.30031, "ay":-10.51889, "alpha":6.46315, "fx":[105.04045,47.59311,78.69139,122.08973], "fy":[-168.65419,-193.02548,-182.70441,-156.9951]}, + {"t":1.1577, "x":1.63278, "y":7.29625, "heading":-0.73438, "vx":-0.79955, "vy":1.55825, "omega":-2.01361, "ax":5.24765, "ay":-10.33056, "alpha":9.94378, "fx":[121.82476,24.59779,70.63673,132.844], "fy":[-157.09492,-197.40506,-186.11366,-148.20813]}, + {"t":1.19629, "x":1.60583, "y":7.34869, "heading":-0.81208, "vx":-0.59705, "vy":1.15959, "omega":-1.62988, "ax":5.20436, "ay":-10.15374, "alpha":12.38799, "fx":[136.27921,10.24984,63.7979,136.6896], "fy":[-144.86093,-198.74258,-188.64351,-144.78514]}, + {"t":1.23488, "x":1.58667, "y":7.38588, "heading":-0.87498, "vx":-0.39621, "vy":0.76776, "omega":-1.15183, "ax":5.15721, "ay":-10.00571, "alpha":14.23486, "fx":[147.08634,0.60446,58.19996,137.98201], "fy":[-133.98647,-199.0564,-190.49158,-143.6271]}, + {"t":1.27347, "x":1.57522, "y":7.40806, "heading":-0.91943, "vx":-0.19719, "vy":0.38164, "omega":-0.60251, "ax":5.10999, "ay":-9.88966, "alpha":15.61306, "fx":[154.49671,-6.25017,54.01348,138.46432], "fy":[-125.46286,-198.99714,-191.7523,-143.21141]}, + {"t":1.31206, "x":1.57141, "y":7.41542, "heading":-0.94268, "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/ConfusingTestPath.traj b/src/main/deploy/choreo/ConfusingTestPath.traj index 28b1fe11..69951c22 100644 --- a/src/main/deploy/choreo/ConfusingTestPath.traj +++ b/src/main/deploy/choreo/ConfusingTestPath.traj @@ -3,11 +3,11 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.44632652401924133, "y":4.026000499725342, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":0.44632652401924133, "y":4.026000499725342, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":6.881347179412842, "y":4.290971755981445, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -19,11 +19,11 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"0.44632652401924133 m", "val":0.44632652401924133}, "y":{"exp":"4.026000499725342 m", "val":4.026000499725342}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"0.44632652401924133 m", "val":0.44632652401924133}, "y":{"exp":"4.026000499725342 m", "val":4.026000499725342}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"6.881347179412842 m", "val":6.881347179412842}, "y":{"exp":"4.290971755981445 m", "val":4.290971755981445}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -38,120 +38,114 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.59086,0.96585,1.39567,1.85025,2.44024], + "waypoints":[0.0,0.59344,1.01952,1.50775,2.02236,2.6234], "samples":[ - {"t":0.0, "x":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.87948, "ay":3.14855, "alpha":-33.63363, "fx":[164.77106,120.72737,-2.70988,-18.83316], "fy":[-8.36323,116.62028,164.59757,-58.63118]}, - {"t":0.02814, "x":0.44786, "y":4.02725, "heading":0.0, "vx":0.10915, "vy":0.08859, "omega":-0.94633, "ax":7.99122, "ay":6.49929, "alpha":-0.35492, "fx":[137.45223,135.78204,134.38737,136.092], "fy":[108.66497,110.74989,112.43204,110.3573]}, - {"t":0.05627, "x":0.4541, "y":4.03231, "heading":-0.02663, "vx":0.334, "vy":0.27146, "omega":-0.95632, "ax":7.99041, "ay":6.49864, "alpha":-0.2905, "fx":[137.16623,135.82682,134.65208,136.01323], "fy":[108.99267,110.66196,112.08311,110.42262]}, - {"t":0.08441, "x":0.46666, "y":4.04252, "heading":-0.05353, "vx":0.55882, "vy":0.4543, "omega":-0.96449, "ax":7.98943, "ay":6.49786, "alpha":-0.21516, "fx":[136.82757,135.85687,134.96261,135.94444], "fy":[109.37902,110.58621,111.67165,110.47037]}, - {"t":0.11255, "x":0.48554, "y":4.05788, "heading":-0.08067, "vx":0.78361, "vy":0.63713, "omega":-0.97054, "ax":7.98822, "ay":6.4969, "alpha":-0.12591, "fx":[136.42246,135.86747,135.33033,135.88886], "fy":[109.8385,110.5268,111.18119,110.49528]}, - {"t":0.14068, "x":0.51075, "y":4.07837, "heading":-0.10798, "vx":1.00837, "vy":0.81993, "omega":-0.97409, "ax":7.98669, "ay":6.49569, "alpha":-0.01849, "fx":[135.93154,135.85202,135.77115,135.85074], "fy":[110.39121,110.48948,110.58842,110.49021]}, - {"t":0.16882, "x":0.54229, "y":4.10402, "heading":-0.13539, "vx":1.23309, "vy":1.00269, "omega":-0.97461, "ax":7.98473, "ay":6.49412, "alpha":0.11322, "fx":[135.3267,135.80098,136.30791,135.836], "fy":[111.06585,110.48245,109.85924,110.44515]}, - {"t":0.19695, "x":0.58014, "y":4.1348, "heading":-0.16281, "vx":1.45775, "vy":1.18542, "omega":-0.97142, "ax":7.9821, "ay":6.49203, "alpha":0.27848, "fx":[134.56534,135.69988,136.97476,135.85303], "fy":[111.90507,110.51796,108.94163,110.34563]}, - {"t":0.22509, "x":0.62432, "y":4.17072, "heading":-0.19014, "vx":1.68234, "vy":1.36808, "omega":-0.96358, "ax":7.97845, "ay":6.48912, "alpha":0.49195, "fx":[133.5798,135.52557,137.82511,135.91442], "fy":[112.97503,110.61535,107.75219,110.16962]}, - {"t":0.25323, "x":0.67481, "y":4.21178, "heading":-0.21725, "vx":1.90682, "vy":1.55066, "omega":-0.94974, "ax":7.97311, "ay":6.48484, "alpha":0.7782, "fx":[132.25564,135.23827,138.9473,136.03996], "fy":[114.38422,110.80729,106.14817,109.88168]}, - {"t":0.28136, "x":0.73162, "y":4.25798, "heading":-0.24397, "vx":2.13116, "vy":1.73312, "omega":-0.92785, "ax":7.96468, "ay":6.4781, "alpha":1.18197, "fx":[130.3831,134.76288,140.49836,136.26314], "fy":[116.32357,111.15428,103.86391,109.42051]}, - {"t":0.3095, "x":0.79473, "y":4.30931, "heading":-0.27008, "vx":2.35526, "vy":1.91539, "omega":-0.89459, "ax":7.94989, "ay":6.46628, "alpha":1.79388, "fx":[127.53299,133.93759,142.785,136.64595], "fy":[119.16167,111.78349,100.34378,108.66919]}, - {"t":0.33764, "x":0.86415, "y":4.36576, "heading":-0.29525, "vx":2.57894, "vy":2.09733, "omega":-0.84412, "ax":7.91926, "ay":6.44199, "alpha":2.82911, "fx":[122.67559,132.33545,146.48784,137.31868], "fy":[123.70688,113.01881,94.20821,107.37192]}, - {"t":0.36577, "x":0.93984, "y":4.42732, "heading":-0.319, "vx":2.80176, "vy":2.27858, "omega":-0.76452, "ax":7.83323, "ay":6.37654, "alpha":4.94849, "fx":[112.6295,128.3394,153.38438,138.61094], "fy":[132.08124,116.00997,80.92391,104.8371]}, - {"t":0.39391, "x":1.02178, "y":4.49396, "heading":-0.34051, "vx":3.02215, "vy":2.45799, "omega":-0.62528, "ax":7.30577, "ay":6.0886, "alpha":11.59121, "fx":[81.96683,106.14043,167.34416,141.6246], "fy":[150.95227,129.63123,35.32825,98.34975]}, - {"t":0.42205, "x":1.1097, "y":4.56552, "heading":-0.3581, "vx":3.22771, "vy":2.6293, "omega":-0.29915, "ax":3.55729, "ay":2.81037, "alpha":10.60415, "fx":[41.80946,29.7427,83.12136,87.3605], "fy":[84.88368,38.38662,9.79704,58.14715]}, - {"t":0.45018, "x":1.20192, "y":4.64062, "heading":-0.36652, "vx":3.3278, "vy":2.70838, "omega":-0.00079, "ax":0.1164, "ay":-0.13444, "alpha":0.00745, "fx":[1.95463,2.08188,2.06185,1.82111], "fy":[-2.13407,-2.40124,-2.38279,-2.229]}, - {"t":0.47832, "x":1.2956, "y":4.71677, "heading":-0.36654, "vx":3.33108, "vy":2.7046, "omega":-0.00058, "ax":0.3533, "ay":-0.43679, "alpha":0.0, "fx":[5.99452,6.01524,6.02437,6.00425], "fy":[-7.44094,-7.45252,-7.4183,-7.40674]}, - {"t":0.50646, "x":1.38947, "y":4.79269, "heading":-0.36656, "vx":3.34102, "vy":2.69231, "omega":-0.00058, "ax":1.07188, "ay":-1.34578, "alpha":-0.00003, "fx":[18.21509,18.2563,18.2482,18.20987], "fy":[-22.87249,-22.8907,-22.90602,-22.89586]}, - {"t":0.53459, "x":1.4839, "y":4.86791, "heading":-0.36658, "vx":3.37118, "vy":2.65444, "omega":-0.00058, "ax":2.62271, "ay":-3.4503, "alpha":-0.02079, "fx":[44.92858,45.12694,43.99986,44.39095], "fy":[-58.3018,-58.9146,-58.97923,-58.55866]}, - {"t":0.56273, "x":1.57979, "y":4.94123, "heading":-0.36659, "vx":3.44497, "vy":2.55736, "omega":-0.00116, "ax":-0.41591, "ay":-7.59516, "alpha":-11.36915, "fx":[15.73999,52.04338,-47.11196,-48.9692], "fy":[-146.88238,-123.91482,-109.86267,-136.10593]}, - {"t":0.59086, "x":1.67655, "y":5.01018, "heading":-0.36663, "vx":3.43327, "vy":2.34366, "omega":-0.32105, "ax":-3.09108, "ay":-7.02992, "alpha":-15.01405, "fx":[-19.20667,17.33978,-113.03743,-95.40926], "fy":[-151.77788,-131.65734,-74.75773,-120.11448]}, - {"t":0.60872, "x":1.73737, "y":5.05091, "heading":-0.37236, "vx":3.37807, "vy":2.21813, "omega":-0.58915, "ax":-3.21903, "ay":-6.13773, "alpha":-13.98731, "fx":[-25.08431,3.50315,-103.30174,-94.13643], "fy":[-139.54834,-110.39547,-59.92221,-107.73832]}, - {"t":0.62658, "x":1.79717, "y":5.08954, "heading":-0.38288, "vx":3.32059, "vy":2.10853, "omega":-0.83892, "ax":-2.99875, "ay":-4.84463, "alpha":-11.89672, "fx":[-29.53648,-7.77401,-81.30428,-85.41651], "fy":[-117.47151,-77.9985,-44.1441,-90.00896]}, - {"t":0.64443, "x":1.85599, "y":5.12642, "heading":-0.39786, "vx":3.26704, "vy":2.02202, "omega":-1.05135, "ax":-2.26177, "ay":-3.10199, "alpha":-8.35968, "fx":[-26.25297,-10.56544,-53.01124,-64.05842], "fy":[-79.13492,-44.84375,-25.85062,-61.2266]}, - {"t":0.66229, "x":1.91397, "y":5.16203, "heading":-0.41663, "vx":3.22666, "vy":1.96663, "omega":-1.20063, "ax":-1.38975, "ay":-1.44402, "alpha":-4.72512, "fx":[-17.60743,-8.21406,-30.09787,-38.63767], "fy":[-39.71192,-19.24294,-9.32387,-29.9708]}, - {"t":0.68015, "x":1.97136, "y":5.19692, "heading":-0.43807, "vx":3.20184, "vy":1.94085, "omega":-1.285, "ax":-0.77551, "ay":-0.32983, "alpha":-2.26072, "fx":[-10.56312,-5.80052,-15.88473,-20.51663], "fy":[-12.91019,-3.04581,1.70587,-8.19124]}, - {"t":0.698, "x":2.02841, "y":5.23152, "heading":-0.46102, "vx":3.18799, "vy":1.93496, "omega":-1.32537, "ax":-0.44824, "ay":0.29177, "alpha":-0.9394, "fx":[-6.62335,-4.52149,-8.63497,-10.71767], "fy":[1.91892,5.9818,8.01239,3.93844]}, - {"t":0.71586, "x":2.08527, "y":5.26612, "heading":-0.48468, "vx":3.17999, "vy":1.94017, "omega":-1.34215, "ax":-0.32746, "ay":0.58535, "alpha":-0.40706, "fx":[-5.17708,-4.20567,-5.96459,-6.9324], "fy":[8.63783,10.37247,11.27894,9.53722]}, - {"t":0.73372, "x":2.142, "y":5.30086, "heading":-0.50865, "vx":3.17414, "vy":1.95062, "omega":-1.34941, "ax":-0.32833, "ay":0.68141, "alpha":-0.34795, "fx":[-5.27663,-4.41033,-5.89345,-6.75875], "fy":[10.45753,11.92381,12.72669,11.25453]}, - {"t":0.75157, "x":2.19863, "y":5.3358, "heading":-0.53275, "vx":3.16828, "vy":1.96279, "omega":-1.35563, "ax":-0.37841, "ay":0.66018, "alpha":-0.52885, "fx":[-6.00195,-4.64982,-6.87137,-8.22314], "fy":[9.48839,11.69565,12.97336,10.76035]}, - {"t":0.76943, "x":2.25514, "y":5.37095, "heading":-0.55695, "vx":3.16152, "vy":1.97458, "omega":-1.36507, "ax":-0.41618, "ay":0.5587, "alpha":-0.7851, "fx":[-6.49185,-4.41784,-7.66666,-9.74031], "fy":[6.89748,10.13255,12.11237,8.87065]}, - {"t":0.78729, "x":2.31153, "y":5.4063, "heading":-0.58133, "vx":3.15409, "vy":1.98455, "omega":-1.37909, "ax":-0.38754, "ay":0.37615, "alpha":-1.00634, "fx":[-5.92017,-3.16348,-7.26393,-10.02026], "fy":[3.04034,7.12375,9.76117,5.66783]}, - {"t":0.80514, "x":2.36779, "y":5.4418, "heading":-0.60595, "vx":3.14717, "vy":1.99127, "omega":-1.39706, "ax":-0.24237, "ay":0.07383, "alpha":-1.12878, "fx":[-3.46672,-0.2517,-4.77813,-7.99409], "fy":[-2.51937,1.98031,5.03968,0.5227]}, - {"t":0.823, "x":2.42395, "y":5.47737, "heading":-0.6309, "vx":3.14284, "vy":1.99259, "omega":-1.41722, "ax":0.06782, "ay":-0.42897, "alpha":-1.13179, "fx":[1.69996,5.07198,0.60976,-2.76746], "fy":[-11.07269,-6.64991,-3.50623,-7.95777]}, - {"t":0.84086, "x":2.48008, "y":5.51288, "heading":-0.65621, "vx":3.14405, "vy":1.98493, "omega":-1.43743, "ax":0.5872, "ay":-1.25715, "alpha":-1.03669, "fx":[10.35534,13.6375,9.62755,6.33205], "fy":[-24.80111,-20.83508,-17.94503,-21.95353]}, - {"t":0.85871, "x":2.53632, "y":5.54812, "heading":-0.68188, "vx":3.15454, "vy":1.96248, "omega":-1.45594, "ax":1.33557, "ay":-2.5563, "alpha":-0.89993, "fx":[22.85195,25.99538,22.59656,19.42645], "fy":[-46.3479,-42.97461,-40.58761,-44.01783]}, - {"t":0.87657, "x":2.59286, "y":5.58276, "heading":-0.70787, "vx":3.17839, "vy":1.91683, "omega":-1.47201, "ax":2.23803, "ay":-4.35942, "alpha":-0.78316, "fx":[37.88205,41.13998,38.27984,34.97107], "fy":[-76.42717,-73.5352,-71.84254,-74.80481]}, - {"t":0.89443, "x":2.64997, "y":5.61629, "heading":-0.73416, "vx":3.21835, "vy":1.83899, "omega":-1.48599, "ax":2.98522, "ay":-6.21807, "alpha":-0.70462, "fx":[50.13655,53.9318,51.46809,47.57476], "fy":[-107.42026,-104.83681,-104.08636,-106.7267]}, - {"t":0.91228, "x":2.70792, "y":5.64814, "heading":-0.76069, "vx":3.27166, "vy":1.72795, "omega":-1.49857, "ax":3.27148, "ay":-7.47629, "alpha":-0.64837, "fx":[54.7235,58.85566,56.62992,52.37869], "fy":[-128.39042,-126.08928,-125.93923,-128.259]}, - {"t":0.93014, "x":2.76686, "y":5.6778, "heading":-0.78745, "vx":3.33007, "vy":1.59445, "omega":-1.51015, "ax":3.26645, "ay":-8.27208, "alpha":-0.53311, "fx":[54.65643,58.36121,56.50782,52.72035], "fy":[-141.55641,-139.78565,-139.85319,-141.62739]}, - {"t":0.948, "x":2.82684, "y":5.70496, "heading":-0.81442, "vx":3.3884, "vy":1.44674, "omega":-1.51967, "ax":3.14526, "ay":-8.78793, "alpha":-0.2827, "fx":[52.96089,55.05937,54.0514,51.92854], "fy":[-149.86152,-148.99209,-149.09965,-149.9676]}, - {"t":0.96585, "x":2.88785, "y":5.72939, "heading":-0.84156, "vx":3.44456, "vy":1.28982, "omega":-1.52472, "ax":3.22836, "ay":-8.79319, "alpha":0.94723, "fx":[57.04004,49.62282,52.94619,60.04487], "fy":[-148.26728,-151.24276,-150.87281,-147.89573]}, - {"t":0.98454, "x":2.95278, "y":5.75196, "heading":-0.87005, "vx":3.5049, "vy":1.12549, "omega":-1.50702, "ax":2.89966, "ay":-8.43562, "alpha":1.3926, "fx":[52.32616,41.75345,46.62105,56.58866], "fy":[-141.34527,-145.52457,-145.62459,-141.4555]}, - {"t":1.00323, "x":3.01879, "y":5.77152, "heading":-0.89821, "vx":3.55908, "vy":0.96785, "omega":-1.48099, "ax":2.42308, "ay":-7.79742, "alpha":1.45968, "fx":[43.91938,33.76853,38.75896,48.41653], "fy":[-129.95183,-134.09688,-135.28518,-131.19344]}, - {"t":1.02192, "x":3.08572, "y":5.78824, "heading":-0.92589, "vx":3.60436, "vy":0.82214, "omega":-1.45372, "ax":1.73285, "ay":-6.60003, "alpha":0.9942, "fx":[30.60283,25.24754,28.39419,33.6569], "fy":[-109.66536,-112.42839,-114.81551,-112.14896]}, - {"t":1.0406, "x":3.15338, "y":5.80245, "heading":-0.95305, "vx":3.63675, "vy":0.6988, "omega":-1.43514, "ax":0.81144, "ay":-4.56505, "alpha":-0.10924, "fx":[13.63467,14.3746,13.98206,13.21832], "fy":[-77.81232,-77.59345,-77.48058,-77.71467]}, - {"t":1.05929, "x":3.22149, "y":5.81471, "heading":-0.97987, "vx":3.65191, "vy":0.61349, "omega":-1.43718, "ax":0.00946, "ay":-2.49763, "alpha":-1.30297, "fx":[-0.79335,4.83092,1.13986,-4.53359], "fy":[-46.59112,-43.25903,-38.34001,-41.74567]}, - {"t":1.07798, "x":3.28973, "y":5.82574, "heading":-1.00673, "vx":3.65209, "vy":0.56681, "omega":-1.46153, "ax":-0.49826, "ay":-0.96538, "alpha":-2.16848, "fx":[-10.14489,-1.03149,-6.79113,-15.93377], "fy":[-23.51874,-18.04386,-9.27568,-14.84526]}, - {"t":1.09667, "x":3.35789, "y":5.83617, "heading":-1.03404, "vx":3.64278, "vy":0.54877, "omega":-1.50205, "ax":-0.75256, "ay":0.04109, "alpha":-2.66254, "fx":[-15.10758,-3.8242,-10.50543,-21.76604], "fy":[-8.08586,-1.54047,9.51795,2.90441]}, - {"t":1.11535, "x":3.42584, "y":5.84643, "heading":-1.06211, "vx":3.62871, "vy":0.54954, "omega":-1.55181, "ax":-0.82723, "ay":0.66301, "alpha":-2.86112, "fx":[-16.86397,-4.52138,-11.31352,-23.58516], "fy":[1.86511,8.63932,20.70348,13.9026]}, - {"t":1.13404, "x":3.49351, "y":5.85681, "heading":-1.09111, "vx":3.61325, "vy":0.56193, "omega":-1.60527, "ax":-0.78121, "ay":1.02338, "alpha":-2.84879, "fx":[-16.37779,-3.85502,-10.24551,-22.67408], "fy":[8.11261,14.53842,26.69906,20.27978]}, - {"t":1.15273, "x":3.56089, "y":5.86749, "heading":-1.12111, "vx":3.59866, "vy":0.58106, "omega":-1.65851, "ax":-0.65414, "ay":1.20161, "alpha":-2.68929, "fx":[-14.31872,-2.29717,-7.97954,-19.91155], "fy":[11.76223,17.47866,29.10394,23.41107]}, - {"t":1.17142, "x":3.62803, "y":5.87856, "heading":-1.15211, "vx":3.58643, "vy":0.60351, "omega":-1.70877, "ax":-0.47001, "ay":1.23779, "alpha":-2.42168, "fx":[-11.10216,-0.12578,-4.9199,-15.83138], "fy":[13.34085,18.14105,28.75485,23.98073]}, - {"t":1.19011, "x":3.69497, "y":5.89006, "heading":-1.18404, "vx":3.57765, "vy":0.62664, "omega":-1.75402, "ax":-0.24163, "ay":1.14014, "alpha":-2.06452, "fx":[-6.94998,2.51296,-1.28604,-10.71697], "fy":[12.90818,16.68693,25.86905,22.10967]}, - {"t":1.20879, "x":3.76178, "y":5.90197, "heading":-1.21682, "vx":3.57313, "vy":0.64795, "omega":-1.79261, "ax":0.02475, "ay":0.8893, "alpha":-1.62322, "fx":[-1.96355,5.5493,2.80433,-4.70606], "fy":[10.10265,12.82283,20.14611,17.43517]}, - {"t":1.22748, "x":3.82856, "y":5.91423, "heading":-1.25032, "vx":3.57359, "vy":0.66457, "omega":-1.82294, "ax":0.32289, "ay":0.43977, "alpha":-1.10337, "fx":[3.75686,8.92124,7.23515,2.05564], "fy":[4.12285,5.80612,10.83764,9.15497]}, - {"t":1.24617, "x":3.8954, "y":5.92673, "heading":-1.28438, "vx":3.57963, "vy":0.67279, "omega":-1.84356, "ax":0.63699, "ay":-0.27719, "alpha":-0.53149, "fx":[9.92223,12.46913,11.7569,9.19159], "fy":[-6.29023,-5.54956,-3.13783,-3.88218]}, - {"t":1.26486, "x":3.96241, "y":5.93925, "heading":-1.31883, "vx":3.59153, "vy":0.66761, "omega":-1.85349, "ax":0.92758, "ay":-1.33725, "alpha":0.01613, "fx":[15.76338,15.76376,15.79928,15.78478], "fy":[-22.6632,-22.66756,-22.82735,-22.82686]}, - {"t":1.28354, "x":4.02969, "y":5.95149, "heading":-1.35347, "vx":3.60887, "vy":0.64262, "omega":-1.85319, "ax":1.11679, "ay":-2.76117, "alpha":0.39168, "fx":[19.71261,17.8877,18.28654,20.09822], "fy":[-45.80162,-46.22195,-48.13036,-47.7129]}, - {"t":1.30223, "x":4.09732, "y":5.96302, "heading":-1.3881, "vx":3.62974, "vy":0.59102, "omega":-1.84587, "ax":1.09807, "ay":-4.35563, "alpha":0.41356, "fx":[19.548,17.48763,17.81487,19.86103], "fy":[-72.93218,-73.33351,-75.24259,-74.84375]}, - {"t":1.32092, "x":4.16534, "y":5.9733, "heading":-1.4226, "vx":3.65026, "vy":0.50962, "omega":-1.83814, "ax":0.83662, "ay":-5.5931, "alpha":0.05318, "fx":[14.35503,14.08142,14.10895,14.37721], "fy":[-94.99433,-95.03715,-95.27946,-95.23752]}, - {"t":1.33961, "x":4.2337, "y":5.98185, "heading":-1.45695, "vx":3.66589, "vy":0.4051, "omega":-1.83715, "ax":0.48735, "ay":-6.20897, "alpha":-0.37671, "fx":[7.28327,9.49203,9.30405,7.07942], "fy":[-106.44017,-106.21373,-104.78435,-105.01276]}, - {"t":1.35829, "x":4.3023, "y":5.98834, "heading":-1.49128, "vx":3.675, "vy":0.28907, "omega":-1.84419, "ax":0.26052, "ay":-6.64717, "alpha":-0.43665, "fx":[3.20193,5.83327,5.67244,3.01791], "fy":[-113.94976,-113.77985,-112.18217,-112.35425]}, - {"t":1.37698, "x":4.37102, "y":5.99258, "heading":-1.52574, "vx":3.67987, "vy":0.16485, "omega":-1.85235, "ax":0.21363, "ay":-7.44512, "alpha":0.1378, "fx":[4.21934,3.03798,3.05825,4.21979], "fy":[-126.53455,-126.5741,-126.74378,-126.70494]}, - {"t":1.39567, "x":4.43982, "y":5.99436, "heading":-1.56036, "vx":3.68386, "vy":0.02571, "omega":-1.84977, "ax":0.49994, "ay":-7.51898, "alpha":1.88427, "fx":[15.78219,1.62763,1.65319,14.95255], "fy":[-125.25007,-125.91495,-130.51656,-129.90115]}, - {"t":1.41461, "x":4.50969, "y":5.9935, "heading":-1.5954, "vx":3.69333, "vy":-0.1167, "omega":-1.81408, "ax":0.51883, "ay":-6.61801, "alpha":2.36444, "fx":[17.17572,1.33814,0.9174,15.86903], "fy":[-108.57531,-108.9333,-116.5749,-116.19818]}, - {"t":1.43355, "x":4.57974, "y":5.9901, "heading":-1.62976, "vx":3.70316, "vy":-0.24205, "omega":-1.7693, "ax":0.18966, "ay":-6.13054, "alpha":1.84266, "fx":[9.40074,-2.20341,-2.73108,8.43786], "fy":[-101.15884,-100.86477,-107.41723,-107.67414]}, - {"t":1.45249, "x":4.64991, "y":5.98442, "heading":-1.66327, "vx":3.70675, "vy":-0.35817, "omega":-1.7344, "ax":-0.37038, "ay":-5.94137, "alpha":0.76158, "fx":[-3.81632,-8.4847,-8.74795,-4.15125], "fy":[-99.87482,-99.49214,-102.25091,-102.62614]}, - {"t":1.47143, "x":4.72006, "y":5.97657, "heading":-1.69612, "vx":3.69973, "vy":-0.4707, "omega":-1.71997, "ax":-1.00034, "ay":-5.80663, "alpha":-0.49183, "fx":[-18.55229,-15.66122,-15.46304,-18.38522], "fy":[-99.48619,-99.89896,-98.05378,-97.6375]}, - {"t":1.49037, "x":4.78995, "y":5.96661, "heading":-1.7287, "vx":3.68079, "vy":-0.58069, "omega":-1.72929, "ax":-1.5509, "ay":-5.53292, "alpha":-1.55281, "fx":[-31.07524,-22.31054,-21.56036,-30.57565], "fy":[-96.26457,-98.00968,-91.97375,-90.20579]}, - {"t":1.50931, "x":4.85939, "y":5.95462, "heading":-1.76145, "vx":3.65141, "vy":-0.68549, "omega":-1.7587, "ax":-1.84709, "ay":-4.79737, "alpha":-2.02199, "fx":[-37.31915,-26.75181,-25.37508,-36.22759], "fy":[-84.66988,-87.16341,-78.53847,-76.03569]}, - {"t":1.52826, "x":4.92822, "y":5.94077, "heading":-1.79476, "vx":3.61642, "vy":-0.77635, "omega":-1.797, "ax":-1.7162, "ay":-3.44506, "alpha":-1.58757, "fx":[-33.78497,-26.06433,-24.54945,-32.3696], "fy":[-61.27175,-63.23626,-55.92763,-53.96235]}, - {"t":1.5472, "x":4.99641, "y":5.92545, "heading":-1.8288, "vx":3.58392, "vy":-0.8416, "omega":-1.82707, "ax":-1.23971, "ay":-1.97757, "alpha":-0.4892, "fx":[-22.48364,-20.28584,-19.68211,-21.8968], "fy":[-34.53636,-35.14378,-32.74144,-32.13009]}, - {"t":1.56614, "x":5.06407, "y":5.90916, "heading":-1.86341, "vx":3.56044, "vy":-0.87906, "omega":-1.83633, "ax":-0.65449, "ay":-0.82806, "alpha":0.75503, "fx":[-8.77392,-12.46166,-13.4795,-9.81584], "fy":[-12.95814,-11.84977,-15.21447,-16.31785]}, - {"t":1.58508, "x":5.13139, "y":5.89236, "heading":-1.89819, "vx":3.54804, "vy":-0.89475, "omega":-1.82203, "ax":-0.14133, "ay":-0.12663, "alpha":1.77627, "fx":[3.16553,-5.21514,-7.95789,0.39147], "fy":[0.44558,3.26251,-4.75678,-7.56714]}, - {"t":1.60402, "x":5.19857, "y":5.87539, "heading":-1.9327, "vx":3.54536, "vy":-0.89714, "omega":-1.78839, "ax":0.19843, "ay":0.11721, "alpha":2.39719, "fx":[10.99718,-0.13932,-4.23401,6.87741], "fy":[5.27,9.4336,-1.28984,-5.43886]}, - {"t":1.62296, "x":5.26576, "y":5.85841, "heading":-1.96657, "vx":3.54912, "vy":-0.89492, "omega":-1.74298, "ax":0.30632, "ay":-0.10968, "alpha":2.5535, "fx":[13.46379,1.72524,-3.02576,8.67866], "fy":[1.34002,6.16409,-5.08742,-9.87891]}, - {"t":1.6419, "x":5.33304, "y":5.84144, "heading":-1.99959, "vx":3.55492, "vy":-0.897, "omega":-1.69462, "ax":0.13479, "ay":-0.8751, "alpha":2.23167, "fx":[9.66929,-0.57698,-5.05145,5.13019], "fy":[-12.37433,-7.8319,-17.42269,-21.91173]}, - {"t":1.66084, "x":5.40039, "y":5.8243, "heading":-2.03168, "vx":3.55748, "vy":-0.91358, "omega":-1.65235, "ax":-0.36365, "ay":-2.29263, "alpha":1.46379, "fx":[-1.14384,-8.06546,-11.18759,-4.34559], "fy":[-37.64728,-34.45158,-40.37427,-43.51456]}, - {"t":1.67978, "x":5.46771, "y":5.80658, "heading":-2.06298, "vx":3.55059, "vy":-0.957, "omega":-1.62462, "ax":-1.19465, "ay":-4.37166, "alpha":0.41547, "fx":[-18.66399,-21.05631,-21.95445,-19.60821], "fy":[-74.25911,-73.26195,-74.47396,-75.44804]}, - {"t":1.69872, "x":5.53475, "y":5.78767, "heading":-2.09375, "vx":3.52796, "vy":-1.0398, "omega":-1.61675, "ax":-2.14001, "ay":-6.51024, "alpha":-0.43342, "fx":[-37.98719,-35.83818,-34.80377,-36.97454], "fy":[-110.94478,-112.02512,-110.53882,-109.44046]}, - {"t":1.71766, "x":5.60119, "y":5.76681, "heading":-2.12438, "vx":3.48743, "vy":-1.16311, "omega":-1.62496, "ax":-2.82518, "ay":-7.74154, "alpha":-0.65397, "fx":[-51.03848,-46.4788,-45.01828,-49.68664], "fy":[-131.11206,-133.04968,-132.26596,-130.29809]}, - {"t":1.7366, "x":5.66673, "y":5.74339, "heading":-2.15515, "vx":3.43392, "vy":-1.30975, "omega":-1.63735, "ax":-3.37149, "ay":-8.33226, "alpha":-0.61998, "fx":[-60.32928,-55.60586,-54.29426,-59.16324], "fy":[-140.81298,-142.88755,-142.66292,-140.55421]}, - {"t":1.75555, "x":5.73117, "y":5.71709, "heading":-2.18617, "vx":3.37006, "vy":-1.46757, "omega":-1.64909, "ax":-3.92381, "ay":-8.5626, "alpha":-0.68069, "fx":[-70.01304,-64.71118,-63.37455,-68.87279], "fy":[-144.37799,-146.93436,-146.9445,-144.33244]}, - {"t":1.77449, "x":5.7943, "y":5.68775, "heading":-2.2174, "vx":3.29574, "vy":-1.62975, "omega":-1.66198, "ax":-4.51444, "ay":-8.56015, "alpha":-0.92651, "fx":[-81.10529,-73.92375,-72.28724,-79.84078], "fy":[-143.56727,-147.48326,-147.71028,-143.66172]}, - {"t":1.79343, "x":5.85591, "y":5.65535, "heading":-2.24888, "vx":3.21023, "vy":-1.79189, "omega":-1.67953, "ax":-5.10016, "ay":-8.41586, "alpha":-1.23752, "fx":[-92.23666,-82.85228,-80.93385,-90.98615], "fy":[-140.07818,-145.89923,-146.37451,-140.25336]}, - {"t":1.81237, "x":5.9158, "y":5.6199, "heading":-2.28069, "vx":3.11363, "vy":-1.95129, "omega":-1.70297, "ax":-5.61407, "ay":-8.21401, "alpha":-1.4002, "fx":[-101.39666,-91.05363,-89.1664,-100.35814], "fy":[-135.91497,-143.10458,-143.75067,-136.10177]}, - {"t":1.83131, "x":5.97377, "y":5.58147, "heading":-2.31295, "vx":3.00729, "vy":-2.10687, "omega":-1.72949, "ax":-6.01209, "ay":-8.02378, "alpha":-1.33089, "fx":[-107.68711,-98.05337,-96.46173,-106.85374], "fy":[-132.59968,-139.91191,-140.59643,-132.82065]}, - {"t":1.85025, "x":6.02965, "y":5.54012, "heading":-2.34571, "vx":2.89342, "vy":-2.25885, "omega":-1.7547, "ax":-5.99615, "ay":-8.12819, "alpha":0.62686, "fx":[-99.20083,-104.04234,-104.69804,-100.02994], "fy":[-140.24185,-136.66554,-136.32167,-139.80336]}, - {"t":1.87707, "x":6.10509, "y":5.47662, "heading":-2.39276, "vx":2.73261, "vy":-2.47683, "omega":-1.73789, "ax":-6.43153, "ay":-7.65315, "alpha":0.89715, "fx":[-105.62439,-112.20626,-113.00607,-106.75715], "fy":[-133.18099,-127.63852,-127.28437,-132.60786]}, - {"t":1.90389, "x":6.17606, "y":5.40745, "heading":-2.43937, "vx":2.56013, "vy":-2.68207, "omega":-1.71383, "ax":-6.81601, "ay":-7.03561, "alpha":0.92478, "fx":[-112.35686,-118.60507,-119.3613,-113.43037], "fy":[-122.85172,-116.76377,-116.624,-122.45545]}, - {"t":1.9307, "x":6.24227, "y":5.33299, "heading":-2.48533, "vx":2.37734, "vy":-2.87075, "omega":-1.68903, "ax":-6.95424, "ay":-6.19493, "alpha":0.84539, "fx":[-115.31422,-120.32947,-121.15594,-116.35859], "fy":[-108.16642,-102.46604,-102.69068,-108.17271]}, - {"t":1.95752, "x":6.30352, "y":5.25377, "heading":-2.53063, "vx":2.19085, "vy":-3.03689, "omega":-1.66636, "ax":-6.05328, "ay":-4.90329, "alpha":1.41821, "fx":[-98.10816,-104.93763,-107.60106,-101.21116], "fy":[-87.17021,-78.41872,-79.88771,-88.13735]}, - {"t":1.98434, "x":6.3601, "y":5.17057, "heading":-2.57532, "vx":2.02851, "vy":-3.16838, "omega":-1.62833, "ax":-4.73218, "ay":-3.38842, "alpha":1.30745, "fx":[-76.11387,-81.15802,-84.73137,-79.96868], "fy":[-60.3551,-53.07572,-55.10637,-62.00681]}, - {"t":2.01116, "x":6.4128, "y":5.08438, "heading":-2.61899, "vx":1.9016, "vy":-3.25925, "omega":-1.59326, "ax":-2.70479, "ay":-1.87572, "alpha":1.69559, "fx":[-40.24408,-44.58973,-51.71647,-47.48049], "fy":[-33.58136,-26.31993,-30.3354,-37.38545]}, - {"t":2.03798, "x":6.46282, "y":4.9963, "heading":-2.66171, "vx":1.82907, "vy":-3.30955, "omega":-1.54779, "ax":0.59205, "ay":1.50122, "alpha":-0.74784, "fx":[8.80923,8.95152,11.50078,11.0206], "fy":[26.08034,22.00824,24.94448,29.10828]}, - {"t":2.06479, "x":6.51209, "y":4.90808, "heading":-2.70322, "vx":1.84494, "vy":-3.26929, "omega":-1.56785, "ax":-0.94672, "ay":8.31378, "alpha":-1.231, "fx":[-22.42067,-18.87627,-9.53044,-13.58662], "fy":[141.33642,139.78856,141.46884,143.06632]}, - {"t":2.09161, "x":6.56123, "y":4.8234, "heading":-2.74527, "vx":1.81955, "vy":-3.04634, "omega":-1.60086, "ax":-4.90909, "ay":8.65161, "alpha":6.50402, "fx":[-55.57898,-59.17181,-103.50173,-115.75636], "fy":[161.98237,162.03175,138.10447,126.52701]}, - {"t":2.11843, "x":6.60826, "y":4.74481, "heading":-2.7882, "vx":1.6879, "vy":-2.81432, "omega":-1.42643, "ax":-5.10148, "ay":8.67865, "alpha":6.09509, "fx":[-62.65521,-62.11329,-104.28884,-118.04183], "fy":[161.60039,162.46663,139.28204,127.1363]}, - {"t":2.14525, "x":6.65169, "y":4.67246, "heading":-2.82645, "vx":1.55109, "vy":-2.58158, "omega":-1.26298, "ax":-5.17165, "ay":8.6983, "alpha":5.6893, "fx":[-66.98056,-63.61214,-103.49961,-117.78108], "fy":[160.63514,162.41726,140.46924,128.30082]}, - {"t":2.17206, "x":6.69143, "y":4.60636, "heading":-2.86032, "vx":1.4124, "vy":-2.34831, "omega":-1.1104, "ax":-5.20913, "ay":8.71356, "alpha":5.33656, "fx":[-70.16683,-64.72676,-102.50783,-117.02151], "fy":[159.66692,162.24683,141.49522,129.45151]}, - {"t":2.19888, "x":6.72743, "y":4.54651, "heading":-2.8901, "vx":1.2727, "vy":-2.11463, "omega":-0.96729, "ax":-5.23285, "ay":8.72608, "alpha":5.02518, "fx":[-72.68564,-65.70807,-101.53837,-116.10483], "fy":[158.77798,162.01635,142.37266,130.54533]}, - {"t":2.2257, "x":6.75968, "y":4.49294, "heading":-2.91604, "vx":1.13237, "vy":-1.88061, "omega":-0.83252, "ax":-5.24938, "ay":8.73681, "alpha":4.74287, "fx":[-74.74997,-66.65008,-100.64289,-115.11865], "fy":[157.9795,161.74195,143.12687,131.59396]}, - {"t":2.25252, "x":6.78816, "y":4.44565, "heading":-2.93837, "vx":0.99159, "vy":-1.64631, "omega":-0.70533, "ax":-5.26161, "ay":8.74632, "alpha":4.48028, "fx":[-76.47914,-67.59659,-99.83035,-114.08813], "fy":[157.26699,161.42865,143.78005,132.61386]}, - {"t":2.27934, "x":6.81286, "y":4.40464, "heading":-2.95728, "vx":0.85048, "vy":-1.41175, "omega":-0.58518, "ax":-5.27105, "ay":8.75498, "alpha":4.23044, "fx":[-77.94949,-68.5706,-99.09674,-113.01917], "fy":[156.63208,161.07788,144.35035,133.61884]}, - {"t":2.30615, "x":6.83377, "y":4.36993, "heading":-2.97298, "vx":0.70913, "vy":-1.17696, "omega":-0.47173, "ax":-5.27852, "ay":8.76304, "alpha":3.98831, "fx":[-79.21419,-69.58432,-98.43385,-111.91179], "fy":[156.06572,160.6901,144.85251,134.61888]}, - {"t":2.33297, "x":6.85089, "y":4.34152, "heading":-2.98563, "vx":0.56757, "vy":-0.94195, "omega":-0.36477, "ax":-5.28453, "ay":8.77063, "alpha":3.75045, "fx":[-80.31212,-70.64243,-97.83261,-110.76594], "fy":[155.55946,160.26611,145.29851,135.61947]}, - {"t":2.35979, "x":6.86421, "y":4.31941, "heading":-2.99541, "vx":0.42585, "vy":-0.70674, "omega":-0.26419, "ax":-5.2894, "ay":8.77782, "alpha":3.5151, "fx":[-81.27208,-71.74252,-97.28477,-109.58517], "fy":[155.10607,159.80835,145.69785,136.62076]}, - {"t":2.38661, "x":6.87373, "y":4.30361, "heading":-3.0025, "vx":0.284, "vy":-0.47134, "omega":-0.16992, "ax":-5.29334, "ay":8.78463, "alpha":3.28227, "fx":[-82.11484,-72.87419,-96.78396,-108.37954], "fy":[154.70002,159.3221,146.05752,137.61652]}, - {"t":2.41343, "x":6.87944, "y":4.29413, "heading":-3.00705, "vx":0.14204, "vy":-0.23576, "omega":-0.0819, "ax":-5.29649, "ay":8.79101, "alpha":3.05382, "fx":[-82.85456,-74.01885,-96.32644,-107.16705], "fy":[154.33771,158.81626,146.38202,138.59409]}, - {"t":2.44024, "x":6.88135, "y":4.29097, "heading":-3.00925, "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":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.88913, "ay":3.13608, "alpha":-42.99403, "fx":[190.04369,138.4564,-8.60736,-60.57299], "fy":[-13.85691,134.43823,190.08233,-101.55616]}, + {"t":0.02967, "x":0.44804, "y":4.02738, "heading":0.0, "vx":0.1154, "vy":0.09305, "omega":-1.27572, "ax":9.28618, "ay":7.50996, "alpha":-0.45841, "fx":[156.75556,154.60388,152.81142,155.01402], "fy":[122.74622,125.45176,127.62144,124.93025]}, + {"t":0.05934, "x":0.45555, "y":4.03345, "heading":-0.03785, "vx":0.39094, "vy":0.31589, "omega":-1.28932, "ax":9.285, "ay":7.50906, "alpha":-0.26904, "fx":[155.93346,154.70436,153.61154,154.85673], "fy":[123.73548,125.27313,126.60588,125.0753]}, + {"t":0.08902, "x":0.47124, "y":4.04613, "heading":-0.07611, "vx":0.66644, "vy":0.5387, "omega":-1.2973, "ax":9.28323, "ay":7.5077, "alpha":-0.0307, "fx":[154.87963,154.74347,154.61429,154.75064], "fy":[124.98572,125.15484,125.31377,125.14477]}, + {"t":0.11869, "x":0.4951, "y":4.06542, "heading":-0.1146, "vx":0.94189, "vy":0.76147, "omega":-1.29821, "ax":9.28046, "ay":7.50556, "alpha":0.27829, "fx":[153.49461,154.69008,155.90024,154.71855], "fy":[126.59832,125.12829,123.6237,125.10576]}, + {"t":0.14836, "x":0.52713, "y":4.09131, "heading":-0.15312, "vx":1.21726, "vy":0.98417, "omega":-1.28996, "ax":9.27589, "ay":7.50198, "alpha":0.69451, "fx":[151.60917,154.49066,157.60299,154.79576], "fy":[128.73909,125.24654,121.32579,124.90645]}, + {"t":0.17803, "x":0.56733, "y":4.12382, "heading":-0.1914, "vx":1.4925, "vy":1.20677, "omega":-1.26935, "ax":9.26773, "ay":7.49557, "alpha":1.28494, "fx":[148.90988,154.04313,159.96083,155.04083], "fy":[131.69914,125.60873,118.02357,124.45876]}, + {"t":0.2077, "x":0.6157, "y":4.16293, "heading":-0.22906, "vx":1.76749, "vy":1.42918, "omega":-1.23122, "ax":9.25146, "ay":7.48274, "alpha":2.18655, "fx":[144.74636,153.12234,163.44036,155.56038], "fy":[136.03897,126.42771,112.87092,123.59733]}, + {"t":0.23738, "x":0.67222, "y":4.20863, "heading":-0.2656, "vx":2.042, "vy":1.65121, "omega":-1.16634, "ax":9.21263, "ay":7.4525, "alpha":3.7288, "fx":[137.52827,151.10547,169.07141,156.57503], "fy":[142.9773,128.26937,103.70635,121.96549]}, + {"t":0.26705, "x":0.73686, "y":4.2609, "heading":-0.3002, "vx":2.31536, "vy":1.87234, "omega":-1.0557, "ax":9.08315, "ay":7.35928, "alpha":6.94154, "fx":[122.2368,145.37059,179.40567,158.63399], "fy":[155.60473,133.33996,83.14283,118.61481]}, + {"t":0.29672, "x":0.80956, "y":4.3197, "heading":-0.33153, "vx":2.58487, "vy":2.0907, "omega":-0.84973, "ax":7.96436, "ay":6.98981, "alpha":17.60624, "fx":[75.43166,96.61161,195.52412,163.48074], "fy":[181.41985,163.89613,10.72895,110.02246]}, + {"t":0.32639, "x":0.88977, "y":4.38481, "heading":-0.35674, "vx":2.82119, "vy":2.2981, "omega":-0.32732, "ax":3.80804, "ay":2.68034, "alpha":11.00204, "fx":[44.65665,30.93334,86.45098,91.87211], "fy":[82.70079,34.36279,5.88361,55.77279]}, + {"t":0.35606, "x":0.97515, "y":4.45418, "heading":-0.36645, "vx":2.93418, "vy":2.37763, "omega":-0.00087, "ax":0.07708, "ay":-0.09132, "alpha":0.00355, "fx":[1.20377,1.32146,1.38427,1.23036], "fy":[-1.5111,-1.58333,-1.5249,-1.46989]}, + {"t":0.38574, "x":1.06225, "y":4.52469, "heading":-0.36648, "vx":2.93647, "vy":2.37492, "omega":-0.00076, "ax":0.01796, "ay":-0.0222, "alpha":0.0, "fx":[0.27915,0.31037,0.31992,0.28777], "fy":[-0.37615,-0.38968,-0.36348,-0.35121]}, + {"t":0.41541, "x":1.14939, "y":4.59515, "heading":-0.3665, "vx":2.937, "vy":2.37427, "omega":-0.00076, "ax":-0.00615, "ay":0.00761, "alpha":0.0, "fx":[-0.10851,-0.09935,-0.0964,-0.1057], "fy":[0.12451,0.12044,0.12914,0.13299]}, + {"t":0.44508, "x":1.23653, "y":4.6656, "heading":-0.36653, "vx":2.93682, "vy":2.37449, "omega":-0.00076, "ax":-0.01549, "ay":0.01916, "alpha":0.0, "fx":[-0.25678,-0.25891,-0.25971,-0.25758], "fy":[0.31997,0.3208,0.31869,0.31787]}, + {"t":0.47475, "x":1.32367, "y":4.73606, "heading":-0.36655, "vx":2.93636, "vy":2.37506, "omega":-0.00076, "ax":0.04631, "ay":-0.05729, "alpha":0.0, "fx":[0.7787,0.76943,0.76509,0.77453], "fy":[-0.95127,-0.948,-0.95847,-0.96203]}, + {"t":0.50442, "x":1.41082, "y":4.80651, "heading":-0.36657, "vx":2.93773, "vy":2.37336, "omega":-0.00076, "ax":0.42511, "ay":-0.52909, "alpha":0.0, "fx":[7.09941,7.08523,7.07164,7.0889], "fy":[-8.80875,-8.80644,-8.82969,-8.83409]}, + {"t":0.5341, "x":1.49817, "y":4.8767, "heading":-0.36659, "vx":2.95035, "vy":2.35766, "omega":-0.00076, "ax":1.79709, "ay":-2.30612, "alpha":-0.00377, "fx":[29.98309,30.07956,29.89906,29.86466], "fy":[-38.31165,-38.41257,-38.54516,-38.49821]}, + {"t":0.56377, "x":1.58651, "y":4.94564, "heading":-0.36662, "vx":3.00367, "vy":2.28923, "omega":-0.00087, "ax":2.09246, "ay":-7.69605, "alpha":-6.71571, "fx":[46.10063,66.51467,17.52132,9.38468], "fy":[-139.35094,-120.50752,-115.5918,-137.70734]}, + {"t":0.59344, "x":1.67655, "y":5.01018, "heading":-0.36664, "vx":3.06576, "vy":2.06088, "omega":-0.20014, "ax":-2.81595, "ay":-8.26016, "alpha":-15.85793, "fx":[-12.88208,28.60845,-106.66565,-96.82272], "fy":[-170.15251,-143.904,-96.54452,-140.1703]}, + {"t":0.61474, "x":1.74123, "y":5.05221, "heading":-0.37091, "vx":3.00577, "vy":1.8849, "omega":-0.53798, "ax":-3.21784, "ay":-6.12325, "alpha":-13.7128, "fx":[-28.87914,-2.84596,-87.88781,-94.94649], "fy":[-141.06139,-95.34012,-60.0147,-111.87002]}, + {"t":0.63605, "x":1.80453, "y":5.09098, "heading":-0.38237, "vx":2.93721, "vy":1.75445, "omega":-0.83012, "ax":-2.48369, "ay":-3.30284, "alpha":-8.87435, "fx":[-28.49885,-12.27197,-55.92649,-68.91041], "fy":[-82.89093,-45.19246,-26.88037,-65.26303]}, + {"t":0.65735, "x":1.86654, "y":5.1276, "heading":-0.40005, "vx":2.8843, "vy":1.68409, "omega":-1.01918, "ax":-1.37907, "ay":-1.11257, "alpha":-4.19162, "fx":[-17.52455,-9.47973,-28.64527,-36.30413], "fy":[-31.92332,-13.40527,-5.13115,-23.72409]}, + {"t":0.67866, "x":1.92768, "y":5.16323, "heading":-0.42176, "vx":2.85492, "vy":1.66038, "omega":-1.10848, "ax":-0.61844, "ay":0.03244, "alpha":-1.41262, "fx":[-8.60723,-5.73337,-12.02965,-14.86609], "fy":[-4.00881,2.22429,5.09459,-1.14727]}, + {"t":0.69996, "x":1.98836, "y":5.19861, "heading":-0.44538, "vx":2.84175, "vy":1.66108, "omega":-1.13857, "ax":-0.24479, "ay":0.49407, "alpha":-0.17408, "fx":[-3.89898,-3.50331,-4.26468,-4.65517], "fy":[7.67045,8.41947,8.80273,8.05129]}, + {"t":0.72126, "x":2.04884, "y":5.23411, "heading":-0.46964, "vx":2.83653, "vy":1.6716, "omega":-1.14228, "ax":-0.14847, "ay":0.60934, "alpha":0.12037, "fx":[-2.61681,-2.86169,-2.334,-2.08735], "fy":[10.5514,10.019,9.76388,10.2952]}, + {"t":0.74257, "x":2.10924, "y":5.26986, "heading":-0.49397, "vx":2.83337, "vy":1.68458, "omega":-1.13972, "ax":-0.22023, "ay":0.58196, "alpha":-0.08924, "fx":[-3.59418,-3.37018,-3.74873,-3.97134], "fy":[9.40784,9.7834,9.99431,9.6183]}, + {"t":0.76387, "x":2.16955, "y":5.30588, "heading":-0.51825, "vx":2.82868, "vy":1.69698, "omega":-1.14162, "ax":-0.3764, "ay":0.51965, "alpha":-0.51926, "fx":[-5.81409,-4.54258,-6.73559,-8.0051], "fy":[6.94003,9.13311,10.38417,8.19209]}, + {"t":0.78518, "x":2.22973, "y":5.34215, "heading":-0.54257, "vx":2.82066, "vy":1.70805, "omega":-1.15268, "ax":-0.55766, "ay":0.47222, "alpha":-0.99678, "fx":[-8.48522,-5.95643,-10.10859,-12.63326], "fy":[4.54222,8.70126,11.19939,7.04413]}, + {"t":0.80648, "x":2.28969, "y":5.37865, "heading":-0.56713, "vx":2.80878, "vy":1.71811, "omega":-1.17392, "ax":-0.72099, "ay":0.45503, "alpha":-1.41787, "fx":[-10.98077,-7.24335,-13.06007,-16.78971], "fy":[2.8198,8.65167,12.34706,6.522]}, + {"t":0.82778, "x":2.34937, "y":5.41535, "heading":-0.59214, "vx":2.79342, "vy":1.72781, "omega":-1.20412, "ax":-0.82848, "ay":0.45994, "alpha":-1.71277, "fx":[-12.70194,-8.01199,-14.92432,-19.60323], "fy":[1.88049,8.81252,13.44934,6.52528]}, + {"t":0.84909, "x":2.40869, "y":5.45227, "heading":-0.61779, "vx":2.77577, "vy":1.7376, "omega":-1.24061, "ax":-0.83537, "ay":0.45377, "alpha":-1.82233, "fx":[-12.90438,-7.72416,-14.95141,-20.12076], "fy":[1.38318,8.62514,13.74271,6.50558]}, + {"t":0.87039, "x":2.46764, "y":5.48939, "heading":-0.64422, "vx":2.75797, "vy":1.74727, "omega":-1.27944, "ax":-0.67788, "ay":0.36096, "alpha":-1.69099, "fx":[-10.50483,-5.51509,-12.09794,-17.08192], "fy":[0.26858,6.85107,11.76715,5.18136]}, + {"t":0.8917, "x":2.52624, "y":5.52669, "heading":-0.67148, "vx":2.74353, "vy":1.75496, "omega":-1.31546, "ax":-0.26179, "ay":0.02288, "alpha":-1.28537, "fx":[-3.88769,0.06415,-4.83928,-8.79285], "fy":[-3.98334,0.90766,4.75237,-0.15135]}, + {"t":0.913, "x":2.58463, "y":5.56409, "heading":-0.6995, "vx":2.73795, "vy":1.75545, "omega":-1.34284, "ax":0.54223, "ay":-0.87281, "alpha":-0.64888, "fx":[9.17997,11.31731,8.90066,6.75667], "fy":[-16.71724,-14.30315,-12.37305,-14.80395]}, + {"t":0.93431, "x":2.64308, "y":5.60129, "heading":-0.72811, "vx":2.7495, "vy":1.73685, "omega":-1.35667, "ax":1.85347, "ay":-2.8488, "alpha":0.0153, "fx":[30.80664,30.93067,30.9906,30.85766], "fy":[-47.34181,-47.39197,-47.6268,-47.59203]}, + {"t":0.95561, "x":2.70208, "y":5.63764, "heading":-0.75701, "vx":2.78899, "vy":1.67616, "omega":-1.35634, "ax":3.45108, "ay":-6.19927, "alpha":0.33412, "fx":[57.4897,56.42198,57.57188,58.62785], "fy":[-102.13646,-103.34314,-104.53288,-103.34321]}, + {"t":0.97691, "x":2.76228, "y":5.67195, "heading":-0.78591, "vx":2.86251, "vy":1.54409, "omega":-1.34922, "ax":4.0425, "ay":-8.88375, "alpha":0.0938, "fx":[67.52031,66.96246,67.25636,67.80678], "fy":[-147.86284,-148.20628,-148.31068,-147.97141]}, + {"t":0.99822, "x":2.82418, "y":5.70282, "heading":-0.81465, "vx":2.94863, "vy":1.35483, "omega":-1.34723, "ax":3.76573, "ay":-10.13418, "alpha":-0.00864, "fx":[62.78049,62.79677,62.76657,62.74774], "fy":[-168.96669,-168.93833,-168.8966,-168.92602]}, + {"t":1.01952, "x":2.88785, "y":5.72939, "heading":-0.84336, "vx":3.02886, "vy":1.13893, "omega":-1.34741, "ax":3.90571, "ay":-10.10441, "alpha":2.1371, "fx":[70.23562,53.17271,60.68962,76.32708], "fy":[-165.30813,-172.32988,-171.54057,-164.56416]}, + {"t":1.04171, "x":2.95603, "y":5.75218, "heading":-0.87326, "vx":3.11553, "vy":0.9147, "omega":-1.29998, "ax":3.29079, "ay":-9.19937, "alpha":2.80793, "fx":[60.56947,40.26375,50.01039,68.5799], "fy":[-148.18801,-156.97076,-158.40189,-149.83569]}, + {"t":1.06391, "x":3.02598, "y":5.77021, "heading":-0.90211, "vx":3.18856, "vy":0.71054, "omega":-1.23767, "ax":2.06397, "ay":-6.74238, "alpha":2.02394, "fx":[36.0938,26.45206,32.80136,42.27428], "fy":[-106.38754,-112.48678,-118.25773,-112.43693]}, + {"t":1.0861, "x":3.09725, "y":5.78432, "heading":-0.92957, "vx":3.23437, "vy":0.56092, "omega":-1.19275, "ax":0.47965, "ay":-3.18292, "alpha":-0.15849, "fx":[7.87814,8.62907,8.11686,7.35815], "fy":[-53.50266,-53.08006,-52.60778,-53.0403]}, + {"t":1.10829, "x":3.16914, "y":5.79598, "heading":-0.95604, "vx":3.24501, "vy":0.49028, "omega":-1.19627, "ax":-0.47904, "ay":-0.90512, "alpha":-1.78745, "fx":[-9.0406,-1.8271,-6.92478,-14.1489], "fy":[-21.05245,-16.11427,-9.10226,-14.08266]}, + {"t":1.13048, "x":3.24104, "y":5.80664, "heading":-0.98259, "vx":3.23438, "vy":0.4702, "omega":-1.23594, "ax":-0.89605, "ay":0.24562, "alpha":-2.58523, "fx":[-16.70302,-6.16548,-13.18337,-23.69523], "fy":[-4.59417,2.35576,12.79928,5.81649]}, + {"t":1.15267, "x":3.31259, "y":5.81713, "heading":-1.01002, "vx":3.2145, "vy":0.47565, "omega":-1.29331, "ax":-0.98961, "ay":0.78086, "alpha":-2.80713, "fx":[-18.71397,-7.04888,-14.30892,-25.91345], "fy":[3.60593,10.88756,22.43017,15.14227]}, + {"t":1.17487, "x":3.38369, "y":5.82788, "heading":-1.03872, "vx":3.19253, "vy":0.49298, "omega":-1.35561, "ax":-0.91059, "ay":1.01926, "alpha":-2.70283, "fx":[-17.60151,-6.14617,-12.79274,-24.17619], "fy":[7.98945,14.69448,25.98547,19.29301]}, + {"t":1.19706, "x":3.45431, "y":5.83907, "heading":-1.0688, "vx":3.17233, "vy":0.51559, "omega":-1.41559, "ax":-0.75343, "ay":1.12111, "alpha":-2.4496, "fx":[-15.00888,-4.43519,-10.14197,-20.65126], "fy":[10.60414,16.36554,26.76231,21.02122]}, + {"t":1.21925, "x":3.52453, "y":5.85079, "heading":-1.10022, "vx":3.15561, "vy":0.54047, "omega":-1.46995, "ax":-0.57212, "ay":1.15656, "alpha":-2.15567, "fx":[-11.91187,-2.45229,-7.1862,-16.59735], "fy":[12.24155,17.00852,26.30667,21.56019]}, + {"t":1.24144, "x":3.59441, "y":5.86307, "heading":-1.13284, "vx":3.14291, "vy":0.56614, "omega":-1.51779, "ax":-0.38879, "ay":1.14683, "alpha":-1.86629, "fx":[-8.72453,-0.41415,-4.25279,-12.53236], "fy":[13.0975,16.946,25.1283,21.29641]}, + {"t":1.26363, "x":3.66407, "y":5.87592, "heading":-1.16652, "vx":3.13428, "vy":0.59159, "omega":-1.5592, "ax":-0.20047, "ay":1.08244, "alpha":-1.57189, "fx":[-5.38913,1.70147,-1.30195,-8.37758], "fy":[13.04036,16.03769,23.04173,20.05533]}, + {"t":1.28583, "x":3.73357, "y":5.88931, "heading":-1.20112, "vx":3.12983, "vy":0.61561, "omega":-1.59409, "ax":0.01277, "ay":0.92681, "alpha":-1.22219, "fx":[-1.50394,4.0742,1.92818,-3.64707], "fy":[11.61467,13.75063,19.28168,17.15076]}, + {"t":1.30802, "x":3.80303, "y":5.9032, "heading":-1.2365, "vx":3.13012, "vy":0.63618, "omega":-1.62121, "ax":0.27599, "ay":0.61029, "alpha":-0.75165, "fx":[3.46283,6.93432,5.74007,2.26499], "fy":[7.85293,9.04558,12.49342,11.30122]}, + {"t":1.33021, "x":3.87256, "y":5.91747, "heading":-1.27248, "vx":3.13624, "vy":0.64972, "omega":-1.63789, "ax":0.5998, "ay":0.02009, "alpha":-0.12723, "fx":[9.78239,10.38901,10.21663,9.60557], "fy":[-0.04475,0.13639,0.71519,0.53276]}, + {"t":1.3524, "x":3.94231, "y":5.93189, "heading":-1.30883, "vx":3.14955, "vy":0.65017, "omega":-1.64072, "ax":0.94542, "ay":-0.99372, "alpha":0.56532, "fx":[16.72675,14.0823,14.79569,17.4339], "fy":[-14.86583,-15.58786,-18.26364,-17.54215]}, + {"t":1.37459, "x":4.01244, "y":5.94608, "heading":-1.34524, "vx":3.17053, "vy":0.62812, "omega":-1.62817, "ax":1.17222, "ay":-2.50285, "alpha":1.01159, "fx":[21.42748,16.58587,17.66132,22.48659], "fy":[-38.76545,-39.90731,-44.67662,-43.5357]}, + {"t":1.39679, "x":4.08309, "y":5.9594, "heading":-1.38137, "vx":3.19655, "vy":0.57257, "omega":-1.60572, "ax":1.03957, "ay":-4.03265, "alpha":0.7055, "fx":[18.74148,15.29571,15.92247,19.35703], "fy":[-65.23109,-65.90432,-69.2128,-68.54123]}, + {"t":1.41898, "x":4.15428, "y":5.97111, "heading":-1.417, "vx":3.21962, "vy":0.48308, "omega":-1.59006, "ax":0.46519, "ay":-4.69731, "alpha":-0.5667, "fx":[6.44519,9.48097,9.077,6.01469], "fy":[-79.72152,-79.28694,-76.87947,-77.31967]}, + {"t":1.44117, "x":4.22585, "y":5.98068, "heading":-1.45229, "vx":3.22994, "vy":0.37884, "omega":-1.60264, "ax":-0.27649, "ay":-4.94195, "alpha":-2.22119, "fx":[-9.67414,1.90988,0.56274,-11.23426], "fy":[-87.8149,-86.73909,-76.91333,-78.05246]}, + {"t":1.46336, "x":4.29746, "y":5.98787, "heading":-1.48786, "vx":3.2238, "vy":0.26917, "omega":-1.65193, "ax":-0.77691, "ay":-5.1899, "alpha":-3.16274, "fx":[-20.37641,-3.84097,-5.31974,-22.26545], "fy":[-93.885,-93.10528,-79.07583,-79.98677]}, + {"t":1.48555, "x":4.36881, "y":5.99256, "heading":-1.52452, "vx":3.20656, "vy":0.15399, "omega":-1.72212, "ax":-0.58565, "ay":-6.58257, "alpha":-1.99068, "fx":[-14.99097,-3.80075,-4.40976,-15.84834], "fy":[-113.8176,-113.69596,-105.61811,-105.78121]}, + {"t":1.50775, "x":4.43982, "y":5.99436, "heading":-1.56273, "vx":3.19357, "vy":0.00791, "omega":-1.7663, "ax":1.30317, "ay":-7.90379, "alpha":5.0801, "fx":[39.78702,5.61972,5.2799,36.20625], "fy":[-122.05663,-124.51233,-141.51408,-138.92633]}, + {"t":1.53114, "x":4.51488, "y":5.99238, "heading":-1.60405, "vx":3.22405, "vy":-0.17697, "omega":-1.64747, "ax":1.11293, "ay":-5.06677, "alpha":4.05446, "fx":[30.34868,8.17813,7.19352,28.48753], "fy":[-75.55582,-75.62227,-93.48234,-93.18182]}, + {"t":1.55453, "x":4.5906, "y":5.98686, "heading":-1.64259, "vx":3.25008, "vy":-0.29549, "omega":-1.55263, "ax":0.58744, "ay":-3.96961, "alpha":2.44891, "fx":[16.72095,3.94735,2.99268,15.50854], "fy":[-60.91071,-60.28095,-71.46958,-72.02463]}, + {"t":1.57792, "x":4.66679, "y":5.97886, "heading":-1.6789, "vx":3.26382, "vy":-0.38835, "omega":-1.49534, "ax":0.02565, "ay":-4.14796, "alpha":1.41483, "fx":[4.48056,-2.83237,-3.58278,3.64456], "fy":[-66.27813,-65.57894,-72.02047,-72.70022]}, + {"t":1.60131, "x":4.74314, "y":5.96864, "heading":-1.71388, "vx":3.26442, "vy":-0.48537, "omega":-1.46225, "ax":-0.56882, "ay":-4.36266, "alpha":0.34278, "fx":[-8.49638,-10.23656,-10.46515,-8.72944], "fy":[-72.06528,-71.8154,-73.38211,-73.63131]}, + {"t":1.6247, "x":4.81935, "y":5.95609, "heading":-1.74809, "vx":3.25112, "vy":-0.58742, "omega":-1.45423, "ax":-1.18922, "ay":-4.32651, "alpha":-0.96482, "fx":[-22.65033,-17.70048,-16.97278,-21.97121], "fy":[-73.77708,-74.73972,-70.46676,-69.5]}, + {"t":1.6481, "x":4.89507, "y":5.94117, "heading":-1.7821, "vx":3.2233, "vy":-0.68863, "omega":-1.4768, "ax":-1.78969, "ay":-4.21174, "alpha":-2.25079, "fx":[-36.41268,-25.11595,-23.13284,-34.67141], "fy":[-73.82847,-76.59891,-66.58982,-63.81385]}, + {"t":1.67149, "x":4.96998, "y":5.92391, "heading":-1.81665, "vx":3.18144, "vy":-0.78715, "omega":-1.52945, "ax":-2.23334, "ay":-3.99839, "alpha":-3.10223, "fx":[-46.32874,-31.14972,-27.93007,-43.50648], "fy":[-71.41586,-75.80617,-61.88213,-57.50082]}, + {"t":1.69488, "x":5.04379, "y":5.9044, "heading":-1.85242, "vx":3.1292, "vy":-0.88068, "omega":-1.60201, "ax":-2.14127, "ay":-3.0631, "alpha":-2.77981, "fx":[-43.98826,-30.84025,-27.30599,-40.64109], "fy":[-55.37415,-59.50478,-46.74122,-42.62126]}, + {"t":1.71827, "x":5.1164, "y":5.88296, "heading":-1.8899, "vx":3.07911, "vy":-0.95233, "omega":-1.66704, "ax":-1.36697, "ay":-1.38729, "alpha":-1.13386, "fx":[-26.25157,-21.03012,-19.31361,-24.55164], "fy":[-24.87155,-26.63482,-21.37951,-19.6159]}, + {"t":1.74166, "x":5.18805, "y":5.86031, "heading":-1.92889, "vx":3.04713, "vy":-0.98478, "omega":-1.69356, "ax":-0.42172, "ay":-0.04256, "alpha":0.79043, "fx":[-4.53871,-8.17185,-9.51655,-5.89217], "fy":[0.39544,1.76209,-1.81512,-3.1801]}, + {"t":1.76505, "x":5.25921, "y":5.83726, "heading":-1.96851, "vx":3.03727, "vy":-0.98577, "omega":-1.67507, "ax":0.37736, "ay":0.74755, "alpha":2.34564, "fx":[13.78292,3.21166,-1.20995,9.37675], "fy":[15.48639,19.92179,9.43869,4.99812]}, + {"t":1.78845, "x":5.33036, "y":5.81441, "heading":-2.00769, "vx":3.04609, "vy":-0.96829, "omega":-1.6202, "ax":0.9223, "ay":1.09868, "alpha":3.31804, "fx":[26.10539,11.40783,4.60377,19.38031], "fy":[22.16172,29.03194,14.47284,7.59126]}, + {"t":1.81184, "x":5.40187, "y":5.79206, "heading":-2.04559, "vx":3.06767, "vy":-0.94259, "omega":-1.54259, "ax":1.1718, "ay":1.11182, "alpha":3.65689, "fx":[31.50045,15.59073,7.5107,23.53182], "fy":[22.31412,30.50363,14.75648,6.55965]}, + {"t":1.83523, "x":5.47394, "y":5.77031, "heading":-2.08167, "vx":3.09508, "vy":-0.91658, "omega":-1.45705, "ax":1.07189, "ay":0.76632, "alpha":3.31647, "fx":[28.86821,14.68013,6.83499,21.08847], "fy":[15.81511,23.73355,9.72682,1.82145]}, + {"t":1.85862, "x":5.54664, "y":5.74908, "heading":-2.11576, "vx":3.12015, "vy":-0.89865, "omega":-1.37947, "ax":0.51514, "ay":-0.16351, "alpha":2.21272, "fx":[16.06591,6.6788,1.1134,10.49057], "fy":[-0.97356,4.60013,-4.49435,-10.0346]}, + {"t":1.88201, "x":5.61976, "y":5.72802, "heading":-2.14802, "vx":3.1322, "vy":-0.90248, "omega":-1.32771, "ax":-0.66751, "ay":-2.19579, "alpha":0.32332, "fx":[-9.92636,-11.47846,-12.31982,-10.78336], "fy":[-36.48504,-35.61903,-36.72738,-37.57986]}, + {"t":1.9054, "x":5.69285, "y":5.7063, "heading":-2.17908, "vx":3.11659, "vy":-0.95384, "omega":-1.32015, "ax":-2.53736, "ay":-5.81535, "alpha":-1.81333, "fx":[-48.80048,-40.66043,-35.74264,-43.98261], "fy":[-97.50721,-102.67577,-96.4295,-91.14383]}, + {"t":1.92879, "x":5.76506, "y":5.6824, "heading":-2.20996, "vx":3.05724, "vy":-1.08987, "omega":-1.36256, "ax":-3.99323, "ay":-8.79389, "alpha":-2.21646, "fx":[-76.45554,-61.63658,-56.08405,-72.08499], "fy":[-143.70038,-151.40562,-149.68201,-141.57186]}, + {"t":1.95219, "x":5.83548, "y":5.6545, "heading":-2.24183, "vx":2.96383, "vy":-1.29558, "omega":-1.41441, "ax":-4.98699, "ay":-9.62796, "alpha":-2.12276, "fx":[-92.86034,-77.37645,-72.59367,-89.6924], "fy":[-156.19491,-164.74497,-165.06152,-155.97256]}, + {"t":1.97558, "x":5.90344, "y":5.62156, "heading":-2.27492, "vx":2.84717, "vy":-1.52079, "omega":-1.46407, "ax":-6.18377, "ay":-9.47657, "alpha":-3.03555, "fx":[-115.69569,-94.24117,-88.77234,-113.61245], "fy":[-150.45939,-164.91971,-166.32808,-150.17242]}, + {"t":1.99897, "x":5.96835, "y":5.5834, "heading":-2.30917, "vx":2.70253, "vy":-1.74246, "omega":-1.53507, "ax":-6.99194, "ay":-9.18735, "alpha":-3.04158, "fx":[-128.31441,-107.40592,-103.10309,-127.38571], "fy":[-144.72247,-160.92987,-162.65166,-144.29075]}, + {"t":2.02236, "x":6.02965, "y":5.54012, "heading":-2.34507, "vx":2.53897, "vy":-1.95737, "omega":-1.60622, "ax":-7.05986, "ay":-9.21587, "alpha":-0.38118, "fx":[-119.29243,-116.48686,-116.04961,-118.90898], "fy":[-152.44984,-154.62004,-154.81435,-152.61213]}, + {"t":2.05098, "x":6.09943, "y":5.48033, "heading":-2.39105, "vx":2.33691, "vy":-2.22113, "omega":-1.61713, "ax":-7.48953, "ay":-8.22329, "alpha":0.10134, "fx":[-124.4484,-125.16255,-125.23817,-124.53834], "fy":[-137.42533,-136.77268,-136.7374,-137.37765]}, + {"t":2.0796, "x":6.16325, "y":5.41339, "heading":-2.43733, "vx":2.12256, "vy":-2.45649, "omega":-1.61423, "ax":-3.69481, "ay":-5.67025, "alpha":5.70051, "fx":[-40.01545,-64.18298,-82.52243,-59.64179], "fy":[-100.43521,-76.03406,-89.79229,-111.8199]}, + {"t":2.10822, "x":6.22248, "y":5.34076, "heading":-2.48353, "vx":2.01681, "vy":-2.61878, "omega":-1.45108, "ax":-3.1408, "ay":-3.48921, "alpha":2.71561, "fx":[-42.89826,-52.07669,-61.731,-52.71626], "fy":[-60.26866,-48.94724,-56.20949,-67.22834]}, + {"t":2.13684, "x":6.27892, "y":5.26438, "heading":-2.52506, "vx":1.92692, "vy":-2.71864, "omega":-1.37335, "ax":-3.69051, "ay":-2.18649, "alpha":-0.76001, "fx":[-64.05217,-61.83891,-58.97929,-61.20584], "fy":[-35.89542,-39.06895,-37.00821,-33.81863]}, + {"t":2.16546, "x":6.33256, "y":5.18567, "heading":-2.56437, "vx":1.82129, "vy":-2.78122, "omega":-1.3951, "ax":-4.04206, "ay":-1.33536, "alpha":-2.83698, "fx":[-76.52385,-69.04806,-58.12171,-65.82312], "fy":[-19.87687,-32.12466,-24.74548,-12.29207]}, + {"t":2.19409, "x":6.38303, "y":5.10553, "heading":-2.6043, "vx":1.70561, "vy":-2.81944, "omega":-1.4763, "ax":-3.16304, "ay":-0.68569, "alpha":-2.29294, "fx":[-60.22428,-54.63693,-45.18481,-50.85957], "fy":[-9.46884,-19.19968,-13.4361,-3.61591]}, + {"t":2.22271, "x":6.43055, "y":5.02455, "heading":-2.64655, "vx":1.61508, "vy":-2.83906, "omega":-1.54193, "ax":-0.59129, "ay":-0.2096, "alpha":1.17644, "fx":[-5.96118,-8.72711,-13.74553,-10.99245], "fy":[-4.76746,0.37668,-2.25809,-7.3269]}, + {"t":2.25133, "x":6.47653, "y":4.94321, "heading":-2.69068, "vx":1.59815, "vy":-2.84506, "omega":-1.50826, "ax":1.4253, "ay":-0.34301, "alpha":4.53234, "fx":[38.37295,28.98774,9.03112,18.64461], "fy":[-10.86828,9.0666,-0.55637,-20.5135]}, + {"t":2.27995, "x":6.52285, "y":4.86164, "heading":-2.73385, "vx":1.63895, "vy":-2.85488, "omega":-1.37854, "ax":2.12974, "ay":-0.3244, "alpha":5.16553, "fx":[51.72309,42.1461,19.10584,29.03166], "fy":[-11.92854,11.28754,1.23594,-22.22502]}, + {"t":2.30857, "x":6.57063, "y":4.7798, "heading":-2.7733, "vx":1.6999, "vy":-2.86416, "omega":-1.2307, "ax":1.13667, "ay":1.79445, "alpha":-1.21695, "fx":[15.10083,16.54493,23.14966,20.99568], "fy":[31.1636,26.22018,28.78485,33.48191]}, + {"t":2.33719, "x":6.61975, "y":4.69856, "heading":-2.80853, "vx":1.73243, "vy":-2.8128, "omega":-1.26553, "ax":-2.83623, "ay":8.93876, "alpha":-0.74547, "fx":[-50.36804,-49.29299,-43.96503,-45.48827], "fy":[148.98588,147.36743,149.08471,150.58149]}, + {"t":2.36581, "x":6.66817, "y":4.62172, "heading":-2.84475, "vx":1.65126, "vy":-2.55697, "omega":-1.28686, "ax":-6.3372, "ay":9.74433, "alpha":7.03636, "fx":[-82.26444,-75.406,-122.80609,-142.07581], "fy":[177.90499,181.99018,154.22831,135.60948]}, + {"t":2.39443, "x":6.71284, "y":4.55253, "heading":-2.88158, "vx":1.46988, "vy":-2.27808, "omega":-1.08548, "ax":-6.38485, "ay":9.85009, "alpha":6.31674, "fx":[-87.03652,-77.65451,-121.1212,-139.91723], "fy":[177.53571,182.3194,156.9492,139.98049]}, + {"t":2.42305, "x":6.75229, "y":4.49136, "heading":-2.91264, "vx":1.28715, "vy":-1.99616, "omega":-0.90469, "ax":-6.40409, "ay":9.89787, "alpha":5.76208, "fx":[-90.31946,-79.43089,-119.5432,-137.71879], "fy":[176.5296,181.9835,158.61732,142.84053]}, + {"t":2.45167, "x":6.78651, "y":4.43828, "heading":-2.93854, "vx":1.10386, "vy":-1.71288, "omega":-0.73977, "ax":-6.41526, "ay":9.92841, "alpha":5.29537, "fx":[-92.81906,-81.07007,-118.19905,-135.66893], "fy":[175.54955,181.47691,159.85185,145.1291]}, + {"t":2.48029, "x":6.81547, "y":4.39332, "heading":-2.95971, "vx":0.92025, "vy":-1.42872, "omega":-0.58821, "ax":-6.42272, "ay":9.95121, "alpha":4.87613, "fx":[-94.8155,-82.69122,-117.04735,-133.70039], "fy":[174.67266,180.87589,160.8346,147.14446]}, + {"t":2.50891, "x":6.83918, "y":4.35651, "heading":-2.97654, "vx":0.73642, "vy":-1.14391, "omega":-0.44866, "ax":-6.42802, "ay":9.96981, "alpha":4.48187, "fx":[-96.45919,-84.34923,-116.04454,-131.75482], "fy":[173.89965,180.19744,161.6508,149.01974]}, + {"t":2.53753, "x":6.85763, "y":4.32785, "heading":-2.98939, "vx":0.55245, "vy":-0.85857, "omega":-0.32038, "ax":-6.43183, "ay":9.9858, "alpha":4.10032, "fx":[-97.84158,-86.06486,-115.15641,-129.79921], "fy":[173.21831,179.44838,162.34941,150.81785]}, + {"t":2.56616, "x":6.8708, "y":4.30737, "heading":-2.99855, "vx":0.36837, "vy":-0.57277, "omega":-0.20303, "ax":-6.43447, "ay":9.99991, "alpha":3.72736, "fx":[-99.01871,-87.82807,-114.35977,-127.83164], "fy":[172.61777,178.64051,162.95982,152.55673]}, + {"t":2.59478, "x":6.87871, "y":4.29507, "heading":-3.00437, "vx":0.18421, "vy":-0.28656, "omega":-0.09635, "ax":-6.43611, "ay":10.01239, "alpha":3.36636, "fx":[-100.02185,-89.59719,-113.64325,-125.88484], "fy":[172.09263,177.7979,163.49769,154.21886]}, + {"t":2.6234, "x":6.88135, "y":4.29097, "heading":-3.00712, "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/FiveMeterTestPath.traj b/src/main/deploy/choreo/FiveMeterTestPath.traj index c7979702..8ab66ea4 100644 --- a/src/main/deploy/choreo/FiveMeterTestPath.traj +++ b/src/main/deploy/choreo/FiveMeterTestPath.traj @@ -3,20 +3,24 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.0, "y":0.0, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.0, "y":0.0, "heading":0.0, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.0, "y":0.0, "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":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5 m", "val":5.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -24,41 +28,129 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.58285], + "waypoints":[0.0,6.00122], "samples":[ - {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":10.30754, "ay":0.0, "alpha":0.0, "fx":[175.32826,175.32826,175.32826,175.32826], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.04946, "x":0.01261, "y":0.0, "heading":0.0, "vx":0.50985, "vy":0.0, "omega":0.0, "ax":10.30628, "ay":0.0, "alpha":0.0, "fx":[175.30682,175.30682,175.30682,175.30682], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.09893, "x":0.05044, "y":0.0, "heading":0.0, "vx":1.01964, "vy":0.0, "omega":0.0, "ax":10.3046, "ay":0.0, "alpha":0.0, "fx":[175.27823,175.27823,175.27823,175.27823], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.14839, "x":0.11348, "y":0.0, "heading":0.0, "vx":1.52935, "vy":0.0, "omega":0.0, "ax":10.30224, "ay":0.0, "alpha":0.0, "fx":[175.23818,175.23818,175.23818,175.23818], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.19786, "x":0.20173, "y":0.0, "heading":0.0, "vx":2.03894, "vy":0.0, "omega":0.0, "ax":10.29871, "ay":0.0, "alpha":0.0, "fx":[175.17807,175.17807,175.17807,175.17807], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.24732, "x":0.31518, "y":0.0, "heading":0.0, "vx":2.54835, "vy":0.0, "omega":0.0, "ax":10.29282, "ay":0.0, "alpha":0.0, "fx":[175.07785,175.07785,175.07785,175.07785], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.29678, "x":0.45383, "y":0.0, "heading":0.0, "vx":3.05748, "vy":0.0, "omega":0.0, "ax":10.28103, "ay":0.0, "alpha":0.0, "fx":[174.87733,174.87733,174.87733,174.87733], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.34625, "x":0.61764, "y":0.0, "heading":0.0, "vx":3.56602, "vy":0.0, "omega":0.0, "ax":10.24571, "ay":0.0, "alpha":0.0, "fx":[174.27652,174.27652,174.27652,174.27652], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.39571, "x":0.80656, "y":0.0, "heading":0.0, "vx":4.07281, "vy":0.0, "omega":0.0, "ax":4.45615, "ay":0.0, "alpha":0.0, "fx":[75.7979,75.7979,75.7979,75.7979], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.44518, "x":1.01347, "y":0.0, "heading":0.0, "vx":4.29323, "vy":0.0, "omega":0.0, "ax":0.00051, "ay":0.0, "alpha":0.0, "fx":[0.00871,0.00871,0.00871,0.00871], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.49464, "x":1.22583, "y":0.0, "heading":0.0, "vx":4.29326, "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.5441, "x":1.43819, "y":0.0, "heading":0.0, "vx":4.29326, "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.59357, "x":1.65055, "y":0.0, "heading":0.0, "vx":4.29326, "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.64303, "x":1.86292, "y":0.0, "heading":0.0, "vx":4.29326, "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.6925, "x":2.07528, "y":0.0, "heading":0.0, "vx":4.29326, "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.74196, "x":2.28764, "y":0.0, "heading":0.0, "vx":4.29326, "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.79142, "x":2.5, "y":0.0, "heading":0.0, "vx":4.29326, "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.84089, "x":2.71236, "y":0.0, "heading":0.0, "vx":4.29326, "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.89035, "x":2.92472, "y":0.0, "heading":0.0, "vx":4.29326, "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.93982, "x":3.13708, "y":0.0, "heading":0.0, "vx":4.29326, "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.98928, "x":3.34945, "y":0.0, "heading":0.0, "vx":4.29326, "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.03874, "x":3.56181, "y":0.0, "heading":0.0, "vx":4.29326, "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.08821, "x":3.77417, "y":0.0, "heading":0.0, "vx":4.29326, "vy":0.0, "omega":0.0, "ax":-0.00051, "ay":0.0, "alpha":0.0, "fx":[-0.00871,-0.00871,-0.00871,-0.00871], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.13767, "x":3.98653, "y":0.0, "heading":0.0, "vx":4.29323, "vy":0.0, "omega":0.0, "ax":-4.45615, "ay":0.0, "alpha":0.0, "fx":[-75.7979,-75.7979,-75.7979,-75.7979], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.18714, "x":4.19344, "y":0.0, "heading":0.0, "vx":4.07281, "vy":0.0, "omega":0.0, "ax":-10.24571, "ay":0.0, "alpha":0.0, "fx":[-174.27652,-174.27652,-174.27652,-174.27652], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.2366, "x":4.38236, "y":0.0, "heading":0.0, "vx":3.56602, "vy":0.0, "omega":0.0, "ax":-10.28103, "ay":0.0, "alpha":0.0, "fx":[-174.87733,-174.87733,-174.87733,-174.87733], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.28606, "x":4.54617, "y":0.0, "heading":0.0, "vx":3.05748, "vy":0.0, "omega":0.0, "ax":-10.29282, "ay":0.0, "alpha":0.0, "fx":[-175.07785,-175.07785,-175.07785,-175.07785], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.33553, "x":4.68482, "y":0.0, "heading":0.0, "vx":2.54835, "vy":0.0, "omega":0.0, "ax":-10.29871, "ay":0.0, "alpha":0.0, "fx":[-175.17807,-175.17807,-175.17807,-175.17807], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.38499, "x":4.79827, "y":0.0, "heading":0.0, "vx":2.03894, "vy":0.0, "omega":0.0, "ax":-10.30224, "ay":0.0, "alpha":0.0, "fx":[-175.23818,-175.23818,-175.23818,-175.23818], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.43446, "x":4.88652, "y":0.0, "heading":0.0, "vx":1.52935, "vy":0.0, "omega":0.0, "ax":-10.3046, "ay":0.0, "alpha":0.0, "fx":[-175.27823,-175.27823,-175.27823,-175.27823], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.48392, "x":4.94956, "y":0.0, "heading":0.0, "vx":1.01964, "vy":0.0, "omega":0.0, "ax":-10.30628, "ay":0.0, "alpha":0.0, "fx":[-175.30682,-175.30682,-175.30682,-175.30682], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.53338, "x":4.98739, "y":0.0, "heading":0.0, "vx":0.50985, "vy":0.0, "omega":0.0, "ax":-10.30754, "ay":0.0, "alpha":0.0, "fx":[-175.32826,-175.32826,-175.32826,-175.32826], "fy":[0.0,0.0,0.0,0.0]}, - {"t":1.58285, "x":5.0, "y":0.0, "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":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99959, "ay":0.0, "alpha":0.00745, "fx":[16.77217,16.43266,16.72304,16.72304], "fy":[-0.02404,0.02404,0.02404,-0.02404]}, + {"t":0.05001, "x":0.00125, "y":0.0, "heading":0.0, "vx":0.04999, "vy":0.0, "omega":0.00037, "ax":0.99979, "ay":0.0, "alpha":0.00019, "fx":[16.73708,16.54679,16.68995,16.68995], "fy":[-0.02306,0.02306,0.02306,-0.02306]}, + {"t":0.10002, "x":0.005, "y":0.0, "heading":0.00002, "vx":0.09999, "vy":0.0, "omega":0.00038, "ax":0.99977, "ay":0.0, "alpha":-0.00911, "fx":[16.68767,16.68767,16.64381,16.64381], "fy":[-0.02246,0.02247,0.02246,-0.02246]}, + {"t":0.15003, "x":0.01125, "y":0.0, "heading":0.00004, "vx":0.14999, "vy":0.0, "omega":-0.00007, "ax":0.99976, "ay":0.0, "alpha":0.00185, "fx":[16.74202,16.52663,16.6967,16.6967], "fy":[-0.02217,0.02217,0.02217,-0.02217]}, + {"t":0.20004, "x":0.02, "y":0.0, "heading":0.00003, "vx":0.19999, "vy":0.0, "omega":0.00002, "ax":0.99974, "ay":0.0, "alpha":-0.00885, "fx":[16.68655,16.68655,16.64397,16.64397], "fy":[-0.02181,0.02181,0.02181,-0.02181]}, + {"t":0.25005, "x":0.03125, "y":0.0, "heading":0.00004, "vx":0.24998, "vy":0.0, "omega":-0.00042, "ax":0.99973, "ay":0.0, "alpha":0.00355, "fx":[16.74892,16.50262,16.70417,16.70417], "fy":[-0.02189,0.02189,0.02189,-0.02189]}, + {"t":0.30006, "x":0.04501, "y":0.0, "heading":0.00001, "vx":0.29998, "vy":0.0, "omega":-0.00025, "ax":0.99971, "ay":0.0, "alpha":-0.0088, "fx":[16.6858,16.6858,16.64347,16.64347], "fy":[-0.02168,0.02168,0.02168,-0.02168]}, + {"t":0.35007, "x":0.06126, "y":0.0, "heading":0.0, "vx":0.34998, "vy":0.0, "omega":-0.00069, "ax":0.99968, "ay":0.0, "alpha":0.00549, "fx":[16.75816,16.47274,16.71305,16.71305], "fy":[-0.02207,0.02207,0.02207,-0.02207]}, + {"t":0.40008, "x":0.08001, "y":0.0, "heading":-0.00003, "vx":0.39997, "vy":0.0, "omega":-0.00041, "ax":0.99966, "ay":0.0, "alpha":-0.00889, "fx":[16.68519,16.68519,16.64241,16.64241], "fy":[-0.0219,0.02191,0.0219,-0.0219]}, + {"t":0.45009, "x":0.10126, "y":0.0, "heading":-0.00005, "vx":0.44996, "vy":0.0, "omega":-0.00086, "ax":0.99962, "ay":0.0, "alpha":0.00796, "fx":[16.77062,16.43332,16.72456,16.72456], "fy":[-0.02254,0.02254,0.02254,-0.02254]}, + {"t":0.5001, "x":0.12502, "y":0.0, "heading":-0.0001, "vx":0.49995, "vy":0.0, "omega":-0.00046, "ax":0.99959, "ay":0.0, "alpha":-0.00904, "fx":[16.68437,16.68437,16.64087,16.64087], "fy":[-0.02227,0.02228,0.02227,-0.02227]}, + {"t":0.55011, "x":0.15127, "y":0.0, "heading":-0.00012, "vx":0.54994, "vy":0.0, "omega":-0.00091, "ax":0.99954, "ay":0.0, "alpha":0.01125, "fx":[16.7868,16.38118,16.73965,16.73965], "fy":[-0.02307,0.02307,0.02307,-0.02307]}, + {"t":0.60012, "x":0.18002, "y":0.0, "heading":-0.00016, "vx":0.59993, "vy":0.0, "omega":-0.00035, "ax":0.99948, "ay":0.0, "alpha":-0.00914, "fx":[16.68282,16.68282,16.63883,16.63883], "fy":[-0.02252,0.02253,0.02251,-0.02252]}, + {"t":0.65013, "x":0.21127, "y":0.0, "heading":-0.00018, "vx":0.64991, "vy":0.0, "omega":-0.0008, "ax":0.9994, "ay":0.0, "alpha":0.01574, "fx":[16.80731,16.31171,16.75954,16.75954], "fy":[-0.02338,0.02338,0.02338,-0.02338]}, + {"t":0.70014, "x":0.24503, "y":0.0, "heading":-0.00022, "vx":0.6999, "vy":0.0, "omega":-0.00002, "ax":0.9993, "ay":0.0, "alpha":-0.00903, "fx":[16.67951,16.67951,16.63602,16.63602], "fy":[-0.02226,0.02227,0.02225,-0.02226]}, + {"t":0.75015, "x":0.28128, "y":0.0, "heading":-0.00022, "vx":0.74987, "vy":0.0, "omega":-0.00047, "ax":0.99914, "ay":0.0, "alpha":0.0219, "fx":[16.83194,16.21926,16.78492,16.78492], "fy":[-0.02301,0.02301,0.02301,-0.02301]}, + {"t":0.80016, "x":0.32003, "y":0.0, "heading":-0.00025, "vx":0.79984, "vy":0.0, "omega":0.00063, "ax":0.99891, "ay":0.0, "alpha":-0.00849, "fx":[16.67184,16.67184,16.63097,16.63097], "fy":[-0.02091,0.02092,0.0209,-0.02091]}, + {"t":0.85017, "x":0.36128, "y":0.0, "heading":-0.00022, "vx":0.84979, "vy":0.0, "omega":0.0002, "ax":0.99851, "ay":0.0, "alpha":0.0287, "fx":[16.84907,16.1183,16.80567,16.80567], "fy":[-0.02124,0.02124,0.02124,-0.02124]}, + {"t":0.90018, "x":0.40502, "y":0.0, "heading":-0.00021, "vx":0.89973, "vy":0.0, "omega":0.00164, "ax":0.99763, "ay":0.0, "alpha":-0.0072, "fx":[16.64737,16.64737,16.61271,16.61272], "fy":[-0.01773,0.01774,0.01771,-0.01773]}, + {"t":0.95019, "x":0.45127, "y":0.0, "heading":-0.00012, "vx":0.94962, "vy":0.0, "omega":0.00128, "ax":0.99423, "ay":0.0, "alpha":0.02853, "fx":[16.7637,16.0711,16.72914,16.72914], "fy":[-0.01692,0.01693,0.01693,-0.01692]}, + {"t":1.0002, "x":0.5, "y":0.0, "heading":-0.00006, "vx":0.99934, "vy":0.0, "omega":0.0027, "ax":0.00915, "ay":0.0, "alpha":-0.00505, "fx":[0.16466,0.16466,0.14036,0.14036], "fy":[-0.01247,0.01241,0.01237,-0.01247]}, + {"t":1.05021, "x":0.54999, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00245, "ax":0.0, "ay":0.0, "alpha":-0.00909, "fx":[-0.01709,0.08766,-0.03538,-0.03538], "fy":[-0.00888,0.00903,0.00903,-0.00888]}, + {"t":1.10022, "x":0.59999, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":0.002, "ax":0.0, "ay":0.0, "alpha":-0.00279, "fx":[0.00672,0.00672,-0.00671,-0.00672], "fy":[-0.00694,0.00681,0.00679,-0.00694]}, + {"t":1.15023, "x":0.64999, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":0.00186, "ax":0.0, "ay":0.0, "alpha":-0.02098, "fx":[-0.09134,0.28866,-0.09866,-0.09866], "fy":[-0.00351,0.00364,0.00364,-0.00351]}, + {"t":1.20024, "x":0.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":0.00081, "ax":0.0, "ay":0.0, "alpha":-0.00132, "fx":[0.00317,0.00318,-0.00317,-0.00318], "fy":[-0.00332,0.00319,0.00318,-0.00332]}, + {"t":1.25025, "x":0.74999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":0.00074, "ax":0.0, "ay":0.0, "alpha":-0.0107, "fx":[-0.04727,0.14836,-0.05054,-0.05055], "fy":[-0.00154,0.00166,0.00166,-0.00154]}, + {"t":1.30026, "x":0.79999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":-0.0006, "fx":[0.00144,0.00144,-0.00144,-0.00144], "fy":[-0.00154,0.00143,0.00142,-0.00154]}, + {"t":1.35027, "x":0.84999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00413, "fx":[-0.01783,0.05655,-0.01936,-0.01936], "fy":[-0.00069,0.0008,0.0008,-0.00069]}, + {"t":1.40028, "x":0.89999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00025, "fx":[0.0006,0.0006,-0.0006,-0.0006], "fy":[-0.00068,0.00058,0.00057,-0.00068]}, + {"t":1.45029, "x":0.94999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00149, "fx":[-0.00639,0.02032,-0.00696,-0.00697], "fy":[-0.00022,0.00033,0.00033,-0.00022]}, + {"t":1.5003, "x":0.99999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00016,0.00016,-0.00016,-0.00016], "fy":[-0.00022,0.00012,0.00012,-0.00022]}, + {"t":1.55032, "x":1.04999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[-0.00266,0.00794,-0.00264,-0.00264], "fy":[0.00006,0.00003,0.00004,0.00006]}, + {"t":1.60033, "x":1.09999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[0.00004,-0.00013,-0.00014,0.00004]}, + {"t":1.65034, "x":1.14999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[-0.00152,0.00378,-0.00113,-0.00113], "fy":[0.00024,-0.00015,-0.00015,0.00025]}, + {"t":1.70035, "x":1.19999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.0002,-0.00029,-0.00029,0.0002]}, + {"t":1.75036, "x":1.24999, "y":0.0, "heading":0.00045, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00115,0.00222,-0.00054,-0.00054], "fy":[0.00035,-0.00026,-0.00026,0.00035]}, + {"t":1.80037, "x":1.29999, "y":0.0, "heading":0.00044, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[0.00028,-0.00037,-0.00038,0.00028]}, + {"t":1.85038, "x":1.34999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00097,0.00146,-0.00024,-0.00025], "fy":[0.0004,-0.00031,-0.00031,0.0004]}, + {"t":1.90039, "x":1.39999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]}, + {"t":1.9504, "x":1.44999, "y":0.0, "heading":0.00042, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00083,0.00098,-0.00007,-0.00008], "fy":[0.00041,-0.00033,-0.00033,0.00041]}, + {"t":2.00041, "x":1.49999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]}, + {"t":2.05042, "x":1.54999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00069,0.00063,0.00003,0.00003], "fy":[0.0004,-0.00032,-0.00032,0.0004]}, + {"t":2.10043, "x":1.59999, "y":0.0, "heading":0.0004, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00034,-0.00034,0.00034,0.00034], "fy":[0.0003,-0.00038,-0.00039,0.0003]}, + {"t":2.15044, "x":1.64999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00056,0.00036,0.0001,0.0001], "fy":[0.00036,-0.00028,-0.00028,0.00036]}, + {"t":2.20045, "x":1.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[0.00026,-0.00034,-0.00035,0.00026]}, + {"t":2.25046, "x":1.74999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00044,0.00018,0.00013,0.00013], "fy":[0.00031,-0.00024,-0.00024,0.00032]}, + {"t":2.30047, "x":1.79999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00021,-0.00029,-0.00029,0.00021]}, + {"t":2.35048, "x":1.84999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[-0.00033,0.00007,0.00013,0.00013], "fy":[0.00026,-0.00018,-0.00018,0.00026]}, + {"t":2.40049, "x":1.89999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00019,-0.00019,0.00019,0.00019], "fy":[0.00016,-0.00023,-0.00023,0.00016]}, + {"t":2.4505, "x":1.94999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00022,0.0,0.00011,0.00011], "fy":[0.0002,-0.00012,-0.00012,0.0002]}, + {"t":2.50051, "x":1.99999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.0001,-0.00016,-0.00017,0.0001]}, + {"t":2.55052, "x":2.04999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00013,-0.00001,0.00007,0.00007], "fy":[0.00013,-0.00006,-0.00006,0.00013]}, + {"t":2.60053, "x":2.09999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00006,-0.00006,0.00006,0.00006], "fy":[0.00003,-0.0001,-0.0001,0.00003]}, + {"t":2.65054, "x":2.14999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00005,0.00001,0.00002,0.00002], "fy":[0.00006,0.0,0.0,0.00006]}, + {"t":2.70055, "x":2.19999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00003,-0.00003,-0.00004,-0.00003]}, + {"t":2.75056, "x":2.24999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00002,0.00005,-0.00004,-0.00004], "fy":[0.0,0.00007,0.00007,0.0]}, + {"t":2.80057, "x":2.29999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[-0.0001,0.00004,0.00003,-0.0001]}, + {"t":2.85058, "x":2.34999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.0001,0.00009,-0.0001,-0.0001], "fy":[-0.00007,0.00014,0.00014,-0.00007]}, + {"t":2.90059, "x":2.39999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[0.00014,0.00014,-0.00014,-0.00014], "fy":[-0.00017,0.00011,0.0001,-0.00016]}, + {"t":2.9506, "x":2.44999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00017,0.00015,-0.00016,-0.00016], "fy":[-0.00014,0.0002,0.0002,-0.00014]}, + {"t":3.00061, "x":2.49999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[0.0002,0.0002,-0.0002,-0.0002], "fy":[-0.00023,0.00018,0.00017,-0.00023]}, + {"t":3.05062, "x":2.55, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00024,0.0002,-0.00022,-0.00022], "fy":[-0.00021,0.00027,0.00027,-0.00021]}, + {"t":3.10063, "x":2.6, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[0.00027,0.00027,-0.00027,-0.00027], "fy":[-0.0003,0.00024,0.00023,-0.0003]}, + {"t":3.15064, "x":2.65, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[0.00032,0.00024,-0.00028,-0.00028], "fy":[-0.00028,0.00033,0.00033,-0.00027]}, + {"t":3.20065, "x":2.7, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00034,0.00034,-0.00034,-0.00034], "fy":[-0.00036,0.00031,0.0003,-0.00036]}, + {"t":3.25066, "x":2.75, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00039,0.00027,-0.00033,-0.00033], "fy":[-0.00034,0.0004,0.0004,-0.00034]}, + {"t":3.30067, "x":2.8, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.0004,0.0004,-0.0004,-0.0004], "fy":[-0.00042,0.00037,0.00037,-0.00042]}, + {"t":3.35068, "x":2.85, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.00048,0.00026,-0.00037,-0.00037], "fy":[-0.00041,0.00046,0.00046,-0.00041]}, + {"t":3.40069, "x":2.9, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.00046,0.00046,-0.00046,-0.00046], "fy":[-0.00049,0.00044,0.00043,-0.00049]}, + {"t":3.4507, "x":2.95, "y":0.0, "heading":0.00027, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00058,0.0002,-0.00039,-0.00039], "fy":[-0.00047,0.00052,0.00052,-0.00047]}, + {"t":3.50071, "x":3.0, "y":0.0, "heading":0.00026, "vx":0.9998, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00052,0.00052,-0.00052,-0.00052], "fy":[-0.00054,0.0005,0.00049,-0.00054]}, + {"t":3.55072, "x":3.05, "y":0.0, "heading":0.00025, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.0007,0.0001,-0.0004,-0.0004], "fy":[-0.00053,0.00058,0.00058,-0.00053]}, + {"t":3.60073, "x":3.1, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00024, "fx":[0.00058,0.00058,-0.00058,-0.00058], "fy":[-0.0006,0.00055,0.00054,-0.0006]}, + {"t":3.65074, "x":3.15, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00081,-0.00004,-0.00039,-0.00039], "fy":[-0.00058,0.00063,0.00063,-0.00058]}, + {"t":3.70075, "x":3.2, "y":0.0, "heading":0.00023, "vx":0.9998, "vy":0.0, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00062,0.00062,-0.00062,-0.00062], "fy":[-0.00064,0.0006,0.00059,-0.00064]}, + {"t":3.75076, "x":3.25, "y":0.0, "heading":0.00022, "vx":0.9998, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00093,-0.00022,-0.00035,-0.00035], "fy":[-0.00062,0.00067,0.00067,-0.00062]}, + {"t":3.80077, "x":3.3, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[0.00066,0.00066,-0.00066,-0.00066], "fy":[-0.00068,0.00064,0.00063,-0.00068]}, + {"t":3.85078, "x":3.35, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00105,-0.00047,-0.00029,-0.00029], "fy":[-0.00066,0.0007,0.0007,-0.00066]}, + {"t":3.90079, "x":3.4, "y":0.0, "heading":0.00018, "vx":0.9998, "vy":0.0, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.0007,0.00066,0.00065,-0.0007]}, + {"t":3.9508, "x":3.45, "y":0.0, "heading":0.00017, "vx":0.9998, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00117,-0.00078,-0.0002,-0.0002], "fy":[-0.00067,0.00071,0.00071,-0.00067]}, + {"t":4.00081, "x":3.5, "y":0.0, "heading":0.00016, "vx":0.9998, "vy":0.0, "omega":-0.00027, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.00069,0.00066,0.00065,-0.00069]}, + {"t":4.05082, "x":3.55, "y":0.0, "heading":0.00014, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":-0.00015, "fx":[0.00128,-0.00117,-0.00005,-0.00005], "fy":[-0.00066,0.00069,0.00069,-0.00066]}, + {"t":4.10083, "x":3.6, "y":0.0, "heading":0.00013, "vx":0.9998, "vy":0.0, "omega":-0.00029, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00065,0.00065,-0.00065,-0.00065], "fy":[-0.00066,0.00063,0.00062,-0.00066]}, + {"t":4.15084, "x":3.65, "y":0.0, "heading":0.00011, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00139,-0.00172,0.00016,0.00016], "fy":[-0.00061,0.00064,0.00064,-0.00061]}, + {"t":4.20085, "x":3.7, "y":0.0, "heading":0.0001, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00023, "fx":[0.00057,0.00057,-0.00057,-0.00057], "fy":[-0.00058,0.00055,0.00054,-0.00058]}, + {"t":4.25086, "x":3.75, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00159,-0.00272,0.00056,0.00056], "fy":[-0.00051,0.00054,0.00055,-0.00051]}, + {"t":4.30087, "x":3.8, "y":0.0, "heading":0.00007, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00044,0.00044,-0.00044,-0.00044], "fy":[-0.00044,0.00042,0.0004,-0.00044]}, + {"t":4.35088, "x":3.85, "y":0.0, "heading":0.00005, "vx":0.9998, "vy":0.0, "omega":-0.00033, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00216,-0.00509,0.00147,0.00147], "fy":[-0.00035,0.00038,0.00038,-0.00035]}, + {"t":4.40089, "x":3.9, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.0002,0.00018,-0.00022]}, + {"t":4.4509, "x":3.95, "y":0.0, "heading":0.00002, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00078, "fx":[0.00408,-0.01188,0.0039,0.0039], "fy":[-0.0001,0.00013,0.00013,-0.0001]}, + {"t":4.50091, "x":4.0, "y":0.0, "heading":0.0, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.00014,-0.00017,-0.00018,0.00014]}, + {"t":4.55093, "x":4.05, "y":0.0, "heading":-0.00001, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00231, "fx":[0.01035,-0.03229,0.01097,0.01097], "fy":[0.00029,-0.00026,-0.00026,0.00029]}, + {"t":4.60094, "x":4.1, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00032, "fx":[-0.00075,-0.00075,0.00075,0.00075], "fy":[0.00078,-0.00079,-0.00081,0.00078]}, + {"t":4.65095, "x":4.15, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00642, "fx":[0.02846,-0.08921,0.03038,0.03037], "fy":[0.00092,-0.0009,-0.0009,0.00092]}, + {"t":4.70096, "x":4.2, "y":0.0, "heading":-0.00004, "vx":0.9998, "vy":0.0, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":0.00081, "fx":[-0.00194,-0.00194,0.00194,0.00194], "fy":[0.002,-0.00201,-0.00202,0.002]}, + {"t":4.75097, "x":4.25, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":0.01524, "fx":[0.06758,-0.21167,0.07205,0.07205], "fy":[0.00217,-0.00216,-0.00216,0.00217]}, + {"t":4.80098, "x":4.3, "y":0.0, "heading":-0.00002, "vx":0.9998, "vy":0.0, "omega":0.00097, "ax":0.0, "ay":0.0, "alpha":0.00183, "fx":[-0.00441,-0.00441,0.00441,0.00441], "fy":[0.00452,-0.00452,-0.00453,0.00452]}, + {"t":4.85099, "x":4.35, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":0.00107, "ax":0.0, "ay":0.0, "alpha":0.02102, "fx":[0.08606,-0.28009,0.09701,0.09701], "fy":[0.00534,-0.00534,-0.00534,0.00534]}, + {"t":4.901, "x":4.4, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00212, "ax":0.0, "ay":0.0, "alpha":0.00372, "fx":[-0.00892,-0.00892,0.00898,0.00898], "fy":[0.00917,-0.00917,-0.00916,0.00917]}, + {"t":4.95101, "x":4.45, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":0.0023, "ax":-0.00899, "ay":0.0, "alpha":-0.00661, "fx":[-0.2199,0.00956,-0.19446,-0.19446], "fy":[0.01242,-0.01243,-0.01243,0.01242]}, + {"t":5.00102, "x":4.49999, "y":0.0, "heading":0.0003, "vx":0.99935, "vy":0.0, "omega":0.00197, "ax":-0.99419, "ay":0.0, "alpha":0.006, "fx":[-16.58714,-16.58714,-16.55829,-16.55829], "fy":[0.01477,-0.01478,-0.01476,0.01477]}, + {"t":5.05103, "x":4.54872, "y":0.0, "heading":0.0004, "vx":0.94963, "vy":0.0, "omega":0.00227, "ax":-0.99763, "ay":0.0, "alpha":-0.02999, "fx":[-16.83337,-16.09669,-16.7949,-16.7949], "fy":[0.01881,-0.01881,-0.01881,0.01881]}, + {"t":5.10104, "x":4.59497, "y":0.0, "heading":0.00051, "vx":0.89974, "vy":0.0, "omega":0.00077, "ax":-0.99851, "ay":0.0, "alpha":0.00763, "fx":[-16.66302,-16.66302,-16.62628,-16.62628], "fy":[0.01882,-0.01883,-0.0188,0.01882]}, + {"t":5.15105, "x":4.63871, "y":0.0, "heading":0.00055, "vx":0.8498, "vy":0.0, "omega":0.00115, "ax":-0.99891, "ay":0.0, "alpha":-0.02646, "fx":[-16.84564,-16.1563,-16.80181,-16.80181], "fy":[0.02143,-0.02143,-0.02143,0.02143]}, + {"t":5.20106, "x":4.67996, "y":0.0, "heading":0.00061, "vx":0.79985, "vy":0.0, "omega":-0.00017, "ax":-0.99914, "ay":0.0, "alpha":0.00835, "fx":[-16.67535,-16.67535,-16.63516,-16.63516], "fy":[0.02059,-0.0206,-0.02058,0.02059]}, + {"t":5.25107, "x":4.71872, "y":0.0, "heading":0.0006, "vx":0.74988, "vy":0.0, "omega":0.00025, "ax":-0.99929, "ay":0.0, "alpha":-0.01953, "fx":[-16.81976,-16.26147,-16.77491,-16.77491], "fy":[0.02193,-0.02193,-0.02193,0.02193]}, + {"t":5.30108, "x":4.75497, "y":0.0, "heading":0.00061, "vx":0.69991, "vy":0.0, "omega":-0.00073, "ax":-0.9994, "ay":0.0, "alpha":0.00844, "fx":[-16.67982,-16.67982,-16.63922,-16.63922], "fy":[0.0208,-0.0208,-0.02079,0.0208]}, + {"t":5.35109, "x":4.78872, "y":0.0, "heading":0.00058, "vx":0.64993, "vy":0.0, "omega":-0.00031, "ax":-0.99948, "ay":0.0, "alpha":-0.0144, "fx":[-16.79597,-16.34261,-16.75235,-16.75235], "fy":[0.02133,-0.02133,-0.02133,0.02133]}, + {"t":5.4011, "x":4.81997, "y":0.0, "heading":0.00056, "vx":0.59994, "vy":0.0, "omega":-0.00103, "ax":-0.99954, "ay":0.0, "alpha":0.00815, "fx":[-16.68144,-16.68144,-16.64221,-16.64221], "fy":[0.0201,-0.02011,-0.0201,0.0201]}, + {"t":5.45111, "x":4.84873, "y":0.0, "heading":0.00051, "vx":0.54995, "vy":0.0, "omega":-0.00062, "ax":-0.99959, "ay":0.0, "alpha":-0.01077, "fx":[-16.77648,-16.40346,-16.73526,-16.73526], "fy":[0.02015,-0.02015,-0.02015,0.02015]}, + {"t":5.50112, "x":4.87498, "y":0.0, "heading":0.00048, "vx":0.49996, "vy":0.0, "omega":-0.00116, "ax":-0.99962, "ay":0.0, "alpha":0.00768, "fx":[-16.68174,-16.68174,-16.64478,-16.64478], "fy":[0.01895,-0.01895,-0.01894,0.01895]}, + {"t":5.55113, "x":4.89873, "y":0.0, "heading":0.00042, "vx":0.44997, "vy":0.0, "omega":-0.00077, "ax":-0.99966, "ay":0.0, "alpha":-0.00829, "fx":[-16.76126,-16.44805,-16.72294,-16.72294], "fy":[0.01873,-0.01873,-0.01873,0.01873]}, + {"t":5.60114, "x":4.91999, "y":0.0, "heading":0.00038, "vx":0.39998, "vy":0.0, "omega":-0.00119, "ax":-0.99968, "ay":0.0, "alpha":0.00714, "fx":[-16.68141,-16.68141,-16.64709,-16.64709], "fy":[0.0176,-0.0176,-0.0176,0.0176]}, + {"t":5.65115, "x":4.93874, "y":0.0, "heading":0.00032, "vx":0.34999, "vy":0.0, "omega":-0.00083, "ax":-0.99971, "ay":0.0, "alpha":-0.00654, "fx":[-16.74906,-16.48199,-16.71375,-16.71375], "fy":[0.01725,-0.01725,-0.01725,0.01725]}, + {"t":5.70116, "x":4.95499, "y":0.0, "heading":0.00028, "vx":0.29999, "vy":0.0, "omega":-0.00116, "ax":-0.99973, "ay":0.0, "alpha":0.00658, "fx":[-16.68079,-16.68079,-16.64915,-16.64915], "fy":[0.01623,-0.01623,-0.01623,0.01623]}, + {"t":5.75117, "x":4.96874, "y":0.0, "heading":0.00022, "vx":0.24999, "vy":0.0, "omega":-0.00083, "ax":-0.99974, "ay":0.0, "alpha":-0.00531, "fx":[-16.73944,-16.5076,-16.707,-16.707], "fy":[0.01585,-0.01585,-0.01585,0.01585]}, + {"t":5.80118, "x":4.98, "y":0.0, "heading":0.00018, "vx":0.2, "vy":0.0, "omega":-0.00109, "ax":-0.99976, "ay":0.0, "alpha":0.00606, "fx":[-16.68008,-16.68008,-16.65095,-16.65095], "fy":[0.01495,-0.01495,-0.01494,0.01495]}, + {"t":5.85119, "x":4.98875, "y":0.0, "heading":0.00013, "vx":0.15, "vy":0.0, "omega":-0.00079, "ax":-0.99977, "ay":0.0, "alpha":-0.00444, "fx":[-16.73178,-16.52731,-16.70193,-16.70193], "fy":[0.01458,-0.01458,-0.01458,0.01458]}, + {"t":5.9012, "x":4.995, "y":0.0, "heading":0.00009, "vx":0.1, "vy":0.0, "omega":-0.00101, "ax":-0.99979, "ay":0.0, "alpha":0.0056, "fx":[-16.67939,-16.67939,-16.65249,-16.65249], "fy":[0.01381,-0.01381,-0.01381,0.01381]}, + {"t":5.95121, "x":4.99875, "y":0.0, "heading":0.00004, "vx":0.05, "vy":0.0, "omega":-0.00073, "ax":-0.9998, "ay":0.0, "alpha":0.01467, "fx":[-16.72458,-16.72458,-16.69857,-16.51676], "fy":[0.01301,-0.01301,-0.01301,0.013]}, + {"t":6.00122, "x":5.0, "y":0.0, "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/constants.traj b/src/main/deploy/choreo/constants.traj index 5e811376..b204ec95 100644 --- a/src/main/deploy/choreo/constants.traj +++ b/src/main/deploy/choreo/constants.traj @@ -8,24 +8,24 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"305.5 in", "val":7.7597}, "y":{"exp":"4.359700000000012 m", "val":4.359700000000012}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"7.2570308 m", "val":7.2570308}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":6506, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":4133, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":1315, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21807, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"305.5 in", "val":7.7597}, "y":{"exp":"4.359700000000012 m", "val":4.359700000000012}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"7.2570308 m", "val":7.2570308}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":59, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":93, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":231, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"61.8667 in", "val":1.57141418}, "y":{"exp":"291.9457 in", "val":7.41542078}, "heading":{"exp":"-54.011392 deg", "val":-0.9426766239853251}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/fetchToA.traj b/src/main/deploy/choreo/fetchToA.traj index cf21135e..67b2739b 100644 --- a/src/main/deploy/choreo/fetchToA.traj +++ b/src/main/deploy/choreo/fetchToA.traj @@ -4,7 +4,7 @@ "snapshot":{ "waypoints":[ {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -16,7 +16,7 @@ "params":{ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "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}, @@ -30,48 +30,47 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.9966,1.25852], + "waypoints":[0.0,1.03509,1.27754], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.03207, "ay":-8.55381, "alpha":15.13169, "fx":[136.76558,-21.71623,39.56227,119.72551], "fy":[-109.32572,-173.83389,-170.78855,-128.04357]}, - {"t":0.03215, "x":1.5735, "y":7.411, "heading":-0.94268, "vx":0.12962, "vy":-0.27499, "omega":0.48646, "ax":4.04237, "ay":-8.63623, "alpha":14.35199, "fx":[133.32416,-17.1468,40.50067,118.36041], "fy":[-113.4526,-174.31685,-170.54969,-129.28009]}, - {"t":0.0643, "x":1.57975, "y":7.3977, "heading":-0.92704, "vx":0.25958, "vy":-0.55263, "omega":0.94785, "ax":4.06225, "ay":-8.73154, "alpha":13.35159, "fx":[128.35389,-11.65886,42.51312,117.1827], "fy":[-118.9922,-174.74062,-170.03613,-130.31492]}, - {"t":0.09644, "x":1.5902, "y":7.37542, "heading":-0.89657, "vx":0.39017, "vy":-0.83334, "omega":1.37708, "ax":4.08606, "ay":-8.84036, "alpha":12.1154, "fx":[121.59573,-5.01192,45.5229,115.90423], "fy":[-125.82764,-175.02474,-169.22583,-131.40966]}, - {"t":0.12859, "x":1.60485, "y":7.34406, "heading":-0.8523, "vx":0.52153, "vy":-1.11754, "omega":1.76657, "ax":4.10875, "ay":-8.96143, "alpha":10.62628, "fx":[112.85682,3.2065,49.43313,114.05851], "fy":[-133.6538,-175.02826,-168.08621,-132.95698]}, - {"t":0.16074, "x":1.62374, "y":7.3035, "heading":-0.7955, "vx":0.65362, "vy":-1.40563, "omega":2.10819, "ax":4.12839, "ay":-9.08994, "alpha":8.85778, "fx":[102.26444,13.645,54.1255,110.85591], "fy":[-141.85347,-174.47565,-166.57976,-135.56024]}, - {"t":0.19289, "x":1.64689, "y":7.25362, "heading":-0.72773, "vx":0.78634, "vy":-1.69786, "omega":2.39295, "ax":4.148, "ay":-9.21731, "alpha":6.72017, "fx":[90.5748,27.34673,59.4603,104.8432], "fy":[-149.52207,-172.79133,-164.6683,-140.15325]}, - {"t":0.22504, "x":1.67431, "y":7.19427, "heading":-0.6508, "vx":0.9197, "vy":-1.99418, "omega":2.60899, "ax":4.17041, "ay":-9.3297, "alpha":3.91616, "fx":[79.32444,46.09029,65.27696,93.05792], "fy":[-155.72205,-168.66251,-162.3114,-148.08632]}, - {"t":0.25719, "x":1.70603, "y":7.12534, "heading":-0.56692, "vx":1.05377, "vy":-2.29411, "omega":2.73489, "ax":4.17323, "ay":-9.38413, "alpha":-0.32495, "fx":[70.59249,72.97407,71.39368,68.98147], "fy":[-159.81385,-158.7322,-159.4363,-160.50331]}, - {"t":0.28933, "x":1.74207, "y":7.04674, "heading":-0.479, "vx":1.18793, "vy":-2.5958, "omega":2.72445, "ax":4.05956, "ay":-9.17327, "alpha":-7.4088, "fx":[66.4462,112.17443,77.61777,19.9696], "fy":[-161.52513,-133.65133,-155.6836,-173.27884]}, - {"t":0.32148, "x":1.78236, "y":6.95855, "heading":-0.39142, "vx":1.31844, "vy":-2.8907, "omega":2.48626, "ax":3.7059, "ay":-7.96691, "alpha":-17.68241, "fx":[68.37079,157.11578,81.13229,-54.47345], "fy":[-160.6467,-75.31992,-140.61073,-165.48205]}, - {"t":0.35363, "x":1.82666, "y":6.8615, "heading":-0.31149, "vx":1.43758, "vy":-3.14683, "omega":1.91781, "ax":2.62075, "ay":-8.12043, "alpha":-19.27883, "fx":[74.1735,159.34352,-6.19945,-49.00488], "fy":[-157.69624,-69.19324,-158.83859,-166.77638]}, - {"t":0.38578, "x":1.87423, "y":6.75614, "heading":-0.24983, "vx":1.52183, "vy":-3.40789, "omega":1.29802, "ax":2.58608, "ay":-8.10726, "alpha":-19.39961, "fx":[77.98599,157.65851,-20.03758,-39.65305], "fy":[-155.05081,-70.25371,-157.97242,-168.33171]}, - {"t":0.41793, "x":1.92449, "y":6.64239, "heading":-0.2081, "vx":1.60497, "vy":-3.66852, "omega":0.67436, "ax":2.52379, "ay":-7.93776, "alpha":-19.26937, "fx":[77.63198,151.64758,-24.03926,-33.52418], "fy":[-152.4166,-73.87681,-147.3186,-166.46404]}, - {"t":0.45008, "x":1.97739, "y":6.52035, "heading":-0.18642, "vx":1.6861, "vy":-3.92371, "omega":0.05488, "ax":-4.50262, "ay":-2.48567, "alpha":-1.4298, "fx":[-73.3383,-73.62976,-79.86472,-79.52066], "fy":[-47.49438,-39.96494,-37.11816,-44.54466]}, - {"t":0.48222, "x":2.02927, "y":6.39293, "heading":-0.18466, "vx":1.54135, "vy":-4.00362, "omega":0.00891, "ax":-5.23191, "ay":-1.89685, "alpha":-0.01845, "fx":[-88.95562,-88.95928,-89.03098,-89.02732], "fy":[-32.33574,-32.23071,-32.19406,-32.29906]}, - {"t":0.51437, "x":2.07611, "y":6.26324, "heading":-0.18437, "vx":1.37315, "vy":-4.0646, "omega":0.00832, "ax":-2.65718, "ay":-0.86653, "alpha":0.00053, "fx":[-45.19892,-45.19926,-45.19687,-45.19654], "fy":[-14.73785,-14.7405,-14.7411,-14.73846]}, - {"t":0.54652, "x":2.11889, "y":6.13212, "heading":-0.1841, "vx":1.28773, "vy":-4.09245, "omega":0.00834, "ax":-0.82342, "ay":-0.25613, "alpha":0.00002, "fx":[-14.0061,-14.00611,-14.00604,-14.00603], "fy":[-4.35661,-4.35668,-4.35669,-4.35662]}, - {"t":0.57867, "x":2.15986, "y":6.00042, "heading":-0.18384, "vx":1.26126, "vy":-4.10069, "omega":0.00834, "ax":-0.217, "ay":-0.06647, "alpha":0.00005, "fx":[-3.69113,-3.69117,-3.69093,-3.69089], "fy":[-1.1305,-1.13074,-1.13078,-1.13054]}, - {"t":0.61082, "x":2.20029, "y":5.86856, "heading":-0.18357, "vx":1.25428, "vy":-4.10282, "omega":0.00834, "ax":0.19263, "ay":0.05914, "alpha":0.00005, "fx":[3.27653,3.27648,3.27672,3.27676], "fy":[1.00604,1.0058,1.00576,1.00599]}, - {"t":0.64297, "x":2.24072, "y":5.73669, "heading":-0.1833, "vx":1.26048, "vy":-4.10092, "omega":0.00834, "ax":1.29306, "ay":0.40478, "alpha":-0.00023, "fx":[21.99498,21.99517,21.99409,21.9939], "fy":[6.88455,6.88566,6.88587,6.88477]}, - {"t":0.67511, "x":2.28191, "y":5.60506, "heading":-0.18303, "vx":1.30204, "vy":-4.08791, "omega":0.00833, "ax":4.51015, "ay":1.52645, "alpha":-0.00028, "fx":[76.71699,76.71703,76.71588,76.71585], "fy":[25.96347,25.965,25.96546,25.96393]}, - {"t":0.70726, "x":2.3261, "y":5.47443, "heading":-0.18276, "vx":1.44704, "vy":-4.03884, "omega":0.00833, "ax":7.63868, "ay":3.00998, "alpha":0.00739, "fx":[129.91494,129.92672,129.94855,129.93677], "fy":[51.2358,51.18682,51.16196,51.21093]}, - {"t":0.73941, "x":2.37656, "y":5.34614, "heading":-0.1825, "vx":1.69261, "vy":-3.94207, "omega":0.00856, "ax":8.439, "ay":4.24903, "alpha":0.56304, "fx":[141.90118,143.50395,145.15471,143.62018], "fy":[75.4131,71.69977,69.14101,72.84513]}, - {"t":0.77156, "x":2.43534, "y":5.22161, "heading":-0.18222, "vx":1.96391, "vy":-3.80547, "omega":0.02666, "ax":3.95517, "ay":8.47585, "alpha":12.659, "fx":[22.66345,13.8333,134.61876,97.98949], "fy":[169.2782,165.60507,101.4206,140.38294]}, - {"t":0.80371, "x":2.50052, "y":5.10365, "heading":-0.18136, "vx":2.09106, "vy":-3.53299, "omega":0.43363, "ax":1.50739, "ay":9.11837, "alpha":15.82965, "fx":[-16.9609,-63.93138,110.40258,73.05091], "fy":[172.62968,159.23877,131.2225,157.31276]}, - {"t":0.83586, "x":2.56852, "y":4.99478, "heading":-0.16742, "vx":2.13952, "vy":-3.23985, "omega":0.94253, "ax":0.40707, "ay":9.85966, "alpha":9.52994, "fx":[-21.88357,-50.82332,55.21986,45.1833], "fy":[172.7617,165.79699,164.15771,168.12337]}, - {"t":0.868, "x":2.63751, "y":4.89572, "heading":-0.13712, "vx":2.15261, "vy":-2.92288, "omega":1.2489, "ax":-0.84979, "ay":10.21342, "alpha":-0.94374, "fx":[-10.89814,-9.12956,-17.8014,-19.98975], "fy":[174.01209,174.17038,173.51021,173.21681]}, - {"t":0.90015, "x":2.70628, "y":4.80703, "heading":-0.09697, "vx":2.12529, "vy":-2.59453, "omega":1.21856, "ax":-1.40909, "ay":10.04549, "alpha":-5.66362, "fx":[-0.285,4.40156,-40.74631,-59.24347], "fy":[174.51626,174.72154,169.99855,164.24703]}, - {"t":0.9323, "x":2.77387, "y":4.72881, "heading":-0.0578, "vx":2.07999, "vy":-2.27159, "omega":1.03648, "ax":-1.68945, "ay":9.85251, "alpha":-8.55686, "fx":[9.65611,9.83689,-52.41959,-82.02157], "fy":[174.38227,174.68474,166.95931,154.32737]}, - {"t":0.96445, "x":2.83987, "y":4.66088, "heading":-0.02448, "vx":2.02568, "vy":-1.95485, "omega":0.76139, "ax":-1.84431, "ay":9.68627, "alpha":-10.55848, "fx":[18.31046,12.22467,-59.79247,-96.22746], "fy":[173.79564,174.64362,164.57323,146.02995]}, - {"t":0.9966, "x":2.90404, "y":4.60304, "heading":0.0, "vx":1.96638, "vy":-1.64345, "omega":0.42196, "ax":-3.12473, "ay":9.57787, "alpha":-7.40092, "fx":[-25.72623,-16.88667,-71.24541,-98.74453], "fy":[172.97615,174.26867,159.96915,144.4532]}, - {"t":1.02934, "x":2.96674, "y":4.55436, "heading":0.01381, "vx":1.86408, "vy":-1.32987, "omega":0.17965, "ax":-5.19774, "ay":8.75801, "alpha":-5.17506, "fx":[-79.20768,-60.1407,-96.04266,-118.25751], "fy":[155.98559,164.42342,146.43311,129.04258]}, - {"t":1.06208, "x":3.02499, "y":4.51552, "heading":0.0197, "vx":1.69391, "vy":-1.04313, "omega":0.01022, "ax":-6.79339, "ay":7.6768, "alpha":-3.25562, "fx":[-114.42754,-98.58947,-117.26395,-131.93319], "fy":[132.44355,144.69877,130.07651,115.10201]}, - {"t":1.09482, "x":3.07681, "y":4.48548, "heading":0.02003, "vx":1.47149, "vy":-0.79179, "omega":-0.09637, "ax":-7.91384, "ay":6.56959, "alpha":-1.61414, "fx":[-135.27697,-127.3748,-134.27859,-141.51843], "fy":[111.1816,120.18671,112.45478,103.16416]}, - {"t":1.12756, "x":3.12074, "y":4.46308, "heading":0.01688, "vx":1.21239, "vy":-0.5767, "omega":-0.14922, "ax":-8.66546, "ay":5.56349, "alpha":-0.2652, "fx":[-147.59355,-146.42487,-147.21174,-148.35788], "fy":[94.32943,96.13827,94.93595,93.12999]}, - {"t":1.1603, "x":3.15579, "y":4.44718, "heading":0.01199, "vx":0.92868, "vy":-0.39455, "omega":-0.1579, "ax":-9.16211, "ay":4.70017, "alpha":0.80974, "fx":[-155.15741,-158.22144,-156.64242,-153.35808], "fy":[81.38777,75.2452,78.45391,84.70735]}, - {"t":1.19304, "x":3.18129, "y":4.43678, "heading":0.00682, "vx":0.62871, "vy":-0.24067, "omega":-0.13139, "ax":-9.49126, "ay":3.9767, "alpha":1.66344, "fx":[-160.03404,-165.33693,-163.29903,-157.1046], "fy":[71.40854,58.06017,63.50503,77.59641]}, - {"t":1.22578, "x":3.19678, "y":4.43103, "heading":0.00252, "vx":0.31797, "vy":-0.11047, "omega":-0.07693, "ax":-9.71178, "ay":3.37407, "alpha":2.34975, "fx":[-163.33148,-169.60297,-167.87017,-159.97406], "fy":[63.58111,44.17849,50.26139,71.547]}, - {"t":1.25852, "x":3.20199, "y":4.42922, "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.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24603, "ay":-7.86821, "alpha":31.52512, "fx":[197.09967,-97.14283,30.55113,152.60885], "fy":[-26.05183,-173.71935,-196.85068,-128.01496]}, + {"t":0.03339, "x":1.57378, "y":7.41103, "heading":-0.94268, "vx":0.14178, "vy":-0.26272, "omega":1.05263, "ax":4.34122, "ay":-8.32288, "alpha":28.83341, "fx":[192.8711,-85.85911,32.51262,149.93941], "fy":[-47.88162,-179.49501,-196.50175,-131.07519]}, + {"t":0.06678, "x":1.58094, "y":7.39762, "heading":-0.90753, "vx":0.28673, "vy":-0.54062, "omega":2.01538, "ax":4.50812, "ay":-8.92903, "alpha":24.66109, "fx":[182.1097,-67.42507,38.33478,147.57306], "fy":[-79.20159,-187.12275,-195.39876,-133.64756]}, + {"t":0.10017, "x":1.59302, "y":7.37459, "heading":-0.84024, "vx":0.43725, "vy":-0.83876, "omega":2.83881, "ax":4.62241, "ay":-9.71664, "alpha":18.64072, "fx":[156.83369,-39.47,47.70134,143.14811], "fy":[-121.56364,-194.8455,-193.24222,-138.23584]}, + {"t":0.13356, "x":1.6102, "y":7.34117, "heading":-0.74545, "vx":0.5916, "vy":-1.1632, "omega":3.46123, "ax":4.6333, "ay":-10.50408, "alpha":11.13401, "fx":[114.97145,3.8696,60.33313,129.76567], "fy":[-161.63846,-198.61814,-189.51175,-150.62346]}, + {"t":0.16695, "x":1.63253, "y":7.29648, "heading":-0.62988, "vx":0.7463, "vy":-1.51393, "omega":3.83299, "ax":4.66017, "ay":-10.95262, "alpha":0.27287, "fx":[78.10514,76.00762,77.27111,79.34734], "fy":[-182.38985,-183.2811,-182.76382,-181.86499]}, + {"t":0.20034, "x":1.66005, "y":7.23982, "heading":-0.50189, "vx":0.90191, "vy":-1.87964, "omega":3.8421, "ax":4.73038, "ay":-8.88499, "alpha":-21.54828, "fx":[65.97068,171.4458,147.62874,-69.6324], "fy":[-187.21217,-99.43423,-120.36262,-185.42486]}, + {"t":0.23373, "x":1.6928, "y":7.17211, "heading":-0.37361, "vx":1.05986, "vy":-2.17631, "omega":3.1226, "ax":2.92116, "ay":-9.36486, "alpha":-22.17167, "fx":[76.65885,180.07468,6.12119,-68.07719], "fy":[-182.8684,-81.87294,-173.91272,-185.77661]}, + {"t":0.26712, "x":1.72982, "y":7.09422, "heading":-0.26934, "vx":1.15739, "vy":-2.48901, "omega":2.38229, "ax":2.86596, "ay":-9.49345, "alpha":-22.12155, "fx":[86.66845,179.85858,-26.21498,-49.21516], "fy":[-177.88337,-80.69715,-183.28488,-191.13939]}, + {"t":0.30051, "x":1.77006, "y":7.00582, "heading":-0.1898, "vx":1.25309, "vy":-2.80599, "omega":1.64365, "ax":3.13943, "ay":-9.32351, "alpha":-22.42256, "fx":[94.91208,180.50523,-29.90121,-36.18513], "fy":[-172.66862,-75.85574,-180.17407,-192.97512]}, + {"t":0.3339, "x":1.81365, "y":6.90693, "heading":-0.13491, "vx":1.35791, "vy":-3.11731, "omega":0.89496, "ax":3.5182, "ay":-8.89034, "alpha":-22.47239, "fx":[101.0819,178.97434,-20.07277,-25.39633], "fy":[-166.01209,-68.64582,-166.86763,-191.26514]}, + {"t":0.36729, "x":1.86096, "y":6.79789, "heading":-0.10503, "vx":1.47539, "vy":-3.41416, "omega":0.1446, "ax":1.74959, "ay":-0.98, "alpha":-3.82108, "fx":[37.04347,39.47388,21.21779,18.92416], "fy":[-26.38366,-7.82979,-6.0141,-25.11716]}, + {"t":0.40068, "x":1.9112, "y":6.68334, "heading":-0.1002, "vx":1.53381, "vy":-3.44688, "omega":0.01702, "ax":-0.26519, "ay":-0.12028, "alpha":-0.00526, "fx":[-4.40916,-4.40661,-4.43207,-4.43462], "fy":[-2.01898,-1.9935,-1.99093,-2.01642]}, + {"t":0.43407, "x":1.96226, "y":6.56818, "heading":-0.09964, "vx":1.52495, "vy":-3.45089, "omega":0.01684, "ax":-1.90039, "ay":-0.82566, "alpha":-0.01317, "fx":[-31.64976,-31.64491,-31.70758,-31.71243], "fy":[-13.79975,-13.73474,-13.72684,-13.79184]}, + {"t":0.46746, "x":2.01212, "y":6.4525, "heading":-0.09907, "vx":1.4615, "vy":-3.47846, "omega":0.0164, "ax":-2.31293, "ay":-0.94746, "alpha":-0.01083, "fx":[-38.53158,-38.52813,-38.57916,-38.58261], "fy":[-15.82415,-15.77016,-15.76317,-15.81715]}, + {"t":0.50085, "x":2.05963, "y":6.33582, "heading":-0.09853, "vx":1.38427, "vy":-3.5101, "omega":0.01604, "ax":-0.97243, "ay":-0.37772, "alpha":0.0024, "fx":[-16.21519,-16.21628,-16.20469,-16.20361], "fy":[-6.28994,-6.30164,-6.30286,-6.29116]}, + {"t":0.53424, "x":2.10531, "y":6.21841, "heading":-0.09799, "vx":1.3518, "vy":-3.52271, "omega":0.01612, "ax":-0.28792, "ay":-0.10989, "alpha":0.00064, "fx":[-4.80085,-4.80116,-4.79808,-4.79778], "fy":[-1.83016,-1.83325,-1.83355,-1.83047]}, + {"t":0.56763, "x":2.15028, "y":6.10072, "heading":-0.09745, "vx":1.34218, "vy":-3.52638, "omega":0.01614, "ax":-0.12773, "ay":-0.04838, "alpha":0.00051, "fx":[-2.13035,-2.13059,-2.12813,-2.12789], "fy":[-0.80513,-0.80759,-0.80783,-0.80537]}, + {"t":0.60102, "x":2.19503, "y":5.98295, "heading":-0.09691, "vx":1.33792, "vy":-3.52799, "omega":0.01616, "ax":-0.08576, "ay":-0.03226, "alpha":0.00068, "fx":[-1.43106,-1.43138,-1.42809,-1.42777], "fy":[-0.53589,-0.53919,-0.53951,-0.53621]}, + {"t":0.63441, "x":2.23965, "y":5.86513, "heading":-0.09637, "vx":1.33506, "vy":-3.52907, "omega":0.01618, "ax":-0.07137, "ay":-0.02667, "alpha":0.00087, "fx":[-1.19158,-1.19199,-1.18776,-1.18735], "fy":[-0.44219,-0.44642,-0.44683,-0.44259]}, + {"t":0.6678, "x":2.28419, "y":5.74728, "heading":-0.09583, "vx":1.33267, "vy":-3.52996, "omega":0.01621, "ax":-0.06336, "ay":-0.02353, "alpha":0.00102, "fx":[-1.05848,-1.05896,-1.05399,-1.05352], "fy":[-0.38956,-0.39452,-0.395,-0.39004]}, + {"t":0.70119, "x":2.32866, "y":5.6294, "heading":-0.09529, "vx":1.33056, "vy":-3.53075, "omega":0.01625, "ax":-0.05122, "ay":-0.01889, "alpha":0.0011, "fx":[-0.85628,-0.85679,-0.85147,-0.85096], "fy":[-0.31193,-0.31725,-0.31776,-0.31244]}, + {"t":0.73458, "x":2.37305, "y":5.5115, "heading":-0.09475, "vx":1.32885, "vy":-3.53138, "omega":0.01628, "ax":-0.00109, "ay":-0.00003, "alpha":0.00097, "fx":[-0.02027,-0.02071,-0.016,-0.01556], "fy":[0.00207,-0.00264,-0.00309,0.00162]}, + {"t":0.76797, "x":2.41742, "y":5.39359, "heading":-0.0942, "vx":1.32881, "vy":-3.53138, "omega":0.01631, "ax":0.29602, "ay":0.11199, "alpha":-0.00006, "fx":[4.93472,4.93475,4.93445,4.93441], "fy":[1.86667,1.86697,1.86701,1.8667]}, + {"t":0.80136, "x":2.46196, "y":5.27574, "heading":-0.09366, "vx":1.33869, "vy":-3.52764, "omega":0.01631, "ax":2.11039, "ay":0.82439, "alpha":-0.00488, "fx":[35.18997,35.19157,35.16849,35.1669], "fy":[13.72857,13.75278,13.75565,13.73145]}, + {"t":0.83475, "x":2.50783, "y":5.15841, "heading":-0.09312, "vx":1.40916, "vy":-3.50011, "omega":0.01615, "ax":7.43146, "ay":3.31775, "alpha":0.0121, "fx":[123.84946,123.86591,123.90835,123.89191], "fy":[55.35654,55.28158,55.25418,55.32912]}, + {"t":0.86814, "x":2.55903, "y":5.04339, "heading":-0.09258, "vx":1.6573, "vy":-3.38933, "omega":0.01655, "ax":9.13813, "ay":5.97783, "alpha":1.92396, "fx":[145.4419,152.34231,158.87906,152.6496], "fy":[109.67277,97.90274,89.60182,101.41271]}, + {"t":0.90153, "x":2.61946, "y":4.93355, "heading":-0.09202, "vx":1.96242, "vy":-3.18973, "omega":0.0808, "ax":3.85026, "ay":10.20358, "alpha":14.51791, "fx":[10.04811,1.81034,145.85885,99.01084], "fy":[194.78479,190.63982,126.41662,168.51355]}, + {"t":0.93492, "x":2.68713, "y":4.83273, "heading":-0.08933, "vx":2.09098, "vy":-2.84904, "omega":0.56555, "ax":2.54346, "ay":10.70286, "alpha":15.40215, "fx":[-9.50515,-35.66912,130.44702,84.32019], "fy":[197.03823,191.95886,146.18897,178.46009]}, + {"t":0.96831, "x":2.75837, "y":4.74357, "heading":-0.07044, "vx":2.17591, "vy":-2.49167, "omega":1.07983, "ax":0.58976, "ay":11.84469, "alpha":-1.49711, "fx":[17.15511,16.93705,3.02913,2.203], "fy":[197.01869,197.15144,197.85654,197.7545]}, + {"t":1.0017, "x":2.83135, "y":4.66698, "heading":-0.03439, "vx":2.1956, "vy":-2.09617, "omega":1.02984, "ax":-1.11606, "ay":10.85556, "alpha":-17.36681, "fx":[72.6067,38.57372,-64.20136,-121.39582], "fy":[184.15255,194.9137,188.07428,156.68766]}, + {"t":1.03509, "x":2.90404, "y":4.60304, "heading":0.0, "vx":2.15833, "vy":-1.7337, "omega":0.44996, "ax":-3.93079, "ay":10.92711, "alpha":-9.30858, "fx":[-32.46623,-20.25728,-86.74178,-122.63196], "fy":[195.73042,197.73765,178.91191,156.21867]}, + {"t":1.06973, "x":2.97644, "y":4.54954, "heading":0.01558, "vx":2.02219, "vy":-1.35523, "omega":0.12755, "ax":-7.15443, "ay":9.41749, "alpha":-5.24384, "fx":[-114.80865,-90.95848,-124.01062,-147.26558], "fy":[162.13232,176.7925,155.48799,133.52759]}, + {"t":1.10436, "x":3.04219, "y":4.50825, "heading":0.02, "vx":1.77439, "vy":-1.02905, "omega":-0.05408, "ax":-8.94206, "ay":7.86654, "alpha":-2.51381, "fx":[-149.85732,-137.33401,-148.90751,-160.14075], "fy":[130.72408,143.88655,131.9304,117.98447]}, + {"t":1.139, "x":3.09828, "y":4.47733, "heading":0.01813, "vx":1.46467, "vy":-0.75658, "omega":-0.14115, "ax":-9.9204, "ay":6.63748, "alpha":-0.61862, "fx":[-165.8059,-163.01288,-164.98351,-167.67102], "fy":[110.01289,114.12277,111.2717,107.16717]}, + {"t":1.17364, "x":3.14306, "y":4.4551, "heading":0.01324, "vx":1.12107, "vy":-0.52669, "omega":-0.16257, "ax":-10.48383, "ay":5.71315, "alpha":0.69955, "fx":[-174.18199,-176.94308,-175.41121,-172.50564], "fy":[96.36502,91.18468,94.07673,99.3157]}, + {"t":1.20827, "x":3.1756, "y":4.44029, "heading":0.00761, "vx":0.75795, "vy":-0.32881, "omega":-0.13834, "ax":-10.8294, "ay":5.01528, "alpha":1.6448, "fx":[-179.12751,-184.80896,-182.30919,-175.83779], "fy":[86.94944,74.08836,79.98916,93.38227]}, + {"t":1.24291, "x":3.19536, "y":4.43191, "heading":0.00282, "vx":0.38286, "vy":-0.1551, "omega":-0.08137, "ax":-11.0539, "ay":4.47792, "alpha":2.34938, "fx":[-182.31954,-189.49675,-186.97816,-178.25854], "fy":[80.13611,61.23389,68.45986,88.74925]}, + {"t":1.27754, "x":3.20199, "y":4.42922, "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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3a02348f..9d6edac0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,6 @@ import frc.robot.commands.algae.OpenLoopAlgaeCommand; import frc.robot.commands.algae.ProcessorAlgaeCommand; import frc.robot.commands.algae.ToggleHasAlgaeCommand; -import frc.robot.commands.auton.NonProcessorShallowAutonCommand; import frc.robot.commands.biscuit.HoldBiscuitCommand; import frc.robot.commands.biscuit.JogBiscuitCommand; import frc.robot.commands.coral.EnableEjectBeamCommand; @@ -47,7 +46,6 @@ import frc.robot.commands.vision.SetVisionUpdatesCommand; import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.PathHandlerConstants; import frc.robot.constants.RobotConstants; import frc.robot.controllers.FlyskyJoystick; import frc.robot.controllers.FlyskyJoystick.Button; @@ -74,7 +72,6 @@ import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; import frc.robot.subsystems.tagAlign.TagAlignSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; -import java.util.List; import org.strykeforce.telemetry.TelemetryController; import org.strykeforce.telemetry.TelemetryService; @@ -163,10 +160,10 @@ public RobotContainer() { tagAlignSubsystem, visionSubsystem); - pathHandler = new PathHandler(driveSubsystem, tagAlignSubsystem, robotStateSubsystem); - driveSubsystem.setRobotStateSubsystem(robotStateSubsystem); + pathHandler = new PathHandler(driveSubsystem, tagAlignSubsystem, robotStateSubsystem); + configureTelemetry(); configureDriverBindings(); configureOperatorBindings(); @@ -455,25 +452,25 @@ public void configurePitDashboard() { .withPosition(1, 0) .withSize(1, 1); - Shuffleboard.getTab("Pit") - .add( - "Start Auton", - new NonProcessorShallowAutonCommand( - driveSubsystem, - pathHandler, - robotStateSubsystem, - algaeSubsystem, - biscuitSubsystem, - coralSubsystem, - elevatorSubsystem, - tagAlignSubsystem, - "startToJ", - PathHandlerConstants.pathNames, - List.of('K', 'L', 'M'), - List.of(4, 4, 4), - 'J')) - .withPosition(3, 0) - .withSize(1, 1); + // Shuffleboard.getTab("Pit") + // .add( + // "Start Auton", + // new NonProcessorShallowAutonCommand( + // driveSubsystem, + // pathHandler, + // robotStateSubsystem, + // algaeSubsystem, + // biscuitSubsystem, + // coralSubsystem, + // elevatorSubsystem, + // tagAlignSubsystem, + // "startToJ", + // PathHandlerConstants.kpathNames, + // List.of('K', 'L', 'M'), + // List.of(4, 4, 4), + // 'J')) + // .withPosition(3, 0) + // .withSize(1, 1); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index dc3a14e8..1d05d939 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -31,15 +31,15 @@ public class DriveConstants { public static final double kDriveGearRatio = (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear); - public static final double kWheelDiameterInches = 4.0; - public static final double kMaxSpeedMetersPerSecond = 3.384; + public static final double kWheelDiameterInches = 3.375; + public static final double kMaxSpeedMetersPerSecond = 3.782; public static final double kSpeedStillThreshold = 0.1; // meters per second public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second public static final double kGyroDifferentThreshold = 5.0; // 5 degrees public static final int kGyroDifferentCount = 3; - public static final double kRobotLength = 0.6223; - public static final double kRobotWidth = 0.6223; + public static final double kRobotLength = 0.61595; + public static final double kRobotWidth = 0.61595; public static final double kFieldMaxX = 17.526; public static final double kCenterLineX = 8.763; diff --git a/src/main/java/frc/robot/constants/PathHandlerConstants.java b/src/main/java/frc/robot/constants/PathHandlerConstants.java index c4c178ed..dcf04ee5 100644 --- a/src/main/java/frc/robot/constants/PathHandlerConstants.java +++ b/src/main/java/frc/robot/constants/PathHandlerConstants.java @@ -1,8 +1,34 @@ package frc.robot.constants; public class PathHandlerConstants { - public static final String[][] pathNames = { - {"", "", "", "", "", "", "", "", "", "", "", ""}, - {"", "", "", "", "", "", "", "", "", "", "", ""} + public static final String[][] kpathNames = { + { + "fetchToA", + "fetchToB", + "FiveMeterTestPath", + "FiveMeterTestPath", + "FiveMeterTestPath", + "fetchToF", + "fetchToG", + "fetchToH", + "fetchToI", + "fetchToJ", + "fetchToK", + "fetchToL" + }, + { + "ATofetch", + "BTofetch", + "FiveMeterTestPath", + "FiveMeterTestPath", + "FiveMeterTestPath", + "FiveMeterTestPath", + "GTofetch", + "HTofetch", + "ITofetch", + "JTofetch", + "KTofetch", + "LTofetch" + } }; } diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index e64bc050..09c5580c 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -14,6 +14,7 @@ import java.util.Optional; import java.util.Set; import net.jafama.FastMath; +import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; @@ -21,6 +22,7 @@ public class PathHandler extends MeasurableSubsystem { private DriveSubsystem driveSubsystem; private RobotStateSubsystem robotStateSubsystem; private TagAlignSubsystem tagAlignSubsystem; + private static final org.slf4j.Logger logger = LoggerFactory.getLogger(PathHandler.class); private PathStates curState = PathStates.DONE; private boolean isHandling = false; @@ -45,7 +47,7 @@ public PathHandler( this.driveSubsystem = driveSubsystem; this.tagAlignSubsystem = tagAlignSubsystem; this.robotStateSubsystem = robotStateSubsystem; - reassignAlliance(); + // reassignAlliance(); } public PathHandler( @@ -106,8 +108,10 @@ public void startPathHandler() { public void reassignAlliance() { mirrorTrajectory = driveSubsystem.shouldFlip(); + Optional> temp; for (int i = 0; i < 12; i++) { - Optional> temp = Choreo.loadTrajectory(pathNames[i][0]); + logger.info(i + ""); + temp = Choreo.loadTrajectory(pathNames[i][0]); fetchPaths.add(temp.get()); temp = Choreo.loadTrajectory(pathNames[i][1]); placePaths.add(temp.get()); From 9412d2e37de6510af85908ed6f7078f3018e45c3 Mon Sep 17 00:00:00 2001 From: David Shen Date: Tue, 25 Feb 2025 17:56:06 -0500 Subject: [PATCH 29/48] initial v --- .../subsystems/drive/DriveSubsystem.java | 4 ++++ .../tagAlign/TagAlignSubsystem.java | 21 +++++++++++++------ 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 9485d4bd..85e95471 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -240,6 +240,10 @@ public ChassisSpeeds getFieldRelSpeed() { return inputs.fieldRelSpeed; } + public ChassisSpeeds getRobotRelSpeed() { + return inputs.robotRelSpeed; + } + public void setDriveState(DriveStates state) { currDriveState = state; } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 9391d3b0..06f04c59 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -182,12 +182,18 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { Pose2d current = driveSubsystem.getPoseMeters(); - driveX.reset(current.getX()); - driveY.reset(current.getY()); + double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; + double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; + + driveX.reset(current.getX(), vX); + driveY.reset(current.getY(), vY); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(this.goalTargetDiag); - alignY.reset(TagServoingConstants.kHorizontalTarget); + double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; + double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; + + alignX.reset(this.goalTargetDiag, robotVx); + alignY.reset(TagServoingConstants.kHorizontalTarget, robotVy); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -232,8 +238,11 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { } private void tagAlign() { - alignX.reset(0); - alignY.reset(0); + double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; + double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; + + alignX.reset(0, robotVx); + alignY.reset(0, robotVy); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; From 2d38ce963f235c0dfb8a7bad72b00fb64fe6ef0b Mon Sep 17 00:00:00 2001 From: David Shen Date: Tue, 25 Feb 2025 19:43:31 -0500 Subject: [PATCH 30/48] drive practice --- src/main/java/frc/robot/RobotContainer.java | 8 ++++++- .../robotState/InterruptAutoCommand.java | 5 ++--- .../tagAlign/TagAlignSubsystem.java | 21 ++++++------------- 3 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19ef8e5e..ff4dcdf6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -207,9 +207,15 @@ private void configureDriverBindings() { coralSubsystem, biscuitSubsystem, algaeSubsystem)); - new JoystickButton(driveJoystick, Button.SWA.id) + + // Interupt + new JoystickButton(driveJoystick, Button.SWG_UP.id) + .onTrue(new InterruptAutoCommand(robotStateSubsystem)) + .onFalse(new InterruptAutoCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWG_DWN.id) .onTrue(new InterruptAutoCommand(robotStateSubsystem)) .onFalse(new InterruptAutoCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWB_UP.id) .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); diff --git a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java index d7790eb2..a75f09ba 100644 --- a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java +++ b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java @@ -3,9 +3,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.robotState.RobotStateSubsystem; -public class InterruptAutoCommand - extends InstantCommand { // TODO fix this command, it dose not work - RobotStateSubsystem robotState; +public class InterruptAutoCommand extends InstantCommand { + private RobotStateSubsystem robotState; public InterruptAutoCommand(RobotStateSubsystem robotState) { this.robotState = robotState; diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 06f04c59..57dd9a05 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -182,18 +182,12 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { Pose2d current = driveSubsystem.getPoseMeters(); - double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; - double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; - - driveX.reset(current.getX(), vX); - driveY.reset(current.getY(), vY); + driveX.reset(current.getX()); + driveY.reset(current.getY()); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; - double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; - - alignX.reset(this.goalTargetDiag, robotVx); - alignY.reset(TagServoingConstants.kHorizontalTarget, robotVy); + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -238,11 +232,8 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { } private void tagAlign() { - double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; - double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; - - alignX.reset(0, robotVx); - alignY.reset(0, robotVy); + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; From 78886b0de4b6eb41b628ed382ff4580187ec75b0 Mon Sep 17 00:00:00 2001 From: David Shen Date: Tue, 25 Feb 2025 20:16:08 -0500 Subject: [PATCH 31/48] untested vision adjustments --- .../frc/robot/constants/BiscuitConstants.java | 2 +- .../frc/robot/constants/VisionConstants.java | 9 +- .../subsystems/vision/VisionSubsystem.java | 82 +++++++++++++------ 3 files changed, 65 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 72b592f8..2b84e5bb 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -24,7 +24,7 @@ public class BiscuitConstants { - public static final double kZero = .36; + public static final double kZero = .37; public static final double kTicksPerRot = 160; public static final int talonID = 25; public static final double kCloseEnough = 0.05; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 86cff6d6..40f7bb24 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -61,8 +61,9 @@ public final class VisionConstants { public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0; public static final double FOV75YUYVBaseTrust = 3.0; - // Gyro error scaling - public static final double kYawErrorThreshold = Units.degreesToRadians(45); + // Gyro error + public static final double kYawErrorThreshold = Units.degreesToRadians(30); + public static final double kCamErrorZThreshold = 0.3; // Constants for cameras public static final int kNumCams = 5; @@ -94,14 +95,14 @@ public final class VisionConstants { public static final int kCircularBufferSize = 1000; // Poses public static final Pose3d kCam1Pose = - new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d()); + new Pose3d(new Translation3d(0.28, 0.03, 0.30), new Rotation3d()); public static final Pose3d kCam2Pose = new Pose3d( new Translation3d(0.236, -0.108, 0.932), new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(22.5))); public static final Pose3d kCam3Pose = - new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d()); + new Pose3d(new Translation3d(0.12, -0.28, 0.30), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( new Translation3d(0.20, 0.035, 0.94), diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index bc423cd9..bdaeefae 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -43,6 +43,14 @@ public class VisionSubsystem extends MeasurableSubsystem { VisionConstants.kCam5Pose.getTranslation() }; + private double[] camHeights = { + camPositions[0].getZ(), + camPositions[1].getZ(), + camPositions[2].getZ(), + camPositions[3].getZ(), + camPositions[4].getZ() + }; + // Array of camera rotations private Rotation3d[] camRotations = { VisionConstants.kCam1Pose.getRotation(), @@ -292,24 +300,39 @@ private double getStdDevFactor(double distance, int numTags, String camName) { private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) { /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro from the rotation of the pose and compare the two*/ - double pose1Error = - FastMath.abs( - Rotation2d.fromRadians(rotation) - .minus(pose1.getRotation().toRotation2d()) - .getRadians()); - double pose2Error = - FastMath.abs( - Rotation2d.fromRadians(rotation) - .minus(pose2.getRotation().toRotation2d()) - .getRadians()); + double pose1Error = 2767; + double pose2Error = 2767; + if (pose1 != null) { + pose1Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose1.getRotation().toRotation2d()) + .getRadians()); + } + if (pose2 != null) { + pose2Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose2.getRotation().toRotation2d()) + .getRadians()); + } Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error); Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error); Logger.recordOutput("Vision/Historical yaw", rotation); - if (pose1Error < pose2Error) { - if (pose1Error > VisionConstants.kYawErrorThreshold) {} + if (pose1Error > VisionConstants.kYawErrorThreshold) { + pose1 = null; + } + if (pose2Error > VisionConstants.kYawErrorThreshold) { + pose2 = null; + } + + if (pose1 == null && pose2 == null) { + return null; + } + if (pose1Error < pose2Error) { return pose1; } else { return pose2; @@ -317,17 +340,25 @@ private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) { } private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) { - // double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); - // double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); + double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + // // This filters out results by seeing if they are close to the right height - // if (dist1 < dist2 && dist1 < 0.5) { - // return pose1; - // } - // if (dist2 < dist1 && dist2 < 0.5) { - // return pose2; - // } + if (dist1 > VisionConstants.kCamErrorZThreshold) { + pose1 = null; + } + if (dist2 > VisionConstants.kCamErrorZThreshold) { + pose2 = null; + } + // If we don't have enough data in the gyro buffer we default to returning a pose - if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1; + if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) { + if (pose1 != null) { + return pose1; + } else { + return pose2; + } + } // See what pose is closer the the gyro at the time of the photo's capture. double rotation = @@ -437,12 +468,17 @@ public void periodic() { Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose); cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx); + + if (cameraPose == null) { + continue; + } + + cameraRotation = cameraPose.getRotation(); robotTranslation = cameraPose .getTranslation() - .minus(camPositions[idx].rotateBy(cameraPose.getRotation())) + .minus(camPositions[idx].rotateBy(cameraRotation)) .rotateBy(camRotations[idx]); - cameraRotation = cameraPose.getRotation(); } Pose2d robotPose = new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d()); From ce9a58b4817cd10b1a51c27e219438e7d01f1056 Mon Sep 17 00:00:00 2001 From: David Shen Date: Tue, 25 Feb 2025 21:21:25 -0500 Subject: [PATCH 32/48] fxs drive --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/constants/DriveConstants.java | 90 +++--- .../frc/robot/subsystems/drive/SwerveFXS.java | 272 ++++++++++++++++++ 3 files changed, 324 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveFXS.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ff4dcdf6..8d153c8d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ import frc.robot.subsystems.coral.CoralIOFX; import frc.robot.subsystems.coral.CoralSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; -import frc.robot.subsystems.drive.Swerve; +import frc.robot.subsystems.drive.SwerveFXS; import frc.robot.subsystems.elevator.ElevatorIO; import frc.robot.subsystems.elevator.ElevatorIOFX; import frc.robot.subsystems.elevator.ElevatorSubsystem; @@ -94,7 +94,7 @@ public class RobotContainer { private final CoralIO coralIO; private final CoralSubsystem coralSubsystem; - private final Swerve swerve; + private final SwerveFXS swerve; private final DriveSubsystem driveSubsystem; private final ElevatorIO elevatorIO; @@ -133,7 +133,7 @@ public RobotContainer() { coralIO = new CoralIOFX(); coralSubsystem = new CoralSubsystem(coralIO); - swerve = new Swerve(); + swerve = new SwerveFXS(); driveSubsystem = new DriveSubsystem(swerve); elevatorIO = new ElevatorIOFX(); diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 7e1fb784..4f50da2c 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -4,11 +4,18 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; +import com.ctre.phoenix6.configs.CommutationConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; +import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -75,62 +82,63 @@ public static Translation2d[] getWheelLocationMeters() { public static final Pose2d kResetOdomPose = new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67)); - // public static TalonFXSConfiguration - // getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration - // // constructor sets encoder to Quad/CTRE_MagEncoder_Relative - // TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration(); + public static TalonFXSConfiguration getAzimuthFXSConfig() { + // constructor sets encoder to Quad/CTRE_MagEncoder_Relative + TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration(); - // HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs(); - // hardwareLimitSwitchConfigs.ForwardLimitEnable = false; - // hardwareLimitSwitchConfigs.ReverseLimitEnable = false; - // azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs; + HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs(); + hardwareLimitSwitchConfigs.ForwardLimitEnable = false; + hardwareLimitSwitchConfigs.ReverseLimitEnable = false; + azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs; - // CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); - // currentConfig.SupplyCurrentLowerTime = 0; - // currentConfig.SupplyCurrentLowerLimit = 0; + CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs(); + currentConfig.SupplyCurrentLimit = 20; + currentConfig.SupplyCurrentLowerTime = 20; + currentConfig.SupplyCurrentLowerLimit = 1; - // currentConfig.SupplyCurrentLimit = 10; - // currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.SupplyCurrentLimitEnable = true; + currentConfig.StatorCurrentLimitEnable = false; - // azimuthConfig.CurrentLimits = currentConfig; + azimuthConfig.CurrentLimits = currentConfig; - // Slot0Configs slot0Config = new Slot0Configs(); - // slot0Config.kP = 360.35; - // slot0Config.kI = 0.0; - // slot0Config.kD = 3.604; + Slot0Configs slot0Config = new Slot0Configs(); + slot0Config.kP = 300; + slot0Config.kI = 0.0; + slot0Config.kD = 3; + slot0Config.kV = 4.3; - // azimuthConfig.Slot0 = slot0Config; + azimuthConfig.Slot0 = slot0Config; - // ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs(); - // externalFeedbackConfigs.VelocityFilterTimeConstant = 0.1; - // externalFeedbackConfigs.ExternalFeedbackSensorSource = - // ExternalFeedbackSensorSourceValue.PulseWidth; - // azimuthConfig.ExternalFeedback = externalFeedbackConfigs; + ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs(); + externalFeedbackConfigs.VelocityFilterTimeConstant = 0.02; // ? + externalFeedbackConfigs.ExternalFeedbackSensorSource = + ExternalFeedbackSensorSourceValue.Quadrature; + azimuthConfig.ExternalFeedback = externalFeedbackConfigs; - // VoltageConfigs voltageConfig = new VoltageConfigs(); - // voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long - // azimuthConfig.Voltage = voltageConfig; + // VoltageConfigs voltageConfig = new VoltageConfigs(); + // voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long + // azimuthConfig.Voltage = voltageConfig; - // MotionMagicConfigs motionConfig = new MotionMagicConfigs(); - // motionConfig.MotionMagicCruiseVelocity = 800; - // motionConfig.MotionMagicAcceleration = 10_000; - // azimuthConfig.MotionMagic = motionConfig; + MotionMagicConfigs motionConfig = new MotionMagicConfigs(); + motionConfig.MotionMagicCruiseVelocity = 2; + motionConfig.MotionMagicAcceleration = 20; + azimuthConfig.MotionMagic = motionConfig; - // MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); - // motorConfigs.DutyCycleNeutralDeadband = 0.04; - // motorConfigs.NeutralMode = NeutralModeValue.Coast; - // azimuthConfig.MotorOutput = motorConfigs; + MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); + // motorConfigs.DutyCycleNeutralDeadband = 0.04; + motorConfigs.NeutralMode = NeutralModeValue.Coast; + azimuthConfig.MotorOutput = motorConfigs; - // CommutationConfigs commutationConfigs = new CommutationConfigs(); - // commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST; + CommutationConfigs commutationConfigs = new CommutationConfigs(); + commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST; - // azimuthConfig.Commutation = commutationConfigs; + azimuthConfig.Commutation = commutationConfigs; - // return azimuthConfig; - // } + return azimuthConfig; + } public static TalonSRXConfiguration - getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration + getAzimuthTalonConfig() { // constructor sets encoder to Quad/CTRE_MagEncoder_Relative TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java new file mode 100644 index 00000000..b7806574 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -0,0 +1,272 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.DriveConstants; +import frc.robot.constants.VisionConstants; +import java.util.function.BooleanSupplier; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.strykeforce.gyro.SF_AHRS; +import org.strykeforce.gyro.SF_PIGEON2; +import org.strykeforce.healthcheck.Checkable; +import org.strykeforce.swerve.FXSwerveModule; +import org.strykeforce.swerve.OdometryStrategy; +import org.strykeforce.swerve.PoseEstimatorOdometryStrategy; +import org.strykeforce.swerve.SwerveDrive; +import org.strykeforce.swerve.SwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule; +import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits; +import org.strykeforce.telemetry.TelemetryService; + +public class SwerveFXS implements SwerveIO, Checkable { + private final SwerveDrive swerveDrive; + + // Grapher stuff + private PoseEstimatorOdometryStrategy odometryStrategy; + + private SF_PIGEON2 pigeon; + private SF_AHRS navx; + private Rotation2d navxOffset = new Rotation2d(); + + private TalonFXSConfigurator configuratorFXS; + private TalonFXConfigurator configurator; + + private TalonFXS[] azimuths = new TalonFXS[4]; + private TalonFX[] drives = new TalonFX[4]; + + private FXSwerveModule[] swerveModules; + private SwerveDriveKinematics kinematics; + private double fieldY = 0.0; + private double fieldX = 0.0; + + public SwerveFXS() { + + var moduleBuilder = + new FXSwerveModule.FXBuilder() + .driveGearRatio(DriveConstants.kDriveGearRatio) + .wheelDiameterInches(DriveConstants.kWheelDiameterInches) + .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) + .latencyCompensation(true); + swerveModules = new FXSwerveModule[4]; + Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); + + for (int i = 0; i < 4; i++) { + var azimuthFXS = new TalonFXS(i, "*"); + configuratorFXS = azimuthFXS.getConfigurator(); + configuratorFXS.apply(new TalonFXSConfiguration()); // factory default + configuratorFXS.apply(DriveConstants.getAzimuthFXSConfig()); + + azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(20); + + azimuths[i] = azimuthFXS; + + var driveTalon = new TalonFX(i + 10, "*"); + configurator = driveTalon.getConfigurator(); + configurator.apply(new TalonFXConfiguration()); // factory default + configurator.apply(DriveConstants.getDriveTalonConfig()); + drives[i] = driveTalon; + + swerveModules[i] = + moduleBuilder + .azimuthTalon(azimuthFXS) + .driveTalon(driveTalon) + .wheelLocationMeters(wheelLocations[i]) + .closedLoopUnits(ClosedLoopUnits.VOLTAGE) + .build(); + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + + pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*"); + pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); + navx = new SF_AHRS(); + swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules); + swerveDrive.resetGyro(); + swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0)); + + kinematics = swerveDrive.getKinematics(); + + odometryStrategy = + new PoseEstimatorOdometryStrategy( + swerveDrive.getHeading(), + new Pose2d(), + swerveDrive.getKinematics(), + VisionConstants.kStateStdDevs, + VisionConstants.kLocalMeasurementStdDevs, + VisionConstants.kVisionMeasurementStdDevs, + getSwerveModulePositions()); + + swerveDrive.setOdometry(odometryStrategy); + } + + // Getters/Setter + @Override + public String getName() { + return "Swerve"; + } + + private SwerveModule[] getSwerveModules() { + return swerveDrive.getSwerveModules(); + } + + public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) { + swerveModules[0].setAzimuthRotation2d(FL); + swerveModules[1].setAzimuthRotation2d(FR); + swerveModules[2].setAzimuthRotation2d(BL); + swerveModules[3].setAzimuthRotation2d(BR); + } + + private SwerveModulePosition[] getSwerveModulePositions() { + SwerveModule[] swerveModules = getSwerveModules(); + SwerveModulePosition[] temp = {null, null, null, null}; + for (int i = 0; i < 4; ++i) { + temp[i] = swerveModules[i].getPosition(); + } + return temp; + } + + public SwerveModuleState[] getSwerveModuleStates() { + V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return swerveModuleStates; + } + + private ChassisSpeeds getRobotRelSpeed() { + SwerveDriveKinematics kinematics = swerveDrive.getKinematics(); + SwerveModule[] swerveModules = swerveDrive.getSwerveModules(); + SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; ++i) { + swerveModuleStates[i] = swerveModules[i].getState(); + } + return kinematics.toChassisSpeeds(swerveModuleStates); + } + + private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { + Rotation2d heading = swerveDrive.getHeading().unaryMinus(); + fieldX = + roboRelSpeed.vxMetersPerSecond * heading.getCos() + + roboRelSpeed.vyMetersPerSecond * heading.getSin(); + fieldY = + -roboRelSpeed.vxMetersPerSecond * heading.getSin() + + roboRelSpeed.vyMetersPerSecond * heading.getCos(); + + return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); + } + + @Override + public void setOdometry(OdometryStrategy odom) { + swerveDrive.setOdometry(odom); + } + + @Override + public void setPigeonGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + } + + @Override + public void setBothGyroOffset(Rotation2d rotation) { + swerveDrive.setGyroOffset(rotation); + navxOffset = rotation; + } + + @Override + public void resetGyro() { + swerveDrive.resetGyro(); + navx.reset(); + } + + @Override + public void resetOdometry(Pose2d pose) { + swerveDrive.resetOdometry(pose); + navx.reset(); + } + + @Override + public void addVisionMeasurement(Pose2d pose, double timestamp) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose, timestamp); + } + + @Override + public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix stdDevs) { + Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp)); + odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs); + } + + @Override + public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) { + swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented); + } + + @Override + public void setAzimuthVel(double vel) { + for (int i = 0; i < 4; i++) { + azimuths[i].set(vel); + } + } + + @Override + public void configDriveCurrents(CurrentLimitsConfigs config) { + for (int i = 0; i < 4; i++) { + drives[i].getConfigurator().apply(config); + } + } + + @Override + public void updateInputs(SwerveIOInputs inputs) { + swerveDrive.updateInputs(); // Call before swerveDrive.periodic() + swerveDrive.periodic(); + + inputs.odometryX = swerveDrive.getPoseMeters().getX(); + inputs.odometryY = swerveDrive.getPoseMeters().getY(); + inputs.swervePose = odometryStrategy.getPoseMeters(); + inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees(); + inputs.gyroRotation2d = swerveDrive.getHeading(); + inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset); + inputs.normalizedGyroRotation = + FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians()); + inputs.gyroPitch = pigeon.getPitch(); + inputs.gyroRoll = pigeon.getRoll(); + inputs.gyroRate = swerveDrive.getGyroRate(); + inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated(); + inputs.poseMeters = swerveDrive.getPoseMeters(); + inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble(); + for (int i = 0; i < 4; ++i) { + inputs.azimuthVels[i] = azimuths[i].getVelocity().getValueAsDouble(); + inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble(); + } + inputs.robotRelSpeed = getRobotRelSpeed(); + inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); + inputs.fieldY = fieldY; + inputs.fieldX = fieldX; + } + + @Override + public void registerWith(TelemetryService telemetryService) { + swerveDrive.registerWith(telemetryService); + pigeon.registerWith(telemetryService); + } +} From 95716d3e11fb02da0551a956e6b9ecbc845163cd Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Tue, 25 Feb 2025 23:13:10 -0500 Subject: [PATCH 33/48] bump thirdcoast, fix swerve inversion problems --- src/main/java/frc/robot/constants/DriveConstants.java | 7 +++++-- .../java/frc/robot/subsystems/biscuit/BiscuitIOFX.java | 2 +- src/main/java/frc/robot/subsystems/drive/SwerveFXS.java | 6 +++--- vendordeps/thirdcoast.json | 4 ++-- 4 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 4f50da2c..d7bb37b4 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -15,8 +15,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorPhaseValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -113,6 +115,7 @@ public static TalonFXSConfiguration getAzimuthFXSConfig() { externalFeedbackConfigs.VelocityFilterTimeConstant = 0.02; // ? externalFeedbackConfigs.ExternalFeedbackSensorSource = ExternalFeedbackSensorSourceValue.Quadrature; + externalFeedbackConfigs.SensorPhase = SensorPhaseValue.Opposed; azimuthConfig.ExternalFeedback = externalFeedbackConfigs; // VoltageConfigs voltageConfig = new VoltageConfigs(); @@ -127,6 +130,7 @@ public static TalonFXSConfiguration getAzimuthFXSConfig() { MotorOutputConfigs motorConfigs = new MotorOutputConfigs(); // motorConfigs.DutyCycleNeutralDeadband = 0.04; motorConfigs.NeutralMode = NeutralModeValue.Coast; + motorConfigs.Inverted = InvertedValue.Clockwise_Positive; azimuthConfig.MotorOutput = motorConfigs; CommutationConfigs commutationConfigs = new CommutationConfigs(); @@ -137,8 +141,7 @@ public static TalonFXSConfiguration getAzimuthFXSConfig() { return azimuthConfig; } - public static TalonSRXConfiguration - getAzimuthTalonConfig() { + public static TalonSRXConfiguration getAzimuthTalonConfig() { // constructor sets encoder to Quad/CTRE_MagEncoder_Relative TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index 7891e05d..a46bfc27 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -55,7 +55,7 @@ public BiscuitIOFX() { velocity = talon.getVelocity(); position = talon.getPosition(); rawQuadrature = talon.getRawQuadraturePosition(); - rawQuadrature.setUpdateFrequency(20); + rawQuadrature.setUpdateFrequency(200); rawPulseWidth = talon.getRawPulseWidthPosition(); rawPulseWidth.setUpdateFrequency(20); zero(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java index b7806574..d8277040 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -19,7 +19,6 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.constants.DriveConstants; import frc.robot.constants.VisionConstants; -import java.util.function.BooleanSupplier; import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; import org.strykeforce.gyro.SF_AHRS; @@ -62,7 +61,8 @@ public SwerveFXS() { .driveGearRatio(DriveConstants.kDriveGearRatio) .wheelDiameterInches(DriveConstants.kWheelDiameterInches) .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond) - .latencyCompensation(true); + .latencyCompensation(true) + .encoderOpposed(false); swerveModules = new FXSwerveModule[4]; Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); @@ -72,7 +72,7 @@ public SwerveFXS() { configuratorFXS.apply(new TalonFXSConfiguration()); // factory default configuratorFXS.apply(DriveConstants.getAzimuthFXSConfig()); - azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(20); + azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(200); azimuths[i] = azimuthFXS; diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index b1be2433..50f62fdd 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.4", + "version": "25.0.5", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.4" + "version": "25.0.5" } ], "jniDependencies": [], From a3da027b7994a26c06105154d1afea1df9460e7b Mon Sep 17 00:00:00 2001 From: David Shen Date: Thu, 27 Feb 2025 08:42:26 -0500 Subject: [PATCH 34/48] tag align tuning, yaw good --- src/main/java/frc/robot/RobotContainer.java | 23 ++++++ .../commands/tagAlign/DriveTuningCommand.java | 55 ++++++++++++++ .../commands/tagAlign/YawTuningCommand.java | 64 ++++++++++++++++ .../frc/robot/constants/DriveConstants.java | 6 +- .../robot/constants/TagServoingConstants.java | 6 +- .../tagAlign/TagAlignSubsystem.java | 74 +++++++++++-------- 6 files changed, 193 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java create mode 100644 src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d153c8d..e0c45ba1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,9 +6,11 @@ import static edu.wpi.first.units.Units.Rotations; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -45,6 +47,8 @@ import frc.robot.commands.robotState.ToggleAutoPlacingCommand; import frc.robot.commands.robotState.ToggleGetAlgaeCommand; import frc.robot.commands.robotState.setScoreSideLeftCommand; +import frc.robot.commands.tagAlign.DriveTuningCommand; +import frc.robot.commands.tagAlign.YawTuningCommand; import frc.robot.commands.vision.SetVisionUpdatesCommand; import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; @@ -435,6 +439,25 @@ public void configurePitDashboard() { .withPosition(2, 1) .withSize(1, 1); + GenericEntry yawP = + Shuffleboard.getTab("Debug") + .add("Yaw kP", 0) + .withPosition(4, 2) + .withSize(1, 1) + .withWidget(BuiltInWidgets.kTextView) + .getEntry(); + Shuffleboard.getTab("Debug") + .add("Yaw Tuning", new YawTuningCommand(driveSubsystem, () -> yawP.getDouble(0))) + .withPosition(3, 2) + .withSize(1, 1); + + Shuffleboard.getTab("Debug") + .add( + "Drive Tuning", + new DriveTuningCommand(driveSubsystem, () -> yawP.getDouble(0), tagAlignSubsystem)) + .withPosition(4, 2) + .withSize(1, 1); + Shuffleboard.getTab("Pit") .add( "Five Meter Path", diff --git a/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java new file mode 100644 index 00000000..575dc5d2 --- /dev/null +++ b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java @@ -0,0 +1,55 @@ +package frc.robot.commands.tagAlign; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates; +import java.util.function.DoubleSupplier; + +public class DriveTuningCommand extends Command { + private DriveSubsystem driveSubsystem; + private TagAlignSubsystem tagAlignSubsystem; + private Pose2d start; + private boolean driving = false; + private DoubleSupplier pSupplier; + + public DriveTuningCommand( + DriveSubsystem driveSubsystem, + DoubleSupplier pSupplier, + TagAlignSubsystem tagAlignSubsystem) { + this.driveSubsystem = driveSubsystem; + this.tagAlignSubsystem = tagAlignSubsystem; + this.pSupplier = pSupplier; + + addRequirements(driveSubsystem); + } + + @Override + public void initialize() { + start = driveSubsystem.getPoseMeters(); + driving = false; + driveSubsystem.move(0.5, 0, 0, false); + } + + @Override + public void execute() { + if (!driving + && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) + > 1) { + driving = true; + tagAlignSubsystem.start(Alliance.Blue, true, false); + } + } + + @Override + public void end(boolean interrupted) { + driveSubsystem.stopDriving(); + } + + @Override + public boolean isFinished() { + return driving && tagAlignSubsystem.getState() == TagAlignStates.DONE; + } +} diff --git a/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java b/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java new file mode 100644 index 00000000..fe2ae787 --- /dev/null +++ b/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java @@ -0,0 +1,64 @@ +package frc.robot.commands.tagAlign; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class YawTuningCommand extends Command { + private DriveSubsystem driveSubsystem; + private Pose2d start; + private ProfiledPIDController driveOmega; + private boolean adjustYaw = false; + private DoubleSupplier pSupplier; + + public YawTuningCommand(DriveSubsystem driveSubsystem, DoubleSupplier pSupplier) { + this.driveSubsystem = driveSubsystem; + this.pSupplier = pSupplier; + + this.driveOmega = new ProfiledPIDController(8.0, 0, 0, new Constraints(1000, 1000)); + this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + + addRequirements(driveSubsystem); + } + + @Override + public void initialize() { + this.driveOmega.setP(pSupplier.getAsDouble()); + Logger.recordOutput("TagAlignSubsystem/OmegaKp", driveOmega.getP()); + start = driveSubsystem.getPoseMeters(); + adjustYaw = false; + driveSubsystem.move(0.5, 0, 0, true); + } + + @Override + public void execute() { + if (!adjustYaw + && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) + > 1) { + adjustYaw = true; + driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); + } + if (adjustYaw) { + double vOmega = + driveOmega.calculate( + driveSubsystem.getGyroRotation2d().getRadians(), Math.toRadians(100)); + Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/OmegaSetpoint", driveOmega.getSetpoint().position); + driveSubsystem.move(0.5, 0, vOmega, true); + } + } + + @Override + public void end(boolean interrupted) { + driveSubsystem.stopDriving(); + } + + @Override + public boolean isFinished() { + return driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) > 4; + } +} diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index d7bb37b4..049e4a8c 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -214,9 +214,9 @@ public static Pigeon2Configuration getPigeon2Configuration() { config.MountPose.MountPoseRoll = 0.0; config.MountPose.MountPosePitch = 0.0; - config.GyroTrim.GyroScalarX = 0.0; - config.GyroTrim.GyroScalarY = 0.0; - config.GyroTrim.GyroScalarZ = -4.55; + config.GyroTrim.GyroScalarX = -1.2; + config.GyroTrim.GyroScalarY = 4.8; + config.GyroTrim.GyroScalarZ = -2.9; return config; } diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 1db91bf7..8c14fbf1 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -19,9 +19,9 @@ public class TagServoingConstants { public static final double kRightCamDiagTarget = 985; // Constraints - public static final Constraints driveXConstraints = new Constraints(2, 2.0); - public static final Constraints driveYConstraints = new Constraints(2, 3.0); - public static final Constraints driveOmegaConstraints = new Constraints(1, 2.0); + public static final Constraints driveXConstraints = new Constraints(2000, 2000); + public static final Constraints driveYConstraints = new Constraints(2000, 3000); + public static final Constraints driveOmegaConstraints = new Constraints(1000, 2000); public static final Constraints alignXConstraints = new Constraints(1, 1.0); public static final Constraints alignYConstraints = new Constraints(1, 1.5); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 57dd9a05..38fe23f3 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -52,17 +52,17 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; - this.driveX = new ProfiledPIDController(5, 0, 0, TagServoingConstants.driveXConstraints); - this.driveY = new ProfiledPIDController(5.5, 0, 0, TagServoingConstants.driveYConstraints); + this.driveX = new ProfiledPIDController(1.66, 0, 0, TagServoingConstants.driveXConstraints); + this.driveY = new ProfiledPIDController(1.66, 0, 0, TagServoingConstants.driveYConstraints); this.driveOmega = - new ProfiledPIDController(5.0, 0, 0, TagServoingConstants.driveOmegaConstraints); + new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.driveOmegaConstraints); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); this.alignX = new ProfiledPIDController(0.0017, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 this.alignY = new ProfiledPIDController(0.00195, 0, 0, TagServoingConstants.alignYConstraints); this.alignOmega = - new ProfiledPIDController(5.5, 0, 0, TagServoingConstants.alignOmegaConstraints); + new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints); this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); @@ -182,12 +182,18 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { Pose2d current = driveSubsystem.getPoseMeters(); - driveX.reset(current.getX()); - driveY.reset(current.getY()); + double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; + double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; + + driveX.reset(current.getX(), vX); + driveY.reset(current.getY(), vY); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(0); - alignY.reset(0); + double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; + double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; + + alignX.reset(0, robotVx); + alignY.reset(0, robotVy); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -232,8 +238,11 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { } private void tagAlign() { - alignX.reset(0); - alignY.reset(0); + double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; + double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; + + alignX.reset(0, robotVx); + alignY.reset(0, robotVy); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; @@ -276,14 +285,14 @@ public void periodic() { double radius = getCurRadius(alliance); boolean ignoreX = radius < stopXRadius && !algae; - if (!algae || tagRelX > 0) { - tagRelX = - tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; - } + // if (!algae || tagRelX > 0) { + // tagRelX = + // tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; + // } - if (ignoreX) { - tagRelX = 0; - } + // if (ignoreX) { + // tagRelX = 0; + // } Translation2d tagRelError = targetPose @@ -296,20 +305,27 @@ public void periodic() { Transform2d poseError = targetPose.minus(current); - if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough - && (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough - || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough)) { - - if (proceedToAlign || !algae) { - tagAlign(); - break; - } else { - driveSubsystem.stopDriving(); - curState = TagAlignStates.WAITING; - break; - } + if (FastMath.abs(tagRelError.getX()) < driveCloseEnough) { + terminate(); + break; } + // if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough + // && (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough + // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough)) { + + // if (proceedToAlign || !algae) { + // tagAlign(); + // break; + // } else { + // driveSubsystem.stopDriving(); + // curState = TagAlignStates.WAITING; + // break; + // } + // } + + tagRelY = 0; + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", tagRelY); From 5a1e20b162ef9bf680558511ece9d650b70cb902 Mon Sep 17 00:00:00 2001 From: KaydenLee456 Date: Thu, 27 Feb 2025 19:19:42 -0500 Subject: [PATCH 35/48] First real commit. Focus before St. Joe is state machine. --- .../frc/robot/constants/ClimbConstants.java | 88 +++------- .../robot/subsystems/climb/ClimbArmIO.java | 23 --- .../robot/subsystems/climb/ClimbArmIOFX.java | 65 ------- .../frc/robot/subsystems/climb/ClimbIO.java | 34 ++++ .../subsystems/climb/ClimbIOServoFX.java | 126 ++++++++++++++ .../subsystems/climb/ClimbSubsystem.java | 161 +++++++++++++++--- .../robot/subsystems/climb/ClimbWheelIO.java | 10 -- .../subsystems/climb/ClimbWheelIOFX.java | 37 ---- 8 files changed, 323 insertions(+), 221 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java delete mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbIO.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java delete mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java delete mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 918ff86b..0985d8b3 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -17,75 +17,26 @@ import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; +//import edu.wpi.first.units.measure.AngularVelocity; public class ClimbConstants { - public static int kWheelFxId = 0; - public static int kArmFxId = 0; - - public static final AngularVelocity kWheelCloseEnough = DegreesPerSecond.of(5.0); - public static final Angle kArmCloseEnough = Degrees.of(5.0); + public static int kPivotArmFrontFxId = 45; + public static int kPivotArmFollowFxId = 46; + public static int kCANcoderId = 47; + public static int kDeployServoId = 1; //the servo to release the pin + public static int kRatchetServoId = 2; + public static int kCageAlignedDIOId = 11; + + public static final Angle kPivotArmCloseEnough = Degrees.of(5.0); //all of these arbitrary numbers public static final Angle kArmMaxFwd = Degrees.of(100); public static final Angle kArmMaxRev = Degrees.of(-100); public static final Angle kArmZeroTicks = Degrees.of(1530); + public static final Angle kPinDeployedPosition = Degrees.of(500); + public static final Angle kPinRetractedPosition = Degrees.of(0); - public static TalonFXConfiguration getWheelFXConfig() { - TalonFXConfiguration wheelFxConfig = new TalonFXConfiguration(); - - CurrentLimitsConfigs current = - new CurrentLimitsConfigs() - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) - .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) - .withSupplyCurrentLimitEnable(true); - wheelFxConfig.CurrentLimits = current; - - HardwareLimitSwitchConfigs hwLimit = - new HardwareLimitSwitchConfigs() - .withForwardLimitAutosetPositionEnable(false) - .withForwardLimitEnable(false) - .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen) - .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin) - .withReverseLimitAutosetPositionEnable(false) - .withReverseLimitEnable(false) - .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) - .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); - wheelFxConfig.HardwareLimitSwitch = hwLimit; - - Slot0Configs slot0 = - new Slot0Configs() - .withKP(0) - .withKI(0) - .withKD(0) - .withGravityType(GravityTypeValue.Elevator_Static) - .withKG(0) - .withKS(0) - .withKV(0) - .withKA(0); - wheelFxConfig.Slot0 = slot0; - - MotionMagicConfigs motionMagic = - new MotionMagicConfigs() - .withMotionMagicAcceleration(0) - .withMotionMagicCruiseVelocity(0) - .withMotionMagicExpo_kA(0) - .withMotionMagicExpo_kV(0) - .withMotionMagicJerk(0); - wheelFxConfig.MotionMagic = motionMagic; - - MotorOutputConfigs motorOut = - new MotorOutputConfigs() - .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); - wheelFxConfig.MotorOutput = motorOut; - - return wheelFxConfig; - } - - public static TalonFXConfiguration getArmFxConfig() { + //Climb positions + public static final Angle kClimbCagePos = Degrees.of(3); + public static TalonFXConfiguration getPivotArmFxConfig() { TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); CurrentLimitsConfigs current = @@ -148,4 +99,15 @@ public static TalonFXConfiguration getArmFxConfig() { return armFxConfig; } + + public static CurrentLimitsConfigs getZeroCurrentLimit() { + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + return config; + } + + public static CurrentLimitsConfigs getRunCurrentLimit() { + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + return config; + } + } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java deleted file mode 100644 index 7ef57741..00000000 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArmIO.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems.climb; - -import static edu.wpi.first.units.Units.Rotations; - -import edu.wpi.first.units.measure.Angle; -import org.littletonrobotics.junction.AutoLog; -import org.strykeforce.telemetry.TelemetryService; - -public interface ClimbArmIO { - - @AutoLog - static class ClimbArmIOInputs { - public Angle position = Rotations.of(0.0); - } - - public default void setPosition(Angle position) {} - - public default void updateInputs(ClimbArmIOInputs inputs) {} - - public default void zero() {} - - public default void registerWith(TelemetryService telemetryService) {} -} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java deleted file mode 100644 index 830a85d5..00000000 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArmIOFX.java +++ /dev/null @@ -1,65 +0,0 @@ -package frc.robot.subsystems.climb; - -import static edu.wpi.first.units.Units.Rotations; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Angle; -import frc.robot.constants.ClimbConstants; -import frc.robot.constants.ExampleConstants; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - -public class ClimbArmIOFX implements ClimbArmIO { - private Logger logger; - private TalonFX talonFx; - - private final Angle absSensorInitial; - private Angle relSetpointOffset; - private Angle setpoint; - - TalonFXConfigurator configurator; - private MotionMagicDutyCycle positionRequest = - new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); - StatusSignal curPosition; - - public ClimbArmIOFX() { - logger = LoggerFactory.getLogger(this.getClass()); - talonFx = new TalonFX(ClimbConstants.kArmFxId); - absSensorInitial = - talonFx.getPosition().getValue(); // relative encoder starts up as absolute position offset - - configurator = talonFx.getConfigurator(); - configurator.apply(new TalonFXConfiguration()); // Factory default motor controller - configurator.apply(ClimbConstants.getArmFxConfig()); - - curPosition = talonFx.getPosition(); - } - - @Override - public void setPosition(Angle position) { - setpoint = position.plus(relSetpointOffset); - - logger.info("Setting position to {} rotations", setpoint.in(Rotations)); - - talonFx.setControl(positionRequest.withPosition(setpoint)); - } - - @Override - public void updateInputs(ClimbArmIOInputs inputs) { - inputs.position = curPosition.refresh().getValue(); - } - - @Override - public void zero() { - relSetpointOffset = ExampleConstants.kZeroTicks; - logger.info( - "Abs: {}, Zero Pos: {}, Offset: {}", - absSensorInitial, - ExampleConstants.kZeroTicks, - absSensorInitial.minus(ExampleConstants.kZeroTicks)); - } -} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java new file mode 100644 index 00000000..04671880 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.climb; +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.Angle; +import org.littletonrobotics.junction.AutoLog; +import org.strykeforce.telemetry.TelemetryService; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; + +public interface ClimbIO { + + @AutoLog + static class ClimbIOInputs { + public Angle position = Rotations.of(0.0); + public double velocity = 0.0; + public double ratchetServoPosition = 0.0; + public double pinServoPosition = 0.0; + } + + public default void setPosition(Angle position) {} + + public default void setRatchetServoPosition(double position) {} //needed? since it's double, not angle + + public default void setPinServoPosition(double position) {} //may make a method to stick these both into one method + + public default void updateInputs(ClimbIOInputs inputs) {} + + public default void zero() {} + + public default void registerWith(TelemetryService telemetryService) {} + + public default void setSoftLimitsEnabled(boolean enable) {} + + public default void setCurrentLimit(CurrentLimitsConfigs config) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java new file mode 100644 index 00000000..ab56e7ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java @@ -0,0 +1,126 @@ +package frc.robot.subsystems.climb; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.controls.Follower; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Servo; +import frc.robot.constants.ClimbConstants; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; + +public class ClimbIOServoFX implements ClimbIO { + //will prob need healthchecks eventually + private Logger logger; + private TalonFX talonFxPivotArmFront; + private TalonFX talonFxPivotArmBack; + private Servo ratchetServo; + private Servo pinServo; + + private final Angle absPivotArmFrontSensorInitial; + private Angle relSetpointOffset; + private Angle pivotArmSetpoint; + + TalonFXConfigurator configuratorFront; + TalonFXConfigurator configuratorBack; + private MotionMagicDutyCycle positionRequestMain = + new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); + private Follower positionRequestFollower = + new Follower(ClimbConstants.kPivotArmFollowFxId, true); + + StatusSignal currPosition; + + public ClimbIOServoFX() { + logger = LoggerFactory.getLogger(this.getClass()); + talonFxPivotArmFront = new TalonFX(ClimbConstants.kPivotArmFrontFxId); + talonFxPivotArmBack = new TalonFX(ClimbConstants.kPivotArmFollowFxId); + ratchetServo = new Servo(ClimbConstants.kRatchetServoId); + pinServo = new Servo(ClimbConstants.kDeployServoId); + + absPivotArmFrontSensorInitial = talonFxPivotArmFront.getPosition().getValue(); //only for logging + configuratorFront = talonFxPivotArmFront.getConfigurator(); + configuratorBack = talonFxPivotArmBack.getConfigurator(); + configuratorFront.apply(new TalonFXConfiguration()); + configuratorFront.apply(ClimbConstants.getPivotArmFxConfig()); + configuratorBack.apply(new TalonFXConfiguration()); + configuratorBack.apply(ClimbConstants.getPivotArmFxConfig()); + + currPosition = talonFxPivotArmFront.getPosition(); + } + + //@Override + public String getName() { + return "Climb"; + } + + @Override + public void setPosition(Angle position) { + pivotArmSetpoint = position.plus(relSetpointOffset); + + logger.info("Setting position to {} rotations", pivotArmSetpoint.in(Rotations)); + + talonFxPivotArmFront.setControl(positionRequestMain.withPosition(pivotArmSetpoint)); + talonFxPivotArmBack.setControl(positionRequestFollower); + } + + @Override + public void setRatchetServoPosition(double position) { + //ratchetServo.setPosition(position); + ratchetServo.set(position); + //could be wrong but I'm assuming set rather than setPosition + } + + @Override + public void setPinServoPosition(double position) { + //pinServo.setPosition(position); + pinServo.set(position); + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + inputs.position = currPosition.refresh().getValue(); + inputs.ratchetServoPosition = ratchetServo.getPosition(); + inputs.pinServoPosition = pinServo.getPosition(); + } + + @Override + public void zero() { + relSetpointOffset = ClimbConstants.kArmZeroTicks; + /* + logger.info( + "Abs: {}, Zero Pos: {}, Offset: {}" + absPivotArmFrontSensorInitial, + ClimbConstants.kZeroTicks, + absPivotArmFrontSensorInitial.minus(ClimbConstants.kZeroTicks)); + setPosition(0.0); + + ); + */ + talonFxPivotArmBack.setPosition(0.0); + talonFxPivotArmFront.setPosition(0.0); + } + + @Override + public void setSoftLimitsEnabled(boolean enable) { + configuratorFront.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable) + .withReverseSoftLimitEnable()); //fixme + //configuratorBack.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable) + //.withReverseSoftLimitEnable()); + } + + @Override + public void setCurrentLimit(CurrentLimitsConfigs config) { + configuratorFront.apply(config); + configuratorBack.apply(config); + } + + public void goToZero() {} + +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index b4538219..2d10f5f2 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -1,80 +1,187 @@ +//still have a non-negligible amount of stuff to do on the state machine + + package frc.robot.subsystems.climb; import static edu.wpi.first.units.Units.Rotations; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.units.measure.Angle; import frc.robot.constants.ClimbConstants; import frc.robot.standards.ClosedLoopPosSubsystem; +import frc.robot.subsystems.climb.ClimbSubsystem.ClimbState; + import java.util.Set; -import org.littletonrobotics.junction.Logger; +//import org.littletonrobotics.junction.Logger; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem { - private final ClimbArmIO io; - private final ClimbWheelIO wheelIo; - - private final ClimbArmIOInputsAutoLogged inputs = new ClimbArmIOInputsAutoLogged(); - - private Angle setpoint = Rotations.of(0.0); + private final ClimbIO climbIo; + private final ClimbIO pivotArmIo; //will probably not need all these individual ios + private final ClimbIO ratchetIo; + private final ClimbIO pinIo; + + private final ClimbIOInputsAutoLogged climbInputs = new ClimbIOInputsAutoLogged(); + private final ClimbIOInputsAutoLogged pivotArmInputs = new ClimbIOInputsAutoLogged(); + private final ClimbIOInputsAutoLogged ratchetInputs = new ClimbIOInputsAutoLogged(); + private final ClimbIOInputsAutoLogged pinInputs = new ClimbIOInputsAutoLogged(); + + private Logger logger = LoggerFactory.getLogger(this.getClass()); + + private boolean isRotatingToPosition = false; + private boolean isClimbZeroed = false; + private boolean isRatchetOn = false; + private boolean inPosition = false; + private boolean isBeamBroken = false; + private boolean isPinDeployed = false; + private boolean proceedToClimb = false; + private int climbZeroStableCounts; //idk what this means + //more + + private Angle setpoints = Rotations.of(0.0); private ClimbState curState = ClimbState.INIT; + private Timer hangTimer = new Timer(); - public ClimbSubsystem(ClimbArmIO io, ClimbWheelIO wheelIo) { - this.io = io; - this.wheelIo = wheelIo; + public ClimbSubsystem(ClimbIO climbIo, ClimbIO pivotArmIo, ClimbIO ratchetIo, ClimbIO pinIo) { + this.climbIo = climbIo; + this.pivotArmIo = pivotArmIo; + this.ratchetIo = ratchetIo; + this.pinIo = pinIo; + + enableRatchet(false); - zero(); } @Override public Angle getPosition() { - return inputs.position; + return climbInputs.position; } @Override public void setPosition(Angle position) { - io.setPosition(position); + climbIo.setPosition(position); + setpoints = position; + } @Override public boolean isFinished() { - return setpoint.minus(inputs.position).abs(Rotations) - <= ClimbConstants.kArmCloseEnough.in(Rotations); + return setpoints.minus(climbInputs.position).abs(Rotations) + <= ClimbConstants.kPivotArmCloseEnough.in(Rotations); } @Override public void zero() { - io.zero(); + climbIo.zero(); curState = ClimbState.ZEROED; } + public void toggleClimbRatchet() { + if (isRatchetOn) enableRatchet(false); + else enableRatchet(true); + } + + public void enableRatchet(boolean enable) { + if (enable) { + ratchetIo.setPosition(); //fixme + logger.info("Engaging Ratchet"); + isRatchetOn = true; + } + else { + ratchetIo.setPosition(); //fixme + logger.info("Disengaging Ratchet"); + isRatchetOn = false; + } + } + + public Angle getSetpoints() { + return setpoints; + } + + public ClimbState getState() { + return curState; + } + + public void prepClimb() {} + + public void climb() {} + + public void deployPin(boolean enable) { + if (enable) { + pinIo.setPosition(); //fixme + logger.info("Deploying Pin"); + isPinDeployed = true; + } + else { + pinIo.setPosition(); //fixme + logger.info("Retracting Pin"); + isPinDeployed = true; + } + + } + + public void togglePin() { + if (isPinDeployed) deployPin(false); + else enableRatchet(true); + } + + public boolean isClimbFinished() {} + + public void descend() {} + + public void stow() { + + } + @Override public void periodic() { // Read Inputs - io.updateInputs(inputs); + climbIo.updateInputs(climbInputs); + pivotArmIo.updateInputs(pivotArmInputs); + pinIo.updateInputs(pinInputs); + ratchetIo.updateInputs(ratchetInputs); + Logger.processInputs(getName(), climbInputs); + //climbInputs.ratchetPosition = ratchetServo.getPosition(); // State Machine switch (curState) { case INIT: break; - case ZEROED: + case ZEROING: + if () { + + } + else { + + } break; default: break; + case ZEROED: + if (proceedToClimb) { + + } + else + + } // Log Outputs - Logger.recordOutput("Coral/curState", curState.ordinal()); - Logger.recordOutput("Coral/setpoint", setpoint.in(Rotations)); + Logger.recordOutput("Coral/curState", curState.ordinal()); //works not + Logger.recordOutput("Coral/setpoint", setpoints.in(Rotations)); } @Override public void registerWith(TelemetryService telemetryService) { super.registerWith(telemetryService); - wheelIo.registerWith(telemetryService); - io.registerWith(telemetryService); + pivotArmIo.registerWith(telemetryService); + pinIo.registerWith(telemetryService); + ratchetIo.registerWith(telemetryService); } @Override @@ -84,6 +191,14 @@ public Set getMeasures() { public enum ClimbState { INIT, - ZEROED + STOWED, + ZEROING, + ZEROED, + PREP_CLIMBING, + PREPPED, + CLIMBING, + CLIMBED, + DESCENDING, + DOWN } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java deleted file mode 100644 index 0ff876ef..00000000 --- a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIO.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.robot.subsystems.climb; - -import org.strykeforce.telemetry.TelemetryService; - -public interface ClimbWheelIO { - - public default void setPercent(double pct) {} - - public default void registerWith(TelemetryService telemetryService) {} -} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java deleted file mode 100644 index f0083b58..00000000 --- a/src/main/java/frc/robot/subsystems/climb/ClimbWheelIOFX.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.subsystems.climb; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.hardware.TalonFX; -import frc.robot.constants.ClimbConstants; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; -import org.strykeforce.telemetry.TelemetryService; - -public class ClimbWheelIOFX implements ClimbWheelIO { - private Logger logger; - private TalonFX talonFx; - - TalonFXConfigurator configurator; - - public ClimbWheelIOFX() { - logger = LoggerFactory.getLogger(this.getClass()); - talonFx = new TalonFX(ClimbConstants.kWheelFxId); - - configurator = talonFx.getConfigurator(); - configurator.apply(new TalonFXConfiguration()); // Factory default motor controller - configurator.apply(ClimbConstants.getWheelFXConfig()); - } - - @Override - public void setPercent(double pct) { - logger.info("Setting percent to {}", pct); - - talonFx.set(pct); - } - - @Override - public void registerWith(TelemetryService telemetryService) { - telemetryService.register(talonFx, true); - } -} From 91eaac2e43e58704a35b08e9df10b04d1ff85a1b Mon Sep 17 00:00:00 2001 From: David Shen Date: Thu, 27 Feb 2025 21:29:46 -0500 Subject: [PATCH 36/48] only odometry drive --- src/main/java/frc/robot/RobotContainer.java | 41 ++- .../commands/tagAlign/DriveTuningCommand.java | 17 +- .../robot/constants/TagServoingConstants.java | 31 +- .../tagAlign/TagAlignSubsystem.java | 297 ++++++++---------- vendordeps/thirdcoast.json | 4 +- 5 files changed, 180 insertions(+), 210 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0c45ba1..75c0e7ab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -33,11 +32,9 @@ import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; -import frc.robot.commands.robotState.AutoReefCycleCommand; import frc.robot.commands.robotState.FloorAlgaeCommand; import frc.robot.commands.robotState.HPAlgaeCommand; import frc.robot.commands.robotState.InterruptAutoCommand; -import frc.robot.commands.robotState.ReefCycleCommand; import frc.robot.commands.robotState.ScoreAlgaeCommand; import frc.robot.commands.robotState.SetScoreSideCommand; import frc.robot.commands.robotState.SetScoreSideRightCommand; @@ -228,25 +225,27 @@ private void configureDriverBindings() { .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); // other stuff - new JoystickButton(driveJoystick, Button.M_SWH.id) - .onTrue( - new ConditionalCommand( - new AutoReefCycleCommand( - robotStateSubsystem, - elevatorSubsystem, - coralSubsystem, - driveSubsystem, - tagAlignSubsystem, - biscuitSubsystem, - algaeSubsystem), - new ReefCycleCommand( - robotStateSubsystem, - elevatorSubsystem, - coralSubsystem, - biscuitSubsystem, - algaeSubsystem), - () -> robotStateSubsystem.getIsAutoPlacing())); + .onTrue(new DriveTuningCommand(driveSubsystem, null, tagAlignSubsystem)); + + // new JoystickButton(driveJoystick, Button.M_SWH.id) + // .onTrue( + // new ConditionalCommand( + // new AutoReefCycleCommand( + // robotStateSubsystem, + // elevatorSubsystem, + // coralSubsystem, + // driveSubsystem, + // tagAlignSubsystem, + // biscuitSubsystem, + // algaeSubsystem), + // new ReefCycleCommand( + // robotStateSubsystem, + // elevatorSubsystem, + // coralSubsystem, + // biscuitSubsystem, + // algaeSubsystem), + // () -> robotStateSubsystem.getIsAutoPlacing())); new JoystickButton(driveJoystick, Button.M_SWE.id) .onTrue( diff --git a/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java index 575dc5d2..954dc5ba 100644 --- a/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java +++ b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java @@ -29,18 +29,19 @@ public DriveTuningCommand( @Override public void initialize() { start = driveSubsystem.getPoseMeters(); - driving = false; - driveSubsystem.move(0.5, 0, 0, false); + driving = true; + // driveSubsystem.move(0, 1.5, 0, false); + tagAlignSubsystem.start(Alliance.Blue, true, false); } @Override public void execute() { - if (!driving - && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) - > 1) { - driving = true; - tagAlignSubsystem.start(Alliance.Blue, true, false); - } + // if (!driving + // && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) + // > 1) { + // driving = true; + // tagAlignSubsystem.start(Alliance.Blue, true, false); + // } } @Override diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 8c14fbf1..9ae71824 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -11,7 +11,7 @@ public class TagServoingConstants { // Offsets public static final double kLeftCamOffset = VisionConstants.kCam1Pose.getY(); - public static final double kRightCamOffset = VisionConstants.kCam2Pose.getY(); + public static final double kRightCamOffset = VisionConstants.kCam3Pose.getY(); // Targets public static final double kHorizontalTarget = 800; @@ -19,13 +19,13 @@ public class TagServoingConstants { public static final double kRightCamDiagTarget = 985; // Constraints - public static final Constraints driveXConstraints = new Constraints(2000, 2000); - public static final Constraints driveYConstraints = new Constraints(2000, 3000); - public static final Constraints driveOmegaConstraints = new Constraints(1000, 2000); + public static final Constraints driveXConstraints = new Constraints(2, 100000); + public static final Constraints driveYConstraints = new Constraints(2, 100000); + public static final Constraints driveOmegaConstraints = new Constraints(10000, 20000); - public static final Constraints alignXConstraints = new Constraints(1, 1.0); - public static final Constraints alignYConstraints = new Constraints(1, 1.5); - public static final Constraints alignOmegaConstraints = new Constraints(1, 1); + public static final Constraints alignXConstraints = new Constraints(2, 100000); + public static final Constraints alignYConstraints = new Constraints(2, 100000); + public static final Constraints alignOmegaConstraints = new Constraints(10000, 20000); public static final double[] kAngleTarget = { Units.degreesToRadians(0), @@ -47,18 +47,21 @@ public class TagServoingConstants { public static final double kNoUpdateMicrosec = 500_000; // Drive - public static final double kCoralInitialDriveRadius = 1.7; // 1.5 - public static final double kCoralStopXDriveRadius = - kCoralInitialDriveRadius; // Should be closer to reef than target pose + public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall + public static final double kCoralAlignRadius = 1.27; + // public static final double kCoralStopXDriveRadius = + // kCoralInitialDriveRadius; // Should be closer to reef than target pose public static final double kAlgaeInitialDriveRadius = 1.6; + public static final double kAlgaeAlignRadius = 1.27; public static final double kAlgaeStopXDriveRadius = kAlgaeInitialDriveRadius; // Should be closer to reef than target pose - public static final double kCoralDriveCloseEnough = 0.1; - public static final double kAlgaeDriveCloseEnough = 0.1; + public static final double kInitialCloseEnough = 0.1; + public static final double kCoralDriveCloseEnough = 0.015; + public static final double kAlgaeDriveCloseEnough = 0.015; public static final double kMinVelX = 0.85; // Reef - public static final Translation2d kBlueReefPose = new Translation2d(4.524, 4.033); + public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); - public static final Translation2d kRedReefPose = new Translation2d(13.084, 4.033); + public static final Translation2d kRedReefPose = new Translation2d(13.058902, 4.0259); } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 38fe23f3..17ac5b80 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.tagAlign; -import WallEye.Point; -import WallEye.WallEyeTagResult; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -9,7 +7,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -29,47 +26,44 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private ProfiledPIDController alignX; private ProfiledPIDController alignY; - private ProfiledPIDController alignOmega; + // private ProfiledPIDController alignOmega; private TagAlignStates curState = TagAlignStates.DONE; // Set by start() private Pose2d targetPose; - private int targetCamId; - private int targetTagId; + // private int targetCamId; + // private int targetTagId; private int fieldRelHexant; private Alliance alliance = Alliance.Blue; - private double goalTargetDiag; - private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius; + // private double goalTargetDiag; + // private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius; private double driveRadius = TagServoingConstants.kCoralInitialDriveRadius; private boolean algae = false; private double driveCloseEnough = TagServoingConstants.kCoralDriveCloseEnough; private boolean proceedToAlign = false; + private boolean scoreLeft = true; - private long startServoTime; + // private long startServoTime; public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) { this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; - this.driveX = new ProfiledPIDController(1.66, 0, 0, TagServoingConstants.driveXConstraints); - this.driveY = new ProfiledPIDController(1.66, 0, 0, TagServoingConstants.driveYConstraints); + this.driveX = new ProfiledPIDController(4, 0, 0, TagServoingConstants.driveXConstraints); + this.driveY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.driveYConstraints); this.driveOmega = new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.driveOmegaConstraints); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); this.alignX = - new ProfiledPIDController(0.0017, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 - this.alignY = new ProfiledPIDController(0.00195, 0, 0, TagServoingConstants.alignYConstraints); - this.alignOmega = - new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints); - this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - - Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); - Logger.recordOutput("TagAlignSubsystem/TargetTag", -1); - Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1); + new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 + this.alignY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints); + // this.alignOmega = + // new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints); + // this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + Logger.recordOutput("TagAlignSubsystem/Hexant", -1); - Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", -1); } public void setProceedToAlign(boolean proceed) { @@ -143,40 +137,38 @@ public TagAlignStates getState() { } public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { - targetPose = getTargetDrivePose(alliance, scoreLeft); this.alliance = alliance; - this.goalTargetDiag = - scoreLeft - ? TagServoingConstants.kRightCamDiagTarget - : TagServoingConstants.kLeftCamDiagTarget; - - this.stopXRadius = - algae - ? TagServoingConstants.kAlgaeStopXDriveRadius - : TagServoingConstants.kCoralStopXDriveRadius; + this.scoreLeft = scoreLeft; + // this.goalTargetDiag = + // scoreLeft + // ? TagServoingConstants.kRightCamDiagTarget + // : TagServoingConstants.kLeftCamDiagTarget; + + // this.stopXRadius = + // algae + // ? TagServoingConstants.kAlgaeStopXDriveRadius + // : TagServoingConstants.kCoralStopXDriveRadius; this.driveRadius = algae ? TagServoingConstants.kAlgaeInitialDriveRadius : TagServoingConstants.kCoralInitialDriveRadius; this.algae = algae; - this.driveCloseEnough = - algae - ? TagServoingConstants.kAlgaeDriveCloseEnough - : TagServoingConstants.kCoralDriveCloseEnough; + this.driveCloseEnough = TagServoingConstants.kInitialCloseEnough; + targetPose = getTargetDrivePose(alliance, scoreLeft); // Inverted, scoring left coral means aligning right camera - targetCamId = - !scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam; - targetTagId = - alliance == Alliance.Blue - ? TagServoingConstants.kBlueTargetTag[computeHexant(alliance)] - : TagServoingConstants.kRedTargetTag[computeHexant(alliance)]; + // targetCamId = + // !scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam; + // targetTagId = + // alliance == Alliance.Blue + // ? TagServoingConstants.kBlueTargetTag[computeHexant(alliance)] + // : TagServoingConstants.kRedTargetTag[computeHexant(alliance)]; fieldRelHexant = computeFieldRelHexant(alliance); proceedToAlign = false; - Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); - Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); + // Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); + // Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); Logger.recordOutput("TagAlignSubsystem/GettingAlgae", algae); @@ -189,46 +181,45 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { driveY.reset(current.getY(), vY); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; - double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; - - alignX.reset(0, robotVx); - alignY.reset(0, robotVy); - alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); + alignX.reset(current.getX(), vX); + alignY.reset(current.getY(), vY); + // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } // CALL setup() first!! Do not use in internal state machine public double calculateAlignY() { - WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); + return 2767; - if (result == null) { - return 2767; - } + // WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); - int tagIndex = -1; - int[] tags = result.getTagIDs(); + // if (result == null) { + // return 2767; + // } - for (int i = 0; i < tags.length; i++) { - if (tags[i] == targetTagId) { - tagIndex = i; - break; - } - } + // int tagIndex = -1; + // int[] tags = result.getTagIDs(); - if (tagIndex == -1) { - // Target tag not found - return 2767; - } + // for (int i = 0; i < tags.length; i++) { + // if (tags[i] == targetTagId) { + // tagIndex = i; + // break; + // } + // } - Point center = result.getTagCenters().get(tagIndex); - double diag = result.getTagDiags()[tagIndex]; + // if (tagIndex == -1) { + // // Target tag not found + // return 2767; + // } - Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); - Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); + // Point center = result.getTagCenters().get(tagIndex); + // double diag = result.getTagDiags()[tagIndex]; - double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); + // Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); + // Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - return vY; + // double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); + + // return vY; } public void start(Alliance alliance, boolean scoreLeft, boolean algae) { @@ -238,15 +229,35 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { } private void tagAlign() { - double robotVx = driveSubsystem.getRobotRelSpeed().vxMetersPerSecond; - double robotVy = driveSubsystem.getRobotRelSpeed().vyMetersPerSecond; + Pose2d current = driveSubsystem.getPoseMeters(); - alignX.reset(0, robotVx); - alignY.reset(0, robotVy); - alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); + double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; + double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; + + alignX.reset(current.getX(), vX); + alignY.reset(current.getY(), vY); + // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; - startServoTime = RobotController.getFPGATime(); + + this.driveCloseEnough = + algae + ? TagServoingConstants.kAlgaeDriveCloseEnough + : TagServoingConstants.kCoralDriveCloseEnough; + + // this.goalTargetDiag = + // scoreLeft + // ? TagServoingConstants.kRightCamDiagTarget + // : TagServoingConstants.kLeftCamDiagTarget; + + // this.stopXRadius = + // algae + // ? TagServoingConstants.kAlgaeStopXDriveRadius + // : TagServoingConstants.kCoralStopXDriveRadius; + this.driveRadius = + algae ? TagServoingConstants.kAlgaeAlignRadius : TagServoingConstants.kCoralAlignRadius; + targetPose = getTargetDrivePose(alliance, scoreLeft); + Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); } public void terminate() { @@ -261,15 +272,30 @@ public void periodic() { // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); switch (curState) { - case DRIVE -> { + case DRIVE, TAG_ALIGN -> { Pose2d current = driveSubsystem.getPoseMeters(); - double vX = driveX.calculate(current.getX(), targetPose.getX()); - double vY = driveY.calculate(current.getY(), targetPose.getY()); + double vX = 0; + double vY = 0; double vOmega = driveOmega.calculate( current.getRotation().getRadians(), targetPose.getRotation().getRadians()); + switch (curState) { + case DRIVE -> { + vX = driveX.calculate(current.getX(), targetPose.getX()); + vY = driveY.calculate(current.getY(), targetPose.getY()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); + } + case TAG_ALIGN -> { + vX = alignX.calculate(current.getX(), targetPose.getX()); + vY = alignY.calculate(current.getY(), targetPose.getY()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getPositionError()); + } + } + Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); @@ -282,8 +308,8 @@ public void periodic() { double tagRelX = tagRelVel.getX(); double tagRelY = tagRelVel.getY(); - double radius = getCurRadius(alliance); - boolean ignoreX = radius < stopXRadius && !algae; + // double radius = getCurRadius(alliance); + boolean ignoreX = false; // radius < stopXRadius && !algae; // if (!algae || tagRelX > 0) { // tagRelX = @@ -301,31 +327,36 @@ public void periodic() { .rotateBy( Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive X Error", tagRelError.getX()); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive Y Error", tagRelError.getY()); Transform2d poseError = targetPose.minus(current); - if (FastMath.abs(tagRelError.getX()) < driveCloseEnough) { - terminate(); - break; + if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough) { + switch (curState) { + case DRIVE -> { + if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough + // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough + || FastMath.abs(tagRelError.getY()) < driveCloseEnough) { + if (proceedToAlign || !algae) { + tagAlign(); + break; + } else { + driveSubsystem.stopDriving(); + curState = TagAlignStates.WAITING; + break; + } + } + } + case TAG_ALIGN -> { + if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough) { + terminate(); + break; + } + } + } } - // if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough - // && (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough - // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough)) { - - // if (proceedToAlign || !algae) { - // tagAlign(); - // break; - // } else { - // driveSubsystem.stopDriving(); - // curState = TagAlignStates.WAITING; - // break; - // } - // } - - tagRelY = 0; - Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", tagRelY); @@ -343,70 +374,6 @@ public void periodic() { } } - case TAG_ALIGN -> { - WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); - - if (result == null) { - return; - } - - int tagIndex = -1; - int[] tags = result.getTagIDs(); - - for (int i = 0; i < tags.length; i++) { - if (tags[i] == targetTagId) { - tagIndex = i; - break; - } - } - - if (tagIndex == -1) { - return; - } - - Point center = result.getTagCenters().get(tagIndex); - double diag = result.getTagDiags()[tagIndex]; - - Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); - Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - - double vX = -alignX.calculate(goalTargetDiag - diag, 0); - double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); - - Logger.recordOutput("TagAlignSubsystem/Diag Error", alignX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/Horizontal Error", alignY.getPositionError()); - Logger.recordOutput( - "TagAlignSubsystem/Last result delay", - (RobotController.getFPGATime() - result.getTimeStamp()) / 1000); - - if (RobotController.getFPGATime() - result.getTimeStamp() - > TagServoingConstants.kNoUpdateMicrosec - && RobotController.getFPGATime() - startServoTime - > TagServoingConstants.kNoUpdateMicrosec) { - terminate(); - break; - } - - if (FastMath.abs(goalTargetDiag - diag) < TagServoingConstants.kDiagCloseEnough - || diag > goalTargetDiag) { - vX = 0; - - if (FastMath.abs(TagServoingConstants.kHorizontalTarget - center.x()) - < TagServoingConstants.kHorizontalCloseEnough) { - - terminate(); - break; - } - } - - double vOmega = - alignOmega.calculate( - driveSubsystem.getGyroRotation2d().getRadians(), - targetPose.getRotation().getRadians()); - - driveSubsystem.move(vX, vY, vOmega, false); - } - case DONE -> {} } } diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index 50f62fdd..d9f6ff20 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.5", + "version": "25.0.6", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.5" + "version": "25.0.6" } ], "jniDependencies": [], From 15d3efefaca54af6334c7460ce15ad031a0c1e20 Mon Sep 17 00:00:00 2001 From: David Shen Date: Fri, 28 Feb 2025 18:32:46 -0500 Subject: [PATCH 37/48] current --- .../robot/constants/TagServoingConstants.java | 7 +++++- .../subsystems/drive/DriveSubsystem.java | 4 ++++ .../frc/robot/subsystems/drive/Swerve.java | 11 +++++++++ .../frc/robot/subsystems/drive/SwerveFXS.java | 11 +++++++++ .../frc/robot/subsystems/drive/SwerveIO.java | 1 + .../tagAlign/TagAlignSubsystem.java | 24 ++++++++++++++++--- 6 files changed, 54 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 9ae71824..3833877e 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -55,10 +55,15 @@ public class TagServoingConstants { public static final double kAlgaeAlignRadius = 1.27; public static final double kAlgaeStopXDriveRadius = kAlgaeInitialDriveRadius; // Should be closer to reef than target pose + // public static final double kMinVelX = 0.85; + + // End conditions public static final double kInitialCloseEnough = 0.1; public static final double kCoralDriveCloseEnough = 0.015; public static final double kAlgaeDriveCloseEnough = 0.015; - public static final double kMinVelX = 0.85; + + public static final double kEndDriveCurrentThreshold = 10; + public static final double kCurrentCountThreshold = 3; // Reef public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 760205db..581242b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -366,6 +366,10 @@ public PIDController getomegaControllerNonProfiled() { omegaController.getP(), omegaController.getI(), omegaController.getD()); } + public double getAvgDriveCurrent() { + return inputs.avgDriveCurrent; + } + public void setDriveMultiplier(double multiplier) { driveMultiplier = multiplier; } diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index a849f4f1..7642cefb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -173,6 +173,16 @@ private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); } + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getSupplyCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + @Override public void setOdometry(OdometryStrategy odom) { swerveDrive.setOdometry(odom); @@ -260,6 +270,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity(); inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); } + inputs.avgDriveCurrent = getAvgDriveCurrent(); inputs.robotRelSpeed = getRobotRelSpeed(); inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java index d8277040..2f010441 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -171,6 +171,16 @@ private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) { return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond); } + public double getAvgDriveCurrent() { + double sum = 0; + + for (int i = 0; i < 4; i++) { + sum += drives[i].getSupplyCurrent().getValueAsDouble(); + } + + return sum / 4.0; + } + @Override public void setOdometry(OdometryStrategy odom) { swerveDrive.setOdometry(odom); @@ -258,6 +268,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.azimuthVels[i] = azimuths[i].getVelocity().getValueAsDouble(); inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble(); } + inputs.avgDriveCurrent = getAvgDriveCurrent(); inputs.robotRelSpeed = getRobotRelSpeed(); inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index 7f674d4d..ec78a07f 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -39,6 +39,7 @@ public static class SwerveIOInputs { public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds(); public double[] azimuthVels = {0, 0, 0, 0}; public double[] azimuthCurrent = {0, 0, 0, 0}; + public double avgDriveCurrent = 0; } private SwerveModule[] getSwerveModules() { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 17ac5b80..5e694bde 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -28,6 +28,9 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private ProfiledPIDController alignY; // private ProfiledPIDController alignOmega; + // private ProfiledPIDController servoX; + // private ProfiledPIDController servoY; + private TagAlignStates curState = TagAlignStates.DONE; // Set by start() @@ -43,6 +46,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private double driveCloseEnough = TagServoingConstants.kCoralDriveCloseEnough; private boolean proceedToAlign = false; private boolean scoreLeft = true; + private int currentThresCount = 0; // private long startServoTime; @@ -63,6 +67,11 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu // new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints); // this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); + // If we revisit tag servoing later... + // this.servoX = + // new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints); + // this.servoY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints); + Logger.recordOutput("TagAlignSubsystem/Hexant", -1); } @@ -154,6 +163,7 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { : TagServoingConstants.kCoralInitialDriveRadius; this.algae = algae; this.driveCloseEnough = TagServoingConstants.kInitialCloseEnough; + this.currentThresCount = 0; targetPose = getTargetDrivePose(alliance, scoreLeft); // Inverted, scoring left coral means aligning right camera @@ -294,6 +304,7 @@ public void periodic() { Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getPositionError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getPositionError()); } + default -> {} } Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); @@ -349,9 +360,16 @@ public void periodic() { } } case TAG_ALIGN -> { - if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough) { - terminate(); - break; + if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough + || driveSubsystem.getAvgDriveCurrent() + > TagServoingConstants.kEndDriveCurrentThreshold) { + currentThresCount++; + if (currentThresCount >= TagServoingConstants.kCurrentCountThreshold) { + terminate(); + break; + } + } else { + currentThresCount = 0; } } } From da4965cd70a95ea17c62e39b97efa44d31b029fd Mon Sep 17 00:00:00 2001 From: David Shen Date: Fri, 28 Feb 2025 21:51:41 -0500 Subject: [PATCH 38/48] bugs, end drive --- .../robot/constants/TagServoingConstants.java | 15 ++-- .../subsystems/drive/DriveSubsystem.java | 4 + .../frc/robot/subsystems/drive/Swerve.java | 13 ++- .../frc/robot/subsystems/drive/SwerveFXS.java | 13 ++- .../frc/robot/subsystems/drive/SwerveIO.java | 1 + .../tagAlign/TagAlignSubsystem.java | 87 ++++++++++++------- 6 files changed, 93 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 3833877e..3538a8fd 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -48,22 +48,25 @@ public class TagServoingConstants { // Drive public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall - public static final double kCoralAlignRadius = 1.27; + public static final double kCoralAlignRadius = 1.34; // 1.291 is perfectly against the reef // public static final double kCoralStopXDriveRadius = // kCoralInitialDriveRadius; // Should be closer to reef than target pose public static final double kAlgaeInitialDriveRadius = 1.6; - public static final double kAlgaeAlignRadius = 1.27; + public static final double kAlgaeAlignRadius = 1.34; public static final double kAlgaeStopXDriveRadius = kAlgaeInitialDriveRadius; // Should be closer to reef than target pose // public static final double kMinVelX = 0.85; // End conditions public static final double kInitialCloseEnough = 0.1; - public static final double kCoralDriveCloseEnough = 0.015; - public static final double kAlgaeDriveCloseEnough = 0.015; + public static final double kCoralDriveXCloseEnough = 0.03; + public static final double kCoralDriveYCloseEnough = 0.015; + public static final double kAlgaeDriveXCloseEnough = 0.03; + public static final double kAlgaeDriveYCloseEnough = 0.015; - public static final double kEndDriveCurrentThreshold = 10; - public static final double kCurrentCountThreshold = 3; + public static final double kEndDriveCurrentThreshold = 25; + public static final double kEndCountThreshold = 1; + public static final double kEndVelThreshold = 1; // Reef public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 581242b6..25940553 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -370,6 +370,10 @@ public double getAvgDriveCurrent() { return inputs.avgDriveCurrent; } + public double getAvgRearDriveVel() { + return inputs.avgRearDriveVel; + } + public void setDriveMultiplier(double multiplier) { driveMultiplier = multiplier; } diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 7642cefb..1635424e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -177,12 +177,22 @@ public double getAvgDriveCurrent() { double sum = 0; for (int i = 0; i < 4; i++) { - sum += drives[i].getSupplyCurrent().getValueAsDouble(); + sum += drives[i].getStatorCurrent().getValueAsDouble(); } return sum / 4.0; } + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + @Override public void setOdometry(OdometryStrategy odom) { swerveDrive.setOdometry(odom); @@ -271,6 +281,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent(); } inputs.avgDriveCurrent = getAvgDriveCurrent(); + inputs.avgRearDriveVel = getRearDriveAvgVel(); inputs.robotRelSpeed = getRobotRelSpeed(); inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java index 2f010441..b1be18e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -175,12 +175,22 @@ public double getAvgDriveCurrent() { double sum = 0; for (int i = 0; i < 4; i++) { - sum += drives[i].getSupplyCurrent().getValueAsDouble(); + sum += drives[i].getStatorCurrent().getValueAsDouble(); } return sum / 4.0; } + public double getRearDriveAvgVel() { + double sum = 0; + + for (int i = 2; i < 4; i++) { + sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble()); + } + + return sum / 2.0; + } + @Override public void setOdometry(OdometryStrategy odom) { swerveDrive.setOdometry(odom); @@ -269,6 +279,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble(); } inputs.avgDriveCurrent = getAvgDriveCurrent(); + inputs.avgRearDriveVel = getRearDriveAvgVel(); inputs.robotRelSpeed = getRobotRelSpeed(); inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index ec78a07f..c555ff59 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -40,6 +40,7 @@ public static class SwerveIOInputs { public double[] azimuthVels = {0, 0, 0, 0}; public double[] azimuthCurrent = {0, 0, 0, 0}; public double avgDriveCurrent = 0; + public double avgRearDriveVel = 0; } private SwerveModule[] getSwerveModules() { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 5e694bde..d7b232c6 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -1,9 +1,9 @@ package frc.robot.subsystems.tagAlign; +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.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -20,12 +20,12 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private DriveSubsystem driveSubsystem; private VisionSubsystem visionSubsystem; - private ProfiledPIDController driveX; - private ProfiledPIDController driveY; + private PIDController driveX; + private PIDController driveY; private ProfiledPIDController driveOmega; - private ProfiledPIDController alignX; - private ProfiledPIDController alignY; + private PIDController alignX; + private PIDController alignY; // private ProfiledPIDController alignOmega; // private ProfiledPIDController servoX; @@ -43,10 +43,12 @@ public class TagAlignSubsystem extends MeasurableSubsystem { // private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius; private double driveRadius = TagServoingConstants.kCoralInitialDriveRadius; private boolean algae = false; - private double driveCloseEnough = TagServoingConstants.kCoralDriveCloseEnough; + private double driveXCloseEnough = TagServoingConstants.kCoralDriveXCloseEnough; + private double driveYCloseEnough = TagServoingConstants.kCoralDriveYCloseEnough; private boolean proceedToAlign = false; private boolean scoreLeft = true; private int currentThresCount = 0; + private boolean finalDrive = false; // private long startServoTime; @@ -54,22 +56,21 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; - this.driveX = new ProfiledPIDController(4, 0, 0, TagServoingConstants.driveXConstraints); - this.driveY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.driveYConstraints); + this.driveX = new PIDController(4, 0, 0); + this.driveY = new PIDController(4, 0, 0); this.driveOmega = new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.driveOmegaConstraints); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - this.alignX = - new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015 - this.alignY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints); + this.alignX = new PIDController(4, 0, 0); // 0.0015 + this.alignY = new PIDController(4, 0, 0); // this.alignOmega = // new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints); // this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); // If we revisit tag servoing later... // this.servoX = - // new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints); + // new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints); // this.servoY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints); Logger.recordOutput("TagAlignSubsystem/Hexant", -1); @@ -148,6 +149,7 @@ public TagAlignStates getState() { public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { this.alliance = alliance; this.scoreLeft = scoreLeft; + this.finalDrive = false; // this.goalTargetDiag = // scoreLeft // ? TagServoingConstants.kRightCamDiagTarget @@ -162,7 +164,8 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { ? TagServoingConstants.kAlgaeInitialDriveRadius : TagServoingConstants.kCoralInitialDriveRadius; this.algae = algae; - this.driveCloseEnough = TagServoingConstants.kInitialCloseEnough; + this.driveXCloseEnough = TagServoingConstants.kInitialCloseEnough; + this.driveYCloseEnough = TagServoingConstants.kInitialCloseEnough; this.currentThresCount = 0; targetPose = getTargetDrivePose(alliance, scoreLeft); @@ -187,12 +190,12 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; - driveX.reset(current.getX(), vX); - driveY.reset(current.getY(), vY); + driveX.reset(); + driveY.reset(); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(current.getX(), vX); - alignY.reset(current.getY(), vY); + alignX.reset(); + alignY.reset(); // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -244,16 +247,21 @@ private void tagAlign() { double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; - alignX.reset(current.getX(), vX); - alignY.reset(current.getY(), vY); + alignX.reset(); + alignY.reset(); // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; - this.driveCloseEnough = + this.driveXCloseEnough = algae - ? TagServoingConstants.kAlgaeDriveCloseEnough - : TagServoingConstants.kCoralDriveCloseEnough; + ? TagServoingConstants.kAlgaeDriveXCloseEnough + : TagServoingConstants.kCoralDriveXCloseEnough; + + this.driveYCloseEnough = + algae + ? TagServoingConstants.kAlgaeDriveYCloseEnough + : TagServoingConstants.kCoralDriveYCloseEnough; // this.goalTargetDiag = // scoreLeft @@ -279,6 +287,7 @@ public void terminate() { @Override public void periodic() { Logger.recordOutput("TagAlignSubsystem/State", curState.toString()); + Logger.recordOutput("TagAlignSubsystem/FinalDrive", finalDrive); // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); switch (curState) { @@ -307,8 +316,9 @@ public void periodic() { default -> {} } - Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); + if (vX > 2) vX = 2; + if (vY > 2) vY = 2; + Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); Translation2d tagRelVel = @@ -341,14 +351,15 @@ public void periodic() { Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive X Error", tagRelError.getX()); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive Y Error", tagRelError.getY()); - Transform2d poseError = targetPose.minus(current); + // Translation2d poseError = targetPose.getTranslation().minus(current.getTranslation()); if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough) { switch (curState) { case DRIVE -> { - if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough + if (FastMath.abs(tagRelError.getX()) < driveXCloseEnough + && FastMath.abs(tagRelError.getY()) < driveYCloseEnough // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough - || FastMath.abs(tagRelError.getY()) < driveCloseEnough) { + || FastMath.abs(tagRelError.getY()) < driveYCloseEnough) { if (proceedToAlign || !algae) { tagAlign(); break; @@ -360,13 +371,21 @@ public void periodic() { } } case TAG_ALIGN -> { - if (FastMath.hypot(poseError.getX(), poseError.getY()) < driveCloseEnough - || driveSubsystem.getAvgDriveCurrent() - > TagServoingConstants.kEndDriveCurrentThreshold) { + if ((FastMath.abs(tagRelError.getX()) < driveXCloseEnough || finalDrive) + && FastMath.abs(tagRelError.getY()) < driveYCloseEnough) { + finalDrive = true; + + driveSubsystem.move(0.25, 0, vOmega, false); + } + + if (finalDrive + && driveSubsystem.getAvgDriveCurrent() + > TagServoingConstants.kEndDriveCurrentThreshold + && driveSubsystem.getAvgRearDriveVel() < TagServoingConstants.kEndVelThreshold) { + currentThresCount++; - if (currentThresCount >= TagServoingConstants.kCurrentCountThreshold) { + if (currentThresCount >= TagServoingConstants.kEndCountThreshold) { terminate(); - break; } } else { currentThresCount = 0; @@ -375,6 +394,10 @@ public void periodic() { } } + if (finalDrive) { + break; + } + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", tagRelY); From a0beac77ca9c7e0bf0778cbc7932a3bb9802e142 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sat, 1 Mar 2025 17:34:43 -0500 Subject: [PATCH 39/48] autoscoring --- src/main/java/frc/robot/RobotContainer.java | 41 +++++----- .../robot/constants/ElevatorConstants.java | 2 +- .../robot/constants/TagServoingConstants.java | 12 +-- .../tagAlign/TagAlignSubsystem.java | 77 ++++++++++++++----- 4 files changed, 85 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75c0e7ab..95bb7359 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -32,9 +33,11 @@ import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; +import frc.robot.commands.robotState.AutoReefCycleCommand; import frc.robot.commands.robotState.FloorAlgaeCommand; import frc.robot.commands.robotState.HPAlgaeCommand; import frc.robot.commands.robotState.InterruptAutoCommand; +import frc.robot.commands.robotState.ReefCycleCommand; import frc.robot.commands.robotState.ScoreAlgaeCommand; import frc.robot.commands.robotState.SetScoreSideCommand; import frc.robot.commands.robotState.SetScoreSideRightCommand; @@ -224,28 +227,24 @@ private void configureDriverBindings() { .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); - // other stuff new JoystickButton(driveJoystick, Button.M_SWH.id) - .onTrue(new DriveTuningCommand(driveSubsystem, null, tagAlignSubsystem)); - - // new JoystickButton(driveJoystick, Button.M_SWH.id) - // .onTrue( - // new ConditionalCommand( - // new AutoReefCycleCommand( - // robotStateSubsystem, - // elevatorSubsystem, - // coralSubsystem, - // driveSubsystem, - // tagAlignSubsystem, - // biscuitSubsystem, - // algaeSubsystem), - // new ReefCycleCommand( - // robotStateSubsystem, - // elevatorSubsystem, - // coralSubsystem, - // biscuitSubsystem, - // algaeSubsystem), - // () -> robotStateSubsystem.getIsAutoPlacing())); + .onTrue( + new ConditionalCommand( + new AutoReefCycleCommand( + robotStateSubsystem, + elevatorSubsystem, + coralSubsystem, + driveSubsystem, + tagAlignSubsystem, + biscuitSubsystem, + algaeSubsystem), + new ReefCycleCommand( + robotStateSubsystem, + elevatorSubsystem, + coralSubsystem, + biscuitSubsystem, + algaeSubsystem), + () -> robotStateSubsystem.getIsAutoPlacing())); new JoystickButton(driveJoystick, Button.M_SWE.id) .onTrue( diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index aef896e5..6053181b 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -38,7 +38,7 @@ public class ElevatorConstants { // Setpoints // Idle - public static final Angle kFunnelSetpoint = Rotations.of(2.03125); // was 2.40430 + public static final Angle kFunnelSetpoint = Rotations.of(0.3676757); // was 2.03125 public static final Angle kStowSetpoint = kFunnelSetpoint; // Algae removal diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 3538a8fd..359876ff 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -25,7 +25,7 @@ public class TagServoingConstants { public static final Constraints alignXConstraints = new Constraints(2, 100000); public static final Constraints alignYConstraints = new Constraints(2, 100000); - public static final Constraints alignOmegaConstraints = new Constraints(10000, 20000); + // public static final Constraints alignOmegaConstraints = new Constraints(10000, 20000); public static final double[] kAngleTarget = { Units.degreesToRadians(0), @@ -48,7 +48,7 @@ public class TagServoingConstants { // Drive public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall - public static final double kCoralAlignRadius = 1.34; // 1.291 is perfectly against the reef + public static final double kCoralAlignRadius = 1.34; // 1.293823 is perfectly against the reef // public static final double kCoralStopXDriveRadius = // kCoralInitialDriveRadius; // Should be closer to reef than target pose public static final double kAlgaeInitialDriveRadius = 1.6; @@ -60,13 +60,13 @@ public class TagServoingConstants { // End conditions public static final double kInitialCloseEnough = 0.1; public static final double kCoralDriveXCloseEnough = 0.03; - public static final double kCoralDriveYCloseEnough = 0.015; + public static final double kCoralDriveYCloseEnough = 0.025; public static final double kAlgaeDriveXCloseEnough = 0.03; - public static final double kAlgaeDriveYCloseEnough = 0.015; + public static final double kAlgaeDriveYCloseEnough = 0.025; public static final double kEndDriveCurrentThreshold = 25; - public static final double kEndCountThreshold = 1; - public static final double kEndVelThreshold = 1; + public static final int kEndCountThreshold = 1; + public static final double kEndVelThreshold = 2; // Reef public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index d7b232c6..c5b077f9 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -13,10 +13,12 @@ import java.util.Set; import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; public class TagAlignSubsystem extends MeasurableSubsystem { + private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class); private DriveSubsystem driveSubsystem; private VisionSubsystem visionSubsystem; @@ -74,6 +76,20 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu // this.servoY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints); Logger.recordOutput("TagAlignSubsystem/Hexant", -1); + + // driveRadius = 1.253823; + // for (int i = 0; i < 6; i++) { + // logger.info("Hexant {}, left and right", i); + + // logger.info( + // "{}, {}", + // getTargetDrivePose(Alliance.Blue, true, i).getX(), + // getTargetDrivePose(Alliance.Blue, true, i).getY()); + // logger.info( + // "{}, {}", + // getTargetDrivePose(Alliance.Blue, false, i).getX(), + // getTargetDrivePose(Alliance.Blue, false, i).getY()); + // } } public void setProceedToAlign(boolean proceed) { @@ -131,6 +147,24 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) { Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); } + private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft, int hexant) { + Translation2d reefT = + color == Alliance.Blue + ? TagServoingConstants.kBlueReefPose + : TagServoingConstants.kRedReefPose; + + logger.info("Using {}", driveRadius); + Translation2d offset = + new Translation2d(driveRadius, Rotation2d.fromDegrees(hexant * 60 + 180)); + + Translation2d sideOffset = + new Translation2d( + scoreLeft ? TagServoingConstants.kRightCamOffset : TagServoingConstants.kLeftCamOffset, + Rotation2d.fromDegrees(hexant * 60 + 180 + 90)); + + return new Pose2d(reefT.plus(offset).plus(sideOffset), Rotation2d.fromDegrees(hexant * 60)); + } + public double getCurRadius(Alliance color) { Translation2d reefT = color == Alliance.Blue @@ -300,16 +334,27 @@ public void periodic() { driveOmega.calculate( current.getRotation().getRadians(), targetPose.getRotation().getRadians()); + Translation2d rotated = + current + .getTranslation() + .rotateBy( + Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + Translation2d rotatedTarget = + targetPose + .getTranslation() + .rotateBy( + Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + switch (curState) { case DRIVE -> { - vX = driveX.calculate(current.getX(), targetPose.getX()); - vY = driveY.calculate(current.getY(), targetPose.getY()); + vX = driveX.calculate(rotated.getX(), rotatedTarget.getX()); + vY = driveY.calculate(rotated.getY(), rotatedTarget.getY()); Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); } case TAG_ALIGN -> { - vX = alignX.calculate(current.getX(), targetPose.getX()); - vY = alignY.calculate(current.getY(), targetPose.getY()); + vX = alignX.calculate(rotated.getX(), rotatedTarget.getX()); + vY = alignY.calculate(rotated.getY(), rotatedTarget.getY()); Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getPositionError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getPositionError()); } @@ -321,10 +366,9 @@ public void periodic() { Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); - Translation2d tagRelVel = - new Translation2d(vX, vY) - .rotateBy( - Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + Translation2d tagRelVel = new Translation2d(vX, vY); + // .rotateBy( + // Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); double tagRelX = tagRelVel.getX(); double tagRelY = tagRelVel.getY(); @@ -341,12 +385,9 @@ public void periodic() { // tagRelX = 0; // } - Translation2d tagRelError = - targetPose - .minus(current) - .getTranslation() - .rotateBy( - Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + Translation2d tagRelError = targetPose.minus(current).getTranslation(); + // .rotateBy( + // Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive X Error", tagRelError.getX()); Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive Y Error", tagRelError.getY()); @@ -371,13 +412,10 @@ public void periodic() { } } case TAG_ALIGN -> { - if ((FastMath.abs(tagRelError.getX()) < driveXCloseEnough || finalDrive) + if (FastMath.abs(tagRelError.getX()) < driveXCloseEnough && FastMath.abs(tagRelError.getY()) < driveYCloseEnough) { finalDrive = true; - - driveSubsystem.move(0.25, 0, vOmega, false); } - if (finalDrive && driveSubsystem.getAvgDriveCurrent() > TagServoingConstants.kEndDriveCurrentThreshold @@ -386,6 +424,7 @@ public void periodic() { currentThresCount++; if (currentThresCount >= TagServoingConstants.kEndCountThreshold) { terminate(); + break; } } else { currentThresCount = 0; @@ -395,7 +434,7 @@ public void periodic() { } if (finalDrive) { - break; + tagRelX = 0.25; } Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); From 27fd2f4946b69ac5d497313654f87be5ecb3b1b0 Mon Sep 17 00:00:00 2001 From: David Shen Date: Sat, 1 Mar 2025 18:09:45 -0500 Subject: [PATCH 40/48] clean up --- .../tagAlign/TagAlignSubsystem.java | 50 ++++++------------- 1 file changed, 15 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index c5b077f9..1adc6a36 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -276,14 +276,8 @@ public void start(Alliance alliance, boolean scoreLeft, boolean algae) { } private void tagAlign() { - Pose2d current = driveSubsystem.getPoseMeters(); - - double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; - double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; - alignX.reset(); alignY.reset(); - // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; @@ -349,14 +343,14 @@ public void periodic() { case DRIVE -> { vX = driveX.calculate(rotated.getX(), rotatedTarget.getX()); vY = driveY.calculate(rotated.getY(), rotatedTarget.getY()); - Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getError()); } case TAG_ALIGN -> { vX = alignX.calculate(rotated.getX(), rotatedTarget.getX()); vY = alignY.calculate(rotated.getY(), rotatedTarget.getY()); - Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getPositionError()); - Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getError()); } default -> {} } @@ -366,13 +360,6 @@ public void periodic() { Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); - Translation2d tagRelVel = new Translation2d(vX, vY); - // .rotateBy( - // Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); - - double tagRelX = tagRelVel.getX(); - double tagRelY = tagRelVel.getY(); - // double radius = getCurRadius(alliance); boolean ignoreX = false; // radius < stopXRadius && !algae; @@ -385,22 +372,15 @@ public void periodic() { // tagRelX = 0; // } - Translation2d tagRelError = targetPose.minus(current).getTranslation(); - // .rotateBy( - // Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); - - Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive X Error", tagRelError.getX()); - Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive Y Error", tagRelError.getY()); - // Translation2d poseError = targetPose.getTranslation().minus(current.getTranslation()); if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough) { switch (curState) { case DRIVE -> { - if (FastMath.abs(tagRelError.getX()) < driveXCloseEnough - && FastMath.abs(tagRelError.getY()) < driveYCloseEnough + if (FastMath.abs(driveX.getError()) < driveXCloseEnough + && FastMath.abs(driveY.getError()) < driveYCloseEnough // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough - || FastMath.abs(tagRelError.getY()) < driveYCloseEnough) { + || FastMath.abs(driveY.getError()) < driveYCloseEnough) { if (proceedToAlign || !algae) { tagAlign(); break; @@ -412,8 +392,8 @@ public void periodic() { } } case TAG_ALIGN -> { - if (FastMath.abs(tagRelError.getX()) < driveXCloseEnough - && FastMath.abs(tagRelError.getY()) < driveYCloseEnough) { + if (FastMath.abs(alignX.getError()) < driveXCloseEnough + && FastMath.abs(alignY.getError()) < driveYCloseEnough) { finalDrive = true; } if (finalDrive @@ -434,18 +414,18 @@ public void periodic() { } if (finalDrive) { - tagRelX = 0.25; + vX = 0.25; } - Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", tagRelX); - Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", tagRelY); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", vX); + Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", vY); - Translation2d adjusted = - new Translation2d(tagRelX, tagRelY) + Translation2d fieldRelV = + new Translation2d(vX, vY) .rotateBy( Rotation2d.fromRadians(TagServoingConstants.kAngleTarget[fieldRelHexant])); - driveSubsystem.move(adjusted.getX(), adjusted.getY(), vOmega, true); + driveSubsystem.move(fieldRelV.getX(), fieldRelV.getY(), vOmega, true); } case WAITING -> { From 9b0c237f285970e1898d05ee76545b35acb261fe Mon Sep 17 00:00:00 2001 From: David Shen Date: Sun, 2 Mar 2025 14:15:56 -0500 Subject: [PATCH 41/48] something --- .../java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 1adc6a36..c17fa192 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -219,10 +219,6 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); Logger.recordOutput("TagAlignSubsystem/GettingAlgae", algae); - Pose2d current = driveSubsystem.getPoseMeters(); - - double vX = driveSubsystem.getFieldRelSpeed().vxMetersPerSecond; - double vY = driveSubsystem.getFieldRelSpeed().vyMetersPerSecond; driveX.reset(); driveY.reset(); From 104c5c5740817e5869e28e8f172f01f2a046fd4f Mon Sep 17 00:00:00 2001 From: David Shen Date: Sun, 2 Mar 2025 15:17:40 -0500 Subject: [PATCH 42/48] bad paths, good cameras --- src/main/deploy/choreo/2025-project.chor | 128 +++++----- src/main/deploy/choreo/ATofetch.traj | 76 +++--- src/main/deploy/choreo/BTofetch.traj | 80 +++---- src/main/deploy/choreo/GToBarge.traj | 2 +- src/main/deploy/choreo/GTofetch.traj | 189 ++++++++------- src/main/deploy/choreo/HToBarge.traj | 44 ++-- src/main/deploy/choreo/HTofetch.traj | 183 +++++++------- src/main/deploy/choreo/ITofetch.traj | 115 +++++---- src/main/deploy/choreo/JTofetch.traj | 111 +++++---- src/main/deploy/choreo/KTofetch.traj | 66 +++--- src/main/deploy/choreo/LTofetch.traj | 65 +++-- src/main/deploy/choreo/constants.traj | 47 ---- src/main/deploy/choreo/fetchToA.traj | 92 ++++---- src/main/deploy/choreo/fetchToB.traj | 96 ++++---- src/main/deploy/choreo/fetchToF.traj | 220 +++++++++-------- src/main/deploy/choreo/fetchToI.traj | 111 +++++---- src/main/deploy/choreo/fetchToJ.traj | 108 ++++----- src/main/deploy/choreo/fetchToK.traj | 64 ++--- src/main/deploy/choreo/fetchToL.traj | 65 +++-- src/main/deploy/choreo/startDeepToA.traj | 223 +++++++++--------- src/main/deploy/choreo/startDeepToB.traj | 223 +++++++++--------- src/main/deploy/choreo/startDeepToK.traj | 140 ++++++----- src/main/deploy/choreo/startDeepToL.traj | 145 ++++++------ src/main/deploy/choreo/startGToG.traj | 4 +- src/main/deploy/choreo/startHToH.traj | 41 ++-- src/main/deploy/choreo/startToG.traj | 47 ++-- src/main/deploy/choreo/startToH.traj | 45 ++-- src/main/deploy/choreo/startToI.traj | 78 +++--- src/main/deploy/choreo/startToJ.traj | 81 ++++--- src/main/deploy/choreo/startToK.traj | 107 +++++---- src/main/deploy/choreo/startToL.traj | 112 +++++---- .../frc/robot/constants/VisionConstants.java | 17 +- .../tagAlign/TagAlignSubsystem.java | 40 ++-- 33 files changed, 1537 insertions(+), 1628 deletions(-) delete mode 100644 src/main/deploy/choreo/constants.traj diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index 687a5b84..b1f23deb 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -15,40 +15,40 @@ "poses":{ "A":{ "x":{ - "exp":"126.0625 in", - "val":3.2019875 + "exp":"3.2654999999999994 m", + "val":3.2654999999999994 }, "y":{ - "exp":"174.378839 in", - "val":4.4292225106 + "exp":"4.3309 m", + "val":4.3309 }, "heading":{ - "exp":"0 rad", + "exp":"0 deg", "val":0.0 } }, "B":{ "x":{ - "exp":"126.0625 in", - "val":3.2019875 + "exp":"3.2654999999999994 m", + "val":3.2654999999999994 }, "y":{ - "exp":"161.438839 in", - "val":4.1005465106 + "exp":"4.0009 m", + "val":4.0009 }, "heading":{ - "exp":"0 rad", + "exp":"0 deg", "val":0.0 } }, "C":{ "x":{ - "exp":"137.65477204339692 in", - "val":3.4964312099022816 + "exp":"3.6132737518457456 m", + "val":3.6132737518457456 }, "y":{ - "exp":"122.54275684567627 in", - "val":3.1125860238801772 + "exp":"3.1185381922643174 m", + "val":3.1185381922643174 }, "heading":{ "exp":"60 deg", @@ -57,12 +57,12 @@ }, "D":{ "x":{ - "exp":"148.86114076836753 in", - "val":3.781072975516535 + "exp":"3.89906213509461 m", + "val":3.89906213509461 }, "y":{ - "exp":"116.07275684567627 in", - "val":2.948248023880177 + "exp":"2.953538192264317 m", + "val":2.953538192264317 }, "heading":{ "exp":"60 deg", @@ -71,12 +71,12 @@ }, "E":{ "x":{ - "exp":"188.34227204339692 in", - "val":4.783893709902282 + "exp":"4.837096751845746 m", + "val":4.837096751845746 }, "y":{ - "exp":"106.66391784567627 in", - "val":2.709263513280177 + "exp":"2.813538192264317 m", + "val":2.813538192264317 }, "heading":{ "exp":"120 deg", @@ -85,12 +85,12 @@ }, "F":{ "x":{ - "exp":"199.54864076836753 in", - "val":5.068535475516535 + "exp":"5.122885135094611 m", + "val":5.122885135094611 }, "y":{ - "exp":"113.13391784567627 in", - "val":2.873601513280177 + "exp":"2.978538192264317 m", + "val":2.978538192264317 }, "heading":{ "exp":"120 deg", @@ -99,12 +99,12 @@ }, "G":{ "x":{ - "exp":"227.4375 in", - "val":5.7769125 + "exp":"5.713146 m", + "val":5.713146 }, "y":{ - "exp":"142.621161 in", - "val":3.6225774894 + "exp":"3.7209 m", + "val":3.7209 }, "heading":{ "exp":"180 deg", @@ -113,12 +113,12 @@ }, "H":{ "x":{ - "exp":"227.4375 in", - "val":5.7769125 + "exp":"5.713146 m", + "val":5.713146 }, "y":{ - "exp":"155.561161 in", - "val":3.9512534894 + "exp":"4.0509 m", + "val":4.0509 }, "heading":{ "exp":"180 deg", @@ -127,58 +127,58 @@ }, "I":{ "x":{ - "exp":"215.84522795660308 in", - "val":5.482468790097718 + "exp":"5.365372248154253 m", + "val":5.365372248154253 }, "y":{ - "exp":"194.45724315432372 in", - "val":4.939213976119822 + "exp":"4.933261807735684 m", + "val":4.933261807735684 }, "heading":{ - "exp":"-120 deg", - "val":-2.0943951023931953 + "exp":"240 deg", + "val":4.1887902047863905 } }, "J":{ "x":{ - "exp":"204.63885923163247 in", - "val":5.197827024483464 + "exp":"5.079583864905389 m", + "val":5.079583864905389 }, "y":{ - "exp":"200.92724315432375 in", - "val":5.103551976119823 + "exp":"5.098261807735684 m", + "val":5.098261807735684 }, "heading":{ - "exp":"-120 deg", - "val":-2.0943951023931953 + "exp":"240 deg", + "val":4.1887902047863905 } }, "K":{ "x":{ - "exp":"165.15772795660308 in", - "val":4.1950062900977185 + "exp":"4.141549248154255 m", + "val":4.141549248154255 }, "y":{ - "exp":"210.33608215432375 in", - "val":5.342536486719823 + "exp":"5.238261807735684 m", + "val":5.238261807735684 }, "heading":{ - "exp":"-60 deg", - "val":-1.0471975511965976 + "exp":"300 deg", + "val":5.235987755982989 } }, "L":{ "x":{ - "exp":"153.95135923163247 in", - "val":3.9103645244834646 + "exp":"3.85576086490539 m", + "val":3.85576086490539 }, "y":{ - "exp":"203.86608215432372 in", - "val":5.178198486719822 + "exp":"5.073261807735684 m", + "val":5.073261807735684 }, "heading":{ - "exp":"-60 deg", - "val":-1.0471975511965976 + "exp":"300 deg", + "val":5.235987755982989 } }, "barge":{ @@ -197,16 +197,16 @@ }, "fetch":{ "x":{ - "exp":"61.8667 in", - "val":1.57141418 + "exp":"1.58 m", + "val":1.58 }, "y":{ - "exp":"291.9457 in", - "val":7.41542078 + "exp":"7.3 m", + "val":7.3 }, "heading":{ - "exp":"-54.011392 deg", - "val":-0.9426766239853251 + "exp":"-52.8554 deg", + "val":-0.9225007574586108 } }, "start":{ diff --git a/src/main/deploy/choreo/ATofetch.traj b/src/main/deploy/choreo/ATofetch.traj index 6e452342..565460f6 100644 --- a/src/main/deploy/choreo/ATofetch.traj +++ b/src/main/deploy/choreo/ATofetch.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.2654999999999994, "y":4.3309, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":31, "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}, @@ -14,8 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":31, "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}, @@ -28,41 +28,41 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.23474], + "waypoints":[0.0,1.23713], "samples":[ - {"t":0.0, "x":3.20199, "y":4.42922, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.1104, "ay":9.94264, "alpha":-14.60769, "fx":[-42.24185,-17.99333,-111.2651,-169.25169], "fy":[194.39532,198.37567,165.25578,104.92931]}, - {"t":0.03859, "x":3.19818, "y":4.43662, "heading":0.0, "vx":-0.19719, "vy":0.38364, "omega":-0.56365, "ax":-5.20371, "ay":10.00203, "alpha":-13.64056, "fx":[-48.58569,-22.18043,-110.55426,-165.65302], "fy":[192.85818,197.91544,165.69541,110.44709]}, - {"t":0.07717, "x":3.1867, "y":4.45887, "heading":-0.02175, "vx":-0.39798, "vy":0.76958, "omega":-1.08997, "ax":-5.32263, "ay":10.08618, "alpha":-12.18478, "fx":[-59.10002,-27.42791,-108.4034,-159.97171], "fy":[189.83515,197.20269,167.05696,118.43218]}, - {"t":0.11576, "x":3.16738, "y":4.49608, "heading":-0.06381, "vx":-0.60335, "vy":1.15876, "omega":-1.56013, "ax":-5.45727, "ay":10.19722, "alpha":-10.10839, "fx":[-72.23535,-35.99472,-104.91713,-150.73334], "fy":[185.15409,195.7329,169.18255,129.86122]}, - {"t":0.15434, "x":3.14004, "y":4.54838, "heading":-0.124, "vx":-0.81392, "vy":1.55222, "omega":-1.95017, "ax":-5.59167, "ay":10.34375, "alpha":-6.85109, "fx":[-85.35885,-53.22437,-100.27529,-133.98298], "fy":[179.3829,191.60773,171.81349,146.89736]}, - {"t":0.19293, "x":3.10447, "y":4.61597, "heading":-0.19925, "vx":-1.02968, "vy":1.95134, "omega":-2.21452, "ax":-5.68624, "ay":10.46624, "alpha":-0.2355, "fx":[-94.65877,-93.37274,-94.91599,-96.19998], "fy":[174.53289,175.23259,174.40834,173.69521]}, - {"t":0.23151, "x":3.06051, "y":4.69906, "heading":-0.2847, "vx":-1.24909, "vy":2.35519, "omega":-2.22361, "ax":-5.2182, "ay":9.50653, "alpha":16.16849, "fx":[-95.3511,-175.32318,-93.94638,16.68121], "fy":[173.99706,92.09107,170.45618,197.33277]}, - {"t":0.2701, "x":3.00843, "y":4.79701, "heading":-0.3705, "vx":-1.45044, "vy":2.722, "omega":-1.59974, "ax":-5.50687, "ay":8.10975, "alpha":22.27664, "fx":[-87.22729,-181.93966,-148.24416,50.22338], "fy":[177.28819,75.40381,97.82061,190.2299]}, - {"t":0.30868, "x":2.94836, "y":4.90808, "heading":-0.43223, "vx":-1.66292, "vy":3.03492, "omega":-0.74018, "ax":-3.49709, "ay":7.41616, "alpha":18.99973, "fx":[-67.5423,-140.51181,-57.21851,32.09361], "fy":[160.41845,88.79629,84.43353,160.84733]}, - {"t":0.34727, "x":2.88159, "y":5.0307, "heading":-0.46079, "vx":-1.79786, "vy":3.32108, "omega":-0.00707, "ax":0.14496, "ay":0.08841, "alpha":0.02105, "fx":[2.39327,2.34771,2.43954,2.4851], "fy":[1.54242,1.45059,1.40498,1.4968]}, - {"t":0.38586, "x":2.81233, "y":5.15891, "heading":-0.46106, "vx":-1.79226, "vy":3.32449, "omega":-0.00625, "ax":-0.00294, "ay":-0.00153, "alpha":0.00008, "fx":[-0.04904,-0.04921,-0.04886,-0.04868], "fy":[-0.02519,-0.02555,-0.02572,-0.02537]}, - {"t":0.42444, "x":2.74317, "y":5.28719, "heading":-0.4613, "vx":-1.79238, "vy":3.32443, "omega":-0.00625, "ax":-0.02216, "ay":-0.01191, "alpha":0.00005, "fx":[-0.36952,-0.36963,-0.36941,-0.3693], "fy":[-0.19829,-0.19852,-0.19863,-0.1984]}, - {"t":0.46303, "x":2.674, "y":5.41545, "heading":-0.46154, "vx":-1.79323, "vy":3.32397, "omega":-0.00625, "ax":-0.02747, "ay":-0.01478, "alpha":0.00002, "fx":[-0.45798,-0.45803,-0.45792,-0.45787], "fy":[-0.24637,-0.24648,-0.24654,-0.24643]}, - {"t":0.50161, "x":2.60478, "y":5.5437, "heading":-0.46178, "vx":-1.79429, "vy":3.3234, "omega":-0.00625, "ax":-0.03225, "ay":-0.01739, "alpha":-0.00001, "fx":[-0.53754,-0.53751,-0.53756,-0.53759], "fy":[-0.28985,-0.28979,-0.28976,-0.28982]}, - {"t":0.5402, "x":2.53552, "y":5.67192, "heading":-0.46202, "vx":-1.79554, "vy":3.32273, "omega":-0.00625, "ax":-0.0391, "ay":-0.02112, "alpha":-0.00006, "fx":[-0.65172,-0.65159,-0.65185,-0.65197], "fy":[-0.35222,-0.35196,-0.35184,-0.35209]}, - {"t":0.57878, "x":2.46621, "y":5.80012, "heading":-0.46227, "vx":-1.79705, "vy":3.32191, "omega":-0.00625, "ax":-0.05097, "ay":-0.02757, "alpha":-0.00009, "fx":[-0.8495,-0.8493,-0.84969,-0.84989], "fy":[-0.45993,-0.45954,-0.45935,-0.45974]}, - {"t":0.61737, "x":2.39684, "y":5.92827, "heading":-0.46251, "vx":-1.79901, "vy":3.32085, "omega":-0.00625, "ax":-0.07217, "ay":-0.03912, "alpha":-0.00009, "fx":[-1.20302,-1.20283,-1.20322,-1.20341], "fy":[-0.65236,-0.65197,-0.65177,-0.65216]}, - {"t":0.65595, "x":2.32737, "y":6.05638, "heading":-0.46275, "vx":-1.8018, "vy":3.31934, "omega":-0.00626, "ax":-0.1044, "ay":-0.0567, "alpha":0.00004, "fx":[-1.74043,-1.74052,-1.74034,-1.74025], "fy":[-0.94498,-0.94516,-0.94525,-0.94507]}, - {"t":0.69454, "x":2.25776, "y":6.18442, "heading":-0.46299, "vx":-1.80583, "vy":3.31715, "omega":-0.00626, "ax":-0.12847, "ay":-0.06995, "alpha":0.00029, "fx":[-2.14191,-2.14255,-2.14126,-2.14062], "fy":[-1.16514,-1.16643,-1.16707,-1.16578]}, - {"t":0.73313, "x":2.18799, "y":6.31236, "heading":-0.46323, "vx":-1.81078, "vy":3.31445, "omega":-0.00624, "ax":-0.118, "ay":-0.0645, "alpha":0.0003, "fx":[-1.96741,-1.96806,-1.96676,-1.96611], "fy":[-1.07418,-1.07548,-1.07613,-1.07483]}, - {"t":0.77171, "x":2.11803, "y":6.4402, "heading":-0.46347, "vx":-1.81534, "vy":3.31196, "omega":-0.00623, "ax":-0.09147, "ay":-0.05018, "alpha":0.00015, "fx":[-1.52487,-1.5252,-1.52454,-1.52421], "fy":[-0.83599,-0.83666,-0.83699,-0.83633]}, - {"t":0.8103, "x":2.04792, "y":6.56796, "heading":-0.46371, "vx":-1.81887, "vy":3.31003, "omega":-0.00623, "ax":-0.11297, "ay":-0.0622, "alpha":0.00001, "fx":[-1.88319,-1.88321,-1.88318,-1.88317], "fy":[-1.03686,-1.03689,-1.0369,-1.03688]}, - {"t":0.84888, "x":1.97765, "y":6.69563, "heading":-0.46395, "vx":-1.82322, "vy":3.30763, "omega":-0.00623, "ax":-0.50308, "ay":-0.29048, "alpha":-0.02349, "fx":[-8.36043,-8.30952,-8.41179,-8.46271], "fy":[-4.91906,-4.81674,-4.76528,-4.8676]}, - {"t":0.88747, "x":1.90693, "y":6.82304, "heading":-0.46419, "vx":-1.84264, "vy":3.29642, "omega":-0.00713, "ax":2.81579, "ay":-7.86084, "alpha":-19.26499, "fx":[57.64456,136.69488,39.35405,-45.94221], "fy":[-166.86368,-98.73185,-96.8424,-161.70804]}, - {"t":0.92605, "x":1.83792, "y":6.94438, "heading":-0.46447, "vx":-1.73399, "vy":2.9931, "omega":-0.75048, "ax":5.55519, "ay":-8.09391, "alpha":-22.93539, "fx":[79.14007,176.34983,167.02885,-52.10911], "fy":[-181.04207,-87.90772,-81.13423,-189.60213]}, - {"t":0.96464, "x":1.77515, "y":7.05385, "heading":-0.49343, "vx":-1.51964, "vy":2.6808, "omega":-1.63546, "ax":5.60395, "ay":-9.25449, "alpha":-16.86485, "fx":[82.0384,166.46701,144.4783,-19.32281], "fy":[-180.65175,-107.50149,-131.96447,-196.95423]}, - {"t":1.00322, "x":1.72069, "y":7.1504, "heading":-0.55653, "vx":-1.30341, "vy":2.32371, "omega":-2.2862, "ax":5.78598, "ay":-10.41253, "alpha":0.1264, "fx":[96.65738,95.719,96.24418,97.17754], "fy":[-173.45341,-173.9758,-173.69079,-173.16756]}, - {"t":1.04181, "x":1.6747, "y":7.23231, "heading":-0.64475, "vx":-1.08015, "vy":1.92193, "omega":-2.28132, "ax":5.7227, "ay":-10.29298, "alpha":6.51348, "fx":[113.47222,55.46401,84.79935,127.84293], "fy":[-163.10487,-190.90811,-179.95081,-152.35226]}, - {"t":1.0804, "x":1.63729, "y":7.29881, "heading":-0.73277, "vx":-0.85934, "vy":1.52477, "omega":-2.02999, "ax":5.65696, "ay":-10.10726, "alpha":10.03161, "fx":[130.2671,32.8705,76.37848,137.67895], "fy":[-150.17796,-196.19118,-183.83113,-143.73236]}, - {"t":1.11898, "x":1.60834, "y":7.35012, "heading":-0.8111, "vx":-0.64106, "vy":1.13478, "omega":-1.64292, "ax":5.59851, "ay":-9.93438, "alpha":12.50397, "fx":[144.10396,18.77424,69.35483,141.06515], "fy":[-137.09263,-198.11273,-186.67108,-140.52897]}, - {"t":1.15757, "x":1.58777, "y":7.38651, "heading":-0.87449, "vx":-0.42504, "vy":0.75146, "omega":-1.16045, "ax":5.53715, "ay":-9.79247, "alpha":14.35249, "fx":[154.14279,9.3132,63.64191,142.10828], "fy":[-125.81969,-198.83282,-188.74217,-139.54849]}, - {"t":1.19615, "x":1.57549, "y":7.40821, "heading":-0.91927, "vx":-0.21139, "vy":0.37361, "omega":-0.60665, "ax":5.47836, "ay":-9.68262, "alpha":15.72215, "fx":[160.91625,2.55465,59.36821,142.44739], "fy":[-117.13166,-199.07247,-190.16165,-139.25245]}, - {"t":1.23474, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.2655, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.30896, "ay":9.88198, "alpha":-14.2027, "fx":[-49.79715,-21.5846,-112.85386,-169.75595], "fy":[192.60411,198.01611,164.17484,104.11653]}, + {"t":0.03866, "x":3.26153, "y":4.33828, "heading":0.0, "vx":-0.20525, "vy":0.38204, "omega":-0.54908, "ax":-5.39586, "ay":9.93662, "alpha":-13.281, "fx":[-55.53562,-25.77834,-112.19147,-166.2803], "fy":[190.97923,197.47805,164.59131,109.50615]}, + {"t":0.07732, "x":3.24957, "y":4.36048, "heading":-0.02123, "vx":-0.41385, "vy":0.7662, "omega":-1.06253, "ax":-5.5072, "ay":10.01397, "alpha":-11.89483, "fx":[-65.17468,-31.06793,-110.16236,-160.80484], "fy":[187.84378,196.66126,165.90257,117.30477]}, + {"t":0.11598, "x":3.22945, "y":4.39759, "heading":-0.06231, "vx":-0.62676, "vy":1.15334, "omega":-1.52239, "ax":-5.63373, "ay":10.11742, "alpha":-9.90408, "fx":[-77.20491,-39.6519,-106.8783,-151.91119], "fy":[183.14628,195.02466,167.95128,128.48823]}, + {"t":0.15464, "x":3.20101, "y":4.44974, "heading":-0.12116, "vx":-0.84457, "vy":1.54449, "omega":-1.90529, "ax":-5.76169, "ay":10.25597, "alpha":-6.74921, "fx":[-89.14902,-56.67983,-102.54631,-135.80334], "fy":[177.53836,190.61584,170.47022,145.22379]}, + {"t":0.1933, "x":3.16405, "y":4.51711, "heading":-0.19482, "vx":-1.06732, "vy":1.94099, "omega":-2.16622, "ax":-5.85423, "ay":10.37353, "alpha":-0.31926, "fx":[-97.43777,-95.68462,-97.73927,-99.48724], "fy":[173.00259,173.98955,172.85305,171.84194]}, + {"t":0.23196, "x":3.11841, "y":4.5999, "heading":-0.27857, "vx":-1.29364, "vy":2.34203, "omega":-2.17856, "ax":-5.42083, "ay":9.46516, "alpha":15.71702, "fx":[-97.44713,-174.89501,-99.69209,10.58371], "fy":[172.8305,92.9213,167.61559,197.75099]}, + {"t":0.27062, "x":3.06435, "y":4.69752, "heading":-0.36279, "vx":-1.50322, "vy":2.70796, "omega":-1.57093, "ax":-5.65468, "ay":8.01707, "alpha":22.30033, "fx":[-89.35516,-182.61572,-152.2676,47.19515], "fy":[176.21693,73.75,93.61352,190.98216]}, + {"t":0.30928, "x":3.00201, "y":4.8082, "heading":-0.42353, "vx":-1.72183, "vy":3.0179, "omega":-0.70879, "ax":-3.34642, "ay":7.0843, "alpha":18.15978, "fx":[-66.21869,-133.09216,-50.597,26.77517], "fy":[154.31283,86.08343,78.21784,153.75344]}, + {"t":0.34794, "x":2.93294, "y":4.93017, "heading":-0.45093, "vx":-1.8512, "vy":3.29178, "omega":-0.00672, "ax":0.1361, "ay":0.08415, "alpha":0.01598, "fx":[2.25059,2.21669,2.28673,2.32063], "fy":[1.45468,1.38464,1.35071,1.42075]}, + {"t":0.3866, "x":2.86147, "y":5.05749, "heading":-0.45119, "vx":-1.84594, "vy":3.29504, "omega":-0.00611, "ax":-0.00182, "ay":-0.00096, "alpha":0.00009, "fx":[-0.03046,-0.03065,-0.03026,-0.03006], "fy":[-0.01575,-0.01615,-0.01634,-0.01594]}, + {"t":0.42527, "x":2.79011, "y":5.18488, "heading":-0.45142, "vx":-1.84601, "vy":3.295, "omega":-0.0061, "ax":-0.01944, "ay":-0.01084, "alpha":0.00007, "fx":[-0.32418,-0.32433,-0.32402,-0.32388], "fy":[-0.18052,-0.18082,-0.18097,-0.18067]}, + {"t":0.46393, "x":2.71873, "y":5.31226, "heading":-0.45166, "vx":-1.84676, "vy":3.29458, "omega":-0.0061, "ax":-0.02407, "ay":-0.01345, "alpha":0.00005, "fx":[-0.40128,-0.40138,-0.40116,-0.40106], "fy":[-0.224,-0.22422,-0.22433,-0.22411]}, + {"t":0.50259, "x":2.64731, "y":5.43962, "heading":-0.4519, "vx":-1.84769, "vy":3.29406, "omega":-0.0061, "ax":-0.02792, "ay":-0.01562, "alpha":0.00002, "fx":[-0.46538,-0.46543,-0.46533,-0.46529], "fy":[-0.26035,-0.26044,-0.26049,-0.26039]}, + {"t":0.54125, "x":2.57586, "y":5.56696, "heading":-0.45213, "vx":-1.84877, "vy":3.29346, "omega":-0.0061, "ax":-0.03281, "ay":-0.01839, "alpha":-0.00002, "fx":[-0.54691,-0.54687,-0.54695,-0.547], "fy":[-0.30668,-0.30659,-0.30655,-0.30663]}, + {"t":0.57991, "x":2.50436, "y":5.69427, "heading":-0.45237, "vx":-1.85004, "vy":3.29275, "omega":-0.0061, "ax":-0.04041, "ay":-0.0227, "alpha":-0.00007, "fx":[-0.67354,-0.67338,-0.67371,-0.67386], "fy":[-0.37858,-0.37825,-0.3781,-0.37842]}, + {"t":0.61857, "x":2.43281, "y":5.82155, "heading":-0.4526, "vx":-1.8516, "vy":3.29187, "omega":-0.0061, "ax":-0.0548, "ay":-0.03083, "alpha":-0.00011, "fx":[-0.91341,-0.91317,-0.91367,-0.91392], "fy":[-0.51436,-0.51386,-0.51361,-0.51412]}, + {"t":0.65723, "x":2.36118, "y":5.94879, "heading":-0.45284, "vx":-1.85372, "vy":3.29068, "omega":-0.00611, "ax":-0.08393, "ay":-0.04731, "alpha":-0.00012, "fx":[-1.39887,-1.39861,-1.39914,-1.3994], "fy":[-0.78905,-0.78852,-0.78827,-0.78879]}, + {"t":0.69589, "x":2.28945, "y":6.07598, "heading":-0.45307, "vx":-1.85697, "vy":3.28885, "omega":-0.00611, "ax":-0.13391, "ay":-0.07567, "alpha":0.00009, "fx":[-2.23234,-2.23254,-2.23213,-2.23193], "fy":[-1.26101,-1.26142,-1.26162,-1.26121]}, + {"t":0.73455, "x":2.21756, "y":6.20307, "heading":-0.45331, "vx":-1.86214, "vy":3.28592, "omega":-0.00611, "ax":-0.16679, "ay":-0.09458, "alpha":0.00047, "fx":[-2.78092,-2.78193,-2.77985,-2.77885], "fy":[-1.57504,-1.57711,-1.57812,-1.57605]}, + {"t":0.77321, "x":2.14544, "y":6.33003, "heading":-0.45355, "vx":-1.86859, "vy":3.28227, "omega":-0.00609, "ax":-0.14307, "ay":-0.08153, "alpha":0.00035, "fx":[-2.38528,-2.38602,-2.38449,-2.38375], "fy":[-1.35794,-1.35946,-1.3602,-1.35868]}, + {"t":0.81187, "x":2.0731, "y":6.45687, "heading":-0.45378, "vx":-1.87412, "vy":3.27911, "omega":-0.00608, "ax":-0.13178, "ay":-0.07546, "alpha":0.00012, "fx":[-2.1969,-2.19714,-2.19664,-2.19639], "fy":[-1.25744,-1.25795,-1.2582,-1.25769]}, + {"t":0.85053, "x":2.00054, "y":6.58358, "heading":-0.45402, "vx":-1.87922, "vy":3.2762, "omega":-0.00607, "ax":-0.44204, "ay":-0.26348, "alpha":-0.01728, "fx":[-7.34912,-7.31237,-7.38799,-7.42473], "fy":[-4.44847,-4.37285,-4.33579,-4.41141]}, + {"t":0.88919, "x":1.92756, "y":6.71005, "heading":-0.45425, "vx":-1.89631, "vy":3.26601, "omega":-0.00674, "ax":2.69567, "ay":-7.51981, "alpha":-18.40304, "fx":[56.53034,128.80933,34.33934,-39.93669], "fy":[-160.75486,-96.01529,-89.40517,-155.23087]}, + {"t":0.92785, "x":1.85627, "y":6.83069, "heading":-0.45451, "vx":-1.79209, "vy":2.97529, "omega":-0.71821, "ax":5.70514, "ay":-7.9955, "alpha":-22.9634, "fx":[81.90445,177.44468,169.4511,-48.39244], "fy":[-179.79677,-85.66873,-77.10406,-190.55508]}, + {"t":0.96651, "x":1.79125, "y":6.93974, "heading":-0.48228, "vx":-1.57153, "vy":2.66618, "omega":-1.60598, "ax":5.767, "ay":-9.23442, "alpha":-16.33864, "fx":[85.17977,166.78278,144.74787,-12.17788], "fy":[-179.19053,-107.02025,-131.99584,-197.52651]}, + {"t":1.00517, "x":1.7348, "y":7.03592, "heading":-0.54437, "vx":-1.34857, "vy":2.30917, "omega":-2.23764, "ax":5.97116, "ay":-10.30787, "alpha":0.17994, "fx":[99.83586,98.5077,99.24201,100.55985], "fy":[-171.65007,-172.41967,-172.00515,-171.23415]}, + {"t":1.04383, "x":1.68713, "y":7.11749, "heading":-0.63088, "vx":-1.11773, "vy":1.91067, "omega":-2.23069, "ax":5.90684, "ay":-10.19527, "alpha":6.39024, "fx":[116.11521,59.72799,88.04782,129.96561], "fy":[-161.24442,-189.62146,-178.3856,-150.54927]}, + {"t":1.08249, "x":1.64833, "y":7.18374, "heading":-0.71711, "vx":-0.88936, "vy":1.51651, "omega":-1.98364, "ax":5.8396, "ay":-10.0207, "alpha":9.81045, "fx":[132.11588,37.8889,79.86673,139.5017], "fy":[-148.56542,-195.28709,-182.34303,-141.96529]}, + {"t":1.12115, "x":1.61831, "y":7.23488, "heading":-0.7938, "vx":-0.6636, "vy":1.12911, "omega":-1.60436, "ax":5.78055, "ay":-9.85858, "alpha":12.20174, "fx":[145.26601,24.24682,73.06989,142.85302], "fy":[-135.87087,-197.52027,-185.24836,-138.71147]}, + {"t":1.15981, "x":1.59697, "y":7.27116, "heading":-0.85583, "vx":-0.44012, "vy":0.74797, "omega":-1.13264, "ax":5.72047, "ay":-9.72537, "alpha":13.98608, "fx":[154.85325,15.08528,67.55032,143.94117], "fy":[-124.9526,-198.48087,-187.37875,-137.65698]}, + {"t":1.19847, "x":1.58423, "y":7.29281, "heading":-0.89962, "vx":-0.21897, "vy":0.37198, "omega":-0.59193, "ax":5.66388, "ay":-9.62184, "alpha":15.31093, "fx":[161.36875,8.53205,63.42083,144.3352], "fy":[-116.51418,-198.90836,-188.84852,-137.29443]}, + {"t":1.23713, "x":1.58, "y":7.3, "heading":-0.9225, "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/BTofetch.traj b/src/main/deploy/choreo/BTofetch.traj index 159372cf..bd94a4c9 100644 --- a/src/main/deploy/choreo/BTofetch.traj +++ b/src/main/deploy/choreo/BTofetch.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.2019875, "y":4.1005465106, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.2654999999999994, "y":4.0009, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "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}, @@ -14,8 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "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}, @@ -28,43 +28,43 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.31206], + "waypoints":[0.0,1.31413], "samples":[ - {"t":0.0, "x":3.20199, "y":4.10055, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.6648, "ay":10.14345, "alpha":-14.83674, "fx":[-27.97393,-12.13773,-106.33687,-164.59155], "fy":[196.96825,198.82189,168.46797,112.08741]}, - {"t":0.03859, "x":3.19851, "y":4.1081, "heading":0.0, "vx":-0.18001, "vy":0.39143, "omega":-0.57255, "ax":-4.75765, "ay":10.21164, "alpha":-13.8093, "fx":[-34.92516,-16.1874,-105.49114,-160.62711], "fy":[195.80609,198.49779,168.96255,117.62581]}, - {"t":0.07718, "x":3.18802, "y":4.13081, "heading":-0.02209, "vx":-0.36361, "vy":0.7855, "omega":-1.10545, "ax":-4.87394, "ay":10.30706, "alpha":-12.27357, "fx":[-46.19984,-21.21101,-103.10999,-154.46403], "fy":[193.39132,197.97059,170.37332,125.5198]}, - {"t":0.11577, "x":3.17036, "y":4.16879, "heading":-0.06475, "vx":-0.5517, "vy":1.18325, "omega":-1.57908, "ax":-5.0056, "ay":10.42937, "alpha":-10.1, "fx":[-60.50185,-29.45503,-99.25587,-144.55118], "fy":[189.3226,196.82643,172.56269,136.69892]}, - {"t":0.15436, "x":3.14535, "y":4.22222, "heading":-0.12569, "vx":-0.74486, "vy":1.58572, "omega":-1.96884, "ax":-5.13533, "ay":10.58366, "alpha":-6.7496, "fx":[-75.36457,-46.29136,-93.97576,-126.78195], "fy":[183.81229,193.40307,175.33558,153.14739]}, - {"t":0.19295, "x":3.11278, "y":4.29129, "heading":-0.20167, "vx":-0.94303, "vy":1.99414, "omega":-2.22931, "ax":-5.2207, "ay":10.70622, "alpha":-0.08326, "fx":[-86.95903,-86.51777,-87.09422,-87.53555], "fy":[178.49838,178.71564,178.43769,178.21865]}, - {"t":0.23154, "x":3.0725, "y":4.37622, "heading":-0.2877, "vx":-1.1445, "vy":2.40729, "omega":-2.23252, "ax":-4.65831, "ay":9.76243, "alpha":16.39189, "fx":[-90.47134,-172.79447,-71.25339,23.9121], "fy":[176.58779,96.70698,181.0281,196.61693]}, - {"t":0.27013, "x":3.02487, "y":4.47639, "heading":-0.37385, "vx":-1.32426, "vy":2.78402, "omega":-1.59996, "ax":-4.95961, "ay":8.39275, "alpha":22.20583, "fx":[-83.03667,-181.25139,-124.49758,58.0881], "fy":[179.31037,76.99386,115.25325,188.05529]}, - {"t":0.30872, "x":2.97007, "y":4.59007, "heading":-0.43559, "vx":-1.51565, "vy":3.1079, "omega":-0.74304, "ax":-3.43616, "ay":7.51218, "alpha":19.0715, "fx":[-66.5983,-140.79904,-55.83711,34.11811], "fy":[161.78949,90.28018,87.05082,161.77699]}, - {"t":0.34731, "x":2.90902, "y":4.7156, "heading":-0.46426, "vx":-1.64825, "vy":3.39779, "omega":-0.00707, "ax":0.06948, "ay":0.04394, "alpha":0.02175, "fx":[1.13454,1.08711,1.18184,1.22927], "fy":[0.80358,0.70884,0.66139,0.75612]}, - {"t":0.3859, "x":2.84547, "y":4.84675, "heading":-0.46454, "vx":-1.64557, "vy":3.39949, "omega":-0.00623, "ax":-0.00813, "ay":-0.00387, "alpha":0.00011, "fx":[-0.13561,-0.13585,-0.13536,-0.13511], "fy":[-0.06418,-0.06467,-0.06492,-0.06442]}, - {"t":0.42449, "x":2.78196, "y":4.97793, "heading":-0.46478, "vx":-1.64589, "vy":3.39934, "omega":-0.00623, "ax":-0.01914, "ay":-0.00921, "alpha":0.0001, "fx":[-0.31908,-0.31929,-0.31887,-0.31865], "fy":[-0.15319,-0.15361,-0.15383,-0.1534]}, - {"t":0.46308, "x":2.71843, "y":5.10911, "heading":-0.46502, "vx":-1.64663, "vy":3.39898, "omega":-0.00622, "ax":-0.0228, "ay":-0.01099, "alpha":0.00009, "fx":[-0.3801,-0.38029,-0.37991,-0.37973], "fy":[-0.18293,-0.1833,-0.18349,-0.18312]}, - {"t":0.50167, "x":2.65487, "y":5.24026, "heading":-0.46526, "vx":-1.6475, "vy":3.39856, "omega":-0.00622, "ax":-0.0261, "ay":-0.0126, "alpha":0.00007, "fx":[-0.43513,-0.43528,-0.43499,-0.43484], "fy":[-0.20989,-0.21018,-0.21032,-0.21003]}, - {"t":0.54026, "x":2.59127, "y":5.3714, "heading":-0.4655, "vx":-1.64851, "vy":3.39807, "omega":-0.00622, "ax":-0.03, "ay":-0.01451, "alpha":0.00004, "fx":[-0.50007,-0.50015,-0.49999,-0.49991], "fy":[-0.2418,-0.24196,-0.24205,-0.24188]}, - {"t":0.57885, "x":2.52764, "y":5.50252, "heading":-0.46574, "vx":-1.64967, "vy":3.39751, "omega":-0.00622, "ax":-0.0351, "ay":-0.01702, "alpha":-0.00001, "fx":[-0.58516,-0.58514,-0.58518,-0.58519], "fy":[-0.28371,-0.28368,-0.28367,-0.2837]}, - {"t":0.61744, "x":2.46395, "y":5.63362, "heading":-0.46598, "vx":-1.65102, "vy":3.39685, "omega":-0.00622, "ax":-0.04298, "ay":-0.02088, "alpha":-0.00008, "fx":[-0.71639,-0.71623,-0.71655,-0.71672], "fy":[-0.34838,-0.34805,-0.34789,-0.34821]}, - {"t":0.65603, "x":2.4002, "y":5.76469, "heading":-0.46622, "vx":-1.65268, "vy":3.39605, "omega":-0.00622, "ax":-0.05877, "ay":-0.02862, "alpha":-0.00015, "fx":[-0.97948,-0.97916,-0.9798,-0.98012], "fy":[-0.47755,-0.47691,-0.47658,-0.47723]}, - {"t":0.69462, "x":2.33638, "y":5.89572, "heading":-0.46646, "vx":-1.65495, "vy":3.39494, "omega":-0.00623, "ax":-0.09597, "ay":-0.04685, "alpha":-0.0002, "fx":[-1.59963,-1.59918,-1.60007,-1.60051], "fy":[-0.78164,-0.78075,-0.78031,-0.78119]}, - {"t":0.73321, "x":2.27245, "y":6.0267, "heading":-0.4667, "vx":-1.65865, "vy":3.39314, "omega":-0.00623, "ax":-0.17689, "ay":-0.08663, "alpha":-0.00001, "fx":[-2.94868,-2.94867,-2.9487,-2.94871], "fy":[-1.44402,-1.44399,-1.44398,-1.444]}, - {"t":0.7718, "x":2.20831, "y":6.15757, "heading":-0.46694, "vx":-1.66548, "vy":3.38979, "omega":-0.00623, "ax":-0.24011, "ay":-0.11816, "alpha":0.00065, "fx":[-4.00316,-4.00459,-4.00174,-4.00031], "fy":[-1.96752,-1.97037,-1.9718,-1.96896]}, - {"t":0.81039, "x":2.14386, "y":6.2883, "heading":-0.46718, "vx":-1.67475, "vy":3.38523, "omega":-0.00621, "ax":-0.19255, "ay":-0.09544, "alpha":0.0004, "fx":[-3.21012,-3.21099,-3.20926,-3.20839], "fy":[-1.58964,-1.59137,-1.59225,-1.59052]}, - {"t":0.84898, "x":2.07909, "y":6.41886, "heading":-0.46742, "vx":-1.68218, "vy":3.38155, "omega":-0.00619, "ax":-0.11227, "ay":-0.05594, "alpha":0.00014, "fx":[-1.87169,-1.872,-1.87139,-1.87108], "fy":[-0.93211,-0.93272,-0.93303,-0.93242]}, - {"t":0.88757, "x":2.01409, "y":6.54931, "heading":-0.46766, "vx":-1.68651, "vy":3.37939, "omega":-0.00619, "ax":-0.11498, "ay":-0.05752, "alpha":0.0, "fx":[-1.91674,-1.91674,-1.91674,-1.91675], "fy":[-0.95875,-0.95875,-0.95875,-0.95875]}, - {"t":0.92616, "x":1.94892, "y":6.67968, "heading":-0.4679, "vx":-1.69095, "vy":3.37717, "omega":-0.00619, "ax":-0.47686, "ay":-0.25093, "alpha":-0.02228, "fx":[-7.92505,-7.87634,-7.97314,-8.02185], "fy":[-4.25595,-4.15908,-4.10994,-4.2068]}, - {"t":0.96475, "x":1.88331, "y":6.80982, "heading":-0.46814, "vx":-1.70935, "vy":3.36749, "omega":-0.00705, "ax":2.51816, "ay":-7.9237, "alpha":-19.07622, "fx":[54.00885,132.86196,29.70436,-48.66929], "fy":[-167.22021,-101.51389,-99.40457,-160.19858]}, - {"t":1.00334, "x":1.81923, "y":6.93387, "heading":-0.46841, "vx":-1.61217, "vy":3.06171, "omega":-0.7432, "ax":5.21723, "ay":-8.2958, "alpha":-22.75464, "fx":[74.58173,175.04104,158.09757,-59.84514], "fy":[-182.97918,-90.42646,-92.36886,-187.37358]}, - {"t":1.04193, "x":1.7609, "y":7.04584, "heading":-0.49709, "vx":-1.41084, "vy":2.74158, "omega":-1.62129, "ax":5.18794, "ay":-9.5036, "alpha":-16.67175, "fx":[76.84977,163.799,132.96933,-27.69631], "fy":[-182.92454,-111.49712,-143.27642,-195.98381]}, - {"t":1.08052, "x":1.71031, "y":7.14457, "heading":-0.55965, "vx":-1.21064, "vy":2.37484, "omega":-2.26466, "ax":5.35242, "ay":-10.64186, "alpha":0.04239, "fx":[89.28373,88.96969,89.16112,89.47464], "fy":[-177.36252,-177.52124,-177.42678,-177.2679]}, - {"t":1.11911, "x":1.66758, "y":7.22829, "heading":-0.64705, "vx":-1.00409, "vy":1.96417, "omega":-2.26302, "ax":5.30031, "ay":-10.51889, "alpha":6.46315, "fx":[105.04045,47.59311,78.69139,122.08973], "fy":[-168.65419,-193.02548,-182.70441,-156.9951]}, - {"t":1.1577, "x":1.63278, "y":7.29625, "heading":-0.73438, "vx":-0.79955, "vy":1.55825, "omega":-2.01361, "ax":5.24765, "ay":-10.33056, "alpha":9.94378, "fx":[121.82476,24.59779,70.63673,132.844], "fy":[-157.09492,-197.40506,-186.11366,-148.20813]}, - {"t":1.19629, "x":1.60583, "y":7.34869, "heading":-0.81208, "vx":-0.59705, "vy":1.15959, "omega":-1.62988, "ax":5.20436, "ay":-10.15374, "alpha":12.38799, "fx":[136.27921,10.24984,63.7979,136.6896], "fy":[-144.86093,-198.74258,-188.64351,-144.78514]}, - {"t":1.23488, "x":1.58667, "y":7.38588, "heading":-0.87498, "vx":-0.39621, "vy":0.76776, "omega":-1.15183, "ax":5.15721, "ay":-10.00571, "alpha":14.23486, "fx":[147.08634,0.60446,58.19996,137.98201], "fy":[-133.98647,-199.0564,-190.49158,-143.6271]}, - {"t":1.27347, "x":1.57522, "y":7.40806, "heading":-0.91943, "vx":-0.19719, "vy":0.38164, "omega":-0.60251, "ax":5.10999, "ay":-9.88966, "alpha":15.61306, "fx":[154.49671,-6.25017,54.01348,138.46432], "fy":[-125.46286,-198.99714,-191.7523,-143.21141]}, - {"t":1.31206, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.2655, "y":4.0009, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.83505, "ay":10.10411, "alpha":-14.45292, "fx":[-34.56375,-15.21964,-107.65803,-164.95009], "fy":[195.92378,198.60886,167.62664,111.56328]}, + {"t":0.03865, "x":3.26189, "y":4.00845, "heading":0.0, "vx":-0.18688, "vy":0.39053, "omega":-0.55862, "ax":-4.92256, "ay":10.16731, "alpha":-13.46854, "fx":[-40.99843,-19.27311,-106.85522,-161.09988], "fy":[194.63015,198.2211,168.10316,116.98228]}, + {"t":0.0773, "x":3.25099, "y":4.03114, "heading":-0.02159, "vx":-0.37714, "vy":0.78351, "omega":-1.07919, "ax":-5.0326, "ay":10.2558, "alpha":-11.99808, "fx":[-51.51855,-24.33421,-104.58499,-155.12615], "fy":[192.04903,197.61027,169.47202,124.70587]}, + {"t":0.11595, "x":3.23265, "y":4.06908, "heading":-0.0633, "vx":-0.57165, "vy":1.17991, "omega":-1.54293, "ax":-5.15731, "ay":10.37043, "alpha":-9.90611, "fx":[-64.84177,-32.59414,-100.91548,-145.52837], "fy":[187.88769,196.3308,171.59803,135.6639]}, + {"t":0.1546, "x":3.20671, "y":4.12243, "heading":-0.12294, "vx":-0.77099, "vy":1.58073, "omega":-1.92581, "ax":-5.2812, "ay":10.51695, "alpha":-6.65428, "fx":[-78.63141,-49.24899,-95.9173,-128.34266], "fy":[182.44602,192.67269,174.2831,151.84829]}, + {"t":0.19325, "x":3.17296, "y":4.19138, "heading":-0.19737, "vx":-0.97511, "vy":1.98722, "omega":-2.183, "ax":-5.36431, "ay":10.63536, "alpha":-0.16255, "fx":[-89.29833,-88.43237,-89.54254,-90.40846], "fy":[177.34455,177.78369,177.23158,176.78574]}, + {"t":0.23191, "x":3.13127, "y":4.27613, "heading":-0.28175, "vx":-1.18245, "vy":2.39829, "omega":-2.18928, "ax":-4.84959, "ay":9.74012, "alpha":15.92924, "fx":[-92.22034,-172.20512,-77.55869,18.6228], "fy":[175.67916,97.76778,178.81929,197.18623]}, + {"t":0.27056, "x":3.08194, "y":4.37611, "heading":-0.36637, "vx":-1.36989, "vy":2.77475, "omega":-1.5736, "ax":-5.10699, "ay":8.31268, "alpha":22.21564, "fx":[-84.85417,-181.87253,-129.52164,55.72403], "fy":[178.44967,75.50588,111.56567,188.75227]}, + {"t":0.30921, "x":3.02518, "y":4.48956, "heading":-0.42719, "vx":-1.56728, "vy":3.09605, "omega":-0.71495, "ax":-3.30428, "ay":7.21347, "alpha":18.32308, "fx":[-65.50741,-134.11262,-49.84902,29.14605], "fy":[156.32275,87.87055,81.24969,155.53712]}, + {"t":0.34786, "x":2.96213, "y":4.61462, "heading":-0.45482, "vx":-1.69499, "vy":3.37485, "omega":-0.00674, "ax":0.0758, "ay":0.04606, "alpha":0.01684, "fx":[1.24478,1.20877,1.28244,1.31845], "fy":[0.82271,0.74904,0.71298,0.78663]}, + {"t":0.38651, "x":2.89668, "y":4.74509, "heading":-0.45508, "vx":-1.69206, "vy":3.37663, "omega":-0.00609, "ax":-0.00577, "ay":-0.00283, "alpha":0.00012, "fx":[-0.09629,-0.09654,-0.09603,-0.09578], "fy":[-0.04678,-0.04728,-0.04753,-0.04703]}, + {"t":0.42516, "x":2.83127, "y":4.8756, "heading":-0.45532, "vx":-1.69229, "vy":3.37652, "omega":-0.00609, "ax":-0.01682, "ay":-0.00837, "alpha":0.0001, "fx":[-0.28055,-0.28077,-0.28032,-0.28009], "fy":[-0.13925,-0.13971,-0.13993,-0.13947]}, + {"t":0.46381, "x":2.76585, "y":5.0061, "heading":-0.45555, "vx":-1.69294, "vy":3.3762, "omega":-0.00609, "ax":-0.02018, "ay":-0.01006, "alpha":0.0001, "fx":[-0.33647,-0.33668,-0.33626,-0.33605], "fy":[-0.16742,-0.16784,-0.16805,-0.16762]}, + {"t":0.50246, "x":2.7004, "y":5.13658, "heading":-0.45579, "vx":-1.69372, "vy":3.37581, "omega":-0.00608, "ax":-0.02307, "ay":-0.01152, "alpha":0.00008, "fx":[-0.38467,-0.38485,-0.38448,-0.3843], "fy":[-0.19179,-0.19216,-0.19234,-0.19197]}, + {"t":0.54111, "x":2.63492, "y":5.26705, "heading":-0.45602, "vx":-1.69461, "vy":3.37537, "omega":-0.00608, "ax":-0.02638, "ay":-0.0132, "alpha":0.00006, "fx":[-0.43981,-0.43995,-0.43966,-0.43953], "fy":[-0.21976,-0.22004,-0.22018,-0.2199]}, + {"t":0.57976, "x":2.5694, "y":5.3975, "heading":-0.45626, "vx":-1.69563, "vy":3.37486, "omega":-0.00608, "ax":-0.03043, "ay":-0.01525, "alpha":0.00003, "fx":[-0.50727,-0.50734,-0.5072,-0.50713], "fy":[-0.25409,-0.25424,-0.25431,-0.25416]}, + {"t":0.61841, "x":2.50384, "y":5.52793, "heading":-0.45649, "vx":-1.6968, "vy":3.37427, "omega":-0.00607, "ax":-0.0359, "ay":-0.01803, "alpha":-0.00002, "fx":[-0.59848,-0.59845,-0.59852,-0.59855], "fy":[-0.3006,-0.30053,-0.30049,-0.30056]}, + {"t":0.65707, "x":2.43823, "y":5.65834, "heading":-0.45673, "vx":-1.69819, "vy":3.37357, "omega":-0.00608, "ax":-0.04499, "ay":-0.02264, "alpha":-0.00009, "fx":[-0.74983,-0.74963,-0.75004,-0.75024], "fy":[-0.37777,-0.37736,-0.37716,-0.37757]}, + {"t":0.69572, "x":2.37256, "y":5.78871, "heading":-0.45696, "vx":-1.69993, "vy":3.37269, "omega":-0.00608, "ax":-0.06606, "ay":-0.03333, "alpha":-0.00019, "fx":[-1.10105,-1.10065,-1.10147,-1.10188], "fy":[-0.55622,-0.55539,-0.55499,-0.55581]}, + {"t":0.73437, "x":2.30681, "y":5.91905, "heading":-0.4572, "vx":-1.70248, "vy":3.37141, "omega":-0.00609, "ax":-0.12937, "ay":-0.06546, "alpha":-0.0003, "fx":[-2.15623,-2.15559,-2.15688,-2.15752], "fy":[-1.09219,-1.09089,-1.09026,-1.09155]}, + {"t":0.77302, "x":2.24091, "y":6.04931, "heading":-0.45743, "vx":-1.70748, "vy":3.36888, "omega":-0.0061, "ax":-0.3243, "ay":-0.165, "alpha":0.0001, "fx":[-5.40607,-5.40627,-5.40586,-5.40565], "fy":[-2.75013,-2.75055,-2.75075,-2.75034]}, + {"t":0.81167, "x":2.17467, "y":6.17939, "heading":-0.45767, "vx":-1.72002, "vy":3.3625, "omega":-0.00609, "ax":-0.43159, "ay":-0.22171, "alpha":0.00131, "fx":[-7.19587,-7.19869,-7.19296,-7.19014], "fy":[-3.69155,-3.69728,-3.70012,-3.69438]}, + {"t":0.85032, "x":2.10787, "y":6.30919, "heading":-0.4579, "vx":-1.7367, "vy":3.35393, "omega":-0.00604, "ax":-0.27618, "ay":-0.14353, "alpha":0.00036, "fx":[-4.60418,-4.60496,-4.60337,-4.60259], "fy":[-2.3914,-2.39298,-2.39377,-2.39218]}, + {"t":0.88897, "x":2.04054, "y":6.43872, "heading":-0.45814, "vx":-1.74737, "vy":3.34838, "omega":-0.00603, "ax":-0.15551, "ay":-0.08135, "alpha":0.0001, "fx":[-2.59231,-2.59252,-2.5921,-2.59189], "fy":[-1.35583,-1.35625,-1.35646,-1.35604]}, + {"t":0.92762, "x":1.97289, "y":6.56807, "heading":-0.45837, "vx":-1.75338, "vy":3.34524, "omega":-0.00603, "ax":-0.43111, "ay":-0.23536, "alpha":-0.01671, "fx":[-7.16783,-7.13199,-7.20494,-7.2408], "fy":[-3.97796,-3.90492,-3.86879,-3.94182]}, + {"t":0.96627, "x":1.90479, "y":6.69719, "heading":-0.4586, "vx":-1.77005, "vy":3.33614, "omega":-0.00667, "ax":2.44237, "ay":-7.5854, "alpha":-18.26497, "fx":[53.19878,125.51112,26.91932,-42.77676], "fy":[-161.32407,-98.55389,-91.70258,-154.19917]}, + {"t":1.00492, "x":1.8382, "y":6.82047, "heading":-0.45886, "vx":-1.67565, "vy":3.04296, "omega":-0.71263, "ax":5.39179, "ay":-8.19331, "alpha":-22.79072, "fx":[77.49306,176.14536,161.73383,-55.85825], "fy":[-181.75667,-88.26156,-87.7235,-188.57261]}, + {"t":1.04357, "x":1.77747, "y":6.93197, "heading":-0.4864, "vx":-1.46725, "vy":2.72628, "omega":-1.59351, "ax":5.37107, "ay":-9.49124, "alpha":-16.08276, "fx":[80.20573,163.86412,133.69464,-19.63209], "fy":[-181.47639,-111.4136,-143.01155,-196.9558]}, + {"t":1.08223, "x":1.72477, "y":7.03025, "heading":-0.54799, "vx":-1.25965, "vy":2.35943, "omega":-2.21513, "ax":5.55987, "ay":-10.53544, "alpha":0.12451, "fx":[92.86545,91.94652,92.49746,93.41182], "fy":[-175.5202,-176.00616,-175.72197,-175.23481]}, + {"t":1.12088, "x":1.68023, "y":7.11357, "heading":-0.63361, "vx":-1.04476, "vy":1.95223, "omega":-2.21031, "ax":5.50575, "ay":-10.41939, "alpha":6.34296, "fx":[108.1843,52.2276,82.20715,124.49391], "fy":[-166.66591,-191.82744,-181.15169,-155.09988]}, + {"t":1.15953, "x":1.64397, "y":7.18125, "heading":-0.71904, "vx":-0.83196, "vy":1.54951, "omega":-1.96515, "ax":5.45047, "ay":-10.24272, "alpha":9.7209, "fx":[124.14623,30.05073,74.37517,134.85473], "fy":[-155.27904,-196.65146,-184.65178,-146.38259]}, + {"t":1.19818, "x":1.61588, "y":7.23349, "heading":-0.795, "vx":-0.62129, "vy":1.15362, "omega":-1.58943, "ax":5.40501, "ay":-10.07747, "alpha":12.08032, "fx":[137.8136,16.18972,67.76085,138.63147], "fy":[-143.41325,-198.34926,-187.25638,-142.92733]}, + {"t":1.23683, "x":1.5959, "y":7.27055, "heading":-0.85643, "vx":-0.41238, "vy":0.76412, "omega":-1.12251, "ax":5.35774, "ay":-9.93922, "alpha":13.85719, "fx":[148.06592,6.86279,62.35789,139.957], "fy":[-132.91268,-198.94101,-189.17097,-141.70315]}, + {"t":1.27548, "x":1.58397, "y":7.29266, "heading":-0.89982, "vx":-0.2053, "vy":0.37996, "omega":-0.58692, "ax":5.31169, "ay":-9.83046, "alpha":15.18518, "fx":[155.14497,0.22017,58.31677,140.49104], "fy":[-124.66813,-199.09708,-190.48734,-141.22339]}, + {"t":1.31413, "x":1.58, "y":7.3, "heading":-0.9225, "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/GToBarge.traj b/src/main/deploy/choreo/GToBarge.traj index 219d771d..1256a3e6 100644 --- a/src/main/deploy/choreo/GToBarge.traj +++ b/src/main/deploy/choreo/GToBarge.traj @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.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}, diff --git a/src/main/deploy/choreo/GTofetch.traj b/src/main/deploy/choreo/GTofetch.traj index cf3d10d8..14545995 100644 --- a/src/main/deploy/choreo/GTofetch.traj +++ b/src/main/deploy/choreo/GTofetch.traj @@ -3,11 +3,11 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.068316459655762, "y":4.486484527587891, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":5.048557281494141, "y":5.589489936828613, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9426766239853251, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.713146, "y":3.7209, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.068316459655762, "y":4.486484527587891, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.048557281494141, "y":5.589489936828613, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9225007574586108, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "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}, @@ -16,11 +16,11 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.068316459655762 m", "val":6.068316459655762}, "y":{"exp":"4.486484527587891 m", "val":4.486484527587891}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.068316459655762 m", "val":6.068316459655762}, "y":{"exp":"4.486484527587891 m", "val":4.486484527587891}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "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}, @@ -32,94 +32,89 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.49858,0.91145,1.50988,2.03588], + "waypoints":[0.0,0.48762,0.94415,1.62519,2.12926], "samples":[ - {"t":0.0, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.90148, "ay":6.34553, "alpha":29.13269, "fx":[175.2204,122.47296,-1.79369,-98.48599], "fy":[2.39999,125.43442,175.23334,128.67472]}, - {"t":0.02624, "x":5.77791, "y":3.62476, "heading":3.14159, "vx":0.07614, "vy":0.16651, "omega":0.76448, "ax":3.6224, "ay":6.83422, "alpha":24.89369, "fx":[175.14462,121.76138,-1.42128,-49.02054], "fy":[4.76968,126.10018,175.21294,158.90993]}, - {"t":0.05248, "x":5.78116, "y":3.63148, "heading":-3.12153, "vx":0.1712, "vy":0.34585, "omega":1.41772, "ax":4.72833, "ay":7.02475, "alpha":20.2, "fx":[174.90285,119.61545,-2.29071,29.48226], "fy":[9.81293,128.10641,175.17116,164.86536]}, - {"t":0.07872, "x":5.78728, "y":3.64298, "heading":-3.08433, "vx":0.29527, "vy":0.53019, "omega":1.94779, "ax":5.67349, "ay":6.7633, "alpha":17.79488, "fx":[174.29563,116.14088,-4.17601,99.75747], "fy":[17.17589,131.22572,175.09244,136.6733]}, - {"t":0.10497, "x":5.79698, "y":3.65922, "heading":-3.03322, "vx":0.44415, "vy":0.70767, "omega":2.41475, "ax":6.02917, "ay":6.53641, "alpha":17.50219, "fx":[173.04912,111.41935,-6.66926,132.41855], "fy":[26.66662,135.20794,174.95568,107.89992]}, - {"t":0.13121, "x":5.81071, "y":3.68004, "heading":-2.96985, "vx":0.60236, "vy":0.87919, "omega":2.87403, "ax":6.016, "ay":6.60283, "alpha":17.42758, "fx":[170.73471,105.37216,-9.25714,142.47184], "fy":[38.52412,139.90616,174.7551,96.06373]}, - {"t":0.15745, "x":5.82859, "y":3.70539, "heading":-2.89443, "vx":0.76023, "vy":1.05246, "omega":3.33135, "ax":5.79087, "ay":6.94841, "alpha":16.82636, "fx":[166.53477,97.71701,-11.14493,140.89747], "fy":[53.55344,145.26383,174.52515,99.41911]}, - {"t":0.18369, "x":5.85053, "y":3.7354, "heading":-2.80701, "vx":0.91219, "vy":1.23479, "omega":3.7729, "ax":5.36265, "ay":7.55437, "alpha":15.3285, "fx":[158.59154,87.913,-10.74383,129.10762], "fy":[73.47752,151.25791,174.36821,114.88682]}, - {"t":0.20993, "x":5.87631, "y":3.7704, "heading":-2.70801, "vx":1.05291, "vy":1.43303, "omega":4.17514, "ax":4.63406, "ay":8.42184, "alpha":12.36133, "fx":[141.51532,74.71533,-4.66142,103.72691], "fy":[102.13391,157.94979,174.34421,138.58448]}, - {"t":0.23617, "x":5.90554, "y":3.8109, "heading":-2.59845, "vx":1.17452, "vy":1.65403, "omega":4.49952, "ax":3.28241, "ay":9.51976, "alpha":6.1859, "fx":[93.88326,53.77583,14.39266,61.27962], "fy":[146.47907,165.71058,173.32301,162.20063]}, - {"t":0.26241, "x":5.93749, "y":3.85759, "heading":-2.48037, "vx":1.26065, "vy":1.90384, "omega":4.66184, "ax":0.05504, "ay":9.919, "alpha":-8.02328, "fx":[-50.7398,-8.54181,56.76796,6.25858], "fy":[165.76305,171.85629,163.63635,173.62149]}, - {"t":0.28865, "x":5.97059, "y":3.91096, "heading":-2.35804, "vx":1.2621, "vy":2.16413, "omega":4.4513, "ax":-3.95203, "ay":8.79082, "alpha":-11.73115, "fx":[-117.44511,-110.89505,8.32962,-48.88125], "fy":[127.75966,130.95456,172.6474,166.75596]}, - {"t":0.3149, "x":6.00235, "y":3.97078, "heading":-2.24123, "vx":1.15839, "vy":2.39481, "omega":4.14346, "ax":-6.78747, "ay":7.24735, "alpha":-8.33007, "fx":[-142.19842,-144.60468,-76.11593,-98.89252], "fy":[99.92572,94.70426,155.46758,143.00369]}, - {"t":0.34114, "x":6.03041, "y":4.03611, "heading":-2.1325, "vx":0.98028, "vy":2.58499, "omega":3.92487, "ax":-8.50115, "ay":5.45963, "alpha":-5.49163, "fx":[-157.38283,-159.8685,-127.74994,-133.40732], "fy":[74.38874,67.68242,117.56337,111.83252]}, - {"t":0.36738, "x":6.05321, "y":4.10583, "heading":-2.02951, "vx":0.7572, "vy":2.72826, "omega":3.78076, "ax":-9.39189, "ay":3.93361, "alpha":-3.84758, "fx":[-166.10661,-167.58917,-152.15453,-153.16304], "fy":[52.89424,47.02928,84.4922,83.22272]}, - {"t":0.39362, "x":6.06984, "y":4.17877, "heading":-1.9303, "vx":0.51074, "vy":2.83148, "omega":3.6798, "ax":-9.84925, "ay":2.71378, "alpha":-3.13575, "fx":[-171.13084,-171.90995,-163.48359,-163.60728], "fy":[34.26923,29.09748,60.54354,60.73243]}, - {"t":0.41986, "x":6.07985, "y":4.25401, "heading":-1.83374, "vx":0.25229, "vy":2.90269, "omega":3.59751, "ax":-10.08255, "ay":1.72572, "alpha":-3.04038, "fx":[-173.80937,-174.10693,-169.00981,-169.07877], "fy":[17.52346,12.44798,43.56452,43.88034]}, - {"t":0.4461, "x":6.083, "y":4.33077, "heading":-1.73933, "vx":-0.01229, "vy":2.94798, "omega":3.51773, "ax":-10.1894, "ay":0.90073, "alpha":-3.35535, "fx":[-174.80666,-174.65484,-171.80566,-172.00767], "fy":[2.01997,-3.4401,31.52841,31.17624]}, - {"t":0.47234, "x":6.07917, "y":4.40844, "heading":-1.64702, "vx":-0.27967, "vy":2.97161, "omega":3.42968, "ax":-10.21438, "ay":0.1814, "alpha":-4.01779, "fx":[-174.43657,-173.71636,-173.21543,-173.60633], "fy":[-12.99351,-19.31348,23.3008,21.34877]}, - {"t":0.49858, "x":6.06832, "y":4.48648, "heading":-1.55702, "vx":-0.54771, "vy":2.97637, "omega":3.32425, "ax":-10.17665, "ay":-0.22696, "alpha":-4.65388, "fx":[-173.15627,-172.05362,-173.21542,-173.98222], "fy":[-23.14473,-28.97058,20.90924,15.76395]}, - {"t":0.51653, "x":6.05685, "y":4.53988, "heading":-1.49735, "vx":-0.73039, "vy":2.9723, "omega":3.24071, "ax":-10.12344, "ay":-0.31308, "alpha":-5.80847, "fx":[-172.10052,-170.63505,-172.20868,-173.84313], "fy":[-29.74124,-35.80757,27.38004,16.86712]}, - {"t":0.53449, "x":6.0421, "y":4.59318, "heading":-1.43918, "vx":-0.91211, "vy":2.96668, "omega":3.13644, "ax":-10.05928, "ay":-0.39737, "alpha":-6.96177, "fx":[-170.77926,-169.01052,-170.86848,-173.76353], "fy":[-36.32038,-42.21947,34.23576,17.26752]}, - {"t":0.55244, "x":6.02411, "y":4.64637, "heading":-1.38288, "vx":-1.09268, "vy":2.95955, "omega":3.01147, "ax":-9.98675, "ay":-0.47989, "alpha":-8.07087, "fx":[-169.22797,-167.27532,-169.24203,-173.74173], "fy":[-42.71851,-48.00904,41.06133,17.01504]}, - {"t":0.57039, "x":6.00289, "y":4.69942, "heading":-1.32882, "vx":-1.27195, "vy":2.95093, "omega":2.86659, "ax":-9.90773, "ay":-0.56499, "alpha":-9.11617, "fx":[-167.46175,-165.46694,-167.41629,-173.7659], "fy":[-48.92718,-53.23191,47.53011,16.18778]}, - {"t":0.58834, "x":5.97846, "y":4.7523, "heading":-1.27736, "vx":-1.4498, "vy":2.94079, "omega":2.70295, "ax":-9.82363, "ay":-0.65748, "alpha":-10.08691, "fx":[-165.48525,-163.58145,-165.50245,-173.81947], "fy":[-54.96924,-58.02147,53.3972,14.85956]}, - {"t":0.60629, "x":5.95085, "y":4.80498, "heading":-1.22884, "vx":-1.62615, "vy":2.92899, "omega":2.52188, "ax":-9.7356, "ay":-0.76137, "alpha":-10.97557, "fx":[-163.30274,-161.58959,-163.62304,-173.88358], "fy":[-60.85416,-62.51826,58.4784,13.09141]}, - {"t":0.62424, "x":5.92009, "y":4.85744, "heading":-1.18357, "vx":-1.80091, "vy":2.91532, "omega":2.32487, "ax":-9.64471, "ay":-0.87976, "alpha":-11.77533, "fx":[-160.92535,-159.44774,-161.90403,-173.93813], "fy":[-66.56593,-66.8445,62.62375,10.92889]}, - {"t":0.64219, "x":5.88621, "y":4.90963, "heading":-1.14184, "vx":-1.97404, "vy":2.89953, "omega":2.11349, "ax":-9.55209, "ay":-1.01525, "alpha":-12.47822, "fx":[-158.37491,-157.10423,-160.47227,-173.96169], "fy":[-72.06456,-71.09753,65.6877,8.3977]}, - {"t":0.66014, "x":5.84923, "y":4.96151, "heading":-1.1039, "vx":-2.1455, "vy":2.8813, "omega":1.8895, "ax":-9.45888, "ay":-1.17085, "alpha":-13.07316, "fx":[-155.68283,-154.5007,-159.45739,-173.93016], "fy":[-77.29601,-75.35467,67.49349,5.49369]}, - {"t":0.67809, "x":5.8092, "y":5.01305, "heading":-1.06998, "vx":-2.3153, "vy":2.86029, "omega":1.65482, "ax":-9.36609, "ay":-1.35149, "alpha":-13.54352, "fx":[-152.88181,-151.56603,-158.99732,-173.81292], "fy":[-82.20929,-79.68953,67.78237,2.16237]}, - {"t":0.69604, "x":5.76613, "y":5.06417, "heading":-1.04028, "vx":-2.48342, "vy":2.83603, "omega":1.41171, "ax":-9.274, "ay":-1.5668, "alpha":-13.86269, "fx":[-149.98601,-148.19706,-159.24617,-173.56335], "fy":[-86.78258,-84.2043,66.12552,-1.74187]}, - {"t":0.71399, "x":5.72005, "y":5.11483, "heading":-1.01494, "vx":-2.6499, "vy":2.8079, "omega":1.16286, "ax":-9.18057, "ay":-1.83635, "alpha":-13.98504, "fx":[-146.95043,-144.21272,-160.37711,-173.09511], "fy":[-91.06588,-89.08573,61.74737,-6.5386]}, - {"t":0.73194, "x":5.67101, "y":5.16494, "heading":-0.99406, "vx":-2.8147, "vy":2.77494, "omega":0.91182, "ax":-9.0771, "ay":-2.20067, "alpha":-13.82404, "fx":[-143.58456,-139.25442,-162.53962,-172.21669], "fy":[-95.26261,-94.69582,53.1311,-12.90395]}, - {"t":0.74989, "x":5.61902, "y":5.21439, "heading":-0.97769, "vx":-2.97764, "vy":2.73543, "omega":0.66367, "ax":-8.9347, "ay":-2.74638, "alpha":-13.19387, "fx":[-139.34512,-132.57635,-165.56184,-170.42374], "fy":[-99.90959,-101.69971,37.06149,-22.31251]}, - {"t":0.76784, "x":5.56413, "y":5.26305, "heading":-0.96578, "vx":-3.13802, "vy":2.68614, "omega":0.42683, "ax":-8.65491, "ay":-3.66225, "alpha":-11.6418, "fx":[-132.79575,-122.6986,-167.26863,-166.10738], "fy":[-106.25534,-111.08675,6.34879,-38.18189]}, - {"t":0.7858, "x":5.5064, "y":5.31068, "heading":-0.95812, "vx":-3.29338, "vy":2.6204, "omega":0.21785, "ax":-7.8976, "ay":-5.26956, "alpha":-8.11386, "fx":[-120.59784,-108.10871,-155.52453,-153.11228], "fy":[-116.31354,-122.76716,-51.28457,-68.16989]}, - {"t":0.80375, "x":5.44601, "y":5.35687, "heading":-0.95421, "vx":-3.43515, "vy":2.5258, "omega":0.0722, "ax":-6.23933, "ay":-7.12972, "alpha":-2.62363, "fx":[-100.96967,-94.34327,-112.53117,-116.6726], "fy":[-127.58708,-130.36911,-114.26264,-112.87926]}, - {"t":0.8217, "x":5.38334, "y":5.40106, "heading":-0.95291, "vx":-3.54715, "vy":2.39782, "omega":0.02511, "ax":-5.08711, "ay":-7.6067, "alpha":-0.35081, "fx":[-85.93587,-84.82449,-87.14468,-88.21602], "fy":[-130.07194,-130.39771,-128.69826,-128.3835]}, - {"t":0.83965, "x":5.31885, "y":5.44288, "heading":-0.95246, "vx":-3.63847, "vy":2.26127, "omega":0.01881, "ax":-4.40662, "ay":-7.34006, "alpha":-0.06109, "fx":[-74.87578,-74.65916,-75.03538,-75.25101], "fy":[-124.97135,-125.0022,-124.73328,-124.70266]}, - {"t":0.8576, "x":5.25283, "y":5.48229, "heading":-0.95212, "vx":-3.71757, "vy":2.12951, "omega":0.01771, "ax":-3.61336, "ay":-6.52214, "alpha":-0.02948, "fx":[-61.4394,-61.32681,-61.48506,-61.59749], "fy":[-111.00473,-110.99635,-110.87486,-110.88329]}, - {"t":0.87555, "x":5.18551, "y":5.51946, "heading":-0.95181, "vx":-3.78243, "vy":2.01244, "omega":0.01718, "ax":-2.36696, "ay":-4.55416, "alpha":-0.01692, "fx":[-40.26064,-40.19363,-40.26187,-40.32886], "fy":[-77.51235,-77.48494,-77.41765,-77.44509]}, - {"t":0.8935, "x":5.11723, "y":5.55485, "heading":-0.9515, "vx":-3.82492, "vy":1.93069, "omega":0.01688, "ax":-0.10749, "ay":-0.1353, "alpha":0.0059, "fx":[-1.82507,-1.84847,-1.83177,-1.80837], "fy":[-2.28133,-2.29802,-2.32142,-2.30474]}, - {"t":0.91145, "x":5.04856, "y":5.58949, "heading":-0.95119, "vx":-3.82685, "vy":1.92826, "omega":0.01699, "ax":0.94084, "ay":1.98327, "alpha":-0.13497, "fx":[15.94072,16.47773,16.06638,15.52918], "fy":[33.29193,33.64444,34.1772,33.82563]}, - {"t":0.93282, "x":4.96698, "y":5.63115, "heading":-0.95083, "vx":-3.80674, "vy":1.97065, "omega":0.0141, "ax":0.46541, "ay":0.89416, "alpha":0.00014, "fx":[7.91652,7.91597,7.91637,7.91692], "fy":[15.20983,15.20944,15.2089,15.20929]}, - {"t":0.9542, "x":4.88573, "y":5.67348, "heading":-0.95053, "vx":-3.7968, "vy":1.98976, "omega":0.0141, "ax":0.20883, "ay":0.39725, "alpha":0.00054, "fx":[3.5524,3.55026,3.55179,3.55394], "fy":[6.75902,6.7575,6.75535,6.75687]}, - {"t":0.97557, "x":4.80463, "y":5.71609, "heading":-0.95023, "vx":-3.79233, "vy":1.99825, "omega":0.01412, "ax":0.09265, "ay":0.17554, "alpha":0.00028, "fx":[1.57608,1.57497,1.57577,1.57687], "fy":[2.98684,2.98605,2.98494,2.98574]}, - {"t":0.99694, "x":4.7236, "y":5.75884, "heading":-0.94993, "vx":-3.79035, "vy":2.002, "omega":0.01412, "ax":0.04098, "ay":0.0775, "alpha":0.00015, "fx":[0.69716,0.69655,0.69698,0.69759], "fy":[1.31885,1.31842,1.31781,1.31824]}, - {"t":1.01831, "x":4.6426, "y":5.80165, "heading":-0.94963, "vx":-3.78948, "vy":2.00365, "omega":0.01413, "ax":0.01811, "ay":0.03422, "alpha":0.0001, "fx":[0.3081,0.30773,0.308,0.30838], "fy":[0.58238,0.58211,0.58173,0.58201]}, - {"t":1.03969, "x":4.56161, "y":5.84448, "heading":-0.94932, "vx":-3.78909, "vy":2.00439, "omega":0.01413, "ax":0.008, "ay":0.01511, "alpha":0.00007, "fx":[0.13616,0.13588,0.13608,0.13636], "fy":[0.25725,0.25705,0.25678,0.25697]}, - {"t":1.06106, "x":4.48063, "y":5.88732, "heading":-0.94902, "vx":-3.78892, "vy":2.00471, "omega":0.01413, "ax":0.00354, "ay":0.00667, "alpha":0.00006, "fx":[0.06021,0.05998,0.06014,0.06037], "fy":[0.11371,0.11354,0.11331,0.11348]}, - {"t":1.08243, "x":4.39965, "y":5.93017, "heading":-0.94872, "vx":-3.78884, "vy":2.00485, "omega":0.01413, "ax":0.00157, "ay":0.00295, "alpha":0.00005, "fx":[0.02667,0.02645,0.02661,0.02682], "fy":[0.05033,0.05018,0.04996,0.05012]}, - {"t":1.1038, "x":4.31868, "y":5.97302, "heading":-0.94842, "vx":-3.78881, "vy":2.00491, "omega":0.01413, "ax":0.0007, "ay":0.0013, "alpha":0.00005, "fx":[0.01185,0.01165,0.0118,0.012], "fy":[0.02234,0.0222,0.02199,0.02214]}, - {"t":1.12518, "x":4.2377, "y":6.01587, "heading":-0.94812, "vx":-3.78879, "vy":2.00494, "omega":0.01413, "ax":0.00031, "ay":0.00058, "alpha":0.00005, "fx":[0.00531,0.00511,0.00526,0.00546], "fy":[0.00999,0.00984,0.00964,0.00979]}, - {"t":1.14655, "x":4.15673, "y":6.05872, "heading":-0.94781, "vx":-3.78879, "vy":2.00495, "omega":0.01413, "ax":0.00014, "ay":0.00026, "alpha":0.00005, "fx":[0.00242,0.00222,0.00237,0.00256], "fy":[0.00452,0.00438,0.00418,0.00433]}, - {"t":1.16792, "x":4.07575, "y":6.10157, "heading":-0.94751, "vx":-3.78878, "vy":2.00496, "omega":0.01413, "ax":0.00007, "ay":0.00011, "alpha":0.00005, "fx":[0.00114,0.00094,0.00108,0.00128], "fy":[0.0021,0.00196,0.00176,0.0019]}, - {"t":1.18929, "x":3.99477, "y":6.14442, "heading":-0.94721, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.00003, "ay":0.00005, "alpha":0.00005, "fx":[0.00056,0.00036,0.0005,0.0007], "fy":[0.001,0.00086,0.00066,0.0008]}, - {"t":1.21067, "x":3.9138, "y":6.18727, "heading":-0.94691, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.00001, "ay":0.00002, "alpha":0.00005, "fx":[0.00026,0.00007,0.00021,0.0004], "fy":[0.00044,0.0003,0.00011,0.00025]}, - {"t":1.23204, "x":3.83282, "y":6.23012, "heading":-0.9466, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":0.0, "ay":-0.00001, "alpha":0.00005, "fx":[0.00005,-0.00014,0.0,0.0002], "fy":[0.00005,-0.00009,-0.00029,-0.00015]}, - {"t":1.25341, "x":3.75185, "y":6.27297, "heading":-0.9463, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00001, "ay":-0.00004, "alpha":0.00005, "fx":[-0.00022,-0.00042,-0.00028,-0.00008], "fy":[-0.00047,-0.00061,-0.00081,-0.00067]}, - {"t":1.27478, "x":3.67087, "y":6.31582, "heading":-0.946, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00005, "ay":-0.0001, "alpha":0.00005, "fx":[-0.00076,-0.00095,-0.00081,-0.00062], "fy":[-0.00148,-0.00163,-0.00182,-0.00168]}, - {"t":1.29616, "x":3.5899, "y":6.35867, "heading":-0.9457, "vx":-3.78878, "vy":2.00496, "omega":0.01414, "ax":-0.00012, "ay":-0.00023, "alpha":0.00005, "fx":[-0.00193,-0.00213,-0.00199,-0.00179], "fy":[-0.0037,-0.00384,-0.00404,-0.0039]}, - {"t":1.31753, "x":3.50892, "y":6.40153, "heading":-0.9454, "vx":-3.78879, "vy":2.00496, "omega":0.01414, "ax":-0.00027, "ay":-0.00052, "alpha":0.00005, "fx":[-0.00457,-0.00477,-0.00462,-0.00443], "fy":[-0.00869,-0.00883,-0.00902,-0.00888]}, - {"t":1.3389, "x":3.42794, "y":6.44438, "heading":-0.94509, "vx":-3.78879, "vy":2.00494, "omega":0.01414, "ax":-0.00062, "ay":-0.00118, "alpha":0.00005, "fx":[-0.01054,-0.01073,-0.01059,-0.0104], "fy":[-0.01996,-0.0201,-0.02029,-0.02016]}, - {"t":1.36027, "x":3.34697, "y":6.48723, "heading":-0.94479, "vx":-3.7888, "vy":2.00492, "omega":0.01414, "ax":-0.00142, "ay":-0.00268, "alpha":0.00005, "fx":[-0.02406,-0.02424,-0.02411,-0.02392], "fy":[-0.0455,-0.04564,-0.04582,-0.04569]}, - {"t":1.38165, "x":3.26599, "y":6.53008, "heading":-0.94449, "vx":-3.78884, "vy":2.00486, "omega":0.01415, "ax":-0.00322, "ay":-0.00608, "alpha":0.00004, "fx":[-0.05467,-0.05483,-0.05471,-0.05455], "fy":[-0.10334,-0.10346,-0.10363,-0.10351]}, - {"t":1.40302, "x":3.18501, "y":6.57292, "heading":-0.94419, "vx":-3.7889, "vy":2.00473, "omega":0.01415, "ax":-0.00729, "ay":-0.01378, "alpha":0.00003, "fx":[-0.12398,-0.12411,-0.12402,-0.12389], "fy":[-0.23434,-0.23443,-0.23455,-0.23447]}, - {"t":1.42439, "x":3.10403, "y":6.61577, "heading":-0.94388, "vx":-3.78906, "vy":2.00444, "omega":0.01415, "ax":-0.01652, "ay":-0.03122, "alpha":0.00001, "fx":[-0.28092,-0.28094,-0.28092,-0.28089], "fy":[-0.53106,-0.53108,-0.53111,-0.53108]}, - {"t":1.44576, "x":3.02305, "y":6.6586, "heading":-0.94358, "vx":-3.78941, "vy":2.00377, "omega":0.01415, "ax":-0.0374, "ay":-0.07072, "alpha":-0.00006, "fx":[-0.63614,-0.63589,-0.63607,-0.63632], "fy":[-1.20315,-1.20297,-1.20272,-1.2029]}, - {"t":1.46714, "x":2.94205, "y":6.70141, "heading":-0.94328, "vx":-3.79021, "vy":2.00226, "omega":0.01415, "ax":-0.08564, "ay":-0.15963, "alpha":-0.00317, "fx":[-1.45838,-1.44588,-1.45495,-1.46745], "fy":[-2.72598,-2.71692,-2.70442,-2.71348]}, - {"t":1.48851, "x":2.86102, "y":6.74417, "heading":-0.94298, "vx":-3.79204, "vy":1.99885, "omega":0.01408, "ax":-0.36107, "ay":-0.27242, "alpha":-0.65828, "fx":[-6.49392,-3.90252,-5.79103,-8.37948], "fy":[-6.87222,-4.99489,-2.39315,-4.2747]}, - {"t":1.50988, "x":2.7799, "y":6.78682, "heading":-0.94268, "vx":-3.79976, "vy":1.99302, "omega":0.00001, "ax":-0.19978, "ay":-0.30768, "alpha":-0.00026, "fx":[-3.39833,-3.39731,-3.39805,-3.39907], "fy":[-5.23448,-5.23374,-5.23272,-5.23346]}, - {"t":1.54495, "x":2.64653, "y":6.85652, "heading":-0.94268, "vx":-3.80676, "vy":1.98224, "omega":0.0, "ax":-0.03401, "ay":-0.06535, "alpha":0.0, "fx":[-0.57851,-0.5785,-0.57851,-0.57851], "fy":[-1.1116,-1.1116,-1.1116,-1.1116]}, - {"t":1.58001, "x":2.51302, "y":6.92599, "heading":-0.94268, "vx":-3.80796, "vy":1.97994, "omega":0.0, "ax":0.01012, "ay":-0.02307, "alpha":0.0, "fx":[0.17206,0.17206,0.17206,0.17206], "fy":[-0.39235,-0.39235,-0.39235,-0.39235]}, - {"t":1.61508, "x":2.37949, "y":6.99541, "heading":-0.94268, "vx":-3.8076, "vy":1.97913, "omega":0.0, "ax":8.23713, "ay":-4.28328, "alpha":-0.00004, "fx":[140.11107,140.11121,140.11125,140.11111], "fy":[-72.85752,-72.85727,-72.85713,-72.85737]}, - {"t":1.65015, "x":2.25103, "y":7.06218, "heading":-0.94268, "vx":-3.51875, "vy":1.82893, "omega":0.0, "ax":9.04507, "ay":-4.70151, "alpha":0.0, "fx":[153.85405,153.85405,153.85405,153.85405], "fy":[-79.97128,-79.97127,-79.97127,-79.97127]}, - {"t":1.68521, "x":2.1332, "y":7.12342, "heading":-0.94268, "vx":-3.20157, "vy":1.66407, "omega":0.0, "ax":9.09619, "ay":-4.72796, "alpha":0.0, "fx":[154.72364,154.72363,154.72363,154.72364], "fy":[-80.42131,-80.42133,-80.42134,-80.42132]}, - {"t":1.72028, "x":2.02653, "y":7.17887, "heading":-0.94268, "vx":-2.88259, "vy":1.49827, "omega":0.0, "ax":9.11463, "ay":-4.73751, "alpha":0.0, "fx":[155.03731,155.0373,155.03729,155.0373], "fy":[-80.58364,-80.58367,-80.58368,-80.58365]}, - {"t":1.75535, "x":1.93105, "y":7.2285, "heading":-0.94268, "vx":-2.56297, "vy":1.33214, "omega":0.0, "ax":9.12413, "ay":-4.74242, "alpha":0.0, "fx":[155.19887,155.19886,155.19885,155.19887], "fy":[-80.66726,-80.66728,-80.66729,-80.66727]}, - {"t":1.79042, "x":1.84678, "y":7.2723, "heading":-0.94268, "vx":-2.24302, "vy":1.16584, "omega":0.0, "ax":9.12992, "ay":-4.74542, "alpha":0.0, "fx":[155.29734,155.29733,155.29732,155.29733], "fy":[-80.71821,-80.71824,-80.71826,-80.71823]}, - {"t":1.82548, "x":1.77374, "y":7.31026, "heading":-0.94268, "vx":-1.92286, "vy":0.99943, "omega":0.0, "ax":9.13382, "ay":-4.74744, "alpha":0.0, "fx":[155.36362,155.36361,155.3636,155.36362], "fy":[-80.75252,-80.75255,-80.75256,-80.75253]}, - {"t":1.86055, "x":1.71193, "y":7.34239, "heading":-0.94268, "vx":-1.60256, "vy":0.83295, "omega":0.0, "ax":9.13662, "ay":-4.74889, "alpha":0.0, "fx":[155.41128,155.41126,155.41126,155.41127], "fy":[-80.77718,-80.77721,-80.77723,-80.7772]}, - {"t":1.89562, "x":1.66135, "y":7.36868, "heading":-0.94268, "vx":-1.28217, "vy":0.66643, "omega":0.0, "ax":9.13873, "ay":-4.74998, "alpha":0.0, "fx":[155.44719,155.44717,155.44717,155.44718], "fy":[-80.79576,-80.7958,-80.79581,-80.79578]}, - {"t":1.93068, "x":1.622, "y":7.38913, "heading":-0.94268, "vx":-0.9617, "vy":0.49986, "omega":0.0, "ax":9.14038, "ay":-4.75083, "alpha":0.0, "fx":[155.47522,155.4752,155.4752,155.47521], "fy":[-80.81027,-80.8103,-80.81032,-80.81029]}, - {"t":1.96575, "x":1.5939, "y":7.40373, "heading":-0.94268, "vx":-0.64118, "vy":0.33326, "omega":0.0, "ax":9.1417, "ay":-4.75152, "alpha":0.0, "fx":[155.49771,155.49769,155.49768,155.4977], "fy":[-80.82191,-80.82194,-80.82196,-80.82192]}, - {"t":2.00082, "x":1.57704, "y":7.4125, "heading":-0.94268, "vx":-0.32061, "vy":0.16664, "omega":0.0, "ax":9.14278, "ay":-4.75208, "alpha":0.0, "fx":[155.51615,155.51613,155.51612,155.51614], "fy":[-80.83145,-80.83148,-80.8315,-80.83147]}, - {"t":2.03588, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.71315, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.87481, "ay":4.52134, "alpha":47.80775, "fx":[199.06009,135.69912,-17.24657,-192.50424], "fy":[-3.76206,145.81172,198.36807,-38.94351]}, + {"t":0.02709, "x":5.71383, "y":3.72256, "heading":3.14159, "vx":0.05079, "vy":0.12248, "omega":1.2951, "ax":1.89954, "ay":5.17512, "alpha":45.50636, "fx":[199.04786,135.81052,-13.3846,-194.81589], "fy":[0.05386,145.66319,198.62064,0.72931]}, + {"t":0.05418, "x":5.71591, "y":3.72778, "heading":-3.10651, "vx":0.10225, "vy":0.26268, "omega":2.52786, "ax":2.59085, "ay":6.95399, "alpha":36.58248, "fx":[198.7419,133.36554,-10.58214,-148.77242], "fy":[9.86664,147.84157,198.71781,107.25284]}, + {"t":0.08127, "x":5.71963, "y":3.73744, "heading":-3.03803, "vx":0.17243, "vy":0.45106, "omega":3.51888, "ax":7.41353, "ay":6.81703, "alpha":21.44169, "fx":[197.35101,128.80461,-7.21228,175.37656], "fy":[24.79746,151.73817,198.75152,79.25912]}, + {"t":0.10836, "x":5.72702, "y":3.75216, "heading":-2.9427, "vx":0.37326, "vy":0.63573, "omega":4.09973, "ax":7.54381, "ay":7.04127, "alpha":20.5181, "fx":[193.57105,123.36444,2.75211,183.31936], "fy":[45.16021,156.03833,198.65595,69.64374]}, + {"t":0.13545, "x":5.7399, "y":3.77197, "heading":-2.83164, "vx":0.57762, "vy":0.82648, "omega":4.65556, "ax":7.69808, "ay":7.80898, "alpha":16.03514, "fx":[184.33855,120.39847,35.30655,173.24953], "fy":[73.64583,158.02056,195.10349,93.91814]}, + {"t":0.16254, "x":5.75837, "y":3.79722, "heading":-2.70552, "vx":0.78616, "vy":1.03802, "omega":5.08995, "ax":8.33681, "ay":8.37434, "alpha":3.45658, "fx":[153.13645,133.11266,122.693,146.9403], "fy":[125.15733,146.37868,154.90574,131.94317]}, + {"t":0.18963, "x":5.78273, "y":3.82842, "heading":-2.56764, "vx":1.01201, "vy":1.26488, "omega":5.18359, "ax":7.74355, "ay":7.59173, "alpha":-16.90046, "fx":[45.62291,184.46597,176.6839,109.55241], "fy":[190.73306,63.25545,87.89108,164.32231]}, + {"t":0.21672, "x":5.81298, "y":3.86547, "heading":-2.42722, "vx":1.22178, "vy":1.47054, "omega":4.72576, "ax":5.8403, "ay":8.93467, "alpha":-17.76608, "fx":[-6.37844,164.95639,157.32065,73.52134], "fy":[195.11023,100.00344,117.95035,182.6826]}, + {"t":0.24381, "x":5.84822, "y":3.90858, "heading":-2.2992, "vx":1.37999, "vy":1.71258, "omega":4.24448, "ax":1.59343, "ay":10.17101, "alpha":-18.89311, "fx":[-104.13862,82.4896,120.57594,7.31981], "fy":[164.51466,163.82625,153.71019,196.13238]}, + {"t":0.2709, "x":5.88619, "y":3.95871, "heading":-2.18421, "vx":1.42316, "vy":1.98811, "omega":3.73267, "ax":1.50526, "ay":9.10582, "alpha":-24.69419, "fx":[-126.75458,120.11283,116.49874,-9.48931], "fy":[143.22332,116.27783,153.68229,193.97538]}, + {"t":0.29799, "x":5.9253, "y":4.01591, "heading":-2.0831, "vx":1.46393, "vy":2.23478, "omega":3.0637, "ax":0.17244, "ay":9.02743, "alpha":-21.83478, "fx":[-124.89815,71.76599,90.81638,-26.18634], "fy":[130.953,124.72393,160.62013,185.63446]}, + {"t":0.32508, "x":5.96502, "y":4.07976, "heading":-2.0001, "vx":1.4686, "vy":2.47933, "omega":2.4722, "ax":-9.09843, "ay":3.69371, "alpha":3.91571, "fx":[-142.47752,-148.63286,-159.98465,-155.57089], "fy":[75.62452,76.93398,49.82913,43.9021]}, + {"t":0.35217, "x":6.00146, "y":4.14828, "heading":-1.93313, "vx":1.22213, "vy":2.5794, "omega":2.57828, "ax":-10.14427, "ay":-1.47543, "alpha":14.48461, "fx":[-169.58754,-180.62563,-182.85799,-143.32937], "fy":[16.30248,46.09192,-46.26932,-114.5039]}, + {"t":0.37926, "x":6.03085, "y":4.21761, "heading":-1.86328, "vx":0.94732, "vy":2.53943, "omega":2.97066, "ax":-10.64955, "ay":-1.68025, "alpha":13.29365, "fx":[-183.32955,-188.28387,-186.0067,-152.47116], "fy":[15.50791,36.94249,-51.40521,-113.08135]}, + {"t":0.40635, "x":6.0526, "y":4.28579, "heading":-1.78281, "vx":0.65883, "vy":2.49391, "omega":3.33079, "ax":-11.24635, "ay":-0.94353, "alpha":9.55158, "fx":[-189.99601,-192.05367,-190.79656,-177.03846], "fy":[22.68595,29.88248,-39.1751,-76.30586]}, + {"t":0.43344, "x":6.06633, "y":4.353, "heading":-1.69258, "vx":0.35417, "vy":2.46835, "omega":3.58954, "ax":-11.68619, "ay":-0.1224, "alpha":3.33254, "fx":[-194.6774,-195.19842,-195.26434,-194.07233], "fy":[12.80514,14.49478,-14.48494,-20.9761]}, + {"t":0.46053, "x":6.07163, "y":4.41982, "heading":-1.59534, "vx":0.03759, "vy":2.46503, "omega":3.67982, "ax":-11.8108, "ay":-0.32027, "alpha":-2.72346, "fx":[-196.80111,-196.25968,-197.09144,-197.36954], "fy":[-16.88724,-19.90256,7.90248,7.53241]}, + {"t":0.48762, "x":6.06832, "y":4.48648, "heading":-1.49565, "vx":-0.28236, "vy":2.45636, "omega":3.60604, "ax":-11.72365, "ay":-0.66556, "alpha":-5.03667, "fx":[-194.75778,-193.38745,-196.31638,-197.249], "fy":[-32.63073,-37.3356,16.05827,9.52967]}, + {"t":0.50936, "x":6.05941, "y":4.53973, "heading":-1.41726, "vx":-0.53723, "vy":2.44189, "omega":3.49654, "ax":-11.55908, "ay":-0.2719, "alpha":-8.38077, "fx":[-192.95982,-190.78264,-190.99487,-195.99987], "fy":[-40.75833,-45.656,45.74834,22.53613]}, + {"t":0.5311, "x":6.045, "y":4.59275, "heading":-1.34125, "vx":-0.78852, "vy":2.43598, "omega":3.31435, "ax":-11.33029, "ay":0.14144, "alpha":-11.3489, "fx":[-191.06007,-188.78271,-181.09504,-194.54419], "fy":[-47.51787,-49.06135,74.58102,31.42942]}, + {"t":0.55284, "x":6.02518, "y":4.64574, "heading":-1.26919, "vx":-1.03484, "vy":2.43905, "omega":3.06762, "ax":-11.06754, "ay":0.4795, "alpha":-13.7812, "fx":[-188.81665,-187.0906,-168.75556,-193.29958], "fy":[-54.1993,-49.0598,98.35998,36.8712]}, + {"t":0.57458, "x":6.00006, "y":4.69888, "heading":-1.2025, "vx":-1.27545, "vy":2.44948, "omega":2.76802, "ax":-10.79554, "ay":0.69942, "alpha":-15.68707, "fx":[-185.93223,-184.98213,-156.49965,-192.41161], "fy":[-61.58434,-47.1657,115.93163,39.45408]}, + {"t":0.59632, "x":5.96979, "y":4.75229, "heading":-1.14233, "vx":-1.51014, "vy":2.46468, "omega":2.42699, "ax":-10.52538, "ay":0.80727, "alpha":-17.15473, "fx":[-182.2572,-181.63025,-146.08693,-191.83789], "fy":[-69.58104,-44.01915,127.72051,39.70688]}, + {"t":0.61806, "x":5.93447, "y":4.80607, "heading":-1.08957, "vx":-1.73896, "vy":2.48223, "omega":2.05405, "ax":-10.25639, "ay":0.83528, "alpha":-18.26998, "fx":[-177.87957,-176.07982,-138.52239,-191.39454], "fy":[-77.37828,-39.43815,134.40584,38.10536]}, + {"t":0.6398, "x":5.89424, "y":4.86023, "heading":-1.04491, "vx":-1.96193, "vy":2.50039, "omega":1.65687, "ax":-9.98293, "ay":0.81135, "alpha":-19.06192, "fx":[-173.0884,-167.4096,-134.3818,-190.76294], "fy":[-83.73222,-33.33566,136.19537,34.97181]}, + {"t":0.66153, "x":5.84923, "y":4.91478, "heading":-1.00889, "vx":-2.17895, "vy":2.51803, "omega":1.24247, "ax":-9.70329, "ay":0.69468, "alpha":-19.4098, "fx":[-167.9723,-155.55233,-134.10855,-189.36322], "fy":[-87.34007,-28.48529,132.04366,30.10161]}, + {"t":0.68327, "x":5.79957, "y":4.96968, "heading":-0.98188, "vx":-2.3899, "vy":2.53313, "omega":0.8205, "ax":-9.38299, "ay":0.26126, "alpha":-18.78086, "fx":[-160.92006,-140.74857,-138.42709,-185.54408], "fy":[-87.28317,-32.9363,116.47472,21.16477]}, + {"t":0.70501, "x":5.74539, "y":5.02481, "heading":-0.96404, "vx":-2.59388, "vy":2.53881, "omega":0.41221, "ax":-8.5023, "ay":-1.42673, "alpha":-14.43191, "fx":[-139.93249,-114.50625,-144.01096,-168.46708], "fy":[-84.93529,-57.52626,54.83634,-7.50631]}, + {"t":0.72675, "x":5.68699, "y":5.07967, "heading":-0.95508, "vx":-2.77872, "vy":2.50779, "omega":0.09847, "ax":-4.24135, "ay":-3.70395, "alpha":-2.11135, "fx":[-70.40703,-63.38916,-71.17587,-77.83283], "fy":[-68.73628,-64.7013,-54.5113,-59.02366]}, + {"t":0.74849, "x":5.62558, "y":5.13331, "heading":-0.95294, "vx":-2.87093, "vy":2.42727, "omega":0.05257, "ax":-1.57003, "ay":-1.80933, "alpha":-0.1438, "fx":[-26.23539,-25.67717,-26.10808,-26.66587], "fy":[-30.64276,-30.26257,-29.67826,-30.05937]}, + {"t":0.77023, "x":5.5628, "y":5.18565, "heading":-0.9518, "vx":-2.90506, "vy":2.38794, "omega":0.04944, "ax":-0.54799, "ay":-0.65726, "alpha":-0.02649, "fx":[-9.14941,-9.04459,-9.12005,-9.22486], "fy":[-11.04608,-10.97187,-10.86647,-10.94069]}, + {"t":0.79197, "x":5.49951, "y":5.23741, "heading":-0.95072, "vx":-2.91697, "vy":2.37365, "omega":0.04886, "ax":-0.20276, "ay":-0.24583, "alpha":-0.00745, "fx":[-3.38416,-3.35462,-3.37573,-3.40527], "fy":[-4.12317,-4.10211,-4.07254,-4.0936]}, + {"t":0.81371, "x":5.43605, "y":5.28896, "heading":-0.94966, "vx":-2.92138, "vy":2.3683, "omega":0.0487, "ax":-0.11758, "ay":-0.14331, "alpha":-0.00325, "fx":[-1.96178,-1.94889,-1.95812,-1.971], "fy":[-2.4,-2.39078,-2.37789,-2.38711]}, + {"t":0.83545, "x":5.37251, "y":5.34041, "heading":-0.9486, "vx":-2.92394, "vy":2.36519, "omega":0.04863, "ax":-0.18328, "ay":-0.22375, "alpha":-0.00622, "fx":[-3.05865,-3.03405,-3.05171,-3.07631], "fy":[-3.75096,-3.73333,-3.70872,-3.72635]}, + {"t":0.85719, "x":5.30891, "y":5.39177, "heading":-0.94754, "vx":-2.92792, "vy":2.36032, "omega":0.0485, "ax":-0.48038, "ay":-0.58958, "alpha":-0.01914, "fx":[-8.01802,-7.94246,-7.99723,-8.07279], "fy":[-9.89303,-9.83895,-9.76308,-9.81716]}, + {"t":0.87893, "x":5.24514, "y":5.44295, "heading":-0.94649, "vx":-2.93837, "vy":2.34751, "omega":0.04808, "ax":-1.33636, "ay":-1.67099, "alpha":-0.05142, "fx":[-22.29969,-22.0996,-22.2534,-22.45344], "fy":[-28.02734,-27.88836,-27.68196,-27.82104]}, + {"t":0.90067, "x":5.18094, "y":5.49359, "heading":-0.94544, "vx":-2.96742, "vy":2.31118, "omega":0.04696, "ax":-3.14526, "ay":-4.12827, "alpha":-0.09124, "fx":[-52.42638,-52.09505,-52.43384,-52.76464], "fy":[-69.10178,-68.9204,-68.53056,-68.71264]}, + {"t":0.92241, "x":5.11569, "y":5.54286, "heading":-0.94442, "vx":-3.03579, "vy":2.22143, "omega":0.04498, "ax":-4.80508, "ay":-7.02145, "alpha":0.13817, "fx":[-80.23162,-80.68898,-79.96671,-79.50604], "fy":[-116.69559,-116.76157,-117.39203,-117.32787]}, + {"t":0.94415, "x":5.04856, "y":5.58949, "heading":-0.94345, "vx":-3.14026, "vy":2.06879, "omega":0.04798, "ax":-4.71975, "ay":-7.17713, "alpha":-0.63359, "fx":[-78.08639,-75.90067,-79.30057,-81.41627], "fy":[-121.18246,-120.94164,-118.07836,-118.35456]}, + {"t":0.96937, "x":4.96785, "y":5.63939, "heading":-0.94224, "vx":-3.2593, "vy":1.88775, "omega":0.032, "ax":-2.17374, "ay":-3.84089, "alpha":-0.06535, "fx":[-36.24693,-35.99318,-36.22358,-36.47717], "fy":[-64.22842,-64.08459,-63.82318,-63.96731]}, + {"t":0.9946, "x":4.88494, "y":5.68578, "heading":-0.94143, "vx":-3.31413, "vy":1.79087, "omega":0.03035, "ax":-0.64258, "ay":-1.19343, "alpha":-0.01356, "fx":[-10.71827,-10.66493,-10.70465,-10.75799], "fy":[-19.93956,-19.90154,-19.84811,-19.88614]}, + {"t":1.01982, "x":4.80115, "y":5.73058, "heading":-0.94066, "vx":-3.33034, "vy":1.76077, "omega":0.03001, "ax":-0.17158, "ay":-0.32355, "alpha":-0.00294, "fx":[-2.86179,-2.85021,-2.85867,-2.87024], "fy":[-5.40346,-5.39504,-5.38346,-5.39189]}, + {"t":1.04505, "x":4.71709, "y":5.77489, "heading":-0.93991, "vx":-3.33467, "vy":1.75261, "omega":0.02994, "ax":-0.0456, "ay":-0.08643, "alpha":-0.00043, "fx":[-0.76028,-0.7586,-0.75983,-0.76151], "fy":[-1.44223,-1.44099,-1.43931,-1.44054]}, + {"t":1.07027, "x":4.63296, "y":5.81907, "heading":-0.93915, "vx":-3.33582, "vy":1.75043, "omega":0.02993, "ax":-0.01206, "ay":-0.02299, "alpha":0.00022, "fx":[-0.20087,-0.20172,-0.2011,-0.20024], "fy":[-0.38242,-0.38305,-0.3839,-0.38328]}, + {"t":1.09549, "x":4.54882, "y":5.86321, "heading":-0.9384, "vx":-3.33612, "vy":1.74985, "omega":0.02993, "ax":-0.00311, "ay":-0.00604, "alpha":0.00039, "fx":[-0.05168,-0.05321,-0.05209,-0.05056], "fy":[-0.09941,-0.10053,-0.10206,-0.10094]}, + {"t":1.12072, "x":4.46467, "y":5.90735, "heading":-0.93764, "vx":-3.3362, "vy":1.7497, "omega":0.02994, "ax":-0.00073, "ay":-0.00152, "alpha":0.00044, "fx":[-0.01186,-0.01358,-0.01232,-0.01061], "fy":[-0.02385,-0.02511,-0.02683,-0.02557]}, + {"t":1.14594, "x":4.38052, "y":5.95148, "heading":-0.93689, "vx":-3.33622, "vy":1.74966, "omega":0.02995, "ax":-0.00009, "ay":-0.00031, "alpha":0.00045, "fx":[-0.00123,-0.003,-0.0017,0.00006], "fy":[-0.00368,-0.00498,-0.00675,-0.00545]}, + {"t":1.17116, "x":4.29636, "y":5.99561, "heading":-0.93613, "vx":-3.33622, "vy":1.74965, "omega":0.02996, "ax":0.00008, "ay":0.00001, "alpha":0.00046, "fx":[0.00161,-0.00018,0.00114,0.00292], "fy":[0.00171,0.00039,-0.00139,-0.00008]}, + {"t":1.19639, "x":4.21221, "y":6.03974, "heading":-0.93537, "vx":-3.33622, "vy":1.74965, "omega":0.02997, "ax":0.00013, "ay":0.0001, "alpha":0.00046, "fx":[0.00237,0.00057,0.0019,0.00369], "fy":[0.00315,0.00183,0.00003,0.00135]}, + {"t":1.22161, "x":4.12806, "y":6.08388, "heading":-0.93462, "vx":-3.33622, "vy":1.74965, "omega":0.02999, "ax":0.00014, "ay":0.00012, "alpha":0.00046, "fx":[0.00257,0.00077,0.0021,0.0039], "fy":[0.00354,0.00221,0.00041,0.00174]}, + {"t":1.24683, "x":4.04391, "y":6.12801, "heading":-0.93386, "vx":-3.33621, "vy":1.74966, "omega":0.03, "ax":0.00014, "ay":0.00012, "alpha":0.00046, "fx":[0.00263,0.00083,0.00216,0.00397], "fy":[0.00364,0.00231,0.0005,0.00184]}, + {"t":1.27206, "x":3.95976, "y":6.17214, "heading":-0.93311, "vx":-3.33621, "vy":1.74966, "omega":0.03001, "ax":0.00014, "ay":0.00013, "alpha":0.00046, "fx":[0.00265,0.00084,0.00218,0.00399], "fy":[0.00367,0.00233,0.00052,0.00186]}, + {"t":1.29728, "x":3.87561, "y":6.21627, "heading":-0.93235, "vx":-3.33621, "vy":1.74966, "omega":0.03002, "ax":0.00015, "ay":0.00013, "alpha":0.00046, "fx":[0.00265,0.00084,0.00218,0.004], "fy":[0.00368,0.00233,0.00051,0.00186]}, + {"t":1.3225, "x":3.79146, "y":6.26041, "heading":-0.93159, "vx":-3.3362, "vy":1.74967, "omega":0.03003, "ax":0.00014, "ay":0.00012, "alpha":0.00047, "fx":[0.00264,0.00082,0.00218,0.004], "fy":[0.00365,0.0023,0.00048,0.00183]}, + {"t":1.34773, "x":3.70731, "y":6.30454, "heading":-0.93083, "vx":-3.3362, "vy":1.74967, "omega":0.03004, "ax":0.00014, "ay":0.00012, "alpha":0.00047, "fx":[0.00259,0.00077,0.00213,0.00395], "fy":[0.00356,0.0022,0.00037,0.00173]}, + {"t":1.37295, "x":3.62316, "y":6.34867, "heading":-0.93008, "vx":-3.3362, "vy":1.74967, "omega":0.03006, "ax":0.00013, "ay":0.00009, "alpha":0.00047, "fx":[0.00239,0.00056,0.00193,0.00376], "fy":[0.00317,0.00181,-0.00002,0.00134]}, + {"t":1.39818, "x":3.539, "y":6.39281, "heading":-0.92932, "vx":-3.33619, "vy":1.74968, "omega":0.03007, "ax":0.00008, "ay":0.00001, "alpha":0.00047, "fx":[0.00163,-0.0002,0.00117,0.003], "fy":[0.00173,0.00036,-0.00147,-0.0001]}, + {"t":1.4234, "x":3.45485, "y":6.43694, "heading":-0.92856, "vx":-3.33619, "vy":1.74968, "omega":0.03008, "ax":-0.00009, "ay":-0.00032, "alpha":0.00047, "fx":[-0.00122,-0.00305,-0.00168,0.00014], "fy":[-0.0037,-0.00506,-0.00688,-0.00552]}, + {"t":1.44862, "x":3.3707, "y":6.48107, "heading":-0.9278, "vx":-3.33619, "vy":1.74967, "omega":0.03009, "ax":-0.00073, "ay":-0.00153, "alpha":0.00046, "fx":[-0.01194,-0.01372,-0.01238,-0.0106], "fy":[-0.02403,-0.02536,-0.02714,-0.02581]}, + {"t":1.47385, "x":3.28655, "y":6.5252, "heading":-0.92704, "vx":-3.33621, "vy":1.74963, "omega":0.0301, "ax":-0.00314, "ay":-0.0061, "alpha":0.00041, "fx":[-0.05207,-0.05367,-0.05247,-0.05088], "fy":[-0.10021,-0.10141,-0.103,-0.1018]}, + {"t":1.49907, "x":3.2024, "y":6.56933, "heading":-0.92628, "vx":-3.33629, "vy":1.74948, "omega":0.03011, "ax":-0.01215, "ay":-0.02318, "alpha":0.00023, "fx":[-0.2024,-0.2033,-0.20263,-0.20172], "fy":[-0.3856,-0.38628,-0.38718,-0.38651]}, + {"t":1.52429, "x":3.11824, "y":6.61345, "heading":-0.92552, "vx":-3.3366, "vy":1.74889, "omega":0.03012, "ax":-0.04587, "ay":-0.08719, "alpha":-0.00044, "fx":[-0.76489,-0.7632,-0.76447,-0.76617], "fy":[-1.45493,-1.45365,-1.45196,-1.45323]}, + {"t":1.54952, "x":3.03407, "y":6.65754, "heading":-0.92476, "vx":-3.33775, "vy":1.74669, "omega":0.03011, "ax":-0.17156, "ay":-0.32695, "alpha":-0.00289, "fx":[-2.86123,-2.84998,-2.85848,-2.86973], "fy":[-5.4599,-5.45143,-5.44018,-5.44865]}, + {"t":1.57474, "x":2.94982, "y":6.70149, "heading":-0.924, "vx":-3.34208, "vy":1.73844, "omega":0.03003, "ax":-0.63035, "ay":-1.21208, "alpha":-0.01857, "fx":[-10.51583,-10.44369,-10.49938,-10.57153], "fy":[-20.26749,-20.21416,-20.14193,-20.19526]}, + {"t":1.59996, "x":2.86532, "y":6.74496, "heading":-0.92325, "vx":-3.35798, "vy":1.70787, "omega":0.02957, "ax":-2.28282, "ay":-3.81255, "alpha":-1.17181, "fx":[-38.17231,-33.7139,-37.96411,-42.36402], "fy":[-67.19922,-64.58208,-59.85957,-62.57262]}, + {"t":1.62519, "x":2.7799, "y":6.78682, "heading":-0.9225, "vx":-3.41556, "vy":1.61171, "omega":0.00001, "ax":-1.48681, "ay":-3.24268, "alpha":-0.00026, "fx":[-24.78453,-24.78353,-24.78439,-24.78539], "fy":[-54.05477,-54.05413,-54.05315,-54.05379]}, + {"t":1.66119, "x":2.65595, "y":6.84275, "heading":-0.9225, "vx":-3.4691, "vy":1.49495, "omega":0.0, "ax":-0.19635, "ay":-0.45859, "alpha":0.0, "fx":[-3.27306,-3.27306,-3.27306,-3.27306], "fy":[-7.64447,-7.64447,-7.64447,-7.64447]}, + {"t":1.6972, "x":2.53092, "y":6.89628, "heading":-0.9225, "vx":-3.47617, "vy":1.47844, "omega":0.0, "ax":-0.02492, "ay":-0.05863, "alpha":0.0, "fx":[-0.41535,-0.41535,-0.41535,-0.41535], "fy":[-0.97741,-0.97741,-0.97741,-0.97741]}, + {"t":1.7332, "x":2.40574, "y":6.94948, "heading":-0.9225, "vx":-3.47706, "vy":1.47633, "omega":0.0, "ax":-0.00317, "ay":-0.00748, "alpha":0.0, "fx":[-0.05291,-0.05291,-0.05291,-0.05291], "fy":[-0.12467,-0.12467,-0.12467,-0.12467]}, + {"t":1.76921, "x":2.28055, "y":7.00263, "heading":-0.9225, "vx":-3.47718, "vy":1.47606, "omega":0.0, "ax":0.00414, "ay":-0.0029, "alpha":0.0, "fx":[0.0691,0.0691,0.0691,0.06909], "fy":[-0.04842,-0.04842,-0.04842,-0.04842]}, + {"t":1.80521, "x":2.15535, "y":7.05577, "heading":-0.9225, "vx":-3.47703, "vy":1.47595, "omega":0.0, "ax":8.77032, "ay":-3.723, "alpha":-0.00005, "fx":[146.19687,146.19706,146.19707,146.19688], "fy":[-62.0609,-62.06057,-62.06035,-62.06067]}, + {"t":1.84122, "x":2.03585, "y":7.1065, "heading":-0.9225, "vx":-3.16125, "vy":1.34191, "omega":0.0, "ax":10.90842, "ay":-4.63048, "alpha":0.0, "fx":[181.83821,181.83819,181.83818,181.8382], "fy":[-77.18783,-77.18787,-77.18789,-77.18785]}, + {"t":1.87723, "x":1.92909, "y":7.15181, "heading":-0.9225, "vx":-2.76849, "vy":1.17518, "omega":0.0, "ax":10.9582, "ay":-4.6516, "alpha":0.00001, "fx":[182.66787,182.66785,182.66784,182.66786], "fy":[-77.53995,-77.54,-77.54003,-77.53998]}, + {"t":1.91323, "x":1.83652, "y":7.19111, "heading":-0.9225, "vx":-2.37393, "vy":1.0077, "omega":0.0, "ax":10.97525, "ay":-4.65884, "alpha":0.00001, "fx":[182.95208,182.95206,182.95205,182.95207], "fy":[-77.66058,-77.66063,-77.66066,-77.66061]}, + {"t":1.94924, "x":1.75816, "y":7.22437, "heading":-0.9225, "vx":-1.97876, "vy":0.83996, "omega":0.0, "ax":10.98385, "ay":-4.66249, "alpha":0.00001, "fx":[183.09556,183.09554,183.09553,183.09555], "fy":[-77.72147,-77.72152,-77.72155,-77.7215]}, + {"t":1.98524, "x":1.69403, "y":7.2516, "heading":-0.9225, "vx":-1.58328, "vy":0.67208, "omega":0.0, "ax":10.98904, "ay":-4.6647, "alpha":0.00001, "fx":[183.18207,183.18205,183.18203,183.18206], "fy":[-77.75818,-77.75824,-77.75827,-77.75822]}, + {"t":2.02125, "x":1.64415, "y":7.27277, "heading":-0.9225, "vx":-1.18762, "vy":0.50413, "omega":0.0, "ax":10.99251, "ay":-4.66617, "alpha":0.00001, "fx":[183.23991,183.23988,183.23987,183.23989], "fy":[-77.78273,-77.78279,-77.78282,-77.78276]}, + {"t":2.05725, "x":1.60851, "y":7.2879, "heading":-0.9225, "vx":-0.79183, "vy":0.33612, "omega":0.0, "ax":10.99499, "ay":-4.66722, "alpha":0.00001, "fx":[183.2813,183.28128,183.28126,183.28129], "fy":[-77.8003,-77.80036,-77.80039,-77.80033]}, + {"t":2.09326, "x":1.58713, "y":7.29697, "heading":-0.9225, "vx":-0.39595, "vy":0.16807, "omega":0.0, "ax":10.99686, "ay":-4.66801, "alpha":0.00001, "fx":[183.31239,183.31236,183.31235,183.31237], "fy":[-77.81349,-77.81355,-77.81358,-77.81352]}, + {"t":2.12926, "x":1.58, "y":7.3, "heading":-0.9225, "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/HToBarge.traj b/src/main/deploy/choreo/HToBarge.traj index 103c4f33..d9ff2e9e 100644 --- a/src/main/deploy/choreo/HToBarge.traj +++ b/src/main/deploy/choreo/HToBarge.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.7597, "y":4.3597, "heading":3.141592653589793, "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":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.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}, @@ -26,27 +26,27 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.88977], + "waypoints":[0.0,0.86594], "samples":[ - {"t":0.0, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":10.09554, "ay":2.07964, "alpha":0.0, "fx":[171.72232,171.72232,171.72232,171.72232], "fy":[35.37413,35.37413,35.37413,35.37413]}, - {"t":0.04943, "x":5.78925, "y":3.95379, "heading":3.14159, "vx":0.49904, "vy":0.1028, "omega":0.0, "ax":10.09431, "ay":2.07939, "alpha":0.0, "fx":[171.70128,171.70128,171.70128,171.70128], "fy":[35.3698,35.3698,35.3698,35.3698]}, - {"t":0.09886, "x":5.82625, "y":3.96142, "heading":3.14159, "vx":0.99802, "vy":0.20559, "omega":0.0, "ax":10.09266, "ay":2.07905, "alpha":0.0, "fx":[171.67321,171.67321,171.67321,171.67321], "fy":[35.36401,35.36401,35.36401,35.36401]}, - {"t":0.14829, "x":5.88791, "y":3.97412, "heading":3.14159, "vx":1.49691, "vy":0.30836, "omega":0.0, "ax":10.09035, "ay":2.07857, "alpha":0.0, "fx":[171.6339,171.6339,171.6339,171.6339], "fy":[35.35592,35.35592,35.35592,35.35592]}, - {"t":0.19773, "x":5.97423, "y":3.9919, "heading":3.14159, "vx":1.9957, "vy":0.41111, "omega":0.0, "ax":10.08688, "ay":2.07786, "alpha":0.0, "fx":[171.57491,171.57491,171.57491,171.57491], "fy":[35.34376,35.34376,35.34376,35.34376]}, - {"t":0.24716, "x":6.08521, "y":4.01476, "heading":3.14159, "vx":2.49431, "vy":0.51382, "omega":0.0, "ax":10.0811, "ay":2.07667, "alpha":0.0, "fx":[171.47656,171.47656,171.47656,171.47656], "fy":[35.3235,35.3235,35.3235,35.3235]}, - {"t":0.29659, "x":6.22082, "y":4.0427, "heading":3.14159, "vx":2.99263, "vy":0.61647, "omega":0.0, "ax":10.06953, "ay":2.07428, "alpha":0.0, "fx":[171.27978,171.27978,171.27978,171.27978], "fy":[35.28297,35.28297,35.28297,35.28297]}, - {"t":0.34602, "x":6.38106, "y":4.0757, "heading":3.14159, "vx":3.49038, "vy":0.71901, "omega":0.0, "ax":10.03487, "ay":2.06714, "alpha":0.0, "fx":[170.6903,170.6903,170.6903,170.6903], "fy":[35.16154,35.16154,35.16154,35.16154]}, - {"t":0.39545, "x":6.56585, "y":4.11377, "heading":3.14159, "vx":3.98642, "vy":0.82119, "omega":0.0, "ax":4.41992, "ay":0.91049, "alpha":0.0, "fx":[75.18164,75.18164,75.18164,75.18164], "fy":[15.48712,15.48712,15.48712,15.48712]}, - {"t":0.44488, "x":6.76831, "y":4.15548, "heading":3.14159, "vx":4.20491, "vy":0.86619, "omega":0.0, "ax":-4.41992, "ay":-0.91049, "alpha":0.0, "fx":[-75.18164,-75.18164,-75.18164,-75.18164], "fy":[-15.48712,-15.48712,-15.48712,-15.48712]}, - {"t":0.49432, "x":6.97076, "y":4.19718, "heading":3.14159, "vx":3.98642, "vy":0.82119, "omega":0.0, "ax":-10.03487, "ay":-2.06714, "alpha":0.0, "fx":[-170.6903,-170.6903,-170.6903,-170.6903], "fy":[-35.16154,-35.16154,-35.16154,-35.16154]}, - {"t":0.54375, "x":7.15556, "y":4.23525, "heading":3.14159, "vx":3.49038, "vy":0.71901, "omega":0.0, "ax":-10.06953, "ay":-2.07428, "alpha":0.0, "fx":[-171.27978,-171.27978,-171.27978,-171.27978], "fy":[-35.28297,-35.28297,-35.28297,-35.28297]}, - {"t":0.59318, "x":7.31579, "y":4.26826, "heading":3.14159, "vx":2.99263, "vy":0.61647, "omega":0.0, "ax":-10.0811, "ay":-2.07667, "alpha":0.0, "fx":[-171.47656,-171.47656,-171.47656,-171.47656], "fy":[-35.3235,-35.3235,-35.3235,-35.3235]}, - {"t":0.64261, "x":7.4514, "y":4.29619, "heading":3.14159, "vx":2.49431, "vy":0.51382, "omega":0.0, "ax":-10.08688, "ay":-2.07786, "alpha":0.0, "fx":[-171.57491,-171.57491,-171.57491,-171.57491], "fy":[-35.34376,-35.34376,-35.34376,-35.34376]}, - {"t":0.69204, "x":7.56238, "y":4.31905, "heading":3.14159, "vx":1.9957, "vy":0.41111, "omega":0.0, "ax":-10.09035, "ay":-2.07857, "alpha":0.0, "fx":[-171.6339,-171.6339,-171.6339,-171.6339], "fy":[-35.35592,-35.35592,-35.35592,-35.35592]}, - {"t":0.74147, "x":7.6487, "y":4.33683, "heading":3.14159, "vx":1.49691, "vy":0.30836, "omega":0.0, "ax":-10.09266, "ay":-2.07905, "alpha":0.0, "fx":[-171.67321,-171.67321,-171.67321,-171.67321], "fy":[-35.36401,-35.36401,-35.36401,-35.36401]}, - {"t":0.79091, "x":7.71036, "y":4.34954, "heading":3.14159, "vx":0.99802, "vy":0.20559, "omega":0.0, "ax":-10.09431, "ay":-2.07939, "alpha":0.0, "fx":[-171.70128,-171.70128,-171.70128,-171.70128], "fy":[-35.3698,-35.3698,-35.3698,-35.3698]}, - {"t":0.84034, "x":7.74737, "y":4.35716, "heading":3.14159, "vx":0.49904, "vy":0.1028, "omega":0.0, "ax":-10.09554, "ay":-2.07964, "alpha":0.0, "fx":[-171.72232,-171.72232,-171.72232,-171.72232], "fy":[-35.37413,-35.37413,-35.37413,-35.37413]}, - {"t":0.88977, "x":7.7597, "y":4.3597, "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.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.81633, "ay":1.78294, "alpha":0.0, "fx":[196.97252,196.97252,196.97252,196.97252], "fy":[29.72075,29.72075,29.72075,29.72075]}, + {"t":0.04811, "x":5.72682, "y":4.05296, "heading":3.14159, "vx":0.56846, "vy":0.08577, "omega":0.0, "ax":11.8142, "ay":1.78262, "alpha":0.0, "fx":[196.93707,196.93707,196.93707,196.93707], "fy":[29.7154,29.7154,29.7154,29.7154]}, + {"t":0.09622, "x":5.76784, "y":4.05915, "heading":3.14159, "vx":1.13681, "vy":0.17153, "omega":0.0, "ax":11.81101, "ay":1.78214, "alpha":0.0, "fx":[196.88392,196.88392,196.88392,196.88392], "fy":[29.70738,29.70738,29.70738,29.70738]}, + {"t":0.14432, "x":5.8362, "y":4.06947, "heading":3.14159, "vx":1.70502, "vy":0.25727, "omega":0.0, "ax":11.8057, "ay":1.78134, "alpha":0.0, "fx":[196.79538,196.79538,196.79538,196.79538], "fy":[29.69402,29.69402,29.69402,29.69402]}, + {"t":0.19243, "x":5.93188, "y":4.0839, "heading":3.14159, "vx":2.27296, "vy":0.34296, "omega":0.0, "ax":11.79509, "ay":1.77974, "alpha":0.0, "fx":[196.61854,196.61854,196.61854,196.61854], "fy":[29.66734,29.66734,29.66734,29.66734]}, + {"t":0.24054, "x":6.05488, "y":4.10246, "heading":3.14159, "vx":2.8404, "vy":0.42858, "omega":0.0, "ax":11.76343, "ay":1.77496, "alpha":0.0, "fx":[196.09071,196.09071,196.09071,196.09071], "fy":[29.58769,29.58769,29.58769,29.58769]}, + {"t":0.28865, "x":6.20514, "y":4.12514, "heading":3.14159, "vx":3.40631, "vy":0.51397, "omega":0.0, "ax":6.85699, "ay":1.03464, "alpha":0.0, "fx":[114.30265,114.30265,114.30265,114.30265], "fy":[17.24687,17.24687,17.24687,17.24687]}, + {"t":0.33675, "x":6.37694, "y":4.15106, "heading":3.14159, "vx":3.73619, "vy":0.56375, "omega":0.0, "ax":0.00061, "ay":0.00009, "alpha":0.0, "fx":[0.01009,0.01009,0.01009,0.01009], "fy":[0.00152,0.00152,0.00152,0.00152]}, + {"t":0.38486, "x":6.55668, "y":4.17818, "heading":3.14159, "vx":3.73622, "vy":0.56375, "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.43297, "x":6.73642, "y":4.2053, "heading":3.14159, "vx":3.73622, "vy":0.56375, "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.48108, "x":6.91616, "y":4.23242, "heading":3.14159, "vx":3.73622, "vy":0.56375, "omega":0.0, "ax":-0.00061, "ay":-0.00009, "alpha":0.0, "fx":[-0.01009,-0.01009,-0.01009,-0.01009], "fy":[-0.00152,-0.00152,-0.00152,-0.00152]}, + {"t":0.52919, "x":7.09591, "y":4.25954, "heading":3.14159, "vx":3.73619, "vy":0.56375, "omega":0.0, "ax":-6.85699, "ay":-1.03464, "alpha":0.0, "fx":[-114.30265,-114.30265,-114.30265,-114.30265], "fy":[-17.24687,-17.24687,-17.24687,-17.24687]}, + {"t":0.57729, "x":7.26771, "y":4.28546, "heading":3.14159, "vx":3.40631, "vy":0.51397, "omega":0.0, "ax":-11.76343, "ay":-1.77496, "alpha":0.0, "fx":[-196.09071,-196.09071,-196.09071,-196.09071], "fy":[-29.58769,-29.58769,-29.58769,-29.58769]}, + {"t":0.6254, "x":7.41797, "y":4.30814, "heading":3.14159, "vx":2.8404, "vy":0.42858, "omega":0.0, "ax":-11.79509, "ay":-1.77974, "alpha":0.0, "fx":[-196.61854,-196.61854,-196.61854,-196.61854], "fy":[-29.66734,-29.66734,-29.66734,-29.66734]}, + {"t":0.67351, "x":7.54096, "y":4.3267, "heading":3.14159, "vx":2.27296, "vy":0.34296, "omega":0.0, "ax":-11.8057, "ay":-1.78134, "alpha":0.0, "fx":[-196.79538,-196.79538,-196.79538,-196.79538], "fy":[-29.69402,-29.69402,-29.69402,-29.69402]}, + {"t":0.72162, "x":7.63665, "y":4.34113, "heading":3.14159, "vx":1.70502, "vy":0.25727, "omega":0.0, "ax":-11.81101, "ay":-1.78214, "alpha":0.0, "fx":[-196.88392,-196.88392,-196.88392,-196.88392], "fy":[-29.70738,-29.70738,-29.70738,-29.70738]}, + {"t":0.76973, "x":7.70501, "y":4.35145, "heading":3.14159, "vx":1.13681, "vy":0.17153, "omega":0.0, "ax":-11.8142, "ay":-1.78262, "alpha":0.0, "fx":[-196.93707,-196.93707,-196.93707,-196.93707], "fy":[-29.7154,-29.7154,-29.7154,-29.7154]}, + {"t":0.81783, "x":7.74603, "y":4.35764, "heading":3.14159, "vx":0.56846, "vy":0.08577, "omega":0.0, "ax":-11.81633, "ay":-1.78294, "alpha":0.0, "fx":[-196.97252,-196.97252,-196.97252,-196.97252], "fy":[-29.72075,-29.72075,-29.72075,-29.72075]}, + {"t":0.86594, "x":7.7597, "y":4.3597, "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/HTofetch.traj b/src/main/deploy/choreo/HTofetch.traj index 9a3dc730..1259d4ad 100644 --- a/src/main/deploy/choreo/HTofetch.traj +++ b/src/main/deploy/choreo/HTofetch.traj @@ -3,11 +3,11 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.946532726287842, "y":4.711103439331055, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":5.048557281494141, "y":5.589489936828613, "heading":-1.773711057838377, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9426766239853251, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.946532726287842, "y":4.711103439331055, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.048557281494141, "y":5.589489936828613, "heading":-1.773711057838377, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9225007574586108, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "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}, @@ -16,11 +16,11 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.946532726287842 m", "val":5.946532726287842}, "y":{"exp":"4.711103439331055 m", "val":4.711103439331055}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"-1.773711057838377 rad", "val":-1.773711057838377}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.946532726287842 m", "val":5.946532726287842}, "y":{"exp":"4.711103439331055 m", "val":4.711103439331055}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"-1.773711057838377 rad", "val":-1.773711057838377}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "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}, @@ -32,91 +32,86 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.43417,0.79118,1.46954,1.99555], + "waypoints":[0.0,0.40545,0.78804,1.56044,2.06453], "samples":[ - {"t":0.0, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.27299, "ay":7.77502, "alpha":8.68726, "fx":[150.78654,114.04652,59.9091,102.06473], "fy":[89.16091,133.06217,164.63284,142.14733]}, - {"t":0.02412, "x":5.77874, "y":3.95352, "heading":3.14159, "vx":0.15131, "vy":0.18754, "omega":0.20954, "ax":6.0078, "ay":7.96277, "alpha":8.88088, "fx":[148.71413,110.96411,54.48007,94.60521], "fy":[92.51688,135.61712,166.48436,147.15926]}, - {"t":0.04824, "x":5.78413, "y":3.96036, "heading":-3.13654, "vx":0.29622, "vy":0.37961, "omega":0.42376, "ax":5.68158, "ay":8.17668, "alpha":9.11051, "fx":[146.01699,107.01919,47.91824,85.61379], "fy":[96.64948,138.721,168.46249,152.4991]}, - {"t":0.07236, "x":5.79293, "y":3.97189, "heading":-3.12632, "vx":0.43327, "vy":0.57684, "omega":0.64351, "ax":5.27293, "ay":8.42018, "alpha":9.38563, "fx":[142.43363,101.95205,39.9169,74.4615], "fy":[101.77732,142.45112,170.50327,158.1674]}, - {"t":0.09648, "x":5.80492, "y":3.98825, "heading":-3.11079, "vx":0.56045, "vy":0.77994, "omega":0.8699, "ax":4.7509, "ay":8.69488, "alpha":9.71996, "fx":[137.53913,95.38289,30.07321,60.25089], "fy":[108.20728,146.88912,172.47389,164.01934]}, - {"t":0.1206, "x":5.81982, "y":4.0096, "heading":-3.08981, "vx":0.67505, "vy":0.98967, "omega":1.10435, "ax":4.07095, "ay":8.99714, "alpha":10.1303, "fx":[130.61162,86.74377,17.86587,41.76129], "fy":[116.36447,152.10123,174.1177,169.57168]}, - {"t":0.14472, "x":5.83729, "y":4.03608, "heading":-3.06317, "vx":0.77324, "vy":1.20668, "omega":1.3487, "ax":3.17076, "ay":9.31069, "alpha":10.62556, "fx":[120.36529,75.16994,2.651,17.54882], "fy":[126.80131,158.08233,174.95989,173.64491]}, - {"t":0.16885, "x":5.85686, "y":4.0679, "heading":-3.03064, "vx":0.84973, "vy":1.43127, "omega":1.605, "ax":1.96992, "ay":9.5936, "alpha":11.16109, "fx":[104.41579,59.34076,-16.27592,-13.44975], "fy":[140.06718,164.60795,174.16152,173.9007]}, - {"t":0.19297, "x":5.87793, "y":4.10521, "heading":-2.99193, "vx":0.89724, "vy":1.66267, "omega":1.87422, "ax":0.37977, "ay":9.76026, "alpha":11.51147, "fx":[78.34541,37.31434,-39.44548,-50.37516], "fy":[155.96257,170.86748,170.34796,166.89863]}, - {"t":0.21709, "x":5.89968, "y":4.14816, "heading":-2.94672, "vx":0.9064, "vy":1.8981, "omega":2.15188, "ax":-1.66842, "ay":9.6567, "alpha":11.11248, "fx":[35.21357,6.65004,-66.65369,-88.72712], "fy":[170.74107,174.66996,161.58341,150.03609]}, - {"t":0.24121, "x":5.92106, "y":4.19675, "heading":-2.89482, "vx":0.86616, "vy":2.13102, "omega":2.41993, "ax":-4.12167, "ay":9.02355, "alpha":9.40623, "fx":[-28.43685,-34.18138,-96.12007,-121.69527], "fy":[171.84636,171.32014,145.9273,124.85795]}, - {"t":0.26533, "x":5.94075, "y":4.25078, "heading":-2.83644, "vx":0.76674, "vy":2.34868, "omega":2.64681, "ax":-6.56019, "ay":7.64165, "alpha":6.90464, "fx":[-95.30646,-81.80189,-124.1816,-145.05759], "fy":[145.83253,154.28437,122.89806,96.91403]}, - {"t":0.28945, "x":5.95734, "y":4.30965, "heading":-2.7726, "vx":0.6085, "vy":2.533, "omega":2.81336, "ax":-8.39333, "ay":5.76456, "alpha":4.52617, "fx":[-139.70708,-125.27568,-146.76738,-159.32244], "fy":[104.39031,121.66165,94.80124,71.36065]}, - {"t":0.31357, "x":5.96957, "y":4.37243, "heading":-2.70474, "vx":0.40605, "vy":2.67205, "omega":2.92253, "ax":-9.4759, "ay":3.89633, "alpha":2.47042, "fx":[-161.18736,-154.40762,-161.87367,-167.26094], "fy":[67.12302,81.71386,65.86187,50.40289]}, - {"t":0.33769, "x":5.97661, "y":4.43801, "heading":-2.63425, "vx":0.17748, "vy":2.76603, "omega":2.98212, "ax":-10.00848, "ay":2.31796, "alpha":0.80567, "fx":[-170.32714,-168.95063,-170.24339,-171.44449], "fy":[39.15195,44.80455,39.70585,34.04867]}, - {"t":0.36181, "x":5.97798, "y":4.50541, "heading":-2.56232, "vx":-0.06393, "vy":2.82194, "omega":3.00155, "ax":-10.22378, "ay":1.07713, "alpha":-0.46361, "fx":[-173.89517,-174.2219,-173.94103,-173.55603], "fy":[18.63832,15.17743,17.99656,21.47464]}, - {"t":0.38593, "x":5.97346, "y":4.57379, "heading":-2.48992, "vx":-0.31054, "vy":2.84792, "omega":2.99037, "ax":-10.27691, "ay":0.12181, "alpha":-1.41324, "fx":[-174.95278,-174.79825,-174.92219,-174.55574], "fy":[3.1938,-7.49568,0.82341,11.76629]}, - {"t":0.41005, "x":5.96298, "y":4.64252, "heading":-2.41779, "vx":-0.55842, "vy":2.85086, "omega":2.95628, "ax":-10.2526, "ay":-0.61636, "alpha":-2.1278, "fx":[-174.82613,-173.25562,-174.5309,-174.96261], "fy":[-8.86335,-24.79841,-12.4341,4.15954]}, - {"t":0.43417, "x":5.94653, "y":4.7111, "heading":-2.34648, "vx":-0.80572, "vy":2.83599, "omega":2.90496, "ax":-10.20363, "ay":-0.98367, "alpha":-2.64213, "fx":[-174.22263,-171.40923,-173.78136,-174.83022], "fy":[-15.36232,-34.50419,-18.51246,1.45136]}, - {"t":0.45118, "x":5.93136, "y":4.75917, "heading":-2.29709, "vx":-0.97919, "vy":2.81927, "omega":2.86004, "ax":-10.16841, "ay":-1.11811, "alpha":-3.41215, "fx":[-173.93995,-169.68118,-173.4866,-174.73931], "fy":[-18.06774,-42.01421,-20.4555,4.46223]}, - {"t":0.46818, "x":5.91324, "y":4.80694, "heading":-2.24847, "vx":-1.15205, "vy":2.80026, "omega":2.80203, "ax":-10.12147, "ay":-1.26703, "alpha":-4.264, "fx":[-173.54956,-167.35236,-173.16799,-174.58352], "fy":[-21.29297,-50.33093,-22.27861,7.69507]}, - {"t":0.48518, "x":5.8922, "y":4.85436, "heading":-2.20084, "vx":-1.32412, "vy":2.77872, "omega":2.72955, "ax":-10.06003, "ay":-1.43072, "alpha":-5.20016, "fx":[-173.02079,-164.26848,-172.83045,-174.35319], "fy":[-25.04057,-59.46893,-23.93336,11.09811]}, - {"t":0.50218, "x":5.86823, "y":4.9014, "heading":-2.15443, "vx":-1.49515, "vy":2.7544, "omega":2.64114, "ax":-9.98152, "ay":-1.60763, "alpha":-6.21499, "fx":[-172.32289,-160.2796,-172.48351,-174.04491], "fy":[-29.27881,-69.34681,-25.33743,14.58194]}, - {"t":0.51918, "x":5.84137, "y":4.94799, "heading":-2.10953, "vx":-1.66483, "vy":2.72707, "omega":2.53548, "ax":-9.88504, "ay":-1.7924, "alpha":-7.285, "fx":[-171.43518,-155.316,-172.14709,-173.66884], "fy":[-33.91133,-79.68695,-26.34235,17.98812]}, - {"t":0.53618, "x":5.81164, "y":4.99409, "heading":-2.06643, "vx":-1.83288, "vy":2.6966, "omega":2.41164, "ax":-9.77427, "ay":-1.97351, "alpha":-8.35496, "fx":[-170.36649,-149.54371,-171.85945,-173.26044], "fy":[-38.73501,-89.89038,-26.69615,21.0462]}, - {"t":0.55318, "x":5.77907, "y":5.03965, "heading":-2.02543, "vx":-1.99905, "vy":2.66305, "omega":2.2696, "ax":-9.6596, "ay":-2.13446, "alpha":-9.33823, "fx":[-169.17304,-143.50076,-171.66934,-172.88527], "fy":[-43.44073,-99.07807,-26.09532,23.38779]}, - {"t":0.57018, "x":5.74369, "y":5.08461, "heading":-1.98685, "vx":-2.16326, "vy":2.62676, "omega":2.11085, "ax":-9.55264, "ay":-2.26527, "alpha":-10.1683, "fx":[-167.93005,-137.83413,-171.58035,-172.60592], "fy":[-47.77485,-106.6258,-24.4781,24.7527]}, - {"t":0.58718, "x":5.70553, "y":5.12894, "heading":-1.95096, "vx":-2.32566, "vy":2.58825, "omega":1.93799, "ax":-9.4559, "ay":-2.37292, "alpha":-10.85118, "fx":[-166.66569,-132.75911,-171.50917,-172.43441], "fy":[-51.72333,-112.65743,-22.25268,25.18289]}, - {"t":0.60418, "x":5.66463, "y":5.1726, "heading":-1.91802, "vx":-2.48641, "vy":2.54791, "omega":1.75351, "ax":-9.36452, "ay":-2.47357, "alpha":-11.42525, "fx":[-165.35693,-128.10688,-171.34078,-172.3464], "fy":[-55.42147,-117.65689,-20.06803,24.84747]}, - {"t":0.62118, "x":5.62101, "y":5.21556, "heading":-1.88821, "vx":-2.64561, "vy":2.50586, "omega":1.55928, "ax":-9.2727, "ay":-2.5839, "alpha":-11.91027, "fx":[-163.95972,-123.65418,-170.97475,-172.31548], "fy":[-59.01405,-122.01158,-18.60748,23.8273]}, - {"t":0.63818, "x":5.57469, "y":5.25778, "heading":-1.8617, "vx":-2.80325, "vy":2.46193, "omega":1.3568, "ax":-9.1747, "ay":-2.72184, "alpha":-12.30062, "fx":[-162.4094,-119.20429,-170.30279,-172.31959], "fy":[-62.64705,-125.97382,-18.6346,22.06452]}, - {"t":0.65518, "x":5.52571, "y":5.29924, "heading":-1.83863, "vx":-2.95922, "vy":2.41566, "omega":1.14769, "ax":-9.06257, "ay":-2.91102, "alpha":-12.56938, "fx":[-160.59724,-114.54814,-169.12893,-172.33255], "fy":[-66.51139,-129.73009,-21.14969,19.32862]}, - {"t":0.67218, "x":5.47409, "y":5.33989, "heading":-1.81912, "vx":-3.11329, "vy":2.36617, "omega":0.93401, "ax":-8.92236, "ay":-3.18752, "alpha":-12.66127, "fx":[-158.32707,-109.45772,-166.97985,-172.30235], "fy":[-70.89515,-133.40578,-27.6515,15.0772]}, - {"t":0.68918, "x":5.41988, "y":5.37965, "heading":-1.80324, "vx":-3.26497, "vy":2.31199, "omega":0.71876, "ax":-8.72592, "ay":-3.6109, "alpha":-12.46746, "fx":[-155.22036,-103.78544,-162.62525,-172.07059], "fy":[-76.26631,-136.96538,-40.49696,8.04735]}, - {"t":0.70618, "x":5.36311, "y":5.41844, "heading":-1.79102, "vx":-3.41331, "vy":2.2506, "omega":0.50681, "ax":-8.41009, "ay":-4.27769, "alpha":-11.77557, "fx":[-150.43332,-97.74685,-153.05549,-170.97707], "fy":[-83.49781,-139.97721,-62.64064,-4.93318]}, - {"t":0.72318, "x":5.30387, "y":5.45608, "heading":-1.78241, "vx":-3.55629, "vy":2.17788, "omega":0.30662, "ax":-7.81345, "ay":-5.33295, "alpha":-10.12216, "fx":[-141.4177,-91.72247,-132.95739,-165.52045], "fy":[-94.84751,-141.64569,-94.09984,-32.25477]}, - {"t":0.74018, "x":5.24228, "y":5.49233, "heading":-1.77719, "vx":-3.68912, "vy":2.08722, "omega":0.13454, "ax":-6.3984, "ay":-6.99014, "alpha":-6.02977, "fx":[-117.78402,-82.70119,-98.93651,-135.91822], "fy":[-116.72541,-142.24913,-126.12647,-90.50006]}, - {"t":0.75718, "x":5.17864, "y":5.52681, "heading":-1.77491, "vx":-3.79789, "vy":1.96838, "omega":0.03204, "ax":-3.89522, "ay":-8.15098, "alpha":0.36864, "fx":[-65.09001,-67.93289,-67.40263,-64.60107], "fy":[-138.92518,-137.67233,-138.38159,-139.60417]}, - {"t":0.77418, "x":5.11351, "y":5.55909, "heading":-1.77436, "vx":-3.86411, "vy":1.82981, "omega":0.0383, "ax":5.0962, "ay":-4.90822, "alpha":18.79489, "fx":[147.93722,90.02279,9.17581,99.60351], "fy":[-34.75609,-30.5035,-147.50408,-121.18609]}, - {"t":0.79118, "x":5.04856, "y":5.58949, "heading":-1.77371, "vx":-3.77748, "vy":1.74637, "omega":0.35782, "ax":5.72809, "ay":-0.36717, "alpha":13.85288, "fx":[124.97881,81.89562,67.79369,115.06483], "fy":[22.18273,52.67748,-51.7097,-48.13262]}, - {"t":0.81541, "x":4.95872, "y":5.63169, "heading":-1.76504, "vx":-3.6387, "vy":1.73748, "omega":0.69344, "ax":4.25886, "ay":-1.81535, "alpha":11.65475, "fx":[103.11066,59.20872,39.81011,87.63865], "fy":[-5.15853,8.79363,-65.37568,-61.77369]}, - {"t":0.83963, "x":4.87181, "y":5.67325, "heading":-1.74824, "vx":-3.53552, "vy":1.6935, "omega":0.9758, "ax":3.30195, "ay":-2.23199, "alpha":9.611, "fx":[84.08423,42.20685,27.9809,70.38896], "fy":[-16.56985,-9.40801,-63.64966,-62.2346]}, - {"t":0.86386, "x":4.78713, "y":5.71363, "heading":-1.7246, "vx":-3.45552, "vy":1.63942, "omega":1.20865, "ax":2.77807, "ay":-2.01032, "alpha":7.8581, "fx":[70.01967,33.98063,24.7133,60.30311], "fy":[-16.64297,-11.93326,-54.12995,-54.074]}, - {"t":0.88809, "x":4.70423, "y":5.75276, "heading":-1.69532, "vx":-3.38822, "vy":1.59072, "omega":1.39903, "ax":2.4085, "ay":-1.47509, "alpha":6.1571, "fx":[58.06424,29.40638,24.06325,52.33746], "fy":[-11.17873,-7.9596,-40.28581,-40.93944]}, - {"t":0.91232, "x":4.62285, "y":5.79086, "heading":-1.66142, "vx":-3.32987, "vy":1.55498, "omega":1.5482, "ax":2.03705, "ay":-0.80314, "alpha":4.40081, "fx":[46.23178,25.59112,23.13107,43.64466], "fy":[-3.55819,-1.64892,-24.3144,-25.12305]}, - {"t":0.93654, "x":4.54277, "y":5.8283, "heading":-1.62392, "vx":-3.28051, "vy":1.53552, "omega":1.65482, "ax":1.61692, "ay":-0.11727, "alpha":2.64757, "fx":[34.13689,21.58365,20.86416,33.42884], "fy":[4.19861,5.0066,-8.34065,-8.84377]}, - {"t":0.96077, "x":4.46377, "y":5.86547, "heading":-1.58382, "vx":-3.24134, "vy":1.53268, "omega":1.71896, "ax":1.1681, "ay":0.48658, "alpha":1.04319, "fx":[22.38498,17.36443,17.34638,22.38045], "fy":[10.77459,10.91209,5.76208,5.65741]}, - {"t":0.985, "x":4.38558, "y":5.90274, "heading":-1.54218, "vx":-3.21304, "vy":1.54447, "omega":1.74423, "ax":0.73504, "ay":0.95081, "alpha":-0.28191, "fx":[11.84283,13.2204,13.16204,11.7863], "fy":[15.47785,15.49976,16.86729,16.84694]}, - {"t":1.00923, "x":4.30795, "y":5.94044, "heading":-1.49992, "vx":-3.19523, "vy":1.5675, "omega":1.73741, "ax":0.35436, "ay":1.26167, "alpha":-1.27392, "fx":[3.12801,9.4243,8.90138,2.65629], "fy":[18.21104,18.60283,24.69995,24.32884]}, - {"t":1.03345, "x":4.23065, "y":5.97878, "heading":-1.45783, "vx":-3.18665, "vy":1.59807, "omega":1.70654, "ax":0.04413, "ay":1.43568, "alpha":-1.94842, "fx":[-3.53913,6.14199,4.97015,-4.57016], "fy":[19.2691,20.32412,29.5551,28.53397]}, - {"t":1.05768, "x":4.15345, "y":6.01792, "heading":-1.41648, "vx":-3.18558, "vy":1.63285, "omega":1.65934, "ax":-0.19231, "ay":1.50176, "alpha":-2.35535, "fx":[-8.24825,3.44418,1.59524,-9.8759], "fy":[19.09592,20.90384,31.97321,30.20482]}, - {"t":1.08191, "x":4.07622, "y":6.05792, "heading":-1.37628, "vx":-3.19024, "vy":1.66924, "omega":1.60227, "ax":-0.36037, "ay":1.49, "alpha":-2.55317, "fx":[-11.26466,1.33273,-1.12709,-13.46037], "fy":[18.12597,20.62665,32.5417,30.08337]}, - {"t":1.10613, "x":3.99882, "y":6.0988, "heading":-1.33746, "vx":-3.19897, "vy":1.70534, "omega":1.54042, "ax":-0.47011, "ay":1.42698, "alpha":-2.59764, "fx":[-12.93132,-0.24119,-3.19508,-15.61806], "fy":[16.71559,19.76794,31.80756,28.79889]}, - {"t":1.13036, "x":3.92118, "y":6.14054, "heading":-1.30014, "vx":-3.21036, "vy":1.73991, "omega":1.47748, "ax":-0.53407, "ay":1.33433, "alpha":-2.53866, "fx":[-13.61198,-1.36382,-4.67813,-16.68353], "fy":[15.12524,18.56564,30.24616,26.84909]}, - {"t":1.15459, "x":3.84325, "y":6.18308, "heading":-1.26435, "vx":-3.2233, "vy":1.77223, "omega":1.41598, "ax":-0.56604, "ay":1.22862, "alpha":-2.42015, "fx":[-13.66065,-2.14672,-5.69889,-17.00653], "fy":[13.5238,17.20735,28.25244,24.61011]}, - {"t":1.17882, "x":3.76499, "y":6.22638, "heading":-1.23004, "vx":-3.23701, "vy":1.802, "omega":1.35735, "ax":-0.58026, "ay":1.12173, "alpha":-2.28072, "fx":[-13.40724,-2.71629,-6.41781,-16.93892], "fy":[12.00015,15.82723,26.14143,22.35222]}, - {"t":1.20304, "x":3.6864, "y":6.27036, "heading":-1.19716, "vx":-3.25107, "vy":1.82918, "omega":1.30209, "ax":-0.59107, "ay":1.02143, "alpha":-2.1548, "fx":[-13.1558,-3.20707,-7.02243,-16.83039], "fy":[10.57579,14.50931,24.15571,20.25623]}, - {"t":1.22727, "x":3.60746, "y":6.31498, "heading":-1.16561, "vx":-3.26539, "vy":1.85392, "omega":1.24988, "ax":-0.61314, "ay":0.9321, "alpha":-2.07471, "fx":[-13.19545,-3.76126,-7.72478,-17.03606], "fy":[9.21451,13.2942,22.47961,18.43045]}, - {"t":1.2515, "x":3.52817, "y":6.36017, "heading":-1.13533, "vx":-3.28024, "vy":1.8765, "omega":1.19962, "ax":-0.66246, "ay":0.8554, "alpha":-2.07387, "fx":[-13.82483,-4.53631,-8.77108,-17.94048], "fy":[7.82622,12.18662,21.26007,16.92742]}, - {"t":1.27573, "x":3.4485, "y":6.40588, "heading":-1.10627, "vx":-3.29629, "vy":1.89723, "omega":1.14938, "ax":-0.75812, "ay":0.79095, "alpha":-2.19166, "fx":[-15.39164,-5.72189,-10.46671,-20.00128], "fy":[6.26155,11.16149,20.6334,15.75887]}, - {"t":1.29995, "x":3.36842, "y":6.45208, "heading":-1.07842, "vx":-3.31466, "vy":1.91639, "omega":1.09628, "ax":-0.9252, "ay":0.73674, "alpha":-2.48017, "fx":[-18.34889,-7.56826,-13.21945,-23.81327], "fy":[4.29385,10.16737,20.75919,14.90682]}, - {"t":1.32418, "x":3.28784, "y":6.49872, "heading":-1.05186, "vx":-3.33707, "vy":1.93424, "omega":1.03619, "ax":-1.1984, "ay":0.68948, "alpha":-3.01267, "fx":[-23.32635,-10.42681,-17.60099,-30.1834], "fy":[1.58568,9.12802,21.86675,14.33089]}, - {"t":1.34841, "x":3.20664, "y":6.54579, "heading":-1.02676, "vx":-3.36611, "vy":1.95094, "omega":0.9632, "ax":-1.6251, "ay":0.64529, "alpha":-3.89147, "fx":[-31.1944,-14.79783,-24.4107,-40.16708], "fy":[-2.35485,7.9498,24.33072,13.9791]}, - {"t":1.37263, "x":3.12461, "y":6.59324, "heading":-1.00342, "vx":-3.40548, "vy":1.96658, "omega":0.86892, "ax":-2.26264, "ay":0.60328, "alpha":-5.2459, "fx":[-43.0239,-21.3536,-34.6632,-54.90695], "fy":[-8.18392,6.57108,28.81946,13.84018]}, - {"t":1.39686, "x":3.04145, "y":6.64107, "heading":-0.98237, "vx":-3.4603, "vy":1.98119, "omega":0.74183, "ax":-3.15707, "ay":0.5803, "alpha":-7.20156, "fx":[-59.713,-30.85005,-49.25466,-74.98601], "fy":[-16.52493,5.20906,36.62796,14.17074]}, - {"t":1.42109, "x":2.95669, "y":6.68923, "heading":-0.9644, "vx":-3.53679, "vy":1.99525, "omega":0.56735, "ax":-4.29768, "ay":0.65983, "alpha":-9.831, "fx":[-81.27488,-43.85352,-67.85557,-99.42526], "fy":[-27.2183,5.44238,50.46456,16.2055]}, - {"t":1.44532, "x":2.86974, "y":6.73777, "heading":-0.95065, "vx":-3.64091, "vy":2.01124, "omega":0.32918, "ax":-5.56054, "ay":1.12258, "alpha":-13.57212, "fx":[-107.3127,-58.67469,-86.06402,-126.28118], "fy":[-39.57391,14.86868,77.34778,23.73659]}, - {"t":1.46954, "x":2.7799, "y":6.78682, "heading":-0.94268, "vx":-3.77562, "vy":2.03844, "omega":0.00036, "ax":-0.79753, "ay":-1.42914, "alpha":-0.01033, "fx":[-13.57064,-13.53004,-13.56086,-13.60145], "fy":[-24.34378,-24.31549,-24.2747,-24.303]}, - {"t":1.50461, "x":2.64701, "y":6.85743, "heading":-0.94266, "vx":-3.80359, "vy":1.98832, "omega":0.0, "ax":-0.15619, "ay":-0.29977, "alpha":0.0, "fx":[-2.65673,-2.65673,-2.65673,-2.65674], "fy":[-5.09902,-5.09901,-5.099,-5.09901]}, - {"t":1.53968, "x":2.51353, "y":6.92697, "heading":-0.94266, "vx":-3.80907, "vy":1.97781, "omega":0.0, "ax":-0.01602, "ay":-0.07344, "alpha":0.0, "fx":[-0.27252,-0.2725,-0.27251,-0.27253], "fy":[-1.24921,-1.2492,-1.24919,-1.2492]}, - {"t":1.57474, "x":2.37995, "y":6.99628, "heading":-0.94266, "vx":-3.80963, "vy":1.97523, "omega":0.0, "ax":8.23851, "ay":-4.27954, "alpha":-0.00146, "fx":[140.13104,140.13688,140.13846,140.13262], "fy":[-72.80159,-72.79166,-72.786,-72.79593]}, - {"t":1.60981, "x":2.25142, "y":7.06291, "heading":-0.94266, "vx":-3.52073, "vy":1.82516, "omega":-0.00005, "ax":9.04986, "ay":-4.69227, "alpha":-0.00002, "fx":[153.93541,153.93547,153.9355,153.93544], "fy":[-79.81434,-79.81423,-79.81417,-79.81428]}, - {"t":1.64488, "x":2.13352, "y":7.12403, "heading":-0.94267, "vx":-3.20338, "vy":1.66062, "omega":-0.00005, "ax":9.10118, "ay":-4.71836, "alpha":0.00008, "fx":[154.80866,154.80834,154.80818,154.8085], "fy":[-80.25747,-80.2581,-80.25841,-80.25779]}, - {"t":1.67995, "x":2.02679, "y":7.17936, "heading":-0.94267, "vx":-2.88423, "vy":1.49516, "omega":-0.00005, "ax":9.11969, "ay":-4.72777, "alpha":0.00012, "fx":[155.12365,155.12318,155.12295,155.12342], "fy":[-80.41729,-80.4182,-80.41865,-80.41775]}, - {"t":1.71501, "x":1.93125, "y":7.22889, "heading":-0.94267, "vx":-2.56442, "vy":1.32937, "omega":-0.00004, "ax":9.12922, "ay":-4.73261, "alpha":0.00014, "fx":[155.28589,155.28534,155.28508,155.28562], "fy":[-80.49961,-80.50066,-80.50118,-80.50014]}, - {"t":1.75008, "x":1.84694, "y":7.27259, "heading":-0.94267, "vx":-2.24429, "vy":1.16341, "omega":-0.00004, "ax":9.13503, "ay":-4.73557, "alpha":0.00015, "fx":[155.38477,155.38418,155.38389,155.38448], "fy":[-80.54978,-80.55091,-80.55149,-80.55035]}, - {"t":1.78515, "x":1.77385, "y":7.31048, "heading":-0.94267, "vx":-1.92395, "vy":0.99735, "omega":-0.00003, "ax":9.13895, "ay":-4.73755, "alpha":0.00016, "fx":[155.45133,155.45071,155.4504,155.45102], "fy":[-80.58355,-80.58475,-80.58535,-80.58415]}, - {"t":1.82021, "x":1.71201, "y":7.34254, "heading":-0.94267, "vx":-1.60348, "vy":0.83122, "omega":-0.00003, "ax":9.14176, "ay":-4.73898, "alpha":0.00016, "fx":[155.49919,155.49854,155.49822,155.49887], "fy":[-80.60782,-80.60907,-80.60969,-80.60845]}, - {"t":1.85528, "x":1.6614, "y":7.36877, "heading":-0.94267, "vx":-1.2829, "vy":0.66503, "omega":-0.00002, "ax":9.14388, "ay":-4.74006, "alpha":0.00017, "fx":[155.53525,155.53459,155.53426,155.53492], "fy":[-80.62612,-80.6274,-80.62804,-80.62676]}, - {"t":1.89035, "x":1.62203, "y":7.38918, "heading":-0.94268, "vx":-0.96225, "vy":0.49881, "omega":-0.00002, "ax":9.14553, "ay":-4.7409, "alpha":0.00017, "fx":[155.5634,155.56272,155.56238,155.56306], "fy":[-80.6404,-80.6417,-80.64236,-80.64105]}, - {"t":1.92541, "x":1.59391, "y":7.40376, "heading":-0.94268, "vx":-0.64154, "vy":0.33257, "omega":-0.00001, "ax":9.14686, "ay":-4.74158, "alpha":0.00017, "fx":[155.58598,155.58529,155.58495,155.58563], "fy":[-80.65186,-80.65318,-80.65384,-80.65252]}, - {"t":1.96048, "x":1.57704, "y":7.41251, "heading":-0.94268, "vx":-0.32079, "vy":0.16629, "omega":-0.00001, "ax":9.14795, "ay":-4.74213, "alpha":0.00017, "fx":[155.60449,155.6038,155.60345,155.60415], "fy":[-80.66125,-80.66259,-80.66326,-80.66192]}, - {"t":1.99555, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.48725, "ay":7.66008, "alpha":31.62453, "fx":[199.01044,137.64181,-3.35502,-100.77411], "fy":[5.64204,143.97036,199.07484,162.07218]}, + {"t":0.02534, "x":5.71427, "y":4.05336, "heading":3.14159, "vx":0.08837, "vy":0.19411, "omega":0.80139, "ax":4.42969, "ay":8.10886, "alpha":26.83778, "fx":[198.84886,137.07195,-1.74327,-38.81406], "fy":[8.90218,144.47371,199.05716,188.25044]}, + {"t":0.05068, "x":5.71793, "y":4.06088, "heading":-3.12129, "vx":0.20062, "vy":0.39959, "omega":1.48147, "ax":5.66188, "ay":8.22824, "alpha":22.01529, "fx":[198.3863,134.87446,-1.12531,45.38771], "fy":[15.55031,146.47515,199.00686,187.61064]}, + {"t":0.07602, "x":5.72483, "y":4.07365, "heading":-3.08374, "vx":0.3441, "vy":0.6081, "omega":2.03935, "ax":6.56355, "ay":8.01438, "alpha":19.56386, "fx":[197.28103,131.20131,-0.76773,109.93016], "fy":[25.51133,149.70255,198.92892,160.24066]}, + {"t":0.10136, "x":5.73566, "y":4.09163, "heading":-3.03207, "vx":0.51042, "vy":0.81119, "omega":2.53511, "ax":6.89129, "ay":7.95842, "alpha":18.5762, "fx":[194.89881,126.14819,0.69033,137.76096], "fy":[39.29289,153.88011,198.80813,138.67069]}, + {"t":0.1267, "x":5.7508, "y":4.11474, "heading":-2.96782, "vx":0.68505, "vy":1.01286, "omega":3.00585, "ax":6.86441, "ay":8.26602, "alpha":17.21531, "fx":[189.7361,119.60068,5.91157,142.45715], "fy":[58.84513,158.8538,198.52285,134.94048]}, + {"t":0.15204, "x":5.77037, "y":4.14306, "heading":-2.89165, "vx":0.859, "vy":1.22233, "omega":3.44209, "ax":6.60864, "ay":8.96938, "alpha":14.04677, "fx":[176.81565,111.07357,21.54326,131.21885], "fy":[89.83647,164.60495,197.07299,146.54685]}, + {"t":0.17738, "x":5.79426, "y":4.17692, "heading":-2.80443, "vx":1.02647, "vy":1.44962, "omega":3.79805, "ax":5.97748, "ay":10.11147, "alpha":5.30917, "fx":[129.43177,98.33678,67.24058,103.55756], "fy":[149.24577,171.60903,185.69632,167.66215]}, + {"t":0.20272, "x":5.82219, "y":4.2169, "heading":-2.70818, "vx":1.17794, "vy":1.70585, "omega":3.93258, "ax":2.25544, "ay":10.10698, "alpha":-18.83728, "fx":[-67.68189,0.23825,159.09183,58.74016], "fy":[184.62909,185.71672,115.17228,188.3962]}, + {"t":0.22807, "x":5.85276, "y":4.26337, "heading":-2.60853, "vx":1.23509, "vy":1.96197, "omega":3.45524, "ax":-2.31199, "ay":10.37478, "alpha":-17.92516, "fx":[-108.15036,-117.40681,77.40927,-6.01084], "fy":[163.97076,152.08144,179.02501,196.69331]}, + {"t":0.25341, "x":5.88332, "y":4.31642, "heading":-2.52097, "vx":1.17651, "vy":2.22487, "omega":3.001, "ax":-7.39319, "ay":8.54949, "alpha":-11.4404, "fx":[-153.35952,-170.71297,-76.24152,-92.64987], "fy":[123.11558,94.39106,179.17666,173.3806]}, + {"t":0.27875, "x":5.91076, "y":4.37555, "heading":-2.44493, "vx":0.98916, "vy":2.44152, "omega":2.71109, "ax":-10.33468, "ay":5.50276, "alpha":-5.19513, "fx":[-179.25883,-187.01473,-165.02236,-157.80095], "fy":[82.05107,60.71499,106.26341,117.88394]}, + {"t":0.30409, "x":5.9325, "y":4.43918, "heading":-2.37622, "vx":0.72727, "vy":2.58096, "omega":2.57945, "ax":-11.38081, "ay":3.22059, "alpha":-2.34262, "fx":[-191.14796,-193.67939,-188.63234,-185.39101], "fy":[50.18628,38.43336,57.93781,68.18524]}, + {"t":0.32943, "x":5.94728, "y":4.50562, "heading":-2.31086, "vx":0.43887, "vy":2.66258, "omega":2.52008, "ax":-11.75006, "ay":1.66141, "alpha":-1.54123, "fx":[-196.3286,-197.18611,-195.63192,-194.32463], "fy":[25.88875,17.32059,29.73733,37.83277]}, + {"t":0.35477, "x":5.95463, "y":4.57362, "heading":-2.247, "vx":0.14112, "vy":2.70468, "omega":2.48103, "ax":-11.87025, "ay":0.5349, "alpha":-1.81364, "fx":[-198.20471,-198.22904,-197.89167,-197.16013], "fy":[7.12905,-3.46881,10.95546,21.0501]}, + {"t":0.38011, "x":5.95439, "y":4.64233, "heading":-2.18413, "vx":-0.15968, "vy":2.71823, "omega":2.43507, "ax":-11.87568, "ay":-0.34907, "alpha":-2.70086, "fx":[-198.3866,-196.97326,-198.36901,-198.11849], "fy":[-8.25377,-24.36053,-2.92155,12.2607]}, + {"t":0.40545, "x":5.94653, "y":4.7111, "heading":-2.12242, "vx":-0.46062, "vy":2.70939, "omega":2.36663, "ax":-11.80686, "ay":-0.92047, "alpha":-3.70295, "fx":[-197.45433,-193.98996,-197.76424,-198.04997], "fy":[-18.93099,-40.71407,-10.87954,9.1497]}, + {"t":0.42458, "x":5.93556, "y":4.76276, "heading":-2.07715, "vx":-0.68648, "vy":2.69178, "omega":2.29579, "ax":-11.73116, "ay":-1.08329, "alpha":-5.30372, "fx":[-196.81027,-190.40646,-197.5275,-197.467], "fy":[-24.01269,-54.46979,-9.94748,16.19804]}, + {"t":0.44371, "x":5.92028, "y":4.81406, "heading":-2.03323, "vx":-0.91089, "vy":2.67105, "omega":2.19433, "ax":-11.63624, "ay":-1.21816, "alpha":-6.87746, "fx":[-196.01031,-185.9341,-197.27099,-196.66676], "fy":[-29.07524,-67.56648,-7.30918,22.72619]}, + {"t":0.46284, "x":5.90073, "y":4.86493, "heading":-1.99125, "vx":-1.13349, "vy":2.64775, "omega":2.06277, "ax":-11.52615, "ay":-1.31705, "alpha":-8.39331, "fx":[-195.07841,-180.83432,-196.90239,-195.72651], "fy":[-33.9247,-79.55093,-2.81888,28.47624]}, + {"t":0.48197, "x":5.87694, "y":4.91534, "heading":-1.95179, "vx":-1.35398, "vy":2.62256, "omega":1.90221, "ax":-11.4044, "ay":-1.37883, "alpha":-9.83529, "fx":[-194.03818,-175.39619,-196.26185,-194.72732], "fy":[-38.44421,-90.15133,3.38221,33.27552]}, + {"t":0.5011, "x":5.84895, "y":4.96526, "heading":-1.91541, "vx":-1.57214, "vy":2.59618, "omega":1.71406, "ax":-11.27391, "ay":-1.41092, "alpha":-11.17954, "fx":[-192.90204,-169.90916,-195.16771,-193.74379], "fy":[-42.57631,-99.19427,10.72479,36.96823]}, + {"t":0.52023, "x":5.81681, "y":5.01467, "heading":-1.88262, "vx":-1.78781, "vy":2.56919, "omega":1.5002, "ax":-11.13617, "ay":-1.4348, "alpha":-12.38515, "fx":[-191.64128,-164.54597,-193.51527,-192.83594], "fy":[-46.38669,-106.70297,18.05437,39.36545]}, + {"t":0.53936, "x":5.78057, "y":5.06355, "heading":-1.85392, "vx":-2.00084, "vy":2.54174, "omega":1.26327, "ax":-10.98944, "ay":-1.49289, "alpha":-13.38507, "fx":[-190.13191,-159.19582,-191.37858,-192.0486], "fy":[-50.16329,-112.99868,23.50156,40.1174]}, + {"t":0.55849, "x":5.74029, "y":5.1119, "heading":-1.82975, "vx":-2.21107, "vy":2.51318, "omega":1.00722, "ax":-10.82402, "ay":-1.66313, "alpha":-14.0615, "fx":[-188.04431,-153.28109,-188.98442,-191.41475], "fy":[-54.58244,-118.74754,24.06372,38.37212]}, + {"t":0.57762, "x":5.69601, "y":5.15967, "heading":-1.81048, "vx":-2.41813, "vy":2.48137, "omega":0.73823, "ax":-10.60508, "ay":-2.10596, "alpha":-14.15054, "fx":[-184.53028,-145.52976,-186.17478,-190.8915], "fy":[-61.10425,-124.90291,13.82952,31.75652]}, + {"t":0.59675, "x":5.64781, "y":5.20675, "heading":-1.79636, "vx":-2.621, "vy":2.44108, "omega":0.46753, "ax":-10.18945, "ay":-3.20276, "alpha":-12.89774, "fx":[-177.05615,-134.12031,-178.73257,-189.50391], "fy":[-73.0056,-131.78333,-21.11884,12.35358]}, + {"t":0.61588, "x":5.59581, "y":5.25287, "heading":-1.78742, "vx":-2.81592, "vy":2.37981, "omega":0.2208, "ax":-8.90534, "ay":-5.61028, "alpha":-8.20716, "fx":[-155.37561,-119.33152,-145.71959,-173.364], "fy":[-98.21719,-135.07479,-91.26144,-49.52942]}, + {"t":0.63501, "x":5.54031, "y":5.29736, "heading":-1.78319, "vx":-2.98628, "vy":2.27249, "omega":0.0638, "ax":-6.18204, "ay":-7.57561, "alpha":-1.37823, "fx":[-105.8448,-97.57218,-100.16263,-108.62662], "fy":[-125.98019,-131.33692,-126.75225,-121.05799]}, + {"t":0.65414, "x":5.48205, "y":5.33945, "heading":-1.78197, "vx":-3.10454, "vy":2.12757, "omega":0.03744, "ax":-4.40397, "ay":-6.6642, "alpha":-0.03656, "fx":[-73.50819,-73.28844,-73.31593,-73.53586], "fy":[-111.10205,-111.2042,-111.07594,-110.97365]}, + {"t":0.67327, "x":5.42186, "y":5.37893, "heading":-1.78126, "vx":-3.18878, "vy":2.00009, "omega":0.03674, "ax":-2.67836, "ay":-4.41696, "alpha":0.04954, "fx":[-44.50671,-44.77146,-44.78706,-44.52256], "fy":[-73.56761,-73.4825,-73.68979,-73.77484]}, + {"t":0.6924, "x":5.36037, "y":5.41638, "heading":-1.78055, "vx":-3.24002, "vy":1.91559, "omega":0.03769, "ax":-1.33496, "ay":-2.30994, "alpha":0.03418, "fx":[-22.15469,-22.32306,-22.35153,-22.18322], "fy":[-38.44741,-38.40659,-38.56386,-38.60468]}, + {"t":0.71153, "x":5.29814, "y":5.45261, "heading":-1.77983, "vx":-3.26556, "vy":1.8714, "omega":0.03834, "ax":-0.77077, "ay":-1.37315, "alpha":0.02741, "fx":[-12.76929,-12.90148,-12.92748,-12.79532], "fy":[-22.8399,-22.81051,-22.93951,-22.9689]}, + {"t":0.73065, "x":5.23553, "y":5.48815, "heading":-1.7791, "vx":-3.2803, "vy":1.84514, "omega":0.03886, "ax":-0.76551, "ay":-1.56377, "alpha":0.24985, "fx":[-12.03758,-13.24893,-13.48285,-12.27366], "fy":[-25.61595,-25.34765,-26.51854,-26.78686]}, + {"t":0.74978, "x":5.17264, "y":5.52316, "heading":-1.77836, "vx":-3.29495, "vy":1.81522, "omega":0.04364, "ax":1.3381, "ay":-4.02663, "alpha":8.13286, "fx":[50.15138,6.11883,-3.56993,36.52181], "fy":[-52.19017,-46.57461,-83.093,-86.62988]}, + {"t":0.76891, "x":5.10985, "y":5.55715, "heading":-1.77752, "vx":-3.26935, "vy":1.73819, "omega":0.19922, "ax":6.80613, "ay":-4.9949, "alpha":21.83373, "fx":[174.18911,128.44925,27.03638,124.14487], "fy":[-28.87736,-6.22251,-167.07937,-130.8714]}, + {"t":0.78804, "x":5.04856, "y":5.58949, "heading":-1.77371, "vx":-3.13915, "vy":1.64264, "omega":0.6169, "ax":2.15608, "ay":-5.27224, "alpha":11.23849, "fx":[78.43834,13.6894,-2.38958,54.02534], "fy":[-65.81397,-63.19064,-111.34924,-111.18928]}, + {"t":0.81665, "x":4.95964, "y":5.63432, "heading":-1.75606, "vx":-3.07747, "vy":1.49182, "omega":0.9384, "ax":2.97361, "ay":-3.985, "alpha":11.06488, "fx":[86.51009,31.18575,14.17853,66.40052], "fy":[-42.56544,-38.13961,-93.34114,-91.66599]}, + {"t":0.84526, "x":4.87282, "y":5.67537, "heading":-1.72922, "vx":-2.9924, "vy":1.37782, "omega":1.25494, "ax":3.0465, "ay":-2.94052, "alpha":9.50546, "fx":[79.73918,34.55576,22.51883,66.32103], "fy":[-27.90791,-23.20295,-72.84204,-72.11556]}, + {"t":0.87387, "x":4.78846, "y":5.71358, "heading":-1.69332, "vx":-2.90525, "vy":1.2937, "omega":1.52686, "ax":2.67961, "ay":-1.7139, "alpha":6.8584, "fx":[63.74908,31.62275,25.83205,57.4671], "fy":[-13.1034,-9.61233,-45.37778,-46.18582]}, + {"t":0.90247, "x":4.70644, "y":5.74989, "heading":-1.64964, "vx":-2.8286, "vy":1.24467, "omega":1.72306, "ax":1.89521, "ay":-0.3029, "alpha":3.32709, "fx":[40.15147,24.40712,23.0297,38.7804], "fy":[2.53398,3.92862,-12.84871,-13.81095]}, + {"t":0.93108, "x":4.6263, "y":5.78537, "heading":-1.60035, "vx":-2.77438, "vy":1.236, "omega":1.81824, "ax":0.88478, "ay":0.9571, "alpha":-0.22048, "fx":[14.20179,15.277,15.29558,14.22132], "fy":[15.44047,15.39645,16.46805,16.51284]}, + {"t":0.95969, "x":4.54729, "y":5.82112, "heading":-1.54833, "vx":-2.74907, "vy":1.26338, "omega":1.81193, "ax":-0.01322, "ay":1.74103, "alpha":-2.77606, "fx":[-7.03796,6.9028,6.46088,-7.20751], "fy":[22.25373,22.56017,35.78578,35.48885]}, + {"t":0.98829, "x":4.46865, "y":5.85798, "heading":-1.4965, "vx":-2.74945, "vy":1.31319, "omega":1.73252, "ax":-0.6337, "ay":2.04811, "alpha":-4.14176, "fx":[-20.49911,0.54864,-0.99496,-21.30826], "fy":[23.47371,25.25514,44.84623,42.98911]}, + {"t":1.0169, "x":4.38973, "y":5.89638, "heading":-1.44693, "vx":-2.76757, "vy":1.37178, "omega":1.61403, "ax":-0.97856, "ay":2.03158, "alpha":-4.59843, "fx":[-26.83402,-3.57099,-6.25424,-28.58956], "fy":[21.41301,24.6669,46.38031,43.00167]}, + {"t":1.04551, "x":4.31016, "y":5.93646, "heading":-1.40076, "vx":-2.79557, "vy":1.4299, "omega":1.48248, "ax":-1.1, "ay":1.83449, "alpha":-4.45438, "fx":[-27.88126,-5.66065,-9.20033,-30.60376], "fy":[18.0257,22.18208,43.19058,38.92193]}, + {"t":1.07412, "x":4.22974, "y":5.97811, "heading":-1.35835, "vx":-2.82704, "vy":1.48238, "omega":1.35506, "ax":-1.06149, "ay":1.5605, "alpha":-3.95723, "fx":[-25.55751,-6.16266,-10.11725,-28.94098], "fy":[14.51497,18.93904,37.54581,33.05136]}, + {"t":1.10272, "x":4.14843, "y":6.02116, "heading":-1.31959, "vx":-2.8574, "vy":1.52702, "omega":1.24185, "ax":-0.92807, "ay":1.27875, "alpha":-3.30292, "fx":[-21.53973,-5.6377,-9.56962,-25.13499], "fy":[11.49159,15.71049,31.15606,26.90659]}, + {"t":1.13133, "x":4.0663, "y":6.06537, "heading":-1.28406, "vx":-2.88395, "vy":1.5636, "omega":1.14736, "ax":-0.75682, "ay":1.02934, "alpha":-2.63615, "fx":[-17.10319,-4.61847,-8.21613,-20.52555], "fy":[9.16722,12.91645,25.15321,21.39729]}, + {"t":1.15994, "x":3.98349, "y":6.11052, "heading":-1.25124, "vx":-2.9056, "vy":1.59305, "omega":1.07195, "ax":-0.58823, "ay":0.8288, "alpha":-2.045, "fx":[-13.04055,-3.49839,-6.61268,-16.07048], "fy":[7.51599,10.70333,20.11387,16.9295]}, + {"t":1.18855, "x":3.90013, "y":6.15643, "heading":-1.22057, "vx":-2.92243, "vy":1.61676, "omega":1.01345, "ax":-0.44414, "ay":0.67772, "alpha":-1.56645, "fx":[-9.71249,-2.50239,-5.1144,-12.28523], "fy":[6.40337,9.04894,16.18854,13.548]}, + {"t":1.21715, "x":3.81635, "y":6.20296, "heading":-1.19158, "vx":-2.93514, "vy":1.63614, "omega":0.96864, "ax":-0.33192, "ay":0.56858, "alpha":-1.20275, "fx":[-7.18663,-1.72171,-3.88841,-9.33486], "fy":[5.67307,7.85499,13.2808,11.10328]}, + {"t":1.24576, "x":3.73224, "y":6.25, "heading":-1.16387, "vx":-2.94463, "vy":1.65241, "omega":0.93423, "ax":-0.25089, "ay":0.49169, "alpha":-0.93975, "fx":[-5.38704,-1.17076,-2.9819,-7.18902], "fy":[5.18968,7.00785,11.20115,9.38625]}, + {"t":1.27437, "x":3.6479, "y":6.29747, "heading":-1.13714, "vx":-2.95181, "vy":1.66648, "omega":0.90734, "ax":-0.19784, "ay":0.43827, "alpha":-0.76049, "fx":[-4.20547,-0.83637,-2.39282,-5.75691], "fy":[4.84798,6.40794,9.76238,8.2048]}, + {"t":1.30298, "x":3.56338, "y":6.34532, "heading":-1.11119, "vx":-2.95747, "vy":1.67901, "omega":0.88559, "ax":-0.17094, "ay":0.40167, "alpha":-0.65393, "fx":[-3.57381,-0.71375,-2.12677,-4.98359], "fy":[4.5628,5.978,8.82754,7.41418]}, + {"t":1.33158, "x":3.4787, "y":6.39352, "heading":-1.08585, "vx":-2.96236, "vy":1.6905, "omega":0.86688, "ax":-0.17295, "ay":0.37736, "alpha":-0.62146, "fx":[-3.51941,-0.83679,-2.24781,-4.9277], "fy":[4.24667,5.65982,8.33346,6.92196]}, + {"t":1.36019, "x":3.39389, "y":6.44203, "heading":-1.06105, "vx":-2.96731, "vy":1.7013, "omega":0.8491, "ax":-0.21543, "ay":0.36246, "alpha":-0.68537, "fx":[-4.23724,-1.31826,-2.94662,-5.86211], "fy":[3.77102,5.40299,8.31229,6.68216]}, + {"t":1.3888, "x":3.30891, "y":6.49085, "heading":-1.03676, "vx":-2.97347, "vy":1.71167, "omega":0.8295, "ax":-0.32669, "ay":0.35443, "alpha":-0.90478, "fx":[-6.22766,-2.42664,-4.66759,-8.46117], "fy":[2.88851,5.13868,8.92646,6.6788]}, + {"t":1.4174, "x":3.22372, "y":6.53996, "heading":-1.01303, "vx":-2.98281, "vy":1.72181, "omega":0.80361, "ax":-0.56873, "ay":0.34674, "alpha":-1.40828, "fx":[-10.59302,-4.75872,-8.38098,-14.18926], "fy":[1.04467,4.69756,10.51404,6.86371]}, + {"t":1.44601, "x":3.13815, "y":6.58936, "heading":-0.99005, "vx":-2.99908, "vy":1.73173, "omega":0.76333, "ax":-1.07389, "ay":0.31095, "alpha":-2.45443, "fx":[-19.67732,-9.66092,-16.18987,-26.07662], "fy":[-3.15794,3.48755,13.53281,6.87142]}, + {"t":1.47462, "x":3.05192, "y":6.63903, "heading":-0.96821, "vx":-3.02981, "vy":1.74062, "omega":0.69311, "ax":-2.11008, "ay":0.12164, "alpha":-4.49102, "fx":[-38.13463,-20.25568,-32.58011,-49.72569], "fy":[-13.59097,-0.88395,17.77593,4.80958]}, + {"t":1.50323, "x":2.96438, "y":6.68887, "heading":-0.94838, "vx":-3.09017, "vy":1.7441, "omega":0.56464, "ax":-4.04878, "ay":-0.66416, "alpha":-7.85167, "fx":[-71.17881,-42.70998,-65.42349,-90.65235], "fy":[-39.72737,-17.75501,19.04196,-5.84457]}, + {"t":1.53183, "x":2.87432, "y":6.73849, "heading":-0.93223, "vx":-3.20599, "vy":1.7251, "omega":0.34002, "ax":-6.62688, "ay":-2.49332, "alpha":-11.8812, "fx":[-108.24421,-76.5289,-117.36196,-139.73283], "fy":[-85.67455,-64.45791,12.64585,-28.7631]}, + {"t":1.56044, "x":2.7799, "y":6.78682, "heading":-0.9225, "vx":-3.39557, "vy":1.65378, "omega":0.00013, "ax":-1.98563, "ay":-4.28048, "alpha":-0.00367, "fx":[-33.09988,-33.08547,-33.09905,-33.11346], "fy":[-71.36465,-71.35664,-71.34253,-71.35055]}, + {"t":1.59645, "x":2.65635, "y":6.8436, "heading":-0.9225, "vx":-3.46707, "vy":1.49965, "omega":0.0, "ax":-0.2803, "ay":-0.65404, "alpha":0.0, "fx":[-4.67255,-4.67255,-4.67255,-4.67255], "fy":[-10.9026,-10.9026,-10.90259,-10.9026]}, + {"t":1.63245, "x":2.53133, "y":6.89717, "heading":-0.9225, "vx":-3.47716, "vy":1.4761, "omega":0.0, "ax":-0.03554, "ay":-0.08381, "alpha":0.0, "fx":[-0.59239,-0.59239,-0.59239,-0.59239], "fy":[-1.39711,-1.39711,-1.39711,-1.39711]}, + {"t":1.66846, "x":2.40611, "y":6.95026, "heading":-0.9225, "vx":-3.47844, "vy":1.47308, "omega":0.0, "ax":-0.00453, "ay":-0.01069, "alpha":0.0, "fx":[-0.07544,-0.07544,-0.07544,-0.07544], "fy":[-0.17821,-0.17821,-0.17821,-0.17821]}, + {"t":1.70447, "x":2.28086, "y":7.0033, "heading":-0.9225, "vx":-3.4786, "vy":1.4727, "omega":0.0, "ax":0.00397, "ay":-0.00332, "alpha":0.0, "fx":[0.06612,0.06612,0.06612,0.06612], "fy":[-0.05526,-0.05526,-0.05526,-0.05526]}, + {"t":1.74047, "x":2.15561, "y":7.05632, "heading":-0.9225, "vx":-3.47846, "vy":1.47258, "omega":0.0, "ax":8.772, "ay":-3.71372, "alpha":-0.00077, "fx":[146.2236,146.2263,146.22639,146.2237], "fy":[-61.9099,-61.90529,-61.90202,-61.90664]}, + {"t":1.77648, "x":2.03605, "y":7.10693, "heading":-0.9225, "vx":-3.16261, "vy":1.33886, "omega":-0.00003, "ax":10.91291, "ay":-4.61989, "alpha":0.00007, "fx":[181.91317,181.91296,181.91284,181.91306], "fy":[-77.01096,-77.01146,-77.01175,-77.01125]}, + {"t":1.81248, "x":1.92925, "y":7.15215, "heading":-0.9225, "vx":-2.76968, "vy":1.17252, "omega":-0.00003, "ax":10.96271, "ay":-4.64097, "alpha":0.00009, "fx":[182.74327,182.74298,182.74281,182.7431], "fy":[-77.36214,-77.36281,-77.36321,-77.36253]}, + {"t":1.84849, "x":1.83663, "y":7.19136, "heading":-0.9225, "vx":-2.37495, "vy":1.00542, "omega":-0.00002, "ax":10.97976, "ay":-4.64819, "alpha":0.0001, "fx":[183.02762,183.0273,183.02712,183.02744], "fy":[-77.48243,-77.48317,-77.4836,-77.48286]}, + {"t":1.8845, "x":1.75824, "y":7.22454, "heading":-0.9225, "vx":-1.97962, "vy":0.83805, "omega":-0.00002, "ax":10.98837, "ay":-4.65183, "alpha":0.0001, "fx":[183.17117,183.17084,183.17065,183.17098], "fy":[-77.54315,-77.54393,-77.54438,-77.5436]}, + {"t":1.9205, "x":1.69408, "y":7.2517, "heading":-0.9225, "vx":-1.58397, "vy":0.67056, "omega":-0.00002, "ax":10.99357, "ay":-4.65403, "alpha":0.0001, "fx":[183.25772,183.25738,183.25718,183.25752], "fy":[-77.57977,-77.58056,-77.58102,-77.58023]}, + {"t":1.95651, "x":1.64418, "y":7.27283, "heading":-0.9225, "vx":-1.18813, "vy":0.50298, "omega":-0.00001, "ax":10.99704, "ay":-4.6555, "alpha":0.0001, "fx":[183.31558,183.31524,183.31504,183.31538], "fy":[-77.60425,-77.60506,-77.60553,-77.60471]}, + {"t":1.99251, "x":1.60852, "y":7.28792, "heading":-0.9225, "vx":-0.79217, "vy":0.33536, "omega":-0.00001, "ax":10.99952, "ay":-4.65655, "alpha":0.00011, "fx":[183.35699,183.35664,183.35644,183.35679], "fy":[-77.62176,-77.62259,-77.62306,-77.62224]}, + {"t":2.02852, "x":1.58713, "y":7.29698, "heading":-0.9225, "vx":-0.39612, "vy":0.16769, "omega":0.0, "ax":11.00139, "ay":-4.65734, "alpha":0.00011, "fx":[183.38809,183.38774,183.38754,183.38789], "fy":[-77.63492,-77.63575,-77.63623,-77.6354]}, + {"t":2.06453, "x":1.58, "y":7.3, "heading":-0.9225, "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/ITofetch.traj b/src/main/deploy/choreo/ITofetch.traj index ee640f39..259e8aaf 100644 --- a/src/main/deploy/choreo/ITofetch.traj +++ b/src/main/deploy/choreo/ITofetch.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.302966594696045, "y":5.351784706115723, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.365372248154253, "y":4.933261807735684, "heading":4.1887902047863905, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.302966594696045, "y":5.351784706115723, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":37, "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}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.302966594696045 m", "val":5.302966594696045}, "y":{"exp":"5.351784706115723 m", "val":5.351784706115723}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.302966594696045 m", "val":5.302966594696045}, "y":{"exp":"5.351784706115723 m", "val":5.351784706115723}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":37, "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}, @@ -28,59 +28,58 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.30265,1.54956], + "waypoints":[0.0,0.28199,1.59662], "samples":[ - {"t":0.0, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.20905, "ay":9.71733, "alpha":8.96118, "fx":[22.63273,-40.7351,-97.10075,-35.09833], "fy":[173.66919,170.40933,145.74536,171.33207]}, - {"t":0.02018, "x":5.48202, "y":4.94119, "heading":-2.0944, "vx":-0.04457, "vy":0.19606, "omega":0.1808, "ax":-2.46304, "ay":9.66252, "alpha":8.85977, "fx":[18.10572,-43.86921,-99.77011,-42.04855], "fy":[174.17584,169.61249,143.9096,169.72892]}, - {"t":0.04035, "x":5.48062, "y":4.94711, "heading":-2.09075, "vx":-0.09427, "vy":0.39101, "omega":0.35956, "ax":-2.74523, "ay":9.59334, "alpha":8.74858, "fx":[12.82842,-47.56006,-102.74699,-49.30343], "fy":[174.61775,168.59551,141.77488,167.73152]}, - {"t":0.06053, "x":5.47816, "y":4.95696, "heading":-2.08349, "vx":-0.14965, "vy":0.58457, "omega":0.53608, "ax":-3.06019, "ay":9.50587, "alpha":8.62149, "fx":[6.67372,-51.86845,-106.06111,-56.95583], "fy":[174.93193,167.29834,139.2848,165.2534]}, - {"t":0.08071, "x":5.47452, "y":4.97069, "heading":-2.07268, "vx":-0.2114, "vy":0.77637, "omega":0.71003, "ax":-3.41321, "ay":9.39484, "alpha":8.47127, "fx":[-0.51746,-56.86746,-109.74302,-65.10297], "fy":[175.02559,165.64068,136.37033,162.17734]}, - {"t":0.10088, "x":5.46956, "y":4.98826, "heading":-2.05835, "vx":-0.28026, "vy":0.96592, "omega":0.88095, "ax":-3.8103, "ay":9.2531, "alpha":8.28885, "fx":[-8.94259,-62.64453,-113.82317,-73.8383], "fy":[174.76055,163.51425,132.94656,158.34889]}, - {"t":0.12106, "x":5.46313, "y":5.00963, "heading":-2.04058, "vx":-0.35714, "vy":1.15261, "omega":1.04818, "ax":-4.25805, "ay":9.071, "alpha":8.06241, "fx":[-18.84252,-69.30289,-118.3298,-83.23785], "fy":[173.92933,160.77147,128.90875,153.57089]}, - {"t":0.14123, "x":5.45505, "y":5.03474, "heading":-2.01943, "vx":-0.44305, "vy":1.33563, "omega":1.21085, "ax":-4.76325, "ay":8.83551, "alpha":7.77587, "fx":[-30.50061,-76.96149,-123.28522,-93.33899], "fy":[172.21904,157.20923,124.12796,147.60183]}, - {"t":0.16141, "x":5.44514, "y":5.06348, "heading":-1.995, "vx":-0.53916, "vy":1.5139, "omega":1.36774, "ax":-5.33204, "ay":8.52917, "alpha":7.40707, "fx":[-44.22592,-85.75046,-128.69928,-104.11024], "fy":[169.15909,152.54565,118.44688,140.16327]}, - {"t":0.18159, "x":5.43318, "y":5.09576, "heading":-1.9674, "vx":-0.64674, "vy":1.68599, "omega":1.51719, "ax":-5.96819, "ay":8.12893, "alpha":6.92577, "fx":[-60.29916,-95.79704,-134.55893,-115.41383], "fy":[164.05372,146.38811,111.67706,130.96398]}, - {"t":0.20176, "x":5.41892, "y":5.13144, "heading":-1.93679, "vx":-0.76716, "vy":1.85, "omega":1.65693, "ax":-6.66998, "ay":7.60536, "alpha":6.29309, "fx":[-78.84451,-107.1913,-140.81244,-126.96942], "fy":[155.91772,138.19285,103.60014,119.74917]}, - {"t":0.22194, "x":5.40208, "y":5.17031, "heading":-1.90336, "vx":-0.90173, "vy":2.00345, "omega":1.7839, "ax":-7.42486, "ay":6.92343, "alpha":5.46578, "fx":[-99.58117,-119.91331,-147.34761,-138.33695], "fy":[143.48231,127.2244,93.97769,106.37767]}, - {"t":0.24212, "x":5.38238, "y":5.21214, "heading":-1.86737, "vx":-1.05154, "vy":2.14314, "omega":1.89418, "ax":-8.20214, "ay":6.04736, "alpha":4.41038, "fx":[-121.45856,-133.69541,-153.96571,-148.94428], "fy":[125.42073,112.54514,82.57598,90.91355]}, - {"t":0.26229, "x":5.35949, "y":5.25661, "heading":-1.82915, "vx":-1.21703, "vy":2.26515, "omega":1.98316, "ax":-8.94659, "ay":4.95296, "alpha":3.12706, "fx":[-142.37856,-147.80201,-160.35812,-158.17701], "fy":[100.97438,93.11147,69.21281,73.6953]}, - {"t":0.28247, "x":5.33311, "y":5.30332, "heading":-1.78914, "vx":-1.39754, "vy":2.36509, "omega":2.04626, "ax":-9.58131, "ay":3.64701, "alpha":1.66627, "fx":[-159.5009,-160.78316,-166.10044,-165.51656], "fy":[70.8644,68.11271,53.82863,55.33287]}, - {"t":0.30265, "x":5.30297, "y":5.35178, "heading":-1.74785, "vx":-1.59085, "vy":2.43867, "omega":2.07987, "ax":-9.89947, "ay":2.75525, "alpha":-1.24901, "fx":[-170.10776,-169.89763,-166.51937,-167.02418], "fy":[40.70863,41.40562,53.40792,51.94196]}, - {"t":0.33827, "x":5.24001, "y":5.44041, "heading":-1.67375, "vx":-1.94353, "vy":2.53683, "omega":2.03538, "ax":-9.89306, "ay":2.18627, "alpha":-5.96393, "fx":[-174.45133,-174.34203,-159.31728,-165.0018], "fy":[11.10646,8.45799,71.418,57.76887]}, - {"t":0.3739, "x":5.16449, "y":5.53218, "heading":-1.60124, "vx":-2.29598, "vy":2.61472, "omega":1.82291, "ax":-9.33786, "ay":1.12746, "alpha":-15.22306, "fx":[-172.32893,-162.05614,-137.30792,-163.64466], "fy":[-28.46143,-62.89962,106.93575,61.13629]}, - {"t":0.40952, "x":5.07677, "y":5.62604, "heading":-1.5363, "vx":-2.62865, "vy":2.65488, "omega":1.28057, "ax":-9.2435, "ay":0.58062, "alpha":-16.27017, "fx":[-169.56121,-154.75525,-138.47711,-166.12369], "fy":[-40.35538,-77.34053,104.29892,52.90139]}, - {"t":0.44515, "x":4.97725, "y":5.721, "heading":-1.49068, "vx":-2.95796, "vy":2.67557, "omega":0.70093, "ax":-9.27581, "ay":-0.62357, "alpha":-15.27243, "fx":[-163.94116,-143.01908,-153.90082,-170.25424], "fy":[-56.42614,-94.51622,75.68539,32.83013]}, - {"t":0.48078, "x":4.86599, "y":5.81592, "heading":-1.4657, "vx":-3.28842, "vy":2.65335, "omega":0.15683, "ax":-7.20084, "ay":-6.62764, "alpha":-4.21327, "fx":[-121.73962,-102.61146,-125.48655,-140.09898], "fy":[-116.87152,-132.50616,-108.93565,-92.62361]}, - {"t":0.5164, "x":4.74426, "y":5.90624, "heading":-1.46012, "vx":-3.54496, "vy":2.41724, "omega":0.00673, "ax":-4.92756, "ay":-7.82322, "alpha":-0.04184, "fx":[-83.85605,-83.59958,-83.7766,-84.03307], "fy":[-133.07904,-133.21088,-133.06249,-132.93031]}, - {"t":0.55203, "x":4.61484, "y":5.98739, "heading":-1.45988, "vx":-3.72051, "vy":2.13853, "omega":0.00524, "ax":-3.34595, "ay":-6.23547, "alpha":-0.00153, "fx":[-56.91623,-56.90722,-56.91116,-56.92017], "fy":[-106.06532,-106.06761,-106.06181,-106.05952]}, - {"t":0.58765, "x":4.48017, "y":6.05962, "heading":-1.45969, "vx":-3.83971, "vy":1.91638, "omega":0.00519, "ax":-1.12831, "ay":-2.32222, "alpha":0.00001, "fx":[-19.19223,-19.19227,-19.19226,-19.19223], "fy":[-39.50024,-39.50024,-39.50027,-39.50027]}, - {"t":0.62328, "x":4.34266, "y":6.12642, "heading":-1.45951, "vx":-3.87991, "vy":1.83365, "omega":0.00519, "ax":-0.23147, "ay":-0.49265, "alpha":0.00006, "fx":[-3.93717,-3.93747,-3.93744,-3.93714], "fy":[-8.37963,-8.37966,-8.37996,-8.37993]}, - {"t":0.65891, "x":4.20429, "y":6.19144, "heading":-1.45932, "vx":-3.88815, "vy":1.8161, "omega":0.00519, "ax":-0.04565, "ay":-0.09785, "alpha":0.00001, "fx":[-0.77648,-0.77655,-0.77655,-0.77648], "fy":[-1.66442,-1.66442,-1.66449,-1.66449]}, - {"t":0.69453, "x":4.06574, "y":6.25608, "heading":-1.45914, "vx":-3.88978, "vy":1.81261, "omega":0.00519, "ax":-0.00902, "ay":-0.01936, "alpha":0.0, "fx":[-0.15345,-0.15346,-0.15346,-0.15345], "fy":[-0.32939,-0.32939,-0.3294,-0.32939]}, - {"t":0.73016, "x":3.92716, "y":6.32064, "heading":-1.45895, "vx":-3.8901, "vy":1.81192, "omega":0.00519, "ax":-0.00178, "ay":-0.00383, "alpha":0.0, "fx":[-0.03036,-0.03035,-0.03035,-0.03036], "fy":[-0.06517,-0.06517,-0.06516,-0.06516]}, - {"t":0.76578, "x":3.78857, "y":6.38519, "heading":-1.45877, "vx":-3.89017, "vy":1.81179, "omega":0.00519, "ax":-0.00035, "ay":-0.00076, "alpha":0.0, "fx":[-0.00601,-0.006,-0.006,-0.00602], "fy":[-0.0129,-0.0129,-0.01289,-0.01289]}, - {"t":0.80141, "x":3.64998, "y":6.44973, "heading":-1.45858, "vx":-3.89018, "vy":1.81176, "omega":0.00519, "ax":-0.00007, "ay":-0.00015, "alpha":0.0, "fx":[-0.00122,-0.0012,-0.0012,-0.00122], "fy":[-0.0026,-0.0026,-0.00258,-0.00258]}, - {"t":0.83704, "x":3.51139, "y":6.51428, "heading":-1.4584, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00002, "ay":-0.00004, "alpha":0.0, "fx":[-0.00035,-0.00034,-0.00034,-0.00035], "fy":[-0.00074,-0.00073,-0.00072,-0.00072]}, - {"t":0.87266, "x":3.3728, "y":6.57883, "heading":-1.45821, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00003, "ay":-0.00007, "alpha":0.0, "fx":[-0.0006,-0.00058,-0.00058,-0.0006], "fy":[-0.00127,-0.00126,-0.00125,-0.00125]}, - {"t":0.90829, "x":3.2342, "y":6.64337, "heading":-1.45803, "vx":-3.89018, "vy":1.81175, "omega":0.00519, "ax":-0.00016, "ay":-0.00035, "alpha":0.0, "fx":[-0.00276,-0.00275,-0.00275,-0.00276], "fy":[-0.00591,-0.00591,-0.0059,-0.0059]}, - {"t":0.94391, "x":3.09561, "y":6.70792, "heading":-1.45784, "vx":-3.89019, "vy":1.81174, "omega":0.00519, "ax":-0.00082, "ay":-0.00175, "alpha":0.0, "fx":[-0.01388,-0.01386,-0.01387,-0.01388], "fy":[-0.02978,-0.02978,-0.02977,-0.02977]}, - {"t":0.97954, "x":2.95702, "y":6.77246, "heading":-1.45766, "vx":-3.89022, "vy":1.81168, "omega":0.00519, "ax":-0.00412, "ay":-0.00885, "alpha":0.0, "fx":[-0.0701,-0.07009,-0.07009,-0.0701], "fy":[-0.15054,-0.15054,-0.15053,-0.15053]}, - {"t":1.01517, "x":2.81842, "y":6.837, "heading":-1.45747, "vx":-3.89036, "vy":1.81136, "omega":0.00519, "ax":-0.02082, "ay":-0.04474, "alpha":0.00001, "fx":[-0.3541,-0.35413,-0.35413,-0.35409], "fy":[-0.761,-0.761,-0.76104,-0.76103]}, - {"t":1.05079, "x":2.67981, "y":6.9015, "heading":-1.45729, "vx":-3.89111, "vy":1.80977, "omega":0.00519, "ax":-0.10366, "ay":-0.22673, "alpha":0.00302, "fx":[-1.75679,-1.77139,-1.76971,-1.75511], "fy":[-3.84847,-3.85013,-3.86472,-3.86306]}, - {"t":1.08642, "x":2.54112, "y":6.96583, "heading":-1.4571, "vx":-3.8948, "vy":1.80169, "omega":0.0053, "ax":1.60058, "ay":-2.01146, "alpha":5.45083, "fx":[40.35521,12.58999,14.92197,41.03472], "fy":[-19.18928,-23.42536,-49.52988,-44.71284]}, - {"t":1.12204, "x":2.40338, "y":7.02874, "heading":-1.45691, "vx":-3.83778, "vy":1.73003, "omega":0.19949, "ax":7.95189, "ay":-2.9313, "alpha":19.87865, "fx":[171.00939,158.83989,63.35958,147.82852], "fy":[21.81946,28.21315,-159.53476,-89.94043]}, - {"t":1.15767, "x":2.2717, "y":7.08852, "heading":-1.44981, "vx":-3.55448, "vy":1.6256, "omega":0.90768, "ax":8.16781, "ay":-2.90688, "alpha":19.47796, "fx":[172.45037,166.21588,67.20039,149.86179], "fy":[22.81506,28.38905,-160.0403,-88.94449]}, - {"t":1.1933, "x":2.15026, "y":7.14458, "heading":-1.41747, "vx":-3.2635, "vy":1.52204, "omega":1.60161, "ax":9.09212, "ay":-4.19564, "alpha":7.61169, "fx":[172.23898,163.36465,128.21826,154.79551], "fy":[-27.40972,-59.30985,-118.04261,-80.70457]}, - {"t":1.22892, "x":2.03976, "y":7.19615, "heading":-1.36041, "vx":-2.93958, "vy":1.37256, "omega":1.87278, "ax":9.31206, "ay":-4.32478, "alpha":0.86108, "fx":[160.80777,158.77433,155.87436,158.12577], "fy":[-68.26798,-72.79752,-78.86865,-74.319]}, - {"t":1.26455, "x":1.94094, "y":7.2423, "heading":-1.29369, "vx":-2.60783, "vy":1.21849, "omega":1.90346, "ax":9.29955, "ay":-4.32189, "alpha":-2.61043, "fx":[150.17046,158.14255,165.26456,159.15312], "fy":[-89.6424,-74.81558,-57.2502,-72.34833]}, - {"t":1.30017, "x":1.85394, "y":7.28297, "heading":-1.22588, "vx":-2.27652, "vy":1.06452, "omega":1.81046, "ax":9.25181, "ay":-4.30155, "alpha":-4.63169, "fx":[142.62858,158.70132,169.46461,158.68789], "fy":[-101.43174,-73.94679,-43.74196,-73.55179]}, - {"t":1.3358, "x":1.77871, "y":7.31816, "heading":-1.16138, "vx":-1.94692, "vy":0.91127, "omega":1.64545, "ax":9.20374, "ay":-4.28316, "alpha":-5.93642, "fx":[137.61545,159.66292,171.68775,157.24566], "fy":[-108.27603,-72.04326,-34.37259,-76.72946]}, - {"t":1.37143, "x":1.71519, "y":7.34791, "heading":-1.10276, "vx":-1.61903, "vy":0.75868, "omega":1.43396, "ax":9.16006, "ay":-4.27043, "alpha":-6.86323, "fx":[134.35111,160.71314,172.98408,155.19172], "fy":[-112.39723,-69.80235,-27.44135,-80.9143]}, - {"t":1.40705, "x":1.66332, "y":7.37223, "heading":-1.05167, "vx":-1.29269, "vy":0.60654, "omega":1.18945, "ax":9.12057, "ay":-4.26247, "alpha":-7.57661, "fx":[132.23086,161.70575,173.78413,152.83247], "fy":[-114.95345,-67.56847,-22.11603,-85.37551]}, - {"t":1.44268, "x":1.62306, "y":7.39113, "heading":-1.0093, "vx":-0.96776, "vy":0.45469, "omega":0.91952, "ax":9.08495, "ay":-4.25746, "alpha":-8.15484, "fx":[130.82588,162.56774,174.29545,150.44068], "fy":[-116.60168,-65.54223,-17.93386,-89.595]}, - {"t":1.4783, "x":1.59434, "y":7.40463, "heading":-0.97654, "vx":-0.6441, "vy":0.30301, "omega":0.629, "ax":9.05335, "ay":-4.25399, "alpha":-8.63251, "fx":[129.83585,163.26145,174.62991,148.25218], "fy":[-117.74277,-63.85414,-14.60853,-93.23094]}, - {"t":1.51393, "x":1.57714, "y":7.41272, "heading":-0.95413, "vx":-0.32156, "vy":0.15146, "omega":0.32146, "ax":9.02613, "ay":-4.25128, "alpha":-9.02315, "fx":[129.04925,163.76668,174.85236,146.45941], "fy":[-118.63545,-62.59549,-11.94972,-96.07188]}, - {"t":1.54956, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.36537, "y":4.93326, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.33825, "ay":10.78681, "alpha":17.42556, "fx":[98.32084,-7.89077,-97.24014,96.042], "fy":[173.06566,198.94005,173.53589,173.70229]}, + {"t":0.02014, "x":5.36564, "y":4.93545, "heading":-2.0944, "vx":0.02696, "vy":0.21727, "omega":0.35099, "ax":0.98965, "ay":10.89848, "alpha":16.68775, "fx":[93.53971,-11.32764,-99.51187,83.28783], "fy":[175.65719,198.74837,172.20667,180.07778]}, + {"t":0.04028, "x":5.36639, "y":4.94204, "heading":-2.08733, "vx":0.04689, "vy":0.43679, "omega":0.68711, "ax":0.56841, "ay":11.00252, "alpha":15.94211, "fx":[87.29458,-15.94712,-102.3803,68.93357], "fy":[178.79805,198.40055,170.47242,185.95572]}, + {"t":0.06043, "x":5.36745, "y":4.95307, "heading":-2.07349, "vx":0.05834, "vy":0.6584, "omega":1.00822, "ax":0.05735, "ay":11.09367, "alpha":15.15684, "fx":[79.20425,-21.90127,-105.8927,52.4134], "fy":[182.47325,197.79473,168.25907,191.17755]}, + {"t":0.08057, "x":5.36863, "y":4.96858, "heading":-2.05318, "vx":0.05949, "vy":0.88185, "omega":1.31351, "ax":-0.56752, "ay":11.16169, "alpha":14.29371, "fx":[68.68638,-29.41207,-110.10198,32.98634], "fy":[186.62348,196.77236,165.46962,195.3745]}, + {"t":0.10071, "x":5.36972, "y":4.9886, "heading":-2.02672, "vx":0.04806, "vy":1.10667, "omega":1.60141, "ax":-1.33778, "ay":11.18705, "alpha":13.30906, "fx":[54.83728,-38.80515,-115.06966,9.8369], "fy":[191.07429,195.07981,161.97439,197.80277]}, + {"t":0.12085, "x":5.37041, "y":5.01316, "heading":-1.99447, "vx":0.02112, "vy":1.332, "omega":1.86948, "ax":-2.29213, "ay":11.1345, "alpha":12.15058, "fx":[36.23712,-50.56228,-120.86738,-17.64219], "fy":[195.36008,192.29358,157.59769,197.17567]}, + {"t":0.14099, "x":5.37037, "y":5.04225, "heading":-1.95681, "vx":-0.02505, "vy":1.55627, "omega":2.11422, "ax":-3.47307, "ay":10.94386, "alpha":10.73731, "fx":[10.68446,-65.39858,-127.57432,-49.28938], "fy":[198.28206,187.6677,152.10009,191.66583]}, + {"t":0.16114, "x":5.36916, "y":5.07582, "heading":-1.91423, "vx":-0.09501, "vy":1.7767, "omega":2.33049, "ax":-4.91876, "ay":10.51669, "alpha":8.90014, "fx":[-24.89285,-84.34906,-135.26422,-83.46714], "fy":[196.84682,179.81082,145.15827,179.41657]}, + {"t":0.18128, "x":5.36625, "y":5.11374, "heading":-1.86728, "vx":-0.19408, "vy":1.98853, "omega":2.50976, "ax":-6.63806, "ay":9.69385, "alpha":6.30645, "fx":[-73.02326,-108.73545,-143.96898,-116.88539], "fy":[184.29857,166.00275,136.35276,159.71347]}, + {"t":0.20142, "x":5.361, "y":5.15576, "heading":-1.81673, "vx":-0.32778, "vy":2.18378, "omega":2.63678, "ax":-8.52116, "ay":8.25039, "alpha":2.62434, "fx":[-129.42087,-139.39385,-153.60736,-145.75251], "fy":[149.97834,140.93511,125.18447,134.0219]}, + {"t":0.22156, "x":5.35267, "y":5.20142, "heading":-1.76362, "vx":-0.49942, "vy":2.34996, "omega":2.68964, "ax":-10.20684, "ay":6.05701, "alpha":-1.88087, "fx":[-175.83929,-173.16194,-163.89796,-167.67338], "fy":[91.22459,95.96359,111.13003,105.55188]}, + {"t":0.2417, "x":5.34054, "y":5.24998, "heading":-1.70945, "vx":-0.705, "vy":2.47196, "omega":2.65176, "ax":-11.23627, "ay":3.37185, "alpha":-6.54531, "fx":[-196.38903,-195.98801,-174.35155,-182.48408], "fy":[26.9507,26.72649,93.62575,77.52559]}, + {"t":0.26185, "x":5.32406, "y":5.30045, "heading":-1.65604, "vx":-0.93133, "vy":2.53988, "omega":2.51992, "ax":-11.49796, "ay":0.85402, "alpha":-10.20558, "fx":[-197.20931,-193.03082,-184.81765,-191.60397], "fy":[-21.70095,-43.41882,70.59482,51.46909]}, + {"t":0.28199, "x":5.30297, "y":5.35178, "heading":-1.60528, "vx":-1.16292, "vy":2.55708, "omega":2.31436, "ax":-11.34954, "ay":-0.06074, "alpha":-12.99525, "fx":[-194.2824,-183.17731,-185.37572,-193.93017], "fy":[-42.35329,-76.41324,70.81131,43.90487]}, + {"t":0.31955, "x":5.25128, "y":5.44779, "heading":-1.51835, "vx":-1.58922, "vy":2.5548, "omega":1.82625, "ax":-11.16685, "ay":-0.32771, "alpha":-14.84387, "fx":[-191.33751,-177.34466,-181.28157,-194.62057], "fy":[-53.47901,-88.248,79.93527,39.94049]}, + {"t":0.35711, "x":5.18371, "y":5.54352, "heading":-1.44975, "vx":-2.00865, "vy":2.54249, "omega":1.2687, "ax":-11.02516, "ay":-0.83264, "alpha":-15.75449, "fx":[-187.42785,-170.28588,-181.57916,-195.84369], "fy":[-64.88313,-99.88494,77.7645,31.4845]}, + {"t":0.39467, "x":5.10049, "y":5.63843, "heading":-1.4021, "vx":-2.42277, "vy":2.51121, "omega":0.67695, "ax":-10.84569, "ay":-2.24015, "alpha":-14.86867, "fx":[-179.28323,-155.68311,-190.888,-197.31554], "fy":[-82.90396,-119.20802,44.21407,8.5288]}, + {"t":0.43223, "x":5.00183, "y":5.73117, "heading":-1.37667, "vx":-2.83014, "vy":2.42707, "omega":0.11846, "ax":-7.95837, "ay":-8.34894, "alpha":-2.94045, "fx":[-131.21202,-118.07983,-135.16089,-146.1963], "fy":[-142.0539,-152.29291,-136.45718,-125.88759]}, + {"t":0.46979, "x":4.88992, "y":5.81645, "heading":-1.37222, "vx":-3.12907, "vy":2.11348, "omega":0.00802, "ax":-5.5511, "ay":-9.22155, "alpha":-0.03265, "fx":[-92.55306,-92.36051,-92.51535,-92.70784], "fy":[-153.73246,-153.82446,-153.70528,-153.61313]}, + {"t":0.50735, "x":4.76847, "y":5.88932, "heading":-1.37192, "vx":-3.33757, "vy":1.76711, "omega":0.00679, "ax":-2.71609, "ay":-5.531, "alpha":-0.00116, "fx":[-45.27778,-45.2716,-45.27406,-45.28024], "fy":[-92.20138,-92.20161,-92.19675,-92.19652]}, + {"t":0.54492, "x":4.64119, "y":5.9518, "heading":-1.37167, "vx":-3.43959, "vy":1.55936, "omega":0.00675, "ax":-0.4024, "ay":-0.89921, "alpha":0.00016, "fx":[-6.70753,-6.70829,-6.70813,-6.70737], "fy":[-14.98888,-14.98903,-14.98979,-14.98963]}, + {"t":0.58248, "x":4.51171, "y":6.00973, "heading":-1.37142, "vx":-3.45471, "vy":1.52558, "omega":0.00675, "ax":-0.04708, "ay":-0.1068, "alpha":0.00003, "fx":[-0.78477,-0.78492,-0.78489,-0.78474], "fy":[-1.78014,-1.78016,-1.78032,-1.78029]}, + {"t":0.62004, "x":4.38192, "y":6.06696, "heading":-1.37116, "vx":-3.45648, "vy":1.52157, "omega":0.00676, "ax":-0.00553, "ay":-0.01257, "alpha":0.0, "fx":[-0.09223,-0.09222,-0.09222,-0.09222], "fy":[-0.20954,-0.20954,-0.20954,-0.20954]}, + {"t":0.6576, "x":4.25209, "y":6.1241, "heading":-1.37091, "vx":-3.45668, "vy":1.5211, "omega":0.00676, "ax":-0.00065, "ay":-0.00148, "alpha":-0.00001, "fx":[-0.01086,-0.01083,-0.01084,-0.01086], "fy":[-0.02464,-0.02464,-0.02461,-0.02462]}, + {"t":0.69516, "x":4.12225, "y":6.18124, "heading":-1.37065, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":-0.00008, "ay":-0.00017, "alpha":-0.00001, "fx":[-0.00128,-0.00125,-0.00126,-0.00129], "fy":[-0.00288,-0.00288,-0.00285,-0.00285]}, + {"t":0.73272, "x":3.99241, "y":6.23837, "heading":-1.3704, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00001, "fx":[-0.00016,-0.00013,-0.00013,-0.00016], "fy":[-0.00033,-0.00032,-0.00029,-0.0003]}, + {"t":0.77028, "x":3.86258, "y":6.2955, "heading":-1.37015, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00002,0.0,0.0,-0.00003], "fy":[-0.00002,-0.00002,0.00001,0.00001]}, + {"t":0.80784, "x":3.73274, "y":6.35263, "heading":-1.36989, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00005,0.00004]}, + {"t":0.8454, "x":3.6029, "y":6.40976, "heading":-1.36964, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]}, + {"t":0.88296, "x":3.47306, "y":6.46689, "heading":-1.36939, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]}, + {"t":0.92052, "x":3.34323, "y":6.52403, "heading":-1.36913, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]}, + {"t":0.95809, "x":3.21339, "y":6.58116, "heading":-1.36888, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00005,0.00004]}, + {"t":0.99565, "x":3.08355, "y":6.63829, "heading":-1.36862, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00003,0.0,-0.00001,-0.00004], "fy":[-0.00004,-0.00003,0.0,-0.00001]}, + {"t":1.03321, "x":2.95371, "y":6.69542, "heading":-1.36837, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00001, "fx":[-0.0002,-0.00017,-0.00018,-0.00021], "fy":[-0.00043,-0.00042,-0.00039,-0.0004]}, + {"t":1.07077, "x":2.82388, "y":6.75255, "heading":-1.36812, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":-0.0001, "ay":-0.00022, "alpha":-0.00001, "fx":[-0.00167,-0.00164,-0.00165,-0.00167], "fy":[-0.00376,-0.00375,-0.00372,-0.00373]}, + {"t":1.10833, "x":2.69404, "y":6.80968, "heading":-1.36786, "vx":-3.45671, "vy":1.52103, "omega":0.00675, "ax":-0.00085, "ay":-0.00192, "alpha":-0.00001, "fx":[-0.01413,-0.0141,-0.0141,-0.01413], "fy":[-0.03208,-0.03207,-0.03205,-0.03205]}, + {"t":1.14589, "x":2.5642, "y":6.86681, "heading":-1.36761, "vx":-3.45675, "vy":1.52096, "omega":0.00675, "ax":-0.0072, "ay":-0.01636, "alpha":0.0, "fx":[-0.11996,-0.11996,-0.11996,-0.11996], "fy":[-0.27273,-0.27273,-0.27273,-0.27273]}, + {"t":1.18345, "x":2.43436, "y":6.92393, "heading":-1.36736, "vx":-3.45702, "vy":1.52034, "omega":0.00675, "ax":-0.06091, "ay":-0.13913, "alpha":0.00036, "fx":[-1.01462,-1.01632,-1.01598,-1.01427], "fy":[-2.31828,-2.31863,-2.32033,-2.31998]}, + {"t":1.22101, "x":2.30447, "y":6.98094, "heading":-1.3671, "vx":-3.4593, "vy":1.51511, "omega":0.00677, "ax":-0.15975, "ay":-1.32101, "alpha":0.88889, "fx":[-0.96359,-5.26094,-4.35308,-0.07405], "fy":[-19.49592,-20.35367,-24.54017,-23.69245]}, + {"t":1.25857, "x":2.17442, "y":7.03691, "heading":-1.36685, "vx":-3.4653, "vy":1.4655, "omega":0.04015, "ax":9.15451, "ay":-3.26707, "alpha":22.58334, "fx":[192.11094,174.84769,71.63979,171.80666], "fy":[37.35986,21.92793,-181.56922,-95.56031]}, + {"t":1.29613, "x":2.05072, "y":7.08966, "heading":-1.36534, "vx":-3.12145, "vy":1.34278, "omega":0.88841, "ax":9.46171, "ay":-3.00963, "alpha":22.58626, "fx":[193.74067,186.41718,76.21186,174.51865], "fy":[39.82576,35.60064,-182.23085,-93.87182]}, + {"t":1.33369, "x":1.94015, "y":7.13797, "heading":-1.33197, "vx":-2.76606, "vy":1.22974, "omega":1.73677, "ax":10.47838, "ay":-4.52087, "alpha":11.36449, "fx":[198.16364,187.49701,134.41004,178.60757], "fy":[-7.62451,-61.48964,-145.66541,-86.66306]}, + {"t":1.37126, "x":1.84364, "y":7.18097, "heading":-1.26674, "vx":-2.37248, "vy":1.05993, "omega":2.16363, "ax":10.90805, "ay":-4.78128, "alpha":-1.43772, "fx":[177.7531,181.71734,185.66404,182.19325], "fy":[-88.69767,-80.35221,-70.67001,-79.08645]}, + {"t":1.40882, "x":1.76222, "y":7.21741, "heading":-1.18547, "vx":-1.96277, "vy":0.88034, "omega":2.10963, "ax":10.76029, "ay":-4.72261, "alpha":-7.02503, "fx":[157.38924,182.41201,196.08828,181.58583], "fy":[-121.58306,-79.46072,-33.19546,-80.65507]}, + {"t":1.44638, "x":1.69609, "y":7.24714, "heading":-1.10623, "vx":-1.5586, "vy":0.70296, "omega":1.84576, "ax":10.59223, "ay":-4.67521, "alpha":-9.88779, "fx":[145.8387,184.20351,198.65991,177.56722], "fy":[-135.41729,-75.52706,-11.49334,-89.29648]}, + {"t":1.48394, "x":1.64502, "y":7.27025, "heading":-1.0369, "vx":-1.16075, "vy":0.52735, "omega":1.47436, "ax":10.43802, "ay":-4.66674, "alpha":-11.74506, "fx":[139.82738,186.02802,199.03918,171.09232], "fy":[-141.73095,-71.09751,2.92568,-101.26674]}, + {"t":1.5215, "x":1.60878, "y":7.28677, "heading":-0.98152, "vx":-0.76869, "vy":0.35206, "omega":1.03321, "ax":10.29502, "ay":-4.67891, "alpha":-13.1806, "fx":[136.70538,187.54592,198.67388,163.527], "fy":[-144.81818,-67.11385,13.12644,-113.17532]}, + {"t":1.55906, "x":1.58717, "y":7.29669, "heading":-0.94271, "vx":-0.382, "vy":0.17632, "omega":0.53813, "ax":10.17001, "ay":-4.69419, "alpha":-14.32693, "fx":[134.91603,188.63887,198.09066,156.47144], "fy":[-146.53658,-64.06617,20.43019,-122.8268]}, + {"t":1.59662, "x":1.58, "y":7.3, "heading":-0.9225, "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/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj index 0767f9be..6fda7d82 100644 --- a/src/main/deploy/choreo/JTofetch.traj +++ b/src/main/deploy/choreo/JTofetch.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.048037528991699, "y":5.385420322418213, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.048037528991699, "y":5.385420322418213, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "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}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.048037528991699 m", "val":5.048037528991699}, "y":{"exp":"5.385420322418213 m", "val":5.385420322418213}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.048037528991699 m", "val":5.048037528991699}, "y":{"exp":"5.385420322418213 m", "val":5.385420322418213}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "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}, @@ -28,57 +28,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.25469,1.45785], + "waypoints":[0.0,0.23407,1.49933], "samples":[ - {"t":0.0, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.10203, "ay":9.45844, "alpha":9.07882, "fx":[9.48232,-51.89092,-108.96565,-59.68401], "fy":[174.8333,167.32099,137.06875,164.31867]}, - {"t":0.01819, "x":5.19731, "y":5.10512, "heading":-2.0944, "vx":-0.05643, "vy":0.17207, "omega":0.16516, "ax":-3.37119, "ay":9.36884, "alpha":9.0322, "fx":[4.58906,-55.27875,-111.82201,-66.86011], "fy":[175.00486,166.21478,134.72628,161.49902]}, - {"t":0.03638, "x":5.19573, "y":5.1098, "heading":-2.09139, "vx":-0.11776, "vy":0.3425, "omega":0.32947, "ax":-3.66634, "ay":9.26101, "alpha":8.97702, "fx":[-1.03828,-59.18031,-114.94889,-74.28642], "fy":[175.03407,164.84615,132.04327,158.18536]}, - {"t":0.05458, "x":5.19298, "y":5.11756, "heading":-2.0854, "vx":-0.18446, "vy":0.51098, "omega":0.49278, "ax":-3.99081, "ay":9.13075, "alpha":8.90786, "fx":[-7.5136,-63.64677,-118.36346,-82.006], "fy":[174.8449,163.1514,128.9628,154.287]}, - {"t":0.07277, "x":5.18896, "y":5.12837, "heading":-2.07643, "vx":-0.25706, "vy":0.67708, "omega":0.65483, "ax":-4.34814, "ay":8.97265, "alpha":8.81807, "fx":[-14.97287,-68.73633,-122.08036,-90.0528], "fy":[174.33014,161.04809,125.41764,149.6928]}, - {"t":0.09096, "x":5.18357, "y":5.14217, "heading":-2.06452, "vx":-0.33616, "vy":0.84031, "omega":0.81525, "ax":-4.74203, "ay":8.77978, "alpha":8.69913, "fx":[-23.57527,-74.51393,-126.10976,-98.44321], "fy":[173.3379,158.42899,121.32843,144.27091]}, - {"t":0.10915, "x":5.17667, "y":5.15891, "heading":-2.04969, "vx":-0.42242, "vy":1.00003, "omega":0.9735, "ax":-5.17601, "ay":8.54336, "alpha":8.53975, "fx":[-33.50069,-81.04976,-130.45454,-107.16473], "fy":[171.65275,155.15409,116.60216,137.87163]}, - {"t":0.12734, "x":5.16813, "y":5.17852, "heading":-2.03198, "vx":-0.51659, "vy":1.15545, "omega":1.12886, "ax":-5.65298, "ay":8.25229, "alpha":8.32481, "fx":[-44.93909,-88.41518,-135.10599,-116.16213], "fy":[168.97038,151.04016,111.13099,130.33517]}, - {"t":0.14553, "x":5.15779, "y":5.2009, "heading":-2.01144, "vx":-0.61942, "vy":1.30558, "omega":1.2803, "ax":-6.1744, "ay":7.89275, "alpha":8.0343, "fx":[-58.06396,-96.67424,-140.03789,-125.32268], "fy":[164.86676,145.84776,104.79249,121.50699]}, - {"t":0.16373, "x":5.1455, "y":5.22596, "heading":-1.98815, "vx":-0.73175, "vy":1.44916, "omega":1.42646, "ax":-6.7389, "ay":7.44795, "alpha":7.64275, "fx":[-72.97702,-105.86728,-145.19855,-134.46399], "fy":[158.7685,139.26653,97.45216,111.26259]}, - {"t":0.18192, "x":5.13108, "y":5.25355, "heading":-1.9622, "vx":-0.85434, "vy":1.58465, "omega":1.5655, "ax":-7.34025, "ay":6.89837, "alpha":7.12059, "fx":[-89.60865,-115.98161,-150.50099,-143.3308], "fy":[149.94415,130.90185,88.97004,99.54134]}, - {"t":0.20011, "x":5.11432, "y":5.28352, "heading":-1.93372, "vx":-0.98787, "vy":1.71015, "omega":1.69503, "ax":-7.96442, "ay":6.22334, "alpha":6.43909, "fx":[-107.56689,-126.90309,-155.81242,-151.60791], "fy":[137.55969,120.27105,79.21362,86.3844]}, - {"t":0.2183, "x":5.09503, "y":5.31566, "heading":-1.90289, "vx":-1.13276, "vy":1.82336, "omega":1.81217, "ax":-8.58646, "ay":5.40476, "alpha":5.58034, "fx":[-125.96873,-138.34389,-160.94536,-158.95469], "fy":[120.86328,106.82627,68.07885,71.96543]}, - {"t":0.23649, "x":5.073, "y":5.34973, "heading":-1.86992, "vx":-1.28897, "vy":1.92169, "omega":1.91369, "ax":-9.16888, "ay":4.43409, "alpha":4.54971, "fx":[-143.37302,-149.75295,-165.65545,-165.05857], "fy":[99.53841,90.03311,55.51989,56.59891]}, - {"t":0.25469, "x":5.04804, "y":5.38542, "heading":-1.83511, "vx":-1.45576, "vy":2.00235, "omega":1.99646, "ax":-9.5195, "ay":3.84152, "alpha":2.35709, "fx":[-156.76286,-158.60664,-166.38923,-165.93682], "fy":[77.68883,73.99832,54.26092,55.42461]}, - {"t":0.29007, "x":4.99056, "y":5.45868, "heading":-1.76446, "vx":-1.79263, "vy":2.13829, "omega":2.07987, "ax":-9.61617, "ay":3.63177, "alpha":-0.39692, "fx":[-164.34792,-164.12868,-162.76813,-163.02849], "fy":[59.71795,60.27856,63.86774,63.23729]}, - {"t":0.32546, "x":4.92111, "y":5.53662, "heading":-1.69086, "vx":-2.13292, "vy":2.26681, "omega":2.06582, "ax":-9.6377, "ay":3.22207, "alpha":-5.16116, "fx":[-172.06522,-171.23838,-152.60449,-159.82969], "fy":[30.37331,33.45443,84.72639,70.67182]}, - {"t":0.36085, "x":4.83959, "y":5.61886, "heading":-1.61775, "vx":-2.47397, "vy":2.38083, "omega":1.88318, "ax":-9.05266, "ay":2.1335, "alpha":-16.00904, "fx":[-173.86035,-167.67192,-117.44607,-156.95415], "fy":[-15.42405,-44.22686,128.29699,76.51473]}, - {"t":0.39623, "x":4.74638, "y":5.70444, "heading":-1.55111, "vx":-2.79431, "vy":2.45632, "omega":1.31667, "ax":-8.87919, "ay":1.6189, "alpha":-17.98096, "fx":[-171.75232,-160.00624,-113.1567,-159.21472], "fy":[-28.29363,-63.25182,131.0429,70.65068]}, - {"t":0.43162, "x":4.64194, "y":5.79238, "heading":-1.50452, "vx":-3.10852, "vy":2.51361, "omega":0.68038, "ax":-8.97674, "ay":0.68806, "alpha":-17.28136, "fx":[-167.53154,-150.28714,-128.96564,-163.98256], "fy":[-41.69228,-77.60162,111.58936,54.51963]}, - {"t":0.46701, "x":4.52631, "y":5.88176, "heading":-1.48044, "vx":-3.42618, "vy":2.53796, "omega":0.06884, "ax":-5.86793, "ay":-7.22216, "alpha":-1.78497, "fx":[-100.72137,-91.08339,-99.06311,-108.37948], "fy":[-123.75387,-129.77939,-122.16632,-115.68781]}, - {"t":0.5024, "x":4.4014, "y":5.96705, "heading":-1.47801, "vx":-3.63383, "vy":2.28239, "omega":0.00568, "ax":-3.7268, "ay":-6.34541, "alpha":-0.00823, "fx":[-63.40475,-63.35659,-63.37896,-63.42713], "fy":[-107.94201,-107.95695,-107.92528,-107.91033]}, - {"t":0.53778, "x":4.27047, "y":6.04384, "heading":-1.47781, "vx":-3.76571, "vy":2.05785, "omega":0.00538, "ax":-1.36535, "ay":-2.57082, "alpha":-0.00004, "fx":[-23.22433,-23.22412,-23.22415,-23.22436], "fy":[-43.729,-43.72899,-43.72879,-43.7288]}, - {"t":0.57317, "x":4.13636, "y":6.11505, "heading":-1.47762, "vx":-3.81403, "vy":1.96687, "omega":0.00538, "ax":-0.28862, "ay":-0.56325, "alpha":0.00007, "fx":[-4.90918,-4.90952,-4.90949,-4.90915], "fy":[-9.58056,-9.58059,-9.58093,-9.5809]}, - {"t":0.60856, "x":4.00121, "y":6.1843, "heading":-1.47743, "vx":-3.82424, "vy":1.94694, "omega":0.00539, "ax":-0.05769, "ay":-0.11348, "alpha":0.00002, "fx":[-0.98134,-0.98142,-0.98141,-0.98133], "fy":[-1.93015,-1.93016,-1.93024,-1.93023]}, - {"t":0.64394, "x":3.86585, "y":6.25313, "heading":-1.47724, "vx":-3.82628, "vy":1.94292, "omega":0.00539, "ax":-0.01155, "ay":-0.02275, "alpha":0.0, "fx":[-0.19645,-0.19645,-0.19645,-0.19645], "fy":[-0.38698,-0.38698,-0.38699,-0.38699]}, - {"t":0.67933, "x":3.73044, "y":6.32187, "heading":-1.47705, "vx":-3.82669, "vy":1.94212, "omega":0.00539, "ax":-0.00231, "ay":-0.00456, "alpha":0.0, "fx":[-0.03937,-0.03936,-0.03936,-0.03937], "fy":[-0.07757,-0.07757,-0.07756,-0.07756]}, - {"t":0.71472, "x":3.59502, "y":6.39059, "heading":-1.47685, "vx":-3.82677, "vy":1.94196, "omega":0.00539, "ax":-0.00047, "ay":-0.00092, "alpha":0.0, "fx":[-0.00792,-0.00791,-0.00791,-0.00792], "fy":[-0.0156,-0.01559,-0.01558,-0.01558]}, - {"t":0.7501, "x":3.45961, "y":6.45931, "heading":-1.47666, "vx":-3.82679, "vy":1.94193, "omega":0.00539, "ax":-0.0001, "ay":-0.0002, "alpha":0.0, "fx":[-0.00172,-0.00171,-0.00171,-0.00172], "fy":[-0.00338,-0.00338,-0.00337,-0.00337]}, - {"t":0.78549, "x":3.32419, "y":6.52803, "heading":-1.47647, "vx":-3.82679, "vy":1.94192, "omega":0.00539, "ax":-0.00006, "ay":-0.00011, "alpha":0.0, "fx":[-0.001,-0.00098,-0.00098,-0.001], "fy":[-0.00195,-0.00195,-0.00194,-0.00194]}, - {"t":0.82088, "x":3.18877, "y":6.59675, "heading":-1.47628, "vx":-3.8268, "vy":1.94191, "omega":0.00539, "ax":-0.0002, "ay":-0.0004, "alpha":0.0, "fx":[-0.00343,-0.00342,-0.00342,-0.00343], "fy":[-0.00675,-0.00675,-0.00674,-0.00674]}, - {"t":0.85627, "x":3.05335, "y":6.66547, "heading":-1.47609, "vx":-3.8268, "vy":1.9419, "omega":0.00539, "ax":-0.00099, "ay":-0.00194, "alpha":0.0, "fx":[-0.01679,-0.01678,-0.01678,-0.01679], "fy":[-0.03308,-0.03308,-0.03307,-0.03307]}, - {"t":0.89165, "x":2.91793, "y":6.73418, "heading":-1.4759, "vx":-3.82684, "vy":1.94183, "omega":0.00539, "ax":-0.00492, "ay":-0.0097, "alpha":0.0, "fx":[-0.0837,-0.0837,-0.0837,-0.0837], "fy":[-0.16497,-0.16497,-0.16497,-0.16497]}, - {"t":0.92704, "x":2.78251, "y":6.80289, "heading":-1.47571, "vx":-3.82701, "vy":1.94149, "omega":0.00539, "ax":-0.02454, "ay":-0.0484, "alpha":0.00001, "fx":[-0.41739,-0.41743,-0.41743,-0.41738], "fy":[-0.8233,-0.8233,-0.82335,-0.82335]}, - {"t":0.96243, "x":2.64706, "y":6.87157, "heading":-1.47552, "vx":-3.82788, "vy":1.93978, "omega":0.00539, "ax":-0.12008, "ay":-0.24254, "alpha":0.00492, "fx":[-2.03178,-2.05566,-2.05337,-2.02949], "fy":[-4.11242,-4.11467,-4.13852,-4.13628]}, - {"t":0.99781, "x":2.51153, "y":6.94006, "heading":-1.47533, "vx":-3.83213, "vy":1.93119, "omega":0.00556, "ax":2.39421, "ay":-2.48258, "alpha":7.6893, "fx":[60.70273,21.0911,22.49153,58.61422], "fy":[-20.54414,-27.22218,-64.75857,-56.3872]}, - {"t":1.0332, "x":2.37742, "y":7.00684, "heading":-1.47513, "vx":-3.74741, "vy":1.84334, "omega":0.27766, "ax":7.85497, "ay":-3.18513, "alpha":19.7877, "fx":[171.47432,159.35323,58.24068,145.37502], "fy":[17.68893,20.98116,-161.51494,-93.86782]}, - {"t":1.06859, "x":2.24973, "y":7.07008, "heading":-1.46531, "vx":-3.46944, "vy":1.73063, "omega":0.97789, "ax":8.06752, "ay":-3.15788, "alpha":19.417, "fx":[172.88321,166.82504,61.71757,147.47915], "fy":[19.05515,21.15064,-162.23756,-92.82669]}, - {"t":1.10397, "x":2.13201, "y":7.12934, "heading":-1.4307, "vx":-3.18396, "vy":1.61888, "omega":1.665, "ax":8.93778, "ay":-4.49298, "alpha":7.74547, "fx":[171.46691,160.84835,123.45681,152.344], "fy":[-31.77408,-65.70281,-122.99797,-85.22205]}, - {"t":1.13936, "x":2.02493, "y":7.18382, "heading":-1.37178, "vx":-2.86767, "vy":1.45989, "omega":1.93909, "ax":9.16135, "ay":-4.63458, "alpha":0.81952, "fx":[158.30576,156.15648,153.2616,155.60361], "fy":[-73.85472,-78.23076,-83.80941,-79.43647]}, - {"t":1.17475, "x":1.92919, "y":7.23258, "heading":-1.30316, "vx":-2.54348, "vy":1.29588, "omega":1.96809, "ax":9.14598, "ay":-4.63033, "alpha":-2.74957, "fx":[146.55018,155.69719,163.60286,156.4318], "fy":[-95.43351,-79.7689,-61.81251,-78.02742]}, - {"t":1.21014, "x":1.84491, "y":7.27554, "heading":-1.23352, "vx":-2.21983, "vy":1.13203, "omega":1.87079, "ax":9.09493, "ay":-4.60714, "alpha":-4.82139, "fx":[138.42983,156.45149,168.30123,155.62578], "fy":[-107.08507,-78.58829,-47.99452,-79.79678]}, - {"t":1.24552, "x":1.77205, "y":7.31271, "heading":-1.16732, "vx":-1.89799, "vy":0.969, "omega":1.70018, "ax":9.04354, "ay":-4.58673, "alpha":-6.16182, "fx":[133.15736,157.59803,170.8264,153.73016], "fy":[-113.71091,-76.44997,-38.3919,-83.52301]}, - {"t":1.28091, "x":1.71055, "y":7.34413, "heading":-1.10715, "vx":-1.57797, "vy":0.80669, "omega":1.48213, "ax":8.99631, "ay":-4.57271, "alpha":-7.12282, "fx":[129.8064,158.81309,172.32322,151.15595], "fy":[-117.61524,-74.01959,-31.28658,-88.20041]}, - {"t":1.3163, "x":1.66035, "y":7.36981, "heading":-1.05471, "vx":-1.25961, "vy":0.64487, "omega":1.23007, "ax":8.95312, "ay":-4.56371, "alpha":-7.87169, "fx":[127.68779,159.94701,173.26416,148.2608], "fy":[-119.97919,-71.62898,-25.83273,-93.06865]}, - {"t":1.35168, "x":1.62138, "y":7.38977, "heading":-1.01118, "vx":-0.94279, "vy":0.48338, "omega":0.95152, "ax":8.91391, "ay":-4.55761, "alpha":-8.4849, "fx":[126.324,160.92548,173.87838,145.36427], "fy":[-121.46439,-69.47383,-21.55708,-97.5994]}, - {"t":1.38707, "x":1.5936, "y":7.40403, "heading":-0.97751, "vx":-0.62735, "vy":0.3221, "omega":0.65126, "ax":8.8791, "ay":-4.55295, "alpha":-8.99399, "fx":[125.38559,161.71038,174.29012,142.73741], "fy":[-122.47127,-67.68322,-18.16438,-101.45894]}, - {"t":1.42246, "x":1.57695, "y":7.41257, "heading":-0.95446, "vx":-0.31315, "vy":0.16098, "omega":0.33299, "ax":8.84922, "ay":-4.54913, "alpha":-9.40993, "fx":[124.64296,162.28135,174.57182,140.59441], "fy":[-123.25702,-66.34855,-15.45659,-104.45519]}, - {"t":1.45785, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.07958, "y":5.09826, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.03058, "ay":10.68207, "alpha":17.55961, "fx":[104.8924,1.44305,-83.35832,112.41828], "fy":[169.12385,199.05367,180.53238,163.55007]}, + {"t":0.01801, "x":5.07991, "y":5.09999, "heading":-2.0944, "vx":0.03656, "vy":0.19233, "omega":0.31616, "ax":1.58061, "ay":10.80398, "alpha":17.02636, "fx":[99.57186,-3.94683,-89.6233,99.39029], "fy":[172.26777,198.99368,177.47151,171.65552]}, + {"t":0.03601, "x":5.08083, "y":5.10521, "heading":-2.0887, "vx":0.06502, "vy":0.38686, "omega":0.62273, "ax":1.02804, "ay":10.9224, "alpha":16.42696, "fx":[92.7234,-10.68987,-96.72699,83.2409], "fy":[175.99994,198.71514,173.66191,179.90775]}, + {"t":0.05402, "x":5.08216, "y":5.11394, "heading":-2.07749, "vx":0.08353, "vy":0.58352, "omega":0.9185, "ax":0.3456, "ay":11.02493, "alpha":15.74144, "fx":[83.90712,-19.00169,-104.74457,62.88305], "fy":[180.31301,198.05752,168.90046,187.84989]}, + {"t":0.07202, "x":5.08372, "y":5.12624, "heading":-2.06095, "vx":0.08975, "vy":0.78203, "omega":1.20193, "ax":-0.50049, "ay":11.08783, "alpha":14.95828, "fx":[72.47686,-29.15367,-113.71196,37.01713], "fy":[185.13682,196.77727,162.94402,194.4568]}, + {"t":0.09003, "x":5.08526, "y":5.14211, "heading":-2.03931, "vx":0.08074, "vy":0.98167, "omega":1.47125, "ax":-1.54536, "ay":11.06836, "alpha":14.09315, "fx":[57.47925,-41.47873,-123.60126,4.55886], "fy":[190.2488,194.50231,155.51402,197.75185]}, + {"t":0.10803, "x":5.08646, "y":5.16158, "heading":-2.01282, "vx":0.05292, "vy":1.18095, "omega":1.725, "ax":-2.81026, "ay":10.89932, "alpha":13.20006, "fx":[37.52933,-56.36243,-134.28857,-34.26138], "fy":[195.07236,190.66146,146.31451,194.69764]}, + {"t":0.12604, "x":5.08696, "y":5.18461, "heading":-1.98176, "vx":0.00232, "vy":1.3772, "omega":1.96267, "ax":-4.27968, "ay":10.49785, "alpha":12.3189, "fx":[10.75229,-74.19234,-145.52006,-76.40048], "fy":[198.24654,184.38261,135.07135,182.27578]}, + {"t":0.14404, "x":5.08631, "y":5.21111, "heading":-1.94642, "vx":-0.07474, "vy":1.56621, "omega":2.18448, "ax":-5.89365, "ay":9.79125, "alpha":11.3196, "fx":[-24.8438,-95.20711,-156.88656,-116.04007], "fy":[196.84905,174.37331,121.60036,160.03907]}, + {"t":0.16205, "x":5.08401, "y":5.2409, "heading":-1.90709, "vx":-0.18086, "vy":1.74251, "omega":2.38829, "ax":-7.56426, "ay":8.72752, "alpha":9.82301, "fx":[-69.62813,-119.14981,-167.82552,-147.76655], "fy":[185.65962,158.87072,105.90826,131.49537]}, + {"t":0.18005, "x":5.07952, "y":5.27369, "heading":-1.86409, "vx":-0.31705, "vy":1.89965, "omega":2.56516, "ax":-9.16046, "ay":7.27047, "alpha":7.51409, "fx":[-118.65707,-144.67369,-177.68016,-169.79114], "fy":[158.76695,135.91358,88.30423,101.79597]}, + {"t":0.19806, "x":5.07233, "y":5.30907, "heading":-1.8179, "vx":-0.48199, "vy":2.03056, "omega":2.70045, "ax":-10.48031, "ay":5.46708, "alpha":4.61478, "fx":[-160.60995,-168.79143,-185.83736,-183.56816], "fy":[116.16697,104.33013,69.45384,74.58339]}, + {"t":0.21606, "x":5.06195, "y":5.34652, "heading":-1.76928, "vx":-0.67069, "vy":2.12899, "omega":2.78354, "ax":-11.35683, "ay":3.5157, "alpha":1.70479, "fx":[-186.45816,-187.32068,-191.9056,-191.56709], "fy":[67.53138,65.35596,50.29546,51.23755]}, + {"t":0.23407, "x":5.04804, "y":5.38542, "heading":-1.71916, "vx":-0.87517, "vy":2.19229, "omega":2.81423, "ax":-11.65903, "ay":2.18361, "alpha":-4.31049, "fx":[-198.11274,-198.22185,-189.65966,-191.4076], "fy":[17.44065,14.66249,59.4844,54.0111]}, + {"t":0.27128, "x":5.0074, "y":5.46852, "heading":-1.61444, "vx":-1.30904, "vy":2.27355, "omega":2.65383, "ax":-11.10276, "ay":1.03982, "alpha":-15.17374, "fx":[-196.27784,-186.84168,-168.56949,-188.62166], "fy":[-31.63195,-66.49033,104.59394,62.86142]}, + {"t":0.30849, "x":4.95099, "y":5.55384, "heading":-1.51568, "vx":-1.72222, "vy":2.31225, "omega":2.08916, "ax":-10.86547, "ay":0.74706, "alpha":-17.51993, "fx":[-193.53002,-180.74322,-160.41273,-189.80283], "fy":[-44.65532,-80.33255,116.1701,58.63035]}, + {"t":0.34571, "x":4.87938, "y":5.64041, "heading":-1.43793, "vx":-2.12656, "vy":2.34005, "omega":1.43718, "ax":-10.76582, "ay":0.3422, "alpha":-18.25826, "fx":[-190.36473,-175.84915,-159.95296,-191.6775], "fy":[-55.27441,-88.48878,115.75113,50.82904]}, + {"t":0.38292, "x":4.79279, "y":5.72772, "heading":-1.38445, "vx":-2.52719, "vy":2.35278, "omega":0.75773, "ax":-10.72546, "ay":-0.80219, "alpha":-17.67921, "fx":[-183.91826,-163.7394,-172.77353,-194.72169], "fy":[-71.07992,-105.58123,91.55468,31.61768]}, + {"t":0.42014, "x":4.69132, "y":5.81472, "heading":-1.35625, "vx":-2.92632, "vy":2.32293, "omega":0.09982, "ax":-7.36563, "ay":-8.55723, "alpha":-2.49511, "fx":[-121.96147,-110.15942,-124.24778,-134.75755], "fy":[-144.92004,-152.95105,-140.55742,-132.15141]}, + {"t":0.45735, "x":4.57732, "y":5.89524, "heading":-1.35254, "vx":-3.20043, "vy":2.00449, "omega":0.00697, "ax":-4.62079, "ay":-8.18381, "alpha":-0.01561, "fx":[-77.04054,-76.95135,-77.0123,-77.10148], "fy":[-136.43582,-136.46384,-136.4046,-136.37657]}, + {"t":0.49456, "x":4.45502, "y":5.96417, "heading":-1.35228, "vx":-3.37238, "vy":1.69994, "omega":0.00639, "ax":-1.27545, "ay":-2.62259, "alpha":-0.00024, "fx":[-21.26154,-21.26039,-21.26069,-21.26185], "fy":[-43.71805,-43.71783,-43.71672,-43.71694]}, + {"t":0.53178, "x":4.32864, "y":6.02562, "heading":-1.35204, "vx":-3.41985, "vy":1.60234, "omega":0.00638, "ax":-0.15789, "ay":-0.33861, "alpha":0.00006, "fx":[-2.63192,-2.63219,-2.63213,-2.63186], "fy":[-5.64425,-5.64431,-5.64458,-5.64452]}, + {"t":0.56899, "x":4.20127, "y":6.08501, "heading":-1.3518, "vx":-3.42572, "vy":1.58974, "omega":0.00638, "ax":-0.01896, "ay":-0.04089, "alpha":0.00001, "fx":[-0.31611,-0.31614,-0.31613,-0.3161], "fy":[-0.68162,-0.68163,-0.68166,-0.68165]}, + {"t":0.6062, "x":4.07377, "y":6.14414, "heading":-1.35157, "vx":-3.42643, "vy":1.58822, "omega":0.00638, "ax":-0.00228, "ay":-0.00493, "alpha":0.0, "fx":[-0.03809,-0.03807,-0.03808,-0.03809], "fy":[-0.08216,-0.08216,-0.08215,-0.08215]}, + {"t":0.64342, "x":3.94626, "y":6.20324, "heading":-1.35133, "vx":-3.42651, "vy":1.58804, "omega":0.00638, "ax":-0.00027, "ay":-0.00059, "alpha":0.0, "fx":[-0.00459,-0.00457,-0.00458,-0.00459], "fy":[-0.00989,-0.00989,-0.00987,-0.00987]}, + {"t":0.68063, "x":3.81875, "y":6.26234, "heading":-1.35109, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00003, "ay":-0.00007, "alpha":0.0, "fx":[-0.00055,-0.00053,-0.00054,-0.00056], "fy":[-0.00118,-0.00117,-0.00115,-0.00116]}, + {"t":0.71784, "x":3.69123, "y":6.32143, "heading":-1.35085, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":-0.00001, "alpha":0.0, "fx":[-0.00007,-0.00005,-0.00005,-0.00007], "fy":[-0.00013,-0.00012,-0.00011,-0.00011]}, + {"t":0.75506, "x":3.56372, "y":6.38053, "heading":-1.35062, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,0.00001,0.00001,-0.00001], "fy":[0.0,0.0,0.00002,0.00002]}, + {"t":0.79227, "x":3.43621, "y":6.43962, "heading":-1.35038, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00004,0.00003]}, + {"t":0.82948, "x":3.30869, "y":6.49872, "heading":-1.35014, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00004,0.00003]}, + {"t":0.8667, "x":3.18118, "y":6.55781, "heading":-1.3499, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00001,0.00001,-0.00001], "fy":[0.00001,0.00001,0.00003,0.00003]}, + {"t":0.90391, "x":3.05367, "y":6.61691, "heading":-1.34967, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00001,-0.00001,-0.00003], "fy":[-0.00004,-0.00004,-0.00002,-0.00003]}, + {"t":0.94112, "x":2.92615, "y":6.67601, "heading":-1.34943, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00023,-0.00022,-0.00022,-0.00024], "fy":[-0.00049,-0.00048,-0.00046,-0.00047]}, + {"t":0.97834, "x":2.79864, "y":6.7351, "heading":-1.34919, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00012, "ay":-0.00025, "alpha":0.0, "fx":[-0.00193,-0.00192,-0.00192,-0.00194], "fy":[-0.00416,-0.00415,-0.00413,-0.00414]}, + {"t":1.01555, "x":2.67113, "y":6.7942, "heading":-1.34895, "vx":-3.42653, "vy":1.588, "omega":0.00638, "ax":-0.00096, "ay":-0.00208, "alpha":0.0, "fx":[-0.01604,-0.01603,-0.01603,-0.01605], "fy":[-0.0346,-0.0346,-0.03458,-0.03459]}, + {"t":1.05277, "x":2.54361, "y":6.85329, "heading":-1.34872, "vx":-3.42656, "vy":1.58792, "omega":0.00638, "ax":-0.00798, "ay":-0.01723, "alpha":0.0, "fx":[-0.13304,-0.13304,-0.13304,-0.13304], "fy":[-0.28717,-0.28717,-0.28718,-0.28717]}, + {"t":1.08998, "x":2.41609, "y":6.91237, "heading":-1.34848, "vx":-3.42686, "vy":1.58728, "omega":0.00638, "ax":-0.06581, "ay":-0.14311, "alpha":0.00072, "fx":[-1.09566,-1.0991,-1.09833,-1.09489], "fy":[-2.38344,-2.3842,-2.38764,-2.38688]}, + {"t":1.12719, "x":2.28852, "y":6.97134, "heading":-1.34824, "vx":-3.42931, "vy":1.58196, "omega":0.00641, "ax":0.25839, "ay":-1.53056, "alpha":2.02822, "fx":[8.15781,-1.70462,0.51905,10.25689], "fy":[-19.67061,-21.88709,-31.33949,-29.15736]}, + {"t":1.16441, "x":2.16109, "y":7.02915, "heading":-1.348, "vx":-3.41969, "vy":1.525, "omega":0.08188, "ax":9.13824, "ay":-3.37467, "alpha":22.48842, "fx":[192.14192,175.37824,69.46167,172.3387], "fy":[39.85917,13.78089,-183.16232,-95.49417]}, + {"t":1.20162, "x":2.04015, "y":7.08356, "heading":-1.34496, "vx":-3.07963, "vy":1.39942, "omega":0.91876, "ax":9.40204, "ay":-3.16216, "alpha":22.42045, "fx":[193.38799,186.73461,72.39016,174.39704], "fy":[42.00163,25.44838,-183.9842,-94.3127]}, + {"t":1.23883, "x":1.93206, "y":7.13345, "heading":-1.31077, "vx":-2.72975, "vy":1.28174, "omega":1.7531, "ax":10.54989, "ay":-4.88993, "alpha":8.55927, "fx":[196.27323,182.57204,146.3711,178.23027], "fy":[-28.67597,-76.10792,-133.79042,-87.47697]}, + {"t":1.27605, "x":1.83778, "y":7.17776, "heading":-1.24553, "vx":-2.33715, "vy":1.09977, "omega":2.07162, "ax":10.79824, "ay":-5.0127, "alpha":-2.0184, "fx":[173.94017,180.12251,185.60865,180.33481], "fy":[-96.04208,-83.98091,-70.92011,-83.29434]}, + {"t":1.31326, "x":1.75829, "y":7.21522, "heading":-1.16844, "vx":-1.93531, "vy":0.91323, "omega":1.99651, "ax":10.66787, "ay":-4.95279, "alpha":-6.83981, "fx":[155.95249,181.32679,195.17512,178.85877], "fy":[-123.45169,-81.93336,-38.25727,-86.60031]}, + {"t":1.35047, "x":1.69365, "y":7.24577, "heading":-1.09414, "vx":-1.53832, "vy":0.72892, "omega":1.74198, "ax":10.52294, "ay":-4.9076, "alpha":-9.45155, "fx":[145.79876,183.18709,198.11756,174.54601], "fy":[-135.47254,-77.96451,-18.65847,-95.13369]}, + {"t":1.38769, "x":1.64369, "y":7.2695, "heading":-1.02931, "vx":-1.14672, "vy":0.54629, "omega":1.39025, "ax":10.38887, "ay":-4.89207, "alpha":-11.19972, "fx":[140.37049,184.98248,198.98524,168.37173], "fy":[-141.19729,-73.7716,-5.43421,-105.79042]}, + {"t":1.4249, "x":1.60821, "y":7.28644, "heading":-0.97758, "vx":-0.76012, "vy":0.36424, "omega":0.97347, "ax":10.26609, "ay":-4.89203, "alpha":-12.54709, "fx":[137.4338,186.46216,199.06394,161.56353], "fy":[-144.12759,-70.0616,4.01297,-116.01498]}, + {"t":1.46211, "x":1.58703, "y":7.29661, "heading":-0.94135, "vx":-0.37808, "vy":0.18219, "omega":0.50655, "ax":10.1597, "ay":-4.89576, "alpha":-13.61192, "fx":[135.66947,187.53245,198.84132,155.38627], "fy":[-145.83803,-67.22749,10.86831,-124.24262]}, + {"t":1.49933, "x":1.58, "y":7.3, "heading":-0.9225, "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/KTofetch.traj b/src/main/deploy/choreo/KTofetch.traj index a52cd88d..3caee708 100644 --- a/src/main/deploy/choreo/KTofetch.traj +++ b/src/main/deploy/choreo/KTofetch.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "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":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "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,36 +26,36 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.19692], + "waypoints":[0.0,1.18786], "samples":[ - {"t":0.0, "x":4.19501, "y":5.34254, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.08578, "ay":6.38853, "alpha":0.62782, "fx":[-135.0894,-138.59076,-140.00364,-136.46362], "fy":[111.73935,107.3692,105.51149,110.0481]}, - {"t":0.04433, "x":4.18706, "y":5.34881, "heading":-1.0472, "vx":-0.35845, "vy":0.28321, "omega":0.02783, "ax":-8.08484, "ay":6.38779, "alpha":0.62181, "fx":[-135.09721,-138.56502,-139.96354,-136.45781], "fy":[111.6975,107.36909,105.52977,110.02143]}, - {"t":0.08866, "x":4.16323, "y":5.36764, "heading":-1.04596, "vx":-0.71685, "vy":0.56638, "omega":0.0554, "ax":-8.08364, "ay":6.38684, "alpha":0.6141, "fx":[-135.10853,-138.53482,-139.91087,-136.44722], "fy":[111.64213,107.36517,105.55471,109.99099]}, - {"t":0.13299, "x":4.12351, "y":5.39903, "heading":-1.04351, "vx":-1.0752, "vy":0.84951, "omega":0.08262, "ax":-8.08203, "ay":6.38556, "alpha":0.60386, "fx":[-135.12427,-138.49632,-139.84004,-136.43118], "fy":[111.56744,107.3576,105.58869,109.95279]}, - {"t":0.17732, "x":4.0679, "y":5.44296, "heading":-1.03985, "vx":-1.43348, "vy":1.13258, "omega":0.10939, "ax":-8.07977, "ay":6.38378, "alpha":0.58959, "fx":[-135.14607,-138.44262,-139.74102,-136.4084], "fy":[111.46301,107.3466,105.63596,109.89974]}, - {"t":0.22165, "x":3.99642, "y":5.49944, "heading":-1.035, "vx":-1.79166, "vy":1.41558, "omega":0.13553, "ax":-8.07637, "ay":6.38111, "alpha":0.56834, "fx":[-135.17699,-138.36003,-139.59394,-136.37619], "fy":[111.30821,107.33245,105.70471,109.81777]}, - {"t":0.26598, "x":3.90906, "y":5.56846, "heading":-1.02899, "vx":-2.14968, "vy":1.69845, "omega":0.16072, "ax":-8.0707, "ay":6.37663, "alpha":0.53337, "fx":[-135.22351,-138.21682,-139.35303,-136.32785], "fy":[111.05551,107.31537,105.81323,109.67463]}, - {"t":0.31031, "x":3.80583, "y":5.65002, "heading":-1.02186, "vx":-2.50746, "vy":1.98113, "omega":0.18436, "ax":-8.05931, "ay":6.36765, "alpha":0.46527, "fx":[-135.30158,-137.91889,-138.88423,-136.24149], "fy":[110.56582,107.29379,106.0117,109.37614]}, - {"t":0.35464, "x":3.68675, "y":5.7441, "heading":-1.01369, "vx":-2.86473, "vy":2.26341, "omega":0.20499, "ax":-8.0249, "ay":6.34051, "alpha":0.27639, "fx":[-135.45065,-137.00926,-137.55649,-135.98842], "fy":[109.18603,107.23609,106.49813,108.48051]}, - {"t":0.39897, "x":3.55187, "y":5.85067, "heading":-1.0046, "vx":-3.22048, "vy":2.54449, "omega":0.21724, "ax":-3.3354, "ay":2.63509, "alpha":-4.89506, "fx":[-63.51845,-40.93333,-51.0611,-71.42416], "fy":[26.8117,44.09236,62.15702,46.22775]}, - {"t":0.4433, "x":3.40583, "y":5.96606, "heading":-0.99497, "vx":-3.36834, "vy":2.6613, "omega":0.00024, "ax":-0.00064, "ay":0.00042, "alpha":-0.00053, "fx":[-0.01133,-0.00916,-0.01057,-0.01273], "fy":[0.00543,0.00684,0.00901,0.0076]}, - {"t":0.48763, "x":3.25651, "y":6.08404, "heading":-0.99496, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":-0.00001, "ay":-0.00001, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, - {"t":0.53196, "x":3.10719, "y":6.20201, "heading":-0.99495, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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":0.57629, "x":2.95787, "y":6.31999, "heading":-0.99494, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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.62062, "x":2.80855, "y":6.43797, "heading":-0.99493, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "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":0.66495, "x":2.65923, "y":6.55594, "heading":-0.99492, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":0.00001, "ay":0.00001, "alpha":0.0, "fx":[0.00009,0.00009,0.00009,0.00009], "fy":[0.00011,0.00011,0.00011,0.00011]}, - {"t":0.70928, "x":2.50991, "y":6.67392, "heading":-0.99491, "vx":-3.36837, "vy":2.66132, "omega":0.00022, "ax":0.00064, "ay":-0.00043, "alpha":0.00053, "fx":[0.01132,0.00915,0.01056,0.01273], "fy":[-0.00544,-0.00685,-0.00902,-0.00761]}, - {"t":0.75361, "x":2.36059, "y":6.7919, "heading":-0.9949, "vx":-3.36834, "vy":2.6613, "omega":0.00024, "ax":3.3354, "ay":-2.63509, "alpha":4.89515, "fx":[63.37291,40.87287,51.20925,71.48177], "fy":[-26.79028,-44.27539,-62.15698,-46.06615]}, - {"t":0.79795, "x":2.21455, "y":6.90729, "heading":-0.99489, "vx":-3.22048, "vy":2.54449, "omega":0.21725, "ax":8.0249, "ay":-6.3405, "alpha":-0.27706, "fx":[135.45815,137.02999,137.54953,135.96714], "fy":[-109.1772,-107.20948,-106.50665,-108.50736]}, - {"t":0.84228, "x":2.07967, "y":7.01385, "heading":-0.98526, "vx":-2.86473, "vy":2.26341, "omega":0.20496, "ax":8.05931, "ay":-6.36765, "alpha":-0.46544, "fx":[135.33433,137.98275,138.85442,136.17469], "fy":[-110.52644,-107.21133,-106.04999,-109.45966]}, - {"t":0.88661, "x":1.96059, "y":7.10793, "heading":-0.97618, "vx":-2.50746, "vy":1.98113, "omega":0.18433, "ax":8.0707, "ay":-6.37663, "alpha":-0.53338, "fx":[135.27906,138.32175,139.30306,136.21735], "fy":[-110.98861,-107.17971,-105.87817,-109.81224]}, - {"t":0.93094, "x":1.85737, "y":7.18949, "heading":-0.968, "vx":-2.14968, "vy":1.69845, "omega":0.16069, "ax":8.07637, "ay":-6.38111, "alpha":-0.56827, "fx":[135.25269,138.50155,139.52628,136.22667], "fy":[-111.21698,-107.1494,-105.79315,-110.00361]}, - {"t":0.97527, "x":1.77, "y":7.25852, "heading":-0.96088, "vx":-1.79166, "vy":1.41558, "omega":0.13549, "ax":8.07977, "ay":-6.38378, "alpha":-0.58948, "fx":[135.23905,138.61544,139.65823,136.22543], "fy":[-111.35092,-107.12297,-105.74455,-110.12687]}, - {"t":1.0196, "x":1.69852, "y":7.315, "heading":-0.95487, "vx":-1.43348, "vy":1.13258, "omega":0.10936, "ax":8.08203, "ay":-6.38557, "alpha":-0.60373, "fx":[135.23152,138.6948,139.74478,136.22075], "fy":[-111.43812,-107.10071,-105.71393,-110.21378]}, - {"t":1.06393, "x":1.64291, "y":7.35893, "heading":-0.95003, "vx":-1.0752, "vy":0.84951, "omega":0.0826, "ax":8.08364, "ay":-6.38684, "alpha":-0.61396, "fx":[135.22694,138.75311,139.80587,136.21556], "fy":[-111.49935,-107.08258,-105.69298,-110.27812]}, - {"t":1.10826, "x":1.60319, "y":7.39031, "heading":-0.94637, "vx":-0.71685, "vy":0.56638, "omega":0.05538, "ax":8.08485, "ay":-6.38779, "alpha":-0.62165, "fx":[135.22359,138.79714,139.85161,136.21129], "fy":[-111.5451,-107.06854,-105.67735,-110.32682]}, - {"t":1.15259, "x":1.57936, "y":7.40914, "heading":-0.94391, "vx":-0.35845, "vy":0.28321, "omega":0.02782, "ax":8.08578, "ay":-6.38853, "alpha":-0.62766, "fx":[135.22047,138.83063,139.88764,136.20872], "fy":[-111.58127,-107.05856,-105.66458,-110.36376]}, - {"t":1.19692, "x":1.57141, "y":7.41542, "heading":-0.94268, "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":4.14155, "y":5.23826, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.30248, "ay":7.48763, "alpha":1.47964, "fx":[-149.29707,-157.60885,-160.94359,-152.42208], "fy":[131.86641,121.81793,117.35584,128.22069]}, + {"t":0.04399, "x":4.13255, "y":5.24551, "heading":-1.0472, "vx":-0.40926, "vy":0.32942, "omega":0.0651, "ax":-9.30148, "ay":7.48696, "alpha":1.40786, "fx":[-149.55806,-157.47153,-160.6394,-152.53601], "fy":[131.52214,121.94427,117.71654,128.03333]}, + {"t":0.08799, "x":4.10554, "y":5.26725, "heading":-1.04433, "vx":-0.81848, "vy":0.6588, "omega":0.12703, "ax":-9.30003, "ay":7.48598, "alpha":1.30707, "fx":[-149.93165,-157.29136,-160.20587,-152.67915], "fy":[131.02795,122.10443,118.22852,127.79009]}, + {"t":0.13198, "x":4.06053, "y":5.30347, "heading":-1.03874, "vx":-1.22763, "vy":0.98815, "omega":0.18454, "ax":-9.29772, "ay":7.48442, "alpha":1.15514, "fx":[-150.49452,-157.01859,-159.55041,-152.89086], "fy":[130.27733,122.34548,118.99602,127.42793]}, + {"t":0.17598, "x":3.99752, "y":5.35419, "heading":-1.03063, "vx":-1.63668, "vy":1.31742, "omega":0.23536, "ax":-9.29355, "ay":7.48156, "alpha":0.9, "fx":[-151.42715,-156.53476,-158.45424,-153.25996], "fy":[129.01491,122.77812,120.26054,126.8023]}, + {"t":0.21997, "x":3.91652, "y":5.41939, "heading":-1.02027, "vx":-2.04555, "vy":1.64657, "omega":0.27495, "ax":-9.28393, "ay":7.47483, "alpha":0.38299, "fx":[-153.27652,-155.46762,-156.24904,-154.04122], "fy":[126.44043,123.74312,122.73557,125.48835]}, + {"t":0.26397, "x":3.81755, "y":5.49907, "heading":-1.00817, "vx":-2.45399, "vy":1.97542, "omega":0.2918, "ax":-9.24422, "ay":7.4461, "alpha":-1.21804, "fx":[-158.80664,-151.70824,-149.47734,-156.39479], "fy":[118.14195,127.08227,129.82424,121.44312]}, + {"t":0.30796, "x":3.70064, "y":5.59318, "heading":-0.99534, "vx":-2.86069, "vy":2.30301, "omega":0.23822, "ax":-1.87667, "ay":1.49597, "alpha":-5.40306, "fx":[-36.16714,-13.06221,-27.04978,-48.85372], "fy":[6.32244,21.85331,43.32978,28.24292]}, + {"t":0.35196, "x":3.57297, "y":5.69595, "heading":-0.98486, "vx":-2.94325, "vy":2.36883, "omega":0.00051, "ax":-0.00078, "ay":-0.00061, "alpha":-0.0003, "fx":[-0.01329,-0.01207,-0.01288,-0.0141], "fy":[-0.0111,-0.01029,-0.00908,-0.00988]}, + {"t":0.39595, "x":3.44348, "y":5.80016, "heading":-0.98483, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":-0.00005, "ay":-0.00006, "alpha":0.0, "fx":[-0.00081,-0.00081,-0.00081,-0.00081], "fy":[-0.00101,-0.00101,-0.00101,-0.00101]}, + {"t":0.43995, "x":3.31399, "y":5.90438, "heading":-0.98481, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00008,-0.00008,-0.00008,-0.00008]}, + {"t":0.48394, "x":3.1845, "y":6.00859, "heading":-0.98479, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "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":0.52794, "x":3.05501, "y":6.11281, "heading":-0.98477, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "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.57193, "x":2.92552, "y":6.21702, "heading":-0.98475, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "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.61593, "x":2.79603, "y":6.32124, "heading":-0.98473, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "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.65992, "x":2.66654, "y":6.42545, "heading":-0.9847, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "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":0.70392, "x":2.53705, "y":6.52967, "heading":-0.98468, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00006,0.00006,0.00006,0.00006], "fy":[0.00008,0.00008,0.00008,0.00008]}, + {"t":0.74791, "x":2.40756, "y":6.63388, "heading":-0.98466, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.00005, "ay":0.00006, "alpha":0.0, "fx":[0.00081,0.00081,0.00081,0.00081], "fy":[0.001,0.001,0.001,0.001]}, + {"t":0.79191, "x":2.27808, "y":6.7381, "heading":-0.98464, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.00078, "ay":0.0006, "alpha":0.0003, "fx":[0.01322,0.012,0.01281,0.01403], "fy":[0.01102,0.01021,0.00899,0.0098]}, + {"t":0.8359, "x":2.14859, "y":6.84231, "heading":-0.98462, "vx":-2.94325, "vy":2.36883, "omega":0.00051, "ax":1.87609, "ay":-1.4956, "alpha":5.41639, "fx":[35.97637,12.96027,27.22164,48.93588], "fy":[-6.22983,-22.04581,-43.39519,-28.05305]}, + {"t":0.87989, "x":2.02092, "y":6.94508, "heading":-0.98459, "vx":-2.86071, "vy":2.30303, "omega":0.2388, "ax":9.24453, "ay":-7.44631, "alpha":1.18094, "fx":[158.61767,151.67634,149.68154,156.43182], "fy":[-118.39608,-127.12547,-129.59049,-121.39333]}, + {"t":0.92389, "x":1.90401, "y":7.03919, "heading":-0.97409, "vx":-2.454, "vy":1.97543, "omega":0.29076, "ax":9.28393, "ay":-7.47482, "alpha":-0.39111, "fx":[153.28157,155.55074,156.24662,153.95546], "fy":[-126.43533,-123.63891,-122.73819,-125.59389]}, + {"t":0.96788, "x":1.80503, "y":7.11887, "heading":-0.9613, "vx":-2.04556, "vy":1.64658, "omega":0.27355, "ax":9.29356, "ay":-7.48156, "alpha":-0.89919, "fx":[151.56152,156.76384,158.33838,153.01329], "fy":[-128.85843,-122.48495,-120.41171,-127.10075]}, + {"t":1.01188, "x":1.72403, "y":7.18407, "heading":-0.94926, "vx":-1.63669, "vy":1.31743, "omega":0.23399, "ax":9.29776, "ay":-7.48444, "alpha":-1.15015, "fx":[150.73576,157.3864,159.34812,152.48673], "fy":[-129.99972,-121.87115,-119.265,-127.91236]}, + {"t":1.05587, "x":1.66102, "y":7.23479, "heading":-0.93897, "vx":-1.22764, "vy":0.98815, "omega":0.18339, "ax":9.30009, "ay":-7.48602, "alpha":-1.29972, "fx":[150.2601,157.77607,159.93583,152.14006], "fy":[-130.65278,-121.47656,-118.59166,-128.4326]}, + {"t":1.09987, "x":1.61601, "y":7.27102, "heading":-0.9309, "vx":-0.81848, "vy":0.65881, "omega":0.12621, "ax":9.30156, "ay":-7.48701, "alpha":-1.39902, "fx":[149.95302,158.0441,160.31892,151.89406], "fy":[-131.07321,-121.20039,-118.15071,-128.79542]}, + {"t":1.14386, "x":1.589, "y":7.29275, "heading":-0.92535, "vx":-0.40926, "vy":0.32942, "omega":0.06466, "ax":9.30257, "ay":-7.48769, "alpha":-1.46973, "fx":[149.73638,158.23601,160.59019,151.71497], "fy":[-131.36881,-121.00131,-117.83714,-129.05769]}, + {"t":1.18786, "x":1.58, "y":7.3, "heading":-0.9225, "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/LTofetch.traj b/src/main/deploy/choreo/LTofetch.traj index a867a04d..b1793876 100644 --- a/src/main/deploy/choreo/LTofetch.traj +++ b/src/main/deploy/choreo/LTofetch.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":27, "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":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":27, "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,36 +26,35 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.17196], + "waypoints":[0.0,1.1599], "samples":[ - {"t":0.0, "x":3.91036, "y":5.1782, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.44665, "ay":7.12277, "alpha":0.64117, "fx":[-124.01568,-128.11294,-129.35809,-125.17464], "fy":[123.90932,119.67059,118.31503,122.72997]}, - {"t":0.04341, "x":3.90335, "y":5.18491, "heading":-1.0472, "vx":-0.32323, "vy":0.30917, "omega":0.02783, "ax":-7.44576, "ay":7.12191, "alpha":0.63702, "fx":[-124.018,-128.0887,-129.32487,-125.16911], "fy":[123.87714,119.6659,118.31936,122.70447]}, - {"t":0.08681, "x":3.88231, "y":5.20504, "heading":-1.04599, "vx":-0.64642, "vy":0.6183, "omega":0.05548, "ax":-7.44461, "ay":7.12082, "alpha":0.63172, "fx":[-124.0227,-128.06069,-129.28064,-125.15866], "fy":[123.83404,119.6565,118.3266,122.67512]}, - {"t":0.13022, "x":3.84723, "y":5.23858, "heading":-1.04358, "vx":-0.96956, "vy":0.92739, "omega":0.0829, "ax":-7.44308, "ay":7.11935, "alpha":0.62469, "fx":[-124.02994,-128.02535,-129.2208,-125.1426], "fy":[123.7756,119.64178,118.33717,122.6382]}, - {"t":0.17362, "x":3.79814, "y":5.28554, "heading":-1.03998, "vx":-1.29263, "vy":1.23641, "omega":0.11002, "ax":-7.44094, "ay":7.11731, "alpha":0.61493, "fx":[-124.03994,-127.97636,-129.13719,-125.11958], "fy":[123.6939,119.62063,118.3518,122.58711]}, - {"t":0.21703, "x":3.73502, "y":5.34592, "heading":-1.03521, "vx":-1.61561, "vy":1.54534, "omega":0.13671, "ax":-7.43773, "ay":7.11424, "alpha":0.60047, "fx":[-124.05296,-127.90118,-129.01368,-125.08684], "fy":[123.57329,119.59068,118.3717,122.50882]}, - {"t":0.26043, "x":3.65789, "y":5.41969, "heading":-1.02927, "vx":-1.93845, "vy":1.85414, "omega":0.16277, "ax":-7.43238, "ay":7.10912, "alpha":0.57688, "fx":[-124.06902,-127.77099,-128.81333,-125.03745], "fy":[123.37788,119.54596,118.39905,122.37348]}, - {"t":0.30384, "x":3.56675, "y":5.50687, "heading":-1.02221, "vx":-2.26106, "vy":2.16272, "omega":0.18781, "ax":-7.4217, "ay":7.0989, "alpha":0.53154, "fx":[-124.08515,-127.50118,-128.42889,-124.94881], "fy":[123.00318,119.4667,118.43702,122.0942]}, - {"t":0.34725, "x":3.46161, "y":5.60743, "heading":-1.01406, "vx":-2.5832, "vy":2.47085, "omega":0.21088, "ax":-7.38987, "ay":7.06845, "alpha":0.40914, "fx":[-124.05689,-126.68894,-127.36159,-124.69083], "fy":[121.9604,119.23609,118.46875,121.26392]}, - {"t":0.39065, "x":3.34252, "y":5.72134, "heading":-1.0049, "vx":-2.90397, "vy":2.77766, "omega":0.22864, "ax":-4.56546, "ay":4.36719, "alpha":-5.26148, "fx":[-89.08373,-60.67549,-68.20924,-92.66012], "fy":[53.9039,77.95047,92.72971,72.55455]}, - {"t":0.43406, "x":3.21217, "y":5.84602, "heading":-0.99498, "vx":-3.10213, "vy":2.96722, "omega":0.00026, "ax":-0.00131, "ay":0.001, "alpha":-0.001, "fx":[-0.02296,-0.0189,-0.02154,-0.02561], "fy":[0.01372,0.01636,0.02043,0.01779]}, - {"t":0.47746, "x":3.07752, "y":5.97482, "heading":-0.99497, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":-0.00002, "ay":-0.00002, "alpha":0.0, "fx":[-0.00027,-0.00027,-0.00027,-0.00027], "fy":[-0.00028,-0.00028,-0.00028,-0.00028]}, - {"t":0.52087, "x":2.94287, "y":6.10361, "heading":-0.99496, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[-0.00004,-0.00004,-0.00004,-0.00004]}, - {"t":0.56428, "x":2.80822, "y":6.23241, "heading":-0.99495, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "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.60768, "x":2.67356, "y":6.36121, "heading":-0.99494, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.00003,0.00003,0.00003,0.00003]}, - {"t":0.65109, "x":2.53891, "y":6.49, "heading":-0.99493, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.00002, "ay":0.00002, "alpha":0.0, "fx":[0.00026,0.00026,0.00026,0.00026], "fy":[0.00027,0.00027,0.00027,0.00027]}, - {"t":0.69449, "x":2.40426, "y":6.6188, "heading":-0.99492, "vx":-3.10219, "vy":2.96727, "omega":0.00022, "ax":0.0013, "ay":-0.00101, "alpha":0.001, "fx":[0.0229,0.01883,0.02148,0.02555], "fy":[-0.01379,-0.01643,-0.0205,-0.01786]}, - {"t":0.7379, "x":2.2696, "y":6.7476, "heading":-0.99491, "vx":-3.10213, "vy":2.96722, "omega":0.00026, "ax":4.56546, "ay":-4.36719, "alpha":5.26176, "fx":[88.93536,60.56537,68.37443,92.75368], "fy":[-53.91785,-78.15434,-92.69308,-72.37313]}, - {"t":0.7813, "x":2.13925, "y":6.87228, "heading":-0.9949, "vx":-2.90397, "vy":2.77766, "omega":0.22866, "ax":7.38987, "ay":-7.06845, "alpha":-0.40926, "fx":[124.07639,126.72016,127.34317,124.6585], "fy":[-121.94112,-119.20258,-118.48794,-121.29751]}, - {"t":0.82471, "x":2.02017, "y":6.98619, "heading":-0.98497, "vx":-2.5832, "vy":2.47085, "omega":0.21089, "ax":7.4217, "ay":-7.0989, "alpha":-0.53158, "fx":[124.13582,127.57944,128.38182,124.86695], "fy":[-122.95275,-119.3827,-118.48727,-122.17838]}, - {"t":0.86812, "x":1.91503, "y":7.08675, "heading":-0.97582, "vx":-2.26106, "vy":2.16272, "omega":0.18782, "ax":7.43238, "ay":-7.10912, "alpha":-0.57689, "fx":[124.14842,127.89277,128.74001,124.90958], "fy":[-123.29871,-119.41522,-118.47795,-122.50448]}, - {"t":0.91152, "x":1.82389, "y":7.17392, "heading":-0.96767, "vx":-1.93845, "vy":1.85414, "omega":0.16278, "ax":7.43773, "ay":-7.11424, "alpha":-0.60049, "fx":[124.15771,128.06123,128.91726,124.91846], "fy":[-123.46876,-119.41884,-118.47591,-122.68099]}, - {"t":0.95493, "x":1.74676, "y":7.2477, "heading":-0.9606, "vx":-1.61561, "vy":1.54534, "omega":0.13671, "ax":7.44094, "ay":-7.11731, "alpha":-0.61495, "fx":[124.16633,128.16885,129.02107,124.91683], "fy":[-123.56772,-119.41393,-118.47761,-122.79418]}, - {"t":0.99833, "x":1.68364, "y":7.30808, "heading":-0.95467, "vx":-1.29263, "vy":1.23641, "omega":0.11002, "ax":7.44308, "ay":-7.11935, "alpha":-0.62471, "fx":[124.17407,128.24422,129.08853,124.91188], "fy":[-123.63166,-119.40673,-118.48071,-122.87363]}, - {"t":1.04174, "x":1.63454, "y":7.35504, "heading":-0.94989, "vx":-0.96956, "vy":0.92739, "omega":0.0829, "ax":7.44461, "ay":-7.12082, "alpha":-0.63174, "fx":[124.18055,128.29973,129.13588,124.90654], "fy":[-123.67636,-119.39977,-118.48388,-122.93223]}, - {"t":1.08515, "x":1.59947, "y":7.38858, "heading":-0.94629, "vx":-0.64642, "vy":0.6183, "omega":0.05548, "ax":7.44576, "ay":-7.12191, "alpha":-0.63704, "fx":[124.18547,128.34163,129.17136,124.90225], "fy":[-123.70982,-119.39425,-118.4863,-122.97649]}, - {"t":1.12855, "x":1.57843, "y":7.40871, "heading":-0.94388, "vx":-0.32323, "vy":0.30917, "omega":0.02783, "ax":7.44665, "ay":-7.12277, "alpha":-0.64119, "fx":[124.1886,128.37339,129.19961,124.89975], "fy":[-123.73652,-119.39083,-118.48749,-123.01005]}, - {"t":1.17196, "x":1.57141, "y":7.41542, "heading":-0.94268, "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.85576, "y":5.07326, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.53361, "ay":8.35003, "alpha":1.69894, "fx":[-135.29516,-146.13702,-149.48934,-138.083], "fy":[146.20273,135.37161,131.63835,143.55109]}, + {"t":0.04461, "x":3.84727, "y":5.08157, "heading":-1.0472, "vx":-0.3807, "vy":0.37251, "omega":0.07579, "ax":-8.53318, "ay":8.34975, "alpha":1.57634, "fx":[-135.77905,-145.85828,-148.95191,-138.3866], "fy":[145.7101,135.62586,132.19697,143.21252]}, + {"t":0.08922, "x":3.82179, "y":5.1065, "heading":-1.04382, "vx":-0.76138, "vy":0.74501, "omega":0.14612, "ax":-8.53244, "ay":8.34924, "alpha":1.40351, "fx":[-136.47457,-145.47963,-148.18347,-138.78876], "fy":[144.99713,135.96655,132.98904,142.75845]}, + {"t":0.13384, "x":3.77934, "y":5.14804, "heading":-1.0373, "vx":-1.14202, "vy":1.11748, "omega":0.20873, "ax":-8.53101, "ay":8.34818, "alpha":1.14149, "fx":[-137.52888,-144.89347,-147.01851,-139.39076], "fy":[143.90244,136.49089,134.17312,142.0739]}, + {"t":0.17845, "x":3.7199, "y":5.2062, "heading":-1.02799, "vx":-1.52261, "vy":1.48991, "omega":0.25965, "ax":-8.52779, "ay":8.34559, "alpha":0.69727, "fx":[-139.29662,-143.83631,-145.0621,-140.42138], "fy":[142.02639,137.43224,136.11628,140.89315]}, + {"t":0.22306, "x":3.64349, "y":5.28097, "heading":-1.0164, "vx":-1.90305, "vy":1.86222, "omega":0.29076, "ax":-8.51789, "ay":8.33712, "alpha":-0.22035, "fx":[-142.89513,-141.43602,-141.08833,-142.53702], "fy":[138.04422,139.53622,139.89811,138.42433]}, + {"t":0.26767, "x":3.55011, "y":5.37235, "heading":-1.00343, "vx":-2.28304, "vy":2.23415, "omega":0.28093, "ax":-8.45458, "ay":8.27976, "alpha":-3.24685, "fx":[-154.46311,-131.99368,-128.55704,-148.72158], "fy":[123.5563,147.22848,150.55531,130.73814]}, + {"t":0.31228, "x":3.43985, "y":5.48026, "heading":-0.9909, "vx":-2.66022, "vy":2.60352, "omega":0.13608, "ax":-0.90434, "ay":0.86738, "alpha":-3.03619, "fx":[-17.37385,-4.81232,-12.88343,-25.23017], "fy":[4.17382,12.4575,24.70221,16.50197]}, + {"t":0.35689, "x":3.32027, "y":5.59727, "heading":-0.98483, "vx":-2.70056, "vy":2.64222, "omega":0.00063, "ax":-0.00077, "ay":-0.00063, "alpha":-0.00016, "fx":[-0.01294,-0.01231,-0.01273,-0.01337], "fy":[-0.01098,-0.01056,-0.00992,-0.01035]}, + {"t":0.40151, "x":3.1998, "y":5.71514, "heading":-0.9848, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":-0.00005, "ay":-0.00005, "alpha":0.0, "fx":[-0.0009,-0.0009,-0.0009,-0.0009], "fy":[-0.00092,-0.00092,-0.00092,-0.00092]}, + {"t":0.44612, "x":3.07932, "y":5.83301, "heading":-0.98477, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]}, + {"t":0.49073, "x":2.95884, "y":5.95088, "heading":-0.98474, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "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":0.53534, "x":2.83836, "y":6.06876, "heading":-0.98472, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "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.57995, "x":2.71788, "y":6.18663, "heading":-0.98469, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "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.62456, "x":2.59741, "y":6.3045, "heading":-0.98466, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "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":0.66918, "x":2.47693, "y":6.42237, "heading":-0.98463, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.00007,0.00007,0.00007,0.00007]}, + {"t":0.71379, "x":2.35645, "y":6.54025, "heading":-0.9846, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.00005, "ay":0.00005, "alpha":0.0, "fx":[0.00088,0.00088,0.00088,0.00088], "fy":[0.0009,0.0009,0.0009,0.0009]}, + {"t":0.7584, "x":2.23597, "y":6.65812, "heading":-0.98458, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.00076, "ay":0.00061, "alpha":0.00016, "fx":[0.01272,0.01208,0.01251,0.01314], "fy":[0.01076,0.01033,0.0097,0.01012]}, + {"t":0.80301, "x":2.11549, "y":6.77599, "heading":-0.98455, "vx":-2.70056, "vy":2.64222, "omega":0.00063, "ax":0.9032, "ay":-0.8666, "alpha":3.03908, "fx":[17.29153,4.76954,12.92714,25.23514], "fy":[-4.1377,-12.50823,-24.71076,-16.42645]}, + {"t":0.84762, "x":1.99592, "y":6.893, "heading":-0.98452, "vx":-2.66027, "vy":2.60356, "omega":0.13621, "ax":8.45532, "ay":-8.28031, "alpha":3.20948, "fx":[154.18578,131.84967,128.887,148.86202], "fy":[-123.90012,-147.36417,-150.2754,-130.5752]}, + {"t":0.89223, "x":1.88565, "y":7.00091, "heading":-0.97844, "vx":-2.28306, "vy":2.23416, "omega":0.27939, "ax":8.51793, "ay":-8.33711, "alpha":0.21384, "fx":[142.84828,141.41995,141.13668,142.55417], "fy":[-138.09283,-139.55309,-139.84968,-138.40657]}, + {"t":0.93685, "x":1.79228, "y":7.09229, "heading":-0.96598, "vx":-1.90306, "vy":1.86223, "omega":0.28893, "ax":8.52782, "ay":-8.34559, "alpha":-0.69454, "fx":[139.42349,144.00052,144.94458,140.24972], "fy":[-141.90268,-137.25972,-136.24069,-141.06472]}, + {"t":0.98146, "x":1.71586, "y":7.16706, "heading":-0.95309, "vx":-1.52262, "vy":1.48992, "omega":0.25795, "ax":8.53107, "ay":-8.3482, "alpha":-1.13454, "fx":[137.82035,145.24937,146.75901,139.00658], "fy":[-143.62451,-136.11127,-134.45556,-142.45089]}, + {"t":1.02607, "x":1.65642, "y":7.22522, "heading":-0.94158, "vx":-1.14204, "vy":1.11749, "omega":0.20733, "ax":8.53252, "ay":-8.34929, "alpha":-1.39429, "fx":[136.91132,146.00717,147.80473,138.20862], "fy":[-144.58619,-135.3989,-133.40818,-143.32143]}, + {"t":1.07068, "x":1.61397, "y":7.26676, "heading":-0.93233, "vx":-0.76139, "vy":0.74501, "omega":0.14513, "ax":8.53328, "ay":-8.34982, "alpha":-1.56576, "fx":[136.33206,146.52052,148.48052,137.64956], "fy":[-145.19422,-134.90912,-132.72442,-143.92239]}, + {"t":1.11529, "x":1.58849, "y":7.29169, "heading":-0.92586, "vx":-0.3807, "vy":0.37251, "omega":0.07528, "ax":8.53373, "ay":-8.35011, "alpha":-1.68742, "fx":[135.92832,146.88692,148.95566,137.24158], "fy":[-145.61559,-134.55655,-132.24017,-144.35701]}, + {"t":1.1599, "x":1.58, "y":7.3, "heading":-0.9225, "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/constants.traj b/src/main/deploy/choreo/constants.traj deleted file mode 100644 index b204ec95..00000000 --- a/src/main/deploy/choreo/constants.traj +++ /dev/null @@ -1,47 +0,0 @@ -{ - "name":"constants", - "version":1, - "snapshot":{ - "waypoints":[], - "constraints":[], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"305.5 in", "val":7.7597}, "y":{"exp":"4.359700000000012 m", "val":4.359700000000012}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"7.2570308 m", "val":7.2570308}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":59, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":93, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"215.84522795660308 in", "val":5.482468790097718}, "y":{"exp":"194.45724315432372 in", "val":4.939213976119822}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"204.63885923163247 in", "val":5.197827024483464}, "y":{"exp":"200.92724315432375 in", "val":5.103551976119823}, "heading":{"exp":"-120 deg", "val":-2.0943951023931953}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"165.15772795660308 in", "val":4.1950062900977185}, "y":{"exp":"210.33608215432375 in", "val":5.342536486719823}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"153.95135923163247 in", "val":3.9103645244834646}, "y":{"exp":"203.86608215432372 in", "val":5.178198486719822}, "heading":{"exp":"-60 deg", "val":-1.0471975511965976}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"174.378839 in", "val":4.4292225106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"126.0625 in", "val":3.2019875}, "y":{"exp":"161.438839 in", "val":4.1005465106}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"137.65477204339692 in", "val":3.4964312099022816}, "y":{"exp":"122.54275684567627 in", "val":3.1125860238801772}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"148.86114076836753 in", "val":3.781072975516535}, "y":{"exp":"116.07275684567627 in", "val":2.948248023880177}, "heading":{"exp":"60 deg", "val":1.0471975511965976}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"188.34227204339692 in", "val":4.783893709902282}, "y":{"exp":"106.66391784567627 in", "val":2.709263513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"199.54864076836753 in", "val":5.068535475516535}, "y":{"exp":"113.13391784567627 in", "val":2.873601513280177}, "heading":{"exp":"120 deg", "val":2.0943951023931953}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"142.621161 in", "val":3.6225774894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"227.4375 in", "val":5.7769125}, "y":{"exp":"155.561161 in", "val":3.9512534894}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":231, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"61.8667 in", "val":1.57141418}, "y":{"exp":"291.9457 in", "val":7.41542078}, "heading":{"exp":"-54.011392 deg", "val":-0.9426766239853251}, "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":true}, - {"from":2, "to":18, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":null, - "waypoints":[], - "samples":[], - "splits":[] - }, - "events":[] -} diff --git a/src/main/deploy/choreo/fetchToA.traj b/src/main/deploy/choreo/fetchToA.traj index 67b2739b..3946a8ce 100644 --- a/src/main/deploy/choreo/fetchToA.traj +++ b/src/main/deploy/choreo/fetchToA.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2654999999999994, "y":4.3309, "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}, @@ -15,9 +15,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":7, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "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}, @@ -30,47 +30,47 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.03509,1.27754], + "waypoints":[0.0,0.99526,1.27164], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24603, "ay":-7.86821, "alpha":31.52512, "fx":[197.09967,-97.14283,30.55113,152.60885], "fy":[-26.05183,-173.71935,-196.85068,-128.01496]}, - {"t":0.03339, "x":1.57378, "y":7.41103, "heading":-0.94268, "vx":0.14178, "vy":-0.26272, "omega":1.05263, "ax":4.34122, "ay":-8.32288, "alpha":28.83341, "fx":[192.8711,-85.85911,32.51262,149.93941], "fy":[-47.88162,-179.49501,-196.50175,-131.07519]}, - {"t":0.06678, "x":1.58094, "y":7.39762, "heading":-0.90753, "vx":0.28673, "vy":-0.54062, "omega":2.01538, "ax":4.50812, "ay":-8.92903, "alpha":24.66109, "fx":[182.1097,-67.42507,38.33478,147.57306], "fy":[-79.20159,-187.12275,-195.39876,-133.64756]}, - {"t":0.10017, "x":1.59302, "y":7.37459, "heading":-0.84024, "vx":0.43725, "vy":-0.83876, "omega":2.83881, "ax":4.62241, "ay":-9.71664, "alpha":18.64072, "fx":[156.83369,-39.47,47.70134,143.14811], "fy":[-121.56364,-194.8455,-193.24222,-138.23584]}, - {"t":0.13356, "x":1.6102, "y":7.34117, "heading":-0.74545, "vx":0.5916, "vy":-1.1632, "omega":3.46123, "ax":4.6333, "ay":-10.50408, "alpha":11.13401, "fx":[114.97145,3.8696,60.33313,129.76567], "fy":[-161.63846,-198.61814,-189.51175,-150.62346]}, - {"t":0.16695, "x":1.63253, "y":7.29648, "heading":-0.62988, "vx":0.7463, "vy":-1.51393, "omega":3.83299, "ax":4.66017, "ay":-10.95262, "alpha":0.27287, "fx":[78.10514,76.00762,77.27111,79.34734], "fy":[-182.38985,-183.2811,-182.76382,-181.86499]}, - {"t":0.20034, "x":1.66005, "y":7.23982, "heading":-0.50189, "vx":0.90191, "vy":-1.87964, "omega":3.8421, "ax":4.73038, "ay":-8.88499, "alpha":-21.54828, "fx":[65.97068,171.4458,147.62874,-69.6324], "fy":[-187.21217,-99.43423,-120.36262,-185.42486]}, - {"t":0.23373, "x":1.6928, "y":7.17211, "heading":-0.37361, "vx":1.05986, "vy":-2.17631, "omega":3.1226, "ax":2.92116, "ay":-9.36486, "alpha":-22.17167, "fx":[76.65885,180.07468,6.12119,-68.07719], "fy":[-182.8684,-81.87294,-173.91272,-185.77661]}, - {"t":0.26712, "x":1.72982, "y":7.09422, "heading":-0.26934, "vx":1.15739, "vy":-2.48901, "omega":2.38229, "ax":2.86596, "ay":-9.49345, "alpha":-22.12155, "fx":[86.66845,179.85858,-26.21498,-49.21516], "fy":[-177.88337,-80.69715,-183.28488,-191.13939]}, - {"t":0.30051, "x":1.77006, "y":7.00582, "heading":-0.1898, "vx":1.25309, "vy":-2.80599, "omega":1.64365, "ax":3.13943, "ay":-9.32351, "alpha":-22.42256, "fx":[94.91208,180.50523,-29.90121,-36.18513], "fy":[-172.66862,-75.85574,-180.17407,-192.97512]}, - {"t":0.3339, "x":1.81365, "y":6.90693, "heading":-0.13491, "vx":1.35791, "vy":-3.11731, "omega":0.89496, "ax":3.5182, "ay":-8.89034, "alpha":-22.47239, "fx":[101.0819,178.97434,-20.07277,-25.39633], "fy":[-166.01209,-68.64582,-166.86763,-191.26514]}, - {"t":0.36729, "x":1.86096, "y":6.79789, "heading":-0.10503, "vx":1.47539, "vy":-3.41416, "omega":0.1446, "ax":1.74959, "ay":-0.98, "alpha":-3.82108, "fx":[37.04347,39.47388,21.21779,18.92416], "fy":[-26.38366,-7.82979,-6.0141,-25.11716]}, - {"t":0.40068, "x":1.9112, "y":6.68334, "heading":-0.1002, "vx":1.53381, "vy":-3.44688, "omega":0.01702, "ax":-0.26519, "ay":-0.12028, "alpha":-0.00526, "fx":[-4.40916,-4.40661,-4.43207,-4.43462], "fy":[-2.01898,-1.9935,-1.99093,-2.01642]}, - {"t":0.43407, "x":1.96226, "y":6.56818, "heading":-0.09964, "vx":1.52495, "vy":-3.45089, "omega":0.01684, "ax":-1.90039, "ay":-0.82566, "alpha":-0.01317, "fx":[-31.64976,-31.64491,-31.70758,-31.71243], "fy":[-13.79975,-13.73474,-13.72684,-13.79184]}, - {"t":0.46746, "x":2.01212, "y":6.4525, "heading":-0.09907, "vx":1.4615, "vy":-3.47846, "omega":0.0164, "ax":-2.31293, "ay":-0.94746, "alpha":-0.01083, "fx":[-38.53158,-38.52813,-38.57916,-38.58261], "fy":[-15.82415,-15.77016,-15.76317,-15.81715]}, - {"t":0.50085, "x":2.05963, "y":6.33582, "heading":-0.09853, "vx":1.38427, "vy":-3.5101, "omega":0.01604, "ax":-0.97243, "ay":-0.37772, "alpha":0.0024, "fx":[-16.21519,-16.21628,-16.20469,-16.20361], "fy":[-6.28994,-6.30164,-6.30286,-6.29116]}, - {"t":0.53424, "x":2.10531, "y":6.21841, "heading":-0.09799, "vx":1.3518, "vy":-3.52271, "omega":0.01612, "ax":-0.28792, "ay":-0.10989, "alpha":0.00064, "fx":[-4.80085,-4.80116,-4.79808,-4.79778], "fy":[-1.83016,-1.83325,-1.83355,-1.83047]}, - {"t":0.56763, "x":2.15028, "y":6.10072, "heading":-0.09745, "vx":1.34218, "vy":-3.52638, "omega":0.01614, "ax":-0.12773, "ay":-0.04838, "alpha":0.00051, "fx":[-2.13035,-2.13059,-2.12813,-2.12789], "fy":[-0.80513,-0.80759,-0.80783,-0.80537]}, - {"t":0.60102, "x":2.19503, "y":5.98295, "heading":-0.09691, "vx":1.33792, "vy":-3.52799, "omega":0.01616, "ax":-0.08576, "ay":-0.03226, "alpha":0.00068, "fx":[-1.43106,-1.43138,-1.42809,-1.42777], "fy":[-0.53589,-0.53919,-0.53951,-0.53621]}, - {"t":0.63441, "x":2.23965, "y":5.86513, "heading":-0.09637, "vx":1.33506, "vy":-3.52907, "omega":0.01618, "ax":-0.07137, "ay":-0.02667, "alpha":0.00087, "fx":[-1.19158,-1.19199,-1.18776,-1.18735], "fy":[-0.44219,-0.44642,-0.44683,-0.44259]}, - {"t":0.6678, "x":2.28419, "y":5.74728, "heading":-0.09583, "vx":1.33267, "vy":-3.52996, "omega":0.01621, "ax":-0.06336, "ay":-0.02353, "alpha":0.00102, "fx":[-1.05848,-1.05896,-1.05399,-1.05352], "fy":[-0.38956,-0.39452,-0.395,-0.39004]}, - {"t":0.70119, "x":2.32866, "y":5.6294, "heading":-0.09529, "vx":1.33056, "vy":-3.53075, "omega":0.01625, "ax":-0.05122, "ay":-0.01889, "alpha":0.0011, "fx":[-0.85628,-0.85679,-0.85147,-0.85096], "fy":[-0.31193,-0.31725,-0.31776,-0.31244]}, - {"t":0.73458, "x":2.37305, "y":5.5115, "heading":-0.09475, "vx":1.32885, "vy":-3.53138, "omega":0.01628, "ax":-0.00109, "ay":-0.00003, "alpha":0.00097, "fx":[-0.02027,-0.02071,-0.016,-0.01556], "fy":[0.00207,-0.00264,-0.00309,0.00162]}, - {"t":0.76797, "x":2.41742, "y":5.39359, "heading":-0.0942, "vx":1.32881, "vy":-3.53138, "omega":0.01631, "ax":0.29602, "ay":0.11199, "alpha":-0.00006, "fx":[4.93472,4.93475,4.93445,4.93441], "fy":[1.86667,1.86697,1.86701,1.8667]}, - {"t":0.80136, "x":2.46196, "y":5.27574, "heading":-0.09366, "vx":1.33869, "vy":-3.52764, "omega":0.01631, "ax":2.11039, "ay":0.82439, "alpha":-0.00488, "fx":[35.18997,35.19157,35.16849,35.1669], "fy":[13.72857,13.75278,13.75565,13.73145]}, - {"t":0.83475, "x":2.50783, "y":5.15841, "heading":-0.09312, "vx":1.40916, "vy":-3.50011, "omega":0.01615, "ax":7.43146, "ay":3.31775, "alpha":0.0121, "fx":[123.84946,123.86591,123.90835,123.89191], "fy":[55.35654,55.28158,55.25418,55.32912]}, - {"t":0.86814, "x":2.55903, "y":5.04339, "heading":-0.09258, "vx":1.6573, "vy":-3.38933, "omega":0.01655, "ax":9.13813, "ay":5.97783, "alpha":1.92396, "fx":[145.4419,152.34231,158.87906,152.6496], "fy":[109.67277,97.90274,89.60182,101.41271]}, - {"t":0.90153, "x":2.61946, "y":4.93355, "heading":-0.09202, "vx":1.96242, "vy":-3.18973, "omega":0.0808, "ax":3.85026, "ay":10.20358, "alpha":14.51791, "fx":[10.04811,1.81034,145.85885,99.01084], "fy":[194.78479,190.63982,126.41662,168.51355]}, - {"t":0.93492, "x":2.68713, "y":4.83273, "heading":-0.08933, "vx":2.09098, "vy":-2.84904, "omega":0.56555, "ax":2.54346, "ay":10.70286, "alpha":15.40215, "fx":[-9.50515,-35.66912,130.44702,84.32019], "fy":[197.03823,191.95886,146.18897,178.46009]}, - {"t":0.96831, "x":2.75837, "y":4.74357, "heading":-0.07044, "vx":2.17591, "vy":-2.49167, "omega":1.07983, "ax":0.58976, "ay":11.84469, "alpha":-1.49711, "fx":[17.15511,16.93705,3.02913,2.203], "fy":[197.01869,197.15144,197.85654,197.7545]}, - {"t":1.0017, "x":2.83135, "y":4.66698, "heading":-0.03439, "vx":2.1956, "vy":-2.09617, "omega":1.02984, "ax":-1.11606, "ay":10.85556, "alpha":-17.36681, "fx":[72.6067,38.57372,-64.20136,-121.39582], "fy":[184.15255,194.9137,188.07428,156.68766]}, - {"t":1.03509, "x":2.90404, "y":4.60304, "heading":0.0, "vx":2.15833, "vy":-1.7337, "omega":0.44996, "ax":-3.93079, "ay":10.92711, "alpha":-9.30858, "fx":[-32.46623,-20.25728,-86.74178,-122.63196], "fy":[195.73042,197.73765,178.91191,156.21867]}, - {"t":1.06973, "x":2.97644, "y":4.54954, "heading":0.01558, "vx":2.02219, "vy":-1.35523, "omega":0.12755, "ax":-7.15443, "ay":9.41749, "alpha":-5.24384, "fx":[-114.80865,-90.95848,-124.01062,-147.26558], "fy":[162.13232,176.7925,155.48799,133.52759]}, - {"t":1.10436, "x":3.04219, "y":4.50825, "heading":0.02, "vx":1.77439, "vy":-1.02905, "omega":-0.05408, "ax":-8.94206, "ay":7.86654, "alpha":-2.51381, "fx":[-149.85732,-137.33401,-148.90751,-160.14075], "fy":[130.72408,143.88655,131.9304,117.98447]}, - {"t":1.139, "x":3.09828, "y":4.47733, "heading":0.01813, "vx":1.46467, "vy":-0.75658, "omega":-0.14115, "ax":-9.9204, "ay":6.63748, "alpha":-0.61862, "fx":[-165.8059,-163.01288,-164.98351,-167.67102], "fy":[110.01289,114.12277,111.2717,107.16717]}, - {"t":1.17364, "x":3.14306, "y":4.4551, "heading":0.01324, "vx":1.12107, "vy":-0.52669, "omega":-0.16257, "ax":-10.48383, "ay":5.71315, "alpha":0.69955, "fx":[-174.18199,-176.94308,-175.41121,-172.50564], "fy":[96.36502,91.18468,94.07673,99.3157]}, - {"t":1.20827, "x":3.1756, "y":4.44029, "heading":0.00761, "vx":0.75795, "vy":-0.32881, "omega":-0.13834, "ax":-10.8294, "ay":5.01528, "alpha":1.6448, "fx":[-179.12751,-184.80896,-182.30919,-175.83779], "fy":[86.94944,74.08836,79.98916,93.38227]}, - {"t":1.24291, "x":3.19536, "y":4.43191, "heading":0.00282, "vx":0.38286, "vy":-0.1551, "omega":-0.08137, "ax":-11.0539, "ay":4.47792, "alpha":2.34938, "fx":[-182.31954,-189.49675,-186.97816,-178.25854], "fy":[80.13611,61.23389,68.45986,88.74925]}, - {"t":1.27754, "x":3.20199, "y":4.42922, "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.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.37175, "ay":-7.48175, "alpha":33.10335, "fx":[198.66606,-98.8079,34.44026,157.20141], "fy":[-7.565,-172.76859,-196.20581,-122.32964]}, + {"t":0.03318, "x":1.58241, "y":7.29588, "heading":-0.9225, "vx":0.14503, "vy":-0.24821, "omega":1.09822, "ax":4.50093, "ay":-7.95038, "alpha":30.38104, "fx":[196.42291,-87.37293,36.39949,154.66383], "fy":[-30.08741,-178.75035,-195.8162,-125.46224]}, + {"t":0.06635, "x":1.58969, "y":7.28327, "heading":-0.88607, "vx":0.29435, "vy":-0.51197, "omega":2.10612, "ax":4.72333, "ay":-8.58592, "alpha":26.06711, "fx":[188.24685,-68.2282,42.4103,152.51334], "fy":[-63.15336,-186.81466,-194.55022,-127.97448]}, + {"t":0.09953, "x":1.60206, "y":7.26156, "heading":-0.8162, "vx":0.45105, "vy":-0.79681, "omega":2.9709, "ax":4.89542, "ay":-9.45137, "alpha":19.60797, "fx":[164.71427,-38.80184,52.14006,148.36503], "fy":[-110.55148,-194.95694,-192.08338,-132.60712]}, + {"t":0.1327, "x":1.61972, "y":7.22993, "heading":-0.71764, "vx":0.61346, "vy":-1.11036, "omega":3.6214, "ax":4.92551, "ay":-10.34068, "alpha":11.44697, "fx":[120.87881,7.3257,65.28107,134.93774], "fy":[-157.19078,-198.48623,-187.84367,-145.97572]}, + {"t":0.16588, "x":1.64278, "y":7.1874, "heading":-0.59749, "vx":0.77687, "vy":-1.45342, "omega":4.00116, "ax":4.9465, "ay":-10.822, "alpha":-0.3874, "fx":[81.89601,84.79126,83.0369,80.09898], "fy":[-180.67652,-179.3252,-180.1258,-181.46254]}, + {"t":0.19905, "x":1.67127, "y":7.13323, "heading":-0.46475, "vx":0.94097, "vy":-1.81244, "omega":3.98831, "ax":4.99099, "ay":-8.52292, "alpha":-22.56257, "fx":[71.05427,175.87142,154.61955,-68.75546], "fy":[-185.30378,-91.24956,-106.03434,-185.70451]}, + {"t":0.23223, "x":1.70524, "y":7.06841, "heading":-0.33244, "vx":1.10655, "vy":-2.09519, "omega":3.23979, "ax":2.98615, "ay":-9.39468, "alpha":-22.09954, "fx":[82.2634,181.52201,-4.39824,-60.27639], "fy":[-180.35752,-78.38001,-179.29189,-188.38994]}, + {"t":0.2654, "x":1.74359, "y":6.99373, "heading":-0.22496, "vx":1.20561, "vy":-2.40686, "omega":2.50663, "ax":3.11534, "ay":-9.44504, "alpha":-22.06113, "fx":[92.29467,181.28857,-24.60156,-41.25706], "fy":[-174.95695,-77.12505,-184.74689,-192.94803]}, + {"t":0.29858, "x":1.7853, "y":6.90868, "heading":-0.1418, "vx":1.30896, "vy":-2.72021, "omega":1.77474, "ax":3.40318, "ay":-9.25368, "alpha":-22.40658, "fx":[100.70062,182.00074,-27.69127,-28.09242], "fy":[-169.27649,-71.79149,-181.68722,-194.26215]}, + {"t":0.33175, "x":1.8306, "y":6.81335, "heading":-0.08292, "vx":1.42187, "vy":-3.0272, "omega":1.0314, "ax":3.74552, "ay":-8.82264, "alpha":-22.5682, "fx":[107.01644,180.67119,-20.47081,-17.47272], "fy":[-162.31676,-63.99655,-169.66042,-192.30278]}, + {"t":0.36493, "x":1.87983, "y":6.70807, "heading":-0.04871, "vx":1.54612, "vy":-3.31989, "omega":0.28269, "ax":2.39255, "ay":-2.53056, "alpha":-7.9516, "fx":[56.18168,62.2805,22.50174,18.56685], "fy":[-60.25273,-22.87184,-22.82632,-62.78168]}, + {"t":0.3981, "x":1.93244, "y":6.59653, "heading":-0.03933, "vx":1.6255, "vy":-3.40385, "omega":0.01889, "ax":-1.70452, "ay":-0.81434, "alpha":-0.03763, "fx":[-28.32521,-28.32157,-28.50199,-28.50561], "fy":[-13.67307,-13.48719,-13.47624,-13.66207]}, + {"t":0.43128, "x":1.98543, "y":6.48316, "heading":-0.0387, "vx":1.56895, "vy":-3.43086, "omega":0.01764, "ax":-2.1554, "ay":-0.96317, "alpha":-0.00731, "fx":[-35.91236,-35.91203,-35.94672,-35.94705], "fy":[-16.07513,-16.03867,-16.0362,-16.07266]}, + {"t":0.46446, "x":2.03629, "y":6.36881, "heading":-0.03812, "vx":1.49744, "vy":-3.46282, "omega":0.0174, "ax":-0.79999, "ay":-0.3418, "alpha":0.00227, "fx":[-13.34077,-13.34107,-13.33009,-13.32978], "fy":[-5.69195,-5.70301,-5.70341,-5.69234]}, + {"t":0.49763, "x":2.08553, "y":6.25374, "heading":-0.03754, "vx":1.4709, "vy":-3.47415, "omega":0.01747, "ax":-0.25477, "ay":-0.10734, "alpha":0.00074, "fx":[-4.24864,-4.24878,-4.24518,-4.24505], "fy":[-1.78743,-1.79103,-1.79116,-1.78756]}, + {"t":0.53081, "x":2.13419, "y":6.13843, "heading":-0.03696, "vx":1.46245, "vy":-3.47772, "omega":0.0175, "ax":-0.12186, "ay":-0.05097, "alpha":0.00066, "fx":[-2.03285,-2.03294,-2.02975,-2.02966], "fy":[-0.84804,-0.85123,-0.85133,-0.84814]}, + {"t":0.56398, "x":2.18264, "y":6.02303, "heading":-0.03638, "vx":1.45841, "vy":-3.47941, "omega":0.01752, "ax":-0.08524, "ay":-0.0354, "alpha":0.00086, "fx":[-1.42293,-1.4231,-1.4189,-1.41873], "fy":[-0.58798,-0.59218,-0.59235,-0.58815]}, + {"t":0.59716, "x":2.23098, "y":5.90758, "heading":-0.0358, "vx":1.45558, "vy":-3.48058, "omega":0.01755, "ax":-0.07234, "ay":-0.02984, "alpha":0.00109, "fx":[-1.20841,-1.2086,-1.2033,-1.20311], "fy":[-0.49473,-0.50004,-0.50023,-0.49492]}, + {"t":0.63033, "x":2.27923, "y":5.79209, "heading":-0.03521, "vx":1.45318, "vy":-3.48157, "omega":0.01759, "ax":-0.06581, "ay":-0.02699, "alpha":0.00127, "fx":[-1.10004,-1.10027,-1.09409,-1.09385], "fy":[-0.44672,-0.45291,-0.45314,-0.44696]}, + {"t":0.66351, "x":2.3274, "y":5.67657, "heading":-0.03463, "vx":1.451, "vy":-3.48247, "omega":0.01763, "ax":-0.06019, "ay":-0.02455, "alpha":0.00138, "fx":[-1.00652,-1.00675,-1.00003,-0.99979], "fy":[-0.40584,-0.41256,-0.4128,-0.40607]}, + {"t":0.69668, "x":2.3755, "y":5.56103, "heading":-0.03405, "vx":1.449, "vy":-3.48328, "omega":0.01767, "ax":-0.05115, "ay":-0.02074, "alpha":0.00141, "fx":[-0.85595,-0.85619,-0.84932,-0.84908], "fy":[-0.34222,-0.34908,-0.34933,-0.34246]}, + {"t":0.72986, "x":2.42355, "y":5.44546, "heading":-0.03346, "vx":1.4473, "vy":-3.48397, "omega":0.01772, "ax":-0.02124, "ay":-0.00833, "alpha":0.00128, "fx":[-0.35699,-0.3572,-0.35099,-0.35078], "fy":[-0.13571,-0.14192,-0.14213,-0.13592]}, + {"t":0.76303, "x":2.47155, "y":5.32987, "heading":-0.03287, "vx":1.4466, "vy":-3.48425, "omega":0.01776, "ax":0.13962, "ay":0.05832, "alpha":0.00043, "fx":[2.32635,2.32628,2.32836,2.32843], "fy":[0.97332,0.97125,0.97117,0.97325]}, + {"t":0.79621, "x":2.51962, "y":5.21431, "heading":-0.03228, "vx":1.45123, "vy":-3.48231, "omega":0.01778, "ax":1.12296, "ay":0.47436, "alpha":-0.00357, "fx":[18.72771,18.72813,18.71085,18.71043], "fy":[7.89832,7.91585,7.91654,7.89901]}, + {"t":0.82938, "x":2.56838, "y":5.09905, "heading":-0.03169, "vx":1.48849, "vy":-3.46657, "omega":0.01766, "ax":5.4701, "ay":2.52125, "alpha":-0.01179, "fx":[91.21294,91.20339,91.15509,91.16464], "fy":[41.98819,42.05469,42.06789,42.00137]}, + {"t":0.86256, "x":2.62077, "y":4.98543, "heading":-0.03111, "vx":1.66996, "vy":-3.38293, "omega":0.01727, "ax":9.06286, "ay":5.10025, "alpha":0.19172, "fx":[150.4861,151.07491,151.65753,151.07538], "fy":[85.98164,84.69923,84.05738,85.33641]}, + {"t":0.89573, "x":2.68116, "y":4.876, "heading":-0.03053, "vx":1.97062, "vy":-3.21373, "omega":0.02363, "ax":7.44331, "ay":8.46713, "alpha":6.46326, "fx":[91.98616,120.12888,154.83817,129.35264], "fy":[167.19016,144.48125,110.93303,141.96783]}, + {"t":0.92891, "x":2.75063, "y":4.77405, "heading":-0.02975, "vx":2.21756, "vy":-2.93283, "omega":0.23805, "ax":3.83408, "ay":10.49287, "alpha":12.68026, "fx":[10.20347,13.96133,138.6035,92.88068], "fy":[196.02037,193.0152,137.32369,173.28522]}, + {"t":0.96209, "x":2.82631, "y":4.68252, "heading":-0.02185, "vx":2.34475, "vy":-2.58472, "omega":0.65872, "ax":-0.11229, "ay":11.37823, "alpha":-11.85497, "fx":[63.41772,39.99402,-41.0631,-69.83581], "fy":[186.62869,193.93447,193.72518,184.39012]}, + {"t":0.99526, "x":2.90404, "y":4.60304, "heading":0.0, "vx":2.34103, "vy":-2.20725, "omega":0.26543, "ax":-3.95659, "ay":11.11062, "alpha":-5.39387, "fx":[-48.56906,-37.27287,-79.15539,-98.82061], "fy":[192.07413,194.88149,182.01072,171.86817]}, + {"t":1.02981, "x":2.98256, "y":4.53341, "heading":0.00917, "vx":2.20434, "vy":-1.8234, "omega":0.07908, "ax":-6.93754, "ay":9.65253, "alpha":-2.81793, "fx":[-112.88706,-100.21561,-118.40107,-131.07813], "fy":[163.32796,171.50654,159.55883,149.21902]}, + {"t":1.06436, "x":3.05457, "y":4.47618, "heading":0.0119, "vx":1.96466, "vy":-1.48992, "omega":-0.01827, "ax":-8.32735, "ay":8.52863, "alpha":-1.37406, "fx":[-138.70229,-131.99336,-139.0633,-145.49267], "fy":[142.39326,148.67326,142.11672,135.48933]}, + {"t":1.0989, "x":3.11748, "y":4.42979, "heading":0.01127, "vx":1.67697, "vy":-1.19527, "omega":-0.06574, "ax":-9.06741, "ay":7.75712, "alpha":-0.49271, "fx":[-151.30295,-148.9393,-151.0222,-153.33282], "fy":[129.13794,131.86815,129.4898,126.73429]}, + {"t":1.13345, "x":3.17, "y":4.39313, "heading":0.009, "vx":1.36371, "vy":-0.92728, "omega":-0.08277, "ax":-9.5112, "ay":7.21536, "alpha":0.08741, "fx":[-158.50231,-158.90699,-158.59283,-158.1862], "fy":[120.33811,119.80161,120.21508,120.75126]}, + {"t":1.168, "x":3.21144, "y":4.3654, "heading":0.00614, "vx":1.03511, "vy":-0.67801, "omega":-0.07975, "ax":-9.80192, "ay":6.81961, "alpha":0.49384, "fx":[-163.08343,-165.28855,-163.73648,-161.46457], "fy":[114.15818,110.93368,113.20073,116.42613]}, + {"t":1.20255, "x":3.24135, "y":4.34604, "heading":0.00338, "vx":0.69648, "vy":-0.4424, "omega":-0.06269, "ax":-10.0052, "ay":6.51981, "alpha":0.79289, "fx":[-166.22778,-169.65423,-167.42547,-163.81989], "fy":[109.60871,104.21595,107.74301,113.1607]}, + {"t":1.2371, "x":3.25944, "y":4.33465, "heading":0.00122, "vx":0.35082, "vy":-0.21716, "omega":-0.03529, "ax":-10.1545, "ay":6.28561, "alpha":1.02155, "fx":[-168.50996,-172.79872,-170.18327,-165.59052], "fy":[106.12894,98.98061,103.39212,110.61094]}, + {"t":1.27164, "x":3.2655, "y":4.3309, "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/fetchToB.traj b/src/main/deploy/choreo/fetchToB.traj index 27c65882..b5a4dcc7 100644 --- a/src/main/deploy/choreo/fetchToB.traj +++ b/src/main/deploy/choreo/fetchToB.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.8945255279541016, "y":4.331230163574219, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2019875, "y":4.1005465106, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.8945255279541016, "y":4.331230163574219, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.2654999999999994, "y":4.0009, "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}, @@ -15,9 +15,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.8945255279541016 m", "val":2.8945255279541016}, "y":{"exp":"4.331230163574219 m", "val":4.331230163574219}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.8945255279541016 m", "val":2.8945255279541016}, "y":{"exp":"4.331230163574219 m", "val":4.331230163574219}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "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}, @@ -30,49 +30,49 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.04495,1.32048], + "waypoints":[0.0,1.05745,1.34678], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.01035, "ay":-8.45166, "alpha":16.07271, "fx":[140.65173,-27.32961,38.35413,121.18355], "fy":[-104.26569,-173.04045,-171.06693,-126.66854]}, - {"t":0.03265, "x":1.57355, "y":7.41091, "heading":-0.94268, "vx":0.13096, "vy":-0.27599, "omega":0.52485, "ax":4.02155, "ay":-8.54436, "alpha":15.24056, "fx":[137.07046,-22.56442,39.30305,119.81286], "fy":[-108.8808,-173.69739,-170.83218,-127.93837]}, - {"t":0.06531, "x":1.57997, "y":7.39735, "heading":-0.92554, "vx":0.26228, "vy":-0.555, "omega":1.02253, "ax":4.04482, "ay":-8.65308, "alpha":14.15042, "fx":[131.77992,-16.71328,41.45925,118.67879], "fy":[-115.1656,-174.32688,-170.29786,-128.95561]}, - {"t":0.09796, "x":1.59069, "y":7.37461, "heading":-0.89215, "vx":0.39436, "vy":-0.83756, "omega":1.4846, "ax":4.07257, "ay":-8.77902, "alpha":12.78343, "fx":[124.40952,-9.50449,44.73385,117.45406], "fy":[-123.01945,-174.83339,-169.43638,-130.02539]}, - {"t":0.13062, "x":1.60574, "y":7.34258, "heading":-0.84367, "vx":0.52735, "vy":-1.12424, "omega":1.90204, "ax":4.09785, "ay":-8.92061, "alpha":11.12242, "fx":[114.66519,-0.45705,49.01091,115.59381], "fy":[-132.07268,-175.04914,-168.20743,-131.6189]}, - {"t":0.16327, "x":1.62515, "y":7.30111, "heading":-0.78156, "vx":0.66116, "vy":-1.41554, "omega":2.26524, "ax":4.11827, "ay":-9.07107, "alpha":9.14377, "fx":[102.70362,11.21254,54.1445,112.14186], "fy":[-141.49819,-174.63606,-166.56604,-134.48482]}, - {"t":0.19593, "x":1.64893, "y":7.25005, "heading":-0.70759, "vx":0.79565, "vy":-1.71175, "omega":2.56383, "ax":4.13909, "ay":-9.21852, "alpha":6.7365, "fx":[89.59245,26.82443,59.95566,105.24634], "fy":[-150.07314,-172.85144,-164.4699,-139.82288]}, - {"t":0.22858, "x":1.67712, "y":7.18924, "heading":-0.62387, "vx":0.93081, "vy":-2.01278, "omega":2.78381, "ax":4.16404, "ay":-9.34397, "alpha":3.47967, "fx":[77.46988,48.75196,66.22148,90.87292], "fy":[-156.6142,-167.87298,-161.8814,-149.38442]}, - {"t":0.26124, "x":1.70974, "y":7.11853, "heading":-0.53296, "vx":1.06678, "vy":-2.31791, "omega":2.89744, "ax":4.15269, "ay":-9.37539, "alpha":-1.74725, "fx":[69.05409,81.24254,72.61503,59.63236], "fy":[-160.44802,-154.58566,-158.74043,-164.11702]}, - {"t":0.29389, "x":1.74679, "y":7.03784, "heading":-0.43835, "vx":1.20239, "vy":-2.62406, "omega":2.84038, "ax":3.95407, "ay":-8.93626, "alpha":-10.65471, "fx":[66.77369,128.55417,78.1576,-4.45505], "fy":[-161.35427,-117.81763,-154.57763,-174.2635]}, - {"t":0.32655, "x":1.78816, "y":6.94739, "heading":-0.34559, "vx":1.33151, "vy":-2.91587, "omega":2.49245, "ax":2.79739, "ay":-8.07304, "alpha":-19.063, "fx":[71.61106,159.68161,13.3555,-54.31678], "fy":[-159.10674,-69.23754,-155.5354,-165.40088]}, - {"t":0.3592, "x":1.83313, "y":6.84787, "heading":-0.2642, "vx":1.42285, "vy":-3.17949, "omega":1.86996, "ax":2.58691, "ay":-8.15952, "alpha":-19.29816, "fx":[77.29401,158.46985,-17.95889,-41.79448], "fy":[-155.89936,-70.12184,-160.73275,-168.41029]}, - {"t":0.39186, "x":1.88097, "y":6.73969, "heading":-0.20314, "vx":1.50733, "vy":-3.44594, "omega":1.23978, "ax":2.61309, "ay":-8.05874, "alpha":-19.43561, "fx":[80.44854,155.98352,-25.27019,-33.37047], "fy":[-152.91897,-70.95584,-155.6936,-168.73926]}, - {"t":0.42451, "x":1.93159, "y":6.62287, "heading":-0.16266, "vx":1.59266, "vy":-3.70909, "omega":0.60512, "ax":2.13071, "ay":-7.46212, "alpha":-18.09747, "fx":[69.79988,131.24631,-27.64446,-28.4309], "fy":[-146.32928,-81.35265,-123.2297,-156.8026]}, - {"t":0.45717, "x":1.98473, "y":6.49777, "heading":-0.1429, "vx":1.66224, "vy":-3.95277, "omega":0.01415, "ax":-5.8618, "ay":-2.37965, "alpha":-0.19334, "fx":[-99.27616,-99.4231,-100.13849,-99.99273], "fy":[-41.25962,-40.10945,-39.69664,-40.84278]}, - {"t":0.48982, "x":2.03589, "y":6.36743, "heading":-0.14243, "vx":1.47082, "vy":-4.03047, "omega":0.00784, "ax":-5.98794, "ay":-2.02498, "alpha":-0.00312, "fx":[-101.84668,-101.84861,-101.85965,-101.85773], "fy":[-34.45691,-34.43783,-34.43163,-34.45071]}, - {"t":0.52248, "x":2.08072, "y":6.23473, "heading":-0.14218, "vx":1.27529, "vy":-4.0966, "omega":0.00773, "ax":-2.47545, "ay":-0.7441, "alpha":0.00007, "fx":[-42.10685,-42.10689,-42.10661,-42.10657], "fy":[-12.65671,-12.65705,-12.6572,-12.65687]}, - {"t":0.55513, "x":2.12105, "y":6.10056, "heading":-0.14193, "vx":1.19445, "vy":-4.1209, "omega":0.00774, "ax":-0.7372, "ay":-0.21131, "alpha":0.00008, "fx":[-12.53979,-12.53985,-12.53948,-12.53943], "fy":[-3.59402,-3.59441,-3.5945,-3.59411]}, - {"t":0.58778, "x":2.15966, "y":5.96588, "heading":-0.14167, "vx":1.17038, "vy":-4.1278, "omega":0.00774, "ax":-0.25749, "ay":-0.07264, "alpha":0.00017, "fx":[-4.3801,-4.38021,-4.37941,-4.37929], "fy":[-1.23514,-1.23594,-1.23607,-1.23526]}, - {"t":0.62044, "x":2.19774, "y":5.83105, "heading":-0.14142, "vx":1.16197, "vy":-4.13017, "omega":0.00774, "ax":-0.11762, "ay":-0.03292, "alpha":0.0003, "fx":[-2.00128,-2.00148,-2.00002,-1.99981], "fy":[-0.55906,-0.56052,-0.56073,-0.55926]}, - {"t":0.65309, "x":2.23562, "y":5.69616, "heading":-0.14117, "vx":1.15813, "vy":-4.13125, "omega":0.00775, "ax":-0.01519, "ay":-0.00412, "alpha":0.00039, "fx":[-0.25917,-0.25944,-0.25757,-0.25731], "fy":[-0.06897,-0.07083,-0.07109,-0.06923]}, - {"t":0.68575, "x":2.27343, "y":5.56126, "heading":-0.14091, "vx":1.15763, "vy":-4.13138, "omega":0.00777, "ax":0.28473, "ay":0.08028, "alpha":0.0003, "fx":[4.84261,4.84242,4.84388,4.84408], "fy":[1.36631,1.36484,1.36464,1.36611]}, - {"t":0.7184, "x":2.31138, "y":5.42639, "heading":-0.14066, "vx":1.16693, "vy":-4.12876, "omega":0.00778, "ax":1.48445, "ay":0.4292, "alpha":-0.0001, "fx":[25.25032,25.25041,25.24992,25.24984], "fy":[7.30025,7.30075,7.30086,7.30036]}, - {"t":0.75106, "x":2.35028, "y":5.2918, "heading":-0.14041, "vx":1.2154, "vy":-4.11474, "omega":0.00777, "ax":4.95774, "ay":1.57261, "alpha":-0.00028, "fx":[84.33034,84.3303,84.3292,84.32924], "fy":[26.74867,26.75024,26.75071,26.74916]}, - {"t":0.78371, "x":2.39261, "y":5.15827, "heading":-0.14015, "vx":1.3773, "vy":-4.06339, "omega":0.00776, "ax":7.90991, "ay":2.97411, "alpha":0.00993, "fx":[134.52296,134.5408,134.56749,134.54967], "fy":[50.63898,50.56989,50.53856,50.60764]}, - {"t":0.81637, "x":2.44181, "y":5.02717, "heading":-0.1399, "vx":1.63559, "vy":-3.96627, "omega":0.00809, "ax":8.5174, "ay":4.2927, "alpha":0.88592, "fx":[142.25473,144.98959,147.40992,144.85995], "fy":[77.96769,71.90982,68.09207,74.10047]}, - {"t":0.84902, "x":2.49976, "y":4.89994, "heading":-0.13964, "vx":1.91373, "vy":-3.82609, "omega":0.03702, "ax":3.37829, "ay":8.64748, "alpha":13.94404, "fx":[10.03426,-5.42596,133.46189,91.78456], "fy":[171.28351,167.25169,104.47651,145.35311]}, - {"t":0.88168, "x":2.56405, "y":4.77961, "heading":-0.13843, "vx":2.02404, "vy":-3.54371, "omega":0.49236, "ax":1.39368, "ay":9.15961, "alpha":15.79525, "fx":[-20.47114,-64.4684,109.95837,69.80555], "fy":[172.4672,159.4116,132.28892,159.04192]}, - {"t":0.91433, "x":2.63089, "y":4.66877, "heading":-0.12235, "vx":2.06955, "vy":-3.24461, "omega":1.00815, "ax":-0.1924, "ay":10.20908, "alpha":2.33866, "fx":[-12.16656,-16.66784,6.99698,8.74706], "fy":[173.72588,173.17492,173.80321,173.91024]}, - {"t":0.94699, "x":2.69837, "y":4.56826, "heading":-0.08943, "vx":2.06327, "vy":-2.91123, "omega":1.08451, "ax":-1.04697, "ay":10.14015, "alpha":-4.11164, "fx":[-0.16299,3.06975,-31.51045,-42.63089], "fy":[174.39462,174.58872,171.77876,169.16196]}, - {"t":0.97964, "x":2.76518, "y":4.4786, "heading":-0.05401, "vx":2.02908, "vy":-2.58011, "omega":0.95025, "ax":-1.43823, "ay":9.95026, "alpha":-7.54565, "fx":[10.09833,9.8868,-46.87576,-70.96492], "fy":[174.28174,174.60107,168.51375,159.60756]}, - {"t":1.0123, "x":2.83068, "y":4.39966, "heading":-0.02298, "vx":1.98212, "vy":-2.25519, "omega":0.70385, "ax":-1.64366, "ay":9.7822, "alpha":-9.74536, "fx":[18.62186,12.6822,-55.52119,-87.61594], "fy":[173.71515,174.56345,166.01161,151.27979]}, - {"t":1.04495, "x":2.89453, "y":4.33123, "heading":0.0, "vx":1.92845, "vy":-1.93575, "omega":0.38562, "ax":-2.92579, "ay":9.68504, "alpha":-6.60839, "fx":[-24.71637,-17.02459,-66.8811,-90.44539], "fy":[173.12733,174.23847,161.82101,149.7723]}, - {"t":1.07939, "x":2.95921, "y":4.27031, "heading":0.01328, "vx":1.82768, "vy":-1.60219, "omega":0.15802, "ax":-4.88544, "ay":8.96782, "alpha":-4.45855, "fx":[-74.04058,-58.54812,-90.54685,-109.26415], "fy":[158.53007,164.99885,149.88722,136.74439]}, - {"t":1.11383, "x":3.01926, "y":4.22044, "heading":0.01872, "vx":1.65942, "vy":-1.29334, "omega":0.00446, "ax":-6.32605, "ay":8.08365, "alpha":-2.71661, "fx":[-105.56651,-93.00178,-109.79262,-122.05597], "fy":[139.64227,148.36865,136.44858,125.54296]}, - {"t":1.14827, "x":3.07266, "y":4.18069, "heading":0.01888, "vx":1.44155, "vy":-1.01493, "omega":-0.0891, "ax":-7.33328, "ay":7.21838, "alpha":-1.30787, "fx":[-124.73348,-118.35205,-124.89707,-130.96552], "fy":[122.92537,129.10629,122.80822,116.29077]}, - {"t":1.18271, "x":3.11795, "y":4.15002, "heading":0.01581, "vx":1.18899, "vy":-0.76633, "omega":-0.13414, "ax":-8.02986, "ay":6.44881, "alpha":-0.18544, "fx":[-136.66046,-135.78821,-136.51555,-137.37857], "fy":[109.5995,110.68144,109.78678,108.70182]}, - {"t":1.21715, "x":3.15414, "y":4.12745, "heading":0.01119, "vx":0.91244, "vy":-0.54423, "omega":-0.14053, "ax":-8.51625, "ay":5.79148, "alpha":0.70127, "fx":[-144.42543,-147.49155,-145.3695,-142.14916], "fy":[99.21677,94.58836,97.80277,102.43749]}, - {"t":1.25159, "x":3.18051, "y":4.11214, "heading":0.00635, "vx":0.61913, "vy":-0.34477, "omega":-0.11637, "ax":-8.86244, "ay":5.23795, "alpha":1.40572, "fx":[-149.72586,-155.36124,-152.09848,-145.80438], "fy":[91.08295,81.07908,87.0121,97.21012]}, - {"t":1.28603, "x":3.19658, "y":4.10338, "heading":0.00234, "vx":0.3139, "vy":-0.16437, "omega":-0.06796, "ax":-9.1144, "ay":4.77253, "alpha":1.97329, "fx":[-153.50604,-160.73381,-157.21762,-148.67603], "fy":[84.60928,69.88687,77.42305,92.79851]}, - {"t":1.32048, "x":3.20199, "y":4.10055, "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.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.29255, "ay":-7.27785, "alpha":34.27516, "fx":[198.8038,-103.95681,33.29364,158.07835], "fy":[2.06266,-169.72816,-196.40719,-121.20025]}, + {"t":0.03411, "x":1.5825, "y":7.29577, "heading":-0.9225, "vx":0.14642, "vy":-0.24826, "omega":1.16917, "ax":4.44566, "ay":-7.80124, "alpha":31.29806, "fx":[197.37594,-91.69967,35.40802,155.34386], "fy":[-22.97789,-176.57258,-195.99958,-124.62168]}, + {"t":0.06822, "x":1.59008, "y":7.28276, "heading":-0.88262, "vx":0.29807, "vy":-0.51437, "omega":2.23678, "ax":4.70384, "ay":-8.5328, "alpha":26.41912, "fx":[188.98942,-70.3335,42.06953,152.91771], "fy":[-60.81716,-186.02741,-194.62132,-127.48496]}, + {"t":0.10233, "x":1.60298, "y":7.26025, "heading":-0.80632, "vx":0.45852, "vy":-0.80543, "omega":3.13797, "ax":4.88, "ay":-9.54354, "alpha":18.94636, "fx":[161.08486,-36.29252,52.92827,147.6681], "fy":[-115.71642,-195.42133,-191.85211,-133.35487]}, + {"t":0.13644, "x":1.62146, "y":7.22722, "heading":-0.69928, "vx":0.62499, "vy":-1.13097, "omega":3.78425, "ax":4.90355, "ay":-10.5044, "alpha":9.51734, "fx":[110.83305,19.69636,67.83816,128.59167], "fy":[-164.43555,-197.59607,-186.8713,-151.51032]}, + {"t":0.17056, "x":1.64563, "y":7.18253, "heading":-0.5702, "vx":0.79225, "vy":-1.48929, "omega":4.1089, "ax":4.81306, "ay":-10.72322, "alpha":-6.17847, "fx":[74.4954,115.43828,90.95216,40.04], "fy":[-183.90556,-161.22161,-175.77079,-194.10559]}, + {"t":0.20467, "x":1.67546, "y":7.12549, "heading":-0.43004, "vx":0.95643, "vy":-1.85507, "omega":3.89814, "ax":4.69633, "ay":-8.55189, "alpha":-22.71177, "fx":[72.66532,178.70648,134.08155,-72.31108], "fy":[-184.64281,-85.38842,-115.86063,-184.33188]}, + {"t":0.23878, "x":1.71082, "y":7.05724, "heading":-0.29707, "vx":1.11663, "vy":-2.14678, "omega":3.12342, "ax":2.88316, "ay":-9.50527, "alpha":-21.98692, "fx":[84.70621,180.55134,-19.24384,-53.77012], "fy":[-179.1351,-80.22631,-184.16164,-190.26999]}, + {"t":0.27289, "x":1.75058, "y":6.97848, "heading":-0.19052, "vx":1.21498, "vy":-2.47102, "omega":2.37342, "ax":3.17646, "ay":-9.4364, "alpha":-22.11592, "fx":[95.04143,181.06782,-28.3094,-35.99974], "fy":[-173.31599,-76.99275,-185.0526,-193.83992]}, + {"t":0.307, "x":1.79388, "y":6.8887, "heading":-0.10956, "vx":1.32333, "vy":-2.79291, "omega":1.61902, "ax":3.52107, "ay":-9.16794, "alpha":-22.56047, "fx":[103.96311,182.37645,-28.31932,-23.24227], "fy":[-166.89637,-69.27061,-180.63216,-194.50145]}, + {"t":0.34111, "x":1.84106, "y":6.7881, "heading":-0.05434, "vx":1.44344, "vy":-3.10564, "omega":0.84945, "ax":3.9917, "ay":-8.54923, "alpha":-22.51992, "fx":[110.77609,180.11458,-13.27203,-11.46004], "fy":[-157.94345,-58.46652,-162.78075,-190.8557]}, + {"t":0.37522, "x":1.89262, "y":6.67719, "heading":-0.02536, "vx":1.5796, "vy":-3.39726, "omega":0.08127, "ax":0.72017, "ay":-0.48647, "alpha":-1.81193, "fx":[16.26634,16.54727,7.72898,7.47686], "fy":[-12.60815,-3.79174,-3.58607,-12.45069]}, + {"t":0.40933, "x":1.94693, "y":6.56102, "heading":-0.02259, "vx":1.60416, "vy":-3.41385, "omega":0.01947, "ax":-3.53314, "ay":-1.61802, "alpha":-0.07396, "fx":[-58.71719,-58.73835,-59.07435,-59.05309], "fy":[-27.18267,-26.79817,-26.76087,-27.14502]}, + {"t":0.44345, "x":1.99959, "y":6.44363, "heading":-0.02192, "vx":1.48365, "vy":-3.46905, "omega":0.01694, "ax":-3.70857, "ay":-1.50513, "alpha":0.01136, "fx":[-61.84721,-61.84401,-61.79303,-61.79623], "fy":[-25.05719,-25.11685,-25.12248,-25.06281]}, + {"t":0.47756, "x":2.04804, "y":6.32442, "heading":-0.02135, "vx":1.35714, "vy":-3.52039, "omega":0.01733, "ax":-0.85052, "ay":-0.32317, "alpha":0.00318, "fx":[-14.18526,-14.18553,-14.17013,-14.16985], "fy":[-5.37908,-5.39463,-5.39503,-5.37949]}, + {"t":0.51167, "x":2.09384, "y":6.20414, "heading":-0.02075, "vx":1.32813, "vy":-3.53141, "omega":0.01744, "ax":-0.23233, "ay":-0.08685, "alpha":0.001, "fx":[-3.87522,-3.87532,-3.87046,-3.87036], "fy":[-1.44522,-1.45009,-1.4502,-1.44533]}, + {"t":0.54578, "x":2.13901, "y":6.08363, "heading":-0.02016, "vx":1.3202, "vy":-3.53438, "omega":0.01747, "ax":-0.11113, "ay":-0.04119, "alpha":0.00089, "fx":[-1.85452,-1.8546,-1.85029,-1.8502], "fy":[-0.68435,-0.68866,-0.68875,-0.68444]}, + {"t":0.57989, "x":2.18398, "y":5.96305, "heading":-0.01956, "vx":1.31641, "vy":-3.53578, "omega":0.0175, "ax":-0.08232, "ay":-0.03026, "alpha":0.00112, "fx":[-1.37498,-1.37508,-1.36964,-1.36953], "fy":[-0.50158,-0.50702,-0.50713,-0.50169]}, + {"t":0.614, "x":2.22883, "y":5.84242, "heading":-0.01897, "vx":1.31361, "vy":-3.53681, "omega":0.01754, "ax":-0.07293, "ay":-0.0266, "alpha":0.00136, "fx":[-1.2189,-1.21902,-1.2124,-1.21227], "fy":[-0.44007,-0.4467,-0.44682,-0.4402]}, + {"t":0.64811, "x":2.2736, "y":5.72176, "heading":-0.01837, "vx":1.31112, "vy":-3.53772, "omega":0.01759, "ax":-0.06775, "ay":-0.02456, "alpha":0.00153, "fx":[-1.13304,-1.13317,-1.12571,-1.12558], "fy":[-0.40561,-0.41307,-0.4132,-0.40574]}, + {"t":0.68222, "x":2.31828, "y":5.60107, "heading":-0.01777, "vx":1.30881, "vy":-3.53856, "omega":0.01764, "ax":-0.06259, "ay":-0.02257, "alpha":0.00162, "fx":[-1.04722,-1.04735,-1.03948,-1.03935], "fy":[-0.3722,-0.38007,-0.38021,-0.37234]}, + {"t":0.71633, "x":2.36289, "y":5.48035, "heading":-0.01717, "vx":1.30667, "vy":-3.53933, "omega":0.01769, "ax":-0.05626, "ay":-0.02019, "alpha":0.00161, "fx":[-0.94166,-0.94179,-0.93395,-0.93382], "fy":[-0.33256,-0.3404,-0.34053,-0.33269]}, + {"t":0.75045, "x":2.40743, "y":5.35961, "heading":-0.01656, "vx":1.30475, "vy":-3.54002, "omega":0.01775, "ax":-0.04806, "ay":-0.01719, "alpha":0.00146, "fx":[-0.80469,-0.80481,-0.7977,-0.79758], "fy":[-0.28294,-0.29005,-0.29016,-0.28305]}, + {"t":0.78456, "x":2.45191, "y":5.23885, "heading":-0.01596, "vx":1.30311, "vy":-3.5406, "omega":0.0178, "ax":-0.03388, "ay":-0.01201, "alpha":0.00128, "fx":[-0.56789,-0.56798,-0.56174,-0.56164], "fy":[-0.19704,-0.20328,-0.20338,-0.19713]}, + {"t":0.81867, "x":2.49634, "y":5.11807, "heading":-0.01535, "vx":1.30196, "vy":-3.54101, "omega":0.01784, "ax":0.01907, "ay":0.00735, "alpha":0.00089, "fx":[0.3158,0.31574,0.32008,0.32014], "fy":[0.12479,0.12045,0.12039,0.12473]}, + {"t":0.85278, "x":2.54076, "y":4.99728, "heading":-0.01474, "vx":1.30261, "vy":-3.54076, "omega":0.01787, "ax":0.34008, "ay":0.12562, "alpha":-0.00092, "fx":[5.67114,5.67121,5.66675,5.66669], "fy":[2.09166,2.09614,2.09623,2.09175]}, + {"t":0.88689, "x":2.5854, "y":4.87658, "heading":-0.01413, "vx":1.31421, "vy":-3.53648, "omega":0.01784, "ax":2.37174, "ay":0.91055, "alpha":-0.00945, "fx":[39.55841,39.55764,39.51317,39.51394], "fy":[15.15362,15.20116,15.20327,15.15574]}, + {"t":0.921, "x":2.63161, "y":4.75647, "heading":-0.01352, "vx":1.39511, "vy":-3.50542, "omega":0.01752, "ax":7.84707, "ay":3.48324, "alpha":-0.01095, "fx":[130.83449,130.81458,130.7794,130.79931], "fy":[58.01751,58.089,58.11036,58.03885]}, + {"t":0.95511, "x":2.68376, "y":4.63892, "heading":-0.01293, "vx":1.66278, "vy":-3.3866, "omega":0.01715, "ax":9.55171, "ay":5.5566, "alpha":0.52183, "fx":[157.52669,159.42461,160.88717,159.05092], "fy":[95.39285,91.79113,89.87135,93.44839]}, + {"t":0.98922, "x":2.74604, "y":4.52664, "heading":-0.01234, "vx":1.9886, "vy":-3.19706, "omega":0.03495, "ax":6.68499, "ay":9.16873, "alpha":8.55668, "fx":[67.17323,101.51194,155.99546,121.06151], "fy":[182.05964,161.97929,114.57185,152.74249]}, + {"t":1.02334, "x":2.81776, "y":4.42291, "heading":-0.01115, "vx":2.21664, "vy":-2.8843, "omega":0.32683, "ax":1.98486, "ay":11.51989, "alpha":-5.97505, "fx":[68.193,53.0124,5.09352,6.04763], "fy":[184.48072,190.05156,197.16883,196.42276]}, + {"t":1.05745, "x":2.89453, "y":4.33123, "heading":0.0, "vx":2.28434, "vy":-2.49134, "omega":0.12301, "ax":-3.09748, "ay":11.44121, "alpha":-2.56216, "fx":[-42.57879,-37.64188,-59.51433,-66.79886], "fy":[193.22505,194.42736,188.92345,186.30198]}, + {"t":1.08959, "x":2.96636, "y":4.25705, "heading":0.00395, "vx":2.18477, "vy":-2.12354, "omega":0.04064, "ax":-6.2004, "ay":10.15351, "alpha":-1.33017, "fx":[-101.43708,-95.84642,-105.2083,-110.93865], "fy":[170.48271,173.75124,168.2825,164.50026]}, + {"t":1.12174, "x":3.03339, "y":4.19403, "heading":0.00526, "vx":1.98544, "vy":-1.79712, "omega":-0.00212, "ax":-7.57593, "ay":9.19877, "alpha":-0.67977, "fx":[-125.919,-122.69711,-126.67184,-129.86029], "fy":[153.65528,156.26384,153.07795,150.35938]}, + {"t":1.15389, "x":3.09331, "y":4.14101, "heading":0.00519, "vx":1.74189, "vy":-1.5014, "omega":-0.02397, "ax":-8.3015, "ay":8.56429, "alpha":-0.29922, "fx":[-138.35081,-136.90335,-138.42007,-139.85415], "fy":[142.79231,144.18882,142.74106,141.32822]}, + {"t":1.18604, "x":3.14501, "y":4.09717, "heading":0.00442, "vx":1.47501, "vy":-1.22608, "omega":-0.03359, "ax":-8.73879, "ay":8.12686, "alpha":-0.05402, "fx":[-145.67904,-145.41863,-145.66417,-145.92402], "fy":[135.4618,135.74251,135.48026,135.19915]}, + {"t":1.21818, "x":3.18792, "y":4.06195, "heading":0.00334, "vx":1.19408, "vy":-0.96482, "omega":-0.03533, "ax":-9.02791, "ay":7.81079, "alpha":0.11583, "fx":[-150.45706,-151.01012,-150.5264,-149.9704], "fy":[130.24485,129.60104,130.16003,130.80245]}, + {"t":1.25033, "x":3.22164, "y":4.03497, "heading":0.00221, "vx":0.90386, "vy":-0.71372, "omega":-0.03161, "ax":-9.23211, "ay":7.57297, "alpha":0.23996, "fx":[-153.7998,-154.9333,-153.99664,-152.84951], "fy":[126.36311,124.96687,126.11452,127.50633]}, + {"t":1.28248, "x":3.24592, "y":4.01594, "heading":0.00119, "vx":0.60706, "vy":-0.47027, "omega":-0.02389, "ax":-9.38352, "ay":7.38803, "alpha":0.33446, "fx":[-156.26193,-157.82544,-156.58961,-154.99825], "fy":[123.37,121.35865,122.94297,124.94808]}, + {"t":1.31463, "x":3.26059, "y":4.00464, "heading":0.00042, "vx":0.30541, "vy":-0.23276, "omega":-0.01314, "ax":-9.50006, "ay":7.24032, "alpha":0.40871, "fx":[-158.1477,-160.04002,-158.59654,-156.66118], "fy":[120.99484,118.47536,120.39385,122.90689]}, + {"t":1.34678, "x":3.2655, "y":4.0009, "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/fetchToF.traj b/src/main/deploy/choreo/fetchToF.traj index 8aefd21f..7cb0af49 100644 --- a/src/main/deploy/choreo/fetchToF.traj +++ b/src/main/deploy/choreo/fetchToF.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.9735233783721924, "y":3.2965428829193115, "heading":1.0253809355466683, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":4.682854175567627, "y":2.118865728378296, "heading":1.9269363946215805, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.9735233783721924, "y":3.2965428829193115, "heading":1.0253809355466683, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.682854175567627, "y":2.118865728378296, "heading":1.9269363946215805, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.068535475516535, "y":2.873601513280177, "heading":2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -17,10 +17,10 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.9735233783721924 m", "val":2.9735233783721924}, "y":{"exp":"3.2965428829193115 m", "val":3.2965428829193115}, "heading":{"exp":"1.0253809355466685 rad", "val":1.0253809355466683}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.9735233783721924 m", "val":2.9735233783721924}, "y":{"exp":"3.2965428829193115 m", "val":3.2965428829193115}, "heading":{"exp":"1.0253809355466685 rad", "val":1.0253809355466683}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"4.682854175567627 m", "val":4.682854175567627}, "y":{"exp":"2.118865728378296 m", "val":2.118865728378296}, "heading":{"exp":"1.9269363946215805 rad", "val":1.9269363946215805}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"F.x", "val":5.068535475516535}, "y":{"exp":"F.y", "val":2.873601513280177}, "heading":{"exp":"F.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"F.x", "val":5.122885135094611}, "y":{"exp":"F.y", "val":2.978538192264317}, "heading":{"exp":"F.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,112 +34,110 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.33357,2.02536,2.49779], + "waypoints":[0.0,1.41138,2.16121,2.59249], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.25339, "ay":-7.30868, "alpha":23.15434, "fx":[168.55266,-52.48541,38.73116,134.59751], "fy":[-46.9864,-167.0472,-170.95919,-112.28118]}, - {"t":0.02837, "x":1.57313, "y":7.41248, "heading":-0.94268, "vx":0.12069, "vy":-0.20738, "omega":0.65698, "ax":4.4697, "ay":-7.9578, "alpha":18.0954, "fx":[155.52288,-24.75451,44.12909,129.21608], "fy":[-80.14294,-173.30068,-169.61194,-118.38395]}, - {"t":0.05675, "x":1.57835, "y":7.40339, "heading":-0.92404, "vx":0.24751, "vy":-0.43317, "omega":1.17041, "ax":4.66554, "ay":-8.52122, "alpha":12.44584, "fx":[134.51229,8.5208,52.62819,121.77671], "fy":[-111.87068,-174.82488,-167.12201,-125.95617]}, - {"t":0.08512, "x":1.58725, "y":7.38767, "heading":-0.89083, "vx":0.37989, "vy":-0.67495, "omega":1.52355, "ax":4.81077, "ay":-8.90435, "alpha":6.63307, "fx":[108.88492,44.54209,64.56294,109.32908], "fy":[-136.96676,-169.25521,-162.80975,-136.81003]}, - {"t":0.1135, "x":1.59997, "y":7.36494, "heading":-0.8476, "vx":0.51639, "vy":-0.9276, "omega":1.71176, "ax":4.87989, "ay":-9.05385, "alpha":1.03885, "fx":[86.44546,77.54015,79.82469,88.21216], "fy":[-152.16435,-156.89679,-155.77508,-151.17723]}, - {"t":0.14187, "x":1.61658, "y":7.33497, "heading":-0.79903, "vx":0.65485, "vy":-1.18449, "omega":1.74123, "ax":4.8787, "ay":-9.05503, "alpha":0.02106, "fx":[83.04706,82.87388,82.9236,83.09657], "fy":[-153.98972,-154.08318,-154.0571,-153.96368]}, - {"t":0.17024, "x":1.63713, "y":7.29772, "heading":-0.74962, "vx":0.79328, "vy":-1.44142, "omega":1.74183, "ax":4.87505, "ay":-9.05123, "alpha":0.00028, "fx":[82.92403,82.92174,82.92252,82.92482], "fy":[-153.95834,-153.95957,-153.95916,-153.95793]}, - {"t":0.19862, "x":1.6616, "y":7.25318, "heading":-0.7002, "vx":0.9316, "vy":-1.69824, "omega":1.74184, "ax":4.87077, "ay":-9.04547, "alpha":-0.00005, "fx":[82.85037,82.85074,82.8506,82.85023], "fy":[-153.86098,-153.86078,-153.86086,-153.86106]}, - {"t":0.22699, "x":1.68999, "y":7.20135, "heading":-0.65078, "vx":1.0698, "vy":-1.95489, "omega":1.74184, "ax":4.86544, "ay":-9.0363, "alpha":-0.00006, "fx":[82.75969,82.76016,82.75995,82.75947], "fy":[-153.70496,-153.7047,-153.70482,-153.70507]}, - {"t":0.25536, "x":1.7223, "y":7.14224, "heading":-0.60136, "vx":1.20785, "vy":-2.21128, "omega":1.74183, "ax":4.85809, "ay":-9.0203, "alpha":-0.00009, "fx":[82.63454,82.63519,82.63486,82.6342], "fy":[-153.43285,-153.43249,-153.43267,-153.43302]}, - {"t":0.28374, "x":1.75853, "y":7.07587, "heading":-0.55193, "vx":1.3457, "vy":-2.46723, "omega":1.74183, "ax":4.84581, "ay":-8.98744, "alpha":-0.00017, "fx":[82.4255,82.42676,82.42602,82.42477], "fy":[-152.87388,-152.8732,-152.87357,-152.87426]}, - {"t":0.31211, "x":1.79866, "y":7.00225, "heading":-0.50251, "vx":1.48319, "vy":-2.72223, "omega":1.74183, "ax":4.81453, "ay":-8.88945, "alpha":-0.00099, "fx":[81.89262,81.89957,81.89499,81.88804], "fy":[-151.20771,-151.20383,-151.20613,-151.21001]}, - {"t":0.34049, "x":1.84269, "y":6.92143, "heading":-0.45309, "vx":1.6198, "vy":-2.97446, "omega":1.7418, "ax":3.60643, "ay":-5.31497, "alpha":-2.21001, "fx":[61.69822,70.34643,61.21733,52.11542], "fy":[-95.59572,-85.7816,-85.03883,-95.20804]}, - {"t":0.36886, "x":1.8901, "y":6.83489, "heading":-0.40367, "vx":1.72213, "vy":-3.12527, "omega":1.67909, "ax":0.09096, "ay":-1.90397, "alpha":-4.19011, "fx":[6.91956,15.55323,-4.12032,-12.16343], "fy":[-45.16288,-27.30145,-19.31247,-37.76685]}, - {"t":0.39723, "x":1.939, "y":6.74545, "heading":-0.35602, "vx":1.72471, "vy":-3.17929, "omega":1.5602, "ax":-0.9255, "ay":-2.07545, "alpha":-3.30774, "fx":[-10.63281,-5.05649,-21.17582,-26.10456], "fy":[-45.44149,-31.14249,-25.0585,-39.56865]}, - {"t":0.42561, "x":1.98756, "y":6.65441, "heading":-0.31176, "vx":1.69845, "vy":-3.23818, "omega":1.46635, "ax":-2.03167, "ay":-3.26071, "alpha":-4.49371, "fx":[-25.87,-20.28654,-44.26824,-47.80751], "fy":[-69.11284,-50.70213,-41.5287,-60.51129]}, - {"t":0.45398, "x":2.03494, "y":6.56121, "heading":-0.27015, "vx":1.6408, "vy":-3.3307, "omega":1.33885, "ax":-4.12779, "ay":-4.73405, "alpha":-5.56499, "fx":[-55.16312,-55.12065,-87.03706,-83.52947], "fy":[-99.1361,-78.40877,-60.62139,-83.93301]}, - {"t":0.48236, "x":2.07983, "y":6.4648, "heading":-0.23216, "vx":1.52368, "vy":-3.46502, "omega":1.18095, "ax":-6.16568, "ay":-5.41719, "alpha":-5.71088, "fx":[-85.10246,-95.72467,-124.89659,-113.78209], "fy":[-115.61108,-92.86905,-66.33256,-93.76694]}, - {"t":0.51073, "x":2.12058, "y":6.36431, "heading":-0.19865, "vx":1.34874, "vy":-3.61873, "omega":1.01891, "ax":-6.84709, "ay":-4.52188, "alpha":-4.1796, "fx":[-103.167,-112.43429,-129.1124,-121.15431], "fy":[-96.32346,-74.15162,-57.0989,-80.08928]}, - {"t":0.5391, "x":2.15609, "y":6.25981, "heading":-0.16974, "vx":1.15446, "vy":-3.74703, "omega":0.90031, "ax":-5.13448, "ay":-3.02162, "alpha":-2.94527, "fx":[-80.0287,-82.08562,-94.66423,-92.56563], "fy":[-62.80753,-46.89817,-40.17362,-55.70852]}, - {"t":0.56748, "x":2.18678, "y":6.15228, "heading":-0.1442, "vx":1.00877, "vy":-3.83277, "omega":0.81675, "ax":-2.60635, "ay":-1.13651, "alpha":-0.66133, "fx":[-42.94336,-42.67441,-45.73038,-45.98459], "fy":[-21.3329,-17.99458,-17.34217,-20.65709]}, - {"t":0.59585, "x":2.21436, "y":6.04307, "heading":-0.12102, "vx":0.93482, "vy":-3.86501, "omega":0.79798, "ax":-1.24854, "ay":-0.24108, "alpha":0.52266, "fx":[-22.3404,-22.62733,-20.13599,-19.8455], "fy":[-2.65931,-5.21606,-5.54617,-2.9811]}, - {"t":0.62422, "x":2.24038, "y":5.93331, "heading":-0.09838, "vx":0.89939, "vy":-3.87185, "omega":0.81281, "ax":-0.80882, "ay":0.01259, "alpha":0.87597, "fx":[-15.6597,-16.07556,-11.8574,-11.43865], "fy":[2.5566,-1.70731,-2.13618,2.14383]}, - {"t":0.6526, "x":2.26557, "y":5.82345, "heading":-0.07532, "vx":0.87644, "vy":-3.8715, "omega":0.83767, "ax":-0.69286, "ay":0.07844, "alpha":1.00739, "fx":[-14.03444,-14.40663,-9.53655,-9.16362], "fy":[3.96992,-0.93556,-1.31072,3.61337]}, - {"t":0.68097, "x":2.29016, "y":5.71363, "heading":-0.05155, "vx":0.85679, "vy":-3.86927, "omega":0.86625, "ax":-0.6758, "ay":0.09309, "alpha":1.08857, "fx":[-13.99198,-14.27061,-8.99804,-8.72007], "fy":[4.37085,-0.93602,-1.21474,4.11338]}, - {"t":0.70935, "x":2.3142, "y":5.60389, "heading":-0.02697, "vx":0.83761, "vy":-3.86663, "omega":0.89714, "ax":-0.6773, "ay":0.08391, "alpha":1.13129, "fx":[-14.18565,-14.34026,-8.85505,-8.70197], "fy":[4.25908,-1.261,-1.41582,4.12718]}, - {"t":0.73772, "x":2.33769, "y":5.49421, "heading":-0.00152, "vx":0.81839, "vy":-3.86425, "omega":0.92924, "ax":-0.65957, "ay":0.05053, "alpha":1.10622, "fx":[-13.89602,-13.90828,-8.54156,-8.53081], "fy":[3.56149,-1.83768,-1.85314,3.56727]}, - {"t":0.76609, "x":2.36065, "y":5.38459, "heading":0.02485, "vx":0.79968, "vy":-3.86281, "omega":0.96062, "ax":-0.59759, "ay":-0.01033, "alpha":0.98187, "fx":[-12.60661,-12.48788,-7.72293,-7.84215], "fy":[2.15961,-2.62964,-2.51855,2.28584]}, - {"t":0.79447, "x":2.3831, "y":5.27498, "heading":0.05211, "vx":0.78272, "vy":-3.86311, "omega":0.98848, "ax":-0.47371, "ay":-0.08857, "alpha":0.76306, "fx":[-10.00776,-9.81138,-6.10791,-6.30372], "fy":[0.25564,-3.45986,-3.2725,0.45034]}, - {"t":0.82284, "x":2.40512, "y":5.16533, "heading":0.08015, "vx":0.76928, "vy":-3.86562, "omega":1.01013, "ax":-0.25824, "ay":-0.17547, "alpha":0.44021, "fx":[-5.54756,-5.3738,-3.23784,-3.41093], "fy":[-2.00101,-4.13832,-3.96914,-1.83039]}, - {"t":0.85122, "x":2.42684, "y":5.05558, "heading":0.10881, "vx":0.76195, "vy":-3.8706, "omega":1.02262, "ax":0.10604, "ay":-0.25391, "alpha":0.00202, "fx":[1.79834,1.7994,1.80917,1.8081], "fy":[-4.3146,-4.32436,-4.32329,-4.31354]}, - {"t":0.87959, "x":2.4485, "y":4.94565, "heading":0.13783, "vx":0.76496, "vy":-3.8778, "omega":1.02268, "ax":0.71042, "ay":-0.27604, "alpha":-0.5104, "fx":[13.47568,13.14611,10.69091,11.02355], "fy":[-5.75375,-3.28484,-3.63476,-6.10781]}, - {"t":0.90796, "x":2.47049, "y":4.83551, "heading":0.16685, "vx":0.78512, "vy":-3.88564, "omega":1.0082, "ax":1.61383, "ay":-0.12713, "alpha":-0.85311, "fx":[29.77687,29.12498,25.11894,25.78209], "fy":[-3.89121,0.28877,-0.41938,-4.62823]}, - {"t":0.93634, "x":2.49342, "y":4.72521, "heading":0.19545, "vx":0.83091, "vy":-3.88924, "omega":0.98399, "ax":2.56668, "ay":0.31987, "alpha":-0.5019, "fx":[45.02113,44.5404,42.29378,42.77871], "fy":[4.40299,6.94565,6.48776,3.92752]}, - {"t":0.96471, "x":2.51803, "y":4.61499, "heading":0.22337, "vx":0.90374, "vy":-3.88017, "omega":0.96975, "ax":3.02274, "ay":1.01925, "alpha":0.88728, "fx":[48.92938,50.0645,53.89579,52.77403], "fy":[19.25678,14.63141,15.45264,20.00799]}, - {"t":0.99308, "x":2.54489, "y":4.5053, "heading":0.25089, "vx":0.9895, "vy":-3.85125, "omega":0.99493, "ax":3.05795, "ay":1.8576, "alpha":2.98327, "fx":[43.07568,48.05082,60.88486,56.04825], "fy":[38.26892,22.6112,25.33768,40.17125]}, - {"t":1.02146, "x":2.57419, "y":4.39678, "heading":0.27912, "vx":1.07627, "vy":-3.79854, "omega":1.07957, "ax":2.93476, "ay":2.60914, "alpha":4.85396, "fx":[34.34033,44.13007,65.35204,55.8556], "fy":[55.22242,30.11918,34.56842,57.61272]}, - {"t":1.04983, "x":2.60591, "y":4.29005, "heading":0.30975, "vx":1.15954, "vy":-3.72451, "omega":1.2173, "ax":2.43521, "ay":2.86852, "alpha":5.43262, "fx":[23.34861,34.85417,59.48213,48.00365], "fy":[59.88305,33.02882,38.8083,63.45079]}, - {"t":1.07821, "x":2.63979, "y":4.18552, "heading":0.34429, "vx":1.22864, "vy":-3.64312, "omega":1.37144, "ax":1.40348, "ay":2.36232, "alpha":4.216, "fx":[10.00017,18.20304,37.89089,29.39736], "fy":[47.23088,27.61819,33.6318,52.2488]}, - {"t":1.10658, "x":2.67522, "y":4.0831, "heading":0.3832, "vx":1.26846, "vy":-3.57609, "omega":1.49107, "ax":0.33546, "ay":1.45037, "alpha":2.12877, "fx":[-1.1683,2.82005,12.62818,8.54444], "fy":[27.58239,18.04135,21.82746,31.23054]}, - {"t":1.13495, "x":2.71135, "y":3.98222, "heading":0.42551, "vx":1.27798, "vy":-3.53494, "omega":1.55147, "ax":-0.35219, "ay":0.79868, "alpha":0.76691, "fx":[-8.46668,-6.9415,-3.50857,-5.04605], "fy":[14.49932,11.11885,12.67412,16.04888]}, - {"t":1.16333, "x":2.74747, "y":3.88224, "heading":0.46953, "vx":1.26798, "vy":-3.51227, "omega":1.57323, "ax":-0.64495, "ay":0.51555, "alpha":0.27006, "fx":[-11.85096,-11.26477,-10.08893,-10.67669], "fy":[9.05324,7.88227,8.48563,9.65651]}, - {"t":1.1917, "x":2.78318, "y":3.78279, "heading":0.51417, "vx":1.24968, "vy":-3.49765, "omega":1.58089, "ax":-0.59496, "ay":0.49498, "alpha":0.27194, "fx":[-11.01976,-10.37615,-9.21975,-9.86484], "fy":[8.66571,7.51478,8.17335,9.32404]}, - {"t":1.22008, "x":2.8184, "y":3.68375, "heading":0.55903, "vx":1.2328, "vy":-3.4836, "omega":1.58861, "ax":0.02516, "ay":0.76279, "alpha":0.61476, "fx":[-1.64593,-0.04989,2.50333,0.9042], "fy":[13.44985,10.92136,12.50317,15.02488]}, - {"t":1.24845, "x":2.85339, "y":3.58522, "heading":0.6041, "vx":1.23352, "vy":-3.46196, "omega":1.60605, "ax":2.15562, "ay":1.46774, "alpha":0.91717, "fx":[33.58979,36.30386,39.72584,37.04668], "fy":[25.74601,21.8074,24.2107,28.09909]}, - {"t":1.27682, "x":2.88926, "y":3.48758, "heading":0.64967, "vx":1.29468, "vy":-3.42031, "omega":1.63208, "ax":6.09701, "ay":2.0631, "alpha":-0.88482, "fx":[105.87886,103.29245,101.51254,104.14968], "fy":[33.87295,38.91371,36.38136,31.20321]}, - {"t":1.3052, "x":2.92845, "y":3.39136, "heading":0.69598, "vx":1.46768, "vy":-3.36177, "omega":1.60697, "ax":8.52312, "ay":1.41482, "alpha":-5.6279, "fx":[152.41388,140.79694,138.68456,148.00817], "fy":[18.04056,54.1354,33.33741,-9.25085]}, - {"t":1.33357, "x":2.97352, "y":3.29654, "heading":0.74158, "vx":1.70951, "vy":-3.32163, "omega":1.44728, "ax":5.28074, "ay":6.35334, "alpha":8.28897, "fx":[52.84715,111.14529,117.74323,77.56007], "fy":[129.73111,80.18419,92.91412,129.44439]}, - {"t":1.35044, "x":3.00312, "y":3.2414, "heading":0.76599, "vx":1.79861, "vy":-3.21443, "omega":1.58714, "ax":5.2561, "ay":5.29673, "alpha":5.20544, "fx":[69.8231,99.82878,106.16342,81.80367], "fy":[101.15779,71.03402,81.80028,106.39108]}, - {"t":1.36732, "x":3.03422, "y":3.18792, "heading":0.79277, "vx":1.8873, "vy":-3.12506, "omega":1.67498, "ax":4.75339, "ay":3.6949, "alpha":1.54254, "fx":[75.9225,82.71986,85.63029,79.1426], "fy":[64.65496,56.9848,61.23035,68.52638]}, - {"t":1.38419, "x":3.06674, "y":3.13572, "heading":0.82104, "vx":1.9675, "vy":-3.06272, "omega":1.701, "ax":3.24359, "ay":2.48852, "alpha":0.29323, "fx":[54.20793,55.36204,56.13343,54.98682], "fy":[42.44639,41.27153,42.2151,43.38302]}, - {"t":1.40106, "x":3.1004, "y":3.08439, "heading":0.84974, "vx":2.02223, "vy":-3.02073, "omega":1.70595, "ax":1.62007, "ay":1.54983, "alpha":0.08421, "fx":[27.26892,27.58929,27.84479,27.52476], "fy":[26.35715,26.07114,26.36721,26.65295]}, - {"t":1.41794, "x":3.13475, "y":3.03364, "heading":0.87852, "vx":2.04957, "vy":-2.99458, "omega":1.70737, "ax":0.47726, "ay":0.87634, "alpha":0.0365, "fx":[7.99228,8.13074,8.24371,8.10527], "fy":[14.89572,14.78173,14.91701,15.03098]}, - {"t":1.43481, "x":3.1694, "y":2.98324, "heading":0.90733, "vx":2.05762, "vy":-2.97979, "omega":1.70799, "ax":-0.16919, "ay":0.46837, "alpha":0.01736, "fx":[-2.93729,-2.87063,-2.81837,-2.88503], "fy":[7.95957,7.90769,7.97426,8.02613]}, - {"t":1.45168, "x":3.20409, "y":2.93303, "heading":0.93615, "vx":2.05476, "vy":-2.97189, "omega":1.70828, "ax":-0.46958, "ay":0.25534, "alpha":0.00919, "fx":[-8.01874,-7.9828,-7.95621,-7.99215], "fy":[4.33847,4.31196,4.34813,4.37463]}, - {"t":1.46855, "x":3.23869, "y":2.88292, "heading":0.96497, "vx":2.04684, "vy":-2.96758, "omega":1.70844, "ax":-0.52258, "ay":0.18703, "alpha":0.00741, "fx":[-8.91403,-8.88445,-8.86388,-8.89346], "fy":[3.17671,3.15615,3.18594,3.20649]}, - {"t":1.48543, "x":3.27316, "y":2.83288, "heading":0.9938, "vx":2.03802, "vy":-2.96442, "omega":1.70856, "ax":-0.34814, "ay":0.25609, "alpha":0.0108, "fx":[-5.95819,-5.91419,-5.88544,-5.92944], "fy":[4.34831,4.31968,4.36382,4.39245]}, - {"t":1.5023, "x":3.30749, "y":2.7829, "heading":1.02263, "vx":2.03215, "vy":-2.9601, "omega":1.70874, "ax":0.11003, "ay":0.50286, "alpha":0.02045, "fx":[1.80298,1.88822,1.94014,1.85491], "fy":[8.537,8.48513,8.56988,8.62174]}, - {"t":1.51917, "x":3.3418, "y":2.73302, "heading":1.05146, "vx":2.03401, "vy":-2.95162, "omega":1.70909, "ax":0.97903, "ay":1.0206, "alpha":0.03978, "fx":[16.52139,16.69154,16.78457,16.61448], "fy":[17.32662,17.22738,17.39364,17.49286]}, - {"t":1.53605, "x":3.37626, "y":2.68336, "heading":1.0803, "vx":2.05052, "vy":-2.9344, "omega":1.70976, "ax":2.35745, "ay":1.90943, "alpha":0.07178, "fx":[39.87329,40.19026,40.32573,40.00912], "fy":[32.42569,32.2319,32.53196,32.72565]}, - {"t":1.55292, "x":3.41119, "y":2.63412, "heading":1.10915, "vx":2.0903, "vy":-2.90218, "omega":1.71097, "ax":4.00142, "ay":3.08163, "alpha":0.11024, "fx":[67.75278,68.26005,68.37261,67.86647], "fy":[52.37308,52.00755,52.46261,52.82745]}, - {"t":1.56979, "x":3.44703, "y":2.58559, "heading":1.13801, "vx":2.15782, "vy":-2.85018, "omega":1.71283, "ax":5.349, "ay":4.21912, "alpha":0.1409, "fx":[90.6464,91.32903,91.32252,90.64165], "fy":[71.77522,71.19275,71.75815,72.33818]}, - {"t":1.58667, "x":3.4842, "y":2.5381, "heading":1.16692, "vx":2.24807, "vy":-2.77899, "omega":1.71521, "ax":6.15504, "ay":5.13545, "alpha":0.15668, "fx":[104.36761,105.17256,105.02222,104.21924], "fy":[87.4285,86.67699,87.27881,88.02564]}, - {"t":1.60354, "x":3.52301, "y":2.49194, "heading":1.19586, "vx":2.35192, "vy":-2.69234, "omega":1.71785, "ax":6.52779, "ay":5.84783, "alpha":0.15919, "fx":[110.72927,111.59844,111.34135,110.47423], "fy":[99.59603,98.76895,99.34689,100.16777]}, - {"t":1.62041, "x":3.56362, "y":2.44735, "heading":1.22484, "vx":2.46207, "vy":-2.59367, "omega":1.72054, "ax":6.62559, "ay":6.4212, "alpha":0.1528, "fx":[112.4135,113.29891,112.98414,112.10086], "fy":[109.37564,108.55389,109.0731,109.88823]}, - {"t":1.63728, "x":3.60611, "y":2.4045, "heading":1.25387, "vx":2.57386, "vy":-2.48533, "omega":1.72311, "ax":6.55552, "ay":6.90459, "alpha":0.14226, "fx":[111.23865,112.11088,111.77523,110.90522], "fy":[117.60712,116.83757,117.28633,118.04962]}, - {"t":1.65416, "x":3.65047, "y":2.36355, "heading":1.28295, "vx":2.68447, "vy":-2.36883, "omega":1.72552, "ax":6.37805, "ay":7.32873, "alpha":0.13511, "fx":[108.2243,109.09787,108.75209,107.88094], "fy":[124.82538,124.10417,124.49678,125.21203]}, - {"t":1.67103, "x":3.69667, "y":2.32462, "heading":1.31206, "vx":2.79209, "vy":-2.24517, "omega":1.7278, "ax":6.11824, "ay":7.71798, "alpha":0.15754, "fx":[103.74397,104.81624,104.39317,103.3249], "fy":[131.4829,130.66497,131.08243,131.89201]}, - {"t":1.6879, "x":3.74465, "y":2.28784, "heading":1.34121, "vx":2.89532, "vy":-2.11495, "omega":1.73045, "ax":5.72664, "ay":8.12897, "alpha":0.30045, "fx":[96.72765,98.89871,98.08031,95.92753], "fy":[138.67288,137.18201,137.88528,139.34543]}, - {"t":1.70478, "x":3.79432, "y":2.25331, "heading":1.37041, "vx":2.99195, "vy":-1.97779, "omega":1.73552, "ax":4.56048, "ay":8.91828, "alpha":0.24919, "fx":[76.81915,78.85992,78.31554,76.29538], "fy":[152.03672,151.00976,151.36768,152.37544]}, - {"t":1.72165, "x":3.84545, "y":2.22121, "heading":1.39969, "vx":3.0689, "vy":-1.82731, "omega":1.73973, "ax":2.00541, "ay":9.88402, "alpha":0.02547, "fx":[33.99544,34.23329,34.2273,33.98978], "fy":[168.14509,168.09699,168.10381,168.15183]}, - {"t":1.73852, "x":3.89752, "y":2.19178, "heading":1.42905, "vx":3.10273, "vy":-1.66054, "omega":1.74016, "ax":0.16486, "ay":10.13626, "alpha":0.00441, "fx":[2.78065,2.82254,2.82783,2.78595], "fy":[172.41499,172.41422,172.41487,172.41564]}, - {"t":1.75539, "x":3.9499, "y":2.16521, "heading":1.45841, "vx":3.10551, "vy":-1.48951, "omega":1.74023, "ax":-1.0175, "ay":10.12237, "alpha":0.00156, "fx":[-17.31634,-17.30172,-17.29859,-17.3132], "fy":[172.17756,172.17899,172.17951,172.17809]}, - {"t":1.77227, "x":4.00215, "y":2.14152, "heading":1.48777, "vx":3.08835, "vy":-1.31871, "omega":1.74026, "ax":-1.80446, "ay":10.0373, "alpha":0.00077, "fx":[-30.69786,-30.69083,-30.68896,-30.696], "fy":[170.73079,170.73203,170.73245,170.7312]}, - {"t":1.78914, "x":4.054, "y":2.12069, "heading":1.51714, "vx":3.0579, "vy":-1.14935, "omega":1.74027, "ax":-2.35553, "ay":9.94083, "alpha":0.00045, "fx":[-40.06944,-40.06543,-40.06426,-40.06827], "fy":[169.09012,169.09106,169.09138,169.09044]}, - {"t":1.80601, "x":4.10526, "y":2.10272, "heading":1.5465, "vx":3.01815, "vy":-0.98162, "omega":1.74028, "ax":-2.75925, "ay":9.85031, "alpha":0.00028, "fx":[-46.93578,-46.93325,-46.93247,-46.93501], "fy":[167.55051,167.55121,167.55145,167.55074]}, - {"t":1.82289, "x":4.1558, "y":2.08756, "heading":1.57586, "vx":2.9716, "vy":-0.81542, "omega":1.74028, "ax":-3.06625, "ay":9.76993, "alpha":0.00019, "fx":[-52.15723,-52.15551,-52.15499,-52.1567], "fy":[166.18332,166.18385,166.18403,166.18349]}, - {"t":1.83976, "x":4.2055, "y":2.07519, "heading":1.60523, "vx":2.91986, "vy":-0.65057, "omega":1.74029, "ax":-3.30687, "ay":9.69978, "alpha":0.00014, "fx":[-56.24977,-56.24855,-56.24818,-56.2494], "fy":[164.99013,164.99055,164.99068,164.99027]}, - {"t":1.85663, "x":4.2543, "y":2.06559, "heading":1.63459, "vx":2.86406, "vy":-0.48691, "omega":1.74029, "ax":-3.50019, "ay":9.63876, "alpha":0.0001, "fx":[-59.53781,-59.53692,-59.53666,-59.53755], "fy":[163.95239,163.95272,163.95282,163.95249]}, - {"t":1.87351, "x":4.30212, "y":2.05875, "heading":1.66395, "vx":2.80501, "vy":-0.32427, "omega":1.74029, "ax":-3.65871, "ay":9.58558, "alpha":0.00008, "fx":[-62.23409,-62.23342,-62.23324,-62.23391], "fy":[163.04781,163.04806,163.04814,163.04788]}, - {"t":1.89038, "x":4.34893, "y":2.05464, "heading":1.69332, "vx":2.74327, "vy":-0.16254, "omega":1.74029, "ax":-3.79095, "ay":9.53901, "alpha":0.00006, "fx":[-64.48325,-64.48275,-64.48262,-64.48312], "fy":[162.25566,162.25586,162.25591,162.25571]}, - {"t":1.90725, "x":4.39468, "y":2.05326, "heading":1.72268, "vx":2.67931, "vy":-0.00158, "omega":1.74029, "ax":-3.90286, "ay":9.498, "alpha":0.00003, "fx":[-66.38674,-66.38645,-66.38638,-66.38667], "fy":[161.55815,161.55827,161.55831,161.55819]}, - {"t":1.92412, "x":4.43933, "y":2.05458, "heading":1.75205, "vx":2.61346, "vy":0.15868, "omega":1.74029, "ax":-3.99876, "ay":9.46168, "alpha":-0.00016, "fx":[-68.01689,-68.01827,-68.01857,-68.01719], "fy":[160.94086,160.94028,160.94014,160.94072]}, - {"t":1.941, "x":4.48286, "y":2.05861, "heading":1.78141, "vx":2.54598, "vy":0.31832, "omega":1.74029, "ax":-4.08181, "ay":9.42934, "alpha":-0.00313, "fx":[-69.41406,-69.4414,-69.44684,-69.41949], "fy":[160.39754,160.38574,160.38325,160.39506]}, - {"t":1.95787, "x":4.52524, "y":2.06532, "heading":1.81077, "vx":2.47711, "vy":0.47742, "omega":1.74024, "ax":-4.15441, "ay":9.40038, "alpha":-0.04865, "fx":[-70.41548,-70.84023,-70.91569,-70.48994], "fy":[160.00893,159.82168,159.78629,159.97423]}, - {"t":1.97474, "x":4.56644, "y":2.07471, "heading":1.84014, "vx":2.40701, "vy":0.63604, "omega":1.73942, "ax":-4.21777, "ay":9.37295, "alpha":-0.61993, "fx":[-68.659,-73.99846,-74.91051,-69.40452], "fy":[160.82139,158.43892,157.98656,160.47787]}, - {"t":1.99162, "x":4.60645, "y":2.08678, "heading":1.86949, "vx":2.33585, "vy":0.79418, "omega":1.72896, "ax":-4.25863, "ay":9.31538, "alpha":-3.14469, "fx":[-58.15135,-83.35406,-88.86788,-59.3787], "fy":[164.98463,153.8089,150.57782,164.43662]}, - {"t":2.00849, "x":4.64526, "y":2.10151, "heading":1.89866, "vx":2.26399, "vy":0.95136, "omega":1.6759, "ax":-4.25485, "ay":9.18646, "alpha":-6.40189, "fx":[-47.06113,-93.15751,-106.15324,-43.12293], "fy":[168.5539,148.1591,138.92221,169.401]}, - {"t":2.02536, "x":4.68285, "y":2.11887, "heading":1.92694, "vx":2.1922, "vy":1.10637, "omega":1.56788, "ax":-4.48089, "ay":9.11781, "alpha":-5.9568, "fx":[-52.63026,-95.73822,-107.16889,-49.33723], "fy":[167.11492,146.74381,138.4977,168.00888]}, - {"t":2.05686, "x":4.74968, "y":2.15823, "heading":1.97632, "vx":2.05108, "vy":1.39353, "omega":1.38027, "ax":-4.93961, "ay":8.85457, "alpha":-6.31413, "fx":[-59.98686,-103.9674,-115.95171,-56.17914], "fy":[164.56731,140.97648,131.14168,165.7692]}, - {"t":2.08835, "x":4.81182, "y":2.20651, "heading":2.01979, "vx":1.8955, "vy":1.6724, "omega":1.18141, "ax":-5.57275, "ay":8.43605, "alpha":-6.78872, "fx":[-69.78068,-114.45127,-128.02364,-66.90773], "fy":[160.58652,132.52484,119.249,161.61905]}, - {"t":2.11985, "x":4.86876, "y":2.26337, "heading":2.057, "vx":1.71999, "vy":1.9381, "omega":0.96759, "ax":-6.47942, "ay":7.70789, "alpha":-7.40216, "fx":[-83.75139,-128.21358,-144.25604,-84.6315], "fy":[153.64806,119.1335,98.7702,152.88404]}, - {"t":2.15134, "x":4.91972, "y":2.32823, "heading":2.08747, "vx":1.51592, "vy":2.18086, "omega":0.73446, "ax":-7.79616, "ay":6.29153, "alpha":-8.02616, "fx":[-105.29244,-146.21329,-163.3197,-115.61615], "fy":[139.56431,95.96678,61.91348,130.6239]}, - {"t":2.18284, "x":4.96359, "y":2.40004, "heading":2.1106, "vx":1.27038, "vy":2.37901, "omega":0.48168, "ax":-9.44329, "ay":3.25061, "alpha":-8.1471, "fx":[-139.53148,-166.45235,-174.50183,-162.02496], "fy":[104.87516,53.13009,-0.20342,63.36566]}, - {"t":2.21433, "x":4.99892, "y":2.47658, "heading":2.12577, "vx":0.97296, "vy":2.48139, "omega":0.22509, "ax":-9.78241, "ay":-2.14933, "alpha":-7.70176, "fx":[-173.62097,-173.44298,-157.36913,-161.15065], "fy":[14.43409,-19.85159,-75.36777,-65.45262]}, - {"t":2.24583, "x":5.02471, "y":2.55366, "heading":2.13286, "vx":0.66487, "vy":2.41369, "omega":-0.01748, "ax":-7.51062, "ay":-6.86796, "alpha":-4.55605, "fx":[-141.44925,-143.06413,-118.08337,-108.41758], "fy":[-101.92782,-100.08887,-128.65408,-136.61735]}, - {"t":2.27732, "x":5.04193, "y":2.62628, "heading":2.13231, "vx":0.42832, "vy":2.19739, "omega":-0.16097, "ax":-4.96762, "ay":-8.97594, "alpha":-1.91685, "fx":[-88.0147,-95.22035,-81.64072,-73.11561], "fy":[-150.90038,-146.54753,-154.56149,-158.703]}, - {"t":2.30882, "x":5.05295, "y":2.69103, "heading":2.12724, "vx":0.27187, "vy":1.91469, "omega":-0.22134, "ax":-3.26342, "ay":-9.75157, "alpha":-0.37319, "fx":[-55.74156,-57.92306,-55.29046,-53.0841], "fy":[-165.79491,-165.055,-165.96295,-166.67267]}, - {"t":2.34031, "x":5.0599, "y":2.7465, "heading":2.12027, "vx":0.16908, "vy":1.60757, "omega":-0.2331, "ax":-2.17407, "ay":-10.05681, "alpha":0.56749, "fx":[-37.07274,-33.16775,-36.89301,-40.78759], "fy":[-171.07551,-171.86585,-171.09426,-170.21805]}, - {"t":2.37181, "x":5.06414, "y":2.79214, "heading":2.11293, "vx":0.10061, "vy":1.29083, "omega":-0.21522, "ax":-1.44553, "ay":-10.18739, "alpha":1.17876, "fx":[-25.3874,-16.63393,-23.73898,-32.59191], "fy":[-173.28043,-174.3246,-173.48008,-172.05325]}, - {"t":2.4033, "x":5.0666, "y":2.82774, "heading":2.10615, "vx":0.05509, "vy":0.96998, "omega":-0.1781, "ax":-0.93199, "ay":-10.24571, "alpha":1.60163, "fx":[-17.51072,-5.12233,-14.0086,-26.76986], "fy":[-174.31037,-175.09837,-174.58814,-173.10953]}, - {"t":2.4348, "x":5.06787, "y":2.85321, "heading":2.10054, "vx":0.02573, "vy":0.64729, "omega":-0.12766, "ax":-0.55332, "ay":-10.27124, "alpha":1.9097, "fx":[-11.87947,3.24513,-6.58204,-22.43088], "fy":[-174.826,-175.18541,-175.06596,-173.7657]}, - {"t":2.46629, "x":5.0684, "y":2.8685, "heading":2.09652, "vx":0.00831, "vy":0.32379, "omega":-0.06751, "ax":-0.26371, "ay":-10.28084, "alpha":2.1435, "fx":[-7.6597,9.56096,-0.76594,-19.07773], "fy":[-175.09292,-174.98676,-175.21987,-174.19708]}, - {"t":2.49779, "x":5.06854, "y":2.8736, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.21054, "ay":-8.12275, "alpha":27.36671, "fx":[195.32878,-56.27088,50.07612,158.29462], "fy":[-37.15471,-190.81694,-192.77315,-120.86464]}, + {"t":0.03003, "x":1.58235, "y":7.29634, "heading":-0.9225, "vx":0.15647, "vy":-0.24392, "omega":0.8218, "ax":5.64042, "ay":-9.1135, "alpha":19.44736, "fx":[177.98317,-11.2966,58.93904,150.46643], "fy":[-88.53196,-198.55403,-190.19192,-130.39294]}, + {"t":0.06006, "x":1.58959, "y":7.2849, "heading":-0.89782, "vx":0.32585, "vy":-0.51759, "omega":1.4058, "ax":6.00033, "ay":-9.89778, "alpha":10.20668, "fx":[143.84101,44.73738,74.76783,136.74436], "fy":[-137.19975,-193.73662,-184.44336,-144.58547]}, + {"t":0.09009, "x":1.60208, "y":7.2649, "heading":-0.85561, "vx":0.50603, "vy":-0.81482, "omega":1.7123, "ax":6.17014, "ay":-10.20396, "alpha":0.98986, "fx":[106.29951,97.9907,99.61343,107.50912], "fy":[-168.0134,-172.99665,-172.09716,-167.27339]}, + {"t":0.12012, "x":1.62006, "y":7.23583, "heading":-0.80419, "vx":0.69132, "vy":-1.12124, "omega":1.74202, "ax":6.17874, "ay":-10.19352, "alpha":0.00932, "fx":[103.02583,102.95034,102.96753,103.04299], "fy":[-169.9032,-169.94903,-169.93897,-169.89315]}, + {"t":0.15015, "x":1.64361, "y":7.19756, "heading":-0.75188, "vx":0.87686, "vy":-1.42734, "omega":1.7423, "ax":6.19136, "ay":-10.17058, "alpha":0.00001, "fx":[103.20698,103.20692,103.20694,103.20699], "fy":[-169.53868,-169.53871,-169.53871,-169.53867]}, + {"t":0.18018, "x":1.67273, "y":7.15011, "heading":-0.69956, "vx":1.06278, "vy":-1.73276, "omega":1.7423, "ax":6.21797, "ay":-10.12467, "alpha":-0.00009, "fx":[103.65037,103.6511,103.65086,103.65012], "fy":[-168.77352,-168.77307,-168.77321,-168.77367]}, + {"t":0.21021, "x":1.70745, "y":7.09352, "heading":-0.64724, "vx":1.24951, "vy":-2.03679, "omega":1.7423, "ax":6.29492, "ay":-9.99468, "alpha":-0.00039, "fx":[104.93238,104.93536,104.93421,104.93122], "fy":[-166.60706,-166.60516,-166.60584,-166.60773]}, + {"t":0.24023, "x":1.74781, "y":7.02785, "heading":-0.59492, "vx":1.43854, "vy":-2.33693, "omega":1.74229, "ax":7.30314, "ay":-7.90868, "alpha":-0.62643, "fx":[120.49339,124.2949,123.04531,119.12585], "fy":[-133.46797,-129.75995,-130.16322,-133.94432]}, + {"t":0.27026, "x":1.7943, "y":6.9541, "heading":-0.5426, "vx":1.65785, "vy":-2.57442, "omega":1.72348, "ax":1.86325, "ay":-0.7829, "alpha":-4.52113, "fx":[34.54543,45.82131,27.82973,16.04175], "fy":[-28.22282,-9.0867,2.45327,-17.3461]}, + {"t":0.30029, "x":1.84492, "y":6.87644, "heading":-0.49084, "vx":1.7138, "vy":-2.59793, "omega":1.58771, "ax":-0.44052, "ay":-0.76217, "alpha":-1.55778, "fx":[-5.76691,-2.19316,-8.93867,-12.47412], "fy":[-17.82765,-11.18057,-7.57302,-14.23903]}, + {"t":0.33032, "x":1.89619, "y":6.79808, "heading":-0.44316, "vx":1.70057, "vy":-2.62082, "omega":1.54093, "ax":-0.75694, "ay":-0.9531, "alpha":-1.41231, "fx":[-10.94719,-8.02979,-14.31195,-17.18221], "fy":[-20.47258,-14.30829,-11.29746,-17.47285]}, + {"t":0.36035, "x":1.94691, "y":6.71895, "heading":-0.39689, "vx":1.67784, "vy":-2.64944, "omega":1.49852, "ax":-0.73288, "ay":-1.45116, "alpha":-2.29904, "fx":[-9.11647,-4.81203,-15.39917,-19.53948], "fy":[-31.46767,-21.34715,-16.88394,-27.06181]}, + {"t":0.39038, "x":1.99697, "y":6.63874, "heading":-0.35189, "vx":1.65583, "vy":-2.69302, "omega":1.42948, "ax":-1.01606, "ay":-2.63068, "alpha":-4.19928, "fx":[-10.36441,-3.25195,-24.00153,-30.13103], "fy":[-56.57411,-38.60842,-30.95527,-49.27087]}, + {"t":0.42041, "x":2.04623, "y":6.55668, "heading":-0.30896, "vx":1.62532, "vy":-2.77201, "omega":1.30338, "ax":-3.25183, "ay":-4.77672, "alpha":-6.00608, "fx":[-40.96587,-35.22233,-69.46196,-71.17592], "fy":[-98.03567,-75.39401,-60.38425,-84.68861]}, + {"t":0.45044, "x":2.09357, "y":6.47129, "heading":-0.26982, "vx":1.52767, "vy":-2.91546, "omega":1.12302, "ax":-7.1791, "ay":-6.10299, "alpha":-5.33053, "fx":[-101.97244,-110.58485,-137.68528,-128.44584], "fy":[-123.94607,-102.63243,-77.6712,-102.68619]}, + {"t":0.48047, "x":2.13621, "y":6.38099, "heading":-0.2361, "vx":1.31209, "vy":-3.09873, "omega":0.96295, "ax":-7.53058, "ay":-6.13608, "alpha":-6.64325, "fx":[-102.52901,-116.33543,-148.28116,-134.97907], "fy":[-130.85765,-102.97103,-71.0172,-104.29632]}, + {"t":0.5105, "x":2.17222, "y":6.28517, "heading":-0.20718, "vx":1.08595, "vy":-3.28299, "omega":0.76345, "ax":-5.9696, "ay":-3.77389, "alpha":-4.0164, "fx":[-89.50092,-91.96436,-109.62855,-106.94766], "fy":[-78.64359,-57.78966,-47.29533,-67.90689]}, + {"t":0.54053, "x":2.20214, "y":6.18488, "heading":-0.18426, "vx":0.90669, "vy":-3.39631, "omega":0.64285, "ax":-2.05046, "ay":-1.05956, "alpha":-0.97527, "fx":[-32.24448,-31.52041,-36.13014,-36.82581], "fy":[-20.55642,-15.799,-14.78186,-19.51206]}, + {"t":0.57056, "x":2.22844, "y":6.08241, "heading":-0.16495, "vx":0.84511, "vy":-3.42813, "omega":0.61356, "ax":-0.69251, "ay":-0.17804, "alpha":0.16872, "fx":[-11.88154,-12.01452,-11.20633,-11.07313], "fy":[-2.49311,-3.30585,-3.44275,-2.62967]}, + {"t":0.60059, "x":2.25351, "y":5.97939, "heading":-0.14653, "vx":0.82432, "vy":-3.43348, "omega":0.61862, "ax":-0.42064, "ay":-0.00258, "alpha":0.42141, "fx":[-7.8764,-8.17543,-6.14766,-5.84819], "fy":[1.1232,-0.90909,-1.20985,0.82386]}, + {"t":0.63062, "x":2.27807, "y":5.87628, "heading":-0.12795, "vx":0.81168, "vy":-3.43356, "omega":0.63128, "ax":-0.38481, "ay":0.05032, "alpha":0.5433, "fx":[-7.55651,-7.89436,-5.27276,-4.93462], "fy":[2.32047,-0.30567,-0.64405,1.98434]}, + {"t":0.66065, "x":2.30227, "y":5.7732, "heading":-0.109, "vx":0.80013, "vy":-3.43205, "omega":0.64759, "ax":-0.40439, "ay":0.08339, "alpha":0.65881, "fx":[-8.15902,-8.50913,-5.32309,-4.97292], "fy":[3.1599,-0.0317,-0.38138,2.81379]}, + {"t":0.69068, "x":2.32612, "y":5.67017, "heading":-0.08955, "vx":0.78798, "vy":-3.42954, "omega":0.66738, "ax":-0.43762, "ay":0.10855, "alpha":0.76986, "fx":[-8.99098,-9.3285,-5.59864,-5.26145], "fy":[3.84447,0.1076,-0.2283,3.51389]}, + {"t":0.7207, "x":2.34958, "y":5.56723, "heading":-0.06951, "vx":0.77484, "vy":-3.42628, "omega":0.6905, "ax":-0.46968, "ay":0.123, "alpha":0.86027, "fx":[-9.76901,-10.06335,-5.88931,-5.59579], "fy":[4.28561,0.10305,-0.18852,4.00115]}, + {"t":0.75073, "x":2.37264, "y":5.4644, "heading":-0.04877, "vx":0.76074, "vy":-3.42259, "omega":0.71633, "ax":-0.49136, "ay":0.12244, "alpha":0.91276, "fx":[-10.29691,-10.51764,-6.08392,-5.8644], "fy":[4.36932,-0.07406,-0.29156,4.16013]}, + {"t":0.78076, "x":2.39526, "y":5.36168, "heading":-0.02726, "vx":0.74598, "vy":-3.41891, "omega":0.74374, "ax":-0.49449, "ay":0.10165, "alpha":0.90699, "fx":[-10.38531,-10.50923,-6.10007,-5.97736], "fy":[3.9627,-0.4564,-0.57799,3.84929]}, + {"t":0.81079, "x":2.41744, "y":5.25906, "heading":-0.00493, "vx":0.73113, "vy":-3.41586, "omega":0.77098, "ax":-0.47956, "ay":0.06754, "alpha":0.85787, "fx":[-10.06845,-10.09135,-5.91925,-5.89726], "fy":[3.22581,-0.9555,-0.97776,3.2106]}, + {"t":0.84082, "x":2.43918, "y":5.15651, "heading":0.01822, "vx":0.71673, "vy":-3.41383, "omega":0.79674, "ax":-0.44797, "ay":0.02607, "alpha":0.77893, "fx":[-9.39545,-9.32728,-5.53914,-5.60782], "fy":[2.29749,-1.49829,-1.43113,2.37004]}, + {"t":0.87085, "x":2.4605, "y":5.05401, "heading":0.04215, "vx":0.70328, "vy":-3.41305, "omega":0.82013, "ax":-0.39694, "ay":-0.01965, "alpha":0.67369, "fx":[-8.32336,-8.18495,-4.91022,-5.04875], "fy":[1.24352,-2.03659,-1.90033,1.38337]}, + {"t":0.90088, "x":2.48144, "y":4.95151, "heading":0.06678, "vx":0.69136, "vy":-3.41364, "omega":0.84036, "ax":-0.31436, "ay":-0.06951, "alpha":0.53048, "fx":[-6.61495,-6.44181,-3.86567,-4.03866], "fy":[0.04487,-2.53394,-2.36312,0.21749]}, + {"t":0.93091, "x":2.50206, "y":4.84897, "heading":0.09201, "vx":0.68192, "vy":-3.41573, "omega":0.85629, "ax":-0.17141, "ay":-0.12563, "alpha":0.3172, "fx":[-3.69774,-3.55524,-2.01714,-2.15946], "fy":[-1.39584,-2.93422,-2.79289,-1.25413]}, + {"t":0.96094, "x":2.52246, "y":4.74634, "heading":0.11773, "vx":0.67677, "vy":-3.4195, "omega":0.86581, "ax":0.08771, "ay":-0.18702, "alpha":-0.01523, "fx":[1.50336,1.49463,1.42097,1.4297], "fy":[-3.14997,-3.07634,-3.0851,-3.15872]}, + {"t":0.99097, "x":2.54282, "y":4.64357, "heading":0.14373, "vx":0.67941, "vy":-3.42511, "omega":0.86536, "ax":0.54743, "ay":-0.23185, "alpha":-0.489, "fx":[10.47003,10.13456,7.77999,8.11724], "fy":[-4.87157,-2.51209,-2.85689,-5.21855]}, + {"t":1.021, "x":2.56347, "y":4.54061, "heading":0.16971, "vx":0.69585, "vy":-3.43208, "omega":0.85067, "ax":1.24559, "ay":-0.18619, "alpha":-0.9536, "fx":[23.4074,22.6511,18.11421,18.88072], "fy":[-5.00664,-0.393,-1.19054,-5.82445]}, + {"t":1.05103, "x":2.58493, "y":4.43746, "heading":0.19526, "vx":0.73325, "vy":-3.43767, "omega":0.82204, "ax":1.92394, "ay":0.04962, "alpha":-0.93896, "fx":[34.6881,33.82331,29.44859,30.32459], "fy":[-1.01438,3.57295,2.6849,-1.93481]}, + {"t":1.08106, "x":2.60781, "y":4.33425, "heading":0.21994, "vx":0.79103, "vy":-3.43618, "omega":0.79384, "ax":2.19343, "ay":0.49121, "alpha":-0.04549, "fx":[36.69281,36.6427,36.4341,36.48424], "fy":[8.09963,8.32344,8.27676,8.05286]}, + {"t":1.11109, "x":2.63256, "y":4.23129, "heading":0.24378, "vx":0.85689, "vy":-3.42143, "omega":0.79247, "ax":2.1812, "ay":1.10579, "alpha":1.57626, "fx":[31.72627,33.78956,40.98796,38.93467], "fy":[21.48805,13.73001,15.43891,23.07496]}, + {"t":1.14112, "x":2.65927, "y":4.12904, "heading":0.26758, "vx":0.92239, "vy":-3.38822, "omega":0.83981, "ax":2.13092, "ay":1.87147, "alpha":3.6342, "fx":[24.39753,29.96413,46.67001,41.05409], "fy":[38.21326,20.40925,24.52439,41.63924]}, + {"t":1.17114, "x":2.68793, "y":4.02814, "heading":0.2928, "vx":0.98638, "vy":-3.33202, "omega":0.94894, "ax":2.11135, "ay":2.67722, "alpha":5.66612, "fx":[17.06457,27.05466,53.49784,43.16356], "fy":[55.45941,28.01826,34.66691,60.36754]}, + {"t":1.20117, "x":2.7185, "y":3.92929, "heading":0.32129, "vx":1.04979, "vy":-3.25163, "omega":1.11909, "ax":1.95473, "ay":3.14479, "alpha":6.63869, "fx":[10.62603,23.47846,54.88604,41.34681], "fy":[64.53263,33.07362,41.48008,70.60191]}, + {"t":1.2312, "x":2.75091, "y":3.83306, "heading":0.3549, "vx":1.10849, "vy":-3.15719, "omega":1.31845, "ax":1.27425, "ay":2.73124, "alpha":5.3967, "fx":[3.45512,13.83273,39.31967,28.35694], "fy":[54.10585,29.35324,37.60459,61.05007]}, + {"t":1.26123, "x":2.78477, "y":3.73949, "heading":0.39449, "vx":1.14675, "vy":-3.07517, "omega":1.48051, "ax":0.30036, "ay":1.64606, "alpha":2.69952, "fx":[-3.73564,1.42411,13.81772,8.52112], "fy":[31.02235,18.97751,23.9493,35.80703]}, + {"t":1.29126, "x":2.81934, "y":3.64788, "heading":0.43895, "vx":1.15577, "vy":-3.02574, "omega":1.56157, "ax":-0.07797, "ay":1.07425, "alpha":1.30428, "fx":[-5.55043,-2.84847,2.96387,0.23639], "fy":[19.42175,13.70791,16.40573,22.09385]}, + {"t":1.32129, "x":2.85402, "y":3.55751, "heading":0.48584, "vx":1.15343, "vy":-2.99348, "omega":1.60074, "ax":1.19447, "ay":1.52426, "alpha":1.55917, "fx":[14.70561,18.4499,25.11136,21.37781], "fy":[27.13646,20.32774,23.72594,30.44444]}, + {"t":1.35132, "x":2.88919, "y":3.4683, "heading":0.53391, "vx":1.1893, "vy":-2.94771, "omega":1.64756, "ax":6.20144, "ay":2.98282, "alpha":1.09095, "fx":[100.22181,103.59846,106.47971,103.19995], "fy":[51.83922,45.52546,47.70022,53.82353]}, + {"t":1.38135, "x":2.9277, "y":3.38113, "heading":0.58339, "vx":1.37552, "vy":-2.85814, "omega":1.68032, "ax":10.0174, "ay":2.75603, "alpha":-4.87592, "fx":[174.06531,162.01505,159.83706,172.02383], "fy":[35.63244,69.87487,59.81776,18.44192]}, + {"t":1.41138, "x":2.97352, "y":3.29654, "heading":0.63385, "vx":1.67634, "vy":-2.77538, "omega":1.5339, "ax":6.86802, "ay":7.43489, "alpha":8.53224, "fx":[76.21343,135.45799,143.60309,102.67189], "fy":[151.257,96.09427,103.12892,145.2639]}, + {"t":1.43013, "x":3.00615, "y":3.24582, "heading":0.6626, "vx":1.80508, "vy":-2.63601, "omega":1.69384, "ax":7.18229, "ay":5.279, "alpha":1.43913, "fx":[115.21659,121.90843,124.06772,117.70863], "fy":[91.42871,82.47039,84.79917,93.29519]}, + {"t":1.44887, "x":3.04125, "y":3.19734, "heading":0.69435, "vx":1.93972, "vy":-2.53705, "omega":1.72082, "ax":5.01168, "ay":3.9007, "alpha":0.1425, "fx":[83.07475,83.63492,84.00895,83.45084], "fy":[65.20473,64.51182,64.84218,65.53254]}, + {"t":1.46762, "x":3.0785, "y":3.15046, "heading":0.72661, "vx":2.03367, "vy":-2.46393, "omega":1.72349, "ax":2.41404, "ay":2.42693, "alpha":0.05321, "fx":[40.05698,40.24515,40.42477,40.23674], "fy":[40.48159,40.2738,40.4301,40.63773]}, + {"t":1.48636, "x":3.11704, "y":3.1047, "heading":0.75892, "vx":2.07892, "vy":-2.41843, "omega":1.72449, "ax":0.66638, "ay":1.31277, "alpha":0.02481, "fx":[11.02208,11.10702,11.19446,11.10952], "fy":[21.8866,21.79869,21.88001,21.9679]}, + {"t":1.50511, "x":3.15613, "y":3.0596, "heading":0.79124, "vx":2.09141, "vy":-2.39382, "omega":1.72495, "ax":-0.26969, "ay":0.66299, "alpha":0.01033, "fx":[-4.53122,-4.49543,-4.45987,-4.49566], "fy":[11.05146,11.01626,11.05204,11.08723]}, + {"t":1.52385, "x":3.19529, "y":3.01484, "heading":0.82358, "vx":2.08636, "vy":-2.3814, "omega":1.72515, "ax":-0.74038, "ay":0.30577, "alpha":0.00346, "fx":[-12.35361,-12.34134,-12.32989,-12.34216], "fy":[5.09648,5.08504,5.09746,5.1089]}, + {"t":1.5426, "x":3.23427, "y":2.97025, "heading":0.85592, "vx":2.07248, "vy":-2.37566, "omega":1.72521, "ax":-0.97616, "ay":0.10224, "alpha":0.00022, "fx":[-16.2728,-16.27198,-16.27128,-16.27209], "fy":[1.70417,1.70346,1.70429,1.705]}, + {"t":1.56134, "x":3.27295, "y":2.92574, "heading":0.88826, "vx":2.05418, "vy":-2.37375, "omega":1.72521, "ax":-1.09536, "ay":-0.02377, "alpha":-0.00132, "fx":[-18.25461,-18.25955,-18.26356,-18.25863], "fy":[-0.39576,-0.39167,-0.39668,-0.40077]}, + {"t":1.58009, "x":3.31126, "y":2.88124, "heading":0.9206, "vx":2.03365, "vy":-2.37419, "omega":1.72519, "ax":-1.15568, "ay":-0.11053, "alpha":-0.00206, "fx":[-19.25762,-19.26553,-19.27153,-19.26361], "fy":[-1.84148,-1.83535,-1.84339,-1.84952]}, + {"t":1.59884, "x":3.34918, "y":2.83671, "heading":0.95294, "vx":2.01198, "vy":-2.37626, "omega":1.72515, "ax":-1.18505, "ay":-0.17684, "alpha":-0.00241, "fx":[-19.74606,-19.75557,-19.76228,-19.75276], "fy":[-2.94649,-2.9396,-2.94926,-2.95615]}, + {"t":1.61758, "x":3.38669, "y":2.79214, "heading":0.98528, "vx":1.98977, "vy":-2.37958, "omega":1.72511, "ax":-1.19681, "ay":-0.23174, "alpha":-0.00257, "fx":[-19.94158,-19.95196,-19.95876,-19.94838], "fy":[-3.86119,-3.85417,-3.8647,-3.87172]}, + {"t":1.63633, "x":3.42378, "y":2.74749, "heading":1.01762, "vx":1.96733, "vy":-2.38392, "omega":1.72506, "ax":-1.1966, "ay":-0.27902, "alpha":-0.00262, "fx":[-19.93815,-19.94894,-19.95551,-19.94471], "fy":[-4.64901,-4.64221,-4.65315,-4.65995]}, + {"t":1.65507, "x":3.46044, "y":2.70275, "heading":1.04995, "vx":1.9449, "vy":-2.38915, "omega":1.72501, "ax":-1.18492, "ay":-0.31894, "alpha":-0.00257, "fx":[-19.74362,-19.75443,-19.76053,-19.74971], "fy":[-5.31419,-5.30787,-5.31882,-5.32514]}, + {"t":1.67382, "x":3.49669, "y":2.65791, "heading":1.08229, "vx":1.92269, "vy":-2.39513, "omega":1.72496, "ax":-1.15635, "ay":-0.34768, "alpha":-0.00241, "fx":[-19.26797,-19.27828,-19.28365,-19.27334], "fy":[-5.79322,-5.78764,-5.79807,-5.80365]}, + {"t":1.69256, "x":3.53253, "y":2.61295, "heading":1.11463, "vx":1.90101, "vy":-2.40165, "omega":1.72491, "ax":-1.09477, "ay":-0.35397, "alpha":-0.00203, "fx":[-18.24266,-18.25151,-18.25576,-18.24691], "fy":[-5.8983,-5.89389,-5.90283,-5.90724]}, + {"t":1.71131, "x":3.56798, "y":2.56787, "heading":1.14696, "vx":1.88049, "vy":-2.40829, "omega":1.72488, "ax":-0.9602, "ay":-0.30982, "alpha":-0.00123, "fx":[-16.00222,-16.00766,-16.01005,-16.00461], "fy":[-5.16298,-5.16051,-5.166,-5.16846]}, + {"t":1.73006, "x":3.60306, "y":2.52267, "heading":1.17929, "vx":1.86249, "vy":-2.41409, "omega":1.72485, "ax":-0.65639, "ay":-0.14691, "alpha":0.00055, "fx":[-10.94347,-10.941,-10.93995,-10.94241], "fy":[-2.44963,-2.4507,-2.44822,-2.44715]}, + {"t":1.7488, "x":3.63786, "y":2.47739, "heading":1.21163, "vx":1.85019, "vy":-2.41685, "omega":1.72486, "ax":0.04577, "ay":0.30245, "alpha":0.0049, "fx":[0.74752,0.76988,0.77831,0.75595], "fy":[5.03476,5.02633,5.04866,5.05709]}, + {"t":1.76755, "x":3.67255, "y":2.43214, "heading":1.24396, "vx":1.85105, "vy":-2.41118, "omega":1.72496, "ax":1.61202, "ay":1.40883, "alpha":0.01642, "fx":[26.82186,26.89811,26.9213,26.84507], "fy":[23.46103,23.4328,23.50805,23.53628]}, + {"t":1.78629, "x":3.70753, "y":2.38718, "heading":1.2763, "vx":1.88126, "vy":-2.38477, "omega":1.72526, "ax":4.2133, "ay":3.45415, "alpha":0.0407, "fx":[70.12849,70.32222,70.33878,70.14515], "fy":[57.53541,57.43692,57.62263,57.72109]}, + {"t":1.80504, "x":3.74354, "y":2.34309, "heading":1.30864, "vx":1.96024, "vy":-2.32002, "omega":1.72603, "ax":6.36473, "ay":5.55101, "alpha":0.0702, "fx":[105.96132,106.31304,106.23243,105.88093], "fy":[92.50723,92.24929,92.55835,92.81585]}, + {"t":1.82378, "x":3.7814, "y":2.30057, "heading":1.34099, "vx":2.07956, "vy":-2.21596, "omega":1.72734, "ax":7.23466, "ay":6.99639, "alpha":0.09093, "fx":[120.45532,120.94783,120.74106,120.24878], "fy":[116.64599,116.23687,116.60764,117.01535]}, + {"t":1.84253, "x":3.82165, "y":2.26026, "heading":1.37337, "vx":2.21517, "vy":-2.08481, "omega":1.72905, "ax":7.33204, "ay":7.99155, "alpha":0.09859, "fx":[122.0732,122.65468,122.36962,121.78852], "fy":[133.27222,132.79787,133.15942,133.63158]}, + {"t":1.86128, "x":3.86447, "y":2.22258, "heading":1.40579, "vx":2.35262, "vy":-1.935, "omega":1.7309, "ax":7.06269, "ay":8.75142, "alpha":0.09592, "fx":[117.57847,118.19198,117.88452,117.27165], "fy":[145.95862,145.4968,145.8065,146.26594]}, + {"t":1.88002, "x":3.90981, "y":2.18785, "heading":1.43823, "vx":2.48501, "vy":-1.77095, "omega":1.73269, "ax":6.60654, "ay":9.37585, "alpha":0.08742, "fx":[109.9728,110.57454,110.28235,109.68145], "fy":[156.37231,155.96693,156.21072,156.61398]}, + {"t":1.89877, "x":3.95755, "y":2.1563, "heading":1.47071, "vx":2.60886, "vy":-1.59519, "omega":1.73433, "ax":6.0411, "ay":9.90841, "alpha":0.07807, "fx":[100.54641,101.12013,100.85744,100.28466], "fy":[165.24618,164.9071,165.0914,165.42874]}, + {"t":1.91751, "x":4.00752, "y":2.12814, "heading":1.50322, "vx":2.7221, "vy":-1.40945, "omega":1.7358, "ax":5.39469, "ay":10.37274, "alpha":0.08306, "fx":[89.74009,90.3872,90.11307,89.46735], "fy":[172.99279,172.66374,172.82552,173.1526]}, + {"t":1.93626, "x":4.0595, "y":2.10354, "heading":1.53576, "vx":2.82323, "vy":-1.21501, "omega":1.73735, "ax":4.57443, "ay":10.82377, "alpha":0.14412, "fx":[75.87926,77.07235,76.62523,75.43749], "fy":[180.56889,180.07244,180.28788,180.77872]}, + {"t":1.955, "x":4.11322, "y":2.08266, "heading":1.56833, "vx":2.90898, "vy":-1.01211, "omega":1.74005, "ax":2.69124, "ay":11.48362, "alpha":0.06772, "fx":[44.62301,45.24139,45.09953,44.48291], "fy":[191.4768,191.33376,191.37634,191.5185]}, + {"t":1.97375, "x":4.16823, "y":2.06571, "heading":1.60095, "vx":2.95943, "vy":-0.79684, "omega":1.74132, "ax":0.41478, "ay":11.82263, "alpha":0.00723, "fx":[6.88179,6.95123,6.94671,6.87729], "fy":[197.07832,197.07593,197.07686,197.07924]}, + {"t":1.99249, "x":4.22378, "y":2.05285, "heading":1.63359, "vx":2.9672, "vy":-0.57522, "omega":1.74146, "ax":-1.15592, "ay":11.79892, "alpha":0.00175, "fx":[-19.27734,-19.26053,-19.25996,-19.27677], "fy":[196.68145,196.68309,196.68329,196.68165]}, + {"t":2.01124, "x":4.27919, "y":2.04414, "heading":1.66624, "vx":2.94553, "vy":-0.35404, "omega":1.74149, "ax":-2.21772, "ay":11.66462, "alpha":0.00071, "fx":[-36.97197,-36.96525,-36.96464,-36.97136], "fy":[194.44286,194.44414,194.4443,194.44303]}, + {"t":2.02999, "x":4.33402, "y":2.03955, "heading":1.69888, "vx":2.90396, "vy":-0.13538, "omega":1.74151, "ax":-2.96231, "ay":11.51176, "alpha":0.00037, "fx":[-49.38212,-49.37871,-49.3783,-49.3817], "fy":[191.89494,191.89581,191.89594,191.89507]}, + {"t":2.04873, "x":4.38794, "y":2.03904, "heading":1.73153, "vx":2.84843, "vy":0.08041, "omega":1.74151, "ax":-3.50613, "ay":11.36833, "alpha":0.00021, "fx":[-58.44662,-58.44469,-58.44443,-58.44636], "fy":[189.50428,189.50488,189.50497,189.50438]}, + {"t":2.06748, "x":4.44072, "y":2.04254, "heading":1.76417, "vx":2.78271, "vy":0.29352, "omega":1.74152, "ax":-3.91777, "ay":11.24125, "alpha":0.0001, "fx":[-65.30789,-65.30694,-65.3068,-65.30775], "fy":[187.38604,187.38637,187.38643,187.38609]}, + {"t":2.08622, "x":4.49219, "y":2.05002, "heading":1.79682, "vx":2.70926, "vy":0.50424, "omega":1.74152, "ax":-4.23878, "ay":11.13069, "alpha":-0.00093, "fx":[-70.65362,-70.66194,-70.66308,-70.65476], "fy":[185.54515,185.54199,185.54152,185.54468]}, + {"t":2.10497, "x":4.54223, "y":2.06143, "heading":1.82946, "vx":2.62981, "vy":0.7129, "omega":1.7415, "ax":-4.49533, "ay":11.0349, "alpha":-0.04017, "fx":[-74.73339,-75.09115,-75.13706,-74.77865], "fy":[184.02926,183.88377,183.86343,184.00928]}, + {"t":2.12371, "x":4.59074, "y":2.07673, "heading":1.86211, "vx":2.54554, "vy":0.91975, "omega":1.74075, "ax":-4.70242, "ay":10.9467, "alpha":-1.24247, "fx":[-72.42074,-83.1132,-84.66283,-73.35171], "fy":[185.04335,180.49856,179.7316,184.63125]}, + {"t":2.14246, "x":4.63763, "y":2.09589, "heading":1.89474, "vx":2.45739, "vy":1.12495, "omega":1.71746, "ax":-4.80332, "ay":10.72226, "alpha":-7.04828, "fx":[-52.07846,-103.42444,-117.26956,-47.50397], "fy":[191.89645,169.83751,160.34837,192.85739]}, + {"t":2.16121, "x":4.68285, "y":2.11887, "heading":1.92694, "vx":2.36735, "vy":1.32595, "omega":1.58533, "ax":-5.16557, "ay":10.59998, "alpha":-6.42761, "fx":[-60.4215,-107.46154,-119.4277,-57.11941], "fy":[189.63817,167.53638,159.0841,190.52743]}, + {"t":2.19201, "x":4.75333, "y":2.16474, "heading":1.97577, "vx":2.20822, "vy":1.65249, "omega":1.38733, "ax":-5.84304, "ay":10.21034, "alpha":-6.91559, "fx":[-70.70537,-118.97599,-132.32085,-67.60071], "fy":[185.97353,159.48231,148.40081,186.94944]}, + {"t":2.22282, "x":4.81858, "y":2.22049, "heading":2.01851, "vx":2.02822, "vy":1.96703, "omega":1.17429, "ax":-6.81571, "ay":9.53823, "alpha":-7.57835, "fx":[-85.19095,-134.21947,-150.35261,-84.69522], "fy":[179.68394,146.75776,129.88719,179.6618]}, + {"t":2.25362, "x":4.87783, "y":2.28561, "heading":2.05469, "vx":1.81826, "vy":2.26086, "omega":0.94083, "ax":-8.2565, "ay":8.23996, "alpha":-8.39341, "fx":[-107.27194,-154.73348,-173.99106,-114.53098], "fy":[167.24278,124.72616,95.53898,161.9168]}, + {"t":2.28443, "x":4.92993, "y":2.35917, "heading":2.08367, "vx":1.56391, "vy":2.5147, "omega":0.68226, "ax":-10.26859, "ay":5.41262, "alpha":-8.92368, "fx":[-143.08329,-180.27959,-195.6565,-165.67059], "fy":[137.44686,83.20491,32.14755,108.10372]}, + {"t":2.31523, "x":4.97323, "y":2.43921, "heading":2.10469, "vx":1.24758, "vy":2.68144, "omega":0.40736, "ax":-11.59248, "ay":-0.39787, "alpha":-8.85616, "fx":[-190.14274,-198.23491,-189.4262,-195.16021], "fy":[55.20189,6.8993,-58.30836,-30.32234]}, + {"t":2.34604, "x":5.00616, "y":2.52162, "heading":2.11724, "vx":0.89046, "vy":2.66918, "omega":0.13454, "ax":-9.63579, "ay":-6.73211, "alpha":-6.17894, "fx":[-178.33248,-176.46442,-148.08128,-139.61803], "fy":[-85.97855,-90.53374,-131.96105,-140.41069]}, + {"t":2.37685, "x":5.02902, "y":2.60065, "heading":2.12138, "vx":0.59362, "vy":2.46179, "omega":-0.0558, "ax":-6.54828, "ay":-9.90781, "alpha":-2.93413, "fx":[-115.63984,-124.17069,-104.18059,-92.63565], "fy":[-161.20119,-154.88882,-169.03564,-175.50834]}, + {"t":2.40765, "x":5.0442, "y":2.67179, "heading":2.11966, "vx":0.3919, "vy":2.15658, "omega":-0.14619, "ax":-4.38276, "ay":-11.08133, "alpha":-1.04148, "fx":[-74.03509,-79.58735,-72.19287,-66.41866], "fy":[-184.37406,-182.0775,-185.15466,-187.27584]}, + {"t":2.43846, "x":5.0542, "y":2.73297, "heading":2.11516, "vx":0.25689, "vy":1.81521, "omega":-0.17828, "ax":-3.01454, "ay":-11.54197, "alpha":0.08928, "fx":[-50.24298,-49.65747,-50.2593,-50.84423], "fy":[-192.4033,-192.55355,-192.39568,-192.24364]}, + {"t":2.46926, "x":5.06068, "y":2.78341, "heading":2.10967, "vx":0.16402, "vy":1.45965, "omega":-0.17553, "ax":-2.11469, "ay":-11.74378, "alpha":0.81143, "fx":[-35.62474,-29.77883,-34.86966,-40.73017], "fy":[-195.74713,-196.71049,-195.85858,-194.73622]}, + {"t":2.50007, "x":5.06473, "y":2.8228, "heading":2.10426, "vx":0.09888, "vy":1.09787, "omega":-0.15053, "ax":-1.48925, "ay":-11.83973, "alpha":1.30374, "fx":[-25.91704,-16.04243,-23.65699,-33.68397], "fy":[-197.3399,-198.37378,-197.5909,-196.14604]}, + {"t":2.53087, "x":5.06707, "y":2.85101, "heading":2.09962, "vx":0.053, "vy":0.73314, "omega":-0.11037, "ax":-1.03313, "ay":-11.88748, "alpha":1.65814, "fx":[-19.05716,-6.1243,-15.19597,-28.5095], "fy":[-198.17161,-198.97836,-198.46954,-197.01449]}, + {"t":2.56168, "x":5.06821, "y":2.86795, "heading":2.09622, "vx":0.02117, "vy":0.36694, "omega":-0.05929, "ax":-0.68727, "ay":-11.91136, "alpha":1.92451, "fx":[-13.96663,1.31972,-8.6234,-24.55541], "fy":[-198.6337,-199.10713,-198.90141,-197.58422]}, + {"t":2.59249, "x":5.06854, "y":2.8736, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToI.traj b/src/main/deploy/choreo/fetchToI.traj index 007b22e8..18875e85 100644 --- a/src/main/deploy/choreo/fetchToI.traj +++ b/src/main/deploy/choreo/fetchToI.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.334167003631592, "y":5.231322765350342, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.334167003631592, "y":5.231322765350342, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.334167003631592 m", "val":5.334167003631592}, "y":{"exp":"5.231322765350342 m", "val":5.231322765350342}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.334167003631592 m", "val":5.334167003631592}, "y":{"exp":"5.231322765350342 m", "val":5.231322765350342}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,58 +28,57 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.27899,1.53726], + "waypoints":[0.0,1.33398,1.57861], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.79753, "ay":-4.55168, "alpha":-10.00183, "fx":[122.94141,162.68044,174.8774,138.07467], "fy":[-124.96508,-65.38182,-11.57945,-107.76502]}, - {"t":0.03654, "x":1.57729, "y":7.41238, "heading":-0.94268, "vx":0.32149, "vy":-0.16633, "omega":-0.36549, "ax":8.83195, "ay":-4.55621, "alpha":-9.56698, "fx":[124.39301,162.48082,174.61885,139.42302], "fy":[-123.4916,-65.82875,-14.70735,-105.97188]}, - {"t":0.07309, "x":1.59493, "y":7.40326, "heading":-0.95603, "vx":0.64423, "vy":-0.33283, "omega":-0.7151, "ax":8.8742, "ay":-4.56271, "alpha":-8.98981, "fx":[125.93572,161.99242,174.21155,141.65056], "fy":[-121.88044,-66.96259,-18.67652,-102.92192]}, - {"t":0.10963, "x":1.6244, "y":7.38805, "heading":-0.98216, "vx":0.96852, "vy":-0.49956, "omega":-1.04361, "ax":8.92351, "ay":-4.57161, "alpha":-8.25435, "fx":[127.82336,161.22543,173.56321,144.53374], "fy":[-119.84882,-68.71361,-23.71913,-98.76539]}, - {"t":0.14617, "x":1.66575, "y":7.36674, "heading":-1.0203, "vx":1.29461, "vy":-0.66662, "omega":-1.34525, "ax":8.97908, "ay":-4.58399, "alpha":-7.32184, "fx":[130.44836,160.19601,172.50413,147.77785], "fy":[-116.91595,-70.98103,-30.23446,-93.75828]}, - {"t":0.18271, "x":1.71905, "y":7.33932, "heading":-1.06946, "vx":1.62272, "vy":-0.83413, "omega":-1.61281, "ax":9.03994, "ay":-4.60181, "alpha":-6.09877, "fx":[134.42428,158.93928,170.69646,151.00708], "fy":[-112.21895,-73.61272,-38.91241,-88.35805]}, - {"t":0.21926, "x":1.78439, "y":7.30577, "heading":-1.1284, "vx":1.95307, "vy":-1.00229, "omega":-1.83567, "ax":9.10368, "ay":-4.62625, "alpha":-4.36498, "fx":[140.70695,157.53936,167.38666,153.77123], "fy":[-104.06972,-76.34936,-51.02492,-83.32098]}, - {"t":0.2558, "x":1.86184, "y":7.26605, "heading":-1.19548, "vx":2.28574, "vy":-1.17135, "omega":-1.99518, "ax":9.15656, "ay":-4.64991, "alpha":-1.60822, "fx":[150.63227,156.21991,160.60075,155.54863], "fy":[-88.80485,-78.63427,-69.1531,-79.78201]}, - {"t":0.29234, "x":1.95148, "y":7.22015, "heading":-1.26839, "vx":2.62035, "vy":-1.34127, "omega":-2.05395, "ax":9.11396, "ay":-4.62529, "alpha":3.35969, "fx":[164.76314,155.75234,143.9132,155.67499], "fy":[-57.84492,-78.64352,-98.89831,-79.31243]}, - {"t":0.32888, "x":2.05332, "y":7.16804, "heading":-1.34344, "vx":2.9534, "vy":-1.51029, "omega":-1.93118, "ax":8.54666, "ay":-4.19891, "alpha":12.95969, "fx":[174.21763,161.71782,92.49453,153.07491], "fy":[5.02969,-59.1798,-147.61068,-83.9285]}, - {"t":0.36543, "x":2.16695, "y":7.11005, "heading":-1.41401, "vx":3.26571, "vy":-1.66373, "omega":-1.45759, "ax":7.9939, "ay":-3.11314, "alpha":19.51706, "fx":[171.80766,164.04861,59.18033,148.85924], "fy":[25.44372,16.02034,-162.98246,-90.29578]}, - {"t":0.40197, "x":2.29162, "y":7.04717, "heading":-1.46728, "vx":3.55783, "vy":-1.77749, "omega":-0.74439, "ax":7.68511, "ay":-3.3142, "alpha":19.62622, "fx":[170.19763,152.97921,55.75756,143.95177], "fy":[16.96379,12.51447,-160.78762,-94.18498]}, - {"t":0.43851, "x":2.42677, "y":6.98001, "heading":-1.49448, "vx":3.83867, "vy":-1.8986, "omega":-0.02719, "ax":-0.26032, "ay":-1.06338, "alpha":0.58926, "fx":[-3.09909,-5.99283,-5.75232,-2.86789], "fy":[-16.56888,-16.77243,-19.60488,-19.40493]}, - {"t":0.47506, "x":2.56687, "y":6.90992, "heading":-1.49547, "vx":3.82915, "vy":-1.93746, "omega":-0.00566, "ax":-0.09131, "ay":-0.18038, "alpha":0.00033, "fx":[-1.55243,-1.55402,-1.5539,-1.55231], "fy":[-3.06728,-3.0674,-3.06899,-3.06887]}, - {"t":0.5116, "x":2.70674, "y":6.839, "heading":-1.49568, "vx":3.82582, "vy":-1.94405, "omega":-0.00565, "ax":-0.01718, "ay":-0.03379, "alpha":0.0, "fx":[-0.29215,-0.29218,-0.29218,-0.29215], "fy":[-0.57476,-0.57476,-0.57478,-0.57478]}, - {"t":0.54814, "x":2.84653, "y":6.76793, "heading":-1.49589, "vx":3.82519, "vy":-1.94529, "omega":-0.00565, "ax":-0.00322, "ay":-0.00633, "alpha":0.0, "fx":[-0.05479,-0.05478,-0.05478,-0.05479], "fy":[-0.10772,-0.10771,-0.1077,-0.1077]}, - {"t":0.58468, "x":2.98631, "y":6.69684, "heading":-1.49609, "vx":3.82507, "vy":-1.94552, "omega":-0.00565, "ax":-0.0006, "ay":-0.00119, "alpha":0.0, "fx":[-0.01028,-0.01026,-0.01026,-0.01028], "fy":[-0.02019,-0.02019,-0.02017,-0.02017]}, - {"t":0.62123, "x":3.12609, "y":6.62575, "heading":-1.4963, "vx":3.82505, "vy":-1.94556, "omega":-0.00565, "ax":-0.00011, "ay":-0.00022, "alpha":0.0, "fx":[-0.00194,-0.00192,-0.00192,-0.00194], "fy":[-0.00379,-0.00379,-0.00377,-0.00377]}, - {"t":0.65777, "x":3.26587, "y":6.55465, "heading":-1.4965, "vx":3.82505, "vy":-1.94557, "omega":-0.00565, "ax":-0.00002, "ay":-0.00004, "alpha":0.0, "fx":[-0.00039,-0.00037,-0.00037,-0.00039], "fy":[-0.00075,-0.00075,-0.00073,-0.00073]}, - {"t":0.69431, "x":3.40564, "y":6.48356, "heading":-1.49671, "vx":3.82505, "vy":-1.94557, "omega":-0.00565, "ax":-0.00001, "ay":-0.00002, "alpha":0.0, "fx":[-0.00019,-0.00018,-0.00018,-0.0002], "fy":[-0.00037,-0.00036,-0.00035,-0.00035]}, - {"t":0.73085, "x":3.54542, "y":6.41246, "heading":-1.49692, "vx":3.82504, "vy":-1.94557, "omega":-0.00565, "ax":-0.00004, "ay":-0.00007, "alpha":0.0, "fx":[-0.00065,-0.00063,-0.00063,-0.00065], "fy":[-0.00126,-0.00126,-0.00124,-0.00125]}, - {"t":0.7674, "x":3.6852, "y":6.34136, "heading":-1.49712, "vx":3.82504, "vy":-1.94557, "omega":-0.00565, "ax":-0.0002, "ay":-0.00039, "alpha":0.0, "fx":[-0.00337,-0.00336,-0.00336,-0.00338], "fy":[-0.00662,-0.00662,-0.0066,-0.0066]}, - {"t":0.80394, "x":3.82498, "y":6.27027, "heading":-1.49733, "vx":3.82504, "vy":-1.94559, "omega":-0.00565, "ax":-0.00105, "ay":-0.00207, "alpha":0.0, "fx":[-0.01795,-0.01793,-0.01794,-0.01795], "fy":[-0.03528,-0.03528,-0.03526,-0.03526]}, - {"t":0.84048, "x":3.96475, "y":6.19917, "heading":-1.49754, "vx":3.825, "vy":-1.94566, "omega":-0.00565, "ax":-0.00563, "ay":-0.01106, "alpha":0.0, "fx":[-0.09575,-0.09574,-0.09574,-0.09575], "fy":[-0.18821,-0.18821,-0.1882,-0.1882]}, - {"t":0.87702, "x":4.10453, "y":6.12806, "heading":-1.49774, "vx":3.82479, "vy":-1.94607, "omega":-0.00565, "ax":-0.03005, "ay":-0.05902, "alpha":0.00001, "fx":[-0.51115,-0.5112,-0.5112,-0.51114], "fy":[-1.00397,-1.00397,-1.00402,-1.00402]}, - {"t":0.91357, "x":4.24427, "y":6.05691, "heading":-1.49795, "vx":3.82369, "vy":-1.94823, "omega":-0.00565, "ax":-0.16072, "ay":-0.31427, "alpha":0.00006, "fx":[-2.73369,-2.73399,-2.73397,-2.73367], "fy":[-5.34555,-5.34557,-5.34587,-5.34585]}, - {"t":0.95011, "x":4.38389, "y":5.9855, "heading":-1.49816, "vx":3.81782, "vy":-1.95971, "omega":-0.00565, "ax":-0.84617, "ay":-1.61718, "alpha":0.00013, "fx":[-14.39279,-14.39342,-14.39336,-14.39273], "fy":[-27.50743,-27.50746,-27.50807,-27.50804]}, - {"t":0.98665, "x":4.52284, "y":5.91281, "heading":-1.49836, "vx":3.7869, "vy":-2.01881, "omega":-0.00564, "ax":-3.08306, "ay":-5.42775, "alpha":-0.00163, "fx":[-52.44507,-52.43582,-52.439,-52.44825], "fy":[-92.32681,-92.32884,-92.3222,-92.32018]}, - {"t":1.0232, "x":4.65917, "y":5.83541, "heading":-1.49857, "vx":3.67424, "vy":-2.21715, "omega":-0.0057, "ax":-5.0948, "ay":-7.49912, "alpha":-0.31979, "fx":[-86.98865,-85.07369,-86.3336,-88.24822], "fy":[-127.64104,-128.66698,-127.48389,-126.43977]}, - {"t":1.05974, "x":4.79003, "y":5.74939, "heading":-1.49878, "vx":3.48806, "vy":-2.49119, "omega":-0.01739, "ax":-9.07212, "ay":-0.81109, "alpha":-15.5178, "fx":[-161.1027,-135.83391,-151.66566,-168.65428], "fy":[-58.64473,-98.49972,71.30528,30.65339]}, - {"t":1.09628, "x":4.91144, "y":5.65781, "heading":-1.49941, "vx":3.15654, "vy":-2.52083, "omega":-0.58445, "ax":-8.98008, "ay":1.18607, "alpha":-17.55451, "fx":[-170.02472,-157.35042,-121.0452,-162.57371], "fy":[-36.18374,-68.32165,123.27189,61.93253]}, - {"t":1.13282, "x":5.02079, "y":5.56648, "heading":-1.52077, "vx":2.82838, "vy":-2.47749, "omega":-1.22594, "ax":-8.98859, "ay":1.66195, "alpha":-17.23725, "fx":[-172.0886,-163.38325,-115.69643,-160.40533], "fy":[-28.67138,-56.86951,129.76556,68.8524]}, - {"t":1.16937, "x":5.11815, "y":5.47706, "heading":-1.56557, "vx":2.49991, "vy":-2.41675, "omega":-1.85583, "ax":-9.6713, "ay":2.92175, "alpha":-6.38693, "fx":[-173.77678,-172.40883,-150.10955,-161.72892], "fy":[17.78566,25.87885,88.96024,66.16748]}, - {"t":1.20591, "x":5.20304, "y":5.39069, "heading":-1.63339, "vx":2.1465, "vy":-2.30999, "omega":-2.08923, "ax":-9.67262, "ay":3.47425, "alpha":-0.49944, "fx":[-165.53166,-165.09465,-163.48966,-163.99835], "fy":[56.29633,57.51293,61.94278,60.63176]}, - {"t":1.24245, "x":5.27502, "y":5.3086, "heading":-1.70973, "vx":1.79304, "vy":-2.18303, "omega":-2.10748, "ax":-9.55272, "ay":3.73705, "alpha":2.72586, "fx":[-156.09236,-159.52479,-167.83622,-166.50281], "fy":[79.01985,72.00596,49.58983,53.64922]}, - {"t":1.27899, "x":5.33417, "y":5.23132, "heading":-1.78674, "vx":1.44395, "vy":-2.04647, "omega":-2.00787, "ax":-9.21452, "ay":4.39156, "alpha":3.82092, "fx":[-145.93061,-152.10216,-165.33218,-163.58013], "fy":[95.75739,85.95035,56.36626,60.7225]}, - {"t":1.29744, "x":5.35924, "y":5.19432, "heading":-1.82378, "vx":1.27397, "vy":-1.96545, "omega":-1.93738, "ax":-8.60346, "ay":5.41894, "alpha":5.01762, "fx":[-127.71939,-140.35258,-160.58456,-156.71292], "fy":[119.01672,104.13886,68.8464,76.69657]}, - {"t":1.31589, "x":5.38127, "y":5.15898, "heading":-1.85952, "vx":1.11526, "vy":-1.86549, "omega":-1.84482, "ax":-7.94456, "ay":6.27877, "alpha":6.02308, "fx":[-108.16438,-128.45724,-155.42214,-148.49527], "fy":[137.09592,118.58816,79.91745,91.59887]}, - {"t":1.33434, "x":5.4005, "y":5.12564, "heading":-1.89356, "vx":0.9687, "vy":-1.74966, "omega":-1.73371, "ax":-7.28216, "ay":6.97996, "alpha":6.82867, "fx":[-89.0061,-117.05964,-150.08687,-139.3173], "fy":[150.30976,129.9256,89.62204,105.05109]}, - {"t":1.35278, "x":5.41713, "y":5.09455, "heading":-1.92554, "vx":0.83437, "vy":-1.6209, "omega":-1.60774, "ax":-6.64554, "ay":7.54333, "alpha":7.455, "fx":[-71.29923,-106.48739,-144.76457,-129.6038], "fy":[159.53767,138.78556,98.0616,116.8548]}, - {"t":1.37123, "x":5.43139, "y":5.06593, "heading":-1.9552, "vx":0.71178, "vy":-1.48175, "omega":-1.47022, "ax":-6.05076, "ay":7.99284, "alpha":7.93539, "fx":[-55.49331,-96.86658,-139.58693,-119.74019], "fy":[165.75942,145.71666,105.36669,126.9808]}, - {"t":1.38968, "x":5.44349, "y":5.03996, "heading":-1.98232, "vx":0.60016, "vy":-1.3343, "omega":-1.32383, "ax":-5.50463, "ay":8.35085, "alpha":8.30332, "fx":[-41.652,-88.20986,-134.64016,-110.02652], "fy":[169.82082,151.15952,111.6757,135.52617]}, - {"t":1.40813, "x":5.45362, "y":5.01676, "heading":-2.00674, "vx":0.49861, "vy":-1.18025, "omega":-1.17066, "ax":-5.00825, "ay":8.63639, "alpha":8.58684, "fx":[-29.6437,-80.47305,-129.97559,-100.66307], "fy":[172.37115,155.45534,117.1218,142.66203]}, - {"t":1.42657, "x":5.46197, "y":4.99646, "heading":-2.02833, "vx":0.40622, "vy":-1.02093, "omega":-1.01225, "ax":-4.55957, "ay":8.86492, "alpha":8.8075, "fx":[-19.26176,-73.5879,-125.61946,-91.75876], "fy":[173.88039,158.86401,121.82621,148.58867]}, - {"t":1.44502, "x":5.46869, "y":4.97914, "heading":-2.04701, "vx":0.32211, "vy":-0.8574, "omega":-0.84978, "ax":-4.15496, "ay":9.04868, "alpha":8.98128, "fx":[-10.28761,-67.47968,-121.58091,-83.35063], "fy":[174.67922,161.58245,125.8955,153.50472]}, - {"t":1.46347, "x":5.47392, "y":4.96486, "heading":-2.06268, "vx":0.24546, "vy":-0.69047, "omega":-0.6841, "ax":-3.79016, "ay":9.19723, "alpha":9.12, "fx":[-2.51908,-62.07597,-117.85795,-75.42541], "fy":[174.99763,163.76005,129.4214,157.58975]}, - {"t":1.48191, "x":5.4778, "y":4.95369, "heading":-2.0753, "vx":0.17554, "vy":-0.52081, "omega":-0.51586, "ax":-3.46083, "ay":9.31798, "alpha":9.23254, "fx":[4.21963,-57.31052,-114.44163,-67.93826], "fy":[174.99523,165.51057,132.48193,160.99677]}, - {"t":1.50036, "x":5.48045, "y":4.94566, "heading":-2.08482, "vx":0.1117, "vy":-0.34892, "omega":-0.34554, "ax":-3.16281, "ay":9.41665, "alpha":9.32583, "fx":[10.07669,-53.12453,-111.31891,-60.8271], "fy":[174.78315,166.92126,135.14299,163.85094]}, - {"t":1.51881, "x":5.48198, "y":4.94083, "heading":-2.09119, "vx":0.05336, "vy":-0.17521, "omega":-0.17351, "ax":-2.8923, "ay":9.49766, "alpha":9.40553, "fx":[15.17487,-49.46671,-108.47449,-54.0227], "fy":[174.43924,168.05939,137.46013,166.25138]}, - {"t":1.53726, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.19863, "ay":-5.26921, "alpha":-19.61353, "fx":[117.80278,188.05418,195.24232,112.24773], "fy":[-160.63517,-65.7931,39.1921,-164.10455]}, + {"t":0.03811, "x":1.58668, "y":7.29617, "heading":-0.9225, "vx":0.35059, "vy":-0.20083, "omega":-0.74754, "ax":9.45706, "ay":-5.23702, "alpha":-17.93093, "fx":[121.80822,187.64797,196.77194,124.35047], "fy":[-157.57539,-66.85427,30.31145,-155.07613]}, + {"t":0.07623, "x":1.60691, "y":7.28471, "heading":-0.95099, "vx":0.71104, "vy":-0.40043, "omega":-1.43096, "ax":9.78535, "ay":-5.21127, "alpha":-15.43487, "fx":[126.52396,186.32811,198.36205,141.25428], "fy":[-153.74464,-70.32395,16.36021,-139.76923]}, + {"t":0.11434, "x":1.64112, "y":7.26567, "heading":-1.00553, "vx":1.08399, "vy":-0.59905, "omega":-2.01924, "ax":10.14178, "ay":-5.19873, "alpha":-12.18245, "fx":[134.43686,184.0885,198.89653,158.81231], "fy":[-146.75447,-75.80137,-4.73501,-119.35079]}, + {"t":0.15245, "x":1.6898, "y":7.23906, "heading":-1.08249, "vx":1.47054, "vy":-0.7972, "omega":-2.48356, "ax":10.48233, "ay":-5.24624, "alpha":-7.53816, "fx":[150.80327,180.97875,195.07312,172.08666], "fy":[-129.63013,-82.60103,-38.39653,-99.1817]}, + {"t":0.19057, "x":1.75346, "y":7.20486, "heading":-1.17715, "vx":1.87006, "vy":-0.99715, "omega":-2.77087, "ax":10.6616, "ay":-5.28719, "alpha":1.72818, "fx":[182.83825,177.44998,172.34172,178.26477], "fy":[-77.41756,-88.96516,-98.61191,-87.5453]}, + {"t":0.22868, "x":1.83248, "y":7.16302, "heading":-1.28276, "vx":2.27641, "vy":-1.19866, "omega":-2.705, "ax":9.38427, "ay":-3.7072, "alpha":20.96226, "fx":[191.88257,185.44618,71.74461,176.65142], "fy":[49.8874,-21.61519,-184.77225,-90.689]}, + {"t":0.2668, "x":1.92606, "y":7.11464, "heading":-1.38586, "vx":2.63408, "vy":-1.33996, "omega":-1.90605, "ax":9.2953, "ay":-3.46207, "alpha":22.41269, "fx":[194.73318,186.94103,67.18608,170.93271], "fy":[34.30435,20.77709,-185.72918,-100.19617]}, + {"t":0.30491, "x":2.03321, "y":7.06106, "heading":-1.4585, "vx":2.98836, "vy":-1.47191, "omega":-1.05182, "ax":9.02425, "ay":-3.72768, "alpha":22.77515, "fx":[194.76897,179.83295,62.07957,165.03797], "fy":[22.2318,21.68532,-185.28448,-107.18719]}, + {"t":0.34302, "x":2.15366, "y":7.00225, "heading":-1.49859, "vx":3.33231, "vy":-1.61399, "omega":-0.18377, "ax":1.04482, "ay":-2.22522, "alpha":4.57909, "fx":[28.64059,5.25172,6.68006,29.09442], "fy":[-25.21224,-27.38722,-49.07854,-46.69513]}, + {"t":0.38114, "x":2.28142, "y":6.93912, "heading":-1.5056, "vx":3.37213, "vy":-1.6988, "omega":-0.00925, "ax":-0.08119, "ay":-0.16298, "alpha":0.00242, "fx":[-1.34791,-1.35969,-1.35891,-1.34713], "fy":[-2.71058,-2.71136,-2.72313,-2.72235]}, + {"t":0.41925, "x":2.40989, "y":6.87425, "heading":-1.50595, "vx":3.36903, "vy":-1.70501, "omega":-0.00915, "ax":-0.00932, "ay":-0.01843, "alpha":0.0, "fx":[-0.15544,-0.15544,-0.15544,-0.15544], "fy":[-0.30714,-0.30715,-0.30714,-0.30714]}, + {"t":0.45736, "x":2.53829, "y":6.80925, "heading":-1.5063, "vx":3.36868, "vy":-1.70571, "omega":-0.00915, "ax":-0.00106, "ay":-0.00208, "alpha":-0.00002, "fx":[-0.01767,-0.01757,-0.01758,-0.01768], "fy":[-0.03479,-0.03478,-0.03468,-0.03469]}, + {"t":0.49548, "x":2.66668, "y":6.74424, "heading":-1.50665, "vx":3.36864, "vy":-1.70579, "omega":-0.00915, "ax":-0.00012, "ay":-0.00023, "alpha":-0.00002, "fx":[-0.00204,-0.00193,-0.00194,-0.00205], "fy":[-0.0039,-0.00389,-0.00379,-0.00379]}, + {"t":0.53359, "x":2.79507, "y":6.67923, "heading":-1.50699, "vx":3.36863, "vy":-1.7058, "omega":-0.00915, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00002, "fx":[-0.00027,-0.00016,-0.00017,-0.00027], "fy":[-0.0004,-0.00039,-0.00028,-0.00029]}, + {"t":0.57171, "x":2.92346, "y":6.61421, "heading":-1.50734, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,0.00004,0.00004,-0.00007], "fy":[0.0,0.00001,0.00012,0.00011]}, + {"t":0.60982, "x":3.05185, "y":6.5492, "heading":-1.50769, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00006,0.00006,-0.00005], "fy":[0.00005,0.00005,0.00016,0.00015]}, + {"t":0.64793, "x":3.18025, "y":6.48418, "heading":-1.50804, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]}, + {"t":0.68605, "x":3.30864, "y":6.41917, "heading":-1.50839, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]}, + {"t":0.72416, "x":3.43703, "y":6.35415, "heading":-1.50874, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]}, + {"t":0.76227, "x":3.56542, "y":6.28914, "heading":-1.50909, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]}, + {"t":0.80039, "x":3.69381, "y":6.22413, "heading":-1.50944, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]}, + {"t":0.8385, "x":3.8222, "y":6.15911, "heading":-1.50979, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00006,0.00006,-0.00005], "fy":[0.00005,0.00005,0.00016,0.00015]}, + {"t":0.87662, "x":3.95059, "y":6.0941, "heading":-1.51014, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,0.00003,0.00003,-0.00008], "fy":[-0.00001,-0.00001,0.0001,0.0001]}, + {"t":0.91473, "x":4.07898, "y":6.02908, "heading":-1.51048, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":-0.00002, "ay":-0.00003, "alpha":-0.00002, "fx":[-0.00033,-0.00022,-0.00023,-0.00033], "fy":[-0.00052,-0.00051,-0.0004,-0.00041]}, + {"t":0.95284, "x":4.20737, "y":5.96407, "heading":-1.51083, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":-0.00015, "ay":-0.00029, "alpha":-0.00002, "fx":[-0.00257,-0.00247,-0.00247,-0.00258], "fy":[-0.00495,-0.00495,-0.00484,-0.00485]}, + {"t":0.99096, "x":4.33577, "y":5.89905, "heading":-1.51118, "vx":3.36863, "vy":-1.70581, "omega":-0.00916, "ax":-0.00134, "ay":-0.00264, "alpha":-0.00002, "fx":[-0.02238,-0.02229,-0.02229,-0.02239], "fy":[-0.04409,-0.04408,-0.04399,-0.04399]}, + {"t":1.02907, "x":4.46416, "y":5.83404, "heading":-1.51153, "vx":3.36857, "vy":-1.70591, "omega":-0.00917, "ax":-0.01182, "ay":-0.02334, "alpha":0.0, "fx":[-0.19706,-0.19708,-0.19708,-0.19706], "fy":[-0.38909,-0.38909,-0.38911,-0.38911]}, + {"t":1.06718, "x":4.59254, "y":5.769, "heading":-1.51188, "vx":3.36812, "vy":-1.7068, "omega":-0.00917, "ax":-0.10447, "ay":-0.20564, "alpha":0.00019, "fx":[-1.74109,-1.74203,-1.74197,-1.74103], "fy":[-3.42749,-3.42754,-3.42848,-3.42843]}, + {"t":1.1053, "x":4.72083, "y":5.7038, "heading":-1.51223, "vx":3.36414, "vy":-1.71464, "omega":-0.00916, "ax":-0.91597, "ay":-1.75358, "alpha":0.00103, "fx":[-15.2664,-15.27148,-15.27108,-15.266], "fy":[-29.22872,-29.2289,-29.23385,-29.23367]}, + {"t":1.14341, "x":4.84839, "y":5.63717, "heading":-1.51258, "vx":3.32923, "vy":-1.78148, "omega":-0.00912, "ax":-4.39536, "ay":-7.34895, "alpha":-0.08639, "fx":[-73.41372,-72.90314,-73.12299,-73.63406], "fy":[-122.58269,-122.75354,-122.42458,-122.25317]}, + {"t":1.18153, "x":4.97208, "y":5.56394, "heading":-1.51293, "vx":3.16171, "vy":-2.06157, "omega":-0.01241, "ax":-9.85912, "ay":-4.35398, "alpha":-12.51517, "fx":[-163.77061,-123.05542,-180.93167,-189.62918], "fy":[-99.68515,-141.89888,-28.16663,-20.56457]}, + {"t":1.21964, "x":5.08543, "y":5.4822, "heading":-1.5134, "vx":2.78594, "vy":-2.22752, "omega":-0.48941, "ax":-10.61238, "ay":0.36801, "alpha":-18.99875, "fx":[-190.44131,-170.13722,-157.64764,-189.38696], "fy":[-51.65603,-95.34089,115.86393,55.67145]}, + {"t":1.25775, "x":5.1839, "y":5.39757, "heading":-1.53205, "vx":2.38146, "vy":-2.21349, "omega":-1.21352, "ax":-10.7035, "ay":0.81794, "alpha":-18.64371, "fx":[-193.17425,-177.40774,-154.80739,-188.29952], "fy":[-44.50356,-85.50256,122.40591,62.13875]}, + {"t":1.29587, "x":5.26689, "y":5.3138, "heading":-1.57831, "vx":1.97351, "vy":-2.18232, "omega":-1.92411, "ax":-10.93816, "ay":0.96515, "alpha":-16.69589, "fx":[-195.10353,-183.26506,-162.58411,-188.38272], "fy":[-37.08059,-74.50844,112.99958,62.94352]}, + {"t":1.33398, "x":5.33417, "y":5.23132, "heading":-1.65164, "vx":1.55662, "vy":-2.14553, "omega":-2.56045, "ax":-11.23346, "ay":3.30457, "alpha":-6.55829, "fx":[-196.41272,-195.44921,-173.92682,-183.23649], "fy":[24.33156,27.40758,93.62974,74.97355]}, + {"t":1.3528, "x":5.36147, "y":5.19153, "heading":-1.69982, "vx":1.34523, "vy":-2.08335, "omega":-2.68386, "ax":-10.40308, "ay":5.71511, "alpha":-0.33086, "fx":[-174.40999,-173.84494,-172.40214,-173.00036], "fy":[93.4614,94.4603,97.08716,96.06351]}, + {"t":1.37162, "x":5.38494, "y":5.15334, "heading":-1.75033, "vx":1.14947, "vy":-1.9758, "omega":-2.69008, "ax":-9.20918, "ay":7.38335, "alpha":4.63954, "fx":[-132.72709,-149.58936,-170.96519,-160.76851], "fy":[146.92029,130.0555,100.09915,115.23238]}, + {"t":1.39043, "x":5.40494, "y":5.11747, "heading":-1.80095, "vx":0.97618, "vy":-1.83687, "omega":-2.60278, "ax":-8.03789, "ay":8.42497, "alpha":8.25392, "fx":[-88.9296,-129.98095,-169.36434,-147.67611], "fy":[177.11258,149.98043,103.16156,131.50613]}, + {"t":1.40925, "x":5.42189, "y":5.0844, "heading":-1.84992, "vx":0.82492, "vy":-1.67833, "omega":-2.44746, "ax":-7.04313, "ay":9.06394, "alpha":10.70514, "fx":[-52.57208,-114.51692,-167.60189,-134.93164], "fy":[191.27108,162.30088,106.28717,144.50673]}, + {"t":1.42807, "x":5.43616, "y":5.05442, "heading":-1.89598, "vx":0.69239, "vy":-1.50777, "omega":-2.24602, "ax":-6.23461, "ay":9.46886, "alpha":12.37044, "fx":[-24.48171,-102.05924,-165.73501,-123.43557], "fy":[196.99489,170.54865,109.39552,154.42616]}, + {"t":1.44689, "x":5.44809, "y":5.02772, "heading":-1.93824, "vx":0.57507, "vy":-1.32959, "omega":-2.01324, "ax":-5.58167, "ay":9.73586, "alpha":13.54061, "fx":[-2.91782,-91.79753,-163.83174,-113.62775], "fy":[198.60329,176.38324,112.39727,161.78451]}, + {"t":1.4657, "x":5.45792, "y":5.00443, "heading":-1.97613, "vx":0.47004, "vy":-1.14638, "omega":-1.75844, "ax":-5.05164, "ay":9.91805, "alpha":14.39419, "fx":[13.86179,-83.21602,-161.95701,-105.5225], "fy":[198.23066,180.66435,115.21746,167.20403]}, + {"t":1.48452, "x":5.46587, "y":4.98461, "heading":-2.00922, "vx":0.37498, "vy":-0.95975, "omega":-1.48757, "ax":-4.61697, "ay":10.04649, "alpha":15.0357, "fx":[27.13236,-75.98715,-160.1683,-98.82746], "fy":[196.92635,183.87888,117.79936,171.2762]}, + {"t":1.50334, "x":5.47211, "y":4.96833, "heading":-2.03721, "vx":0.2881, "vy":-0.7707, "omega":-1.20464, "ax":-4.25501, "ay":10.14068, "alpha":15.52641, "fx":[37.76504,-69.89789,-158.51496,-93.06811], "fy":[195.22629,186.32405,120.1022,174.50869]}, + {"t":1.52216, "x":5.47678, "y":4.95562, "heading":-2.05988, "vx":0.20803, "vy":-0.57988, "omega":-0.91247, "ax":-3.94696, "ay":10.21359, "alpha":15.90278, "fx":[46.35173,-64.80538,-157.03876,-87.68348], "fy":[193.41786,188.19265,122.09733,177.31469]}, + {"t":1.54097, "x":5.48, "y":4.94652, "heading":-2.07705, "vx":0.13376, "vy":-0.38768, "omega":-0.61322, "ax":-3.67693, "ay":10.2741, "alpha":16.18789, "fx":[53.30134,-60.61059,-155.77477,-82.08638], "fy":[191.66227,189.61572,123.76468,180.01477]}, + {"t":1.55979, "x":5.48186, "y":4.94104, "heading":-2.08859, "vx":0.06457, "vy":-0.19435, "omega":-0.3086, "ax":-3.43119, "ay":10.32817, "alpha":16.39967, "fx":[58.90352,-57.24242,-154.75241,-75.69378], "fy":[190.05286,190.6852,125.08954,182.83519]}, + {"t":1.57861, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToJ.traj b/src/main/deploy/choreo/fetchToJ.traj index afedb9c2..be6170a3 100644 --- a/src/main/deploy/choreo/fetchToJ.traj +++ b/src/main/deploy/choreo/fetchToJ.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.066040992736816, "y":5.357499599456787, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.066040992736816, "y":5.357499599456787, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.066040992736816 m", "val":5.066040992736816}, "y":{"exp":"5.357499599456787 m", "val":5.357499599456787}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.066040992736816 m", "val":5.066040992736816}, "y":{"exp":"5.357499599456787 m", "val":5.357499599456787}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,56 +28,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.21683,1.45822], + "waypoints":[0.0,1.26054,1.48928], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.76941, "ay":-4.58984, "alpha":-10.09461, "fx":[122.23365,162.49911,174.85969,137.06798], "fy":[-125.65353,-65.82443,-11.778,-109.03122]}, - {"t":0.03579, "x":1.57703, "y":7.41248, "heading":-0.94268, "vx":0.31385, "vy":-0.16427, "omega":-0.36128, "ax":8.80402, "ay":-4.59424, "alpha":-9.66251, "fx":[123.66873,162.29944,174.60053,138.44698], "fy":[-124.21319,-66.26837,-14.87282,-107.23271]}, - {"t":0.07158, "x":1.5939, "y":7.40366, "heading":-0.95561, "vx":0.62894, "vy":-0.32869, "omega":-0.70709, "ax":8.84633, "ay":-4.6006, "alpha":-9.09182, "fx":[125.18444,161.81681,174.19665,140.69609], "fy":[-122.64874,-67.3805,-18.77822,-104.21205]}, - {"t":0.10737, "x":1.62208, "y":7.38895, "heading":-0.98091, "vx":0.94554, "vy":-0.49334, "omega":-1.03248, "ax":8.89554, "ay":-4.60925, "alpha":-8.36872, "fx":[127.01995,161.06108,173.56143,143.60022], "fy":[-120.6978,-69.0944,-23.70789,-100.10767]}, - {"t":0.14316, "x":1.66161, "y":7.36834, "heading":-1.01786, "vx":1.2639, "vy":-0.6583, "omega":-1.33199, "ax":8.95083, "ay":-4.62111, "alpha":-7.45943, "fx":[129.54098,160.04752,172.53859,146.87716], "fy":[-117.92043,-71.31502,-30.02541,-95.15425]}, - {"t":0.17895, "x":1.71258, "y":7.34182, "heading":-1.06553, "vx":1.58425, "vy":-0.82369, "omega":-1.59896, "ax":9.01122, "ay":-4.63797, "alpha":-6.28264, "fx":[133.31252,158.80821,170.82446,150.16785], "fy":[-113.54157,-73.90133,-38.34858,-89.77077]}, - {"t":0.21473, "x":1.77505, "y":7.30937, "heading":-1.12276, "vx":1.90675, "vy":-0.98968, "omega":-1.82381, "ax":9.07469, "ay":-4.66117, "alpha":-4.64886, "fx":[139.20619,157.41784,167.76401,153.0436], "fy":[-106.08208,-76.6201,-49.78931,-84.64888]}, - {"t":0.25052, "x":1.8491, "y":7.27097, "heading":-1.18803, "vx":2.23153, "vy":-1.1565, "omega":-1.99019, "ax":9.13095, "ay":-4.68534, "alpha":-2.12509, "fx":[148.45708,156.06649,161.72084,155.01476], "fy":[-92.43101,-78.99699,-66.53476,-80.82241]}, - {"t":0.28631, "x":1.93481, "y":7.22658, "heading":-1.25926, "vx":2.55832, "vy":-1.32418, "omega":-2.06624, "ax":9.1192, "ay":-4.67484, "alpha":2.27048, "fx":[161.91076,155.35542,147.70939,155.48444], "fy":[-65.51047,-79.63802,-93.21289,-79.70965]}, - {"t":0.3221, "x":2.03221, "y":7.17619, "heading":-1.33321, "vx":2.88468, "vy":-1.49149, "omega":-1.98498, "ax":8.72388, "ay":-4.42214, "alpha":10.54327, "fx":[173.99515,158.68856,107.33343,153.546], "fy":[-11.09336,-69.3562,-137.31095,-83.11683]}, - {"t":0.35789, "x":2.14104, "y":7.11998, "heading":-1.40425, "vx":3.19691, "vy":-1.64975, "omega":-1.60765, "ax":8.02096, "ay":-3.12321, "alpha":19.38589, "fx":[171.84343,165.09634,59.41183,149.3851], "fy":[26.57224,13.83431,-163.16836,-89.73791]}, - {"t":0.39368, "x":2.26059, "y":7.05894, "heading":-1.46179, "vx":3.48397, "vy":-1.76153, "omega":-0.91384, "ax":7.82208, "ay":-3.23064, "alpha":19.71071, "fx":[171.27677,158.43048,57.05558,145.44267], "fy":[18.69129,17.07614,-161.90091,-93.67589]}, - {"t":0.42947, "x":2.39029, "y":6.99382, "heading":-1.49449, "vx":3.76391, "vy":-1.87715, "omega":-0.20841, "ax":1.59335, "ay":-2.12546, "alpha":5.65719, "fx":[41.39022,12.36603,13.70895,40.94461], "fy":[-21.0421,-24.44894,-51.62072,-47.50208]}, - {"t":0.46526, "x":2.52602, "y":6.92528, "heading":-1.50195, "vx":3.82094, "vy":-1.95322, "omega":-0.00595, "ax":-0.11321, "ay":-0.22419, "alpha":0.00341, "fx":[-1.91789,-1.93448,-1.93332,-1.91672], "fy":[-3.80459,-3.80573,-3.82231,-3.82117]}, - {"t":0.50105, "x":2.66269, "y":6.85523, "heading":-1.50216, "vx":3.81689, "vy":-1.96125, "omega":-0.00582, "ax":-0.02248, "ay":-0.04373, "alpha":0.00001, "fx":[-0.38236,-0.38241,-0.3824,-0.38236], "fy":[-0.74385,-0.74386,-0.7439,-0.7439]}, - {"t":0.53684, "x":2.79928, "y":6.78501, "heading":-1.50237, "vx":3.81608, "vy":-1.96281, "omega":-0.00582, "ax":-0.0044, "ay":-0.00856, "alpha":0.0, "fx":[-0.07487,-0.07486,-0.07486,-0.07487], "fy":[-0.14554,-0.14554,-0.14553,-0.14553]}, - {"t":0.57263, "x":2.93586, "y":6.71476, "heading":-1.50258, "vx":3.81593, "vy":-1.96312, "omega":-0.00582, "ax":-0.00086, "ay":-0.00167, "alpha":0.0, "fx":[-0.01467,-0.01465,-0.01465,-0.01467], "fy":[-0.0285,-0.0285,-0.02848,-0.02848]}, - {"t":0.60842, "x":3.07242, "y":6.6445, "heading":-1.50279, "vx":3.81589, "vy":-1.96318, "omega":-0.00582, "ax":-0.00017, "ay":-0.00033, "alpha":0.0, "fx":[-0.00293,-0.00291,-0.00291,-0.00293], "fy":[-0.00568,-0.00567,-0.00565,-0.00566]}, - {"t":0.6442, "x":3.20899, "y":6.57424, "heading":-1.503, "vx":3.81589, "vy":-1.96319, "omega":-0.00582, "ax":-0.00005, "ay":-0.00009, "alpha":0.0, "fx":[-0.00084,-0.00082,-0.00082,-0.00084], "fy":[-0.00162,-0.00162,-0.0016,-0.0016]}, - {"t":0.67999, "x":3.34556, "y":6.50398, "heading":-1.50321, "vx":3.81589, "vy":-1.96319, "omega":-0.00582, "ax":-0.00009, "ay":-0.00017, "alpha":0.0, "fx":[-0.00151,-0.00149,-0.00149,-0.00151], "fy":[-0.00291,-0.00291,-0.00289,-0.00289]}, - {"t":0.71578, "x":3.48213, "y":6.43372, "heading":-1.50341, "vx":3.81588, "vy":-1.9632, "omega":-0.00582, "ax":-0.00042, "ay":-0.00081, "alpha":0.0, "fx":[-0.00712,-0.0071,-0.0071,-0.00712], "fy":[-0.01383,-0.01382,-0.0138,-0.01381]}, - {"t":0.75157, "x":3.61869, "y":6.36346, "heading":-1.50362, "vx":3.81587, "vy":-1.96323, "omega":-0.00582, "ax":-0.00213, "ay":-0.00414, "alpha":0.0, "fx":[-0.03625,-0.03624,-0.03624,-0.03625], "fy":[-0.07045,-0.07045,-0.07043,-0.07043]}, - {"t":0.78736, "x":3.75526, "y":6.29319, "heading":-1.50383, "vx":3.81579, "vy":-1.96338, "omega":-0.00583, "ax":-0.01089, "ay":-0.02116, "alpha":0.0, "fx":[-0.18525,-0.18525,-0.18525,-0.18525], "fy":[-0.35995,-0.35995,-0.35996,-0.35996]}, - {"t":0.82315, "x":3.89181, "y":6.22291, "heading":-1.50404, "vx":3.8154, "vy":-1.96413, "omega":-0.00582, "ax":-0.05572, "ay":-0.1081, "alpha":0.00002, "fx":[-0.94766,-0.94777,-0.94776,-0.94765], "fy":[-1.83868,-1.83869,-1.8388,-1.83879]}, - {"t":0.85894, "x":4.02833, "y":6.15255, "heading":-1.50425, "vx":3.81341, "vy":-1.968, "omega":-0.00582, "ax":-0.28544, "ay":-0.54959, "alpha":0.0001, "fx":[-4.85497,-4.85545,-4.85542,-4.85493], "fy":[-9.34811,-9.34814,-9.34863,-9.3486]}, - {"t":0.89473, "x":4.16463, "y":6.08176, "heading":-1.50446, "vx":3.80319, "vy":-1.98767, "omega":-0.00582, "ax":-1.38218, "ay":-2.56754, "alpha":0.00004, "fx":[-23.51034,-23.51055,-23.51053,-23.51031], "fy":[-43.67299,-43.67299,-43.6732,-43.6732]}, - {"t":0.93052, "x":4.29985, "y":6.00898, "heading":-1.50466, "vx":3.75373, "vy":-2.07956, "omega":-0.00582, "ax":-3.80608, "ay":-6.36785, "alpha":-0.03172, "fx":[-64.79243,-64.60521,-64.68809,-64.87538], "fy":[-108.34459,-108.40704,-108.28604,-108.2235]}, - {"t":0.96631, "x":4.43176, "y":5.93048, "heading":-1.50487, "vx":3.61751, "vy":-2.30746, "omega":-0.00695, "ax":-7.399, "ay":-5.49768, "alpha":-6.83211, "fx":[-126.30463,-97.27831,-130.93232,-148.90394], "fy":[-103.22182,-126.60447,-82.49768,-61.73219]}, - {"t":1.0021, "x":4.55649, "y":5.84437, "heading":-1.50512, "vx":3.35271, "vy":-2.50422, "omega":-0.25147, "ax":-8.88543, "ay":1.18344, "alpha":-17.7753, "fx":[-169.21962,-154.79315,-118.94526,-161.59679], "fy":[-36.01814,-69.22156,123.39241,62.36742]}, - {"t":1.03789, "x":4.67079, "y":5.75551, "heading":-1.51412, "vx":3.0347, "vy":-2.46186, "omega":-0.88763, "ax":-8.83529, "ay":1.76617, "alpha":-18.04183, "fx":[-171.77682,-161.99813,-108.42867,-158.93934], "fy":[-28.7121,-57.99339,135.32516,71.54849]}, - {"t":1.07367, "x":4.77374, "y":5.66853, "heading":-1.54589, "vx":2.7185, "vy":-2.39865, "omega":-1.53333, "ax":-9.2919, "ay":2.55238, "alpha":-12.63255, "fx":[-174.37308,-173.20416,-125.96618,-158.66684], "fy":[-7.94108,-11.62732,120.24525,72.98432]}, - {"t":1.10946, "x":4.86508, "y":5.58432, "heading":-1.60077, "vx":2.38595, "vy":-2.30731, "omega":-1.98544, "ax":-9.62388, "ay":3.4612, "alpha":-3.28526, "fx":[-169.82865,-167.80283,-156.01389,-161.15212], "fy":[41.13522,48.2165,78.49977,67.64458]}, - {"t":1.14525, "x":4.94431, "y":5.50396, "heading":-1.67182, "vx":2.04152, "vy":-2.18343, "omega":-2.10302, "ax":-9.55162, "ay":3.79349, "alpha":1.0669, "fx":[-160.01059,-161.2611,-164.7602,-163.84932], "fy":[70.56647,67.74396,58.6977,61.09669]}, - {"t":1.18104, "x":5.01125, "y":5.42825, "heading":-1.74709, "vx":1.69967, "vy":-2.04767, "omega":-2.06484, "ax":-9.43685, "ay":3.96234, "alpha":3.69388, "fx":[-151.12953,-156.40125,-167.96179,-166.57961], "fy":[88.18524,78.64782,49.29761,53.46257]}, - {"t":1.21683, "x":5.06604, "y":5.3575, "heading":-1.82099, "vx":1.36194, "vy":-1.90586, "omega":-1.93264, "ax":-9.10076, "ay":4.57055, "alpha":4.64621, "fx":[-140.94411,-149.04647,-165.38834,-163.8261], "fy":[103.05245,91.28888,56.43298,60.20071]}, - {"t":1.2354, "x":5.08976, "y":5.3229, "heading":-1.85687, "vx":1.19295, "vy":-1.82099, "omega":-1.84636, "ax":-8.47812, "ay":5.56374, "alpha":5.74815, "fx":[-122.10371,-137.19911,-160.57211,-156.9665], "fy":[124.85043,108.36468,69.04571,76.28998]}, - {"t":1.25397, "x":5.11045, "y":5.29004, "heading":-1.89116, "vx":1.03552, "vy":-1.71768, "omega":-1.73963, "ax":-7.81288, "ay":6.39766, "alpha":6.656, "fx":[-102.31961,-125.29622,-155.2904,-148.6735], "fy":[141.57834,122.00428,80.30423,91.40264]}, - {"t":1.27254, "x":5.12833, "y":5.25925, "heading":-1.92346, "vx":0.89045, "vy":-1.59889, "omega":-1.61604, "ax":-7.14613, "ay":7.08082, "alpha":7.36681, "fx":[-83.1943,-113.91619,-149.79022,-139.31407], "fy":[153.66035,132.75504,90.22123,105.13453]}, - {"t":1.29111, "x":5.14363, "y":5.23078, "heading":-1.95347, "vx":0.75776, "vy":-1.46741, "omega":-1.47925, "ax":-6.50507, "ay":7.63241, "alpha":7.90511, "fx":[-65.66081,-103.36271,-144.26803,-129.30564], "fy":[161.99022,141.1812,98.87567,117.25355]}, - {"t":1.30967, "x":5.15658, "y":5.20485, "heading":-1.98093, "vx":0.63697, "vy":-1.32568, "omega":-1.33246, "ax":-5.90481, "ay":8.07448, "alpha":8.30615, "fx":[-50.0898,-93.75778,-138.8678,-119.04095], "fy":[167.51628,147.78064,106.38338,127.69786]}, - {"t":1.32824, "x":5.16739, "y":5.18163, "heading":-2.00568, "vx":0.52732, "vy":-1.17575, "omega":-1.17823, "ax":-5.35199, "ay":8.42777, "alpha":8.60395, "fx":[-36.50228,-85.11712,-133.68713,-108.83704], "fy":[171.042,152.96045,112.87458,136.53882]}, - {"t":1.34681, "x":5.17626, "y":5.16125, "heading":-2.02755, "vx":0.42794, "vy":-1.01926, "omega":-1.01846, "ax":-4.84795, "ay":8.71016, "alpha":8.82636, "fx":[-24.74651,-77.40084,-128.78666,-98.91503], "fy":[173.17873,157.04064,118.47908,143.93069]}, - {"t":1.36538, "x":5.18337, "y":5.14383, "heading":-2.04647, "vx":0.33793, "vy":-0.85753, "omega":-0.85457, "ax":-4.39095, "ay":8.93633, "alpha":8.99438, "fx":[-14.6076,-70.54406,-124.19951,-89.40428], "fy":[174.3668,160.26793,123.31786,150.06478]}, - {"t":1.38395, "x":5.18889, "y":5.12944, "heading":-2.06233, "vx":0.25639, "vy":-0.69159, "omega":-0.68756, "ax":-3.97769, "ay":9.11805, "alpha":9.12333, "fx":[-5.86445,-64.47408,-119.93938,-80.35958], "fy":[174.91486,162.83097,127.49917,155.13635]}, - {"t":1.40252, "x":5.19297, "y":5.11817, "heading":-2.0751, "vx":0.18253, "vy":-0.52229, "omega":-0.51815, "ax":-3.60416, "ay":9.2646, "alpha":9.22441, "fx":[1.68511,-59.1193,-116.00693,-71.78205], "fy":[175.03685,164.87379,131.1174,159.32495]}, - {"t":1.42108, "x":5.19573, "y":5.11007, "heading":-2.08472, "vx":0.11561, "vy":-0.35026, "omega":-0.34687, "ax":-3.26622, "ay":9.38326, "alpha":9.306, "fx":[8.21586,-54.41345,-112.39434,-63.63815], "fy":[174.88062,166.50662,134.25364,162.78522]}, - {"t":1.43965, "x":5.19732, "y":5.10519, "heading":-2.09116, "vx":0.05496, "vy":-0.17602, "omega":-0.17407, "ax":-2.95987, "ay":9.47964, "alpha":9.37463, "fx":[13.87455,-50.29716,-109.08855,-55.87476], "fy":[174.54865,167.81416,136.97695,165.64443]}, - {"t":1.45822, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.08607, "ay":-5.28672, "alpha":-20.28608, "fx":[116.26898,188.16113,194.57562,106.83609], "fy":[-161.74078,-65.47054,42.33172,-167.62871]}, + {"t":0.03707, "x":1.58624, "y":7.29637, "heading":-0.9225, "vx":0.33686, "vy":-0.196, "omega":-0.7521, "ax":9.35394, "ay":-5.25091, "alpha":-18.60079, "fx":[120.12667,187.78515,196.1771,119.61367], "fy":[-158.8518,-66.45028,33.88221,-158.70065]}, + {"t":0.07415, "x":1.60516, "y":7.28549, "heading":-0.95038, "vx":0.68366, "vy":-0.39068, "omega":-1.44171, "ax":9.69813, "ay":-5.21835, "alpha":-16.08914, "fx":[124.59001,186.51668,197.96649,137.57948], "fy":[-155.30509,-69.80357,20.47415,-143.31509]}, + {"t":0.11122, "x":1.63717, "y":7.26742, "heading":-1.00384, "vx":1.04321, "vy":-0.58415, "omega":-2.03821, "ax":10.07579, "ay":-5.19108, "alpha":-12.84879, "fx":[132.03077,184.35351,198.9373,156.51261], "fy":[-148.90942,-75.13597,0.16986,-122.25583]}, + {"t":0.1483, "x":1.68277, "y":7.2422, "heading":-1.0794, "vx":1.41677, "vy":-0.7766, "omega":-2.51458, "ax":10.44052, "ay":-5.21908, "alpha":-8.36671, "fx":[147.54214,181.35341,196.18373,171.07448], "fy":[-133.31135,-81.76187,-32.12787,-100.79716]}, + {"t":0.18537, "x":1.74248, "y":7.20982, "heading":-1.17263, "vx":1.80384, "vy":-0.9701, "omega":-2.82477, "ax":10.68225, "ay":-5.26509, "alpha":0.37808, "fx":[179.20442,177.98341,176.91878,178.16528], "fy":[-85.44018,-87.93173,-90.08116,-87.61278]}, + {"t":0.22245, "x":1.81669, "y":7.17023, "heading":-1.27736, "vx":2.19988, "vy":-1.1653, "omega":-2.81075, "ax":9.53343, "ay":-4.07363, "alpha":19.31854, "fx":[193.19837,185.36173,79.88613,177.22428], "fy":[44.23824,-45.07551,-181.32797,-89.45685]}, + {"t":0.25952, "x":1.90481, "y":7.12423, "heading":-1.38156, "vx":2.55333, "vy":-1.31633, "omega":-2.09452, "ax":9.33447, "ay":-3.40646, "alpha":22.30618, "fx":[194.54259,186.90674,69.33351,171.62179], "fy":[35.04793,21.59237,-184.85881,-98.9178]}, + {"t":0.2966, "x":2.00589, "y":7.07309, "heading":-1.45922, "vx":2.8994, "vy":-1.44262, "omega":-1.26753, "ax":9.07912, "ay":-3.66143, "alpha":22.68317, "fx":[194.71942,180.39849,64.56132,165.69911], "fy":[22.53694,23.80829,-184.36845,-106.11365]}, + {"t":0.33367, "x":2.11962, "y":7.01709, "heading":-1.50621, "vx":3.23601, "vy":-1.57837, "omega":-0.42656, "ax":3.5871, "ay":-3.39727, "alpha":11.23752, "fx":[91.16888,33.47687,31.33337,83.20212], "fy":[-24.31183,-34.49804,-90.95894,-76.75449]}, + {"t":0.37075, "x":2.24206, "y":6.95623, "heading":-1.52202, "vx":3.369, "vy":-1.70432, "omega":-0.00994, "ax":-0.09909, "ay":-0.20308, "alpha":0.00852, "fx":[-1.63211,-1.67354,-1.67149,-1.63006], "fy":[-3.36357,-3.3656,-3.40702,-3.40499]}, + {"t":0.40782, "x":2.36689, "y":6.89291, "heading":-1.52239, "vx":3.36532, "vy":-1.71185, "omega":-0.00962, "ax":-0.01238, "ay":-0.02433, "alpha":0.00001, "fx":[-0.20628,-0.20633,-0.20632,-0.20628], "fy":[-0.4056,-0.4056,-0.40565,-0.40564]}, + {"t":0.4449, "x":2.49165, "y":6.82942, "heading":-1.52275, "vx":3.36487, "vy":-1.71275, "omega":-0.00962, "ax":-0.0015, "ay":-0.00293, "alpha":-0.00002, "fx":[-0.02498,-0.02487,-0.02487,-0.02498], "fy":[-0.04894,-0.04894,-0.04883,-0.04884]}, + {"t":0.48197, "x":2.6164, "y":6.76592, "heading":-1.52311, "vx":3.36481, "vy":-1.71286, "omega":-0.00962, "ax":-0.00018, "ay":-0.00035, "alpha":-0.00003, "fx":[-0.00306,-0.00294,-0.00295,-0.00307], "fy":[-0.00586,-0.00585,-0.00573,-0.00574]}, + {"t":0.51904, "x":2.74115, "y":6.70242, "heading":-1.52346, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":-0.00002, "ay":-0.00004, "alpha":-0.00003, "fx":[-0.00042,-0.00029,-0.00029,-0.00042], "fy":[-0.00066,-0.00065,-0.00052,-0.00053]}, + {"t":0.55612, "x":2.8659, "y":6.63891, "heading":-1.52382, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00009,0.00003,0.00002,-0.0001], "fy":[-0.00003,-0.00002,0.00011,0.0001]}, + {"t":0.59319, "x":2.99065, "y":6.57541, "heading":-1.52418, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00006,0.00007,0.00006,-0.00006], "fy":[0.00005,0.00005,0.00018,0.00018]}, + {"t":0.63027, "x":3.1154, "y":6.51191, "heading":-1.52453, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00008,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00018]}, + {"t":0.66734, "x":3.24015, "y":6.4484, "heading":-1.52489, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00007,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00019]}, + {"t":0.70442, "x":3.3649, "y":6.3849, "heading":-1.52525, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00008,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00019]}, + {"t":0.74149, "x":3.48964, "y":6.32139, "heading":-1.5256, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00007,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00018]}, + {"t":0.77857, "x":3.61439, "y":6.25789, "heading":-1.52596, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00007,0.00006,0.00006,-0.00007], "fy":[0.00003,0.00004,0.00017,0.00016]}, + {"t":0.81564, "x":3.73914, "y":6.19439, "heading":-1.52632, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":-0.00001, "ay":-0.00001, "alpha":-0.00003, "fx":[-0.00017,-0.00004,-0.00004,-0.00017], "fy":[-0.00016,-0.00016,-0.00003,-0.00004]}, + {"t":0.85272, "x":3.86389, "y":6.13088, "heading":-1.52667, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":-0.00006, "ay":-0.0001, "alpha":-0.00003, "fx":[-0.00099,-0.00086,-0.00087,-0.00099], "fy":[-0.00178,-0.00178,-0.00165,-0.00165]}, + {"t":0.88979, "x":3.98864, "y":6.06738, "heading":-1.52703, "vx":3.3648, "vy":-1.71288, "omega":-0.00963, "ax":-0.00046, "ay":-0.00091, "alpha":-0.00003, "fx":[-0.0078,-0.00767,-0.00768,-0.0078], "fy":[-0.01516,-0.01516,-0.01503,-0.01504]}, + {"t":0.92686, "x":4.11339, "y":6.00387, "heading":-1.52739, "vx":3.36478, "vy":-1.71291, "omega":-0.00963, "ax":-0.00385, "ay":-0.00755, "alpha":-0.00002, "fx":[-0.06415,-0.06406,-0.06407,-0.06415], "fy":[-0.12592,-0.12592,-0.12583,-0.12584]}, + {"t":0.96394, "x":4.23813, "y":5.94036, "heading":-1.52775, "vx":3.36464, "vy":-1.71319, "omega":-0.00963, "ax":-0.03186, "ay":-0.06254, "alpha":0.00005, "fx":[-0.53097,-0.53123,-0.53122,-0.53096], "fy":[-1.04246,-1.04247,-1.04273,-1.04272]}, + {"t":1.00101, "x":4.36285, "y":5.8768, "heading":-1.5281, "vx":3.36346, "vy":-1.71551, "omega":-0.00963, "ax":-0.26479, "ay":-0.51569, "alpha":0.00053, "fx":[-4.41273,-4.41529,-4.41518,-4.41262], "fy":[-8.59497,-8.59508,-8.59763,-8.59753]}, + {"t":1.03809, "x":4.48737, "y":5.81285, "heading":-1.52846, "vx":3.35364, "vy":-1.73463, "omega":-0.00961, "ax":-2.01103, "ay":-3.69607, "alpha":-0.00051, "fx":[-33.52399,-33.52134,-33.52171,-33.52437], "fy":[-61.61285,-61.61301,-61.61067,-61.61051]}, + {"t":1.07516, "x":4.61032, "y":5.746, "heading":-1.52882, "vx":3.27908, "vy":-1.87166, "omega":-0.00963, "ax":-5.85744, "ay":-8.27493, "alpha":-1.1148, "fx":[-98.94265,-92.38136,-96.33328,-102.9055], "fy":[-138.3716,-141.78979,-137.59299,-134.00212]}, + {"t":1.11224, "x":4.72787, "y":5.67092, "heading":-1.52917, "vx":3.06192, "vy":-2.17845, "omega":-0.05096, "ax":-10.42693, "ay":0.36677, "alpha":-19.6169, "fx":[-189.11827,-165.09406,-153.4343,-187.60115], "fy":[-51.48763,-98.4443,117.24991,57.13761]}, + {"t":1.14931, "x":4.83422, "y":5.59041, "heading":-1.53106, "vx":2.67535, "vy":-2.16485, "omega":-0.77825, "ax":-10.48588, "ay":1.14178, "alpha":-19.88874, "fx":[-193.29718,-176.60528,-143.57636,-185.6998], "fy":[-42.08819,-84.67181,134.4169,68.47478]}, + {"t":1.18639, "x":4.9262, "y":5.51093, "heading":-1.55992, "vx":2.28659, "vy":-2.12252, "omega":-1.51562, "ax":-10.61282, "ay":1.2559, "alpha":-19.15651, "fx":[-194.70988,-180.1442,-147.1425,-185.64579], "fy":[-38.05375,-80.27894,131.95741,70.11624]}, + {"t":1.22346, "x":5.00368, "y":5.4331, "heading":-1.61611, "vx":1.89312, "vy":-2.07596, "omega":-2.22584, "ax":-11.39194, "ay":1.98463, "alpha":-9.94836, "fx":[-198.48838,-197.46546,-175.68642,-187.95218], "fy":[-7.17876,-16.69596,91.90705,64.29875]}, + {"t":1.26054, "x":5.06604, "y":5.3575, "heading":-1.69863, "vx":1.47077, "vy":-2.00238, "omega":-2.59467, "ax":-11.19154, "ay":3.89078, "alpha":-2.37829, "fx":[-190.79274,-189.67193,-181.68297,-184.08287], "fy":[52.61493,55.89106,78.15099,72.77256]}, + {"t":1.27813, "x":5.09019, "y":5.32287, "heading":-1.74428, "vx":1.27385, "vy":-1.93392, "omega":-2.63652, "ax":-10.29682, "ay":5.8743, "alpha":2.43215, "fx":[-163.68164,-168.67869,-178.70207,-175.50978], "fy":[111.25837,103.80925,85.29608,91.32307]}, + {"t":1.29573, "x":5.11101, "y":5.28975, "heading":-1.79068, "vx":1.09267, "vy":-1.83055, "omega":-2.59372, "ax":-9.19016, "ay":7.30164, "alpha":6.39724, "fx":[-124.07758,-147.5387,-175.84171,-165.3243], "fy":[154.34043,132.52638,91.46063,108.53174]}, + {"t":1.31332, "x":5.12881, "y":5.25867, "heading":-1.83631, "vx":0.93096, "vy":-1.70208, "omega":-2.48116, "ax":-8.11825, "ay":8.25317, "alpha":9.34806, "fx":[-84.40426,-129.60757,-173.05574,-154.24171], "fy":[179.32351,150.35755,96.94479,123.67956]}, + {"t":1.33092, "x":5.14394, "y":5.23, "heading":-1.87997, "vx":0.78812, "vy":-1.55686, "omega":-2.31667, "ax":-7.18582, "ay":8.88008, "alpha":11.40978, "fx":[-50.92795,-114.83168,-170.34044,-143.03671], "fy":[191.70246,162.0923,101.88539,136.42658]}, + {"t":1.34851, "x":5.15669, "y":5.20398, "heading":-1.92073, "vx":0.66168, "vy":-1.40061, "omega":-2.11591, "ax":-6.40407, "ay":9.30373, "alpha":12.84074, "fx":[-24.3627,-102.59797,-167.71362,-132.33696], "fy":[196.98452,170.22025,106.3464,146.80368]}, + {"t":1.36611, "x":5.16734, "y":5.18077, "heading":-1.95797, "vx":0.54899, "vy":-1.2369, "omega":-1.88997, "ax":-5.753, "ay":9.59978, "alpha":13.85563, "fx":[-3.52439,-92.36264,-165.20207,-122.50995], "fy":[198.56263,176.0739,110.36048,155.09759]}, + {"t":1.38371, "x":5.17611, "y":5.1605, "heading":-1.99122, "vx":0.44776, "vy":-1.06799, "omega":-1.64617, "ax":-5.20829, "ay":9.81335, "alpha":14.59532, "fx":[12.93293,-83.7272,-162.83473,-113.64954], "fy":[198.2604,180.40937,113.94738,161.71802]}, + {"t":1.4013, "x":5.18318, "y":5.14322, "heading":-2.02019, "vx":0.35612, "vy":-0.89531, "omega":-1.38935, "ax":-4.74815, "ay":9.97233, "alpha":15.14607, "fx":[26.07817,-76.41104,-160.63952,-105.62513], "fy":[197.0353,183.68232,117.12164,167.09658]}, + {"t":1.4189, "x":5.18871, "y":5.12901, "heading":-2.04463, "vx":0.27257, "vy":-0.71984, "omega":-1.12285, "ax":-4.35411, "ay":10.09484, "alpha":15.56049, "fx":[36.686,-70.21628,-158.64187,-98.15167], "fy":[195.39904,186.1826,119.89551,171.62744]}, + {"t":1.43649, "x":5.19284, "y":5.11791, "heading":-2.06439, "vx":0.19596, "vy":-0.54222, "omega":-0.84905, "ax":-4.0105, "ay":10.19309, "alpha":15.87201, "fx":[45.30645,-65.00126,-156.86423,-90.85337], "fy":[193.63345,188.10323,122.27992,175.63879]}, + {"t":1.45409, "x":5.19566, "y":5.10995, "heading":-2.07933, "vx":0.12539, "vy":-0.36286, "omega":-0.56977, "ax":-3.70387, "ay":10.27535, "alpha":16.10418, "fx":[52.33227,-60.66233,-155.32612,-83.31048], "fy":[191.89837,189.57754,124.28454,179.38041]}, + {"t":1.47168, "x":5.1973, "y":5.10515, "heading":-2.08936, "vx":0.06022, "vy":-0.18206, "omega":-0.28641, "ax":-3.42252, "ay":10.34702, "alpha":16.27726, "fx":[58.04852,-57.12184,-154.04432,-75.08916], "fy":[190.28618,190.70016,125.91743,183.01591]}, + {"t":1.48928, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj index 936fb870..1c9ae16e 100644 --- a/src/main/deploy/choreo/fetchToK.traj +++ b/src/main/deploy/choreo/fetchToK.traj @@ -3,7 +3,7 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "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,36 +26,36 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.19692], + "waypoints":[0.0,1.18221], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.08578, "ay":-6.38853, "alpha":-0.62757, "fx":[135.22249,138.83326,139.88579,136.20595], "fy":[-111.57882,-107.05515,-105.66702,-110.36719]}, - {"t":0.04433, "x":1.57936, "y":7.40914, "heading":-0.94268, "vx":0.35845, "vy":-0.28321, "omega":-0.02782, "ax":8.08485, "ay":-6.38779, "alpha":-0.62158, "fx":[135.22889,138.80527,139.84682,136.20267], "fy":[-111.53869,-107.05798,-105.68367,-110.33748]}, - {"t":0.08866, "x":1.60319, "y":7.39031, "heading":-0.94391, "vx":0.71685, "vy":-0.56638, "omega":-0.05538, "ax":8.08364, "ay":-6.38684, "alpha":-0.6139, "fx":[135.23535,138.76658,139.79828,136.20129], "fy":[-111.48918,-107.0651,-105.70297,-110.29578]}, - {"t":0.13299, "x":1.64291, "y":7.35893, "heading":-0.94636, "vx":1.0752, "vy":-0.84951, "omega":-0.08259, "ax":8.08203, "ay":-6.38557, "alpha":-0.6037, "fx":[135.2428,138.71335,139.73461,136.2011], "fy":[-111.4245,-107.07664,-105.7273,-110.2381]}, - {"t":0.17732, "x":1.69852, "y":7.315, "heading":-0.95003, "vx":1.43348, "vy":-1.13258, "omega":-0.10935, "ax":8.07977, "ay":-6.38378, "alpha":-0.58948, "fx":[135.25282,138.63869,139.64581,136.20083], "fy":[-111.33429,-107.09283,-105.76084,-110.15735]}, - {"t":0.22165, "x":1.77, "y":7.25852, "heading":-0.95487, "vx":1.79166, "vy":-1.41558, "omega":-0.13548, "ax":8.07637, "ay":-6.38111, "alpha":-0.56832, "fx":[135.26839,138.52885,139.5121,136.19783], "fy":[-111.19803,-107.11402,-105.81169,-110.03939]}, - {"t":0.26598, "x":1.85737, "y":7.18949, "heading":-0.96088, "vx":2.14968, "vy":-1.69845, "omega":-0.16068, "ax":8.0707, "ay":-6.37663, "alpha":-0.53351, "fx":[135.29572,138.35187,139.28798,136.18564], "fy":[-110.96852,-107.14072,-105.89777,-109.85169]}, - {"t":0.31031, "x":1.96059, "y":7.10793, "heading":-0.968, "vx":2.50746, "vy":-1.98113, "omega":-0.18433, "ax":8.05931, "ay":-6.36765, "alpha":-0.46572, "fx":[135.34983,138.01286,138.84027,136.14319], "fy":[-110.50779,-107.17241,-106.06816,-109.49901]}, - {"t":0.35464, "x":2.07967, "y":7.01385, "heading":-0.97617, "vx":2.86473, "vy":-2.26341, "omega":-0.20497, "ax":8.0249, "ay":-6.3405, "alpha":-0.27781, "fx":[135.46577,137.05066,137.54244,135.94591], "fy":[-109.16822,-107.1829,-106.51534,-108.53415]}, - {"t":0.39897, "x":2.21455, "y":6.90729, "heading":-0.98526, "vx":3.22048, "vy":-2.54449, "omega":-0.21729, "ax":3.3354, "ay":-2.6351, "alpha":4.89611, "fx":[63.22878,40.81151,51.3561,71.54043], "fy":[-26.76748,-44.45733,-62.15853,-45.9056]}, - {"t":0.4433, "x":2.36059, "y":6.7919, "heading":-0.99489, "vx":3.36834, "vy":-2.6613, "omega":-0.00024, "ax":0.00064, "ay":-0.00043, "alpha":0.00053, "fx":[0.01131,0.00915,0.01055,0.01272], "fy":[-0.00545,-0.00686,-0.00903,-0.00762]}, - {"t":0.48763, "x":2.50991, "y":6.67392, "heading":-0.9949, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.00009,0.00008,0.00008,0.00009], "fy":[0.0001,0.0001,0.0001,0.0001]}, - {"t":0.53196, "x":2.65923, "y":6.55594, "heading":-0.99491, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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":0.57629, "x":2.80855, "y":6.43797, "heading":-0.99492, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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.62062, "x":2.95787, "y":6.31999, "heading":-0.99493, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "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":0.66495, "x":3.10719, "y":6.20201, "heading":-0.99494, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":-0.00001, "ay":-0.00001, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[-0.00011,-0.00011,-0.00011,-0.00011]}, - {"t":0.70928, "x":3.25651, "y":6.08404, "heading":-0.99495, "vx":3.36837, "vy":-2.66132, "omega":-0.00022, "ax":-0.00064, "ay":0.00042, "alpha":-0.00053, "fx":[-0.01132,-0.00915,-0.01056,-0.01273], "fy":[0.00544,0.00685,0.00901,0.00761]}, - {"t":0.75361, "x":3.40583, "y":5.96606, "heading":-0.99496, "vx":3.36834, "vy":-2.6613, "omega":-0.00024, "ax":-3.3354, "ay":2.6351, "alpha":-4.89603, "fx":[-63.37516,-40.87032,-51.20753,-71.48407], "fy":[26.78713,44.27431,62.16009,46.06744]}, - {"t":0.79795, "x":3.55187, "y":5.85067, "heading":-0.99497, "vx":3.22048, "vy":-2.54449, "omega":-0.21729, "ax":-8.0249, "ay":6.3405, "alpha":0.27714, "fx":[-135.45782,-137.03004,-137.54985,-135.96708], "fy":[109.17761,107.20941,106.50624,108.50743]}, - {"t":0.84228, "x":3.68675, "y":5.7441, "heading":-1.00461, "vx":2.86473, "vy":-2.26341, "omega":-0.205, "ax":-8.05931, "ay":6.36765, "alpha":0.46556, "fx":[-135.31595,-137.9495,-138.87125,-136.20945], "fy":[110.54858,107.25429,106.02835,109.41619]}, - {"t":0.88661, "x":3.80583, "y":5.65002, "heading":-1.01369, "vx":2.50746, "vy":-1.98113, "omega":-0.18436, "ax":-8.0707, "ay":6.37663, "alpha":0.5335, "fx":[-135.23855,-138.2477,-139.33963,-136.29531], "fy":[111.03743,107.27548,105.83063,109.71517]}, - {"t":0.93094, "x":3.90906, "y":5.56846, "heading":-1.02187, "vx":2.14968, "vy":-1.69845, "omega":-0.16071, "ax":-8.07637, "ay":6.38111, "alpha":0.56839, "fx":[-135.19083,-138.38824,-139.5817,-136.34637], "fy":[111.29155,107.29601,105.72071,109.85486]}, - {"t":0.97527, "x":3.99642, "y":5.49944, "heading":-1.02899, "vx":1.79166, "vy":-1.41558, "omega":-0.13551, "ax":-8.07977, "ay":6.38378, "alpha":0.58959, "fx":[-135.15797,-138.4668,-139.73055,-136.38279], "fy":[111.44868,107.31536,105.64969,109.93157]}, - {"t":1.0196, "x":4.0679, "y":5.44296, "heading":-1.035, "vx":1.43348, "vy":-1.13258, "omega":-0.10938, "ax":-8.08203, "ay":6.38556, "alpha":0.60383, "fx":[-135.13387,-138.51571,-139.83162,-136.41061], "fy":[111.55588,107.33254,105.59976,109.97835]}, - {"t":1.06393, "x":4.12351, "y":5.39903, "heading":-1.03985, "vx":1.0752, "vy":-0.84951, "omega":-0.08261, "ax":-8.08364, "ay":6.38684, "alpha":0.61405, "fx":[-135.11562,-138.54897,-139.90466,-136.4322], "fy":[111.63359,107.34689,105.56289,110.00964]}, - {"t":1.10826, "x":4.16323, "y":5.36764, "heading":-1.04351, "vx":0.71685, "vy":-0.56638, "omega":-0.05539, "ax":-8.08484, "ay":6.38779, "alpha":0.62174, "fx":[-135.10168,-138.5736,-139.95962,-136.44869], "fy":[111.69212,107.358,105.53494,110.03274]}, - {"t":1.15259, "x":4.18706, "y":5.34881, "heading":-1.04596, "vx":0.35845, "vy":-0.28321, "omega":-0.02783, "ax":-8.08578, "ay":6.38853, "alpha":0.62774, "fx":[-135.09115,-138.59355,-140.00208,-136.46065], "fy":[111.73724,107.36558,105.51355,110.05179]}, - {"t":1.19692, "x":4.19501, "y":5.34254, "heading":-1.0472, "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.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.56034, "ay":-7.15659, "alpha":-1.40829, "fx":[154.37156,162.12086,164.50001,156.47267], "fy":[-125.88713,-115.74236,-112.3134,-123.2447]}, + {"t":0.04379, "x":1.58916, "y":7.29314, "heading":-0.9225, "vx":0.41861, "vy":-0.31336, "omega":-0.06166, "ax":9.55917, "ay":-7.15582, "alpha":-1.35316, "fx":[154.54533,161.99642,164.27658,156.56876], "fy":[-125.62332,-115.86251,-112.58174,-123.06857]}, + {"t":0.08757, "x":1.61666, "y":7.27256, "heading":-0.9252, "vx":0.83716, "vy":-0.62668, "omega":-0.12091, "ax":9.5575, "ay":-7.15471, "alpha":-1.27582, "fx":[154.78137,161.8092,163.96912,156.71578], "fy":[-125.26116,-116.04793,-112.9478,-122.80554]}, + {"t":0.13136, "x":1.66247, "y":7.23826, "heading":-0.93049, "vx":1.25564, "vy":-0.93995, "omega":-0.17677, "ax":9.55491, "ay":-7.153, "alpha":-1.15948, "fx":[155.13486,161.52318,163.50748,156.93731], "fy":[-124.71494,-116.331,-113.49365,-122.40849]}, + {"t":0.17514, "x":1.72661, "y":7.19025, "heading":-0.93824, "vx":1.67401, "vy":-1.25315, "omega":-0.22754, "ax":9.55038, "ay":-7.14999, "alpha":-0.96481, "fx":[155.734,161.05181,162.72724,157.288], "fy":[-123.78218,-116.78927,-114.40851,-121.7674]}, + {"t":0.21893, "x":1.80906, "y":7.12852, "heading":-0.9482, "vx":2.09218, "vy":-1.56622, "omega":-0.26979, "ax":9.54053, "ay":-7.14339, "alpha":-0.57288, "fx":[156.96151,160.12407,161.13108,157.92776], "fy":[-121.84056,-117.66276,-116.24903,-120.55496]}, + {"t":0.26271, "x":1.90982, "y":7.0531, "heading":-0.96001, "vx":2.50992, "vy":-1.87899, "omega":-0.29487, "ax":9.50451, "ay":-7.11882, "alpha":0.61971, "fx":[160.7023,157.25566,156.19134,159.5931], "fy":[-115.59533,-120.21933,-121.66558,-117.18925]}, + {"t":0.3065, "x":2.02883, "y":6.964, "heading":-0.97292, "vx":2.92608, "vy":-2.1907, "omega":-0.26774, "ax":2.25073, "ay":-1.67377, "alpha":6.10391, "fx":[42.93959,16.90104,33.04672,57.18731], "fy":[-6.55303,-25.1991,-48.88199,-30.96965]}, + {"t":0.35028, "x":2.1591, "y":6.86648, "heading":-0.98465, "vx":3.02463, "vy":-2.26398, "omega":-0.00047, "ax":0.00069, "ay":0.00046, "alpha":0.00035, "fx":[0.01178,0.01035,0.0113,0.01272], "fy":[0.0088,0.00785,0.00643,0.00737]}, + {"t":0.39407, "x":2.29154, "y":6.76735, "heading":-0.98467, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.00004, "ay":0.00005, "alpha":0.0, "fx":[0.00063,0.00063,0.00063,0.00063], "fy":[0.00085,0.00085,0.00085,0.00085]}, + {"t":0.43786, "x":2.42398, "y":6.66822, "heading":-0.98469, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[0.00007,0.00007,0.00007,0.00007]}, + {"t":0.48164, "x":2.55641, "y":6.56909, "heading":-0.98471, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00001,0.00001,0.00001,0.00001]}, + {"t":0.52543, "x":2.68885, "y":6.46996, "heading":-0.98473, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "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.56921, "x":2.82129, "y":6.37083, "heading":-0.98475, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "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.613, "x":2.95372, "y":6.2717, "heading":-0.98477, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "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.65678, "x":3.08616, "y":6.17257, "heading":-0.98479, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]}, + {"t":0.70057, "x":3.2186, "y":6.07345, "heading":-0.98481, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00005,-0.00005,-0.00005,-0.00005], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]}, + {"t":0.74436, "x":3.35103, "y":5.97432, "heading":-0.98483, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":-0.00004, "ay":-0.00005, "alpha":0.0, "fx":[-0.00064,-0.00064,-0.00064,-0.00064], "fy":[-0.00085,-0.00085,-0.00085,-0.00085]}, + {"t":0.78814, "x":3.48347, "y":5.87519, "heading":-0.98485, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":-0.00069, "ay":-0.00046, "alpha":-0.00035, "fx":[-0.01182,-0.01039,-0.01134,-0.01276], "fy":[-0.00885,-0.0079,-0.00648,-0.00742]}, + {"t":0.83193, "x":3.6159, "y":5.77606, "heading":-0.98487, "vx":3.02463, "vy":-2.26398, "omega":-0.00047, "ax":-2.25107, "ay":1.67397, "alpha":-6.08946, "fx":[-43.17201,-17.01484,-32.82442,-57.08584], "fy":[6.65494,24.94442,48.81109,31.2064]}, + {"t":0.87571, "x":3.74618, "y":5.67853, "heading":-0.98489, "vx":2.92606, "vy":-2.19069, "omega":-0.2671, "ax":-9.50436, "ay":7.11873, "alpha":-0.64968, "fx":[-160.83794,-157.25633,-156.04955,-159.58852], "fy":[115.40551,120.21451,121.8463,117.19707]}, + {"t":0.9195, "x":3.86519, "y":5.58944, "heading":-0.99658, "vx":2.50991, "vy":-1.87899, "omega":-0.29555, "ax":-9.54054, "ay":7.1434, "alpha":0.5658, "fx":[-156.93459,-160.01254,-161.15289,-158.04512], "fy":[121.87394,117.81433,116.21968,120.40055]}, + {"t":0.96328, "x":3.96594, "y":5.51401, "heading":-1.00952, "vx":2.09217, "vy":-1.56621, "omega":-0.27078, "ax":-9.55037, "ay":7.14999, "alpha":0.96485, "fx":[-155.59756,-160.80845,-162.84205,-157.55252], "fy":[123.95205,117.12475,114.24668,121.42398]}, + {"t":1.00707, "x":4.04839, "y":5.45229, "heading":-1.02138, "vx":1.674, "vy":-1.25314, "omega":-0.22853, "ax":-9.55488, "ay":7.15298, "alpha":1.16287, "fx":[-154.91056,-161.15807,-163.69169,-157.34067], "fy":[124.99175,116.8371,113.22975,121.88847]}, + {"t":1.05085, "x":4.11253, "y":5.40427, "heading":-1.03139, "vx":1.25563, "vy":-0.93995, "omega":-0.17761, "ax":-9.55745, "ay":7.15468, "alpha":1.28111, "fx":[-154.48785,-161.34282,-164.20574,-157.23616], "fy":[125.62129,116.69634,112.60556,122.13742]}, + {"t":1.09464, "x":4.15835, "y":5.36998, "heading":-1.03916, "vx":0.83715, "vy":-0.62667, "omega":-0.12152, "ax":-9.55911, "ay":7.15578, "alpha":1.35967, "fx":[-154.20122,-161.45441,-164.55019,-157.17756], "fy":[126.04385,116.61746,112.18346,122.28898]}, + {"t":1.13843, "x":4.18584, "y":5.3494, "heading":-1.04448, "vx":0.4186, "vy":-0.31335, "omega":-0.06199, "ax":-9.56028, "ay":7.15655, "alpha":1.41565, "fx":[-153.99588,-161.53194,-164.79564,-157.13732], "fy":[126.34491,116.56365,111.88107,122.39511]}, + {"t":1.18221, "x":4.19501, "y":5.34254, "heading":-1.0472, "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/fetchToL.traj b/src/main/deploy/choreo/fetchToL.traj index 1f7afbd9..c2277900 100644 --- a/src/main/deploy/choreo/fetchToL.traj +++ b/src/main/deploy/choreo/fetchToL.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":1.57141418, "y":7.41542078, "heading":-0.9426766239853251, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "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":"fetch.x", "val":1.57141418}, "y":{"exp":"fetch.y", "val":7.41542078}, "heading":{"exp":"fetch.heading", "val":-0.9426766239853251}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "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,36 +26,35 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.17196], + "waypoints":[0.0,1.1599], "samples":[ - {"t":0.0, "x":1.57141, "y":7.41542, "heading":-0.94268, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.44665, "ay":-7.12277, "alpha":-0.64117, "fx":[124.19086,128.37629,129.19751,124.89671], "fy":[-123.73426,-119.38771,-118.48977,-123.01315]}, - {"t":0.04341, "x":1.57843, "y":7.40871, "heading":-0.94268, "vx":0.32323, "vy":-0.30917, "omega":-0.02783, "ax":7.44576, "ay":-7.12191, "alpha":-0.63703, "fx":[124.19199,128.35036,129.16529,124.89306], "fy":[-123.70329,-119.38485,-118.4929,-122.98583]}, - {"t":0.08681, "x":1.59947, "y":7.38858, "heading":-0.94388, "vx":0.64642, "vy":-0.6183, "omega":-0.05548, "ax":7.44461, "ay":-7.12082, "alpha":-0.63173, "fx":[124.1912,128.31417,129.12598,124.89135], "fy":[-123.6657,-119.38422,-118.49463,-122.94769]}, - {"t":0.13022, "x":1.63454, "y":7.35504, "heading":-0.94629, "vx":0.96956, "vy":-0.92739, "omega":-0.0829, "ax":7.44308, "ay":-7.11935, "alpha":-0.6247, "fx":[124.18864,128.2642,129.07499,124.89086], "fy":[-123.61708,-119.38523,-118.4954,-122.89503]}, - {"t":0.17362, "x":1.68364, "y":7.30808, "heading":-0.94989, "vx":1.29263, "vy":-1.23641, "omega":-0.11002, "ax":7.44094, "ay":-7.11731, "alpha":-0.61495, "fx":[124.18451,128.19411,129.00417,124.89028], "fy":[-123.54953,-119.38676,-118.4959,-122.82125]}, - {"t":0.21703, "x":1.74676, "y":7.2477, "heading":-0.95467, "vx":1.61561, "vy":-1.54534, "omega":-0.13671, "ax":7.43773, "ay":-7.11424, "alpha":-0.6005, "fx":[124.17904,128.09131,128.89744,124.88687], "fy":[-123.44744,-119.38649,-118.49732,-122.71324]}, - {"t":0.26043, "x":1.82389, "y":7.17392, "heading":-0.9606, "vx":1.93845, "vy":-1.85414, "omega":-0.16278, "ax":7.43238, "ay":-7.10912, "alpha":-0.57692, "fx":[124.17209,127.92678,128.71799,124.87391], "fy":[-123.27508,-119.37865,-118.50166,-122.54098]}, - {"t":0.30384, "x":1.91503, "y":7.08675, "heading":-0.96767, "vx":2.26106, "vy":-2.16272, "omega":-0.18782, "ax":7.4217, "ay":-7.0989, "alpha":-0.53164, "fx":[124.16017,127.61532,128.35907,124.82945], "fy":[-122.92848,-119.34414,-118.51156,-122.21692]}, - {"t":0.34725, "x":2.02017, "y":6.98619, "heading":-0.97582, "vx":2.5832, "vy":-2.47085, "omega":-0.21089, "ax":7.38987, "ay":-7.06845, "alpha":-0.40941, "fx":[124.09628,126.75098,127.32433,124.6266], "fy":[-121.92144,-119.16946,-118.50759,-121.33066]}, - {"t":0.39065, "x":2.13925, "y":6.87228, "heading":-0.98497, "vx":2.90397, "vy":-2.77766, "omega":-0.22866, "ax":4.56547, "ay":-4.36718, "alpha":5.26197, "fx":[88.78635,60.45819,68.53937,92.84528], "fy":[-53.93424,-78.35616,-92.65493,-72.19277]}, - {"t":0.43406, "x":2.2696, "y":6.7476, "heading":-0.9949, "vx":3.10213, "vy":-2.96722, "omega":-0.00026, "ax":0.0013, "ay":-0.00101, "alpha":0.001, "fx":[0.02284,0.01877,0.02141,0.02548], "fy":[-0.01386,-0.01649,-0.02056,-0.01793]}, - {"t":0.47746, "x":2.40426, "y":6.6188, "heading":-0.99491, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.00001, "ay":0.00002, "alpha":0.0, "fx":[0.00026,0.00025,0.00026,0.00026], "fy":[0.00026,0.00026,0.00026,0.00026]}, - {"t":0.52087, "x":2.53891, "y":6.49, "heading":-0.99492, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[0.00003,0.00003,0.00003,0.00003]}, - {"t":0.56428, "x":2.67356, "y":6.36121, "heading":-0.99493, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "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.60768, "x":2.80822, "y":6.23241, "heading":-0.99494, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00003,-0.00003,-0.00003], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, - {"t":0.65109, "x":2.94287, "y":6.10361, "heading":-0.99495, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":-0.00002, "ay":-0.00002, "alpha":0.0, "fx":[-0.00026,-0.00026,-0.00026,-0.00026], "fy":[-0.00027,-0.00027,-0.00027,-0.00027]}, - {"t":0.69449, "x":3.07752, "y":5.97482, "heading":-0.99496, "vx":3.10219, "vy":-2.96727, "omega":-0.00022, "ax":-0.0013, "ay":0.00101, "alpha":-0.001, "fx":[-0.0229,-0.01884,-0.02148,-0.02555], "fy":[0.01379,0.01642,0.0205,0.01786]}, - {"t":0.7379, "x":3.21217, "y":5.84602, "heading":-0.99497, "vx":3.10213, "vy":-2.96722, "omega":-0.00026, "ax":-4.56546, "ay":4.36719, "alpha":-5.26169, "fx":[-88.93608,-60.56625,-68.3736,-92.75297], "fy":[53.91803,78.15309,92.69306,72.37418]}, - {"t":0.7813, "x":3.34252, "y":5.72134, "heading":-0.99498, "vx":2.90397, "vy":-2.77766, "omega":-0.22865, "ax":-7.38987, "ay":7.06845, "alpha":0.4093, "fx":[-124.07616,-126.72012,-127.34339,-124.65854], "fy":[121.94135,119.20262,118.4877,121.29747]}, - {"t":0.82471, "x":3.46161, "y":5.60743, "heading":-1.0049, "vx":2.5832, "vy":-2.47085, "omega":-0.21089, "ax":-7.4217, "ay":7.0989, "alpha":0.53161, "fx":[-124.10816,-127.53787,-128.40758,-124.91041], "fy":[122.9803,119.42734,118.45976,122.1337]}, - {"t":0.86812, "x":3.56675, "y":5.50687, "heading":-1.01406, "vx":2.26106, "vy":-2.16272, "omega":-0.18781, "ax":-7.43238, "ay":7.10912, "alpha":0.57691, "fx":[-124.09086,-127.8061,-128.79326,-125.00054], "fy":[123.35612,119.50829,118.42065,122.41131]}, - {"t":0.91152, "x":3.65789, "y":5.41969, "heading":-1.02221, "vx":1.93845, "vy":-1.85414, "omega":-0.16277, "ax":-7.43773, "ay":7.11424, "alpha":0.60049, "fx":[-124.07225,-127.9325,-128.99606,-125.05385], "fy":[123.55407,119.55709,118.39075,122.54258]}, - {"t":0.95493, "x":3.73502, "y":5.34592, "heading":-1.02927, "vx":1.61561, "vy":-1.54534, "omega":-0.13671, "ax":-7.44094, "ay":7.11731, "alpha":0.61493, "fx":[-124.0561,-128.00285,-129.12248,-125.09164], "fy":[123.67779,119.59223,118.36775,122.61568]}, - {"t":0.99833, "x":3.79814, "y":5.28554, "heading":-1.03521, "vx":1.29263, "vy":-1.23641, "omega":-0.11001, "ax":-7.44308, "ay":7.11935, "alpha":0.62468, "fx":[-124.04272,-128.04643,-129.2092,-125.12034], "fy":[123.76286,119.61918,118.34977,122.66095]}, - {"t":1.04174, "x":3.84723, "y":5.23858, "heading":-1.03998, "vx":0.96956, "vy":-0.92739, "omega":-0.0829, "ax":-7.44461, "ay":7.12082, "alpha":0.63171, "fx":[-124.03195,-128.07599,-129.27225,-125.14249], "fy":[123.82482,119.64009,118.33571,122.69164]}, - {"t":1.08515, "x":3.88231, "y":5.20504, "heading":-1.04358, "vx":0.64642, "vy":-0.6183, "omega":-0.05548, "ax":-7.44576, "ay":7.12191, "alpha":0.63701, "fx":[-124.02364,-128.09799,-129.31977,-125.15929], "fy":[123.87152,119.65595,118.32492,122.7145]}, - {"t":1.12855, "x":3.90335, "y":5.18491, "heading":-1.04599, "vx":0.32323, "vy":-0.30917, "omega":-0.02783, "ax":-7.44665, "ay":7.12277, "alpha":0.64115, "fx":[-124.01763,-128.11603,-129.35631,-125.17137], "fy":[123.90737,119.66728,118.31697,122.7333]}, - {"t":1.17196, "x":3.91036, "y":5.1782, "heading":-1.0472, "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.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.53374, "ay":-8.35012, "alpha":-1.68607, "fx":[135.95079,146.9041,148.93542,137.22294], "fy":[-145.59464,-134.53775,-132.26291,-144.37478]}, + {"t":0.04461, "x":1.58849, "y":7.29169, "heading":-0.9225, "vx":0.3807, "vy":-0.37251, "omega":-0.07522, "ax":8.53329, "ay":-8.34983, "alpha":-1.56474, "fx":[136.3828,146.57425,148.43588,137.59029], "fy":[-145.14667,-134.85064,-132.77418,-143.97916]}, + {"t":0.08922, "x":1.61397, "y":7.26676, "heading":-0.92586, "vx":0.76139, "vy":-0.74501, "omega":-0.14502, "ax":8.53252, "ay":-8.34929, "alpha":-1.39371, "fx":[136.97942,146.08674,147.74458,138.12139], "fy":[-144.52186,-135.31288,-133.47452,-143.40568]}, + {"t":0.13384, "x":1.65642, "y":7.22522, "heading":-0.93233, "vx":1.14204, "vy":-1.11749, "omega":-0.2072, "ax":8.53107, "ay":-8.3482, "alpha":-1.13457, "fx":[137.88949,145.33793,146.69701,138.91083], "fy":[-143.5584,-136.01647,-134.52283,-142.54448]}, + {"t":0.17845, "x":1.71586, "y":7.16706, "heading":-0.94157, "vx":1.52262, "vy":-1.48992, "omega":-0.25781, "ax":8.52781, "ay":-8.34558, "alpha":-0.69561, "fx":[139.46752,144.06852,144.90378,140.17814], "fy":[-141.85967,-137.18813,-136.2837,-141.13602]}, + {"t":0.22306, "x":1.79228, "y":7.09229, "heading":-0.95307, "vx":1.90306, "vy":-1.86223, "omega":-0.28885, "ax":8.51793, "ay":-8.33711, "alpha":0.21062, "fx":[142.82103,141.40719,141.16407,142.56676], "fy":[-138.1208,-139.5661,-139.82202,-138.3933]}, + {"t":0.26767, "x":1.88565, "y":7.00091, "heading":-0.96596, "vx":2.28306, "vy":-2.23416, "omega":-0.27945, "ax":8.45547, "ay":-8.28041, "alpha":3.20082, "fx":[154.01859,131.62117,129.10416,149.05055], "fy":[-124.10285,-147.57134,-150.09079,-130.3569]}, + {"t":0.31228, "x":1.99592, "y":6.893, "heading":-0.97842, "vx":2.66028, "vy":-2.60356, "omega":-0.13666, "ax":0.90305, "ay":-0.8665, "alpha":3.04911, "fx":[17.23345,4.71998,12.98054,25.2796], "fy":[-4.08942,-12.56409,-24.75408,-16.36879]}, + {"t":0.35689, "x":2.11549, "y":6.77599, "heading":-0.98452, "vx":2.70056, "vy":-2.64222, "omega":-0.00063, "ax":0.00075, "ay":0.00061, "alpha":0.00016, "fx":[0.01269,0.01205,0.01248,0.01312], "fy":[0.01073,0.01031,0.00967,0.01009]}, + {"t":0.40151, "x":2.23597, "y":6.65812, "heading":-0.98455, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.00005, "ay":0.00005, "alpha":0.0, "fx":[0.00088,0.00088,0.00088,0.00088], "fy":[0.0009,0.0009,0.0009,0.0009]}, + {"t":0.44612, "x":2.35645, "y":6.54025, "heading":-0.98458, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.00007,0.00007,0.00007,0.00007]}, + {"t":0.49073, "x":2.47693, "y":6.42237, "heading":-0.9846, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "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":0.53534, "x":2.59741, "y":6.3045, "heading":-0.98463, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "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.57995, "x":2.71788, "y":6.18663, "heading":-0.98466, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "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.62456, "x":2.83836, "y":6.06876, "heading":-0.98469, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "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":0.66918, "x":2.95884, "y":5.95088, "heading":-0.98472, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]}, + {"t":0.71379, "x":3.07932, "y":5.83301, "heading":-0.98474, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":-0.00005, "ay":-0.00005, "alpha":0.0, "fx":[-0.0009,-0.0009,-0.0009,-0.0009], "fy":[-0.00092,-0.00092,-0.00092,-0.00092]}, + {"t":0.7584, "x":3.1998, "y":5.71514, "heading":-0.98477, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":-0.00077, "ay":-0.00063, "alpha":-0.00016, "fx":[-0.01293,-0.01229,-0.01271,-0.01335], "fy":[-0.01097,-0.01055,-0.00991,-0.01033]}, + {"t":0.80301, "x":3.32027, "y":5.59727, "heading":-0.9848, "vx":2.70056, "vy":-2.64222, "omega":-0.00063, "ax":-0.90421, "ay":0.86727, "alpha":-3.04617, "fx":[-17.31655,-4.76284,-12.93644,-25.27491], "fy":[4.12528,12.51263,24.74551,16.44474]}, + {"t":0.84762, "x":3.43985, "y":5.48026, "heading":-0.98483, "vx":2.66022, "vy":-2.60353, "omega":-0.13653, "ax":-8.45473, "ay":8.27986, "alpha":-3.23837, "fx":[-154.29948,-131.75602,-128.7738,-148.91578], "fy":[123.75539,147.44423,150.37196,130.51388]}, + {"t":0.89223, "x":3.55011, "y":5.37235, "heading":-0.99092, "vx":2.28304, "vy":-2.23415, "omega":-0.28099, "ax":-8.51789, "ay":8.33712, "alpha":-0.21713, "fx":[-142.8679,-141.42141,-141.1157,-142.55135], "fy":[138.07219,139.55111,139.87049,138.40927]}, + {"t":0.93685, "x":3.64349, "y":5.28097, "heading":-1.00345, "vx":1.90305, "vy":-1.86222, "omega":-0.29068, "ax":-8.52778, "ay":8.34559, "alpha":0.69834, "fx":[-139.33681,-143.90723,-145.02547,-140.34646], "fy":[141.98724,137.35776,136.15491,140.96793]}, + {"t":0.98146, "x":3.7199, "y":5.2062, "heading":-1.01642, "vx":1.52261, "vy":-1.48991, "omega":-0.25953, "ax":-8.53101, "ay":8.34818, "alpha":1.14154, "fx":[-137.59133,-144.98783,-146.96436,-139.28799], "fy":[143.84301,136.39046,134.23204,142.17486]}, + {"t":1.02607, "x":3.77934, "y":5.14804, "heading":-1.028, "vx":1.14202, "vy":-1.11748, "omega":-0.2086, "ax":-8.53244, "ay":8.34924, "alpha":1.40296, "fx":[-136.53538,-145.56573,-148.13228,-138.69326], "fy":[144.94008,135.87421,133.04577,142.8514]}, + {"t":1.07068, "x":3.82179, "y":5.1065, "heading":-1.03731, "vx":0.76138, "vy":-0.74501, "omega":-0.14601, "ax":-8.53318, "ay":8.34976, "alpha":1.57536, "fx":[-135.82428,-145.9173,-148.9143,-138.32045], "fy":[145.66806,135.56226,132.23916,143.27651]}, + {"t":1.11529, "x":3.84727, "y":5.08157, "heading":-1.04382, "vx":0.3807, "vy":-0.37251, "omega":-0.07573, "ax":-8.53362, "ay":8.35004, "alpha":1.69761, "fx":[-135.31584,-146.15667,-149.47163,-138.06111], "fy":[146.18362,135.35036,131.6584,143.57218]}, + {"t":1.1599, "x":3.85576, "y":5.07326, "heading":-1.0472, "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/startDeepToA.traj b/src/main/deploy/choreo/startDeepToA.traj index 1a6c10b4..3d631628 100644 --- a/src/main/deploy/choreo/startDeepToA.traj +++ b/src/main/deploy/choreo/startDeepToA.traj @@ -3,12 +3,12 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":4.5624847412109375, "y":6.526900768280029, "heading":-1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.4915122985839844, "y":6.0326056480407715, "heading":-1.1801894971536726, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.5624847412109375, "y":6.526900768280029, "heading":-1.5707963267948966, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4915122985839844, "y":6.0326056480407715, "heading":-1.1801894971536726, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":2.7500696182250977, "y":5.027539253234863, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2019875, "y":4.4292225106, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.2654999999999994, "y":4.3309, "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}, @@ -17,12 +17,12 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"4.5624847412109375 m", "val":4.5624847412109375}, "y":{"exp":"6.526900768280029 m", "val":6.526900768280029}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"3.4915122985839844 m", "val":3.4915122985839844}, "y":{"exp":"6.0326056480407715 m", "val":6.0326056480407715}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.5624847412109375 m", "val":4.5624847412109375}, "y":{"exp":"6.526900768280029 m", "val":6.526900768280029}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4915122985839844 m", "val":3.4915122985839844}, "y":{"exp":"6.0326056480407715 m", "val":6.0326056480407715}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"2.7500696182250977 m", "val":2.7500696182250977}, "y":{"exp":"5.027539253234863 m", "val":5.027539253234863}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"A.x", "val":3.2019875}, "y":{"exp":"A.y", "val":4.4292225106}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "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}, @@ -34,111 +34,106 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.59235,0.88055,1.15702,1.58136,2.00844], + "waypoints":[0.0,0.64964,0.98523,1.30887,1.72044,2.14431], "samples":[ - {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.47751, "ay":-0.76283, "alpha":34.00404, "fx":[-42.33677,-77.67458,-168.48115,-152.22984], "fy":[-169.92267,156.61175,48.32264,-86.91351]}, - {"t":0.02692, "x":7.09854, "y":7.25675, "heading":3.14159, "vx":-0.17441, "vy":-0.02054, "omega":0.91555, "ax":-6.78067, "ay":-0.85566, "alpha":32.43865, "fx":[-49.20726,-90.30722,-169.06842,-152.76633], "fy":[-168.01509,149.58716,46.13005,-85.92042]}, - {"t":0.05385, "x":7.09139, "y":7.25589, "heading":-3.11694, "vx":-0.35697, "vy":-0.04358, "omega":1.78896, "ax":-7.11559, "ay":-1.05709, "alpha":30.5905, "fx":[-55.75541,-105.73531,-170.2472,-152.399], "fy":[-165.88735,139.03258,41.44156,-86.5099]}, - {"t":0.08077, "x":7.0792, "y":7.25433, "heading":-3.06877, "vx":-0.54856, "vy":-0.07204, "omega":2.6126, "ax":-7.48296, "ay":-1.35255, "alpha":28.37289, "fx":[-63.80432,-122.18296,-171.78823,-151.3562], "fy":[-162.85662,124.76428,34.30783,-88.24175]}, - {"t":0.1077, "x":7.06171, "y":7.2519, "heading":-2.99843, "vx":-0.75004, "vy":-0.10846, "omega":3.37654, "ax":-7.90817, "ay":-1.71255, "alpha":25.50162, "fx":[-76.25217,-138.42686,-173.37388,-150.00988], "fy":[-157.25428,106.39739,24.73818,-90.40123]}, - {"t":0.13462, "x":7.03865, "y":7.24836, "heading":-2.90752, "vx":-0.96296, "vy":-0.15457, "omega":4.06317, "ax":-8.44315, "ay":-2.07664, "alpha":21.40907, "fx":[-97.1058,-153.65537,-174.59651,-149.10456], "fy":[-145.06375,82.81114,12.68374,-91.72329]}, - {"t":0.16155, "x":7.00966, "y":7.24345, "heading":-2.79812, "vx":-1.1903, "vy":-0.21048, "omega":4.6396, "ax":-9.10619, "ay":-2.33119, "alpha":15.47279, "fx":[-127.74682,-166.66364,-174.9266,-150.2378], "fy":[-118.63458,51.65475,-2.04908,-89.58235]}, - {"t":0.18847, "x":6.97431, "y":7.23694, "heading":-2.6732, "vx":-1.43548, "vy":-0.27325, "omega":5.05621, "ax":-9.71879, "ay":-2.4173, "alpha":7.93845, "fx":[-156.8174,-174.16256,-173.53669,-156.73902], "fy":[-75.89542,8.76109,-20.25854,-77.07724]}, - {"t":0.2154, "x":6.93214, "y":7.2287, "heading":-2.53706, "vx":-1.69716, "vy":-0.33833, "omega":5.26995, "ax":-9.93668, "ay":-2.43951, "alpha":-1.6122, "fx":[-170.22357,-166.62367,-167.91304,-171.32011], "fy":[-37.40061,-50.98141,-46.14497,-31.45429]}, - {"t":0.24232, "x":6.88284, "y":7.21871, "heading":-2.39517, "vx":-1.9647, "vy":-0.40402, "omega":5.22654, "ax":-8.42043, "ay":-2.70964, "alpha":-18.58613, "fx":[-173.1259,-126.18291,-118.90006,-154.70781], "fy":[-21.14235,-120.04682,-122.30264,79.13093]}, - {"t":0.26925, "x":6.82689, "y":7.20685, "heading":-2.25444, "vx":-2.19142, "vy":-0.47697, "omega":4.72611, "ax":-8.04395, "ay":-2.70736, "alpha":-19.93338, "fx":[-172.18687,-111.54481,-115.69392,-147.87564], "fy":[-27.28163,-133.47219,-114.73472,91.28317]}, - {"t":0.29617, "x":6.76497, "y":7.19303, "heading":-2.12719, "vx":-2.408, "vy":-0.54987, "omega":4.18941, "ax":-8.49138, "ay":-1.04779, "alpha":-18.85782, "fx":[-170.40526,-102.79592,-153.9391,-150.60361], "fy":[-36.12199,-139.9295,18.21864,86.54222]}, - {"t":0.3231, "x":6.69706, "y":7.17784, "heading":-2.01439, "vx":-2.63663, "vy":-0.57808, "omega":3.68166, "ax":-8.67216, "ay":-0.95344, "alpha":-18.42764, "fx":[-168.13395,-105.27294,-157.93083,-158.7061], "fy":[-44.61885,-137.50593,47.19041,70.06324]}, - {"t":0.35002, "x":6.62293, "y":7.16193, "heading":-1.91526, "vx":-2.87013, "vy":-0.60375, "omega":3.1855, "ax":-8.68379, "ay":-1.15836, "alpha":-18.4197, "fx":[-165.515,-104.67924,-157.38465,-163.25614], "fy":[-52.48164,-137.22297,53.15445,57.7367]}, - {"t":0.37695, "x":6.5425, "y":7.14526, "heading":-1.8295, "vx":-3.10394, "vy":-0.63494, "omega":2.68955, "ax":-8.59439, "ay":-1.40038, "alpha":-18.77454, "fx":[-162.38457,-100.81668,-155.70262,-165.84835], "fy":[-60.20089,-139.08197,55.9211,48.08171]}, - {"t":0.40387, "x":6.45581, "y":7.12765, "heading":-1.75708, "vx":-3.33534, "vy":-0.67264, "omega":2.18405, "ax":-8.41438, "ay":-1.67332, "alpha":-19.36878, "fx":[-158.52668,-93.78843,-152.99491,-167.19507], "fy":[-67.96866,-142.44614,56.703,39.86129]}, - {"t":0.4308, "x":6.36296, "y":7.10893, "heading":-1.69827, "vx":-3.56189, "vy":-0.7177, "omega":1.66255, "ax":-8.13209, "ay":-1.99967, "alpha":-19.99036, "fx":[-153.55746,-83.68541,-148.67096,-167.38404], "fy":[-75.7925,-146.20564,53.92984,32.01287]}, - {"t":0.45772, "x":6.26411, "y":7.08889, "heading":-1.65351, "vx":-3.78085, "vy":-0.77154, "omega":1.12431, "ax":-7.66335, "ay":-2.46093, "alpha":-20.21129, "fx":[-146.24476,-70.11094,-139.56929,-165.4806], "fy":[-83.67606,-148.35565,41.76302,22.82983]}, - {"t":0.48465, "x":6.15953, "y":7.06722, "heading":-1.62324, "vx":-3.98718, "vy":-0.8378, "omega":0.58013, "ax":-6.33151, "ay":-3.54729, "alpha":-18.25084, "fx":[-128.01922,-48.92433,-100.65809,-153.1872], "fy":[-92.31698,-140.97746,-10.48368,2.42437]}, - {"t":0.51157, "x":6.04988, "y":7.04338, "heading":-1.60762, "vx":-4.15766, "vy":-0.93331, "omega":0.08873, "ax":0.12945, "ay":-3.72683, "alpha":-1.97357, "fx":[-3.36249,7.28017,7.97963,-3.08978], "fy":[-67.4646,-67.67571,-59.33004,-59.09905]}, - {"t":0.5385, "x":5.93798, "y":7.0169, "heading":-1.60523, "vx":-4.15417, "vy":-1.03365, "omega":0.03559, "ax":0.89634, "ay":-3.52382, "alpha":-0.07443, "fx":[15.02839,15.42921,15.46482,15.06347], "fy":[-60.10617,-60.09576,-59.77229,-59.78265]}, - {"t":0.56542, "x":5.82646, "y":6.98779, "heading":-1.60427, "vx":-4.13004, "vy":-1.12853, "omega":0.03358, "ax":1.60578, "ay":-5.82908, "alpha":-0.45863, "fx":[25.63915,28.49907,29.00658,26.11041], "fy":[-100.11274,-99.77631,-98.18577,-98.52934]}, - {"t":0.59235, "x":5.71584, "y":6.95529, "heading":-1.60337, "vx":-4.08681, "vy":-1.28548, "omega":0.02124, "ax":2.48153, "ay":-4.3992, "alpha":2.45781, "fx":[50.46575,36.8313,34.21863,47.32462], "fy":[-68.49787,-70.60143,-81.16443,-79.05303]}, - {"t":0.60607, "x":5.65999, "y":6.93723, "heading":-1.60308, "vx":-4.05275, "vy":-1.34585, "omega":0.05497, "ax":1.31689, "ay":-3.53702, "alpha":0.26563, "fx":[23.19121,21.76768,21.61171,23.02888], "fy":[-59.54328,-59.61848,-60.78473,-60.70861]}, - {"t":0.61979, "x":5.60449, "y":6.91843, "heading":-1.60232, "vx":-4.03468, "vy":-1.3944, "omega":0.05861, "ax":0.88418, "ay":-2.52208, "alpha":-0.01334, "fx":[15.00312,15.07135,15.07623,15.00799], "fy":[-42.93108,-42.9303,-42.8687,-42.86948]}, - {"t":0.63352, "x":5.5492, "y":6.89906, "heading":-1.60152, "vx":-4.02254, "vy":-1.42901, "omega":0.05843, "ax":0.60548, "ay":-1.71255, "alpha":-0.02945, "fx":[10.22199,10.36871,10.37603,10.22928], "fy":[-29.19921,-29.20071,-29.06072,-29.05921]}, - {"t":0.64724, "x":5.49405, "y":6.87928, "heading":-1.60072, "vx":-4.01423, "vy":-1.45251, "omega":0.05802, "ax":0.40774, "ay":-1.13694, "alpha":-0.02013, "fx":[6.88421,6.98319,6.98702,6.88802], "fy":[-19.38651,-19.38855,-19.29158,-19.28953]}, - {"t":0.66097, "x":5.439, "y":6.85924, "heading":-1.59992, "vx":-4.00864, "vy":-1.46811, "omega":0.05775, "ax":0.27144, "ay":-0.74865, "alpha":-0.01227, "fx":[4.58615,4.64614,4.64812,4.58813], "fy":[-12.76329,-12.76479,-12.70533,-12.70383]}, - {"t":0.67469, "x":5.38401, "y":6.83902, "heading":-1.59913, "vx":-4.00491, "vy":-1.47839, "omega":0.05758, "ax":0.18035, "ay":-0.49361, "alpha":-0.0072, "fx":[3.04968,3.08479,3.08584,3.05073], "fy":[-8.41322,-8.41415,-8.37917,-8.37824]}, - {"t":0.68841, "x":5.32907, "y":6.81869, "heading":-1.59834, "vx":-4.00244, "vy":-1.48516, "omega":0.05748, "ax":0.121, "ay":-0.32931, "alpha":-0.00404, "fx":[2.04797,2.06767,2.06822,2.04852], "fy":[-5.61108,-5.61161,-5.59194,-5.59141]}, - {"t":0.70214, "x":5.27415, "y":6.79827, "heading":-1.59755, "vx":-4.00078, "vy":-1.48968, "omega":0.05743, "ax":0.08354, "ay":-0.22637, "alpha":-0.00211, "fx":[1.41562,1.42592,1.4262,1.4159], "fy":[-3.85552,-3.85579,-3.8455,-3.84523]}, - {"t":0.71586, "x":5.21925, "y":6.77781, "heading":-1.59676, "vx":-3.99963, "vy":-1.49279, "omega":0.0574, "ax":0.06142, "ay":-0.16585, "alpha":-0.001, "fx":[1.04231,1.04717,1.04729,1.04244], "fy":[-2.82348,-2.82361,-2.81876,-2.81863]}, - {"t":0.72959, "x":5.16437, "y":6.75731, "heading":-1.59597, "vx":-3.99879, "vy":-1.49507, "omega":0.05738, "ax":0.0507, "ay":-0.1365, "alpha":-0.00046, "fx":[0.86118,0.8634,0.86345,0.86124], "fy":[-2.32292,-2.32298,-2.32076,-2.32071]}, - {"t":0.74331, "x":5.10949, "y":6.73677, "heading":-1.59518, "vx":-3.99809, "vy":-1.49694, "omega":0.05738, "ax":0.04941, "ay":-0.13283, "alpha":-0.00038, "fx":[0.83956,0.84141,0.84145,0.83961], "fy":[-2.26022,-2.26027,-2.25842,-2.25838]}, - {"t":0.75703, "x":5.05463, "y":6.71622, "heading":-1.5944, "vx":-3.99741, "vy":-1.49876, "omega":0.05737, "ax":0.05738, "ay":-0.15413, "alpha":-0.00075, "fx":[0.97413,0.97777,0.97786,0.97422], "fy":[-2.62343,-2.62352,-2.61988,-2.61979]}, - {"t":0.77076, "x":4.99977, "y":6.69564, "heading":-1.59361, "vx":-3.99662, "vy":-1.50088, "omega":0.05736, "ax":0.07613, "ay":-0.20436, "alpha":-0.00162, "fx":[1.29085,1.29875,1.29893,1.29103], "fy":[-3.4799,-3.48007,-3.47218,-3.472]}, - {"t":0.78448, "x":4.94493, "y":6.67502, "heading":-1.59282, "vx":-3.99558, "vy":-1.50368, "omega":0.05734, "ax":0.10924, "ay":-0.2928, "alpha":-0.00314, "fx":[1.8503,1.86561,1.86596,1.85064], "fy":[-4.98791,-4.98824,-4.97295,-4.97262]}, - {"t":0.79821, "x":4.89011, "y":6.65435, "heading":-1.59203, "vx":-3.99408, "vy":-1.5077, "omega":0.0573, "ax":0.16304, "ay":-0.43563, "alpha":-0.00554, "fx":[2.75938,2.78641,2.78702,2.75999], "fy":[-7.42307,-7.42361,-7.39666,-7.39612]}, - {"t":0.81193, "x":4.83531, "y":6.63362, "heading":-1.59125, "vx":-3.99184, "vy":-1.51368, "omega":0.05722, "ax":0.24771, "ay":-0.65827, "alpha":-0.00914, "fx":[4.19069,4.23536,4.23641,4.19174], "fy":[-11.21871,-11.21948,-11.17513,-11.17435]}, - {"t":0.82565, "x":4.78055, "y":6.61279, "heading":-1.59046, "vx":-3.98844, "vy":-1.52271, "omega":0.05709, "ax":0.37887, "ay":-0.99799, "alpha":-0.01433, "fx":[6.40832,6.47864,6.48052,6.4102], "fy":[-17.00972,-17.01059,-16.94137,-16.94051]}, - {"t":0.83938, "x":4.72584, "y":6.59179, "heading":-1.58968, "vx":-3.98324, "vy":-1.53641, "omega":0.0569, "ax":0.57851, "ay":-1.50386, "alpha":-0.02172, "fx":[9.78462,9.89228,9.89605,9.78838], "fy":[-25.63214,-25.63237,-25.52847,-25.52824]}, - {"t":0.8531, "x":4.67123, "y":6.57057, "heading":-1.5889, "vx":-3.9753, "vy":-1.55705, "omega":0.0566, "ax":0.87172, "ay":-2.22592, "alpha":-0.03501, "fx":[14.73449,14.91155,14.92101,14.74388], "fy":[-37.94576,-37.9425,-37.77862,-37.78187]}, - {"t":0.86683, "x":4.61676, "y":6.54899, "heading":-1.58812, "vx":-3.96334, "vy":-1.5876, "omega":0.05612, "ax":1.26693, "ay":-3.18157, "alpha":-0.11195, "fx":[21.23064,21.81821,21.87021,21.28163], "fy":[-54.38502,-54.35232,-53.85017,-53.88268]}, - {"t":0.88055, "x":4.56248, "y":6.5269, "heading":-1.58735, "vx":-3.94595, "vy":-1.63126, "omega":0.05458, "ax":1.366, "ay":-3.05788, "alpha":0.16937, "fx":[23.7163,22.83349,22.75522,23.63585], "fy":[-51.60387,-51.65566,-52.42377,-52.37146]}, - {"t":0.89371, "x":4.51065, "y":6.50516, "heading":-1.58663, "vx":-3.92797, "vy":-1.67152, "omega":0.05681, "ax":0.94213, "ay":-2.19187, "alpha":-0.01102, "fx":[15.99609,16.05171,16.05468,15.99905], "fy":[-37.30958,-37.30832,-37.25658,-37.25784]}, - {"t":0.90688, "x":4.45902, "y":6.48296, "heading":-1.58589, "vx":-3.91557, "vy":-1.70038, "omega":0.05667, "ax":0.6556, "ay":-1.51402, "alpha":-0.02158, "fx":[11.09629,11.20317,11.20675,11.09985], "fy":[-25.80493,-25.80453,-25.70126,-25.70166]}, - {"t":0.92005, "x":4.40753, "y":6.46045, "heading":-1.58514, "vx":-3.90693, "vy":-1.72031, "omega":0.05638, "ax":0.4513, "ay":-1.0309, "alpha":-0.01509, "fx":[7.63866,7.71271,7.71442,7.64036], "fy":[-17.57151,-17.57191,-17.49903,-17.49863]}, - {"t":0.93321, "x":4.35613, "y":6.43771, "heading":-1.5844, "vx":-3.90099, "vy":-1.73388, "omega":0.05619, "ax":0.30967, "ay":-0.7014, "alpha":-0.00953, "fx":[5.24377,5.29033,5.29116,5.24459], "fy":[-11.9535,-11.95394,-11.90772,-11.90728]}, - {"t":0.94638, "x":4.3048, "y":6.41482, "heading":-1.58366, "vx":-3.89692, "vy":-1.74311, "omega":0.05606, "ax":0.21461, "ay":-0.48312, "alpha":-0.00589, "fx":[3.63588,3.66464,3.66506,3.63631], "fy":[-8.23195,-8.23226,-8.20361,-8.2033]}, - {"t":0.95954, "x":4.25351, "y":6.39183, "heading":-1.58292, "vx":-3.89409, "vy":-1.74948, "omega":0.05598, "ax":0.15329, "ay":-0.34353, "alpha":-0.00364, "fx":[2.5984,2.61616,2.61639,2.59863], "fy":[-5.85211,-5.85231,-5.83458,-5.83438]}, - {"t":0.97271, "x":4.20226, "y":6.36877, "heading":-1.58218, "vx":-3.89207, "vy":-1.754, "omega":0.05593, "ax":0.11665, "ay":-0.26053, "alpha":-0.00233, "fx":[1.97847,1.98985,1.98998,1.97861], "fy":[-4.43712,-4.43724,-4.42588,-4.42575]}, - {"t":0.98587, "x":4.15103, "y":6.34565, "heading":-1.58145, "vx":-3.89054, "vy":-1.75743, "omega":0.0559, "ax":0.09906, "ay":-0.22064, "alpha":-0.0017, "fx":[1.68081,1.68912,1.68921,1.6809], "fy":[-3.75716,-3.75724,-3.74894,-3.74886]}, - {"t":0.99904, "x":4.09982, "y":6.3225, "heading":-1.58071, "vx":-3.88923, "vy":-1.76033, "omega":0.05588, "ax":0.09778, "ay":-0.21732, "alpha":-0.00163, "fx":[1.6592,1.66713,1.66722,1.65928], "fy":[-3.70047,-3.70055,-3.69262,-3.69254]}, - {"t":1.0122, "x":4.04862, "y":6.2993, "heading":-1.57997, "vx":-3.88795, "vy":-1.76319, "omega":0.05586, "ax":0.11269, "ay":-0.24998, "alpha":-0.00208, "fx":[1.9117,1.92183,1.92193,1.9118], "fy":[-4.25707,-4.25716,-4.24704,-4.24695]}, - {"t":1.02537, "x":3.99744, "y":6.27607, "heading":-1.57924, "vx":-3.88646, "vy":-1.76648, "omega":0.05583, "ax":0.14634, "ay":-0.32389, "alpha":-0.0031, "fx":[2.48153,2.49666,2.49681,2.48167], "fy":[-5.51675,-5.51686,-5.50175,-5.50164]}, - {"t":1.03853, "x":3.94629, "y":6.25278, "heading":-1.5785, "vx":-3.88454, "vy":-1.77075, "omega":0.05579, "ax":0.20438, "ay":-0.45088, "alpha":-0.00483, "fx":[3.46456,3.48813,3.48835,3.46479], "fy":[-7.68108,-7.68122,-7.65773,-7.65759]}, - {"t":1.0517, "x":3.89517, "y":6.22943, "heading":-1.57777, "vx":-3.88184, "vy":-1.77669, "omega":0.05573, "ax":0.29645, "ay":-0.65077, "alpha":-0.00743, "fx":[5.02411,5.06042,5.0608,5.02449], "fy":[-11.08737,-11.08749,-11.05141,-11.05129]}, - {"t":1.06487, "x":3.84408, "y":6.20598, "heading":-1.57704, "vx":-3.87794, "vy":-1.78525, "omega":0.05563, "ax":0.43732, "ay":-0.95288, "alpha":-0.01107, "fx":[7.41124,7.4655,7.46626,7.412], "fy":[-16.23511,-16.23502,-16.18148,-16.18157]}, - {"t":1.07803, "x":3.79307, "y":6.1824, "heading":-1.5763, "vx":-3.87218, "vy":-1.7978, "omega":0.05548, "ax":0.64785, "ay":-1.39581, "alpha":-0.01553, "fx":[10.98062,11.05732,11.05904,10.98232], "fy":[-23.78005,-23.77917,-23.70462,-23.70549]}, - {"t":1.0912, "x":3.74214, "y":6.15861, "heading":-1.57557, "vx":-3.86365, "vy":-1.81617, "omega":0.05528, "ax":0.95378, "ay":-2.01941, "alpha":-0.01801, "fx":[16.17657,16.26685,16.27046,16.18016], "fy":[-34.3935,-34.39073,-34.30563,-34.3084]}, - {"t":1.10436, "x":3.69136, "y":6.13452, "heading":-1.57484, "vx":-3.8511, "vy":-1.84276, "omega":0.05504, "ax":1.38244, "ay":-2.83748, "alpha":0.00377, "fx":[23.52531,23.50587,23.50443,23.52387], "fy":[-48.25549,-48.25677,-48.2741,-48.27282]}, - {"t":1.11753, "x":3.64078, "y":6.11001, "heading":-1.57412, "vx":-3.8329, "vy":-1.88012, "omega":0.05509, "ax":1.98807, "ay":-3.77466, "alpha":0.21233, "fx":[34.45909,33.32341,33.1758,34.30751], "fy":[-63.66742,-63.80993,-64.74473,-64.60178]}, - {"t":1.13069, "x":3.59049, "y":6.08493, "heading":-1.57339, "vx":-3.80672, "vy":-1.92981, "omega":0.05789, "ax":3.07363, "ay":-4.50118, "alpha":1.57641, "fx":[57.56215,49.0147,47.09499,55.45446], "fy":[-72.12077,-74.10445,-80.99982,-79.03009]}, - {"t":1.14386, "x":3.54064, "y":6.05914, "heading":-1.57263, "vx":-3.76626, "vy":-1.98907, "omega":0.07864, "ax":5.2881, "ay":-3.97262, "alpha":6.85095, "fx":[110.8079,82.28532,68.1651,98.5382], "fy":[-43.26507,-54.59454,-93.23684,-79.19627]}, - {"t":1.15702, "x":3.49151, "y":6.03261, "heading":-1.5716, "vx":-3.69664, "vy":-2.04137, "omega":0.16884, "ax":7.42079, "ay":-3.88507, "alpha":10.5922, "fx":[152.22553,130.48717,90.58159,131.6078], "fy":[-22.18218,-38.19342,-118.47939,-85.48086]}, - {"t":1.17723, "x":3.41833, "y":5.99056, "heading":-1.56819, "vx":-3.54669, "vy":-2.11988, "omega":0.38287, "ax":8.56626, "ay":-1.82992, "alpha":14.75973, "fx":[164.95963,150.95055,114.35206,152.57607], "fy":[18.36641,37.06898,-112.72962,-67.21133]}, - {"t":1.19744, "x":3.34841, "y":5.94735, "heading":-1.56045, "vx":-3.3736, "vy":-2.15685, "omega":0.68111, "ax":8.8683, "ay":-1.24159, "alpha":15.58618, "fx":[167.20003,152.87631,124.66637,158.64617], "fy":[28.55327,56.97911,-109.04225,-60.96674]}, - {"t":1.21764, "x":3.28206, "y":5.90352, "heading":-1.54669, "vx":-3.1944, "vy":-2.18194, "omega":0.99605, "ax":9.0228, "ay":-1.05974, "alpha":15.63609, "fx":[168.27625,154.81601,129.49288,161.31589], "fy":[32.03413,61.78196,-107.62183,-58.29772]}, - {"t":1.23785, "x":3.21935, "y":5.85921, "heading":-1.52656, "vx":-3.01208, "vy":-2.20336, "omega":1.312, "ax":9.13165, "ay":-1.03293, "alpha":15.37171, "fx":[169.02688,157.41578,132.05213,162.81248], "fy":[33.16749,60.58116,-107.20015,-56.8278]}, - {"t":1.25806, "x":3.16035, "y":5.81448, "heading":-1.50005, "vx":-2.82756, "vy":-2.22423, "omega":1.62261, "ax":9.2226, "ay":-1.08131, "alpha":14.91253, "fx":[169.62665,160.43966,133.60274,163.82614], "fy":[33.27533,56.00284,-107.12064,-55.72842]}, - {"t":1.27826, "x":3.1051, "y":5.76932, "heading":-1.46726, "vx":-2.64121, "vy":-2.24608, "omega":1.92394, "ax":9.30667, "ay":-1.17248, "alpha":14.29225, "fx":[170.1394,163.6457,134.78878,164.64104], "fy":[32.84632,48.96792,-106.96248,-54.62627]}, - {"t":1.29847, "x":3.05363, "y":5.72369, "heading":-1.42839, "vx":-2.45315, "vy":-2.26977, "omega":2.21273, "ax":9.38951, "ay":-1.28969, "alpha":13.51755, "fx":[170.60467,166.78795,136.05616,165.40292], "fy":[32.04594,39.85178,-106.35087,-53.29565]}, - {"t":1.31868, "x":3.00598, "y":5.67756, "heading":-1.38367, "vx":-2.26342, "vy":-2.29583, "omega":2.48588, "ax":9.47455, "ay":-1.41977, "alpha":12.58985, "fx":[171.05912,169.57616,137.79977,166.20251], "fy":[30.86343,28.95085,-104.86372,-51.55039]}, - {"t":1.33888, "x":2.96218, "y":5.63088, "heading":-1.33344, "vx":-2.07198, "vy":-2.32452, "omega":2.74027, "ax":9.56378, "ay":-1.54676, "alpha":11.51945, "fx":[171.54175,171.68808,140.37925,167.09951], "fy":[29.15977,16.82824,-102.01218,-49.21576]}, - {"t":1.35909, "x":2.92226, "y":5.5836, "heading":-1.27807, "vx":-1.87873, "vy":-2.35577, "omega":2.97304, "ax":9.65776, "ay":-1.64962, "alpha":10.33416, "fx":[172.08601,172.89308,144.00845,168.11567], "fy":[26.71423,4.55505,-97.3406,-46.16666]}, - {"t":1.37929, "x":2.88627, "y":5.53566, "heading":-1.218, "vx":-1.68358, "vy":-2.3891, "omega":3.18186, "ax":9.7554, "ay":-1.70748, "alpha":9.07284, "fx":[172.69874,173.23814,148.59092,169.21816], "fy":[23.31688,-6.44854,-90.63449,-42.40862]}, - {"t":1.3995, "x":2.85424, "y":5.48704, "heading":-1.1537, "vx":-1.48645, "vy":-2.42361, "omega":3.36519, "ax":9.85405, "ay":-1.70902, "alpha":7.76329, "fx":[173.34249,173.0646,153.72242,170.32878], "fy":[18.86477,-15.0158,-82.02237,-38.10654]}, - {"t":1.41971, "x":2.82622, "y":5.43771, "heading":-1.08571, "vx":-1.28734, "vy":-2.45814, "omega":3.52206, "ax":9.95159, "ay":-1.6519, "alpha":6.39113, "fx":[173.94467,172.79255,158.98449,171.37326], "fy":[13.3223,-20.60596,-71.65336,-33.4567]}, - {"t":1.43991, "x":2.80224, "y":5.38771, "heading":-1.01454, "vx":-1.08625, "vy":-2.49152, "omega":3.6512, "ax":10.05677, "ay":-1.50655, "alpha":4.74091, "fx":[174.42841,172.80429,164.63599,172.38229], "fy":[6.0663,-22.61482,-57.85576,-28.09997]}, - {"t":1.46012, "x":2.78234, "y":5.33705, "heading":-0.94076, "vx":-0.88304, "vy":-2.52196, "omega":3.74699, "ax":10.18178, "ay":-1.15524, "alpha":2.11636, "fx":[174.53005,173.48615,171.24824,173.49189], "fy":[-5.27037,-19.00794,-34.065,-20.25823]}, - {"t":1.48033, "x":2.76658, "y":5.28586, "heading":-0.86505, "vx":-0.6773, "vy":-2.5453, "omega":3.78976, "ax":10.24287, "ay":-0.68988, "alpha":-0.98777, "fx":[173.70782,174.31799,174.61906,174.26825], "fy":[-18.49084,-11.82182,-4.9763,-11.64979]}, - {"t":1.50053, "x":2.75498, "y":5.23429, "heading":-0.78847, "vx":-0.47033, "vy":-2.55924, "omega":3.7698, "ax":10.21525, "ay":-0.26071, "alpha":-3.73095, "fx":[172.19421,174.80859,173.46981,174.56133], "fy":[-29.9009,-3.95325,21.24914,-5.13352]}, - {"t":1.52074, "x":2.74756, "y":5.18252, "heading":-0.71229, "vx":-0.26392, "vy":-2.56451, "omega":3.69441, "ax":10.13303, "ay":0.10333, "alpha":-5.98397, "fx":[170.54169,174.90442,169.38647,174.60695], "fy":[-38.57179,3.95858,43.29107,-1.64727]}, - {"t":1.54095, "x":2.7443, "y":5.13072, "heading":-0.63764, "vx":-0.05916, "vy":-2.56243, "omega":3.57349, "ax":10.02419, "ay":0.39996, "alpha":-7.82656, "fx":[169.14259,174.62664,163.67824,174.58689], "fy":[-44.58444,11.72933,61.59529,-1.52716]}, - {"t":1.56115, "x":2.74515, "y":5.07902, "heading":-0.56544, "vx":0.14339, "vy":-2.55434, "omega":3.41535, "ax":9.90365, "ay":0.63108, "alpha":-9.39024, "fx":[168.16804,174.01202,157.14315,174.50973], "fy":[-48.35404,19.25717,76.8497,-4.81467]}, - {"t":1.58136, "x":2.75007, "y":5.02754, "heading":-0.49642, "vx":0.34351, "vy":-2.54159, "omega":3.2256, "ax":9.89468, "ay":1.02134, "alpha":-9.17397, "fx":[170.60893,172.99601,154.8717,174.74598], "fy":[-39.25597,27.31495,81.5163,-0.0845]}, - {"t":1.60648, "x":2.76182, "y":4.96401, "heading":-0.41539, "vx":0.59208, "vy":-2.51593, "omega":2.99513, "ax":9.74156, "ay":1.66748, "alpha":-9.9329, "fx":[172.18892,170.40886,145.79744,174.40944], "fy":[-31.28655,40.22682,96.68022,7.83289]}, - {"t":1.6316, "x":2.77977, "y":4.90133, "heading":-0.34015, "vx":0.83681, "vy":-2.47404, "omega":2.7456, "ax":9.46681, "ay":2.54138, "alpha":-10.7859, "fx":[173.69141,165.89343,131.68555,172.84067], "fy":[-20.76005,55.82326,115.02939,22.82002]}, - {"t":1.65673, "x":2.80378, "y":4.83998, "heading":-0.27117, "vx":1.07464, "vy":-2.4102, "omega":2.47463, "ax":8.94573, "ay":3.76799, "alpha":-11.63142, "fx":[174.70579,158.08279,109.72045,166.14819], "fy":[-6.10589,74.97325,136.01464,51.48802]}, - {"t":1.68185, "x":2.8336, "y":4.78062, "heading":-0.209, "vx":1.29937, "vy":-2.31554, "omega":2.18243, "ax":7.83727, "ay":5.52882, "alpha":-12.55915, "fx":[173.93061,144.53641,77.09326,137.67832], "fy":[15.71997,98.4326,156.72862,105.29352]}, - {"t":1.70697, "x":2.86872, "y":4.72419, "heading":-0.15417, "vx":1.49626, "vy":-2.17664, "omega":1.86692, "ax":5.44309, "ay":7.55188, "alpha":-15.32148, "fx":[166.85603,121.36227,34.22056,47.90257], "fy":[50.64048,125.7609,171.22741,166.19251]}, - {"t":1.73209, "x":2.90802, "y":4.6719, "heading":-0.10727, "vx":1.633, "vy":-1.98692, "omega":1.48201, "ax":2.31577, "ay":8.80534, "alpha":-17.80236, "fx":[137.56429,84.38661,-11.92635,-52.4624], "fy":[106.53796,152.94539,174.21932,165.40243]}, - {"t":1.75721, "x":2.94978, "y":4.62476, "heading":-0.07004, "vx":1.69118, "vy":-1.76571, "omega":1.03478, "ax":-0.89845, "ay":9.4738, "alpha":-14.07151, "fx":[56.76384,34.42211,-52.0675,-100.24799], "fy":[164.25395,171.2159,166.75281,142.36376]}, - {"t":1.78234, "x":2.99198, "y":4.58339, "heading":-0.04405, "vx":1.66861, "vy":-1.52771, "omega":0.68127, "ax":-3.80517, "ay":9.16469, "alpha":-9.2268, "fx":[-37.44799,-18.16711,-82.08911,-121.19505], "fy":[170.00407,173.72858,154.30221,125.52039]}, - {"t":1.80746, "x":3.0327, "y":4.5479, "heading":-0.02693, "vx":1.57302, "vy":-1.29748, "omega":0.44947, "ax":-5.68615, "ay":8.35769, "alpha":-6.35859, "fx":[-90.25355,-61.64882,-103.12472,-131.85215], "fy":[149.29117,163.52506,141.21868,114.61273]}, - {"t":1.83258, "x":3.07042, "y":4.51794, "heading":-0.01564, "vx":1.43017, "vy":-1.08751, "omega":0.28973, "ax":-6.82159, "ay":7.58173, "alpha":-4.47216, "fx":[-115.81186,-92.5236,-117.74141,-138.05642], "fy":[130.8023,148.37087,129.38412,107.29472]}, - {"t":1.8577, "x":3.1042, "y":4.49302, "heading":-0.00836, "vx":1.25879, "vy":-0.89704, "omega":0.17738, "ax":-7.5363, "ay":6.94226, "alpha":-3.09884, "fx":[-129.42569,-113.22156,-128.07038,-142.04338], "fy":[117.58984,133.36287,119.25993,102.13065]}, - {"t":1.88282, "x":3.13344, "y":4.47267, "heading":-0.00391, "vx":1.06947, "vy":-0.72264, "omega":0.09953, "ax":-8.00978, "ay":6.43126, "alpha":-2.06897, "fx":[-137.52332,-127.09427,-135.56202,-144.79637], "fy":[108.18475,120.31996,110.75117,98.31981]}, - {"t":1.90795, "x":3.15778, "y":4.45655, "heading":-0.0014, "vx":0.86824, "vy":-0.56107, "omega":0.04756, "ax":-8.33854, "ay":6.02266, "alpha":-1.28091, "fx":[-142.77542,-136.61923,-141.14796,-146.80211], "fy":[101.28612,109.47733,103.60989,95.40185]}, - {"t":1.93307, "x":3.17696, "y":4.44435, "heading":-0.00021, "vx":0.65876, "vy":-0.40977, "omega":0.01538, "ax":-8.57634, "ay":5.69241, "alpha":-0.66499, "fx":[-146.41058,-143.366,-145.42256,-148.3249], "fy":[96.06099,100.56161,97.5831,93.09922]}, - {"t":1.95819, "x":3.19081, "y":4.43585, "heading":0.00018, "vx":0.44331, "vy":-0.26676, "omega":-0.00133, "ax":-8.7544, "ay":5.42179, "alpha":-0.17347, "fx":[-149.05404,-148.29551,-148.77045,-149.51903], "fy":[91.98949,93.21048,92.4555,91.23683]}, - {"t":1.98331, "x":3.19918, "y":4.43086, "heading":0.00014, "vx":0.22338, "vy":-0.13056, "omega":-0.00569, "ax":-8.89167, "ay":5.19694, "alpha":0.22641, "fx":[-151.05204,-152.00101,-151.44611,-150.47994], "fy":[88.73907,87.09993,88.0549,89.69969]}, - {"t":2.00844, "x":3.20199, "y":4.42922, "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.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.98703, "ay":-0.68351, "alpha":51.68758, "fx":[30.03545,51.61357,-184.83133,-162.66523], "fy":[-196.75842,191.97313,74.15801,-114.94761]}, + {"t":0.03094, "x":7.09898, "y":7.2567, "heading":3.14159, "vx":-0.12334, "vy":-0.02114, "omega":1.59896, "ax":-4.14924, "ay":-0.72301, "alpha":51.16733, "fx":[25.4868,46.23445,-185.94531,-162.43919], "fy":[-197.30364,193.12829,71.13776,-115.1716]}, + {"t":0.06187, "x":7.09318, "y":7.2557, "heading":-3.09213, "vx":-0.2517, "vy":-0.04351, "omega":3.18183, "ax":-4.54265, "ay":-0.90461, "alpha":49.88583, "fx":[25.79996,19.53815,-189.75435,-158.47904], "fy":[-197.07578,197.27507,59.88388,-120.40078]}, + {"t":0.09281, "x":7.08322, "y":7.25392, "heading":-2.9937, "vx":-0.39222, "vy":-0.0715, "omega":4.72505, "ax":-5.46426, "ay":-1.45173, "alpha":46.38521, "fx":[22.58789,-41.00113,-195.29986,-150.63319], "fy":[-197.02144,193.14038,36.83151,-129.74911]}, + {"t":0.12374, "x":7.06847, "y":7.25102, "heading":-2.84753, "vx":-0.56126, "vy":-0.1164, "omega":6.15998, "ax":-8.49805, "ay":-4.57609, "alpha":26.18352, "fx":[-41.18479,-188.30673,-196.44537,-140.69668], "fy":[-192.34473,49.45038,-22.99055,-139.24001]}, + {"t":0.15468, "x":7.04704, "y":7.24523, "heading":-2.65697, "vx":-0.82415, "vy":-0.25797, "omega":6.96997, "ax":-8.01607, "ay":-5.6046, "alpha":-23.97468, "fx":[-188.46436,-122.53281,-35.21495,-188.284], "fy":[-50.92258,-152.29371,-188.47126,17.98355]}, + {"t":0.18561, "x":7.01771, "y":7.23457, "heading":-2.44135, "vx":-1.07213, "vy":-0.43135, "omega":6.22831, "ax":-8.89897, "ay":-2.14035, "alpha":-25.35479, "fx":[-194.85724,-144.09136,-104.33255,-150.08536], "fy":[-3.85878,-129.34847,-129.93915,120.43181]}, + {"t":0.21655, "x":6.98029, "y":7.2202, "heading":-2.24868, "vx":-1.34742, "vy":-0.49756, "omega":5.44396, "ax":-9.56214, "ay":1.94527, "alpha":-21.60102, "fx":[-194.34089,-158.6342,-134.47111,-150.13897], "fy":[5.02446,-107.45085,110.63316,121.50002]}, + {"t":0.24748, "x":6.93403, "y":7.20574, "heading":-2.08027, "vx":-1.64322, "vy":-0.43738, "omega":4.77573, "ax":-9.93867, "ay":2.00997, "alpha":-19.31499, "fx":[-193.38579,-168.14759,-139.8891,-161.26922], "fy":[-0.28474,-86.88238,116.00404,105.1842]}, + {"t":0.27842, "x":6.87844, "y":7.19317, "heading":-1.93253, "vx":-1.95068, "vy":-0.3752, "omega":4.17822, "ax":-10.048, "ay":1.43064, "alpha":-18.40624, "fx":[-191.16835,-164.80396,-144.60109,-169.40813], "fy":[-14.31128,-86.26643,107.78896,88.18094]}, + {"t":0.30935, "x":6.81329, "y":7.18225, "heading":-1.80328, "vx":-2.26151, "vy":-0.33094, "omega":3.60882, "ax":-9.86456, "ay":0.36132, "alpha":-18.64914, "fx":[-185.46531,-149.825,-147.35381,-175.10556], "fy":[-35.35086,-101.13769,92.34129,68.23972]}, + {"t":0.34029, "x":6.73861, "y":7.17218, "heading":-1.69164, "vx":-2.56667, "vy":-0.31977, "omega":3.03191, "ax":-9.22173, "ay":-1.12562, "alpha":-19.31829, "fx":[-173.0428,-120.6793,-144.47865,-176.68647], "fy":[-61.28595,-122.79926,65.62821,43.4027]}, + {"t":0.37122, "x":6.6548, "y":7.16175, "heading":-1.59785, "vx":-2.85195, "vy":-0.35459, "omega":2.43429, "ax":-7.88069, "ay":-2.80364, "alpha":-19.07933, "fx":[-150.76612,-80.94905,-124.53738,-169.21702], "fy":[-86.28072,-136.14825,22.22253,13.26504]}, + {"t":0.40216, "x":6.5628, "y":7.14944, "heading":-1.52254, "vx":-3.09574, "vy":-0.44132, "omega":1.84407, "ax":-5.8603, "ay":-3.94977, "alpha":-16.57223, "fx":[-118.91902,-47.20771,-80.5114,-144.11565], "fy":[-98.27601,-126.74938,-21.39976,-16.93772]}, + {"t":0.43309, "x":6.46423, "y":7.1339, "heading":-1.4655, "vx":-3.27703, "vy":-0.56351, "omega":1.33141, "ax":-3.62381, "ay":-3.66164, "alpha":-11.66535, "fx":[-79.49112,-25.54642,-39.93755,-96.65345], "fy":[-86.20225,-93.51933,-32.13374,-32.29578]}, + {"t":0.46403, "x":6.36112, "y":7.11471, "heading":-1.42431, "vx":-3.38913, "vy":-0.67678, "omega":0.97054, "ax":-1.72977, "ay":-2.35792, "alpha":-6.08777, "fx":[-40.35326,-11.28869,-16.77472,-46.92104], "fy":[-54.79295,-52.87843,-23.10352,-26.4464]}, + {"t":0.49496, "x":6.25545, "y":7.09265, "heading":-1.39428, "vx":-3.44264, "vy":-0.74972, "omega":0.78221, "ax":-0.69808, "ay":-1.46027, "alpha":-2.73023, "fx":[-16.9264,-3.7999,-6.26512,-19.55499], "fy":[-31.8504,-29.80749,-16.75348,-18.9567]}, + {"t":0.5259, "x":6.14862, "y":7.06876, "heading":-1.37009, "vx":-3.46424, "vy":-0.79489, "omega":0.69775, "ax":-0.28741, "ay":-1.35153, "alpha":-1.59467, "fx":[-7.8201,-0.15345,-1.73319,-9.4573], "fy":[-27.02736,-25.55795,-18.01236,-19.51976]}, + {"t":0.55683, "x":6.04131, "y":7.04352, "heading":-1.3485, "vx":-3.47313, "vy":-0.8367, "omega":0.64842, "ax":-0.25537, "ay":-1.67823, "alpha":-1.9022, "fx":[-7.76272,1.38455,-0.69991,-9.94938], "fy":[-33.37175,-31.4539,-22.54703,-24.52828]}, + {"t":0.58777, "x":5.93375, "y":7.01683, "heading":-1.32844, "vx":-3.48103, "vy":-0.88862, "omega":0.58958, "ax":-1.00296, "ay":-2.56386, "alpha":-5.05428, "fx":[-25.32371,-1.14393,-7.68136,-32.72659], "fy":[-56.53547,-52.12469,-28.50965,-33.78365]}, + {"t":0.6187, "x":5.82558, "y":6.98812, "heading":-1.3102, "vx":-3.51205, "vy":-0.96793, "omega":0.43322, "ax":-2.2915, "ay":-6.02917, "alpha":-12.59065, "fx":[-55.84516,8.34302,-15.59264,-89.69812], "fy":[-125.53502,-125.12478,-74.28058,-77.07319]}, + {"t":0.64964, "x":5.71584, "y":6.95529, "heading":-1.2968, "vx":-3.58294, "vy":-1.15445, "omega":0.04373, "ax":4.89392, "ay":-2.85174, "alpha":11.45515, "fx":[106.63836,52.71082,59.48851,107.47973], "fy":[-6.78814,-31.83826,-89.67447,-61.84778]}, + {"t":0.66642, "x":5.65641, "y":6.93552, "heading":-1.29607, "vx":-3.50082, "vy":-1.2023, "omega":0.23594, "ax":2.16237, "ay":-1.76856, "alpha":4.32288, "fx":[44.15163,23.35708,28.37695,48.29685], "fy":[-16.0309,-22.75867,-43.04648,-36.088]}, + {"t":0.6832, "x":5.59797, "y":6.91509, "heading":-1.29211, "vx":-3.46454, "vy":-1.23197, "omega":0.30848, "ax":0.7843, "ay":-0.88331, "alpha":1.32411, "fx":[15.33822,9.09346,10.82973,17.0343], "fy":[-10.72196,-12.55098,-18.72992,-16.89485]}, + {"t":0.69998, "x":5.53995, "y":6.8943, "heading":-1.28693, "vx":-3.45138, "vy":-1.24679, "omega":0.3307, "ax":0.29947, "ay":-0.43981, "alpha":0.44514, "fx":[5.73257,3.64761,4.25247,6.33533], "fy":[-5.98681,-6.59737,-8.67606,-8.06544]}, + {"t":0.71676, "x":5.48208, "y":6.87332, "heading":-1.28138, "vx":-3.44635, "vy":-1.25417, "omega":0.33817, "ax":0.14591, "ay":-0.23433, "alpha":0.24196, "fx":[2.82931,1.69928,2.0353,3.165], "fy":[-3.17322,-3.51,-4.63901,-4.30224]}, + {"t":0.73353, "x":5.42427, "y":6.85224, "heading":-1.27571, "vx":-3.44391, "vy":-1.25811, "omega":0.34223, "ax":0.10316, "ay":-0.14526, "alpha":0.22204, "fx":[2.07986,1.04494,1.35934,2.39408], "fy":[-1.74671,-2.06141,-3.09596,-2.78126]}, + {"t":0.75031, "x":5.3665, "y":6.83111, "heading":-1.26997, "vx":-3.44218, "vy":-1.26054, "omega":0.34595, "ax":0.09723, "ay":-0.11577, "alpha":0.23992, "fx":[2.00584,0.88962,1.23578,2.35183], "fy":[-1.19864,-1.54504,-2.66102,-2.3146]}, + {"t":0.76709, "x":5.30875, "y":6.80994, "heading":-1.26416, "vx":-3.44054, "vy":-1.26249, "omega":0.34998, "ax":0.10405, "ay":-0.11939, "alpha":0.25683, "fx":[2.14207,0.94931,1.3268,2.51936], "fy":[-1.20499,-1.58277,-2.77525,-2.39745]}, + {"t":0.78387, "x":5.25104, "y":6.78874, "heading":-1.25829, "vx":-3.4388, "vy":-1.26449, "omega":0.35429, "ax":0.11592, "ay":-0.1435, "alpha":0.26463, "fx":[2.34776,1.12101,1.51714,2.74362], "fy":[-1.58058,-1.9771,-3.20342,-2.80689]}, + {"t":0.80065, "x":5.19335, "y":6.7675, "heading":-1.25235, "vx":-3.43685, "vy":-1.2669, "omega":0.35873, "ax":0.13046, "ay":-0.1818, "alpha":0.26296, "fx":[2.58272,1.36596,1.76675,2.9832], "fy":[-2.22182,-2.62317,-3.83924,-3.43789]}, + {"t":0.81743, "x":5.1357, "y":6.74622, "heading":-1.24633, "vx":-3.43466, "vy":-1.26995, "omega":0.36314, "ax":0.14683, "ay":-0.23063, "alpha":0.253, "fx":[2.8357,1.66713,2.05968,3.2279], "fy":[-3.06414,-3.45747,-4.62496,-4.23166]}, + {"t":0.83421, "x":5.07809, "y":6.72488, "heading":-1.24023, "vx":-3.4322, "vy":-1.27382, "omega":0.36738, "ax":0.16456, "ay":-0.28706, "alpha":0.23609, "fx":[3.10105,2.01259,2.38542,3.47351], "fy":[-4.05474,-4.42856,-5.51548,-5.14171]}, + {"t":0.85099, "x":5.02052, "y":6.70346, "heading":-1.23407, "vx":-3.42944, "vy":-1.27863, "omega":0.37135, "ax":0.18309, "ay":-0.34797, "alpha":0.21357, "fx":[3.37187,2.38901,2.7322,3.71471], "fy":[-5.13785,-5.48223,-6.46307,-6.11876]}, + {"t":0.86777, "x":4.963, "y":6.68196, "heading":-1.22784, "vx":-3.42637, "vy":-1.28447, "omega":0.37493, "ax":0.20181, "ay":-0.41014, "alpha":0.18702, "fx":[3.64079,2.78168,3.08743,3.94622], "fy":[-6.25493,-6.562,-7.41867,-7.11168]}, + {"t":0.88455, "x":4.90554, "y":6.66035, "heading":-1.22155, "vx":-3.42298, "vy":-1.29136, "omega":0.37807, "ax":0.22079, "ay":-0.47167, "alpha":0.15876, "fx":[3.91252,3.18455,3.44854,4.17627], "fy":[-7.36725,-7.63262,-8.3579,-8.09259]}, + {"t":0.90133, "x":4.84813, "y":6.63861, "heading":-1.2152, "vx":-3.41928, "vy":-1.29927, "omega":0.38073, "ax":0.24292, "ay":-0.53638, "alpha":0.13347, "fx":[4.24199,3.63112,3.85679,4.46746], "fy":[-8.52365,-8.75074,-9.35871,-9.13168]}, + {"t":0.91811, "x":4.79079, "y":6.61673, "heading":-1.20881, "vx":-3.4152, "vy":-1.30827, "omega":0.38297, "ax":0.27925, "ay":-0.62471, "alpha":0.12161, "fx":[4.82843,4.27269,4.48165,5.0372], "fy":[-10.0321,-10.24277,-10.79495,-10.58433]}, + {"t":0.93489, "x":4.73353, "y":6.59469, "heading":-1.20239, "vx":-3.41051, "vy":-1.31875, "omega":0.38501, "ax":0.36078, "ay":-0.80039, "alpha":0.14328, "fx":[6.21652,5.56197,5.81174,6.46595], "fy":[-12.89179,-13.1449,-13.79254,-13.53954]}, + {"t":0.95167, "x":4.67635, "y":6.57245, "heading":-1.19593, "vx":-3.40446, "vy":-1.33218, "omega":0.38742, "ax":0.56267, "ay":-1.23279, "alpha":0.2213, "fx":[9.69334,8.67745,9.06633,10.08095], "fy":[-19.85402,-20.25529,-21.2458,-20.84496]}, + {"t":0.96845, "x":4.6193, "y":6.54993, "heading":-1.18943, "vx":-3.39502, "vy":-1.35287, "omega":0.39113, "ax":1.05199, "ay":-2.31143, "alpha":0.34386, "fx":[18.05394,16.42936,17.02124,18.63997], "fy":[-37.45743,-38.11622,-39.60237,-38.94572]}, + {"t":0.98523, "x":4.56248, "y":6.5269, "heading":-1.18286, "vx":-3.37737, "vy":-1.39165, "omega":0.3969, "ax":0.66561, "ay":-2.503, "alpha":-1.13833, "fx":[9.4472,14.80648,12.77431,7.35344], "fy":[-45.21896,-43.10316,-38.21287,-40.36015]}, + {"t":1.00226, "x":4.50505, "y":6.50283, "heading":-1.1761, "vx":-3.36603, "vy":-1.43429, "omega":0.37751, "ax":0.3547, "ay":-1.31252, "alpha":-0.60344, "fx":[5.10207,7.84849,6.72772,3.97263], "fy":[-23.78321,-22.65045,-19.97254,-21.11018]}, + {"t":1.0193, "x":4.44777, "y":6.47821, "heading":-1.16967, "vx":-3.35999, "vy":-1.45665, "omega":0.36723, "ax":0.26277, "ay":-0.80059, "alpha":-0.23395, "fx":[4.07497,5.1291,4.6859,3.63096], "fy":[-14.09025,-13.64424,-12.6005,-13.04693]}, + {"t":1.03633, "x":4.39057, "y":6.45328, "heading":-1.16342, "vx":-3.35551, "vy":-1.47028, "omega":0.36324, "ax":0.24041, "ay":-0.59627, "alpha":-0.05124, "fx":[3.94189,4.1717,4.07309,3.84325], "fy":[-10.10331,-10.00418,-9.7757,-9.87484]}, + {"t":1.05336, "x":4.33345, "y":6.42815, "heading":-1.15723, "vx":-3.35142, "vy":-1.48044, "omega":0.36237, "ax":0.23434, "ay":-0.51763, "alpha":0.01625, "fx":[3.92679,3.85416,3.88589,3.95851], "fy":[-8.57649,-8.60837,-8.68067,-8.64879]}, + {"t":1.0704, "x":4.2764, "y":6.40286, "heading":-1.15106, "vx":-3.34742, "vy":-1.48926, "omega":0.36265, "ax":0.22728, "ay":-0.48238, "alpha":0.02421, "fx":[3.81869,3.71079,3.75874,3.86664], "fy":[-7.96317,-8.01133,-8.1188,-8.07064]}, + {"t":1.08743, "x":4.21941, "y":6.37742, "heading":-1.14488, "vx":-3.34355, "vy":-1.49747, "omega":0.36306, "ax":0.21446, "ay":-0.45569, "alpha":0.00475, "fx":[3.5808,3.55968,3.56923,3.59034], "fy":[-7.58087,-7.59045,-7.61149,-7.60191]}, + {"t":1.10447, "x":4.16249, "y":6.35185, "heading":-1.1387, "vx":-3.3399, "vy":-1.50524, "omega":0.36314, "ax":0.19626, "ay":-0.42548, "alpha":-0.02437, "fx":[3.24236,3.35032,3.30069,3.19272], "fy":[-7.17133,-7.12154,-7.01391,-7.0637]}, + {"t":1.1215, "x":4.10562, "y":6.32614, "heading":-1.13251, "vx":-3.33656, "vy":-1.51248, "omega":0.36273, "ax":0.17622, "ay":-0.39078, "alpha":-0.05086, "fx":[2.87764,3.1022,2.99722,2.77263], "fy":[-6.67876,-6.57352,-6.34952,-6.45477]}, + {"t":1.13853, "x":4.04881, "y":6.30032, "heading":-1.12633, "vx":-3.33356, "vy":-1.51914, "omega":0.36186, "ax":0.16099, "ay":-0.35644, "alpha":-0.06141, "fx":[2.61269,2.88299,2.75453,2.4842], "fy":[-6.14085,-6.01214,-5.7424,-5.87113]}, + {"t":1.15557, "x":3.99205, "y":6.2744, "heading":-1.12017, "vx":-3.33081, "vy":-1.52521, "omega":0.36081, "ax":0.16155, "ay":-0.33117, "alpha":-0.03594, "fx":[2.65211,2.80982,2.73366,2.57595], "fy":[-5.63738,-5.56107,-5.40366,-5.47997]}, + {"t":1.1726, "x":3.93534, "y":6.24837, "heading":-1.11402, "vx":-3.32806, "vy":-1.53085, "omega":0.3602, "ax":0.1963, "ay":-0.3278, "alpha":0.06082, "fx":[3.34003,3.07391,3.2044,3.47049], "fy":[-5.26612,-5.39696,-5.66256,-5.53174]}, + {"t":1.18963, "x":3.87868, "y":6.22224, "heading":-1.10789, "vx":-3.32472, "vy":-1.53644, "omega":0.36124, "ax":0.29768, "ay":-0.36513, "alpha":0.29495, "fx":[5.28595,3.99881,4.63895,5.9253], "fy":[-5.12292,-5.76627,-7.05016,-6.40701]}, + {"t":1.20667, "x":3.82209, "y":6.19602, "heading":-1.10173, "vx":-3.31965, "vy":-1.54266, "omega":0.36626, "ax":0.5239, "ay":-0.4711, "alpha":0.79108, "fx":[9.59154,6.14561,7.87921,11.31644], "fy":[-5.25969,-7.0144,-10.44601,-8.69224]}, + {"t":1.2237, "x":3.76562, "y":6.16967, "heading":-1.09549, "vx":-3.31072, "vy":-1.55068, "omega":0.37974, "ax":0.97311, "ay":-0.68616, "alpha":1.76482, "fx":[18.14189,10.45139,14.33679,21.95473], "fy":[-5.61321,-9.63136,-17.26231,-13.24507]}, + {"t":1.24074, "x":3.70937, "y":6.14316, "heading":-1.08903, "vx":-3.29415, "vy":-1.56237, "omega":0.4098, "ax":1.77167, "ay":-1.06558, "alpha":3.48087, "fx":[33.48515,18.22775,25.8144,40.60416], "fy":[-6.07319,-14.42815,-29.4557,-21.09373]}, + {"t":1.25777, "x":3.65351, "y":6.11639, "heading":-1.08205, "vx":-3.26397, "vy":-1.58052, "omega":0.46909, "ax":2.95597, "ay":-1.68597, "alpha":5.90756, "fx":[56.81647,30.49895,42.72076,67.06207], "fy":[-7.436,-23.30019,-48.70617,-32.97498]}, + {"t":1.2748, "x":3.59834, "y":6.08923, "heading":-1.07405, "vx":-3.21362, "vy":-1.60924, "omega":0.56972, "ax":4.29106, "ay":-2.6871, "alpha":8.15179, "fx":[84.34428,46.62864,61.0818,94.0652], "fy":[-14.19056,-41.02272,-74.48072,-49.47694]}, + {"t":1.29184, "x":3.54422, "y":6.06142, "heading":-1.06435, "vx":-3.14052, "vy":-1.65501, "omega":0.70858, "ax":5.39735, "ay":-4.32245, "alpha":8.69562, "fx":[108.55689,63.8346,74.96916,112.52403], "fy":[-36.95646,-74.59132,-103.72753,-72.93748]}, + {"t":1.30887, "x":3.49151, "y":6.03261, "heading":-1.05228, "vx":-3.04859, "vy":-1.72864, "omega":0.8567, "ax":0.7547, "ay":-6.59931, "alpha":-8.47555, "fx":[2.32607,46.32817,26.97964,-25.31187], "fy":[-129.74049,-114.14414,-88.98736,-107.15762]}, + {"t":1.32945, "x":3.42894, "y":5.99564, "heading":-1.03465, "vx":-3.03306, "vy":-1.86444, "omega":0.68228, "ax":0.06626, "ay":-4.17338, "alpha":-6.75405, "fx":[-4.94336,25.76493,8.20802,-24.61119], "fy":[-88.90082,-75.05757,-49.12661,-65.18767]}, + {"t":1.35003, "x":3.36653, "y":5.95638, "heading":-1.02061, "vx":-3.03169, "vy":-1.95033, "omega":0.54329, "ax":0.22867, "ay":-2.4829, "alpha":-3.598, "fx":[0.79359,16.25847,7.01247,-8.81743], "fy":[-52.83079,-44.17594,-29.70612,-38.84195]}, + {"t":1.37061, "x":3.30419, "y":5.91572, "heading":-1.00943, "vx":-3.02699, "vy":-2.00142, "omega":0.46925, "ax":0.89774, "ay":-1.89041, "alpha":-0.97714, "fx":[14.13306,18.28375,15.81301,11.62996], "fy":[-34.74144,-32.17366,-28.27056,-30.86307]}, + {"t":1.39119, "x":3.24209, "y":5.87414, "heading":-0.99977, "vx":-3.00851, "vy":-2.04032, "omega":0.44914, "ax":2.92392, "ay":-2.53103, "alpha":2.69936, "fx":[51.75392,39.86835,45.97827,57.36097], "fy":[-32.70636,-41.18846,-51.54951,-43.3202]}, + {"t":1.41177, "x":3.1808, "y":5.83161, "heading":-0.99053, "vx":-2.94834, "vy":-2.09241, "omega":0.50469, "ax":6.46845, "ay":-3.73809, "alpha":8.18179, "fx":[122.67795,86.10849,95.252,127.26525], "fy":[-25.87781,-66.82936,-95.30367,-61.23768]}, + {"t":1.43234, "x":3.1215, "y":5.78776, "heading":-0.98014, "vx":-2.81523, "vy":-2.16933, "omega":0.67306, "ax":8.69231, "ay":-4.17068, "alpha":10.73637, "fx":[166.60173,126.37939,123.29413,163.31127], "fy":[-9.81711,-84.75469,-119.10853,-64.4128]}, + {"t":1.45292, "x":3.06541, "y":5.74224, "heading":-0.96629, "vx":-2.63635, "vy":-2.25516, "omega":0.894, "ax":9.70564, "ay":-4.14178, "alpha":10.87422, "fx":[182.87978,150.03569,137.49745,176.74039], "fy":[-1.7903,-88.29964,-123.83955,-62.2367]}, + {"t":1.4735, "x":3.01321, "y":5.69495, "heading":-0.9479, "vx":-2.43662, "vy":-2.34039, "omega":1.11778, "ax":10.26627, "ay":-3.95823, "alpha":10.20067, "fx":[189.64092,164.06772,147.83391,182.99236], "fy":[0.26389,-84.77796,-120.61653,-58.79669]}, + {"t":1.49408, "x":2.96524, "y":5.64595, "heading":-0.92489, "vx":-2.22536, "vy":-2.42185, "omega":1.32769, "ax":10.64183, "ay":-3.71505, "alpha":9.23034, "fx":[192.97911,173.11957,156.79933,186.67907], "fy":[-0.55719,-78.64563,-113.66311,-54.84617]}, + {"t":1.51466, "x":2.9217, "y":5.59533, "heading":-0.89757, "vx":-2.00636, "vy":-2.4983, "omega":1.51764, "ax":10.92524, "ay":-3.4422, "alpha":8.11618, "fx":[194.83033,179.44096,164.96057,189.24199], "fy":[-2.79634,-71.61732,-104.51417,-50.59133]}, + {"t":1.53524, "x":2.88272, "y":5.54319, "heading":-0.86634, "vx":-1.78154, "vy":-2.56913, "omega":1.68466, "ax":11.15337, "ay":-3.14954, "alpha":6.9107, "fx":[195.91464,184.15012,172.41359,191.2067], "fy":[-5.79339,-64.2561,-93.81381,-46.14208]}, + {"t":1.55582, "x":2.84842, "y":5.48965, "heading":-0.83167, "vx":-1.55202, "vy":-2.63395, "omega":1.82687, "ax":11.34222, "ay":-2.84062, "alpha":5.6368, "fx":[196.54256,187.82851,179.1171,192.78904], "fy":[-9.21714,-56.71174,-81.88506,-41.59342]}, + {"t":1.57639, "x":2.81889, "y":5.43485, "heading":-0.79408, "vx":-1.31861, "vy":-2.6924, "omega":1.94287, "ax":11.49882, "ay":-2.51734, "alpha":4.3092, "fx":[196.86643,190.7852,184.98173,194.0861], "fy":[-12.86827,-49.00551,-68.9335,-37.04429]}, + {"t":1.59697, "x":2.79419, "y":5.37891, "heading":-0.7541, "vx":-1.08198, "vy":-2.74421, "omega":2.03155, "ax":11.62608, "ay":-2.18161, "alpha":2.94148, "fx":[196.97225,193.18358,189.90206,195.14646], "fy":[-16.59776,-41.13518,-55.13059,-32.60196]}, + {"t":1.61755, "x":2.77438, "y":5.32197, "heading":-0.71229, "vx":-0.84273, "vy":-2.7891, "omega":2.09208, "ax":11.72499, "ay":-1.83597, "alpha":1.5481, "fx":[196.9179,195.10744,193.77481,195.99954], "fy":[-20.27524,-33.11174,-40.65059,-28.38144]}, + {"t":1.63813, "x":2.75952, "y":5.26419, "heading":-0.66924, "vx":-0.60145, "vy":-2.82688, "omega":2.12394, "ax":11.79594, "ay":-1.4838, "alpha":0.14429, "fx":[196.75007,196.5984,196.51345,196.6688], "fy":[-23.78102,-24.96901,-25.68494,-24.50199]}, + {"t":1.65871, "x":2.74964, "y":5.2057, "heading":-0.62553, "vx":-0.3587, "vy":-2.85742, "omega":2.12691, "ax":11.8394, "ay":-1.12883, "alpha":-1.25724, "fx":[196.5099,197.67938,198.06172,197.17758], "fy":[-27.01614,-16.75227,-10.42189,-21.07805]}, + {"t":1.67929, "x":2.74477, "y":5.14666, "heading":-0.58176, "vx":-0.11506, "vy":-2.88065, "omega":2.10104, "ax":11.85594, "ay":-0.76885, "alpha":-2.67882, "fx":[196.21946,198.37293,198.38595,197.55297], "fy":[-30.0042,-8.37301,5.28197,-18.17031]}, + {"t":1.69987, "x":2.74491, "y":5.08722, "heading":-0.53853, "vx":0.12892, "vy":-2.89647, "omega":2.04591, "ax":11.83737, "ay":-0.34679, "alpha":-4.43183, "fx":[195.75247,198.6768,197.03001,197.83376], "fy":[-33.61831,1.45584,24.54701,-15.50772]}, + {"t":1.72044, "x":2.75007, "y":5.02754, "heading":-0.49642, "vx":0.37251, "vy":-2.90361, "omega":1.95471, "ax":11.84661, "ay":0.31591, "alpha":-4.40013, "fx":[197.44245,198.4163,195.50333,198.54737], "fy":[-22.62101,12.1724,35.39767,-3.88482]}, + {"t":1.74538, "x":2.76304, "y":4.95524, "heading":-0.44769, "vx":0.66789, "vy":-2.89573, "omega":1.845, "ax":11.77177, "ay":1.14815, "alpha":-4.861, "fx":[198.26538,196.95797,191.52347,198.17186], "fy":[-11.7263,26.14389,52.35042,9.78871]}, + {"t":1.77031, "x":2.78335, "y":4.8834, "heading":-0.40168, "vx":0.9614, "vy":-2.8671, "omega":1.7238, "ax":11.60375, "ay":2.23969, "alpha":-4.82773, "fx":[198.33165,193.81107,185.66767,195.90535], "fy":[6.39123,43.03758,69.85354,30.05555]}, + {"t":1.79524, "x":2.81093, "y":4.81261, "heading":-0.3587, "vx":1.25072, "vy":-2.81126, "omega":1.60343, "ax":11.23073, "ay":3.73217, "alpha":-3.80091, "fx":[194.53181,187.37719,178.1047,188.82972], "fy":[37.78508,64.85697,86.80837,59.40344]}, + {"t":1.82018, "x":2.8456, "y":4.74367, "heading":-0.31873, "vx":1.53074, "vy":-2.7182, "omega":1.50866, "ax":10.30287, "ay":5.87586, "alpha":-1.14884, "fx":[175.5665,172.05761,167.8057,171.54581], "fy":[91.08469,97.67762,104.70829,98.32067]}, + {"t":1.84511, "x":2.88697, "y":4.67773, "heading":-0.28111, "vx":1.78762, "vy":-2.5717, "omega":1.48001, "ax":7.53294, "ay":9.13402, "alpha":0.77318, "fx":[121.82315,124.02017,129.35975,127.07888], "fy":[155.34466,153.50633,149.07573,151.11226]}, + {"t":1.87004, "x":2.93389, "y":4.61644, "heading":-0.24421, "vx":1.97544, "vy":-2.34396, "omega":1.49929, "ax":1.66573, "ay":11.70821, "alpha":-2.1321, "fx":[37.3307,38.40291,19.2366,16.09718], "fy":[193.72525,193.72553,196.5742,196.656]}, + {"t":1.89498, "x":2.98366, "y":4.56164, "heading":-0.20683, "vx":2.01697, "vy":-2.05203, "omega":1.44613, "ax":-3.31006, "ay":11.3269, "alpha":-4.32346, "fx":[-45.77857,-29.17881,-62.74133,-83.00948], "fy":[192.19058,195.73981,187.81736,179.50805]}, + {"t":1.91991, "x":3.03292, "y":4.514, "heading":-0.17077, "vx":1.93444, "vy":-1.76962, "omega":1.33833, "ax":-5.98801, "ay":10.15756, "alpha":-5.24877, "fx":[-97.38233,-68.71217,-102.93272,-130.24169], "fy":[172.33041,185.95199,169.5946,149.4098]}, + {"t":1.94484, "x":3.07929, "y":4.47303, "heading":-0.1374, "vx":1.78514, "vy":-1.51636, "omega":1.20746, "ax":-7.39578, "ay":9.18202, "alpha":-5.66547, "fx":[-125.49204,-91.66087,-123.80653,-152.17697], "fy":[153.45745,176.04279,155.30689,127.43253]}, + {"t":1.96978, "x":3.1215, "y":4.43808, "heading":-0.10729, "vx":1.60074, "vy":-1.28742, "omega":1.06621, "ax":-8.2068, "ay":8.46678, "alpha":-5.88447, "fx":[-141.44662,-105.90688,-136.05341,-163.80646], "fy":[139.1873,168.0513,144.90257,112.4075]}, + {"t":1.99471, "x":3.15886, "y":4.40861, "heading":-0.08071, "vx":1.39612, "vy":-1.07631, "omega":0.91949, "ax":-8.7183, "ay":7.94187, "alpha":-6.01194, "fx":[-151.21947,-115.3805,-143.9725,-170.74717], "fy":[128.73348,161.83711,137.18548,101.79259]}, + {"t":2.01964, "x":3.19096, "y":4.38424, "heading":-0.05778, "vx":1.17874, "vy":-0.8783, "omega":0.76959, "ax":-9.06496, "ay":7.54674, "alpha":-6.09182, "fx":[-157.65071,-122.05009,-149.46368,-175.26948], "fy":[120.95366,156.97817,131.29359,93.97649]}, + {"t":2.04458, "x":3.21753, "y":4.36469, "heading":-0.0386, "vx":0.95272, "vy":-0.69013, "omega":0.6177, "ax":-9.31324, "ay":7.24092, "alpha":-6.14512, "fx":[-162.15152,-126.96338,-153.46379,-178.4105], "fy":[114.9953,153.11784,126.68265,88.01488]}, + {"t":2.06951, "x":3.23839, "y":4.34974, "heading":-0.02319, "vx":0.72051, "vy":-0.50959, "omega":0.46448, "ax":-9.49886, "ay":6.9982, "alpha":-6.18292, "fx":[-165.47331,-130.71552,-156.47948,-180.69715], "fy":[110.28108,149.99663,123.00864,83.34035]}, + {"t":2.09444, "x":3.2534, "y":4.3392, "heading":-0.01161, "vx":0.48368, "vy":-0.3351, "omega":0.31032, "ax":-9.64238, "ay":6.80133, "alpha":-6.21148, "fx":[-168.04363,-133.66489,-158.80579,-182.42093], "fy":[106.42283,147.43106,120.04803,79.59756]}, + {"t":2.11938, "x":3.26247, "y":4.33296, "heading":-0.00388, "vx":0.24326, "vy":-0.16552, "omega":0.15545, "ax":-9.7564, "ay":6.63864, "alpha":-6.23459, "fx":[-170.12003,-136.03853,-160.62509,-183.75442], "fy":[103.15532,145.29071,117.65079,76.55484]}, + {"t":2.14431, "x":3.2655, "y":4.3309, "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/startDeepToB.traj b/src/main/deploy/choreo/startDeepToB.traj index 9ae000d8..d3a5b4ef 100644 --- a/src/main/deploy/choreo/startDeepToB.traj +++ b/src/main/deploy/choreo/startDeepToB.traj @@ -3,12 +3,12 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":4.595438003540039, "y":6.493947982788086, "heading":-1.5707963267948966, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.4256062507629395, "y":5.90079402923584, "heading":-1.1801894971536726, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":4.595438003540039, "y":6.493947982788086, "heading":-1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4256062507629395, "y":5.90079402923584, "heading":-1.1801894971536726, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":2.7665462493896484, "y":4.763915061950684, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.2019875, "y":4.1005465106, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.2654999999999994, "y":4.0009, "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}, @@ -17,12 +17,12 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"4.595438003540039 m", "val":4.595438003540039}, "y":{"exp":"6.493947982788086 m", "val":6.493947982788086}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"3.4256062507629395 m", "val":3.4256062507629395}, "y":{"exp":"5.90079402923584 m", "val":5.90079402923584}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"4.595438003540039 m", "val":4.595438003540039}, "y":{"exp":"6.493947982788086 m", "val":6.493947982788086}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4256062507629395 m", "val":3.4256062507629395}, "y":{"exp":"5.90079402923584 m", "val":5.90079402923584}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"2.7665462493896484 m", "val":2.7665462493896484}, "y":{"exp":"4.763915061950684 m", "val":4.763915061950684}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"B.x", "val":3.2019875}, "y":{"exp":"B.y", "val":4.1005465106}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "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}, @@ -34,111 +34,106 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.60983,0.89425,1.20214,1.60477,2.03261], + "waypoints":[0.0,0.65215,0.98345,1.34899,1.75906,2.18507], "samples":[ - {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.39219, "ay":-0.57766, "alpha":38.87343, "fx":[-17.84615,-32.81466,-166.58675,-149.63111], "fy":[-174.22044,171.7344,54.50336,-91.32086]}, - {"t":0.02772, "x":7.09882, "y":7.25681, "heading":3.14159, "vx":-0.14947, "vy":-0.01601, "omega":1.07755, "ax":-5.70391, "ay":-0.62751, "alpha":37.54287, "fx":[-24.64305,-46.0,-167.2345,-150.20982], "fy":[-173.33778,168.5721,52.38678,-90.31605]}, - {"t":0.05544, "x":7.09248, "y":7.25612, "heading":-3.11172, "vx":-0.30758, "vy":-0.03341, "omega":2.11821, "ax":-6.10519, "ay":-0.81304, "alpha":35.71071, "fx":[-30.30237,-66.78098,-168.7792,-149.52781], "fy":[-172.35756,161.37117,47.04227,-91.37418]}, - {"t":0.08316, "x":7.08161, "y":7.25489, "heading":-3.05301, "vx":-0.47681, "vy":-0.05594, "omega":3.10808, "ax":-6.59125, "ay":-1.14817, "alpha":33.24879, "fx":[-37.37866,-92.32472,-170.88443,-147.87344], "fy":[-170.83296,148.15566,38.4929,-93.93606]}, - {"t":0.11088, "x":7.06586, "y":7.25289, "heading":-2.96685, "vx":-0.65951, "vy":-0.08777, "omega":4.02972, "ax":-7.19205, "ay":-1.61896, "alpha":29.75011, "fx":[-50.99084,-119.48833,-173.051,-145.80872], "fy":[-167.05696,127.16966,26.71043,-96.97507]}, - {"t":0.1386, "x":7.04482, "y":7.24984, "heading":-2.85515, "vx":-0.85887, "vy":-0.13265, "omega":4.85437, "ax":-8.02449, "ay":-2.13454, "alpha":24.08436, "fx":[-81.00333,-145.64686,-174.61231,-144.71494], "fy":[-154.36571,95.94893,11.56323,-98.37833]}, - {"t":0.16632, "x":7.01793, "y":7.24534, "heading":-2.72059, "vx":-1.08131, "vy":-0.19181, "omega":5.52197, "ax":-9.14266, "ay":-2.4112, "alpha":14.75551, "fx":[-132.00222,-167.17469,-174.63508,-148.24442], "fy":[-113.30948,49.2455,-7.46533,-92.52567]}, - {"t":0.19404, "x":6.98444, "y":7.2391, "heading":-2.56753, "vx":-1.33473, "vy":-0.25865, "omega":5.93099, "ax":-9.92018, "ay":-2.39568, "alpha":2.86701, "fx":[-166.68538,-172.59211,-171.03951,-164.64095], "fy":[-49.91191,-22.74852,-33.38914,-56.94986]}, - {"t":0.22175, "x":6.94363, "y":7.23101, "heading":-2.40313, "vx":-1.60971, "vy":-0.32506, "omega":6.01046, "ax":-8.64505, "ay":-2.46787, "alpha":-17.373, "fx":[-173.2103,-129.79478,-129.76941,-155.4246], "fy":[-19.1182,-115.84925,-110.1739,77.23012]}, - {"t":0.24947, "x":6.89569, "y":7.22105, "heading":-2.23652, "vx":-1.84935, "vy":-0.39347, "omega":5.52889, "ax":-8.19574, "ay":-2.01719, "alpha":-19.52615, "fx":[-172.51875,-110.71522,-130.94396,-143.45058], "fy":[-24.25081,-133.88058,-76.99516,97.87934]}, - {"t":0.27719, "x":6.84128, "y":7.20937, "heading":-2.08326, "vx":-2.07653, "vy":-0.44938, "omega":4.98764, "ax":-8.7673, "ay":-0.48107, "alpha":-18.06358, "fx":[-170.73358,-114.38397,-157.1709,-154.22868], "fy":[-33.71076,-130.19213,51.53351,79.63821]}, - {"t":0.30491, "x":6.78035, "y":7.19673, "heading":-1.94501, "vx":-2.31955, "vy":-0.46272, "omega":4.48693, "ax":-8.94177, "ay":-0.71092, "alpha":-17.21691, "fx":[-168.51194,-119.41115,-158.53634,-161.92841], "fy":[-42.45798,-124.87966,56.97149,61.99607]}, - {"t":0.33263, "x":6.71262, "y":7.18363, "heading":-1.82063, "vx":-2.56741, "vy":-0.48242, "omega":4.00969, "ax":-8.96757, "ay":-1.01476, "alpha":-16.95381, "fx":[-165.64352,-119.64766,-158.7012,-166.15108], "fy":[-51.46291,-123.7511,57.59663,48.57419]}, - {"t":0.36035, "x":6.63801, "y":7.16987, "heading":-1.70949, "vx":-2.81599, "vy":-0.51055, "omega":3.53974, "ax":-8.86494, "ay":-1.38194, "alpha":-17.28785, "fx":[-161.71235,-114.73701,-158.10473,-168.60635], "fy":[-61.49828,-127.13857,57.34592,37.2656]}, - {"t":0.38807, "x":6.55654, "y":7.15518, "heading":-1.61137, "vx":-3.06172, "vy":-0.54886, "omega":3.06053, "ax":-8.62543, "ay":-1.83695, "alpha":-18.15794, "fx":[-156.15504,-103.91911,-156.83893,-169.95143], "fy":[-72.99352,-134.55126,56.04288,26.51802]}, - {"t":0.41579, "x":6.46836, "y":7.13926, "heading":-1.52653, "vx":-3.30081, "vy":-0.59978, "omega":2.55721, "ax":-8.22053, "ay":-2.40646, "alpha":-19.38706, "fx":[-148.1852,-86.08347,-154.79374,-170.25308], "fy":[-86.10941,-144.47553,51.90702,14.94512]}, - {"t":0.44351, "x":6.37371, "y":7.12171, "heading":-1.45565, "vx":-3.52868, "vy":-0.66648, "omega":2.01981, "ax":-7.59554, "ay":-3.12149, "alpha":-20.58348, "fx":[-136.64377,-60.56693,-150.67292,-168.9079], "fy":[-100.66862,-153.8996,41.06582,1.12013]}, - {"t":0.47123, "x":6.27298, "y":7.10204, "heading":-1.39966, "vx":-3.73922, "vy":-0.75301, "omega":1.44925, "ax":-6.63353, "ay":-4.00423, "alpha":-20.97251, "fx":[-120.27809,-29.96472,-137.15754,-163.93747], "fy":[-115.07475,-157.8074,16.26307,-15.82417]}, - {"t":0.49895, "x":6.16678, "y":7.07963, "heading":-1.35949, "vx":-3.9231, "vy":-0.864, "omega":0.86791, "ax":-5.39634, "ay":-4.53278, "alpha":-19.49133, "fx":[-103.40312,-13.58926,-99.92831,-150.23991], "fy":[-118.52612,-146.8548,-13.59309,-29.43103]}, - {"t":0.52667, "x":6.05596, "y":7.05394, "heading":-1.33543, "vx":-4.07268, "vy":-0.98965, "omega":0.32762, "ax":-1.69472, "ay":-4.76065, "alpha":-9.05052, "fx":[-42.91476,3.1751,-11.51238,-64.05446], "fy":[-99.86126,-98.61765,-61.14724,-64.28314]}, - {"t":0.55439, "x":5.94242, "y":7.02468, "heading":-1.32635, "vx":-4.11966, "vy":-1.12161, "omega":0.07675, "ax":0.99621, "ay":-4.14785, "alpha":-0.61892, "fx":[15.5506,18.95336,18.36414,14.91272], "fy":[-72.18486,-71.34536,-68.91671,-69.76831]}, - {"t":0.58211, "x":5.82861, "y":6.99199, "heading":-1.32422, "vx":-4.09204, "vy":-1.23658, "omega":0.05959, "ax":1.72245, "ay":-6.31301, "alpha":-1.24583, "fx":[25.67109,33.71778,33.10537,24.69907], "fy":[-110.21416,-108.09288,-104.50188,-106.72101]}, - {"t":0.60983, "x":5.71584, "y":6.95529, "heading":-1.32257, "vx":-4.0443, "vy":-1.41158, "omega":0.02506, "ax":3.1594, "ay":-4.46181, "alpha":4.23182, "fx":[65.74608,41.40843,42.86899,64.93866], "fy":[-62.33444,-72.7192,-88.95178,-79.57092]}, - {"t":0.62337, "x":5.66135, "y":6.93576, "heading":-1.32223, "vx":-4.00151, "vy":-1.47201, "omega":0.08237, "ax":1.75539, "ay":-3.80796, "alpha":0.8699, "fx":[31.90089,27.17805,27.86108,32.4949], "fy":[-62.27603,-63.7338,-67.25722,-65.82238]}, - {"t":0.63691, "x":5.60732, "y":6.91548, "heading":-1.32112, "vx":-3.97773, "vy":-1.52358, "omega":0.09415, "ax":1.12886, "ay":-2.82248, "alpha":0.05368, "fx":[19.31154,19.03827,19.09189,19.36492], "fy":[-47.85497,-47.93071,-48.16425,-48.08856]}, - {"t":0.65046, "x":5.55355, "y":6.89458, "heading":-1.31984, "vx":-3.96244, "vy":-1.56181, "omega":0.09488, "ax":0.76096, "ay":-1.96373, "alpha":-0.07961, "fx":[12.79329,13.183,13.09447,12.70441], "fy":[-33.6351,-33.53126,-33.16989,-33.27379]}, - {"t":0.664, "x":5.49995, "y":6.87325, "heading":-1.31856, "vx":-3.95214, "vy":-1.5884, "omega":0.0938, "ax":0.51452, "ay":-1.3236, "alpha":-0.07081, "fx":[8.62357,8.96322,8.88031,8.54047], "fy":[-22.72273,-22.6336,-22.30537,-22.39454]}, - {"t":0.67754, "x":5.44647, "y":6.85162, "heading":-1.31729, "vx":-3.94517, "vy":-1.60633, "omega":0.09284, "ax":0.3458, "ay":-0.88022, "alpha":-0.04592, "fx":[5.80052,6.01864,5.96347,5.7453], "fy":[-15.10818,-15.0512,-14.83636,-14.89334]}, - {"t":0.69109, "x":5.39307, "y":6.82978, "heading":-1.31603, "vx":-3.94048, "vy":-1.61825, "omega":0.09222, "ax":0.23185, "ay":-0.58381, "alpha":-0.02664, "fx":[3.89701,4.02299,3.99053,3.86453], "fy":[-10.0094,-9.97647,-9.85133,-9.88426]}, - {"t":0.70463, "x":5.33972, "y":6.80781, "heading":-1.31478, "vx":-3.93734, "vy":-1.62616, "omega":0.09186, "ax":0.15635, "ay":-0.38962, "alpha":-0.01388, "fx":[2.63517,2.70067,2.6836,2.6181], "fy":[-6.6685,-6.65133,-6.58602,-6.60319]}, - {"t":0.71818, "x":5.28641, "y":6.78575, "heading":-1.31353, "vx":-3.93523, "vy":-1.63144, "omega":0.09167, "ax":0.10746, "ay":-0.26497, "alpha":-0.00579, "fx":[1.81786,1.84517,1.838,1.81068], "fy":[-4.52432,-4.51712,-4.48985,-4.49704]}, - {"t":0.73172, "x":5.23312, "y":6.76363, "heading":-1.31229, "vx":-3.93377, "vy":-1.63503, "omega":0.09159, "ax":0.07711, "ay":-0.18798, "alpha":-0.00085, "fx":[1.31006,1.31408,1.31302,1.30899], "fy":[-3.19997,-3.19891,-3.19489,-3.19595]}, - {"t":0.74526, "x":5.17985, "y":6.74147, "heading":-1.31105, "vx":-3.93273, "vy":-1.63757, "omega":0.09158, "ax":0.06007, "ay":-0.14488, "alpha":0.0019, "fx":[1.02505,1.01609,1.01847,1.02743], "fy":[-2.45863,-2.46101,-2.46996,-2.46758]}, - {"t":0.75881, "x":5.12659, "y":6.71927, "heading":-1.30981, "vx":-3.93191, "vy":-1.63953, "omega":0.09161, "ax":0.05339, "ay":-0.12791, "alpha":0.003, "fx":[0.91335,0.89921,0.90298,0.91712], "fy":[-2.16678,-2.17056,-2.1847,-2.18092]}, - {"t":0.77235, "x":5.07335, "y":6.69706, "heading":-1.30857, "vx":-3.93119, "vy":-1.64127, "omega":0.09165, "ax":0.05591, "ay":-0.13402, "alpha":0.00267, "fx":[0.95569,0.94315,0.94651,0.95905], "fy":[-2.27162,-2.27498,-2.28752,-2.28415]}, - {"t":0.78589, "x":5.02011, "y":6.67481, "heading":-1.30733, "vx":-3.93043, "vy":-1.64308, "omega":0.09168, "ax":0.06813, "ay":-0.16427, "alpha":0.00083, "fx":[1.16034,1.15643,1.15748,1.1614], "fy":[-2.79165,-2.79271,-2.79662,-2.79557]}, - {"t":0.79944, "x":4.96688, "y":6.65255, "heading":-1.30609, "vx":-3.92951, "vy":-1.64531, "omega":0.0917, "ax":0.0923, "ay":-0.22407, "alpha":-0.00279, "fx":[1.5652,1.57834,1.57478,1.56165], "fy":[-3.81969,-3.81613,-3.80301,-3.80657]}, - {"t":0.81298, "x":4.91367, "y":6.63024, "heading":-1.30485, "vx":-3.92826, "vy":-1.64834, "omega":0.09166, "ax":0.13288, "ay":-0.32404, "alpha":-0.00876, "fx":[2.24525,2.28644,2.27526,2.23406], "fy":[-5.53802,-5.52678,-5.48568,-5.49691]}, - {"t":0.82653, "x":4.86048, "y":6.60789, "heading":-1.30361, "vx":-3.92646, "vy":-1.65273, "omega":0.09154, "ax":0.1974, "ay":-0.48167, "alpha":-0.01775, "fx":[3.32732,3.4109,3.38818,3.3046], "fy":[-8.24605,-8.22312,-8.13992,-8.16285]}, - {"t":0.84007, "x":4.80732, "y":6.58546, "heading":-1.30237, "vx":-3.92379, "vy":-1.65925, "omega":0.0913, "ax":0.29796, "ay":-0.7235, "alpha":-0.02986, "fx":[5.01685,5.15781,5.11965,4.97867], "fy":[-12.39572,-12.35674,-12.21723,-12.25622]}, - {"t":0.85361, "x":4.7542, "y":6.56292, "heading":-1.30113, "vx":-3.91975, "vy":-1.66905, "omega":0.09089, "ax":0.4541, "ay":-1.08668, "alpha":-0.04022, "fx":[7.65403,7.84503,7.79414,7.60309], "fy":[-18.60416,-18.5507,-18.3641,-18.41757]}, - {"t":0.86716, "x":4.70115, "y":6.54021, "heading":-1.2999, "vx":-3.9136, "vy":-1.68377, "omega":0.09035, "ax":0.70075, "ay":-1.61426, "alpha":-0.01549, "fx":[11.89174,11.96633,11.94728,11.87269], "fy":[-27.50415,-27.48281,-27.41197,-27.43331]}, - {"t":0.8807, "x":4.64821, "y":6.51726, "heading":-1.29867, "vx":-3.90411, "vy":-1.70563, "omega":0.09014, "ax":1.11546, "ay":-2.32712, "alpha":0.27775, "fx":[19.50377,18.12928,18.44623,19.81539], "fy":[-38.75992,-39.17329,-40.40703,-39.99459]}, - {"t":0.89425, "x":4.59544, "y":6.49395, "heading":-1.29745, "vx":-3.889, "vy":-1.73715, "omega":0.0939, "ax":0.85376, "ay":-2.58565, "alpha":-0.84844, "fx":[12.91794,17.13933,16.15299,11.87846], "fy":[-46.44181,-45.23045,-41.51378,-42.73874]}, - {"t":0.90891, "x":4.53851, "y":6.4682, "heading":-1.29608, "vx":-3.87648, "vy":-1.77506, "omega":0.08146, "ax":0.68993, "ay":-1.63534, "alpha":-0.18626, "fx":[11.4041,12.29973,12.06787,11.17061], "fy":[-28.3714,-28.11202,-27.26194,-27.52162]}, - {"t":0.92357, "x":4.48175, "y":6.442, "heading":-1.29488, "vx":-3.86637, "vy":-1.79904, "omega":0.07873, "ax":0.46366, "ay":-1.03406, "alpha":-0.06095, "fx":[7.78191,8.07049,7.9916,7.70291], "fy":[-17.77178,-17.689,-17.40639,-17.48919]}, - {"t":0.93823, "x":4.42511, "y":6.41551, "heading":-1.29373, "vx":-3.85957, "vy":-1.8142, "omega":0.07784, "ax":0.30002, "ay":-0.65541, "alpha":-0.02796, "fx":[5.056,5.18756,5.15067,5.0191], "fy":[-11.23239,-11.19477,-11.06432,-11.10194]}, - {"t":0.95289, "x":4.36856, "y":6.38884, "heading":-1.29259, "vx":-3.85517, "vy":-1.82381, "omega":0.07743, "ax":0.19541, "ay":-0.42233, "alpha":-0.01415, "fx":[3.30006,3.36646,3.3476,3.2812], "fy":[-7.22622,-7.20721,-7.14104,-7.16005]}, - {"t":0.96755, "x":4.31205, "y":6.36206, "heading":-1.29145, "vx":-3.85231, "vy":-1.83, "omega":0.07722, "ax":0.13282, "ay":-0.28478, "alpha":-0.00695, "fx":[2.24763,2.28019,2.27087,2.23831], "fy":[-4.86502,-4.85567,-4.82316,-4.83251]}, - {"t":0.98221, "x":4.25559, "y":6.3352, "heading":-1.29032, "vx":-3.85036, "vy":-1.83418, "omega":0.07712, "ax":0.09962, "ay":-0.21219, "alpha":-0.00328, "fx":[1.68902,1.70439,1.69996,1.6846], "fy":[-3.61924,-3.61481,-3.59945,-3.60388]}, - {"t":0.99688, "x":4.19915, "y":6.30828, "heading":-1.28919, "vx":-3.8489, "vy":-1.83729, "omega":0.07707, "ax":0.08869, "ay":-0.18814, "alpha":-0.00206, "fx":[1.50512,1.51474,1.51196,1.50234], "fy":[-3.20633,-3.20354,-3.19393,-3.19671]}, - {"t":1.01154, "x":4.14272, "y":6.28132, "heading":-1.28806, "vx":-3.8476, "vy":-1.84004, "omega":0.07704, "ax":0.0977, "ay":-0.20711, "alpha":-0.00292, "fx":[1.65699,1.67065,1.66669,1.65303], "fy":[-3.53169,-3.52772,-3.51407,-3.51804]}, - {"t":1.0262, "x":4.08632, "y":6.25432, "heading":-1.28693, "vx":-3.84617, "vy":-1.84308, "omega":0.077, "ax":0.12882, "ay":-0.27334, "alpha":-0.00602, "fx":[2.18127,2.20942,2.20123,2.17307], "fy":[-4.66768,-4.65946,-4.63135,-4.63957]}, - {"t":1.04086, "x":4.02995, "y":6.22727, "heading":-1.2858, "vx":-3.84428, "vy":-1.84709, "omega":0.07691, "ax":0.1893, "ay":-0.40159, "alpha":-0.01194, "fx":[3.20009,3.25598,3.23969,3.18379], "fy":[-6.86702,-6.85061,-6.79489,-6.8113]}, - {"t":1.05552, "x":3.9736, "y":6.20015, "heading":-1.28467, "vx":-3.8415, "vy":-1.85298, "omega":0.07673, "ax":0.29303, "ay":-0.61976, "alpha":-0.02172, "fx":[4.94816,5.05002,5.02044,4.91857], "fy":[-10.60758,-10.57746,-10.47637,-10.50649]}, - {"t":1.07018, "x":3.91731, "y":6.17291, "heading":-1.28355, "vx":-3.83721, "vy":-1.86206, "omega":0.07641, "ax":0.46311, "ay":-0.97252, "alpha":-0.03672, "fx":[7.81565,7.98877,7.93921,7.76606], "fy":[-16.65318,-16.60141,-16.43148,-16.48326]}, - {"t":1.08485, "x":3.8611, "y":6.14551, "heading":-1.28243, "vx":-3.83042, "vy":-1.87632, "omega":0.07588, "ax":0.73379, "ay":-1.52141, "alpha":-0.05814, "fx":[12.3809,12.65852,12.58242,12.30465], "fy":[-26.05387,-25.969,-25.70373,-25.78863]}, - {"t":1.09951, "x":3.80502, "y":6.11783, "heading":-1.28132, "vx":-3.81966, "vy":-1.89863, "omega":0.07502, "ax":1.14537, "ay":-2.32676, "alpha":-0.08531, "fx":[19.32433,19.74357,19.64059,19.22084], "fy":[-39.83287,-39.69895,-39.32198,-39.45601]}, - {"t":1.11417, "x":3.74914, "y":6.08975, "heading":-1.28022, "vx":-3.80286, "vy":-1.93274, "omega":0.07377, "ax":1.71884, "ay":-3.38695, "alpha":-0.11203, "fx":[29.00228,29.58331,29.47232,28.88998], "fy":[-57.94341,-57.74258,-57.27859,-57.47978]}, - {"t":1.12883, "x":3.69357, "y":6.06105, "heading":-1.27913, "vx":-3.77766, "vy":-1.9824, "omega":0.07213, "ax":2.41653, "ay":-4.5619, "alpha":-0.12009, "fx":[40.80599,41.47876,41.40403,40.72914], "fy":[-77.94992,-77.68976,-77.24281,-77.50379]}, - {"t":1.14349, "x":3.63844, "y":6.03149, "heading":-1.27808, "vx":-3.74223, "vy":-2.04929, "omega":0.07037, "ax":3.15576, "ay":-5.61638, "alpha":-0.06113, "fx":[53.49713,53.86827,53.86023,53.48839], "fy":[-95.71389,-95.55064,-95.35182,-95.51544]}, - {"t":1.15815, "x":3.58392, "y":6.00084, "heading":-1.27704, "vx":-3.69597, "vy":-2.13163, "omega":0.06947, "ax":3.90373, "ay":-6.36996, "alpha":0.21351, "fx":[67.13647,65.7586,65.67109,67.03933], "fy":[-107.69324,-108.3913,-109.0055,-108.31443]}, - {"t":1.17282, "x":3.53015, "y":5.9689, "heading":-1.27603, "vx":-3.63873, "vy":-2.22502, "omega":0.0726, "ax":4.7555, "ay":-6.70987, "alpha":1.09608, "fx":[85.09717,77.86406,76.80192,83.79577], "fy":[-110.37571,-114.78885,-117.76611,-113.60106]}, - {"t":1.18748, "x":3.47731, "y":5.93556, "heading":-1.27496, "vx":-3.56901, "vy":-2.3234, "omega":0.08867, "ax":5.82332, "ay":-6.51164, "alpha":3.16845, "fx":[111.72361,91.94816,87.01228,105.52798], "fy":[-97.6269,-113.56809,-122.6832,-109.1664]}, - {"t":1.20214, "x":3.42561, "y":5.90079, "heading":-1.27366, "vx":-3.48363, "vy":-2.41887, "omega":0.13513, "ax":6.87173, "ay":-6.24303, "alpha":4.68657, "fx":[135.42821,109.55866,98.68198,123.87571], "fy":[-83.21214,-111.22588,-126.30644,-104.02417]}, - {"t":1.22131, "x":3.36008, "y":5.85327, "heading":-1.27107, "vx":-3.35188, "vy":-2.53857, "omega":0.22498, "ax":8.18907, "ay":-4.33364, "alpha":9.12658, "fx":[162.82602,138.30882,108.75441,147.28545], "fy":[-20.72088,-73.29995,-122.94961,-77.88555]}, - {"t":1.24048, "x":3.29732, "y":5.8038, "heading":-1.26676, "vx":-3.19487, "vy":-2.62166, "omega":0.39997, "ax":8.77545, "ay":-2.93645, "alpha":11.44089, "fx":[167.26877,156.12642,116.34202,157.3346], "fy":[13.84807,-31.2142,-119.69413,-62.73213]}, - {"t":1.25966, "x":3.23768, "y":5.753, "heading":-1.25909, "vx":-3.02662, "vy":-2.67796, "omega":0.61932, "ax":9.05624, "ay":-2.06557, "alpha":12.47734, "fx":[167.33656,162.80269,123.64797,162.38866], "fy":[29.75807,-1.86477,-114.96147,-53.47056]}, - {"t":1.27883, "x":3.18131, "y":5.70128, "heading":-1.24721, "vx":-2.85299, "vy":-2.71756, "omega":0.85854, "ax":9.21968, "ay":-1.57559, "alpha":12.76259, "fx":[167.10954,165.18379,129.7062,165.29698], "fy":[37.38841,13.0168,-110.19598,-47.41077]}, - {"t":1.298, "x":3.12831, "y":5.64888, "heading":-1.23075, "vx":-2.67622, "vy":-2.74777, "omega":1.10324, "ax":9.339, "ay":-1.31139, "alpha":12.62055, "fx":[167.08033,166.70231,134.47413,167.1581], "fy":[41.15748,18.71068,-105.94687,-43.14684]}, - {"t":1.31718, "x":3.07871, "y":5.59596, "heading":-1.2096, "vx":-2.49717, "vy":-2.77291, "omega":1.34521, "ax":9.44112, "ay":-1.17413, "alpha":12.21752, "fx":[167.25334,168.1639,138.46266,168.48291], "fy":[42.84404,19.01473,-101.95122,-39.79372]}, - {"t":1.33635, "x":3.03257, "y":5.54258, "heading":-1.18381, "vx":-2.31616, "vy":-2.79542, "omega":1.57945, "ax":9.53642, "ay":-1.10584, "alpha":11.63734, "fx":[167.59072,169.59503,142.13515,169.52632], "fy":[43.22397,16.12762,-97.78805,-36.80393]}, - {"t":1.35552, "x":2.98992, "y":5.48878, "heading":-1.15353, "vx":-2.13332, "vy":-2.81662, "omega":1.80257, "ax":9.62896, "ay":-1.06997, "alpha":10.92844, "fx":[168.06696,170.86156,145.79623,170.41876], "fy":[42.64703,11.48136,-93.08293,-33.84475]}, - {"t":1.37469, "x":2.95079, "y":5.43458, "heading":-1.11897, "vx":-1.9487, "vy":-2.83714, "omega":2.0121, "ax":9.7199, "ay":-1.04165, "alpha":10.12394, "fx":[168.66602,171.85549,149.58704,171.22231], "fy":[41.26907,6.16069,-87.57216,-30.72996]}, - {"t":1.39387, "x":2.91521, "y":5.37999, "heading":-1.08039, "vx":-1.76235, "vy":-2.85711, "omega":2.2062, "ax":9.8089, "ay":-1.00428, "alpha":9.24777, "fx":[169.37102,172.55315,153.50391,171.95803], "fy":[39.16399,1.0173,-81.12798,-27.38355]}, - {"t":1.41304, "x":2.88322, "y":5.32503, "heading":-1.03809, "vx":-1.57428, "vy":-2.87636, "omega":2.38351, "ax":9.8948, "ay":-0.9483, "alpha":8.31566, "fx":[170.15719,173.00885,157.44143,172.62364], "fy":[36.38122,-3.34235,-73.74855,-23.81127]}, - {"t":1.43221, "x":2.85486, "y":5.26971, "heading":-0.99239, "vx":-1.38457, "vy":-2.89454, "omega":2.54294, "ax":9.97605, "ay":-0.8698, "alpha":7.33613, "fx":[170.99002,173.31359,161.24838,173.20701], "fy":[32.97278,-6.56351,-65.5176,-20.07193]}, - {"t":1.45138, "x":2.83015, "y":5.21405, "heading":-0.94364, "vx":-1.19331, "vy":-2.91122, "omega":2.68359, "ax":10.0509, "ay":-0.76867, "alpha":6.31255, "fx":[171.82759,173.55293,164.77517,173.69581], "fy":[29.00327,-8.49855,-56.55273,-16.25134]}, - {"t":1.47056, "x":2.80912, "y":5.1581, "heading":-0.89219, "vx":-1.0006, "vy":-2.92596, "omega":2.80462, "ax":10.11784, "ay":-0.64569, "alpha":5.24108, "fx":[172.62697,173.78484,167.91056,174.0836], "fy":[24.53606,-9.11813,-46.91881,-12.43089]}, - {"t":1.48973, "x":2.79179, "y":5.10188, "heading":-0.83841, "vx":-0.80662, "vy":-2.93834, "omega":2.90511, "ax":10.17717, "ay":-0.49318, "alpha":4.07029, "fx":[173.36453,174.04459,170.65899,174.3749], "fy":[19.47471,-8.28014,-36.18727,-8.56248]}, - {"t":1.5089, "x":2.7782, "y":5.04545, "heading":-0.78272, "vx":-0.61149, "vy":-2.94779, "omega":2.98315, "ax":10.23301, "ay":-0.25631, "alpha":2.49876, "fx":[174.06999,174.3671,173.22732,174.57788], "fy":[12.80295,-4.89567,-21.40876,-3.93788]}, - {"t":1.52808, "x":2.76835, "y":4.98889, "heading":-0.72552, "vx":-0.4153, "vy":-2.95271, "omega":3.03105, "ax":10.26506, "ay":0.1383, "alpha":0.12479, "fx":[174.59298,174.6031,174.61639,174.61039], "fy":[3.20707,2.31261,1.49833,2.39208]}, - {"t":1.54725, "x":2.76228, "y":4.9323, "heading":-0.66741, "vx":-0.21849, "vy":-2.95006, "omega":3.03345, "ax":10.23097, "ay":0.57336, "alpha":-2.41945, "fx":[174.5749,174.43413,172.69444,174.39976], "fy":[-6.70072,10.72126,26.35797,8.63256]}, - {"t":1.56642, "x":2.75997, "y":4.87585, "heading":-0.60925, "vx":-0.02233, "vy":-2.93906, "omega":2.98706, "ax":10.14103, "ay":0.96364, "alpha":-4.66894, "fx":[174.13561,173.85921,167.90232,174.08677], "fy":[-14.99929,18.78125,48.47458,13.30875]}, - {"t":1.58559, "x":2.7614, "y":4.81967, "heading":-0.55198, "vx":0.1721, "vy":-2.92059, "omega":2.89754, "ax":10.02037, "ay":1.29333, "alpha":-6.56242, "fx":[173.54267,172.9642,161.42169,173.84622], "fy":[-21.3478,26.29426,67.11334,15.93708]}, - {"t":1.60477, "x":2.76655, "y":4.76392, "heading":-0.49642, "vx":0.36422, "vy":-2.89579, "omega":2.77172, "ax":9.94934, "ay":1.81928, "alpha":-6.51688, "fx":[174.58164,171.43778,157.94158,172.98055], "fy":[-11.86959,35.44303,75.26438,24.94392]}, - {"t":1.62993, "x":2.77886, "y":4.69161, "heading":-0.42667, "vx":0.61461, "vy":-2.85, "omega":2.60771, "ax":9.69604, "ay":2.71818, "alpha":-7.21456, "fx":[174.90673,167.53379,147.60739,169.6594], "fy":[-0.77148,50.60021,93.78537,41.32773]}, - {"t":1.6551, "x":2.7974, "y":4.62075, "heading":-0.36104, "vx":0.85864, "vy":-2.7816, "omega":2.42614, "ax":9.2192, "ay":3.90082, "alpha":-8.04584, "fx":[174.21692,160.68322,130.97096,161.39305], "fy":[14.346,69.1806,115.76095,66.12005]}, - {"t":1.68027, "x":2.82193, "y":4.55198, "heading":-0.29998, "vx":1.09066, "vy":-2.68342, "omega":2.22365, "ax":8.31476, "ay":5.4308, "alpha":-9.05592, "fx":[170.9936,148.94513,105.19621,140.59148], "fy":[35.63836,91.61903,139.49593,102.7523]}, - {"t":1.70544, "x":2.85201, "y":4.48616, "heading":-0.24401, "vx":1.29992, "vy":-2.54674, "omega":1.99573, "ax":6.65701, "ay":7.21517, "alpha":-10.49398, "fx":[161.44277,129.56447,68.95093,92.97693], "fy":[66.18181,117.32079,160.47305,146.93612]}, - {"t":1.7306, "x":2.88684, "y":4.42435, "heading":-0.19379, "vx":1.46746, "vy":-2.36516, "omega":1.73163, "ax":4.12559, "ay":8.77282, "alpha":-12.13169, "fx":[136.83628,99.93325,25.75086,18.17984], "fy":[107.89884,143.32231,172.74458,172.92713]}, - {"t":1.75577, "x":2.92508, "y":4.36761, "heading":-0.15021, "vx":1.57129, "vy":-2.14437, "omega":1.42631, "ax":1.19474, "ay":9.65711, "alpha":-11.85171, "fx":[85.22231,60.4345,-16.41859,-47.9496], "fy":[151.81837,163.90537,173.92611,167.40915]}, - {"t":1.78094, "x":2.965, "y":4.3167, "heading":-0.11431, "vx":1.60136, "vy":-1.90133, "omega":1.12803, "ax":-1.58657, "ay":9.77609, "alpha":-9.60914, "fx":[14.4751,16.78327,-51.16244,-88.04418], "fy":[173.55975,173.90311,167.11868,150.57222]}, - {"t":1.8061, "x":3.0048, "y":4.27194, "heading":-0.08592, "vx":1.56143, "vy":-1.65529, "omega":0.88619, "ax":-3.75385, "ay":9.31802, "alpha":-7.49341, "fx":[-44.63866,-23.21644,-77.05724,-110.4956], "fy":[168.5673,173.22053,156.96123,135.2384]}, - {"t":1.83127, "x":3.04291, "y":4.23323, "heading":-0.06362, "vx":1.46695, "vy":-1.42078, "omega":0.69761, "ax":-5.24027, "ay":8.67162, "alpha":-6.00161, "fx":[-81.9801,-55.21733,-95.65054,-123.69429], "fy":[154.14922,165.89625,146.4636,123.49793]}, - {"t":1.85644, "x":3.07817, "y":4.20022, "heading":-0.04606, "vx":1.33507, "vy":-1.20254, "omega":0.54656, "ax":-6.23796, "ay":8.0519, "alpha":-4.91297, "fx":[-104.33315,-79.06562,-108.99815,-132.02703], "fy":[140.19712,156.02902,136.90225,114.71396]}, - {"t":1.88161, "x":3.10979, "y":4.17251, "heading":-0.03231, "vx":1.17808, "vy":-0.99989, "omega":0.42292, "ax":-6.92352, "ay":7.51801, "alpha":-4.07835, "fx":[-118.24912,-96.44279,-118.74051,-137.63624], "fy":[128.84173,146.00756,128.61592,108.05184]}, - {"t":1.90677, "x":3.13725, "y":4.14972, "heading":-0.02166, "vx":1.00383, "vy":-0.81068, "omega":0.32027, "ax":-7.41073, "ay":7.07219, "alpha":-3.42168, "fx":[-127.42608,-109.16168,-126.01398,-141.61551], "fy":[119.90404,136.83069,121.56171,102.88723]}, - {"t":1.93194, "x":3.16016, "y":4.13156, "heading":-0.0136, "vx":0.81732, "vy":-0.6327, "omega":0.23416, "ax":-7.76864, "ay":6.70186, "alpha":-2.89603, "fx":[-133.80911,-118.62693,-131.57335,-144.56007], "fy":[112.84162,128.77437,115.5772,98.79394]}, - {"t":1.95711, "x":3.17827, "y":4.11776, "heading":-0.00771, "vx":0.62181, "vy":-0.46403, "omega":0.16127, "ax":-8.03956, "ay":6.39285, "alpha":-2.46885, "fx":[-138.45146,-125.8183,-135.91816,-146.81451], "fy":[107.18084,121.81347,110.48395,95.48359]}, - {"t":1.98228, "x":3.19138, "y":4.10811, "heading":-0.00365, "vx":0.41947, "vy":-0.30314, "omega":0.09914, "ax":-8.25005, "ay":6.13284, "alpha":-2.11671, "fx":[-141.95325,-131.39826,-139.3832,-148.58931], "fy":[102.56933,115.82163,106.12236,92.75833]}, - {"t":2.00744, "x":3.19932, "y":4.10242, "heading":-0.00115, "vx":0.21184, "vy":-0.14879, "omega":0.04587, "ax":-8.41733, "ay":5.91202, "alpha":-1.82255, "fx":[-144.6746,-135.8144,-142.19708,-150.01912], "fy":[98.75398,110.65363,102.3596,90.47954]}, - {"t":2.03261, "x":3.20199, "y":4.10055, "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.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.95492, "ay":-0.67683, "alpha":51.78215, "fx":[30.93604,52.64672,-184.60707,-162.6819], "fy":[-196.61836,191.69748,74.7142,-114.92336]}, + {"t":0.03105, "x":7.09898, "y":7.2567, "heading":3.14159, "vx":-0.12282, "vy":-0.02102, "omega":1.60808, "ax":-4.10988, "ay":-0.71741, "alpha":51.28451, "fx":[26.50708,47.6008,-185.72063,-162.42584], "fy":[-197.16732,192.80096,71.71914,-115.18822]}, + {"t":0.06211, "x":7.09318, "y":7.25571, "heading":-3.09165, "vx":-0.25045, "vy":-0.0433, "omega":3.2007, "ax":-4.49168, "ay":-0.89963, "alpha":50.04431, "fx":[27.09084,21.36427,-189.59334,-158.35831], "fy":[-196.89658,197.08566,60.37868,-120.55331]}, + {"t":0.09316, "x":7.08324, "y":7.25393, "heading":-2.99226, "vx":-0.38994, "vy":-0.07124, "omega":4.75482, "ax":-5.4052, "ay":-1.44843, "alpha":46.58791, "fx":[24.1256,-39.01018,-195.27286,-150.25117], "fy":[-196.81635,193.51502,36.89364,-130.17104]}, + {"t":0.12422, "x":7.06853, "y":7.25102, "heading":-2.8446, "vx":-0.55779, "vy":-0.11622, "omega":6.20159, "ax":-8.55668, "ay":-4.86313, "alpha":24.78681, "fx":[-43.93903,-191.22024,-195.74378,-139.64014], "fy":[-191.58159,34.9083,-27.44185,-140.14902]}, + {"t":0.15527, "x":7.04708, "y":7.24506, "heading":-2.65201, "vx":-0.82352, "vy":-0.26724, "omega":6.97134, "ax":-8.05823, "ay":-5.44736, "alpha":-24.23166, "fx":[-188.81219,-123.74288,-37.50132,-187.25065], "fy":[-48.67402,-150.97072,-187.35503,23.78009]}, + {"t":0.18633, "x":7.01762, "y":7.23414, "heading":-2.43552, "vx":-1.07377, "vy":-0.43641, "omega":6.21883, "ax":-8.91458, "ay":-1.7378, "alpha":-25.17613, "fx":[-194.68842,-145.29838,-108.79507,-145.62487], "fy":[0.18736,-127.3866,-114.24267,125.56852]}, + {"t":0.21738, "x":6.97997, "y":7.21975, "heading":-2.24239, "vx":-1.35061, "vy":-0.49037, "omega":5.43699, "ax":-9.56524, "ay":2.20078, "alpha":-21.29643, "fx":[-193.94642,-162.58175,-131.86972,-149.39414], "fy":[8.60586,-100.32684,116.39276,122.07219]}, + {"t":0.24844, "x":6.93342, "y":7.20558, "heading":-2.07355, "vx":-1.64765, "vy":-0.42203, "omega":4.77564, "ax":-9.92196, "ay":2.22204, "alpha":-18.98559, "fx":[-193.02903,-170.81043,-137.48311,-160.25443], "fy":[2.91705,-79.95157,118.98573,106.21017]}, + {"t":0.27949, "x":6.87747, "y":7.19354, "heading":-1.92524, "vx":-1.95577, "vy":-0.35302, "omega":4.18605, "ax":-10.02576, "ay":1.59694, "alpha":-18.11939, "fx":[-190.8562,-166.61741,-142.55501,-168.46993], "fy":[-11.83088,-80.58663,109.85594,89.04237]}, + {"t":0.31055, "x":6.8119, "y":7.18335, "heading":-1.79525, "vx":-2.26712, "vy":-0.30343, "omega":3.62335, "ax":-9.83004, "ay":0.46222, "alpha":-18.41763, "fx":[-184.95654,-150.73405,-145.53392,-174.22378], "fy":[-33.85082,-97.1029,93.30797,68.4657]}, + {"t":0.3416, "x":6.73675, "y":7.17415, "heading":-1.68272, "vx":-2.57239, "vy":-0.28908, "omega":3.0514, "ax":-9.14167, "ay":-1.11128, "alpha":-19.08277, "fx":[-171.73147,-120.01236,-142.32664,-175.47874], "fy":[-60.90634,-120.14363,64.54674,42.40526]}, + {"t":0.37266, "x":6.65246, "y":7.16464, "heading":-1.58796, "vx":-2.85628, "vy":-0.32359, "omega":2.45879, "ax":-7.70158, "ay":-2.86354, "alpha":-18.66998, "fx":[-147.81342,-78.69138,-120.40704,-166.61452], "fy":[-86.49898,-133.37913,18.3296,10.61346]}, + {"t":0.40371, "x":6.56004, "y":7.15321, "heading":-1.51161, "vx":-3.09545, "vy":-0.41251, "omega":1.879, "ax":-5.59057, "ay":-3.98975, "alpha":-15.88503, "fx":[-114.0989,-44.62143,-75.19449,-138.85376], "fy":[-97.81827,-122.86995,-24.87909,-20.4615]}, + {"t":0.43477, "x":6.46122, "y":7.13847, "heading":-1.45325, "vx":-3.26907, "vy":-0.53641, "omega":1.38569, "ax":-3.36874, "ay":-3.70641, "alpha":-10.97211, "fx":[-74.26447,-23.00691,-36.56698,-90.78306], "fy":[-85.74985,-91.20788,-34.73035,-35.44819]}, + {"t":0.46582, "x":6.35808, "y":7.12003, "heading":-1.41022, "vx":-3.37368, "vy":-0.65152, "omega":1.04496, "ax":-1.59663, "ay":-2.70255, "alpha":-5.93932, "fx":[-37.68861,-9.09574,-14.91369,-44.76223], "fy":[-60.07401,-57.9175,-29.38334,-32.82618]}, + {"t":0.49688, "x":6.25254, "y":7.09849, "heading":-1.37777, "vx":-3.42326, "vy":-0.73544, "omega":0.86051, "ax":-0.5726, "ay":-2.18721, "alpha":-2.93692, "fx":[-15.14523,-0.87255,-3.78911,-18.3728], "fy":[-44.44224,-42.12656,-28.37745,-30.8927]}, + {"t":0.52793, "x":6.14595, "y":7.0746, "heading":-1.35105, "vx":-3.44105, "vy":-0.80337, "omega":0.76931, "ax":-0.01665, "ay":-2.13277, "alpha":-1.49397, "fx":[-3.09884,4.18365,2.58782,-4.78273], "fy":[-39.74736,-38.23351,-31.33861,-32.88952]}, + {"t":0.55898, "x":6.03908, "y":7.04862, "heading":-1.32716, "vx":-3.44156, "vy":-0.8696, "omega":0.72291, "ax":0.10607, "ay":-2.16166, "alpha":-1.34612, "fx":[-0.69484,5.84371,4.26768,-2.34401], "fy":[-39.87656,-38.34115,-32.17576,-33.74155]}, + {"t":0.59004, "x":5.93226, "y":7.02058, "heading":-1.30471, "vx":-3.43827, "vy":-0.93673, "omega":0.68111, "ax":-1.09495, "ay":-2.7119, "alpha":-5.5505, "fx":[-27.22308,-0.82357,-8.77734,-36.18519], "fy":[-60.43019,-55.24429,-29.41231,-35.73753]}, + {"t":0.62109, "x":5.82496, "y":6.99018, "heading":-1.28356, "vx":-3.47227, "vy":-1.02094, "omega":0.50874, "ax":-2.66575, "ay":-6.60205, "alpha":-14.56468, "fx":[-62.44385,12.70679,-21.05851,-106.95121], "fy":[-137.41208,-138.77461,-82.10064,-81.92477]}, + {"t":0.65215, "x":5.71584, "y":6.95529, "heading":-1.26776, "vx":-3.55506, "vy":-1.22597, "omega":0.05644, "ax":5.44094, "ay":-3.93804, "alpha":11.57852, "fx":[119.29546,61.75819,65.75096,115.98709], "fy":[-21.02874,-56.77799,-108.71541,-76.05859]}, + {"t":0.66959, "x":5.65468, "y":6.93331, "heading":-1.26677, "vx":-3.46018, "vy":-1.29464, "omega":0.25833, "ax":2.69162, "ay":-2.60889, "alpha":4.6072, "fx":[53.90145,31.20869,36.51001,57.85191], "fy":[-28.67645,-37.50133,-58.2964,-49.48159]}, + {"t":0.68702, "x":5.59475, "y":6.91034, "heading":-1.26227, "vx":-3.41325, "vy":-1.34013, "omega":0.33867, "ax":0.98237, "ay":-1.32915, "alpha":1.23395, "fx":[18.41847,12.60201,14.35759,20.12441], "fy":[-18.36616,-20.28579,-25.94607,-24.0273]}, + {"t":0.70446, "x":5.53538, "y":6.88677, "heading":-1.25636, "vx":-3.39612, "vy":-1.3633, "omega":0.36019, "ax":0.35027, "ay":-0.64872, "alpha":0.29045, "fx":[6.29672,4.94664,5.3814,6.73025], "fy":[-9.92276,-10.36372,-11.70471,-11.26392]}, + {"t":0.7219, "x":5.47622, "y":6.8629, "heading":-1.25008, "vx":-3.39001, "vy":-1.37462, "omega":0.36525, "ax":0.15382, "ay":-0.32686, "alpha":0.13547, "fx":[2.77361,2.14686,2.35462,2.98124], "fy":[-5.03157,-5.23992,-5.86558,-5.65726]}, + {"t":0.73933, "x":5.41713, "y":6.83888, "heading":-1.24371, "vx":-3.38733, "vy":-1.38032, "omega":0.36761, "ax":0.10139, "ay":-0.18145, "alpha":0.16742, "fx":[1.94539,1.17293,1.43481,2.20716], "fy":[-2.50767,-2.76982,-3.54183,-3.2797]}, + {"t":0.75677, "x":5.35808, "y":6.81479, "heading":-1.2373, "vx":-3.38556, "vy":-1.38348, "omega":0.37053, "ax":0.09516, "ay":-0.12538, "alpha":0.22007, "fx":[1.91747,0.90447,1.25527,2.26812], "fy":[-1.40811,-1.75914,-2.77186,-2.42083]}, + {"t":0.77421, "x":5.29906, "y":6.79064, "heading":-1.23084, "vx":-3.3839, "vy":-1.38567, "omega":0.37437, "ax":0.1044, "ay":-0.11809, "alpha":0.2567, "fx":[2.12134,0.94241,1.35921,2.53794], "fy":[-1.17064,-1.58773,-2.76637,-2.34928]}, + {"t":0.79164, "x":5.24007, "y":6.76647, "heading":-1.22431, "vx":-3.38208, "vy":-1.38772, "omega":0.37885, "ax":0.11987, "ay":-0.1405, "alpha":0.27412, "fx":[2.3997,1.14364,1.5969,2.85268], "fy":[-1.48738,-1.94106,-3.19667,-2.74299]}, + {"t":0.80908, "x":5.18112, "y":6.74225, "heading":-1.21771, "vx":-3.37999, "vy":-1.39017, "omega":0.38363, "ax":0.13911, "ay":-0.1833, "alpha":0.27586, "fx":[2.71732,1.45617,1.92054,3.18135], "fy":[-2.19286,-2.65787,-3.91825,-3.45326]}, + {"t":0.82652, "x":5.1222, "y":6.71798, "heading":-1.21102, "vx":-3.37757, "vy":-1.39337, "omega":0.38844, "ax":0.16157, "ay":-0.24131, "alpha":0.2659, "fx":[3.07196,1.85906,2.31467,3.52716], "fy":[-3.18846,-3.64501,-4.85664,-4.40014]}, + {"t":0.84396, "x":5.06333, "y":6.69364, "heading":-1.20425, "vx":-3.37475, "vy":-1.39758, "omega":0.39307, "ax":0.18715, "ay":-0.3104, "alpha":0.24818, "fx":[3.46815,2.33862,2.77133,3.90041], "fy":[-4.39342,-4.82741,-5.955,-5.5211]}, + {"t":0.86139, "x":5.00452, "y":6.66923, "heading":-1.19739, "vx":-3.37149, "vy":-1.40299, "omega":0.3974, "ax":0.21577, "ay":-0.38593, "alpha":0.22742, "fx":[3.91153,2.87879,3.28216,4.31446], "fy":[-5.71565,-6.12068,-7.15073,-6.7458]}, + {"t":0.87883, "x":4.94576, "y":6.64471, "heading":-1.19046, "vx":-3.36772, "vy":-1.40972, "omega":0.40137, "ax":0.24779, "ay":-0.46242, "alpha":0.21096, "fx":[4.41841,3.4625,3.84304,4.7985], "fy":[-7.04076,-7.42338,-8.37575,-7.99326]}, + {"t":0.89627, "x":4.88707, "y":6.62005, "heading":-1.18346, "vx":-3.3634, "vy":-1.41778, "omega":0.40504, "ax":0.28602, "ay":-0.53514, "alpha":0.21275, "fx":[5.05388,4.09198,4.48215,5.44352], "fy":[-8.24541,-8.63838,-9.59552,-9.2027]}, + {"t":0.9137, "x":4.82847, "y":6.59525, "heading":-1.1764, "vx":-3.35842, "vy":-1.42711, "omega":0.40875, "ax":0.34141, "ay":-0.60514, "alpha":0.26373, "fx":[6.04058,4.85074,5.34221,6.53112], "fy":[-9.2481,-9.74436,-10.92661,-10.43062]}, + {"t":0.93114, "x":4.76996, "y":6.57027, "heading":-1.16927, "vx":-3.35246, "vy":-1.43767, "omega":0.41335, "ax":0.44713, "ay":-0.69195, "alpha":0.43655, "fx":[8.02432,6.05829,6.88394,8.84693], "fy":[-10.14057,-10.97875,-12.92808,-12.09063]}, + {"t":0.94858, "x":4.71157, "y":6.5451, "heading":-1.16207, "vx":-3.34467, "vy":-1.44973, "omega":0.42096, "ax":0.68943, "ay":-0.86446, "alpha":0.89424, "fx":[12.65568,8.6283,10.33803,14.34804], "fy":[-11.54161,-13.30405,-17.27729,-15.51764]}, + {"t":0.96601, "x":4.65336, "y":6.51969, "heading":-1.15473, "vx":-3.33264, "vy":-1.46481, "omega":0.43656, "ax":1.26349, "ay":-1.31765, "alpha":1.97009, "fx":[23.69517,14.75018,18.49732,27.30415], "fy":[-15.58274,-19.66892,-28.33854,-24.26812]}, + {"t":0.98345, "x":4.59544, "y":6.49395, "heading":-1.14711, "vx":-3.31061, "vy":-1.48778, "omega":0.47091, "ax":0.02486, "ay":-1.88231, "alpha":-2.36135, "fx":[-2.50424,8.17048,3.40502,-7.41365], "fy":[-38.76154,-34.21428,-23.92092,-28.61177]}, + {"t":1.00173, "x":4.53493, "y":6.46644, "heading":-1.13851, "vx":-3.31016, "vy":-1.52218, "omega":0.42775, "ax":0.18569, "ay":-1.05605, "alpha":-0.87103, "fx":[2.0456,5.92536,4.15149,0.25911], "fy":[-20.39764,-18.62823,-14.80504,-16.58459]}, + {"t":1.02001, "x":4.47446, "y":6.43844, "heading":-1.13069, "vx":-3.30676, "vy":-1.54149, "omega":0.41183, "ax":0.25541, "ay":-0.73474, "alpha":-0.27545, "fx":[3.93334,5.1521,4.58233,3.36256], "fy":[-13.13809,-12.56554,-11.35703,-11.93019]}, + {"t":1.03828, "x":4.41407, "y":6.41015, "heading":-1.12316, "vx":-3.3021, "vy":-1.55492, "omega":0.4068, "ax":0.26691, "ay":-0.59144, "alpha":-0.07708, "fx":[4.36063,4.70003,4.53801,4.19854], "fy":[-10.1092,-9.9463,-9.60889,-9.77182]}, + {"t":1.05656, "x":4.35376, "y":6.38163, "heading":-1.11573, "vx":-3.29722, "vy":-1.56572, "omega":0.40539, "ax":0.24598, "ay":-0.50148, "alpha":-0.03843, "fx":[4.05713,4.22562,4.14354,3.97503], "fy":[-8.48451,-8.40207,-8.2343,-8.31675]}, + {"t":1.07484, "x":4.29354, "y":6.35293, "heading":-1.10832, "vx":-3.29272, "vy":-1.57489, "omega":0.40469, "ax":0.20875, "ay":-0.42517, "alpha":-0.06, "fx":[3.41394,3.67589,3.54572,3.28374], "fy":[-7.28325,-7.15268,-6.89154,-7.02213]}, + {"t":1.09311, "x":4.23339, "y":6.32407, "heading":-1.10092, "vx":-3.28891, "vy":-1.58266, "omega":0.40359, "ax":0.16716, "ay":-0.35472, "alpha":-0.09712, "fx":[2.68229,3.10448,2.89054,2.46829], "fy":[-6.2309,-6.01654,-5.59524,-5.80963]}, + {"t":1.11139, "x":4.17331, "y":6.29509, "heading":-1.09354, "vx":-3.28585, "vy":-1.58914, "omega":0.40181, "ax":0.13188, "ay":-0.29303, "alpha":-0.12359, "fx":[2.06901,2.60408,2.32777,1.79263], "fy":[-5.2901,-5.01346,-4.47915,-4.75583]}, + {"t":1.12967, "x":4.11327, "y":6.26599, "heading":-1.0862, "vx":-3.28344, "vy":-1.5945, "omega":0.39956, "ax":0.11443, "ay":-0.24619, "alpha":-0.1161, "fx":[1.78878,2.2894,2.02611,1.52543], "fy":[-4.48561,-4.22209,-3.72196,-3.98552]}, + {"t":1.14795, "x":4.05328, "y":6.23681, "heading":-1.0789, "vx":-3.28135, "vy":-1.599, "omega":0.39743, "ax":0.13004, "ay":-0.2219, "alpha":-0.04386, "fx":[2.124,2.31237,2.21154,2.02316], "fy":[-3.84357,-3.74263,-3.55441,-3.65536]}, + {"t":1.16622, "x":3.99333, "y":6.20755, "heading":-1.07163, "vx":-3.27897, "vy":-1.60306, "omega":0.39663, "ax":0.20328, "ay":-0.23033, "alpha":0.14533, "fx":[3.53026,2.90845,3.24697,3.86866], "fy":[-3.35921,-3.69844,-4.31963,-3.98043]}, + {"t":1.1845, "x":3.93343, "y":6.17821, "heading":-1.06438, "vx":-3.27526, "vy":-1.60727, "omega":0.39929, "ax":0.37763, "ay":-0.28526, "alpha":0.54898, "fx":[6.81874,4.47815,5.7724,8.11017], "fy":[-2.93588,-4.23701,-6.57415,-5.27329]}, + {"t":1.20278, "x":3.87363, "y":6.14879, "heading":-1.05709, "vx":-3.26836, "vy":-1.61248, "omega":0.40932, "ax":0.72893, "ay":-0.40306, "alpha":1.3454, "fx":[13.41698,7.69658,10.89983,16.59056], "fy":[-2.2388,-5.49018,-11.19912,-7.94683]}, + {"t":1.22105, "x":3.81402, "y":6.11925, "heading":-1.04961, "vx":-3.25503, "vy":-1.61985, "omega":0.43391, "ax":1.36871, "ay":-0.59532, "alpha":2.807, "fx":[25.47551,13.55503,20.26776,31.96494], "fy":[-0.47061,-7.47283,-19.38797,-12.36324]}, + {"t":1.23933, "x":3.75476, "y":6.08954, "heading":-1.04168, "vx":-3.23002, "vy":-1.63073, "omega":0.48522, "ax":2.38263, "ay":-0.85579, "alpha":5.14327, "fx":[44.82648,22.99652,35.18744,55.85898], "fy":[3.53416,-10.06709,-32.1431,-18.38622]}, + {"t":1.25761, "x":3.69612, "y":6.05959, "heading":-1.03281, "vx":-3.18647, "vy":-1.64637, "omega":0.57922, "ax":3.66771, "ay":-1.17022, "alpha":8.04829, "fx":[69.82707,35.91784,54.23782,84.57335], "fy":[9.80739,-13.62864,-49.03975,-25.16706]}, + {"t":1.27589, "x":3.63849, "y":6.02931, "heading":-1.02222, "vx":-3.11943, "vy":-1.66776, "omega":0.72632, "ax":4.92435, "ay":-1.59239, "alpha":10.6019, "fx":[94.69868,50.88543,72.63852,110.12385], "fy":[14.88398,-20.58028,-67.90203,-32.57898]}, + {"t":1.29416, "x":3.5823, "y":5.99856, "heading":-1.00895, "vx":-3.02943, "vy":-1.69686, "omega":0.92009, "ax":5.935, "ay":-2.31954, "alpha":11.94137, "fx":[115.4052,66.27067,86.20658,127.85211], "fy":[11.99933,-36.74335,-87.10152,-42.81663]}, + {"t":1.31244, "x":3.52792, "y":5.96716, "heading":-0.99213, "vx":-2.92096, "vy":-1.73925, "omega":1.13835, "ax":6.6182, "ay":-3.65447, "alpha":11.42174, "fx":[131.11213,79.84156,93.67199,136.66318], "fy":[-8.72619,-68.33473,-106.5571,-60.05489]}, + {"t":1.33072, "x":3.47564, "y":5.93476, "heading":-0.97132, "vx":-2.79999, "vy":-1.80605, "omega":1.3471, "ax":6.83366, "ay":-5.72231, "alpha":8.30488, "fx":[135.78563,88.85622,96.518,134.49538], "fy":[-58.50403,-108.71701,-125.66355,-88.6683]}, + {"t":1.34899, "x":3.42561, "y":5.90079, "heading":-0.9467, "vx":-2.6751, "vy":-1.91064, "omega":1.49889, "ax":3.59337, "ay":-7.27554, "alpha":-4.84872, "fx":[51.5643,78.79716,70.58669,38.65067], "fy":[-133.60339,-117.78215,-107.85344,-125.88002]}, + {"t":1.3695, "x":3.37151, "y":5.86009, "heading":-0.91597, "vx":-2.60142, "vy":-2.05981, "omega":1.39948, "ax":1.32861, "ay":-5.75444, "alpha":-7.76522, "fx":[16.17287,52.64146,30.70048,-10.92533], "fy":[-116.32815,-96.65772,-73.89005,-96.81915]}, + {"t":1.39, "x":3.31846, "y":5.81665, "heading":-0.88728, "vx":-2.57418, "vy":-2.17779, "omega":1.24026, "ax":0.58865, "ay":-4.44236, "alpha":-7.09012, "fx":[6.53816,36.71499,14.06263,-18.06601], "fy":[-94.54709,-75.99879,-52.17165,-73.4906]}, + {"t":1.4105, "x":3.2658, "y":5.77106, "heading":-0.86185, "vx":-2.56211, "vy":-2.26887, "omega":1.09489, "ax":0.63059, "ay":-3.53792, "alpha":-5.47565, "fx":[8.5656,30.61271,12.89129,-10.0232], "fy":[-75.86768,-60.09839,-41.29332,-58.64206]}, + {"t":1.43101, "x":3.2134, "y":5.7238, "heading":-0.8394, "vx":-2.54918, "vy":-2.34141, "omega":0.98263, "ax":0.88553, "ay":-3.02939, "alpha":-4.14653, "fx":[13.5317,29.69236,16.24317,-0.42192], "fy":[-63.74794,-50.90007,-36.82357,-50.52208]}, + {"t":1.45151, "x":3.16132, "y":5.67516, "heading":-0.81925, "vx":-2.53102, "vy":-2.40353, "omega":0.89761, "ax":1.31497, "ay":-3.046, "alpha":-3.53415, "fx":[20.91818,34.58085,23.15533,9.02544], "fy":[-62.16479,-50.64486,-39.06211,-51.22936]}, + {"t":1.47201, "x":3.1097, "y":5.62524, "heading":-0.80085, "vx":-2.50406, "vy":-2.46598, "omega":0.82515, "ax":3.04565, "ay":-4.22942, "alpha":-3.00937, "fx":[48.92563,61.52675,53.00829,39.61754], "fy":[-80.09058,-68.96896,-60.54804,-72.40211]}, + {"t":1.49252, "x":3.059, "y":5.57379, "heading":-0.78393, "vx":-2.44162, "vy":-2.5527, "omega":0.76344, "ax":6.51989, "ay":-5.62195, "alpha":-0.34215, "fx":[108.08596,109.76801,109.29039,107.58911], "fy":[-94.97673,-93.11536,-92.4422,-94.32675]}, + {"t":1.51302, "x":3.01031, "y":5.52027, "heading":-0.76828, "vx":-2.30794, "vy":-2.66796, "omega":0.75643, "ax":8.78424, "ay":-5.43055, "alpha":2.56013, "fx":[152.39034,140.23218,140.88524,152.20854], "fy":[-78.3855,-97.11612,-101.67075,-84.92635]}, + {"t":1.53352, "x":2.96484, "y":5.46442, "heading":-0.75277, "vx":-2.12783, "vy":-2.77931, "omega":0.80892, "ax":9.95079, "ay":-4.73168, "alpha":4.06087, "fx":[174.95652,158.82341,157.23569,172.48424], "fy":[-56.36412,-90.28821,-98.59956,-70.2472]}, + {"t":1.55403, "x":2.9233, "y":5.40644, "heading":-0.73618, "vx":-1.92381, "vy":-2.87632, "omega":0.89218, "ax":10.60561, "ay":-4.02376, "alpha":4.51458, "fx":[185.57975,171.27395,167.90176,182.40617], "fy":[-39.87259,-79.56826,-90.81857,-58.03725]}, + {"t":1.57453, "x":2.88609, "y":5.34662, "heading":-0.71789, "vx":-1.70636, "vy":-2.95882, "omega":0.98475, "ax":11.01583, "ay":-3.39297, "alpha":4.35766, "fx":[190.79178,179.89509,175.91152,187.91597], "fy":[-29.08995,-67.95884,-80.91213,-48.27553]}, + {"t":1.59503, "x":2.85342, "y":5.28525, "heading":-0.6977, "vx":-1.4805, "vy":-3.02839, "omega":1.07409, "ax":11.29502, "ay":-2.8338, "alpha":3.84744, "fx":[193.55058,185.96003,182.31062,191.3091], "fy":[-22.37822,-56.59997,-69.78131,-40.19314]}, + {"t":1.61554, "x":2.82543, "y":5.22256, "heading":-0.67568, "vx":-1.24892, "vy":-3.08649, "omega":1.15298, "ax":11.49457, "ay":-2.32896, "alpha":3.12238, "fx":[195.12621,190.26186,187.49133,193.55644], "fy":[-18.38336,-45.85843,-57.81312,-33.23537]}, + {"t":1.63604, "x":2.80224, "y":5.15879, "heading":-0.65204, "vx":-1.01324, "vy":-3.13424, "omega":1.21699, "ax":11.63968, "ay":-1.86405, "alpha":2.25958, "fx":[196.08506,193.31152,191.6008,195.114], "fy":[-16.19509,-35.79649,-45.23011,-27.06955]}, + {"t":1.65654, "x":2.78392, "y":5.09413, "heading":-0.62708, "vx":-0.77459, "vy":-3.17246, "omega":1.26332, "ax":11.74369, "ay":-1.42911, "alpha":1.30557, "fx":[196.69552,195.44327,194.68971,196.21804], "fy":[-15.21678,-26.36755,-32.19944,-21.50661]}, + {"t":1.67705, "x":2.7705, "y":5.02879, "heading":-0.60118, "vx":-0.5338, "vy":-3.20176, "omega":1.29009, "ax":11.81422, "ay":-1.01773, "alpha":0.29127, "fx":[197.09351,196.87901,196.77366,197.00307], "fy":[-15.04934,-17.49805,-18.86992,-16.44298]}, + {"t":1.69755, "x":2.76204, "y":4.96292, "heading":-0.57473, "vx":-0.29157, "vy":-3.22263, "omega":1.29606, "ax":11.85603, "ay":-0.62606, "alpha":-0.76109, "fx":[197.35287,197.76941,197.86145,197.55325], "fy":[-15.41842,-9.11927,-5.38243,-11.82412]}, + {"t":1.71805, "x":2.75855, "y":4.89672, "heading":-0.54816, "vx":-0.04849, "vy":-3.23547, "omega":1.28046, "ax":11.87238, "ay":-0.25074, "alpha":-1.84108, "fx":[197.51528,198.22061,197.96549,197.92637], "fy":[-16.15183,-1.14838,8.19335,-7.61189]}, + {"t":1.73856, "x":2.76006, "y":4.83033, "heading":-0.5219, "vx":0.19493, "vy":-3.24061, "omega":1.24271, "ax":11.86198, "ay":0.1395, "alpha":-3.09079, "fx":[197.56691,198.28801,196.91689,198.1619], "fy":[-17.59537,7.12963,23.32155,-3.55426]}, + {"t":1.75906, "x":2.76655, "y":4.76392, "heading":-0.49642, "vx":0.43814, "vy":-3.23775, "omega":1.17934, "ax":11.83707, "ay":0.97228, "alpha":-2.91618, "fx":[198.49042,197.56022,195.17471,198.04756], "fy":[-2.89727,19.99649,36.04449,11.68576]}, + {"t":1.78412, "x":2.78124, "y":4.68308, "heading":-0.46687, "vx":0.73477, "vy":-3.21338, "omega":1.10626, "ax":11.68988, "ay":2.06257, "alpha":-2.68007, "fx":[197.61903,194.94004,191.23693,195.66281], "fy":[16.6735,36.85516,52.42613,31.57373]}, + {"t":1.80918, "x":2.80333, "y":4.60321, "heading":-0.43915, "vx":1.02771, "vy":-3.1617, "omega":1.0391, "ax":11.33561, "ay":3.5309, "alpha":-1.51213, "fx":[191.904,189.02195,185.74833,189.16263], "fy":[48.9752,59.33466,68.70844,58.4151]}, + {"t":1.83424, "x":2.83264, "y":4.52509, "heading":-0.41311, "vx":1.31177, "vy":-3.07321, "omega":1.00121, "ax":10.48305, "ay":5.53731, "alpha":1.2126, "fx":[170.85783,174.30091,178.52756,175.30342], "fy":[99.51184,93.17745,84.95111,91.57674]}, + {"t":1.8593, "x":2.8688, "y":4.44981, "heading":-0.38802, "vx":1.57447, "vy":-2.93445, "omega":1.0316, "ax":8.506, "ay":8.03332, "alpha":6.21684, "fx":[116.37562,129.89281,168.89442,152.00098], "fy":[159.57465,147.88008,101.81866,126.37304]}, + {"t":1.88435, "x":2.91093, "y":4.3788, "heading":-0.36217, "vx":1.78762, "vy":-2.73314, "omega":1.18739, "ax":4.70199, "ay":10.22147, "alpha":13.02472, "fx":[38.37325,13.05353,144.96266,117.13016], "fy":[193.84929,195.84957,132.64244,159.20672]}, + {"t":1.90941, "x":2.9572, "y":4.31352, "heading":-0.33241, "vx":1.90545, "vy":-2.477, "omega":1.51378, "ax":-0.27987, "ay":11.56826, "alpha":8.71211, "fx":[-24.17547,-62.76981,27.30532,40.97864], "fy":[196.31647,186.93041,194.76278,193.33941]}, + {"t":1.93447, "x":3.00486, "y":4.25508, "heading":-0.29448, "vx":1.89844, "vy":-2.18711, "omega":1.73209, "ax":-4.09965, "ay":11.13589, "alpha":1.22189, "fx":[-69.42679,-76.11554,-67.21911,-60.59517], "fy":[185.38423,182.66751,186.06445,188.40384]}, + {"t":1.95953, "x":3.05115, "y":4.20377, "heading":-0.25107, "vx":1.7957, "vy":-1.90805, "omega":1.76271, "ax":-6.01442, "ay":10.22391, "alpha":-2.99114, "fx":[-100.41174,-82.34112,-100.62277,-117.65446], "fy":[170.80633,180.36333,170.95372,159.58755]}, + {"t":1.98459, "x":3.09426, "y":4.15916, "heading":-0.2069, "vx":1.64499, "vy":-1.65185, "omega":1.68776, "ax":-7.02939, "ay":9.48109, "alpha":-5.40589, "fx":[-120.28739,-86.06452,-116.77202,-145.5826], "fy":[157.6418,178.8768,160.69147,134.9711]}, + {"t":2.00965, "x":3.13327, "y":4.12075, "heading":-0.16461, "vx":1.46884, "vy":-1.41426, "omega":1.55229, "ax":-7.62902, "ay":8.93006, "alpha":-6.90031, "fx":[-132.96588,-88.6368,-126.50402,-160.58186], "fy":[147.28181,177.79315,153.35423,117.01036]}, + {"t":2.03471, "x":3.16769, "y":4.08811, "heading":-0.12571, "vx":1.27766, "vy":-1.19048, "omega":1.37937, "ax":-8.01662, "ay":8.51872, "alpha":-7.89637, "fx":[-141.22164,-90.59048,-133.15941,-169.5615], "fy":[139.53629,176.9329,147.75248,103.79007]}, + {"t":2.05977, "x":3.19719, "y":4.06095, "heading":-0.09114, "vx":1.07677, "vy":-0.97701, "omega":1.1815, "ax":-8.2846, "ay":8.20446, "alpha":-8.5986, "fx":[-146.77603,-92.16172,-138.05304,-175.41049], "fy":[133.80948,176.21475,143.29228,93.74103]}, + {"t":2.08483, "x":3.22157, "y":4.03905, "heading":-0.06154, "vx":0.86916, "vy":-0.77141, "omega":0.96602, "ax":-8.47954, "ay":7.95839, "alpha":-9.11648, "fx":[-150.68128,-93.46007,-141.79802,-179.4599], "fy":[129.50504,175.60295,139.66439,85.87766]}, + {"t":2.10989, "x":3.24069, "y":4.02221, "heading":-0.03733, "vx":0.65667, "vy":-0.57198, "omega":0.73757, "ax":-8.6271, "ay":7.76116, "alpha":-9.51368, "fx":[-153.59771,-94.53625,-144.7168,-182.38741], "fy":[126.1259,175.08378,136.69813,79.59176]}, + {"t":2.13495, "x":3.25443, "y":4.01032, "heading":-0.01884, "vx":0.44048, "vy":-0.37749, "omega":0.49917, "ax":-8.74248, "ay":7.59959, "alpha":-9.82957, "fx":[-155.95564,-95.41283,-146.99472,-184.56842], "fy":[123.27947,174.65409,134.29425,74.498]}, + {"t":2.16001, "x":3.26273, "y":4.00324, "heading":-0.00634, "vx":0.2214, "vy":-0.18705, "omega":0.25284, "ax":-8.83522, "ay":7.4644, "alpha":-10.08985, "fx":[-158.04558,-96.09911,-148.74546,-186.22533], "fy":[120.65961,174.31526,132.39246,70.34432]}, + {"t":2.18507, "x":3.2655, "y":4.0009, "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/startDeepToK.traj b/src/main/deploy/choreo/startDeepToK.traj index 869fb520..09605127 100644 --- a/src/main/deploy/choreo/startDeepToK.traj +++ b/src/main/deploy/choreo/startDeepToK.traj @@ -3,10 +3,10 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.25449800491333, "y":6.856431007385254, "heading":-1.348714396235503, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.25449800491333, "y":6.856431007385254, "heading":-1.348714396235503, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.315337657928467, "y":6.098512172698975, "heading":-1.231504335673354, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "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}, @@ -16,10 +16,10 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.25449800491333 m", "val":5.25449800491333}, "y":{"exp":"6.856431007385254 m", "val":6.856431007385254}, "heading":{"exp":"-1.348714396235503 rad", "val":-1.348714396235503}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.25449800491333 m", "val":5.25449800491333}, "y":{"exp":"6.856431007385254 m", "val":6.856431007385254}, "heading":{"exp":"-1.348714396235503 rad", "val":-1.348714396235503}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.315337657928467 m", "val":4.315337657928467}, "y":{"exp":"6.098512172698975 m", "val":6.098512172698975}, "heading":{"exp":"-1.231504335673354 rad", "val":-1.231504335673354}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "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}, @@ -32,72 +32,70 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.73383,1.04227,1.44583], + "waypoints":[0.0,0.78448,1.1217,1.51614], "samples":[ - {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.67919, "ay":-0.64602, "alpha":37.62249, "fx":[-23.7765,-45.4132,-167.41749,-149.79852], "fy":[-173.45579,168.70739,51.78483,-90.99091]}, - {"t":0.02367, "x":7.0993, "y":7.25685, "heading":3.14159, "vx":-0.13444, "vy":-0.01529, "omega":0.8906, "ax":-6.25804, "ay":-0.78162, "alpha":34.95301, "fx":[-36.02302,-70.41092,-168.59917,-150.75706], "fy":[-171.27185,159.75125,47.67643,-89.33623]}, - {"t":0.04734, "x":7.09436, "y":7.25627, "heading":-3.12051, "vx":-0.28258, "vy":-0.03379, "omega":1.718, "ax":-6.9638, "ay":-1.09327, "alpha":31.27313, "fx":[-50.10129,-102.23548,-170.40524,-151.06715], "fy":[-167.60745,141.38094,40.57494,-88.73329]}, - {"t":0.07102, "x":7.08572, "y":7.25516, "heading":-3.07984, "vx":-0.44742, "vy":-0.05967, "omega":2.4583, "ax":-7.74375, "ay":-1.57568, "alpha":26.49393, "fx":[-69.08414,-134.12975,-172.47214,-151.19005], "fy":[-160.58893,111.48799,30.30841,-88.415]}, - {"t":0.09469, "x":7.07296, "y":7.25331, "heading":-3.02165, "vx":-0.63073, "vy":-0.09697, "omega":3.08546, "ax":-8.54757, "ay":-2.1231, "alpha":20.45828, "fx":[-96.41233,-158.97426,-174.26273,-151.91784], "fy":[-145.64882,71.73287,16.45776,-86.99521]}, - {"t":0.11836, "x":7.05563, "y":7.25042, "heading":-2.94861, "vx":-0.83307, "vy":-0.14723, "omega":3.56975, "ax":-9.30402, "ay":-2.54366, "alpha":13.00669, "fx":[-130.92972,-172.49402,-174.90792,-154.70299], "fy":[-115.39961,25.98556,-1.99018,-81.66338]}, - {"t":0.14203, "x":7.03331, "y":7.24622, "heading":-2.86411, "vx":-1.05332, "vy":-0.20745, "omega":3.87764, "ax":-9.80214, "ay":-2.727, "alpha":4.80846, "fx":[-159.37872,-173.1933,-172.5531,-161.80155], "fy":[-71.02164,-21.22364,-27.38467,-65.91219]}, - {"t":0.1657, "x":7.00563, "y":7.24055, "heading":-2.77232, "vx":-1.28535, "vy":-0.272, "omega":3.99147, "ax":-9.87663, "ay":-2.75936, "alpha":0.17838, "fx":[-167.78359,-168.24861,-168.21213,-167.75004], "fy":[-47.68596,-46.0222,-46.19406,-47.84131]}, - {"t":0.18938, "x":6.97243, "y":7.23333, "heading":-2.67783, "vx":-1.51915, "vy":-0.33732, "omega":3.99569, "ax":-9.85759, "ay":-2.7715, "alpha":0.00283, "fx":[-167.67178,-167.67908,-167.67788,-167.67058], "fy":[-47.15289,-47.12708,-47.13209,-47.1579]}, - {"t":0.21305, "x":6.93371, "y":7.22457, "heading":-2.58324, "vx":-1.7525, "vy":-0.40293, "omega":3.99576, "ax":-9.82588, "ay":-2.79135, "alpha":-0.00015, "fx":[-167.13556,-167.13518,-167.13528,-167.13566], "fy":[-47.47958,-47.4809,-47.48052,-47.4792]}, - {"t":0.23672, "x":6.88947, "y":7.21425, "heading":-2.48866, "vx":-1.9851, "vy":-0.469, "omega":3.99576, "ax":-9.76329, "ay":-2.82981, "alpha":-0.00051, "fx":[-166.07118,-166.0699,-166.07033,-166.07161], "fy":[-48.1329,-48.13724,-48.13553,-48.1312]}, - {"t":0.26039, "x":6.83974, "y":7.20236, "heading":-2.39407, "vx":-2.21621, "vy":-0.53599, "omega":3.99574, "ax":-9.58237, "ay":-2.93651, "alpha":-0.00743, "fx":[-162.99951,-162.98012,-162.98733,-163.00672], "fy":[-49.93403,-49.99427,-49.9645,-49.90423]}, - {"t":0.28406, "x":6.7846, "y":7.18885, "heading":-2.29948, "vx":-2.44305, "vy":-0.6055, "omega":3.99557, "ax":-5.16255, "ay":-4.18101, "alpha":-9.57157, "fx":[-114.16821,-76.37471,-54.94828,-105.76265], "fy":[-63.35535,-103.3954,-86.83979,-30.88089]}, - {"t":0.30774, "x":6.72532, "y":7.17334, "heading":-2.2049, "vx":-2.56525, "vy":-0.70447, "omega":3.76899, "ax":-5.43791, "ay":-0.26394, "alpha":-12.86776, "fx":[-123.19867,-88.36109,-58.01343,-100.41597], "fy":[-10.84764,-61.07096,5.18552,48.77521]}, - {"t":0.33141, "x":6.66307, "y":7.15659, "heading":-2.11568, "vx":-2.69398, "vy":-0.71072, "omega":3.46439, "ax":-5.51193, "ay":1.23377, "alpha":-11.25596, "fx":[-120.97794,-93.40428,-62.97155,-97.67173], "fy":[7.95345,-29.49315,42.95919,62.52509]}, - {"t":0.35508, "x":6.59775, "y":7.14011, "heading":-2.03367, "vx":-2.82446, "vy":-0.68152, "omega":3.19793, "ax":-5.38115, "ay":1.51934, "alpha":-10.13142, "fx":[-116.60616,-89.82915,-63.34035,-96.35189], "fy":[10.55887,-17.65707,49.39675,61.07534]}, - {"t":0.37875, "x":6.52939, "y":7.12441, "heading":-1.95797, "vx":-2.95184, "vy":-0.64555, "omega":2.9581, "ax":-5.12807, "ay":1.3341, "alpha":-9.29135, "fx":[-110.40529,-82.65371,-61.73925,-94.10962], "fy":[6.73519,-15.24707,45.43225,53.85034]}, - {"t":0.40242, "x":6.45807, "y":7.1095, "heading":-1.88795, "vx":-3.07323, "vy":-0.61397, "omega":2.73816, "ax":-4.76183, "ay":0.97481, "alpha":-8.5195, "fx":[-102.23192,-73.56289,-58.23428,-89.96063], "fy":[0.77572,-16.3276,37.59833,44.27865]}, - {"t":0.4261, "x":6.38399, "y":7.09524, "heading":-1.82313, "vx":-3.18595, "vy":-0.59089, "omega":2.53649, "ax":-4.31281, "ay":0.58172, "alpha":-7.72836, "fx":[-92.51681,-63.75666,-53.26659,-83.89855], "fy":[-5.26322,-18.17484,28.82278,34.19487]}, - {"t":0.44977, "x":6.30736, "y":7.08141, "heading":-1.76308, "vx":-3.28805, "vy":-0.57712, "omega":2.35354, "ax":-3.8345, "ay":0.21747, "alpha":-6.9396, "fx":[-82.24231,-54.33363,-47.64972,-76.66956], "fy":[-10.5736,-19.92709,20.5874,24.71001]}, - {"t":0.47344, "x":6.22846, "y":7.06781, "heading":-1.70737, "vx":-3.37882, "vy":-0.57198, "omega":2.18927, "ax":-3.38672, "ay":-0.10986, "alpha":-6.24459, "fx":[-72.6359,-46.09307,-42.2334,-69.4665], "fy":[-15.28102,-21.83472,13.35915,16.28199]}, - {"t":0.49711, "x":6.14752, "y":7.05424, "heading":-1.65555, "vx":-3.45899, "vy":-0.57458, "omega":2.04145, "ax":-3.01734, "ay":-0.43102, "alpha":-5.75856, "fx":[-64.7794,-39.38793,-37.61701,-63.51215], "fy":[-20.18183,-24.73258,6.85969,8.72883]}, - {"t":0.52078, "x":6.0648, "y":7.04052, "heading":-1.60722, "vx":-3.53041, "vy":-0.58478, "omega":1.90513, "ax":-2.75298, "ay":-0.79867, "alpha":-5.5871, "fx":[-59.34553,-34.14263,-34.06618,-59.75499], "fy":[-26.42817,-29.68515,0.38326,1.38927]}, - {"t":0.54445, "x":5.98045, "y":7.02645, "heading":-1.56212, "vx":-3.59558, "vy":-0.60369, "omega":1.77287, "ax":-2.59951, "ay":-1.27215, "alpha":-5.81132, "fx":[-56.53724,-29.95119,-31.57766,-58.8013], "fy":[-35.21289,-37.76614,-6.94572,-6.63092]}, - {"t":0.56813, "x":5.89461, "y":7.01181, "heading":-1.52016, "vx":-3.65712, "vy":-0.6338, "omega":1.63531, "ax":-2.54552, "ay":-1.90232, "alpha":-6.47657, "fx":[-56.11811,-26.15258,-29.95486,-60.96857], "fy":[-47.41674,-49.81562,-16.0042,-16.19485]}, - {"t":0.5918, "x":5.80733, "y":6.99627, "heading":-1.48145, "vx":-3.71738, "vy":-0.67883, "omega":1.48199, "ax":-2.56229, "ay":-2.71216, "alpha":-7.56379, "fx":[-57.41239,-21.87209,-28.79302,-66.25759], "fy":[-63.14445,-66.00193,-27.54371,-27.84203]}, - {"t":0.61547, "x":5.71861, "y":6.97944, "heading":-1.44636, "vx":-3.77803, "vy":-0.74303, "omega":1.30294, "ax":-2.59785, "ay":-3.68327, "alpha":-8.94127, "fx":[-59.27119,-16.1158,-27.28974,-74.07828], "fy":[-81.4222,-85.34173,-42.21602,-41.62529]}, - {"t":0.63914, "x":5.62845, "y":6.96082, "heading":-1.41552, "vx":-3.83953, "vy":-0.83022, "omega":1.09129, "ax":-2.56382, "ay":-4.77386, "alpha":-10.31834, "fx":[-60.03108,-7.99583,-23.74199,-82.6708], "fy":[-100.60034,-105.72253,-60.96059,-57.52463]}, - {"t":0.66281, "x":5.53684, "y":6.93983, "heading":-1.38969, "vx":-3.90022, "vy":-0.94323, "omega":0.84703, "ax":-2.34698, "ay":-5.93212, "alpha":-11.28461, "fx":[-57.90599,2.58623,-15.47321,-88.89318], "fy":[-118.97622,-124.41014,-84.35479,-75.8734]}, - {"t":0.68649, "x":5.44386, "y":6.91584, "heading":-1.36964, "vx":-3.95577, "vy":-1.08366, "omega":0.5799, "ax":-1.93839, "ay":-7.03313, "alpha":-11.65616, "fx":[-52.89519,13.82782,-2.25121,-90.56705], "fy":[-134.80774,-139.13809,-108.965,-95.61541]}, - {"t":0.71016, "x":5.34968, "y":6.88822, "heading":-1.35591, "vx":-4.00166, "vy":-1.25014, "omega":0.30398, "ax":-1.60638, "ay":-7.8198, "alpha":-12.56622, "fx":[-50.24979,24.33089,12.10033,-95.47753], "fy":[-146.54051,-149.76611,-126.9864,-108.7572]}, - {"t":0.73383, "x":5.2545, "y":6.85643, "heading":-1.34871, "vx":-4.03969, "vy":-1.43525, "omega":0.00651, "ax":3.18631, "ay":-8.39195, "alpha":-0.08425, "fx":[53.83291,54.50404,54.56487,53.8912], "fy":[-142.92868,-142.68897,-142.56016,-142.80114]}, - {"t":0.75197, "x":5.18173, "y":6.82901, "heading":-1.3486, "vx":-3.98187, "vy":-1.58751, "omega":0.00498, "ax":3.73801, "ay":-8.80409, "alpha":0.01737, "fx":[63.66483,63.52147,63.50027,63.64351], "fy":[-149.71492,-149.7735,-149.79518,-149.73668]}, - {"t":0.77012, "x":5.1101, "y":6.79876, "heading":-1.34851, "vx":-3.91405, "vy":-1.74725, "omega":0.0053, "ax":4.21581, "ay":-8.86259, "alpha":0.05321, "fx":[71.97239,71.53462,71.4474,71.88415], "fy":[-150.61478,-150.81718,-150.88511,-150.68349]}, - {"t":0.78826, "x":5.03978, "y":6.7656, "heading":-1.34841, "vx":-3.83756, "vy":-1.90805, "omega":0.00626, "ax":4.74064, "ay":-8.74766, "alpha":0.34116, "fx":[82.36594,79.618,78.92638,81.63712], "fy":[-147.80844,-149.27005,-149.76396,-148.3386]}, - {"t":0.8064, "x":4.97093, "y":6.72954, "heading":-1.3483, "vx":-3.75155, "vy":-2.06677, "omega":0.01245, "ax":5.61845, "ay":-8.29875, "alpha":1.85, "fx":[105.05208,91.06832,86.45947,99.69296], "fy":[-134.37942,-143.98317,-147.34881,-138.92606]}, - {"t":0.82455, "x":4.90379, "y":6.69067, "heading":-1.34807, "vx":-3.64961, "vy":-2.21733, "omega":0.04602, "ax":6.8122, "ay":-7.33988, "alpha":4.53419, "fx":[137.33472,109.20217,94.8755,122.08185], "fy":[-102.90735,-131.53989,-143.39346,-121.5564]}, - {"t":0.84269, "x":4.83869, "y":6.64923, "heading":-1.34724, "vx":-3.52601, "vy":-2.35051, "omega":0.12828, "ax":7.71392, "ay":-6.31802, "alpha":6.48757, "fx":[157.2393,127.86862,102.98685,136.75172], "fy":[-70.92417,-114.22626,-138.59318,-106.12729]}, - {"t":0.86084, "x":4.77599, "y":6.60555, "heading":-1.34491, "vx":-3.38606, "vy":-2.46514, "omega":0.24599, "ax":8.34048, "ay":-5.4379, "alpha":7.30977, "fx":[166.10854,143.38953,112.14464,145.83411], "fy":[-48.83078,-95.03441,-131.93946,-94.18407]}, - {"t":0.87898, "x":4.71592, "y":6.55993, "heading":-1.34044, "vx":-3.23473, "vy":-2.5638, "omega":0.37862, "ax":8.88122, "ay":-4.62309, "alpha":6.78227, "fx":[169.47547,155.35801,126.53465,152.90023], "fy":[-37.51802,-75.47275,-118.71412,-82.84514]}, - {"t":0.89712, "x":4.6587, "y":6.51265, "heading":-1.33357, "vx":-3.07359, "vy":-2.64768, "omega":0.50167, "ax":9.32494, "ay":-3.87797, "alpha":5.43312, "fx":[170.75679,162.69817,142.17157,158.83163], "fy":[-32.83058,-59.80795,-99.93407,-71.27985]}, - {"t":0.91527, "x":4.60447, "y":6.46397, "heading":-1.32447, "vx":-2.9044, "vy":-2.71804, "omega":0.60025, "ax":9.62981, "ay":-3.25917, "alpha":4.05649, "fx":[171.41526,166.70102,153.75601,163.32892], "fy":[-30.49083,-49.09304,-81.48307,-60.68336]}, - {"t":0.93341, "x":4.55335, "y":6.41412, "heading":-1.31358, "vx":-2.72968, "vy":-2.77717, "omega":0.67385, "ax":9.82969, "ay":-2.76102, "alpha":2.88219, "fx":[171.83368,169.01514,161.36142,166.59041], "fy":[-29.06329,-41.68916,-65.64851,-51.45555]}, - {"t":0.95155, "x":4.50545, "y":6.36328, "heading":-1.30136, "vx":-2.55134, "vy":-2.82727, "omega":0.72614, "ax":9.96132, "ay":-2.36036, "alpha":1.92256, "fx":[172.12906,170.4698,166.22847,168.92981], "fy":[-28.0916,-36.31904,-52.62758,-43.55817]}, - {"t":0.9697, "x":4.46079, "y":6.31159, "heading":-1.28818, "vx":-2.3706, "vy":-2.87009, "omega":0.76102, "ax":10.04963, "ay":-2.03507, "alpha":1.14151, "fx":[172.34964,171.45194,169.35507,170.60872], "fy":[-27.39224,-32.23332,-42.00604,-36.83219]}, - {"t":0.98784, "x":4.41944, "y":6.25918, "heading":-1.27437, "vx":-2.18827, "vy":-2.90702, "omega":0.78174, "ax":10.11009, "ay":-1.76758, "alpha":0.50032, "fx":[172.52009,172.15356,171.38629,171.81915], "fy":[-26.87401,-28.99628,-33.29237,-31.10136]}, - {"t":1.00598, "x":4.3814, "y":6.20615, "heading":-1.26019, "vx":-2.00483, "vy":-2.93909, "omega":0.79081, "ax":10.15226, "ay":-1.5447, "alpha":-0.03254, "fx":[172.65488,172.67743,172.71915,172.69687], "fy":[-26.48403,-26.34516,-26.06595,-26.20452]}, - {"t":1.02413, "x":4.34669, "y":6.15257, "heading":-1.24584, "vx":-1.82064, "vy":-2.96711, "omega":0.79022, "ax":10.18213, "ay":-1.35666, "alpha":-0.48096, "fx":[172.76333,173.08242,173.59772,173.33693], "fy":[-26.18826,-24.11469,-19.99869,-22.00371]}, - {"t":1.04227, "x":4.31534, "y":6.09851, "heading":-1.2315, "vx":-1.6359, "vy":-2.99173, "omega":0.7815, "ax":10.26394, "ay":-0.65499, "alpha":-0.69593, "fx":[174.27188,174.51232,174.84905,174.71323], "fy":[-15.60862,-12.87935,-6.76079,-9.31614]}, - {"t":1.06918, "x":4.27504, "y":6.01779, "heading":-1.21048, "vx":-1.35976, "vy":-3.00935, "omega":0.76277, "ax":10.24808, "ay":0.80363, "alpha":-1.006, "fx":[174.748,174.62574,173.83176,174.06182], "fy":[7.52422,10.46553,19.58062,17.10797]}, - {"t":1.09608, "x":4.24217, "y":5.93712, "heading":-1.18996, "vx":-1.08405, "vy":-2.98773, "omega":0.73571, "ax":9.95366, "ay":2.54264, "alpha":-1.35752, "fx":[171.13475,170.71628,167.53965,167.84491], "fy":[35.8544,38.00238,50.16805,48.97346]}, - {"t":1.12298, "x":4.2166, "y":5.85766, "heading":-1.17016, "vx":-0.81626, "vy":-2.91932, "omega":0.69919, "ax":9.2496, "ay":4.45559, "alpha":-1.71684, "fx":[160.90522,160.96662,154.13358,153.32648], "fy":[68.30125,68.31352,82.58767,83.95075]}, - {"t":1.14989, "x":4.19799, "y":5.78073, "heading":-1.15135, "vx":-0.56741, "vy":-2.79945, "omega":0.653, "ax":8.08772, "ay":6.31581, "alpha":-2.02676, "fx":[142.33454,144.52705,133.59978,129.81779], "fy":[101.42978,98.41183,112.8242,117.05437]}, - {"t":1.17679, "x":4.18565, "y":5.7077, "heading":-1.13379, "vx":-0.34983, "vy":-2.62954, "omega":0.59847, "ax":6.58396, "ay":7.86777, "alpha":-2.23716, "fx":[116.48684,122.57156,108.48151,100.42496], "fy":[130.31231,124.71414,137.18236,143.10512]}, - {"t":1.20369, "x":4.17862, "y":5.6398, "heading":-1.11769, "vx":-0.17269, "vy":-2.41786, "omega":0.53828, "ax":4.97059, "ay":8.97521, "alpha":-2.34097, "fx":[87.35962,98.01277,82.50931,70.31169], "fy":[151.43502,144.85315,154.25418,160.12089]}, - {"t":1.2306, "x":4.17578, "y":5.578, "heading":-1.1032, "vx":-0.03897, "vy":-2.1764, "omega":0.4753, "ax":3.45565, "ay":9.66299, "alpha":-2.36565, "fx":[59.32217,73.90875,58.562,43.32581], "fy":[164.51372,158.56239,164.89349,169.489]}, - {"t":1.2575, "x":4.17598, "y":5.52294, "heading":-1.09042, "vx":0.054, "vy":-1.91643, "omega":0.41166, "ax":2.14466, "ay":10.03936, "alpha":-2.34332, "fx":[34.90501,52.20343,37.93299,20.8788], "fy":[171.42499,167.02132,170.87273,173.74787]}, - {"t":1.2844, "x":4.17821, "y":5.47502, "heading":-1.07934, "vx":0.1117, "vy":-1.64634, "omega":0.34861, "ax":1.05686, "ay":10.21515, "alpha":-2.29722, "fx":[14.73328,33.60339,20.76012,2.81065], "fy":[174.37768,171.7797,173.8418,175.0278]}, - {"t":1.31131, "x":4.1816, "y":5.43442, "heading":-1.06996, "vx":0.14013, "vy":-1.37151, "omega":0.28681, "ax":0.16909, "ay":10.27188, "alpha":-2.2415, "fx":[-1.57918,18.04331,6.64981,-11.60913], "fy":[175.04181,174.14511,174.98973,174.71021]}, - {"t":1.33821, "x":4.18543, "y":5.40124, "heading":-1.06225, "vx":0.14468, "vy":-1.09516, "omega":0.22651, "ax":-0.5539, "ay":10.26158, "alpha":-2.18397, "fx":[-14.72601,5.13288,-4.93129,-23.16259], "fy":[174.47156,175.0389,175.08032,173.59525]}, - {"t":1.36511, "x":4.18912, "y":5.37549, "heading":-1.05615, "vx":0.12978, "vy":-0.81909, "omega":0.16775, "ax":-1.1459, "ay":10.21531, "alpha":-2.12853, "fx":[-25.3835,-5.58808,-14.48508,-32.50891], "fy":[173.27922,175.05694,174.57882,172.12295]}, - {"t":1.39202, "x":4.1922, "y":5.35715, "heading":-1.05164, "vx":0.09895, "vy":-0.54426, "omega":0.11049, "ax":-1.63495, "ay":10.15105, "alpha":-2.07694, "fx":[-34.11042,-14.54137,-22.42962,-40.15879], "fy":[171.80634,174.56918,173.76196,170.52864]}, - {"t":1.41892, "x":4.19427, "y":5.34618, "heading":-1.04867, "vx":0.05497, "vy":-0.27116, "omega":0.05461, "ax":-2.04311, "ay":10.07906, "alpha":-2.02977, "fx":[-41.34028,-22.07844,-29.09609,-46.49592], "fy":[170.23884,173.801,172.7925,168.93514]}, - {"t":1.44583, "x":4.19501, "y":5.34254, "heading":-1.0472, "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.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.47932, "ay":-0.72111, "alpha":43.21051, "fx":[-24.92178,-47.01019,-189.92906,-170.16731], "fy":[-197.39403,192.9434,59.84905,-103.4806]}, + {"t":0.02615, "x":7.09867, "y":7.25678, "heading":3.14159, "vx":-0.16943, "vy":-0.01886, "omega":1.12993, "ax":-7.31643, "ay":-0.91857, "alpha":39.43408, "fx":[-42.27699,-82.45346,-191.65576,-171.45904], "fy":[-194.32503,180.43763,53.87546,-101.23622]}, + {"t":0.0523, "x":7.09174, "y":7.25598, "heading":-3.11205, "vx":-0.36075, "vy":-0.04288, "omega":2.1611, "ax":-8.44952, "ay":-1.46376, "alpha":33.398, "fx":[-65.32239,-131.64786,-194.48595,-171.94145], "fy":[-187.68238,148.16424,42.19271,-100.27552]}, + {"t":0.07845, "x":7.07942, "y":7.25436, "heading":-3.05553, "vx":-0.5817, "vy":-0.08115, "omega":3.03444, "ax":-9.7636, "ay":-2.34496, "alpha":24.40774, "fx":[-103.12893,-177.32925,-197.54514,-173.01506], "fy":[-169.60763,88.32896,23.09025,-98.16878]}, + {"t":0.1046, "x":7.06087, "y":7.25143, "heading":-2.97619, "vx":-0.83701, "vy":-0.14247, "omega":3.67269, "ax":-10.98662, "ay":-3.11225, "alpha":11.9618, "fx":[-157.99936,-197.94616,-198.47,-178.15092], "fy":[-119.76242,8.82234,-8.6179,-87.96056]}, + {"t":0.13075, "x":7.03523, "y":7.24664, "heading":-2.88015, "vx":-1.1243, "vy":-0.22386, "omega":3.98548, "ax":-11.39941, "ay":-3.36904, "alpha":0.41508, "fx":[-189.4208,-190.57111,-190.61225,-189.48685], "fy":[-58.15202,-54.26128,-54.20704,-58.02083]}, + {"t":0.1569, "x":7.00193, "y":7.23964, "heading":-2.77593, "vx":-1.42239, "vy":-0.31195, "omega":3.99633, "ax":-11.30636, "ay":-3.55302, "alpha":0.00279, "fx":[-188.46771,-188.4758,-188.47554,-188.46746], "fy":[-59.23913,-59.21347,-59.21512,-59.24078]}, + {"t":0.18305, "x":6.96087, "y":7.23026, "heading":-2.67143, "vx":-1.71805, "vy":-0.40486, "omega":3.99641, "ax":-11.0173, "ay":-4.07605, "alpha":-0.0038, "fx":[-183.65898,-183.64639,-183.64723,-183.65983], "fy":[-67.93104,-67.96485,-67.96069,-67.92687]}, + {"t":0.20919, "x":6.91218, "y":7.21828, "heading":-2.56692, "vx":-2.00614, "vy":-0.51145, "omega":3.99631, "ax":-1.86701, "ay":-9.89062, "alpha":-4.60079, "fx":[-57.70924,-32.65345,-4.49592,-29.62975], "fy":[-159.14418,-169.83115,-169.4336,-161.07832]}, + {"t":0.23534, "x":6.85908, "y":7.20153, "heading":-2.46242, "vx":-2.05496, "vy":-0.77008, "omega":3.876, "ax":-2.55379, "ay":-3.46312, "alpha":-11.1718, "fx":[-80.99858,-43.47627,-0.97331,-44.83367], "fy":[-51.30583,-92.01239,-68.63034,-18.96588]}, + {"t":0.26149, "x":6.80447, "y":7.18021, "heading":-2.36107, "vx":-2.12174, "vy":-0.86064, "omega":3.58386, "ax":-3.98621, "ay":2.41128, "alpha":-10.1995, "fx":[-97.41588,-73.17948,-31.80225,-63.39548], "fy":[37.18877,1.02825,46.64003,75.92227]}, + {"t":0.28764, "x":6.74762, "y":7.15853, "heading":-2.26735, "vx":-2.22598, "vy":-0.79759, "omega":3.31715, "ax":-4.53785, "ay":3.11553, "alpha":-9.6461, "fx":[-104.99351,-81.37577,-42.55674,-73.64927], "fy":[44.39343,14.64569,64.15134,84.54725]}, + {"t":0.31379, "x":6.68786, "y":7.13874, "heading":-2.18061, "vx":-2.34464, "vy":-0.71612, "omega":3.06491, "ax":-4.77216, "ay":2.68112, "alpha":-9.41926, "fx":[-107.50102,-81.83317,-48.45683,-80.40748], "fy":[34.89165,7.99588,59.20841,76.67583]}, + {"t":0.33994, "x":6.62492, "y":7.12093, "heading":-2.10047, "vx":-2.46943, "vy":-0.64601, "omega":2.81861, "ax":-4.69508, "ay":2.01532, "alpha":-8.99082, "fx":[-104.3714,-76.58827,-49.8378,-82.26145], "fy":[22.70906,-1.0653,48.55255,64.18115]}, + {"t":0.36609, "x":6.55874, "y":7.10472, "heading":-2.02676, "vx":-2.5922, "vy":-0.59331, "omega":2.5835, "ax":-4.36367, "ay":1.3886, "alpha":-8.25311, "fx":[-96.44967,-67.74067,-47.48329,-79.2878], "fy":[12.00199,-7.66764,37.34666,50.90849]}, + {"t":0.39224, "x":6.48947, "y":7.08968, "heading":-1.9592, "vx":-2.70631, "vy":-0.557, "omega":2.36769, "ax":-3.8659, "ay":0.90118, "alpha":-7.25285, "fx":[-85.21814,-57.36007,-42.72978,-72.4629], "fy":[4.24547,-10.80678,27.82009,38.82985]}, + {"t":0.41839, "x":6.41738, "y":7.07543, "heading":-1.89729, "vx":-2.8074, "vy":-0.53343, "omega":2.17803, "ax":-3.29988, "ay":0.57461, "alpha":-6.11328, "fx":[-72.48733,-47.14072,-37.01831,-63.38343], "fy":[-0.35556,-11.03899,20.72019,28.98823]}, + {"t":0.44454, "x":6.34284, "y":7.06167, "heading":-1.84034, "vx":-2.89369, "vy":-0.51841, "omega":2.01817, "ax":-2.75265, "ay":0.38465, "alpha":-4.98226, "fx":[-60.03981,-38.21109,-31.48593,-53.80489], "fy":[-2.38546,-9.4974,15.88019,21.65071]}, + {"t":0.47069, "x":6.26623, "y":7.04825, "heading":-1.78756, "vx":-2.96567, "vy":-0.50835, "omega":1.88789, "ax":-2.28255, "ay":0.28567, "alpha":-3.98259, "fx":[-49.207,-31.08824,-26.78622,-45.11479], "fy":[-2.81676,-7.32249,12.70026,16.48681]}, + {"t":0.49684, "x":6.1879, "y":7.03505, "heading":-1.7382, "vx":-3.02536, "vy":-0.50088, "omega":1.78375, "ax":-1.91502, "ay":0.22984, "alpha":-3.18655, "fx":[-40.65946,-25.80067,-23.14457,-38.085], "fy":[-2.64607,-5.39811,10.50422,12.86534]}, + {"t":0.52299, "x":6.10813, "y":7.02203, "heading":-1.69155, "vx":-3.07543, "vy":-0.49487, "omega":1.70042, "ax":-1.65181, "ay":0.17591, "alpha":-2.62386, "fx":[-34.52841,-22.10325,-20.52578,-32.98175], "fy":[-2.71807,-4.34983,8.69753,10.09947]}, + {"t":0.54914, "x":6.02715, "y":7.00915, "heading":-1.64709, "vx":-3.11863, "vy":-0.49027, "omega":1.63181, "ax":-1.48444, "ay":0.08735, "alpha":-2.30794, "fx":[-30.69039,-19.66338,-18.79209,-29.83376], "fy":[-3.77038,-4.69957,6.76215,7.53202]}, + {"t":0.57529, "x":5.94509, "y":6.99636, "heading":-1.60442, "vx":-3.15745, "vy":-0.48798, "omega":1.57146, "ax":-1.40452, "ay":-0.07582, "alpha":-2.26349, "fx":[-29.01672,-18.15782,-17.80052,-28.67563], "fy":[-6.61651,-7.08055,4.16086,4.4807]}, + {"t":0.60144, "x":5.86204, "y":6.98358, "heading":-1.56332, "vx":-3.19417, "vy":-0.48997, "omega":1.51227, "ax":-1.40889, "ay":-0.36822, "alpha":-2.54837, "fx":[-29.51,-17.28011,-17.44099,-29.71131], "fy":[-12.38069,-12.46829,0.19672,0.10023]}, + {"t":0.62758, "x":5.77803, "y":6.97064, "heading":-1.52378, "vx":-3.23101, "vy":-0.4996, "omega":1.44563, "ax":-1.49961, "ay":-0.8695, "alpha":-3.26577, "fx":[-32.30455,-16.66402,-17.6233,-33.39935], "fy":[-22.68037,-22.37635,-6.1435,-6.77614]}, + {"t":0.65373, "x":5.69303, "y":6.95728, "heading":-1.48598, "vx":-3.27023, "vy":-0.52233, "omega":1.36023, "ax":-1.67477, "ay":-1.68539, "alpha":-4.54621, "fx":[-37.42622,-15.67643,-18.1667,-40.4014], "fy":[-39.4907,-38.83213,-16.33766,-17.71838]}, + {"t":0.67988, "x":5.60695, "y":6.94304, "heading":-1.45041, "vx":-3.31402, "vy":-0.5664, "omega":1.24135, "ax":-1.87791, "ay":-2.95566, "alpha":-6.41104, "fx":[-43.7017,-12.69941,-18.07495,-50.7397], "fy":[-64.6249,-64.07993,-33.17537,-35.19764]}, + {"t":0.70603, "x":5.51964, "y":6.92722, "heading":-1.41795, "vx":-3.36313, "vy":-0.64369, "omega":1.07371, "ax":-1.67661, "ay":-5.0597, "alpha":-8.02383, "fx":[-43.10506,-1.67455,-10.10368,-56.90975], "fy":[-100.94563,-100.90574,-67.22218,-68.29758]}, + {"t":0.73218, "x":5.43113, "y":6.90866, "heading":-1.38987, "vx":-3.40697, "vy":-0.776, "omega":0.86389, "ax":0.32191, "ay":-8.12585, "alpha":-5.88579, "fx":[-10.22378,27.2832,24.83437,-20.42927], "fy":[-144.82377,-141.13642,-125.99863,-129.85714]}, + {"t":0.75833, "x":5.34215, "y":6.88559, "heading":-1.36728, "vx":-3.39855, "vy":-0.98849, "omega":0.70998, "ax":3.56899, "ay":-9.6786, "alpha":-0.2099, "fx":[58.58307,60.24371,60.41045,58.73623], "fy":[-161.79001,-161.22014,-160.88179,-161.45837]}, + {"t":0.78448, "x":5.2545, "y":6.85643, "heading":-1.34871, "vx":-3.30523, "vy":-1.24158, "omega":0.70449, "ax":5.04266, "ay":-8.79125, "alpha":2.59578, "fx":[95.58621,76.33264,73.26132,91.05485], "fy":[-138.433,-148.53298,-153.99703,-145.2207]}, + {"t":0.80556, "x":5.18596, "y":6.82831, "heading":-1.33387, "vx":-3.19895, "vy":-1.42686, "omega":0.7592, "ax":3.05379, "ay":-7.96292, "alpha":-2.52705, "fx":[42.49975,59.23988,60.03702,41.84418], "fy":[-138.74776,-133.18589,-126.42305,-132.5957]}, + {"t":0.82663, "x":5.11921, "y":6.79647, "heading":-1.31787, "vx":-3.13458, "vy":-1.59469, "omega":0.70594, "ax":0.87828, "ay":-6.00446, "alpha":-5.72055, "fx":[2.05603,33.86279,29.58885,-6.94564], "fy":[-113.36938,-106.60851,-86.32344,-94.06451]}, + {"t":0.84771, "x":5.05334, "y":6.76153, "heading":-1.30299, "vx":-3.11607, "vy":-1.72124, "omega":0.58537, "ax":-0.11427, "ay":-3.82525, "alpha":-5.31952, "fx":[-11.32732,15.18401,8.51181,-19.98774], "fy":[-77.81252,-72.18779,-49.31498,-55.74503]}, + {"t":0.86879, "x":4.98764, "y":6.7244, "heading":-1.29065, "vx":-3.11848, "vy":-1.80186, "omega":0.47326, "ax":-0.1711, "ay":-2.46707, "alpha":-3.70135, "fx":[-9.0806,8.66751,3.65772,-14.65333], "fy":[-51.68783,-47.18208,-30.391,-35.23852]}, + {"t":0.88986, "x":4.92188, "y":6.68587, "heading":-1.28067, "vx":-3.12209, "vy":-1.85386, "omega":0.39525, "ax":0.15554, "ay":-2.28984, "alpha":-2.80045, "fx":[-2.12047,11.2835,7.46865,-6.26036], "fy":[-46.33004,-42.58388,-29.93101,-33.83703]}, + {"t":0.91094, "x":4.85611, "y":6.64629, "heading":-1.27234, "vx":-3.11881, "vy":-1.90212, "omega":0.33623, "ax":0.9854, "ay":-3.4817, "alpha":-2.75782, "fx":[11.37347,25.11936,21.76335,7.44831], "fy":[-65.94537,-61.77711,-50.04094,-54.38938]}, + {"t":0.93201, "x":4.7906, "y":6.60543, "heading":-1.26526, "vx":-3.09804, "vy":-1.9755, "omega":0.2781, "ax":2.75004, "ay":-5.79675, "alpha":-2.58109, "fx":[39.48025,54.0082,52.69199,37.18691], "fy":[-103.89273,-98.32447,-89.16759,-95.13134]}, + {"t":0.95309, "x":4.72591, "y":6.56251, "heading":-1.2594, "vx":-3.04008, "vy":-2.09767, "omega":0.2237, "ax":4.86595, "ay":-7.58987, "alpha":-1.60407, "fx":[75.58282,85.77222,86.88963,76.2072], "fy":[-131.47217,-126.0017,-121.36925,-127.23504]}, + {"t":0.97417, "x":4.66292, "y":6.51661, "heading":-1.25468, "vx":-2.93753, "vy":-2.25764, "omega":0.18989, "ax":6.44864, "ay":-8.17552, "alpha":-0.65669, "fx":[104.8241,109.09753,110.20337,105.85811], "fy":[-138.67138,-135.66274,-133.84145,-136.95238]}, + {"t":0.99524, "x":4.60244, "y":6.46721, "heading":-1.25068, "vx":-2.80161, "vy":-2.42995, "omega":0.17605, "ax":7.51627, "ay":-8.12696, "alpha":-0.17389, "fx":[124.54986,125.63335,126.03719,124.95037], "fy":[-136.19925,-135.26824,-134.74142,-135.68083]}, + {"t":1.01632, "x":4.54506, "y":6.41419, "heading":-1.24697, "vx":-2.6432, "vy":-2.60123, "omega":0.17239, "ax":8.25904, "ay":-7.85524, "alpha":-0.13818, "fx":[137.08571,137.88844,138.26318,137.45947], "fy":[-131.57941,-130.7796,-130.30405,-131.10935]}, + {"t":1.03739, "x":4.49119, "y":6.35762, "heading":-1.24334, "vx":-2.46913, "vy":-2.76679, "omega":0.16948, "ax":8.79279, "ay":-7.53116, "alpha":-0.46532, "fx":[144.63444,147.14622,148.50872,145.99683], "fy":[-127.826,-125.03561,-123.22528,-126.07662]}, + {"t":1.05847, "x":4.4411, "y":6.29764, "heading":-1.23976, "vx":-2.28381, "vy":-2.92552, "omega":0.15967, "ax":9.18165, "ay":-7.22051, "alpha":-1.08363, "fx":[148.66517,154.14703,157.41918,151.98358], "fy":[-125.91051,-119.33621,-114.65656,-121.54661]}, + {"t":1.07955, "x":4.39501, "y":6.23438, "heading":-1.2364, "vx":-2.0903, "vy":-3.0777, "omega":0.13683, "ax":9.45696, "ay":-6.95503, "alpha":-1.96532, "fx":[149.87349,159.34042,165.27936,156.07863], "fy":[-126.25388,-114.35876,-105.12714,-118.00844]}, + {"t":1.10062, "x":4.35305, "y":6.16797, "heading":-1.23352, "vx":-1.89098, "vy":-3.22429, "omega":0.09541, "ax":9.63018, "ay":-6.74328, "alpha":-3.43739, "fx":[147.15085,163.27378,173.38379,158.31326], "fy":[-130.59712,-110.16057,-92.77398,-116.09714]}, + {"t":1.1217, "x":4.31534, "y":6.09851, "heading":-1.2315, "vx":-1.68801, "vy":-3.36641, "omega":0.02296, "ax":10.68037, "ay":-5.08003, "alpha":-0.53148, "fx":[176.46698,178.08446,179.57725,178.01803], "fy":[-87.94418,-84.71274,-81.40548,-84.66402]}, + {"t":1.148, "x":4.27464, "y":6.00823, "heading":-1.2309, "vx":-1.40716, "vy":-3.49999, "omega":0.00899, "ax":11.26585, "ay":-3.45229, "alpha":1.25676, "fx":[190.19148,188.14523,185.2078,187.64068], "fy":[-49.51276,-56.27863,-65.63574,-58.76514]}, + {"t":1.17429, "x":4.24154, "y":5.915, "heading":-1.23066, "vx":-1.11092, "vy":-3.59077, "omega":0.04203, "ax":11.23878, "ay":1.69977, "alpha":10.63061, "fx":[182.35329,178.53139,191.73739,196.75791], "fy":[73.47891,77.89809,-37.70721,-0.33265]}, + {"t":1.20059, "x":4.21621, "y":5.82117, "heading":-1.22956, "vx":-0.81539, "vy":-3.54608, "omega":0.32157, "ax":8.3953, "ay":7.31317, "alpha":13.72072, "fx":[129.76816,70.33227,176.94067,182.74157], "fy":[148.80248,183.35101,81.47469,73.99985]}, + {"t":1.22688, "x":4.19767, "y":5.73045, "heading":-1.2211, "vx":-0.59463, "vy":-3.35377, "omega":0.68237, "ax":5.45283, "ay":10.4099, "alpha":5.3392, "fx":[90.51112,57.59007,93.36575,122.11731], "fy":[175.96951,189.04095,173.69232,155.40947]}, + {"t":1.25318, "x":4.18392, "y":5.64586, "heading":-1.20316, "vx":-0.45124, "vy":-3.08003, "omega":0.82277, "ax":3.775, "ay":11.26295, "alpha":1.42997, "fx":[63.81866,53.6971,62.02463,72.16924], "fy":[187.63712,190.72726,188.10862,184.51908]}, + {"t":1.27947, "x":4.17336, "y":5.56876, "heading":-1.18152, "vx":-0.35197, "vy":-2.78387, "omega":0.86037, "ax":2.81552, "ay":11.56413, "alpha":-0.74256, "fx":[46.13982,51.8292,47.70219,42.06203], "fy":[192.96845,191.54062,192.63764,193.92751]}, + {"t":1.30577, "x":4.16508, "y":5.49956, "heading":-1.1589, "vx":-0.27794, "vy":-2.47978, "omega":0.84085, "ax":2.21377, "ay":11.69197, "alpha":-2.07433, "fx":[34.14506,50.77093,39.38272,23.31091], "fy":[195.60288,192.0112,194.72607,197.25764]}, + {"t":1.33207, "x":4.15853, "y":5.43839, "heading":-1.13679, "vx":-0.21972, "vy":-2.17233, "omega":0.7863, "ax":1.80567, "ay":11.75333, "alpha":-2.96385, "fx":[25.73401,50.10811,33.81099,10.7454], "fy":[196.9998,192.31399,195.90651,198.46949]}, + {"t":1.35836, "x":4.15338, "y":5.38533, "heading":-1.11611, "vx":-0.17224, "vy":-1.86326, "omega":0.70836, "ax":1.51209, "ay":11.78519, "alpha":-3.59676, "fx":[19.64282,49.65961,29.74005,1.78075], "fy":[197.78657,192.52407,196.65878,198.84443]}, + {"t":1.38466, "x":4.14937, "y":5.34041, "heading":-1.09749, "vx":-0.13248, "vy":-1.55336, "omega":0.61378, "ax":1.29128, "ay":11.80249, "alpha":-4.06862, "fx":[15.0931,49.33555,26.60104,-4.9293], "fy":[198.25354,192.67858,197.17551,198.85991]}, + {"t":1.41095, "x":4.14634, "y":5.30364, "heading":-1.08135, "vx":-0.09852, "vy":-1.24301, "omega":0.5068, "ax":1.11941, "ay":11.81205, "alpha":-4.43327, "fx":[11.58632,49.08814,24.10151,-10.13579], "fy":[198.54395,192.79757,197.54788,198.71559]}, + {"t":1.43725, "x":4.14413, "y":5.27504, "heading":-1.06802, "vx":-0.06909, "vy":-0.9324, "omega":0.39022, "ax":0.98194, "ay":11.81725, "alpha":-4.72334, "fx":[8.79028,48.89104,22.0777,-14.28538], "fy":[198.73224,192.89244,197.82447,198.50263]}, + {"t":1.46354, "x":4.14265, "y":5.25461, "heading":-1.05776, "vx":-0.04327, "vy":-0.62166, "omega":0.26601, "ax":0.86952, "ay":11.81989, "alpha":-4.95973, "fx":[6.4754,48.72939,20.43186,-17.65846], "fy":[198.85865,192.97001,198.03357,198.26546]}, + {"t":1.48984, "x":4.14182, "y":5.24235, "heading":-1.05076, "vx":-0.0204, "vy":-0.31084, "omega":0.13559, "ax":0.77591, "ay":11.82097, "alpha":-5.1565, "fx":[4.478,48.59456,19.10267,-20.43904], "fy":[198.94552,193.03448,198.19277,198.0268]}, + {"t":1.51614, "x":4.14155, "y":5.23826, "heading":-1.0472, "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/startDeepToL.traj b/src/main/deploy/choreo/startDeepToL.traj index 10ed5821..cdef07d3 100644 --- a/src/main/deploy/choreo/startDeepToL.traj +++ b/src/main/deploy/choreo/startDeepToL.traj @@ -3,10 +3,10 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.336880683898926, "y":6.839954376220703, "heading":-1.865030672844723, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.101142883300781, "y":5.999652862548828, "heading":-1.1659041662799314, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.336880683898926, "y":6.839954376220703, "heading":-1.865030672844723, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.101142883300781, "y":5.999652862548828, "heading":-1.1659041662799314, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -15,10 +15,10 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.336880683898926 m", "val":5.336880683898926}, "y":{"exp":"6.839954376220703 m", "val":6.839954376220703}, "heading":{"exp":"-1.865030672844723 rad", "val":-1.865030672844723}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.101142883300781 m", "val":4.101142883300781}, "y":{"exp":"5.999652862548828 m", "val":5.999652862548828}, "heading":{"exp":"-1.1659041662799314 rad", "val":-1.1659041662799314}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.336880683898926 m", "val":5.336880683898926}, "y":{"exp":"6.839954376220703 m", "val":6.839954376220703}, "heading":{"exp":"-1.865030672844723 rad", "val":-1.865030672844723}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.101142883300781 m", "val":4.101142883300781}, "y":{"exp":"5.999652862548828 m", "val":5.999652862548828}, "heading":{"exp":"-1.1659041662799314 rad", "val":-1.1659041662799314}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "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}, @@ -30,74 +30,69 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.66911,1.09796,1.51059], + "waypoints":[0.0,0.70317,1.18034,1.59507], "samples":[ - {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.59698, "ay":-1.14619, "alpha":27.82044, "fx":[-69.47717,-122.18867,-170.57535,-154.64832], "fy":[-160.7136,124.98887,40.23992,-82.50042]}, - {"t":0.02478, "x":7.09855, "y":7.25668, "heading":3.14159, "vx":-0.18827, "vy":-0.0284, "omega":0.68945, "ax":-7.80661, "ay":-1.24697, "alpha":26.47518, "fx":[-74.99925,-130.07267,-171.00054,-155.08014], "fy":[-158.17082,116.67948,38.29086,-81.64223]}, - {"t":0.04956, "x":7.09149, "y":7.25559, "heading":-3.12451, "vx":-0.38173, "vy":-0.05931, "omega":1.34555, "ax":-8.01373, "ay":-1.39884, "alpha":25.06947, "fx":[-80.59668,-137.94576,-171.72727,-154.97529], "fy":[-155.33586,107.18985,34.7573,-81.78701]}, - {"t":0.07435, "x":7.07957, "y":7.25369, "heading":-3.09116, "vx":-0.58033, "vy":-0.09397, "omega":1.96682, "ax":-8.22654, "ay":-1.58933, "alpha":23.52579, "fx":[-87.0842,-145.53228,-172.64621,-154.46182], "fy":[-151.71489,96.58478,29.67861,-82.68468]}, - {"t":0.09913, "x":7.06266, "y":7.25088, "heading":-3.04242, "vx":-0.7842, "vy":-0.13336, "omega":2.54984, "ax":-8.46063, "ay":-1.80449, "alpha":21.68972, "fx":[-95.52581,-152.79662,-173.61473,-153.71441], "fy":[-146.4378,84.57455,23.06643,-83.97862]}, - {"t":0.12391, "x":7.04063, "y":7.24702, "heading":-2.97923, "vx":-0.99387, "vy":-0.17808, "omega":3.08735, "ax":-8.73572, "ay":-2.02432, "alpha":19.33431, "fx":[-107.14827,-159.74871,-174.45511,-153.01649], "fy":[-138.01191,70.50382,14.90258,-85.12706]}, - {"t":0.14869, "x":7.01332, "y":7.24198, "heading":-2.90272, "vx":-1.21036, "vy":-0.22825, "omega":3.5665, "ax":-9.06522, "ay":-2.21792, "alpha":16.20435, "fx":[-122.74529,-166.22865,-174.94653,-152.86701], "fy":[-124.1455,53.34805,5.12373,-85.23078]}, - {"t":0.17347, "x":6.98054, "y":7.23564, "heading":-2.81433, "vx":-1.43501, "vy":-0.28321, "omega":3.96807, "ax":-9.43145, "ay":-2.35138, "alpha":12.15582, "fx":[-141.10641,-171.63703,-174.80007,-154.16147], "fy":[-102.55447,31.62677,-6.44378,-82.61388]}, - {"t":0.19826, "x":6.94208, "y":7.2279, "heading":-2.716, "vx":-1.66874, "vy":-0.34148, "omega":4.26932, "ax":-9.76129, "ay":-2.42534, "alpha":7.2671, "fx":[-157.73925,-174.41906,-173.56255,-158.42617], "fy":[-74.24463,3.21109,-20.32518,-73.65856]}, - {"t":0.22304, "x":6.89773, "y":7.2187, "heading":-2.61019, "vx":-1.91065, "vy":-0.40159, "omega":4.44941, "ax":-9.9405, "ay":-2.47823, "alpha":1.25928, "fx":[-168.04909,-170.86931,-170.14491,-167.27681], "fy":[-46.32684,-34.62353,-38.35241,-49.31288]}, - {"t":0.24782, "x":6.84733, "y":7.20798, "heading":-2.49993, "vx":-2.15699, "vy":-0.463, "omega":4.48062, "ax":-9.67218, "ay":-2.46894, "alpha":-7.9411, "fx":[-172.23636,-153.17038,-159.1224,-173.5546], "fy":[-27.10818,-83.02269,-69.12181,11.26877]}, - {"t":0.2726, "x":6.7909, "y":7.19575, "heading":-2.38889, "vx":-2.39669, "vy":-0.52419, "omega":4.28382, "ax":-8.03531, "ay":-2.89468, "alpha":-20.68597, "fx":[-173.06204,-121.90319,-100.52212,-151.2262], "fy":[-21.36318,-124.32222,-136.71036,85.44488]}, - {"t":0.29738, "x":6.72904, "y":7.18187, "heading":-2.28273, "vx":-2.59582, "vy":-0.59592, "omega":3.77118, "ax":-7.98471, "ay":-2.86633, "alpha":-20.2341, "fx":[-172.14467,-113.61948,-108.10361,-149.40284], "fy":[-26.98327,-131.60437,-124.85571,88.42154]}, - {"t":0.32217, "x":6.66226, "y":7.16622, "heading":-2.18927, "vx":-2.79369, "vy":-0.66696, "omega":3.26974, "ax":-8.05772, "ay":-2.51223, "alpha":-19.51942, "fx":[-170.87922,-104.2521,-124.36984,-148.7368], "fy":[-33.18119,-138.73515,-88.23776,89.22497]}, - {"t":0.34695, "x":6.59055, "y":7.14892, "heading":-2.10824, "vx":-2.99338, "vy":-0.72921, "omega":2.78601, "ax":-8.35988, "ay":-1.29962, "alpha":-19.10899, "fx":[-169.21757,-99.34506,-148.36273,-151.87159], "fy":[-39.64437,-141.72022,9.7752,83.1649]}, - {"t":0.37173, "x":6.5138, "y":7.13045, "heading":-2.0392, "vx":-3.20056, "vy":-0.76142, "omega":2.31245, "ax":-8.43258, "ay":-1.13451, "alpha":-19.04817, "fx":[-167.31977,-98.93031,-151.11059,-156.38225], "fy":[-45.41577,-141.16033,36.21738,73.16822]}, - {"t":0.39651, "x":6.4319, "y":7.11124, "heading":-1.98189, "vx":-3.40953, "vy":-0.78954, "omega":1.8404, "ax":-8.38399, "ay":-1.22903, "alpha":-19.12661, "fx":[-165.16596,-97.31507,-148.95931,-158.99698], "fy":[-50.42616,-140.95902,42.62483,65.13883]}, - {"t":0.42129, "x":6.34483, "y":7.09129, "heading":-1.93628, "vx":-3.6173, "vy":-0.81999, "omega":1.36641, "ax":-8.24036, "ay":-1.40332, "alpha":-19.19632, "fx":[-162.44403,-94.41293,-143.8638,-159.94393], "fy":[-54.72907,-140.53111,41.50857,58.27103]}, - {"t":0.44608, "x":6.25265, "y":7.07054, "heading":-1.90242, "vx":-3.82152, "vy":-0.85477, "omega":0.89068, "ax":-7.90498, "ay":-1.70117, "alpha":-18.92321, "fx":[-157.78554,-90.00294,-131.80805,-158.24949], "fy":[-58.15443,-137.56585,29.53281,50.44155]}, - {"t":0.47086, "x":6.15552, "y":7.04883, "heading":-1.88035, "vx":-4.01742, "vy":-0.89693, "omega":0.42173, "ax":-6.18299, "ay":-2.3984, "alpha":-15.55814, "fx":[-132.82425,-74.4432,-78.54897,-134.86748], "fy":[-58.74096,-111.03614,-16.0924,22.68507]}, - {"t":0.49564, "x":6.05406, "y":7.02587, "heading":-1.8699, "vx":-4.17064, "vy":-0.95637, "omega":0.03617, "ax":0.03375, "ay":-0.98753, "alpha":-0.45733, "fx":[-0.8304,1.31501,1.98088,-0.16893], "fy":[-17.52867,-18.17611,-16.06819,-15.41764]}, - {"t":0.52042, "x":5.95072, "y":7.00187, "heading":-1.869, "vx":-4.16981, "vy":-0.98084, "omega":0.02483, "ax":0.11087, "ay":-0.47691, "alpha":-0.00528, "fx":[1.86976,1.89438,1.90197,1.87735], "fy":[-8.12062,-8.12813,-8.1036,-8.09608]}, - {"t":0.5452, "x":5.84741, "y":6.97741, "heading":-1.86838, "vx":-4.16706, "vy":-0.99266, "omega":0.0247, "ax":0.12589, "ay":-0.52677, "alpha":-0.00174, "fx":[2.13609,2.1442,2.1467,2.13859], "fy":[-8.96296,-8.96542,-8.95734,-8.95487]}, - {"t":0.56999, "x":5.74418, "y":6.95265, "heading":-1.86777, "vx":-4.16394, "vy":-1.00571, "omega":0.02466, "ax":0.27955, "ay":-1.14363, "alpha":-0.00343, "fx":[4.7444,4.76055,4.7656,4.74945], "fy":[-19.45837,-19.46311,-19.44727,-19.44253]}, - {"t":0.59477, "x":5.64108, "y":6.92738, "heading":-1.86716, "vx":-4.15701, "vy":-1.03406, "omega":0.02457, "ax":0.72341, "ay":-2.80203, "alpha":-0.00062, "fx":[12.30287,12.30595,12.30701,12.30393], "fy":[-47.66282,-47.66353,-47.66079,-47.66007]}, - {"t":0.61955, "x":5.53828, "y":6.90089, "heading":-1.86655, "vx":-4.13908, "vy":-1.10349, "omega":0.02456, "ax":1.77185, "ay":-5.45068, "alpha":0.49502, "fx":[32.16871,29.40507,28.11753,30.86308], "fy":[-91.84043,-91.73063,-93.58977,-93.69746]}, - {"t":0.64433, "x":5.43625, "y":6.87187, "heading":-1.86594, "vx":-4.09517, "vy":-1.23857, "omega":0.03683, "ax":6.88677, "ay":-3.97417, "alpha":12.45183, "fx":[148.03672,129.8205,73.74712,116.96328], "fy":[-32.99933,-18.50706,-119.58754,-99.30381]}, - {"t":0.66911, "x":5.33688, "y":6.83995, "heading":-1.86503, "vx":-3.92451, "vy":-1.33706, "omega":0.34541, "ax":7.32463, "ay":-2.80149, "alpha":13.78679, "fx":[152.32346,132.96789,86.64327,126.42472], "fy":[-13.45203,16.40062,-106.54687,-87.01206]}, - {"t":0.68776, "x":5.26498, "y":6.81454, "heading":-1.85859, "vx":-3.78794, "vy":-1.3893, "omega":0.60247, "ax":7.03374, "ay":-1.61012, "alpha":13.88569, "fx":[145.61033,117.75228,88.52987,126.67542], "fy":[3.09498,39.31282,-81.41929,-70.53937]}, - {"t":0.7064, "x":5.19557, "y":6.78835, "heading":-1.84736, "vx":-3.65679, "vy":-1.41932, "omega":0.86138, "ax":6.47845, "ay":-0.3786, "alpha":13.63696, "fx":[135.38719,98.49702,83.41478,123.48701], "fy":[19.76855,57.67875,-51.36375,-51.84284]}, - {"t":0.72505, "x":5.12852, "y":6.76182, "heading":-1.8313, "vx":-3.53599, "vy":-1.42638, "omega":1.11564, "ax":5.67453, "ay":0.67769, "alpha":12.87124, "fx":[121.58479,78.55978,70.58495,115.3589], "fy":[33.39258,67.62618,-22.05846,-32.851]}, - {"t":0.7437, "x":5.06357, "y":6.73535, "heading":-1.81049, "vx":-3.43019, "vy":-1.41374, "omega":1.35564, "ax":4.71055, "ay":1.37058, "alpha":11.55865, "fx":[104.65773,60.34878,53.90525,101.58865], "fy":[41.65111,68.59778,-0.99657,-15.99948]}, - {"t":0.76234, "x":5.00043, "y":6.70922, "heading":-1.78522, "vx":-3.34236, "vy":-1.38819, "omega":1.57115, "ax":3.6799, "ay":1.5651, "alpha":9.71854, "fx":[85.1628,44.46984,38.13379,82.60985], "fy":[42.42887,60.77541,7.96513,-4.68179]}, - {"t":0.78099, "x":4.93875, "y":6.68361, "heading":-1.75592, "vx":-3.27374, "vy":-1.359, "omega":1.75236, "ax":2.68081, "ay":1.11392, "alpha":7.36005, "fx":[64.09888,31.4764,26.07912,60.74501], "fy":[32.27329,42.4528,4.23252,-3.16875]}, - {"t":0.79963, "x":4.87818, "y":6.65847, "heading":-1.72325, "vx":-3.22376, "vy":-1.33823, "omega":1.88959, "ax":1.88989, "ay":-0.24234, "alpha":4.51407, "fx":[44.30011,23.40996,19.9033,40.97235], "fy":[5.22991,9.00819,-13.99201,-16.7343]}, - {"t":0.81828, "x":4.8184, "y":6.63347, "heading":-1.68802, "vx":-3.18852, "vy":-1.34275, "omega":1.97376, "ax":1.63913, "ay":-2.63839, "alpha":1.57453, "fx":[32.61043,24.70189,23.21116,31.00088], "fy":[-41.33289,-41.0403,-48.47276,-48.66695]}, - {"t":0.83692, "x":4.75923, "y":6.60798, "heading":-1.65121, "vx":-3.15796, "vy":-1.39195, "omega":2.00312, "ax":2.17874, "ay":-5.1908, "alpha":-0.26053, "fx":[36.11228,37.61539,38.01122,36.50042], "fy":[-88.8991,-88.7115,-87.68829,-87.87696]}, - {"t":0.85557, "x":4.70073, "y":6.58112, "heading":-1.61387, "vx":-3.11733, "vy":-1.48873, "omega":1.99826, "ax":3.12801, "ay":-6.82555, "alpha":-0.72355, "fx":[50.0787,54.71831,56.37232,51.65659], "fy":[-117.92568,-116.63964,-114.24799,-115.58932]}, - {"t":0.87421, "x":4.64315, "y":6.55218, "heading":-1.57661, "vx":-3.05901, "vy":-1.616, "omega":1.98477, "ax":4.01654, "ay":-7.59625, "alpha":-0.6961, "fx":[65.00033,69.66957,71.67424,66.93636], "fy":[-131.19257,-129.30452,-127.18289,-129.15997]}, - {"t":0.89286, "x":4.58681, "y":6.52073, "heading":-1.5396, "vx":-2.98412, "vy":-1.75763, "omega":1.97179, "ax":4.7251, "ay":-7.88964, "alpha":-0.68492, "fx":[76.96208,81.59123,83.81212,79.12498], "fy":[-136.37896,-134.0326,-131.96673,-134.42408]}, - {"t":0.91151, "x":4.53199, "y":6.48658, "heading":-1.50283, "vx":-2.89602, "vy":-1.90474, "omega":1.95902, "ax":5.28262, "ay":-7.94977, "alpha":-0.80677, "fx":[85.78854,91.18614,93.95711,88.49182], "fy":[-138.02082,-134.83467,-132.33767,-135.69986]}, - {"t":0.93015, "x":4.47891, "y":6.44968, "heading":-1.46631, "vx":-2.79752, "vy":-2.05297, "omega":1.94398, "ax":5.73334, "ay":-7.89434, "alpha":-1.05541, "fx":[92.2242,99.15987,102.86904,95.83659], "fy":[-138.18936,-133.61145,-130.2085,-135.1126]}, - {"t":0.9488, "x":4.42775, "y":6.41003, "heading":-1.43006, "vx":-2.69062, "vy":-2.20016, "omega":1.9243, "ax":6.1096, "ay":-7.77844, "alpha":-1.4021, "fx":[96.9802,106.00002,110.93658,101.77343], "fy":[-137.77176,-131.27492,-126.543,-133.64629]}, - {"t":0.96744, "x":4.37864, "y":6.36766, "heading":-1.39418, "vx":-2.5767, "vy":-2.3452, "omega":1.89816, "ax":6.43229, "ay":-7.62883, "alpha":-1.82018, "fx":[100.5746,112.01939,118.3505,106.70127], "fy":[-137.1427,-128.28545,-121.85524,-131.77322]}, - {"t":0.98609, "x":4.33171, "y":6.3226, "heading":-1.35879, "vx":-2.45677, "vy":-2.48744, "omega":1.86422, "ax":6.71462, "ay":-7.45937, "alpha":-2.28884, "fx":[103.36177,117.41633,125.20361,110.87309], "fy":[-136.45734,-124.89101,-116.44315,-129.73523]}, - {"t":1.00473, "x":4.28707, "y":6.27493, "heading":-1.32403, "vx":-2.33157, "vy":-2.62652, "omega":1.82154, "ax":6.96503, "ay":-7.2778, "alpha":-2.79246, "fx":[105.58235,122.31729,131.5404,114.4524], "fy":[-135.77582,-121.23579,-110.49667,-127.66513]}, - {"t":1.02338, "x":4.24481, "y":6.22469, "heading":-1.29007, "vx":-2.20171, "vy":-2.76222, "omega":1.76947, "ax":7.18908, "ay":-7.08885, "alpha":-3.31934, "fx":[107.40091,126.80416,137.38249,117.54903], "fy":[-135.11734,-117.41101,-104.1485,-125.64037]}, - {"t":1.04202, "x":4.20501, "y":6.17196, "heading":-1.25707, "vx":-2.06766, "vy":-2.8944, "omega":1.70758, "ax":7.39123, "ay":-6.89459, "alpha":-3.86751, "fx":[108.91531,130.94987,142.78054,120.24513], "fy":[-134.49725,-113.45891,-97.44188,-123.70203]}, - {"t":1.06067, "x":4.16774, "y":6.11679, "heading":-1.22523, "vx":-1.92985, "vy":-3.02295, "omega":1.63547, "ax":7.60479, "ay":-6.64309, "alpha":-4.76867, "fx":[109.43197,135.66474,149.52683,122.79787], "fy":[-134.55361,-108.36827,-87.45173,-121.61477]}, - {"t":1.07932, "x":4.13308, "y":6.05927, "heading":-1.19474, "vx":-1.78805, "vy":-3.14682, "omega":1.54656, "ax":8.06947, "ay":-5.42331, "alpha":-10.83264, "fx":[97.70077,150.29478,172.01858,129.02314], "fy":[-143.77594,-87.97179,-23.39115,-113.85693]}, - {"t":1.09796, "x":4.10114, "y":5.99965, "heading":-1.1659, "vx":-1.63759, "vy":-3.24794, "omega":1.34458, "ax":9.28646, "ay":-3.26605, "alpha":-9.76006, "fx":[129.0325,163.84845,174.07082,164.88845], "fy":[-117.05485,-60.10432,7.93838,-52.99766]}, - {"t":1.12547, "x":4.05961, "y":5.90907, "heading":-1.12892, "vx":-1.38214, "vy":-3.33778, "omega":1.07609, "ax":9.74962, "ay":0.65126, "alpha":-10.80226, "fx":[162.2707,174.10401,162.20416,164.77412], "fy":[-62.39984,-10.16531,63.6715,53.20444]}, - {"t":1.15298, "x":4.02528, "y":5.8175, "heading":-1.09932, "vx":-1.11394, "vy":-3.31987, "omega":0.77894, "ax":8.79279, "ay":4.51999, "alpha":-9.55794, "fx":[171.43972,167.5875,136.85486,122.36948], "fy":[28.00713,48.16699,108.05914,123.30167]}, - {"t":1.18049, "x":3.99796, "y":5.72789, "heading":-1.07789, "vx":-0.87206, "vy":-3.19553, "omega":0.51602, "ax":7.06011, "ay":7.18744, "alpha":-6.63543, "fx":[139.15436,144.34455,109.61246,87.25074], "fy":[104.45102,97.98073,135.81897,150.77477]}, - {"t":1.20799, "x":3.97664, "y":5.6427, "heading":-1.06369, "vx":-0.67785, "vy":-2.99781, "omega":0.33349, "ax":5.43347, "ay":8.60705, "alpha":-4.55428, "fx":[101.81362,115.83158,86.77284,65.26879], "fy":[141.53097,130.64081,151.60135,161.84062]}, - {"t":1.2355, "x":3.96005, "y":5.56349, "heading":-1.05452, "vx":-0.52839, "vy":-2.76105, "omega":0.2082, "ax":4.2053, "ay":9.32675, "alpha":-3.17993, "fx":[75.25242,90.53383,69.09245,51.24524], "fy":[157.55452,149.44805,160.56118,167.01774]}, - {"t":1.26301, "x":3.94711, "y":5.49107, "heading":-1.04879, "vx":-0.4127, "vy":-2.50448, "omega":0.12073, "ax":3.31126, "ay":9.71002, "alpha":-2.21788, "fx":[57.44186,70.5139,55.57873,41.75975], "fy":[165.06711,159.99361,165.82224,169.77564]}, - {"t":1.29052, "x":3.93701, "y":5.42585, "heading":-1.04547, "vx":-0.32162, "vy":-2.23737, "omega":0.05972, "ax":2.65099, "ay":9.92714, "alpha":-1.51684, "fx":[45.13332,55.11507,45.13234,34.98986], "fy":[168.97214,166.02231,169.04013,171.39652]}, - {"t":1.31803, "x":3.92916, "y":5.36806, "heading":-1.04383, "vx":-0.24869, "vy":-1.96429, "omega":0.01799, "ax":2.15069, "ay":10.05706, "alpha":-0.98952, "fx":[36.26214,43.22037,36.90591,29.94231], "fy":[171.1839,169.5801,171.08411,172.4224]}, - {"t":1.34554, "x":3.92314, "y":5.31783, "heading":-1.04333, "vx":-0.18953, "vy":-1.68764, "omega":-0.00923, "ax":1.7616, "ay":10.13835, "alpha":-0.58166, "fx":[29.62065,33.8912,30.30011,26.04495], "fy":[172.52305,171.74443,172.42445,173.10979]}, - {"t":1.37305, "x":3.91859, "y":5.27524, "heading":-1.04359, "vx":-0.14107, "vy":-1.40875, "omega":-0.02523, "ax":1.45178, "ay":10.19105, "alpha":-0.25839, "fx":[24.48766,26.44202,24.89818,22.9496], "fy":[173.37688,173.09303,173.32606,173.59149]}, - {"t":1.40055, "x":3.91526, "y":5.24035, "heading":-1.04428, "vx":-0.10114, "vy":-1.12841, "omega":-0.03234, "ax":1.19999, "ay":10.22616, "alpha":0.00332, "fx":[20.41475,20.38912,20.40832,20.43395], "fy":[173.94375,173.94672,173.94441,173.94144]}, - {"t":1.42806, "x":3.91293, "y":5.21317, "heading":-1.04517, "vx":-0.06813, "vy":-0.84711, "omega":-0.03224, "ax":0.99173, "ay":10.25003, "alpha":0.21909, "fx":[17.11128,15.39131,16.62312,18.35017], "fy":[174.33239,174.49047,174.37432,174.20329]}, - {"t":1.45557, "x":3.91143, "y":5.19375, "heading":-1.04606, "vx":-0.04084, "vy":-0.56514, "omega":-0.02622, "ax":0.81682, "ay":10.26649, "alpha":0.39982, "fx":[14.38168,11.20499,13.39248,16.59645], "fy":[174.60577,174.83487,174.6756,174.40406]}, - {"t":1.48308, "x":3.91062, "y":5.18209, "heading":-1.04678, "vx":-0.01838, "vy":-0.28273, "omega":-0.01522, "ax":0.66799, "ay":10.27793, "alpha":0.55325, "fx":[12.08992,7.65329,10.60582,15.10058], "fy":[174.80211,175.04814,174.88717,174.5612]}, - {"t":1.51059, "x":3.91036, "y":5.1782, "heading":-1.0472, "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.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.1157, "ay":-0.62485, "alpha":44.73771, "fx":[-17.60513,-31.32055,-188.65648,-170.20095], "fy":[-198.20588,196.19354,63.80111,-103.45237]}, + {"t":0.02813, "x":7.09847, "y":7.25678, "heading":3.14159, "vx":-0.17202, "vy":-0.01757, "omega":1.25833, "ax":-6.54037, "ay":-0.70208, "alpha":42.97268, "fx":[-26.6454,-49.00594,-189.71499,-170.73279], "fy":[-197.11386,192.36322,60.43095,-102.49351]}, + {"t":0.05625, "x":7.09104, "y":7.25601, "heading":-3.1062, "vx":-0.35598, "vy":-0.03732, "omega":2.46702, "ax":-7.16306, "ay":-0.99789, "alpha":40.14913, "fx":[-35.55912,-80.34168,-192.13587,-169.58237], "fy":[-195.56522,181.31689,51.98,-104.26883]}, + {"t":0.08438, "x":7.0782, "y":7.25457, "heading":-3.03681, "vx":-0.55745, "vy":-0.06539, "omega":3.59629, "ax":-8.0428, "ay":-1.59248, "alpha":35.52511, "fx":[-50.93333,-122.63705,-195.33555,-167.37252], "fy":[-191.8851,155.58511,37.70776,-107.59133]}, + {"t":0.11251, "x":7.05934, "y":7.2521, "heading":-2.93566, "vx":-0.78367, "vy":-0.11018, "omega":4.5955, "ax":-9.32965, "ay":-2.5334, "alpha":27.0282, "fx":[-88.57947,-169.37678,-198.1552,-165.97166], "fy":[-177.17232,102.25272,15.37307,-109.37602]}, + {"t":0.14063, "x":7.0336, "y":7.248, "heading":-2.8064, "vx":-1.04608, "vy":-0.18144, "omega":5.35572, "ax":-10.92183, "ay":-3.54645, "alpha":10.69184, "fx":[-161.05411,-197.58186,-196.66268,-172.94806], "fy":[-114.40487,-0.21714,-24.96902,-96.87976]}, + {"t":0.16876, "x":6.99986, "y":7.24149, "heading":-2.65576, "vx":-1.35328, "vy":-0.28119, "omega":5.65644, "ax":-9.37215, "ay":-3.9267, "alpha":-22.85924, "fx":[-196.42171,-148.1804,-94.51563,-185.79891], "fy":[-22.6905,-130.80915,-171.00542,62.6803]}, + {"t":0.19689, "x":6.95809, "y":7.23203, "heading":-2.49666, "vx":-1.61689, "vy":-0.39163, "omega":5.01349, "ax":-9.12241, "ay":-3.26395, "alpha":-25.16118, "fx":[-196.86511,-143.93228,-97.98769,-169.4799], "fy":[-16.61758,-134.87576,-165.07479,98.93397]}, + {"t":0.22501, "x":6.909, "y":7.21972, "heading":-2.35565, "vx":-1.87347, "vy":-0.48344, "omega":4.30578, "ax":-9.22183, "ay":-2.80837, "alpha":-23.88119, "fx":[-196.49932,-137.34766,-119.02262,-162.0244], "fy":[-18.12974,-140.83779,-139.01246,110.72344]}, + {"t":0.25314, "x":6.85266, "y":7.20501, "heading":-2.23454, "vx":-2.13285, "vy":-0.56243, "omega":3.63408, "ax":-9.4544, "ay":-1.45363, "alpha":-22.52847, "fx":[-195.41558,-126.66444,-148.18373,-160.13741], "fy":[-24.88242,-149.61475,-35.4415,113.01324]}, + {"t":0.28127, "x":6.78893, "y":7.18862, "heading":-2.13233, "vx":-2.39878, "vy":-0.60332, "omega":3.00042, "ax":-9.73696, "ay":-0.53039, "alpha":-21.83634, "fx":[-193.32601,-124.99058,-162.25397,-168.67146], "fy":[-34.52587,-149.65435,50.28121,98.5334]}, + {"t":0.3094, "x":6.71761, "y":7.17144, "heading":-2.04793, "vx":-2.67265, "vy":-0.61823, "omega":2.38623, "ax":-9.71163, "ay":-0.79368, "alpha":-21.70631, "fx":[-190.32886,-121.13105,-161.53137,-174.56124], "fy":[-44.21092,-150.75227,57.02134,85.02097]}, + {"t":0.33752, "x":6.63859, "y":7.15374, "heading":-1.98082, "vx":-2.94581, "vy":-0.64056, "omega":1.7757, "ax":-9.49721, "ay":-1.25037, "alpha":-21.7262, "fx":[-186.0021,-113.81927,-156.02013,-177.41451], "fy":[-53.55358,-152.82043,49.95491,73.04686]}, + {"t":0.36565, "x":6.55198, "y":7.13523, "heading":-1.93087, "vx":-3.21293, "vy":-0.67573, "omega":1.16461, "ax":-8.98954, "ay":-1.87229, "alpha":-21.34461, "fx":[-178.67419,-104.08565,-140.50577,-176.13938], "fy":[-61.56757,-151.47154,28.45134,59.74716]}, + {"t":0.39378, "x":6.45806, "y":7.11548, "heading":-1.89811, "vx":-3.46578, "vy":-0.72839, "omega":0.56426, "ax":-6.99377, "ay":-2.63156, "alpha":-17.47355, "fx":[-149.00611,-84.28652,-83.92893,-149.10942], "fy":[-63.40593,-121.69293,-17.27627,26.90754]}, + {"t":0.4219, "x":6.35781, "y":7.09395, "heading":-1.88224, "vx":-3.66249, "vy":-0.80241, "omega":0.07278, "ax":-0.22051, "ay":-0.68203, "alpha":-0.86342, "fx":[-6.32395,-2.31169,-1.02262,-5.04505], "fy":[-12.71763,-14.00932,-10.02235,-8.7274]}, + {"t":0.45003, "x":6.25471, "y":7.07111, "heading":-1.8802, "vx":-3.66869, "vy":-0.82159, "omega":0.0485, "ax":0.02439, "ay":-0.13426, "alpha":-0.0132, "fx":[0.36612,0.42735,0.44693,0.3857], "fy":[-2.25882,-2.27839,-2.21717,-2.19761]}, + {"t":0.47816, "x":6.15153, "y":7.04795, "heading":-1.87883, "vx":-3.66801, "vy":-0.82537, "omega":0.04812, "ax":0.00676, "ay":-0.0306, "alpha":0.00019, "fx":[0.11335,0.11245,0.11216,0.11306], "fy":[-0.50982,-0.50953,-0.51044,-0.51072]}, + {"t":0.50628, "x":6.04836, "y":7.02472, "heading":-1.87748, "vx":-3.66782, "vy":-0.82623, "omega":0.04813, "ax":0.00345, "ay":-0.01511, "alpha":0.00066, "fx":[0.05957,0.05652,0.05556,0.0586], "fy":[-0.25082,-0.24986,-0.2529,-0.25387]}, + {"t":0.53441, "x":5.9452, "y":7.00148, "heading":-1.87613, "vx":-3.66772, "vy":-0.82665, "omega":0.04815, "ax":0.00971, "ay":-0.04371, "alpha":0.0001, "fx":[0.16222,0.16174,0.16159,0.16207], "fy":[-0.72854,-0.72839,-0.72886,-0.72901]}, + {"t":0.56254, "x":5.84204, "y":6.97821, "heading":-1.87477, "vx":-3.66745, "vy":-0.82788, "omega":0.04815, "ax":0.04323, "ay":-0.1959, "alpha":-0.00277, "fx":[0.7121,0.72498,0.72902,0.71614], "fy":[-3.27006,-3.27409,-3.26122,-3.25719]}, + {"t":0.59066, "x":5.7389, "y":6.95485, "heading":-1.87342, "vx":-3.66623, "vy":-0.83339, "omega":0.04807, "ax":0.20104, "ay":-0.88906, "alpha":-0.01239, "fx":[3.31312,3.37098,3.38921,3.33134], "fy":[-14.84,-14.85775,-14.8004,-14.78265]}, + {"t":0.61879, "x":5.63586, "y":6.93105, "heading":-1.87206, "vx":-3.66058, "vy":-0.8584, "omega":0.04772, "ax":0.94582, "ay":-3.64006, "alpha":0.07557, "fx":[16.02245,15.64573,15.5103,15.88675], "fy":[-60.55634,-60.47337,-60.79986,-60.88262]}, + {"t":0.64692, "x":5.53328, "y":6.90547, "heading":-1.87072, "vx":-3.63398, "vy":-0.96078, "omega":0.04985, "ax":3.94253, "ay":-7.68502, "alpha":3.64959, "fx":[82.65895,62.80932,48.70948,68.70272], "fy":[-119.20456,-122.96251,-136.77856,-133.47656]}, + {"t":0.67504, "x":5.43262, "y":6.87541, "heading":-1.86932, "vx":-3.52308, "vy":-1.17694, "omega":0.1525, "ax":8.47278, "ay":-5.93608, "alpha":13.52403, "fx":[178.01654,166.3716,84.84064,135.71993], "fy":[-54.86856,-51.36273,-160.0326,-129.54278]}, + {"t":0.70317, "x":5.33688, "y":6.83995, "heading":-1.86503, "vx":-3.28477, "vy":-1.3439, "omega":0.53289, "ax":8.93072, "ay":-5.08812, "alpha":13.99414, "fx":[181.44178,172.10326,98.28835,143.65013], "fy":[-41.5296,-27.44376,-150.37629,-119.91652]}, + {"t":0.72589, "x":5.26455, "y":6.8081, "heading":-1.85292, "vx":-3.08184, "vy":-1.45951, "omega":0.85087, "ax":9.13687, "ay":-2.72951, "alpha":15.91799, "fx":[179.83887,160.38464,115.03288,153.97278], "fy":[-5.71711,34.34237,-117.41609,-93.20768]}, + {"t":0.74862, "x":5.19688, "y":6.77424, "heading":-1.83359, "vx":-2.87423, "vy":-1.52154, "omega":1.21257, "ax":8.57898, "ay":-0.07937, "alpha":17.18463, "fx":[168.70384,126.40422,118.86867,158.05284], "fy":[32.65686,88.14627,-65.84781,-60.24733]}, + {"t":0.77134, "x":5.13378, "y":6.73964, "heading":-1.80604, "vx":-2.67929, "vy":-1.52334, "omega":1.60305, "ax":7.22157, "ay":2.26137, "alpha":17.03529, "fx":[148.37913,86.62482,95.19271,151.32395], "fy":[63.66266,115.8189,-2.92447,-25.77334]}, + {"t":0.79406, "x":5.07477, "y":6.70561, "heading":-1.76961, "vx":-2.5152, "vy":-1.47196, "omega":1.99013, "ax":5.40425, "ay":3.80426, "alpha":15.43728, "fx":[120.64324,54.11673,55.17402,130.41073], "fy":[83.03104,120.24775,42.10775,8.27423]}, + {"t":0.81678, "x":5.01901, "y":6.67315, "heading":-1.72439, "vx":-2.3924, "vy":-1.38551, "omega":2.3409, "ax":3.26022, "ay":4.35551, "alpha":12.29552, "fx":[83.16351,24.75907,19.17895,90.28351], "fy":[89.70321,108.97929,56.18027,35.55414]}, + {"t":0.83951, "x":4.96549, "y":6.64279, "heading":-1.6712, "vx":-2.31832, "vy":-1.28655, "omega":2.62029, "ax":0.64541, "ay":3.82085, "alpha":7.09063, "fx":[29.98602,-5.82806,-10.58391,29.46096], "fy":[77.02498,81.1501,50.52942,46.06254]}, + {"t":0.86223, "x":4.91298, "y":6.61454, "heading":-1.61166, "vx":-2.30366, "vy":-1.19973, "omega":2.78141, "ax":-2.10832, "ay":2.07939, "alpha":0.09089, "fx":[-34.90179,-35.34249,-35.38779,-34.94684], "fy":[34.88897,34.8798,34.43618,34.44503]}, + {"t":0.88495, "x":4.86009, "y":6.58782, "heading":-1.54846, "vx":-2.35157, "vy":-1.15248, "omega":2.78347, "ax":-3.96006, "ay":-0.16256, "alpha":-6.06555, "fx":[-78.73525,-52.4374,-53.31755,-79.55927], "fy":[-18.54305,-19.67698,14.72746,12.65308]}, + {"t":0.90767, "x":4.80564, "y":6.56159, "heading":-1.48521, "vx":-2.44155, "vy":-1.15617, "omega":2.64565, "ax":-4.71125, "ay":-2.18804, "alpha":-10.20306, "fx":[-94.73409,-51.62488,-62.77326,-105.00478], "fy":[-61.16996,-68.09378,-6.62758,-10.00286]}, + {"t":0.9304, "x":4.74894, "y":6.53475, "heading":-1.4251, "vx":-2.5486, "vy":-1.20589, "omega":2.41381, "ax":-4.72431, "ay":-3.96384, "alpha":-12.95196, "fx":[-95.38078,-37.54852,-63.40477,-118.67389], "fy":[-94.38589,-105.83992,-31.18423,-32.89119]}, + {"t":0.95312, "x":4.68981, "y":6.50633, "heading":-1.37025, "vx":-2.65595, "vy":-1.29596, "omega":2.11951, "ax":-4.18142, "ay":-5.59395, "alpha":-14.69294, "fx":[-86.94297,-15.31717,-52.51582,-124.03326], "fy":[-121.70266,-133.29172,-60.9187,-57.08065]}, + {"t":0.97584, "x":4.62838, "y":6.47544, "heading":-1.32209, "vx":-2.75096, "vy":-1.42307, "omega":1.78565, "ax":-3.0722, "ay":-7.16215, "alpha":-15.37198, "fx":[-71.03713,10.91863,-24.62151,-120.10807], "fy":[-144.74257,-151.33403,-96.38344,-85.09828]}, + {"t":0.99856, "x":4.56508, "y":6.44125, "heading":-1.28151, "vx":-2.82077, "vy":-1.58581, "omega":1.43636, "ax":-1.35281, "ay":-8.60779, "alpha":-14.72203, "fx":[-46.7454,37.57243,19.15583,-100.18544], "fy":[-163.78884,-161.20459,-128.848,-120.10929]}, + {"t":1.02129, "x":4.50064, "y":6.403, "heading":-1.24887, "vx":-2.85151, "vy":-1.7814, "omega":1.10184, "ax":0.86495, "ay":-9.63649, "alpha":-12.60259, "fx":[-13.27429,63.63627,63.29937,-55.98795], "fy":[-177.21342,-164.12508,-144.95773,-156.24668]}, + {"t":1.04401, "x":4.43607, "y":6.36003, "heading":-1.22384, "vx":-2.83185, "vy":-2.00036, "omega":0.81547, "ax":3.21544, "ay":-9.98734, "alpha":-9.53622, "fx":[25.3993,87.79833,96.61742,4.58416], "fy":[-182.0148,-161.27315,-146.59919,-176.04955]}, + {"t":1.06673, "x":4.37255, "y":6.312, "heading":-1.20531, "vx":-2.75879, "vy":-2.2273, "omega":0.59879, "ax":5.16803, "ay":-9.76082, "alpha":-6.79926, "fx":[61.17725,107.52136,118.41127,57.48458], "fy":[-178.23708,-155.12384,-142.29765,-175.17406]}, + {"t":1.08945, "x":4.3112, "y":6.25887, "heading":-1.1917, "vx":-2.64136, "vy":-2.44909, "omega":0.44429, "ax":6.55275, "ay":-9.28605, "alpha":-5.0935, "fx":[88.48605,122.79967,133.32178,92.31705], "fy":[-170.09908,-147.68553,-135.76764,-165.62398]}, + {"t":1.11218, "x":4.25287, "y":6.20082, "heading":-1.18161, "vx":-2.49246, "vy":-2.66009, "omega":0.32855, "ax":7.4959, "ay":-8.77904, "alpha":-4.33547, "fx":[106.76814,134.6558,144.93175,113.45669], "fy":[-161.91402,-139.97334,-127.74204,-155.74]}, + {"t":1.1349, "x":4.19817, "y":6.13811, "heading":-1.17414, "vx":-2.32214, "vy":-2.85957, "omega":0.23004, "ax":8.133, "ay":-8.32925, "alpha":-4.29293, "fx":[117.66709,143.75445,154.70642,126.16503], "fy":[-156.01741,-132.7251,-118.59864,-148.03703]}, + {"t":1.15762, "x":4.14751, "y":6.07099, "heading":-1.16891, "vx":-2.13734, "vy":-3.04884, "omega":0.13249, "ax":8.5349, "ay":-7.96622, "alpha":-5.16145, "fx":[121.2479,150.92375,164.50691,132.41248], "fy":[-154.53654,-126.07393,-106.61883,-143.94294]}, + {"t":1.18034, "x":4.10114, "y":5.99965, "heading":-1.1659, "vx":-1.9434, "vy":-3.22985, "omega":0.01521, "ax":10.29218, "ay":-5.78236, "alpha":-0.42263, "fx":[170.16882,171.78838,172.95089,171.35474], "fy":[-98.86575,-96.08848,-93.89351,-96.70861]}, + {"t":1.20997, "x":4.04809, "y":5.90144, "heading":-1.16545, "vx":-1.63851, "vy":-3.40114, "omega":0.00269, "ax":10.79392, "ay":-4.61571, "alpha":-0.01245, "fx":[179.89643,179.93173,179.96252,179.92725], "fy":[-77.0191,-76.94049,-76.86435,-76.94298]}, + {"t":1.23959, "x":4.00429, "y":5.79866, "heading":-1.16537, "vx":-1.31876, "vy":-3.53787, "omega":0.00233, "ax":11.17062, "ay":-3.25467, "alpha":0.7252, "fx":[187.54558,186.24194,184.81322,186.23502], "fy":[-49.61491,-53.76337,-58.89901,-54.73762]}, + {"t":1.26921, "x":3.97012, "y":5.69243, "heading":-1.1653, "vx":-0.98785, "vy":-3.63429, "omega":0.02381, "ax":8.67867, "ay":6.68088, "alpha":14.65671, "fx":[132.05344,78.84021,183.91266,183.87101], "fy":[144.99921,177.317,55.83424,67.3179]}, + {"t":1.29884, "x":3.94467, "y":5.5877, "heading":-1.1646, "vx":-0.73076, "vy":-3.43638, "omega":0.45799, "ax":4.70137, "ay":10.82216, "alpha":3.4575, "fx":[78.14076,56.39978,79.30123,99.63661], "fy":[181.50719,189.18004,180.47844,170.43525]}, + {"t":1.32846, "x":3.92508, "y":5.49065, "heading":-1.15103, "vx":-0.5915, "vy":-3.11579, "omega":0.56041, "ax":3.35382, "ay":11.40537, "alpha":0.3986, "fx":[56.11924,53.2973,55.69327,58.5163], "fy":[190.08609,190.88119,190.17709,189.34369]}, + {"t":1.35808, "x":3.90903, "y":5.40336, "heading":-1.13443, "vx":-0.49214, "vy":-2.77793, "omega":0.57222, "ax":2.74469, "ay":11.58551, "alpha":-0.95691, "fx":[44.991,52.10656,46.4898,39.42327], "fy":[193.32998,191.56193,193.03285,194.57511]}, + {"t":1.38771, "x":3.89566, "y":5.32615, "heading":-1.11748, "vx":-0.41084, "vy":-2.43473, "omega":0.54387, "ax":2.40252, "ay":11.66572, "alpha":-1.70898, "fx":[38.50271,51.49288,41.48892,28.71064], "fy":[194.90903,191.92531,194.3751,196.63806]}, + {"t":1.41733, "x":3.88454, "y":5.25914, "heading":-1.10137, "vx":-0.33967, "vy":-2.08915, "omega":0.49324, "ax":2.18425, "ay":11.70925, "alpha":-2.18489, "fx":[34.35692,51.1239,38.27362,21.88719], "fy":[195.80355,192.15018,195.15905,197.63739]}, + {"t":1.44695, "x":3.87544, "y":5.20239, "heading":-1.08676, "vx":-0.27496, "vy":-1.74229, "omega":0.42852, "ax":2.03316, "ay":11.73597, "alpha":-2.51246, "fx":[31.53059,50.87832,35.99592,17.16227], "fy":[196.36454,192.30294,195.67621,198.18814]}, + {"t":1.47658, "x":3.86818, "y":5.15593, "heading":-1.07406, "vx":-0.21473, "vy":-1.39463, "omega":0.35409, "ax":1.92246, "ay":11.75379, "alpha":-2.75142, "fx":[29.50137,50.70233,34.28551,13.6965], "fy":[196.74386,192.41367,196.04359,198.51927]}, + {"t":1.5062, "x":3.86267, "y":5.11977, "heading":-1.06357, "vx":-0.15779, "vy":-1.04644, "omega":0.27259, "ax":1.8379, "ay":11.76642, "alpha":-2.93334, "fx":[27.97471,50.56909,32.9565,11.04719], "fy":[197.01649,192.49783,196.31686,198.73106]}, + {"t":1.53582, "x":3.8588, "y":5.09394, "heading":-1.0555, "vx":-0.10334, "vy":-0.69788, "omega":0.18569, "ax":1.77122, "ay":11.77577, "alpha":-3.07646, "fx":[26.77162,50.46419,31.90601,8.95953], "fy":[197.22325,192.56406,196.52577,198.87288]}, + {"t":1.56545, "x":3.85651, "y":5.07843, "heading":-1.05, "vx":-0.05087, "vy":-0.34905, "omega":0.09456, "ax":1.71729, "ay":11.78295, "alpha":-3.19203, "fx":[25.77683,50.37938,31.07292,7.27676], "fy":[197.38812,192.61751,196.68761,198.97126]}, + {"t":1.59507, "x":3.85576, "y":5.07326, "heading":-1.0472, "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/startGToG.traj b/src/main/deploy/choreo/startGToG.traj index d134f654..e995b6b6 100644 --- a/src/main/deploy/choreo/startGToG.traj +++ b/src/main/deploy/choreo/startGToG.traj @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startG.x", "val":7.1008875}, "y":{"exp":"startG.y", "val":3.6225774894}, "heading":{"exp":"startG.heading", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"startG.x", "val":7.1008875}, "y":{"exp":"startG.y", "val":3.6225774894}, "heading":{"exp":"startG.heading", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "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}, diff --git a/src/main/deploy/choreo/startHToH.traj b/src/main/deploy/choreo/startHToH.traj index 265ae220..7e9dd17a 100644 --- a/src/main/deploy/choreo/startHToH.traj +++ b/src/main/deploy/choreo/startHToH.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":3.9512534894, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":3.9512534894, "heading":3.141592653589793, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.713146, "y":4.0509, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":3.9512534894}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":3.9512534894}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.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}, @@ -26,24 +26,23 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.71864], + "waypoints":[0.0,0.68648], "samples":[ - {"t":0.0, "x":7.10089, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.30717, "ay":0.0, "alpha":0.0, "fx":[-175.32205,-175.32205,-175.32205,-175.32205], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.04791, "x":7.08906, "y":3.95125, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":-10.30564, "ay":0.0, "alpha":0.0, "fx":[-175.29602,-175.29602,-175.29602,-175.29602], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.09582, "x":7.05357, "y":3.95125, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":-10.3035, "ay":0.0, "alpha":0.0, "fx":[-175.2596,-175.2596,-175.2596,-175.2596], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.14373, "x":6.99444, "y":3.95125, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":-10.30029, "ay":0.0, "alpha":0.0, "fx":[-175.20495,-175.20495,-175.20495,-175.20495], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.19164, "x":6.91165, "y":3.95125, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":-10.29493, "ay":0.0, "alpha":0.0, "fx":[-175.11389,-175.11389,-175.11389,-175.11389], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.23955, "x":6.80523, "y":3.95125, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":-10.28423, "ay":0.0, "alpha":0.0, "fx":[-174.93185,-174.93185,-174.93185,-174.93185], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.28746, "x":6.67519, "y":3.95125, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":-10.25218, "ay":0.0, "alpha":0.0, "fx":[-174.3866,-174.3866,-174.3866,-174.3866], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.33537, "x":6.52159, "y":3.95125, "heading":3.14159, "vx":-3.45178, "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.38328, "x":6.35621, "y":3.95125, "heading":3.14159, "vx":-3.45178, "vy":0.0, "omega":0.0, "ax":10.25218, "ay":0.0, "alpha":0.0, "fx":[174.3866,174.3866,174.3866,174.3866], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.43118, "x":6.20261, "y":3.95125, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":10.28423, "ay":0.0, "alpha":0.0, "fx":[174.93185,174.93185,174.93185,174.93185], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.47909, "x":6.07257, "y":3.95125, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":10.29493, "ay":0.0, "alpha":0.0, "fx":[175.11389,175.11389,175.11389,175.11389], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.527, "x":5.96615, "y":3.95125, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":10.30029, "ay":0.0, "alpha":0.0, "fx":[175.20495,175.20495,175.20495,175.20495], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.57491, "x":5.88336, "y":3.95125, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":10.3035, "ay":0.0, "alpha":0.0, "fx":[175.2596,175.2596,175.2596,175.2596], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.62282, "x":5.82423, "y":3.95125, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":10.30564, "ay":0.0, "alpha":0.0, "fx":[175.29602,175.29602,175.29602,175.29602], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.67073, "x":5.78874, "y":3.95125, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":10.30717, "ay":0.0, "alpha":0.0, "fx":[175.32205,175.32205,175.32205,175.32205], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.71864, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-11.91965, "ay":0.85589, "alpha":0.0, "fx":[-198.69483,-198.69483,-198.69483,-198.69483], "fy":[14.26724,14.26724,14.26724,14.26724]}, + {"t":0.04903, "x":7.08656, "y":3.95228, "heading":3.14159, "vx":-0.58447, "vy":0.04197, "omega":0.0, "ax":-11.91755, "ay":0.85574, "alpha":0.0, "fx":[-198.65988,-198.65988,-198.65988,-198.65988], "fy":[14.26473,14.26473,14.26473,14.26473]}, + {"t":0.09807, "x":7.04357, "y":3.95537, "heading":3.14159, "vx":-1.16885, "vy":0.08393, "omega":0.0, "ax":-11.91441, "ay":0.85551, "alpha":0.0, "fx":[-198.60745,-198.60745,-198.60745,-198.60745], "fy":[14.26097,14.26097,14.26097,14.26097]}, + {"t":0.1471, "x":6.97193, "y":3.96051, "heading":3.14159, "vx":-1.75306, "vy":0.12588, "omega":0.0, "ax":-11.90916, "ay":0.85514, "alpha":0.0, "fx":[-198.52004,-198.52004,-198.52004,-198.52004], "fy":[14.25469,14.25469,14.25469,14.25469]}, + {"t":0.19614, "x":6.87166, "y":3.96771, "heading":3.14159, "vx":-2.33702, "vy":0.16781, "omega":0.0, "ax":-11.89868, "ay":0.85438, "alpha":0.0, "fx":[-198.34521,-198.34521,-198.34521,-198.34521], "fy":[14.24214,14.24214,14.24214,14.24214]}, + {"t":0.24517, "x":6.74276, "y":3.97697, "heading":3.14159, "vx":-2.92047, "vy":0.2097, "omega":0.0, "ax":-11.86726, "ay":0.85213, "alpha":0.0, "fx":[-197.82156,-197.82156,-197.82156,-197.82156], "fy":[14.20454,14.20454,14.20454,14.20454]}, + {"t":0.29421, "x":6.58529, "y":3.98828, "heading":3.14159, "vx":-3.50238, "vy":0.25149, "omega":0.0, "ax":-5.43311, "ay":0.39012, "alpha":0.0, "fx":[-90.56729,-90.56729,-90.56729,-90.56729], "fy":[6.50317,6.50317,6.50317,6.50317]}, + {"t":0.34324, "x":6.40702, "y":4.00108, "heading":3.14159, "vx":-3.76879, "vy":0.27062, "omega":0.0, "ax":5.43311, "ay":-0.39012, "alpha":0.0, "fx":[90.56729,90.56729,90.56729,90.56729], "fy":[-6.50317,-6.50317,-6.50317,-6.50317]}, + {"t":0.39228, "x":6.22875, "y":4.01388, "heading":3.14159, "vx":-3.50238, "vy":0.25149, "omega":0.0, "ax":11.86726, "ay":-0.85213, "alpha":0.0, "fx":[197.82156,197.82156,197.82156,197.82156], "fy":[-14.20454,-14.20454,-14.20454,-14.20454]}, + {"t":0.44131, "x":6.07128, "y":4.02518, "heading":3.14159, "vx":-2.92047, "vy":0.2097, "omega":0.0, "ax":11.89868, "ay":-0.85438, "alpha":0.0, "fx":[198.34521,198.34521,198.34521,198.34521], "fy":[-14.24214,-14.24214,-14.24214,-14.24214]}, + {"t":0.49035, "x":5.94238, "y":4.03444, "heading":3.14159, "vx":-2.33702, "vy":0.16781, "omega":0.0, "ax":11.90916, "ay":-0.85514, "alpha":0.0, "fx":[198.52004,198.52004,198.52004,198.52004], "fy":[-14.25469,-14.25469,-14.25469,-14.25469]}, + {"t":0.53938, "x":5.8421, "y":4.04164, "heading":3.14159, "vx":-1.75306, "vy":0.12588, "omega":0.0, "ax":11.91441, "ay":-0.85551, "alpha":0.0, "fx":[198.60745,198.60745,198.60745,198.60745], "fy":[-14.26097,-14.26097,-14.26097,-14.26097]}, + {"t":0.58841, "x":5.77046, "y":4.04678, "heading":3.14159, "vx":-1.16885, "vy":0.08393, "omega":0.0, "ax":11.91755, "ay":-0.85574, "alpha":0.0, "fx":[198.65988,198.65988,198.65988,198.65988], "fy":[-14.26473,-14.26473,-14.26473,-14.26473]}, + {"t":0.63745, "x":5.72748, "y":4.04987, "heading":3.14159, "vx":-0.58447, "vy":0.04197, "omega":0.0, "ax":11.91965, "ay":-0.85589, "alpha":0.0, "fx":[198.69483,198.69483,198.69483,198.69483], "fy":[-14.26724,-14.26724,-14.26724,-14.26724]}, + {"t":0.68648, "x":5.71315, "y":4.0509, "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/startToG.traj b/src/main/deploy/choreo/startToG.traj index 313008ea..5fa6c15b 100644 --- a/src/main/deploy/choreo/startToG.traj +++ b/src/main/deploy/choreo/startToG.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.713146, "y":3.7209, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"G.x", "val":5.7769125}, "y":{"exp":"G.y", "val":3.6225774894}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "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}, @@ -26,27 +26,26 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.87609], + "waypoints":[0.0,0.83154], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.94202, "ay":-7.61907, "alpha":0.0, "fx":[-118.08182,-118.08182,-118.08182,-118.08182], "fy":[-129.59826,-129.59826,-129.59826,-129.59826]}, - {"t":0.04867, "x":7.09266, "y":5.06665, "heading":3.14159, "vx":-0.33788, "vy":-0.37083, "omega":0.0, "ax":-6.94116, "ay":-7.61813, "alpha":0.0, "fx":[-118.06713,-118.06713,-118.06713,-118.06713], "fy":[-129.58214,-129.58214,-129.58214,-129.58214]}, - {"t":0.09734, "x":7.068, "y":5.03958, "heading":3.14159, "vx":-0.67572, "vy":-0.74162, "omega":0.0, "ax":-6.94001, "ay":-7.61686, "alpha":0.0, "fx":[-118.04755,-118.04755,-118.04755,-118.04755], "fy":[-129.56064,-129.56064,-129.56064,-129.56064]}, - {"t":0.14601, "x":7.02689, "y":4.99446, "heading":3.14159, "vx":-1.0135, "vy":-1.11234, "omega":0.0, "ax":-6.9384, "ay":-7.61509, "alpha":0.0, "fx":[-118.02013,-118.02013,-118.02013,-118.02013], "fy":[-129.53055,-129.53055,-129.53055,-129.53055]}, - {"t":0.19469, "x":6.96934, "y":4.93131, "heading":3.14159, "vx":-1.3512, "vy":-1.48298, "omega":0.0, "ax":-6.93598, "ay":-7.61244, "alpha":0.0, "fx":[-117.97901,-117.97901,-117.97901,-117.97901], "fy":[-129.48542,-129.48542,-129.48542,-129.48542]}, - {"t":0.24336, "x":6.89536, "y":4.85011, "heading":3.14159, "vx":-1.68878, "vy":-1.85349, "omega":0.0, "ax":-6.93195, "ay":-7.60802, "alpha":0.0, "fx":[-117.91053,-117.91053,-117.91053,-117.91053], "fy":[-129.41025,-129.41025,-129.41025,-129.41025]}, - {"t":0.29203, "x":6.80496, "y":4.75089, "heading":3.14159, "vx":-2.02617, "vy":-2.22378, "omega":0.0, "ax":-6.92391, "ay":-7.5992, "alpha":0.0, "fx":[-117.77377,-117.77377,-117.77377,-117.77377], "fy":[-129.26016,-129.26016,-129.26016,-129.26016]}, - {"t":0.3407, "x":6.69814, "y":4.63365, "heading":3.14159, "vx":-2.36317, "vy":-2.59365, "omega":0.0, "ax":-6.89994, "ay":-7.57289, "alpha":0.0, "fx":[-117.36604,-117.36604,-117.36604,-117.36604], "fy":[-128.81266,-128.81266,-128.81266,-128.81266]}, - {"t":0.38937, "x":6.57495, "y":4.49844, "heading":3.14159, "vx":-2.699, "vy":-2.96223, "omega":0.0, "ax":-3.954, "ay":-4.33963, "alpha":0.0, "fx":[-67.25641,-67.25641,-67.25641,-67.25641], "fy":[-73.81588,-73.81588,-73.81588,-73.81588]}, - {"t":0.43804, "x":6.4389, "y":4.34913, "heading":3.14159, "vx":-2.89145, "vy":-3.17345, "omega":0.0, "ax":3.954, "ay":4.33963, "alpha":0.0, "fx":[67.25641,67.25641,67.25641,67.25641], "fy":[73.81588,73.81588,73.81588,73.81588]}, - {"t":0.48672, "x":6.30285, "y":4.19981, "heading":3.14159, "vx":-2.699, "vy":-2.96223, "omega":0.0, "ax":6.89994, "ay":7.57289, "alpha":0.0, "fx":[117.36604,117.36604,117.36604,117.36604], "fy":[128.81266,128.81266,128.81266,128.81266]}, - {"t":0.53539, "x":6.17966, "y":4.06461, "heading":3.14159, "vx":-2.36317, "vy":-2.59365, "omega":0.0, "ax":6.92391, "ay":7.5992, "alpha":0.0, "fx":[117.77377,117.77377,117.77377,117.77377], "fy":[129.26016,129.26016,129.26016,129.26016]}, - {"t":0.58406, "x":6.07284, "y":3.94737, "heading":3.14159, "vx":-2.02617, "vy":-2.22378, "omega":0.0, "ax":6.93195, "ay":7.60802, "alpha":0.0, "fx":[117.91053,117.91053,117.91053,117.91053], "fy":[129.41025,129.41025,129.41025,129.41025]}, - {"t":0.63273, "x":5.98244, "y":3.84815, "heading":3.14159, "vx":-1.68878, "vy":-1.85349, "omega":0.0, "ax":6.93598, "ay":7.61244, "alpha":0.0, "fx":[117.97901,117.97901,117.97901,117.97901], "fy":[129.48542,129.48542,129.48542,129.48542]}, - {"t":0.6814, "x":5.90846, "y":3.76695, "heading":3.14159, "vx":-1.3512, "vy":-1.48298, "omega":0.0, "ax":6.9384, "ay":7.61509, "alpha":0.0, "fx":[118.02013,118.02013,118.02013,118.02013], "fy":[129.53055,129.53055,129.53055,129.53055]}, - {"t":0.73007, "x":5.85091, "y":3.70379, "heading":3.14159, "vx":-1.0135, "vy":-1.11234, "omega":0.0, "ax":6.94001, "ay":7.61686, "alpha":0.0, "fx":[118.04755,118.04755,118.04755,118.04755], "fy":[129.56064,129.56064,129.56064,129.56064]}, - {"t":0.77874, "x":5.8098, "y":3.65867, "heading":3.14159, "vx":-0.67572, "vy":-0.74162, "omega":0.0, "ax":6.94116, "ay":7.61813, "alpha":0.0, "fx":[118.06713,118.06713,118.06713,118.06713], "fy":[129.58214,129.58214,129.58214,129.58214]}, - {"t":0.82742, "x":5.78514, "y":3.6316, "heading":3.14159, "vx":-0.33788, "vy":-0.37083, "omega":0.0, "ax":6.94202, "ay":7.61907, "alpha":0.0, "fx":[118.08182,118.08182,118.08182,118.08182], "fy":[129.59826,129.59826,129.59826,129.59826]}, - {"t":0.87609, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.55109, "ay":-8.34798, "alpha":0.0, "fx":[-142.54254,-142.54254,-142.54254,-142.54254], "fy":[-139.15676,-139.15676,-139.15676,-139.15676]}, + {"t":0.04891, "x":7.09066, "y":5.06569, "heading":3.14159, "vx":-0.41827, "vy":-0.40833, "omega":0.0, "ax":-8.54958, "ay":-8.3465, "alpha":0.0, "fx":[-142.5174,-142.5174,-142.5174,-142.5174], "fy":[-139.13222,-139.13222,-139.13222,-139.13222]}, + {"t":0.09783, "x":7.05997, "y":5.03573, "heading":3.14159, "vx":-0.83646, "vy":-0.81659, "omega":0.0, "ax":-8.54732, "ay":-8.3443, "alpha":0.0, "fx":[-142.47968,-142.47968,-142.47968,-142.47968], "fy":[-139.0954,-139.0954,-139.0954,-139.0954]}, + {"t":0.14674, "x":7.00883, "y":4.98581, "heading":3.14159, "vx":-1.25454, "vy":-1.22474, "omega":0.0, "ax":-8.54355, "ay":-8.34061, "alpha":0.0, "fx":[-142.41681,-142.41681,-142.41681,-142.41681], "fy":[-139.03401,-139.03401,-139.03401,-139.03401]}, + {"t":0.19566, "x":6.93725, "y":4.91592, "heading":3.14159, "vx":-1.67244, "vy":-1.63272, "omega":0.0, "ax":-8.536, "ay":-8.33325, "alpha":0.0, "fx":[-142.29108,-142.29108,-142.29108,-142.29108], "fy":[-138.91127,-138.91127,-138.91127,-138.91127]}, + {"t":0.24457, "x":6.84523, "y":4.82609, "heading":3.14159, "vx":-2.08997, "vy":-2.04033, "omega":0.0, "ax":-8.51342, "ay":-8.3112, "alpha":0.0, "fx":[-141.91464,-141.91464,-141.91464,-141.91464], "fy":[-138.54377,-138.54377,-138.54377,-138.54377]}, + {"t":0.29348, "x":6.73282, "y":4.71635, "heading":3.14159, "vx":-2.5064, "vy":-2.44686, "omega":0.0, "ax":-4.03419, "ay":-3.93837, "alpha":0.0, "fx":[-67.248,-67.248,-67.248,-67.248], "fy":[-65.65067,-65.65067,-65.65067,-65.65067]}, + {"t":0.3424, "x":6.60539, "y":4.59195, "heading":3.14159, "vx":-2.70372, "vy":-2.6395, "omega":0.0, "ax":-0.00029, "ay":-0.00028, "alpha":0.0, "fx":[-0.00483,-0.00483,-0.00483,-0.00483], "fy":[-0.00472,-0.00472,-0.00472,-0.00472]}, + {"t":0.39131, "x":6.47314, "y":4.46284, "heading":3.14159, "vx":-2.70374, "vy":-2.63952, "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.44023, "x":6.34089, "y":4.33373, "heading":3.14159, "vx":-2.70374, "vy":-2.63952, "omega":0.0, "ax":0.00029, "ay":0.00028, "alpha":0.0, "fx":[0.00483,0.00483,0.00483,0.00483], "fy":[0.00472,0.00472,0.00472,0.00472]}, + {"t":0.48914, "x":6.20864, "y":4.20463, "heading":3.14159, "vx":-2.70372, "vy":-2.6395, "omega":0.0, "ax":4.03419, "ay":3.93837, "alpha":0.0, "fx":[67.248,67.248,67.248,67.248], "fy":[65.65067,65.65067,65.65067,65.65067]}, + {"t":0.53805, "x":6.08122, "y":4.08023, "heading":3.14159, "vx":-2.5064, "vy":-2.44686, "omega":0.0, "ax":8.51342, "ay":8.3112, "alpha":0.0, "fx":[141.91464,141.91464,141.91464,141.91464], "fy":[138.54377,138.54377,138.54377,138.54377]}, + {"t":0.58697, "x":5.9688, "y":3.97049, "heading":3.14159, "vx":-2.08997, "vy":-2.04033, "omega":0.0, "ax":8.536, "ay":8.33325, "alpha":0.0, "fx":[142.29108,142.29108,142.29108,142.29108], "fy":[138.91127,138.91127,138.91127,138.91127]}, + {"t":0.63588, "x":5.87679, "y":3.88065, "heading":3.14159, "vx":-1.67244, "vy":-1.63272, "omega":0.0, "ax":8.54355, "ay":8.34061, "alpha":0.0, "fx":[142.41681,142.41681,142.41681,142.41681], "fy":[139.03401,139.03401,139.03401,139.03401]}, + {"t":0.68479, "x":5.8052, "y":3.81077, "heading":3.14159, "vx":-1.25454, "vy":-1.22474, "omega":0.0, "ax":8.54732, "ay":8.3443, "alpha":0.0, "fx":[142.47968,142.47968,142.47968,142.47968], "fy":[139.0954,139.0954,139.0954,139.0954]}, + {"t":0.73371, "x":5.75406, "y":3.76084, "heading":3.14159, "vx":-0.83646, "vy":-0.81659, "omega":0.0, "ax":8.54958, "ay":8.3465, "alpha":0.0, "fx":[142.5174,142.5174,142.5174,142.5174], "fy":[139.13222,139.13222,139.13222,139.13222]}, + {"t":0.78262, "x":5.72338, "y":3.73089, "heading":3.14159, "vx":-0.41827, "vy":-0.40833, "omega":0.0, "ax":8.55109, "ay":8.34798, "alpha":0.0, "fx":[142.54254,142.54254,142.54254,142.54254], "fy":[139.15676,139.15676,139.15676,139.15676]}, + {"t":0.83154, "x":5.71315, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToH.traj b/src/main/deploy/choreo/startToH.traj index e04c8948..ee42fda4 100644 --- a/src/main/deploy/choreo/startToH.traj +++ b/src/main/deploy/choreo/startToH.traj @@ -3,8 +3,8 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7769125, "y":3.9512534894, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.713146, "y":4.0509, "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}, @@ -13,8 +13,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"H.x", "val":5.7769125}, "y":{"exp":"H.y", "val":3.9512534894}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.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}, @@ -26,26 +26,25 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.82274], + "waypoints":[0.0,0.77479], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.85635, "ay":-6.67224, "alpha":0.0, "fx":[-133.63427,-133.63427,-133.63427,-133.63427], "fy":[-113.49289,-113.49289,-113.49289,-113.49289]}, - {"t":0.0484, "x":7.09169, "y":5.06786, "heading":3.14159, "vx":-0.38022, "vy":-0.32291, "omega":0.0, "ax":-7.85537, "ay":-6.6714, "alpha":0.0, "fx":[-133.61753,-133.61753,-133.61753,-133.61753], "fy":[-113.47868,-113.47868,-113.47868,-113.47868]}, - {"t":0.09679, "x":7.06409, "y":5.04442, "heading":3.14159, "vx":-0.76039, "vy":-0.64578, "omega":0.0, "ax":-7.85405, "ay":-6.67029, "alpha":0.0, "fx":[-133.59521,-133.59521,-133.59521,-133.59521], "fy":[-113.45972,-113.45972,-113.45972,-113.45972]}, - {"t":0.14519, "x":7.01809, "y":5.00536, "heading":3.14159, "vx":-1.14049, "vy":-0.9686, "omega":0.0, "ax":-7.85222, "ay":-6.66873, "alpha":0.0, "fx":[-133.56394,-133.56394,-133.56394,-133.56394], "fy":[-113.43317,-113.43317,-113.43317,-113.43317]}, - {"t":0.19358, "x":6.9537, "y":4.95067, "heading":3.14159, "vx":-1.52051, "vy":-1.29134, "omega":0.0, "ax":-7.84946, "ay":-6.66639, "alpha":0.0, "fx":[-133.51701,-133.51701,-133.51701,-133.51701], "fy":[-113.39331,-113.39331,-113.39331,-113.39331]}, - {"t":0.24198, "x":6.87092, "y":4.88037, "heading":3.14159, "vx":-1.9004, "vy":-1.61397, "omega":0.0, "ax":-7.84485, "ay":-6.66248, "alpha":0.0, "fx":[-133.43873,-133.43873,-133.43873,-133.43873], "fy":[-113.32683,-113.32683,-113.32683,-113.32683]}, - {"t":0.29038, "x":6.76976, "y":4.79446, "heading":3.14159, "vx":-2.28006, "vy":-1.93641, "omega":0.0, "ax":-7.83564, "ay":-6.65465, "alpha":0.0, "fx":[-133.28199,-133.28199,-133.28199,-133.28199], "fy":[-113.19371,-113.19371,-113.19371,-113.19371]}, - {"t":0.33877, "x":6.65024, "y":4.69295, "heading":3.14159, "vx":-2.65927, "vy":-2.25847, "omega":0.0, "ax":-7.80796, "ay":-6.63115, "alpha":0.0, "fx":[-132.81122,-132.81122,-132.81122,-132.81122], "fy":[-112.7939,-112.7939,-112.7939,-112.7939]}, - {"t":0.38717, "x":6.51239, "y":4.57588, "heading":3.14159, "vx":-3.03715, "vy":-2.57939, "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.43557, "x":6.36541, "y":4.45105, "heading":3.14159, "vx":-3.03715, "vy":-2.57939, "omega":0.0, "ax":7.80796, "ay":6.63115, "alpha":0.0, "fx":[132.81122,132.81122,132.81122,132.81122], "fy":[112.7939,112.7939,112.7939,112.7939]}, - {"t":0.48396, "x":6.22756, "y":4.33398, "heading":3.14159, "vx":-2.65927, "vy":-2.25847, "omega":0.0, "ax":7.83564, "ay":6.65465, "alpha":0.0, "fx":[133.28199,133.28199,133.28199,133.28199], "fy":[113.19371,113.19371,113.19371,113.19371]}, - {"t":0.53236, "x":6.10804, "y":4.23247, "heading":3.14159, "vx":-2.28006, "vy":-1.93641, "omega":0.0, "ax":7.84485, "ay":6.66248, "alpha":0.0, "fx":[133.43873,133.43873,133.43873,133.43873], "fy":[113.32683,113.32683,113.32683,113.32683]}, - {"t":0.58075, "x":6.00688, "y":4.14656, "heading":3.14159, "vx":-1.9004, "vy":-1.61397, "omega":0.0, "ax":7.84946, "ay":6.66639, "alpha":0.0, "fx":[133.51701,133.51701,133.51701,133.51701], "fy":[113.39331,113.39331,113.39331,113.39331]}, - {"t":0.62915, "x":5.9241, "y":4.07626, "heading":3.14159, "vx":-1.52051, "vy":-1.29134, "omega":0.0, "ax":7.85222, "ay":6.66873, "alpha":0.0, "fx":[133.56394,133.56394,133.56394,133.56394], "fy":[113.43317,113.43317,113.43317,113.43317]}, - {"t":0.67755, "x":5.85971, "y":4.02157, "heading":3.14159, "vx":-1.14049, "vy":-0.9686, "omega":0.0, "ax":7.85405, "ay":6.67029, "alpha":0.0, "fx":[133.59521,133.59521,133.59521,133.59521], "fy":[113.45972,113.45972,113.45972,113.45972]}, - {"t":0.72594, "x":5.81371, "y":3.98251, "heading":3.14159, "vx":-0.76039, "vy":-0.64578, "omega":0.0, "ax":7.85537, "ay":6.6714, "alpha":0.0, "fx":[133.61753,133.61753,133.61753,133.61753], "fy":[113.47868,113.47868,113.47868,113.47868]}, - {"t":0.77434, "x":5.78611, "y":3.95907, "heading":3.14159, "vx":-0.38022, "vy":-0.32291, "omega":0.0, "ax":7.85635, "ay":6.67224, "alpha":0.0, "fx":[133.63427,133.63427,133.63427,133.63427], "fy":[113.49289,113.49289,113.49289,113.49289]}, - {"t":0.82274, "x":5.77691, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.61317, "ay":-7.09886, "alpha":0.0, "fx":[-160.247,-160.247,-160.247,-160.247], "fy":[-118.33452,-118.33452,-118.33452,-118.33452]}, + {"t":0.04842, "x":7.08962, "y":5.06736, "heading":3.14159, "vx":-0.46551, "vy":-0.34376, "omega":0.0, "ax":-9.61146, "ay":-7.09759, "alpha":0.0, "fx":[-160.21842,-160.21842,-160.21842,-160.21842], "fy":[-118.31342,-118.31342,-118.31342,-118.31342]}, + {"t":0.09685, "x":7.0558, "y":5.04239, "heading":3.14159, "vx":-0.93094, "vy":-0.68746, "omega":0.0, "ax":-9.60889, "ay":-7.09569, "alpha":0.0, "fx":[-160.17555,-160.17555,-160.17555,-160.17555], "fy":[-118.28176,-118.28176,-118.28176,-118.28176]}, + {"t":0.14527, "x":6.99946, "y":5.00078, "heading":3.14159, "vx":-1.39625, "vy":-1.03106, "omega":0.0, "ax":-9.6046, "ay":-7.09253, "alpha":0.0, "fx":[-160.10413,-160.10413,-160.10413,-160.10413], "fy":[-118.22902,-118.22902,-118.22902,-118.22902]}, + {"t":0.1937, "x":6.92058, "y":4.94253, "heading":3.14159, "vx":-1.86135, "vy":-1.37451, "omega":0.0, "ax":-9.59604, "ay":-7.0862, "alpha":0.0, "fx":[-159.9614,-159.9614,-159.9614,-159.9614], "fy":[-118.12362,-118.12362,-118.12362,-118.12362]}, + {"t":0.24212, "x":6.8192, "y":4.86767, "heading":3.14159, "vx":-2.32603, "vy":-1.71766, "omega":0.0, "ax":-9.57045, "ay":-7.06731, "alpha":0.0, "fx":[-159.53485,-159.53485,-159.53485,-159.53485], "fy":[-117.80863,-117.80863,-117.80863,-117.80863]}, + {"t":0.29055, "x":6.69534, "y":4.7762, "heading":3.14159, "vx":-2.78948, "vy":-2.05989, "omega":0.0, "ax":-5.16445, "ay":-3.81369, "alpha":0.0, "fx":[-86.08886,-86.08886,-86.08886,-86.08886], "fy":[-63.57238,-63.57238,-63.57238,-63.57238]}, + {"t":0.33897, "x":6.55421, "y":4.67198, "heading":3.14159, "vx":-3.03956, "vy":-2.24457, "omega":0.0, "ax":-0.00042, "ay":-0.00031, "alpha":0.0, "fx":[-0.00694,-0.00694,-0.00694,-0.00694], "fy":[-0.00512,-0.00512,-0.00512,-0.00512]}, + {"t":0.3874, "x":6.40702, "y":4.56329, "heading":3.14159, "vx":-3.03958, "vy":-2.24458, "omega":0.0, "ax":0.00042, "ay":0.00031, "alpha":0.0, "fx":[0.00694,0.00694,0.00694,0.00694], "fy":[0.00512,0.00512,0.00512,0.00512]}, + {"t":0.43582, "x":6.25983, "y":4.4546, "heading":3.14159, "vx":-3.03956, "vy":-2.24457, "omega":0.0, "ax":5.16445, "ay":3.81369, "alpha":0.0, "fx":[86.08886,86.08886,86.08886,86.08886], "fy":[63.57238,63.57238,63.57238,63.57238]}, + {"t":0.48425, "x":6.11869, "y":4.35038, "heading":3.14159, "vx":-2.78948, "vy":-2.05989, "omega":0.0, "ax":9.57045, "ay":7.06731, "alpha":0.0, "fx":[159.53485,159.53485,159.53485,159.53485], "fy":[117.80863,117.80863,117.80863,117.80863]}, + {"t":0.53267, "x":5.99483, "y":4.25891, "heading":3.14159, "vx":-2.32603, "vy":-1.71766, "omega":0.0, "ax":9.59604, "ay":7.0862, "alpha":0.0, "fx":[159.9614,159.9614,159.9614,159.9614], "fy":[118.12362,118.12362,118.12362,118.12362]}, + {"t":0.58109, "x":5.89345, "y":4.18404, "heading":3.14159, "vx":-1.86135, "vy":-1.37451, "omega":0.0, "ax":9.6046, "ay":7.09253, "alpha":0.0, "fx":[160.10413,160.10413,160.10413,160.10413], "fy":[118.22902,118.22902,118.22902,118.22902]}, + {"t":0.62952, "x":5.81458, "y":4.1258, "heading":3.14159, "vx":-1.39625, "vy":-1.03106, "omega":0.0, "ax":9.60889, "ay":7.09569, "alpha":0.0, "fx":[160.17555,160.17555,160.17555,160.17555], "fy":[118.28176,118.28176,118.28176,118.28176]}, + {"t":0.67794, "x":5.75823, "y":4.08419, "heading":3.14159, "vx":-0.93094, "vy":-0.68746, "omega":0.0, "ax":9.61146, "ay":7.09759, "alpha":0.0, "fx":[160.21842,160.21842,160.21842,160.21842], "fy":[118.31342,118.31342,118.31342,118.31342]}, + {"t":0.72637, "x":5.72442, "y":4.05922, "heading":3.14159, "vx":-0.46551, "vy":-0.34376, "omega":0.0, "ax":9.61317, "ay":7.09886, "alpha":0.0, "fx":[160.247,160.247,160.247,160.247], "fy":[118.33452,118.33452,118.33452,118.33452]}, + {"t":0.77479, "x":5.71315, "y":4.0509, "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/startToI.traj b/src/main/deploy/choreo/startToI.traj index 144e1aae..06487118 100644 --- a/src/main/deploy/choreo/startToI.traj +++ b/src/main/deploy/choreo/startToI.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.0711771011352536, "y":5.008563137054443, "heading":-2.0943951023931953, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.0711771011352536, "y":5.008563137054443, "heading":4.1887902047863905, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.365372248154253, "y":4.933261807735684, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"I.x", "val":5.482468790097718}, "y":{"exp":"I.y", "val":4.939213976119822}, "heading":{"exp":"I.heading", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,41 +28,39 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.47831,0.81778], + "waypoints":[0.0,0.47516,0.82156], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.68422, "ay":-0.36994, "alpha":21.53352, "fx":[-122.28034,-135.32501,-168.38311,-164.87608], "fy":[-125.15111,110.83102,48.516,-59.36616]}, - {"t":0.02174, "x":7.09884, "y":5.07559, "heading":3.14159, "vx":-0.18881, "vy":-0.00804, "omega":0.46817, "ax":-8.7789, "ay":-0.38206, "alpha":20.8178, "fx":[-125.27686,-138.23513,-168.64407,-165.1502], "fy":[-122.10181,107.11956,47.52888,-58.54186]}, - {"t":0.04348, "x":7.09266, "y":5.07533, "heading":-3.13141, "vx":-0.37967, "vy":-0.01635, "omega":0.92078, "ax":-8.88101, "ay":-0.41033, "alpha":20.02147, "fx":[-128.63718,-141.25931,-169.11417,-165.24321], "fy":[-118.49076,103.03895,45.74272,-58.20961]}, - {"t":0.06522, "x":7.0823, "y":5.07487, "heading":-3.1114, "vx":-0.57276, "vy":-0.02527, "omega":1.35607, "ax":-8.99354, "ay":-0.44993, "alpha":19.10895, "fx":[-132.5319,-144.41829,-169.76182,-165.19813], "fy":[-114.03242,98.50092,43.17228,-58.25353]}, - {"t":0.08697, "x":7.06772, "y":5.07422, "heading":-3.08191, "vx":-0.76829, "vy":-0.03505, "omega":1.77152, "ax":-9.12016, "ay":-0.49503, "alpha":18.03048, "fx":[-137.14424,-147.76596,-170.54593,-165.06927], "fy":[-108.33096,93.33717,39.82752,-58.5149]}, - {"t":0.10871, "x":7.04886, "y":5.07334, "heading":-3.0434, "vx":-0.96657, "vy":-0.04582, "omega":2.16353, "ax":-9.26432, "ay":-0.53845, "alpha":16.72705, "fx":[-142.6162,-151.36879,-171.41604,-164.93257], "fy":[-100.87576,87.29589,35.71495,-58.77087]}, - {"t":0.13045, "x":7.02566, "y":5.07222, "heading":-2.99636, "vx":-1.16799, "vy":-0.05752, "omega":2.5272, "ax":-9.4276, "ay":-0.57195, "alpha":15.14014, "fx":[-148.94721,-155.28304,-172.31234,-164.9002], "fy":[-91.0847,80.02923,30.83798,-58.69723]}, - {"t":0.15219, "x":6.99804, "y":5.07083, "heading":-2.94141, "vx":-1.37296, "vy":-0.06996, "omega":2.85637, "ax":-9.60745, "ay":-0.58804, "alpha":13.22675, "fx":[-155.8463,-159.52782,-173.16553,-165.14006], "fy":[-78.46285,71.05898,25.19389,-57.79965]}, - {"t":0.17393, "x":6.96592, "y":5.06917, "heading":-2.87931, "vx":-1.58184, "vy":-0.08274, "omega":3.14393, "ax":-9.79519, "ay":-0.58441, "alpha":10.96972, "fx":[-162.61678,-164.04307,-173.89575,-165.8976], "fy":[-62.94649,59.70004,18.76152,-55.27735]}, - {"t":0.19567, "x":6.92921, "y":5.06723, "heading":-2.81096, "vx":-1.7948, "vy":-0.09545, "omega":3.38243, "ax":-9.97623, "ay":-0.5691, "alpha":8.34921, "fx":[-168.26781,-168.59767,-174.40708,-167.49877], "fy":[-45.3634,44.90952,11.46576,-49.73268]}, - {"t":0.21741, "x":6.88783, "y":5.06502, "heading":-2.73742, "vx":-2.0117, "vy":-0.10782, "omega":3.56395, "ax":-10.13132, "ay":-0.55932, "alpha":5.221, "fx":[-171.99219,-172.5468,-174.56501,-170.21925], "fy":[-27.63164,25.01849,3.07701,-38.51947]}, - {"t":0.23915, "x":6.8417, "y":5.06255, "heading":-2.65994, "vx":-2.23197, "vy":-0.11998, "omega":3.67747, "ax":-10.22099, "ay":-0.56388, "alpha":1.07267, "fx":[-173.71341,-174.16021,-174.10108,-173.44961], "fy":[-12.26794,-2.63369,-7.12254,-16.34188]}, - {"t":0.2609, "x":6.79076, "y":5.0598, "heading":-2.57998, "vx":-2.45418, "vy":-0.13224, "omega":3.70079, "ax":-10.09537, "ay":-0.56874, "alpha":-5.22766, "fx":[-174.10918,-168.91891,-172.05728,-171.79176], "fy":[-1.44465,-41.56803,-21.91626,26.23222]}, - {"t":0.28264, "x":6.73502, "y":5.0568, "heading":-2.49952, "vx":-2.67367, "vy":-0.14461, "omega":3.58713, "ax":-9.21816, "ay":-0.85605, "alpha":-14.97646, "fx":[-174.05392,-147.43853,-158.46903,-147.23161], "fy":[3.56823,-91.88415,-61.49257,91.56372]}, - {"t":0.30438, "x":6.67471, "y":5.05345, "heading":-2.42153, "vx":-2.87409, "vy":-0.16322, "omega":3.26152, "ax":-8.3616, "ay":-1.36264, "alpha":-19.80359, "fx":[-173.90451,-131.92003,-130.92643,-132.16281], "fy":[0.47241,-112.51295,-92.50879,111.83709]}, - {"t":0.32612, "x":6.61024, "y":5.04958, "heading":-2.35062, "vx":-3.05588, "vy":-0.19284, "omega":2.83097, "ax":-8.33726, "ay":-0.98447, "alpha":-19.38487, "fx":[-173.47389,-125.58057,-135.99081,-132.21261], "fy":[-4.86053,-118.70833,-54.51021,111.09695]}, - {"t":0.34786, "x":6.54184, "y":5.04515, "heading":-2.28907, "vx":-3.23714, "vy":-0.21425, "omega":2.40951, "ax":-8.35347, "ay":-0.28138, "alpha":-19.06643, "fx":[-172.60575,-121.76339,-138.62767,-135.36366], "fy":[-10.37518,-121.26971,6.54914,105.95083]}, - {"t":0.3696, "x":6.46948, "y":5.04043, "heading":-2.23669, "vx":-3.41876, "vy":-0.22036, "omega":1.99498, "ax":-8.30265, "ay":-0.22338, "alpha":-18.8651, "fx":[-170.92584,-119.3784,-134.96267,-139.63586], "fy":[-16.04334,-121.05974,24.47427,97.43042]}, - {"t":0.39134, "x":6.39319, "y":5.03558, "heading":-2.19331, "vx":-3.59927, "vy":-0.22522, "omega":1.58483, "ax":-8.02053, "ay":-0.83535, "alpha":-18.39026, "fx":[-166.55142,-113.74811,-122.747,-142.66141], "fy":[-23.74949,-119.80984,2.69347,84.02972]}, - {"t":0.41309, "x":6.31304, "y":5.03049, "heading":-2.15886, "vx":-3.77365, "vy":-0.24338, "omega":1.185, "ax":-3.84263, "ay":-4.62884, "alpha":-9.86128, "fx":[-94.93168,-51.11821,-28.16192,-87.23649], "fy":[-76.57492,-109.04856,-86.87386,-42.4437]}, - {"t":0.43483, "x":6.23009, "y":5.02411, "heading":-2.13309, "vx":-3.85719, "vy":-0.34402, "omega":0.9706, "ax":9.48704, "ay":-0.65437, "alpha":-7.41465, "fx":[159.12141,158.0338,167.63881,160.69329], "fy":[-29.47539,-52.48603,-0.27601,37.71481]}, - {"t":0.45657, "x":6.14847, "y":5.01647, "heading":-2.11199, "vx":-3.65093, "vy":-0.35825, "omega":0.8094, "ax":8.81026, "ay":-0.5047, "alpha":-17.73218, "fx":[146.16597,149.35233,172.29618,131.62525], "fy":[-76.71409,-86.74571,18.3563,110.76413]}, - {"t":0.47831, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-3.45938, "vy":-0.36922, "omega":0.42388, "ax":9.97262, "ay":0.5036, "alpha":-7.93764, "fx":[173.416,169.60085,173.54811,161.96046], "fy":[-7.54417,-40.18634,17.99921,63.99577]}, - {"t":0.51226, "x":5.95949, "y":4.99632, "heading":-2.08001, "vx":-3.12084, "vy":-0.35212, "omega":0.15442, "ax":10.16399, "ay":0.85565, "alpha":-4.09773, "fx":[174.28446,174.25611,173.68105,169.32472], "fy":[8.25607,-12.05422,19.32854,42.68677]}, - {"t":0.5462, "x":5.8594, "y":4.98486, "heading":-2.07476, "vx":-2.77581, "vy":-0.32308, "omega":0.01531, "ax":10.21006, "ay":1.01231, "alpha":-2.23742, "fx":[174.2243,174.8557,173.78328,171.81734], "fy":[14.20184,2.41609,19.81848,32.43971]}, - {"t":0.58015, "x":5.77105, "y":4.97447, "heading":-2.07424, "vx":-2.42921, "vy":-0.28871, "omega":-0.06064, "ax":10.22321, "ay":1.1019, "alpha":-1.14657, "fx":[174.10744,174.64275,173.85749,172.9679], "fy":[17.32573,11.08552,20.05778,26.50327]}, - {"t":0.6141, "x":5.69448, "y":4.96531, "heading":-2.0763, "vx":-2.08216, "vy":-0.25131, "omega":-0.09956, "ax":10.22631, "ay":1.15992, "alpha":-0.43183, "fx":[174.00819,174.26507,173.91042,173.60294], "fy":[19.23215,16.82931,20.21405,22.64427]}, - {"t":0.64804, "x":5.62969, "y":4.95745, "heading":-2.07968, "vx":-1.73501, "vy":-0.21193, "omega":-0.11422, "ax":10.22588, "ay":1.20052, "alpha":0.07194, "fx":[173.93146,173.88189,173.9479,173.99606], "fy":[20.4986,20.90569,20.34221,19.93584]}, - {"t":0.68199, "x":5.57668, "y":4.95094, "heading":-2.08356, "vx":-1.38787, "vy":-0.17118, "omega":-0.11178, "ax":10.22416, "ay":1.23051, "alpha":0.44583, "fx":[173.87308,173.53356,173.97435,174.25949], "fy":[21.38841,23.9451,20.45992,17.92916]}, - {"t":0.71594, "x":5.53546, "y":4.94584, "heading":-2.08735, "vx":-1.0408, "vy":-0.12941, "omega":-0.09665, "ax":10.22203, "ay":1.25355, "alpha":0.73417, "fx":[173.82833,173.22714,173.99315,174.44647], "fy":[22.04087,26.29661,20.57101,16.38191]}, - {"t":0.74988, "x":5.50602, "y":4.94217, "heading":-2.09064, "vx":-0.69379, "vy":-0.08685, "omega":-0.07172, "ax":10.21983, "ay":1.27181, "alpha":0.96324, "fx":[173.79329,172.96005,174.00685,174.58507], "fy":[22.5376,28.16863,20.67392,15.1523]}, - {"t":0.78383, "x":5.48836, "y":4.93996, "heading":-2.09307, "vx":-0.34686, "vy":-0.04368, "omega":-0.03902, "ax":10.21771, "ay":1.28663, "alpha":1.14957, "fx":[173.76492,172.72729,174.01745,174.69133], "fy":[22.93021,29.69322,20.76504,14.15204]}, - {"t":0.81778, "x":5.48247, "y":4.93921, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.5334, "ay":-0.23846, "alpha":38.98149, "fx":[-62.46701,-73.25328,-185.8047,-180.78761], "fy":[-188.75032,184.72701,71.60764,-83.48433]}, + {"t":0.02376, "x":7.09876, "y":5.07561, "heading":3.14159, "vx":-0.17898, "vy":-0.00567, "omega":0.92613, "ax":-7.88082, "ay":-0.27029, "alpha":37.29377, "fx":[-72.37092,-85.05554,-186.57552,-181.47584], "fy":[-185.09051,179.48978,69.4669,-81.88889]}, + {"t":0.04752, "x":7.09229, "y":5.0754, "heading":-3.11959, "vx":-0.36621, "vy":-0.01209, "omega":1.81216, "ax":-8.32246, "ay":-0.39854, "alpha":35.01714, "fx":[-83.64137,-101.51012,-188.25455,-181.51979], "fy":[-180.13949,170.60558,64.62911,-81.66901]}, + {"t":0.07127, "x":7.08124, "y":5.075, "heading":-3.07654, "vx":-0.56394, "vy":-0.02156, "omega":2.6441, "ax":-8.89692, "ay":-0.60412, "alpha":31.78712, "fx":[-99.42841,-121.9092,-190.64063,-181.25153], "fy":[-171.71104,156.5409,56.9755,-82.08671]}, + {"t":0.09503, "x":7.06533, "y":5.07432, "heading":-3.01372, "vx":-0.77531, "vy":-0.03591, "omega":3.3993, "ax":-9.66225, "ay":-0.82759, "alpha":26.91595, "fx":[-124.10144,-145.42941,-193.42929,-181.30024], "fy":[-154.46688,134.78001,46.21018,-81.70573]}, + {"t":0.11879, "x":7.04418, "y":5.07323, "heading":-2.93296, "vx":-1.00487, "vy":-0.05557, "omega":4.03877, "ax":-10.62855, "ay":-0.94402, "alpha":19.50463, "fx":[-158.83904,-170.63889,-196.18095,-183.03267], "fy":[-117.90678,100.65138,31.56396,-77.25423]}, + {"t":0.14255, "x":7.01731, "y":5.07165, "heading":-2.837, "vx":-1.25738, "vy":-0.078, "omega":4.50216, "ax":-11.52988, "ay":-0.9297, "alpha":9.51428, "fx":[-188.76185,-192.64941,-198.08329,-189.29555], "fy":[-58.59084,45.22986,10.40609,-59.03541]}, + {"t":0.16631, "x":6.98418, "y":5.06953, "heading":-2.73004, "vx":-1.53131, "vy":-0.10009, "omega":4.7282, "ax":-11.73549, "ay":-1.01011, "alpha":-4.47617, "fx":[-197.69514,-193.16332,-194.58547,-197.05602], "fy":[-5.11766,-41.95634,-32.31004,12.03209]}, + {"t":0.19006, "x":6.94449, "y":5.06687, "heading":-2.61771, "vx":-1.81012, "vy":-0.12408, "omega":4.62186, "ax":-9.62842, "ay":-1.662, "alpha":-24.50404, "fx":[-197.32777,-163.16014,-131.02215,-150.49426], "fy":[14.41945,-111.36131,-140.74016,126.86305]}, + {"t":0.21382, "x":6.89876, "y":5.06345, "heading":-2.5079, "vx":-2.03888, "vy":-0.16357, "omega":4.03969, "ax":-9.59433, "ay":-1.49098, "alpha":-23.78566, "fx":[-197.38964,-157.14643,-139.74997,-145.44557], "fy":[10.54069,-119.08733,-123.35908,132.49014]}, + {"t":0.23758, "x":6.84762, "y":5.05914, "heading":-2.41193, "vx":-2.26682, "vy":-0.19899, "omega":3.47459, "ax":-9.58912, "ay":-1.02806, "alpha":-22.87308, "fx":[-197.36323,-149.19034,-149.69022,-143.14011], "fy":[4.76397,-128.18825,-79.79664,134.67157]}, + {"t":0.26134, "x":6.79105, "y":5.05412, "heading":-2.32938, "vx":-2.49464, "vy":-0.22342, "omega":2.93117, "ax":-9.65532, "ay":0.46747, "alpha":-22.14552, "fx":[-197.0163,-143.96034,-155.56061,-147.2608], "fy":[-2.56185,-133.05238,37.26402,129.51989]}, + {"t":0.2851, "x":6.72906, "y":5.04895, "heading":-2.25974, "vx":-2.72403, "vy":-0.21231, "omega":2.40503, "ax":-9.71433, "ay":0.70571, "alpha":-21.90802, "fx":[-196.16542,-142.66212,-154.18881,-154.71674], "fy":[-9.84688,-132.97849,70.57087,119.31015]}, + {"t":0.30886, "x":6.6616, "y":5.0441, "heading":-2.2026, "vx":-2.95483, "vy":-0.19555, "omega":1.88454, "ax":-9.67806, "ay":0.52448, "alpha":-21.8629, "fx":[-194.68838,-139.95685,-150.97713,-159.69179], "fy":[-16.56328,-133.52323,74.70833,110.34961]}, + {"t":0.33261, "x":6.58867, "y":5.03961, "heading":-2.15782, "vx":-3.18476, "vy":-0.18308, "omega":1.36512, "ax":-9.52318, "ay":0.19888, "alpha":-21.77589, "fx":[-192.07563,-135.70289,-144.90832,-162.30044], "fy":[-22.57623,-133.5622,67.6228,101.77644]}, + {"t":0.35637, "x":6.51032, "y":5.03531, "heading":-2.12539, "vx":-3.41101, "vy":-0.17836, "omega":0.84776, "ax":-9.08857, "ay":-0.31495, "alpha":-21.11162, "fx":[-185.81186,-129.20731,-129.98971,-160.99958], "fy":[-27.43742,-128.50162,45.00174,89.93698]}, + {"t":0.38013, "x":6.42671, "y":5.03099, "heading":-2.10525, "vx":-3.62694, "vy":-0.18584, "omega":0.34619, "ax":-5.43121, "ay":-1.13341, "alpha":-12.84248, "fx":[-122.79783,-80.19154,-54.30482,-104.8487], "fy":[-27.74734,-71.22489,-8.18593,31.58436]}, + {"t":0.40389, "x":6.33901, "y":5.02625, "heading":-2.09703, "vx":-3.75597, "vy":-0.21277, "omega":0.04108, "ax":-0.07135, "ay":-0.60925, "alpha":-0.25185, "fx":[-2.02961,-0.96608,-0.34881,-1.4128], "fy":[-10.37729,-10.99215,-9.93488,-9.3194]}, + {"t":0.42765, "x":6.24976, "y":5.02102, "heading":-2.09605, "vx":-3.75767, "vy":-0.22725, "omega":0.03509, "ax":0.0543, "ay":-0.99371, "alpha":-0.02258, "fx":[0.82952,0.9253,0.98091,0.88513], "fy":[-16.58463,-16.63932,-16.54477,-16.49006]}, + {"t":0.4514, "x":6.1605, "y":5.01534, "heading":-2.09522, "vx":-3.75638, "vy":-0.25085, "omega":0.03456, "ax":-0.26789, "ay":-2.91092, "alpha":-1.45404, "fx":[-9.55406,-3.06378,0.66154,-5.90576], "fy":[-49.70823,-53.07084,-47.38157,-43.93406]}, + {"t":0.47516, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-3.76274, "vy":-0.32001, "omega":0.00001, "ax":1.97945, "ay":-2.31211, "alpha":-0.00039, "fx":[32.99508,32.99669,32.99776,32.99615], "fy":[-38.54225,-38.54309,-38.54139,-38.54055]}, + {"t":0.5098, "x":5.94202, "y":4.99609, "heading":-2.09439, "vx":-3.69418, "vy":-0.4001, "omega":0.0, "ax":11.75911, "ay":1.23153, "alpha":-0.00001, "fx":[196.01869,196.0187,196.01869,196.01868], "fy":[20.52901,20.52893,20.52904,20.52911]}, + {"t":0.54444, "x":5.82111, "y":4.98297, "heading":-2.09439, "vx":-3.28684, "vy":-0.35744, "omega":0.0, "ax":11.82592, "ay":1.26986, "alpha":0.0, "fx":[197.13237,197.13237,197.13237,197.13237], "fy":[21.16789,21.16789,21.16789,21.16789]}, + {"t":0.57908, "x":5.71435, "y":4.97135, "heading":-2.09439, "vx":-2.87719, "vy":-0.31346, "omega":0.0, "ax":11.84747, "ay":1.28225, "alpha":0.0, "fx":[197.49167,197.49167,197.49167,197.49167], "fy":[21.37442,21.37444,21.37441,21.37439]}, + {"t":0.61372, "x":5.62179, "y":4.96126, "heading":-2.09439, "vx":-2.46679, "vy":-0.26904, "omega":0.0, "ax":11.8581, "ay":1.28836, "alpha":0.00001, "fx":[197.6688,197.66879,197.6688,197.6688], "fy":[21.47628,21.47632,21.47627,21.47623]}, + {"t":0.64836, "x":5.54346, "y":4.95271, "heading":-2.09439, "vx":-2.05603, "vy":-0.22441, "omega":0.0, "ax":11.86442, "ay":1.29199, "alpha":0.00001, "fx":[197.77421,197.77421,197.77422,197.77422], "fy":[21.53691,21.53696,21.5369,21.53685]}, + {"t":0.683, "x":5.47935, "y":4.94572, "heading":-2.09439, "vx":-1.64504, "vy":-0.17965, "omega":0.0, "ax":11.86862, "ay":1.29441, "alpha":0.00001, "fx":[197.84412,197.84411,197.84412,197.84413], "fy":[21.57712,21.57717,21.5771,21.57706]}, + {"t":0.71764, "x":5.42949, "y":4.94027, "heading":-2.0944, "vx":-1.23391, "vy":-0.13482, "omega":0.0, "ax":11.8716, "ay":1.29612, "alpha":0.00001, "fx":[197.89387,197.89386,197.89387,197.89387], "fy":[21.60574,21.60579,21.60572,21.60566]}, + {"t":0.75228, "x":5.39387, "y":4.93638, "heading":-2.0944, "vx":-0.82268, "vy":-0.08992, "omega":0.0, "ax":11.87383, "ay":1.29741, "alpha":0.00001, "fx":[197.93107,197.93106,197.93107,197.93108], "fy":[21.62713,21.62719,21.62711,21.62706]}, + {"t":0.78692, "x":5.3725, "y":4.93404, "heading":-2.0944, "vx":-0.41137, "vy":-0.04498, "omega":0.0, "ax":11.87556, "ay":1.2984, "alpha":0.00001, "fx":[197.95994,197.95994,197.95994,197.95995], "fy":[21.64374,21.6438,21.64372,21.64366]}, + {"t":0.82156, "x":5.36537, "y":4.93326, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToJ.traj b/src/main/deploy/choreo/startToJ.traj index 90fd306d..3ea7f607 100644 --- a/src/main/deploy/choreo/startToJ.traj +++ b/src/main/deploy/choreo/startToJ.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.123365116119384, "y":5.0878586769104, "heading":-2.0943951023931953, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.123365116119384, "y":5.0878586769104, "heading":4.1887902047863905, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.123365116119384 m", "val":6.123365116119384}, "y":{"exp":"5.0878586769104 m", "val":5.0878586769104}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"J.x", "val":5.197827024483464}, "y":{"exp":"J.y", "val":5.103551976119823}, "heading":{"exp":"J.heading", "val":-2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.123365116119384 m", "val":6.123365116119384}, "y":{"exp":"5.0878586769104 m", "val":5.0878586769104}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,42 +28,41 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.46112,0.8859], + "waypoints":[0.0,0.46084,0.89582], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.45182, "ay":0.06208, "alpha":23.24675, "fx":[-122.66991,-120.28781,-165.72569,-166.36902], "fy":[-124.71991,127.03341,56.9578,-55.04746]}, - {"t":0.02196, "x":7.09885, "y":5.07569, "heading":3.14159, "vx":-0.18558, "vy":0.00136, "omega":0.51045, "ax":-8.58524, "ay":0.06601, "alpha":22.30431, "fx":[-126.86884,-124.44934,-166.08049,-166.731], "fy":[-120.39547,122.91046,55.85584,-53.8797]}, - {"t":0.04392, "x":7.09271, "y":5.07574, "heading":-3.13038, "vx":-0.3741, "vy":0.00281, "omega":1.0002, "ax":-8.73278, "ay":0.0518, "alpha":21.22148, "fx":[-131.54948,-128.97852,-166.72246,-166.91798], "fy":[-115.19394,118.10143,53.83968,-53.22275]}, - {"t":0.06587, "x":7.08239, "y":5.07581, "heading":-3.10842, "vx":-0.56585, "vy":0.00395, "omega":1.46618, "ax":-8.89659, "ay":0.02756, "alpha":19.95925, "fx":[-136.89934,-133.82204,-167.60481,-166.98786], "fy":[-108.69082,112.53617,50.94112,-52.9114]}, - {"t":0.08783, "x":7.06782, "y":5.07591, "heading":-3.07623, "vx":-0.7612, "vy":0.00456, "omega":1.90444, "ax":-9.07861, "ay":0.00304, "alpha":18.46765, "fx":[-143.05245,-138.96515,-168.66798,-167.01281], "fy":[-100.33627,106.06968,47.19252,-52.71909]}, - {"t":0.10979, "x":7.04891, "y":5.07601, "heading":-3.03441, "vx":-0.96055, "vy":0.00462, "omega":2.30995, "ax":-9.27869, "ay":-0.0109, "alpha":16.69878, "fx":[-149.97041,-144.40899,-169.84121,-167.09054], "fy":[-89.5158,98.4725,42.63059,-52.32896]}, - {"t":0.13175, "x":7.02558, "y":5.07611, "heading":-2.98369, "vx":-1.16429, "vy":0.00438, "omega":2.67662, "ax":-9.49203, "ay":-0.00466, "alpha":14.62808, "fx":[-157.28184,-150.14316,-171.04463,-167.35696], "fy":[-75.74762,89.41452,37.29988,-51.28396]}, - {"t":0.15371, "x":6.99773, "y":5.0762, "heading":-2.92492, "vx":-1.37271, "vy":0.00428, "omega":2.99783, "ax":-9.7075, "ay":0.0251, "alpha":12.2734, "fx":[-164.18724,-156.1127,-172.19193,-167.99542], "fy":[-59.07854,78.42939,31.25404,-48.89723]}, - {"t":0.17566, "x":6.96525, "y":5.0763, "heading":-2.85909, "vx":-1.58587, "vy":0.00483, "omega":3.26732, "ax":-9.90955, "ay":0.07, "alpha":9.6779, "fx":[-169.65105,-162.16644,-173.1933,-169.22358], "fy":[-40.5476,64.84326,24.55154,-44.08464]}, - {"t":0.19762, "x":6.92804, "y":5.07642, "heading":-2.78735, "vx":-1.80346, "vy":0.00637, "omega":3.47983, "ax":-10.08333, "ay":0.11147, "alpha":6.81598, "fx":[-172.96446,-167.94729,-173.95605,-171.19048], "fy":[-22.25679,47.65087,17.24094,-35.05099]}, - {"t":0.21958, "x":6.88601, "y":5.07659, "heading":-2.71094, "vx":-2.02487, "vy":0.00882, "omega":3.62949, "ax":-10.21192, "ay":0.13534, "alpha":3.43822, "fx":[-174.2496,-172.6294,-174.37604,-173.5525], "fy":[-6.68876,25.34189,9.32337,-18.76819]}, - {"t":0.24154, "x":6.83908, "y":5.07682, "heading":-2.63124, "vx":-2.2491, "vy":0.01179, "omega":3.70499, "ax":-10.2443, "ay":0.14832, "alpha":-1.02787, "fx":[-174.3326,-174.30417,-174.29529,-174.07847], "fy":[4.26936,-4.20138,0.63762,9.38567]}, - {"t":0.26349, "x":6.78723, "y":5.07711, "heading":-2.54989, "vx":-2.47404, "vy":0.01504, "omega":3.68242, "ax":-10.02387, "ay":0.14741, "alpha":-7.21869, "fx":[-174.12736,-168.79669,-173.2753,-165.81338], "fy":[9.77426,-43.0777,-9.6761,53.00899]}, - {"t":0.28545, "x":6.73049, "y":5.07748, "heading":-2.46903, "vx":-2.69415, "vy":0.01828, "omega":3.52391, "ax":-9.27764, "ay":-0.0686, "alpha":-14.7523, "fx":[-174.13931,-149.23197,-167.97845,-139.89006], "fy":[9.84685,-89.61083,-28.27975,103.37656]}, - {"t":0.30741, "x":6.66909, "y":5.07786, "heading":-2.39165, "vx":-2.89786, "vy":0.01677, "omega":3.19998, "ax":-8.47821, "ay":-0.12723, "alpha":-19.16578, "fx":[-174.2887,-130.89343,-147.49866,-124.16694], "fy":[4.99497,-114.50693,-20.88342,121.7389]}, - {"t":0.32937, "x":6.60342, "y":5.0782, "heading":-2.32139, "vx":-3.08403, "vy":0.01398, "omega":2.77914, "ax":-8.48871, "ay":0.91996, "alpha":-19.11542, "fx":[-174.19988,-129.50327,-143.39221,-130.46678], "fy":[-0.96144,-115.65549,64.49394,114.71624]}, - {"t":0.35133, "x":6.53365, "y":5.07873, "heading":-2.26036, "vx":-3.27042, "vy":0.03418, "omega":2.35941, "ax":-8.52782, "ay":0.97636, "alpha":-19.13043, "fx":[-173.88369,-129.51581,-140.40159,-136.42194], "fy":[-6.11891,-115.09586,80.45498,107.19041]}, - {"t":0.37328, "x":6.45979, "y":5.07972, "heading":-2.20856, "vx":-3.45767, "vy":0.05562, "omega":1.93935, "ax":-8.52822, "ay":0.91523, "alpha":-19.27527, "fx":[-173.38858,-128.81294,-137.57962,-140.46912], "fy":[-10.55119,-115.17019,86.71551,101.27691]}, - {"t":0.39524, "x":6.38181, "y":5.08116, "heading":-2.16597, "vx":-3.64493, "vy":0.07572, "omega":1.5161, "ax":-8.49445, "ay":0.83247, "alpha":-19.4917, "fx":[-172.721,-127.55028,-134.55559,-143.12568], "fy":[-14.28105,-115.58531,89.86043,96.64626]}, - {"t":0.4172, "x":6.29972, "y":5.08302, "heading":-2.13268, "vx":-3.83145, "vy":0.094, "omega":1.08811, "ax":-8.43072, "ay":0.74853, "alpha":-19.70145, "fx":[-171.84275,-126.01427,-131.10421,-144.65555], "fy":[-17.18734,-115.79137,90.91953,92.98797]}, - {"t":0.43916, "x":6.21356, "y":5.08527, "heading":-2.10879, "vx":-4.01658, "vy":0.11043, "omega":0.65551, "ax":-8.29817, "ay":0.69961, "alpha":-20.02099, "fx":[-170.61102,-124.0897,-125.14642,-144.7509], "fy":[-18.97659,-115.39975,91.49278,90.48411]}, - {"t":0.46112, "x":6.12337, "y":5.08786, "heading":-2.0944, "vx":-4.19879, "vy":0.12579, "omega":0.21589, "ax":5.55257, "ay":-1.69018, "alpha":-6.88086, "fx":[75.65537,95.75004,111.69242,94.69287], "fy":[-42.6886,-54.48597,-18.73214,0.9087]}, - {"t":0.49651, "x":5.97821, "y":5.09125, "heading":-2.08675, "vx":-4.00223, "vy":0.06596, "omega":-0.02768, "ax":10.18229, "ay":-0.21329, "alpha":-0.06999, "fx":[173.1893,173.18971,173.20674,173.20531], "fy":[-3.76448,-4.0836,-3.49223,-3.17167]}, - {"t":0.53191, "x":5.84292, "y":5.09345, "heading":-2.08773, "vx":-3.6418, "vy":0.05841, "omega":-0.03016, "ax":10.24817, "ay":-0.18329, "alpha":0.02564, "fx":[174.3204,174.32104,174.31645,174.31567], "fy":[-3.06814,-2.94927,-3.16721,-3.28589]}, - {"t":0.56731, "x":5.72043, "y":5.09541, "heading":-2.0888, "vx":-3.27903, "vy":0.05193, "omega":-0.02925, "ax":10.27041, "ay":-0.17311, "alpha":0.05861, "fx":[174.70051,174.70258,174.69338,174.69055], "fy":[-2.83205,-2.55845,-3.05741,-3.33001]}, - {"t":0.60271, "x":5.61079, "y":5.09714, "heading":-2.08984, "vx":-2.91547, "vy":0.0458, "omega":-0.02718, "ax":10.28158, "ay":-0.16799, "alpha":0.07533, "fx":[174.89094,174.89399,174.883,174.8787], "fy":[-2.71364,-2.36044,-3.00208,-3.35363]}, - {"t":0.63811, "x":5.51403, "y":5.09865, "heading":-2.0908, "vx":-2.55152, "vy":0.03985, "omega":-0.02451, "ax":10.28828, "ay":-0.16491, "alpha":0.08544, "fx":[175.00526,175.00899,174.99711,174.99176], "fy":[-2.64267,-2.24077,-2.96861,-3.36839]}, - {"t":0.67351, "x":5.43016, "y":5.09996, "heading":-2.09167, "vx":-2.18733, "vy":0.03401, "omega":-0.02149, "ax":10.29276, "ay":-0.16286, "alpha":0.09221, "fx":[175.08149,175.08571,175.07331,175.0672], "fy":[-2.59548,-2.16061,-2.94611,-3.37851]}, - {"t":0.7089, "x":5.35918, "y":5.10106, "heading":-2.09243, "vx":-1.82299, "vy":0.02825, "omega":-0.01822, "ax":10.29596, "ay":-0.16139, "alpha":0.09706, "fx":[175.13594,175.14052,175.12779,175.1211], "fy":[-2.56187,-2.10317,-2.92991,-3.38588]}, - {"t":0.7443, "x":5.3011, "y":5.10196, "heading":-2.09307, "vx":-1.45853, "vy":0.02254, "omega":-0.01479, "ax":10.29836, "ay":-0.16029, "alpha":0.10071, "fx":[175.17677,175.18164,175.16868,175.16154], "fy":[-2.53673,-2.05999,-2.91769,-3.3915]}, - {"t":0.7797, "x":5.25592, "y":5.10266, "heading":-2.0936, "vx":-1.09398, "vy":0.01686, "omega":-0.01122, "ax":10.30023, "ay":-0.15943, "alpha":0.10355, "fx":[175.20852,175.21362,175.2005,175.19299], "fy":[-2.5172,-2.02635,-2.90815,-3.39591]}, - {"t":0.8151, "x":5.22365, "y":5.10316, "heading":-2.09399, "vx":-0.72937, "vy":0.01122, "omega":-0.00756, "ax":10.30172, "ay":-0.15875, "alpha":0.10583, "fx":[175.23392,175.23921,175.22596,175.21816], "fy":[-2.50157,-1.9994,-2.90053,-3.39947]}, - {"t":0.8505, "x":5.20428, "y":5.10345, "heading":-2.09426, "vx":-0.36471, "vy":0.0056, "omega":-0.00381, "ax":10.30294, "ay":-0.15819, "alpha":0.10769, "fx":[175.2547,175.26015,175.2468,175.23875], "fy":[-2.48875,-1.97735,-2.89432,-3.40239]}, - {"t":0.8859, "x":5.19783, "y":5.10355, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.51509, "ay":0.05345, "alpha":39.09871, "fx":[-68.41964,-66.01077,-182.7646,-183.89644], "fy":[-186.59936,187.48875,79.04036,-76.36584]}, + {"t":0.02304, "x":7.09889, "y":5.07569, "heading":3.14159, "vx":-0.17316, "vy":0.00123, "omega":0.90091, "ax":-7.84186, "ay":0.06014, "alpha":37.5235, "fx":[-78.82117,-76.00759,-183.45077,-184.60077], "fy":[-182.3517,183.57027,77.3416,-74.55017]}, + {"t":0.04608, "x":7.09282, "y":5.07574, "heading":-3.12083, "vx":-0.35385, "vy":0.00262, "omega":1.76552, "ax":-8.26524, "ay":-0.00627, "alpha":35.36671, "fx":[-90.85741,-90.38966,-185.08143,-184.78212], "fy":[-176.50893,176.82888,73.22762,-73.96589]}, + {"t":0.06913, "x":7.08247, "y":5.0758, "heading":-3.08015, "vx":-0.5443, "vy":0.00247, "omega":2.58044, "ax":-8.82806, "ay":-0.1171, "alpha":32.25499, "fx":[-107.61098,-108.80473,-187.48477,-184.73749], "fy":[-166.57936,166.01105,66.64664,-73.88645]}, + {"t":0.09217, "x":7.06759, "y":5.07582, "heading":-3.0207, "vx":-0.74771, "vy":-0.00023, "omega":3.32365, "ax":-9.58658, "ay":-0.19757, "alpha":27.53522, "fx":[-132.93094,-130.87756,-190.40383,-185.00268], "fy":[-146.77285,149.06395,57.46264,-72.92702]}, + {"t":0.11521, "x":7.04781, "y":5.07576, "heading":-2.94411, "vx":-0.96861, "vy":-0.00478, "omega":3.95811, "ax":-10.5352, "ay":-0.1119, "alpha":20.50071, "fx":[-166.2782,-156.03799,-193.49588,-186.65458], "fy":[-106.93508,122.2119,45.33399,-68.07203]}, + {"t":0.13825, "x":7.0227, "y":5.07562, "heading":-2.85291, "vx":-1.21136, "vy":-0.00736, "omega":4.43049, "ax":-11.42662, "ay":0.13268, "alpha":11.46708, "fx":[-191.91514,-182.06091,-196.29682,-191.63242], "fy":[-46.84749,77.75214,29.12395,-51.18202]}, + {"t":0.16129, "x":6.99175, "y":5.07549, "heading":-2.75082, "vx":-1.47465, "vy":-0.0043, "omega":4.69471, "ax":-11.85585, "ay":0.22487, "alpha":-0.28637, "fx":[-197.63697,-197.66849,-197.63281,-197.58702], "fy":[4.45972,1.9362,3.02427,5.57371]}, + {"t":0.18434, "x":6.95463, "y":5.07545, "heading":-2.64265, "vx":-1.74783, "vy":0.00088, "omega":4.68811, "ax":-10.00815, "ay":-0.66064, "alpha":-22.3902, "fx":[-196.01254,-171.07308,-157.89512,-142.34361], "fy":[27.20374,-98.75319,-108.68827,136.18756]}, + {"t":0.20738, "x":6.9117, "y":5.0753, "heading":-2.53462, "vx":-1.97844, "vy":-0.01434, "omega":4.1722, "ax":-9.67314, "ay":-0.67124, "alpha":-23.41645, "fx":[-196.44506,-162.34469,-152.52245,-133.67403], "fy":[22.63104,-111.95319,-100.05047,144.61549]}, + {"t":0.23042, "x":6.86354, "y":5.07479, "heading":-2.43849, "vx":-2.20132, "vy":-0.02981, "omega":3.63264, "ax":-9.61316, "ay":0.5258, "alpha":-22.46424, "fx":[-196.91663,-154.33552,-156.33704,-133.39809], "fy":[15.47716,-122.06326,-2.97907,144.62452]}, + {"t":0.25346, "x":6.81027, "y":5.07424, "heading":-2.35479, "vx":-2.42283, "vy":-0.01769, "omega":3.11502, "ax":-9.67218, "ay":1.55333, "alpha":-21.95795, "fx":[-197.02815,-153.21416,-151.57449,-143.10556], "fy":[7.37599,-122.4732,84.19985,134.47061]}, + {"t":0.2765, "x":6.75187, "y":5.07424, "heading":-2.28301, "vx":-2.6457, "vy":0.0181, "omega":2.60907, "ax":-9.73071, "ay":1.461, "alpha":-21.84654, "fx":[-196.65043,-151.77656,-149.41594,-150.98202], "fy":[-0.32961,-122.89238,96.00645,124.63251]}, + {"t":0.29954, "x":6.68833, "y":5.07505, "heading":-2.22289, "vx":-2.86991, "vy":0.05176, "omega":2.10569, "ax":-9.71264, "ay":1.20976, "alpha":-21.9311, "fx":[-195.71112,-148.55619,-146.99996,-156.35312], "fy":[-7.70109,-124.8015,96.93073,116.23649]}, + {"t":0.32259, "x":6.61962, "y":5.07656, "heading":-2.17437, "vx":-3.09371, "vy":0.07964, "omega":1.60035, "ax":-9.60869, "ay":0.86987, "alpha":-22.04914, "fx":[-193.94053,-143.61663,-143.35508,-159.77651], "fy":[-14.72775,-127.26189,91.50163,108.48937]}, + {"t":0.34563, "x":6.54579, "y":5.07863, "heading":-2.1375, "vx":-3.31511, "vy":0.09968, "omega":1.0923, "ax":-9.36436, "ay":0.39332, "alpha":-21.90345, "fx":[-190.38478,-136.77,-136.23031,-161.01272], "fy":[-21.28764,-128.19179,75.94731,99.75793]}, + {"t":0.36867, "x":6.46691, "y":5.08103, "heading":-2.11233, "vx":-3.53088, "vy":0.10875, "omega":0.5876, "ax":-8.60632, "ay":-0.45222, "alpha":-20.41751, "fx":[-179.14628,-124.59096,-114.25799,-155.85777], "fy":[-27.46092,-119.56869,34.74466,82.13152]}, + {"t":0.39171, "x":6.38327, "y":5.08342, "heading":-2.09879, "vx":-3.72919, "vy":0.09833, "omega":0.11714, "ax":-1.47131, "ay":-1.37813, "alpha":-3.45319, "fx":[-35.80766,-21.29191,-13.01604,-27.98837], "fy":[-25.71332,-34.54489,-20.3002,-11.3328]}, + {"t":0.41475, "x":6.29695, "y":5.08532, "heading":-2.09609, "vx":-3.76309, "vy":0.06657, "omega":0.03758, "ax":-0.03625, "ay":-0.52795, "alpha":-0.06757, "fx":[-0.82953,-0.54423,-0.37894,-0.66427], "fy":[-8.86035,-9.02511,-8.74101,-8.57621]}, + {"t":0.4378, "x":6.21024, "y":5.08671, "heading":-2.09523, "vx":-3.76392, "vy":0.05441, "omega":0.03602, "ax":-0.53687, "ay":-0.39517, "alpha":-1.56275, "fx":[-14.13232,-7.55458,-3.75042,-10.3602], "fy":[-7.96283,-11.79512,-5.21378,-1.37766]}, + {"t":0.46084, "x":6.12337, "y":5.08786, "heading":-2.0944, "vx":-3.77629, "vy":0.0453, "omega":0.00001, "ax":-0.02871, "ay":-0.20163, "alpha":-0.0003, "fx":[-0.47958,-0.47831,-0.47758,-0.47885], "fy":[-3.36133,-3.36206,-3.36079,-3.36006]}, + {"t":0.49709, "x":5.98646, "y":5.08937, "heading":-2.09439, "vx":-3.77734, "vy":0.03799, "omega":0.0, "ax":-0.00026, "ay":-0.02536, "alpha":0.0, "fx":[-0.00427,-0.00427,-0.00427,-0.00427], "fy":[-0.42269,-0.42269,-0.42269,-0.42269]}, + {"t":0.53333, "x":5.84954, "y":5.09073, "heading":-2.09439, "vx":-3.77734, "vy":0.03707, "omega":0.0, "ax":0.00351, "ay":-0.00326, "alpha":0.0, "fx":[0.05848,0.05848,0.05848,0.05848], "fy":[-0.05433,-0.05433,-0.05433,-0.05433]}, + {"t":0.56958, "x":5.71262, "y":5.09207, "heading":-2.09439, "vx":-3.77722, "vy":0.03695, "omega":0.0, "ax":8.82385, "ay":-0.08666, "alpha":-0.00006, "fx":[147.08923,147.08935,147.08942,147.0893], "fy":[-1.44472,-1.44496,-1.44454,-1.44431]}, + {"t":0.60583, "x":5.58149, "y":5.09335, "heading":-2.09439, "vx":-3.45737, "vy":0.03381, "omega":0.0, "ax":11.85014, "ay":-0.11591, "alpha":0.00001, "fx":[197.53615,197.53615,197.53614,197.53614], "fy":[-1.93208,-1.93205,-1.9321,-1.93213]}, + {"t":0.64208, "x":5.46396, "y":5.0945, "heading":-2.09439, "vx":-3.02781, "vy":0.02961, "omega":0.0, "ax":11.90444, "ay":-0.11643, "alpha":0.00001, "fx":[198.44122,198.44122,198.44122,198.44122], "fy":[-1.94081,-1.94077,-1.94083,-1.94087]}, + {"t":0.67833, "x":5.36202, "y":5.0955, "heading":-2.09439, "vx":-2.5963, "vy":0.02539, "omega":0.0, "ax":11.9229, "ay":-0.11661, "alpha":0.00001, "fx":[198.74895,198.74895,198.74895,198.74895], "fy":[-1.94377,-1.94374,-1.9438,-1.94384]}, + {"t":0.71458, "x":5.27574, "y":5.09634, "heading":-2.09439, "vx":-2.16411, "vy":0.02116, "omega":0.0, "ax":11.93219, "ay":-0.1167, "alpha":0.00001, "fx":[198.90384,198.90384,198.90384,198.90384], "fy":[-1.94527,-1.94523,-1.9453,-1.94534]}, + {"t":0.75083, "x":5.20514, "y":5.09703, "heading":-2.09439, "vx":-1.73158, "vy":0.01693, "omega":0.0, "ax":11.93778, "ay":-0.11675, "alpha":0.00001, "fx":[198.99708,198.99708,198.99708,198.99708], "fy":[-1.94617,-1.94613,-1.9462,-1.94624]}, + {"t":0.78707, "x":5.15021, "y":5.09757, "heading":-2.0944, "vx":-1.29886, "vy":0.0127, "omega":0.0, "ax":11.94152, "ay":-0.11679, "alpha":0.00001, "fx":[199.05936,199.05936,199.05936,199.05936], "fy":[-1.94677,-1.94673,-1.9468,-1.94684]}, + {"t":0.82332, "x":5.11098, "y":5.09795, "heading":-2.0944, "vx":-0.86599, "vy":0.00847, "omega":0.0, "ax":11.94419, "ay":-0.11681, "alpha":0.00001, "fx":[199.1039,199.1039,199.1039,199.1039], "fy":[-1.9472,-1.94715,-1.94723,-1.94727]}, + {"t":0.85957, "x":5.08743, "y":5.09819, "heading":-2.0944, "vx":-0.43303, "vy":0.00423, "omega":0.0, "ax":11.9462, "ay":-0.11683, "alpha":0.00001, "fx":[199.13733,199.13733,199.13733,199.13733], "fy":[-1.94752,-1.94748,-1.94755,-1.94759]}, + {"t":0.89582, "x":5.07958, "y":5.09826, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/startToK.traj b/src/main/deploy/choreo/startToK.traj index 3a148a02..fd43a2c8 100644 --- a/src/main/deploy/choreo/startToK.traj +++ b/src/main/deploy/choreo/startToK.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":4.455554008483887, "y":5.959114074707031, "heading":-1.325818250323842, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "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}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"4.455554008483887 m", "val":4.455554008483887}, "y":{"exp":"5.959114074707031 m", "val":5.959114074707031}, "heading":{"exp":"-1.325818250323842 rad", "val":-1.325818250323842}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"K.x", "val":4.1950062900977185}, "y":{"exp":"K.y", "val":5.342536486719823}, "heading":{"exp":"K.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "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}, @@ -28,57 +28,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.01258,1.41325], + "waypoints":[0.0,1.0504,1.44926], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.66326, "ay":4.08466, "alpha":19.89713, "fx":[-174.29945,-36.35714,-135.46412,-175.27903], "fy":[-4.09958,171.42994,111.30349,-0.71839]}, - {"t":0.03068, "x":7.09728, "y":5.0776, "heading":3.14159, "vx":-0.23514, "vy":0.12533, "omega":0.61053, "ax":-7.70099, "ay":4.2955, "alpha":18.95747, "fx":[-174.1029,-39.12919,-135.48363,-175.25079], "fy":[8.5894,170.79226,111.25268,1.62657]}, - {"t":0.06137, "x":7.08644, "y":5.08347, "heading":-3.12286, "vx":-0.47144, "vy":0.25714, "omega":1.19222, "ax":-7.74975, "ay":4.60664, "alpha":17.46219, "fx":[-171.62353,-44.04808,-136.4213,-175.1914], "fy":[30.20677,169.5602,110.06629,3.59754]}, - {"t":0.09205, "x":7.06833, "y":5.09353, "heading":-3.08628, "vx":-0.70924, "vy":0.39849, "omega":1.72804, "ax":-7.7708, "ay":4.97682, "alpha":15.77532, "fx":[-164.48135,-51.01819,-138.12354,-175.09298], "fy":[57.4363,167.55884,107.87644,5.74589]}, - {"t":0.12274, "x":7.0429, "y":5.1081, "heading":-3.03325, "vx":-0.94768, "vy":0.5512, "omega":2.21209, "ax":-7.75642, "ay":5.33016, "alpha":14.2765, "fx":[-152.5142,-59.97692,-140.3495,-174.89728], "fy":[84.2194,164.51878,104.90004,9.01975]}, - {"t":0.15342, "x":7.01017, "y":5.12752, "heading":-2.96538, "vx":-1.18568, "vy":0.71475, "omega":2.65016, "ax":-7.75501, "ay":5.60108, "alpha":12.99142, "fx":[-139.42959,-71.04844,-142.76819,-174.39603], "fy":[104.53705,159.98538,101.48894,15.0795]}, - {"t":0.18411, "x":6.97014, "y":5.15209, "heading":-2.88406, "vx":-1.42363, "vy":0.88662, "omega":3.04879, "ax":-7.83002, "ay":5.79308, "alpha":11.40375, "fx":[-130.27417,-84.75468,-144.88281,-172.83415], "fy":[115.8378,153.06788,98.29513,26.95338]}, - {"t":0.21479, "x":6.92277, "y":5.18202, "heading":-2.79051, "vx":-1.66389, "vy":1.06437, "omega":3.39871, "ax":-7.99156, "ay":5.97829, "alpha":8.51767, "fx":[-128.22413,-102.4144,-145.68103,-167.4169], "fy":[118.18071,141.68691,96.83974,50.04877]}, - {"t":0.24547, "x":6.86796, "y":5.2175, "heading":-2.68622, "vx":-1.90911, "vy":1.24781, "omega":3.66006, "ax":-8.116, "ay":6.22578, "alpha":2.80116, "fx":[-134.13234,-127.1594,-141.85,-149.0614], "fy":[111.4786,119.58615,101.82173,90.70856]}, - {"t":0.27616, "x":6.80556, "y":5.25871, "heading":-2.57392, "vx":-2.15814, "vy":1.43885, "omega":3.74602, "ax":-7.69964, "ay":6.29959, "alpha":-8.56929, "fx":[-146.86479,-163.19968,-109.8091,-104.00111], "fy":[94.08684,60.21179,134.4293,139.88881]}, - {"t":0.30684, "x":6.73571, "y":5.30583, "heading":-2.45897, "vx":-2.3944, "vy":1.63215, "omega":3.48307, "ax":-6.98366, "ay":5.87854, "alpha":-17.21122, "fx":[-155.58922,-173.00265,-57.31237,-89.25617], "fy":[78.45244,9.41339,162.56996,149.53315]}, - {"t":0.33753, "x":6.65895, "y":5.35868, "heading":-2.3521, "vx":-2.60869, "vy":1.81252, "omega":2.95496, "ax":-7.12261, "ay":5.40271, "alpha":-18.59448, "fx":[-160.80393,-172.37799,-54.10612,-97.32596], "fy":[66.17717,-5.38792,162.82088,143.98404]}, - {"t":0.36821, "x":6.57555, "y":5.41684, "heading":-2.26143, "vx":-2.82724, "vy":1.9783, "omega":2.3844, "ax":-7.23516, "ay":4.71176, "alpha":-20.54389, "fx":[-165.7071,-168.69792,-52.20463,-105.66267], "fy":[50.69862,-29.02806,161.71721,137.19528]}, - {"t":0.3989, "x":6.48539, "y":5.47976, "heading":-2.18826, "vx":-3.04925, "vy":2.12288, "omega":1.75403, "ax":-7.02484, "ay":3.83963, "alpha":-24.07171, "fx":[-169.06341,-156.61588,-40.49934,-111.78313], "fy":[32.41845,-62.93254,161.13375,130.62451]}, - {"t":0.42958, "x":6.38852, "y":5.54671, "heading":-2.13444, "vx":-3.2648, "vy":2.2407, "omega":1.0154, "ax":-6.72832, "ay":3.88352, "alpha":-23.5052, "fx":[-165.1354,-151.4919,-32.36395,-108.79594], "fy":[33.2275,-52.83235,155.4312,128.40415]}, - {"t":0.46026, "x":6.28518, "y":5.61729, "heading":-2.10328, "vx":-3.47125, "vy":2.35986, "omega":0.29416, "ax":-3.07182, "ay":0.66139, "alpha":-8.91899, "fx":[-78.85365,-47.42189,-23.61247,-59.11494], "fy":[2.89791,-21.83227,22.0839,41.85047]}, - {"t":0.49095, "x":6.17722, "y":5.69001, "heading":-2.09426, "vx":-3.56551, "vy":2.38015, "omega":0.02049, "ax":-0.87052, "ay":-1.29965, "alpha":-0.03315, "fx":[-14.91786,-14.77509,-14.69661,-14.83943], "fy":[-22.13364,-22.21662,-22.07971,-21.99671]}, - {"t":0.52163, "x":6.0674, "y":5.76243, "heading":-2.09363, "vx":-3.59222, "vy":2.34027, "omega":0.01947, "ax":-2.26188, "ay":-3.59622, "alpha":0.01253, "fx":[-38.43087,-38.49229,-38.51691,-38.45551], "fy":[-61.16672,-61.13041,-61.17474,-61.21105]}, - {"t":0.55232, "x":5.95612, "y":5.83255, "heading":-2.09303, "vx":-3.66162, "vy":2.22993, "omega":0.01986, "ax":-3.81248, "ay":-6.71073, "alpha":0.06542, "fx":[-64.59718,-65.02952,-65.10064,-64.66949], "fy":[-114.2106,-113.96408,-114.08503,-114.33088]}, - {"t":0.583, "x":5.84197, "y":5.89781, "heading":-2.09242, "vx":-3.77861, "vy":2.02401, "omega":0.02187, "ax":-3.49684, "ay":-8.52685, "alpha":1.94111, "fx":[-49.96476,-66.21137,-68.23758,-53.50733], "fy":[-148.00046,-141.16066,-142.30108,-148.69476]}, - {"t":0.61369, "x":5.72438, "y":5.9559, "heading":-2.09175, "vx":-3.8859, "vy":1.76237, "omega":0.08143, "ax":0.63224, "ay":-9.26119, "alpha":11.60606, "fx":[88.09956,-14.00488,-54.4922,23.41469], "fy":[-142.43851,-159.94672,-159.48804,-168.24782]}, - {"t":0.64437, "x":5.60544, "y":6.00562, "heading":-2.08925, "vx":-3.8665, "vy":1.4782, "omega":0.43755, "ax":2.40583, "ay":-8.94231, "alpha":13.65364, "fx":[125.02887,41.9609,-48.51929,45.21918], "fy":[-117.80013,-159.05939,-164.79945,-166.76527]}, - {"t":0.67505, "x":5.48793, "y":6.04677, "heading":-2.07583, "vx":-3.79268, "vy":1.20381, "omega":0.8565, "ax":3.16439, "ay":-8.80155, "alpha":13.68574, "fx":[132.65142,68.9556,-39.96922,53.66352], "fy":[-111.38338,-153.74405,-168.42325,-165.29639]}, - {"t":0.70574, "x":5.37304, "y":6.07956, "heading":-2.04955, "vx":-3.69559, "vy":0.93374, "omega":1.27644, "ax":3.53568, "ay":-8.79806, "alpha":13.03893, "fx":[134.2319,76.36349,-29.48886,59.45679], "fy":[-110.52379,-153.00458,-171.26475,-163.81705]}, - {"t":0.73642, "x":5.26131, "y":6.10407, "heading":-2.01038, "vx":-3.5871, "vy":0.66378, "omega":1.67653, "ax":3.77251, "ay":-8.84457, "alpha":12.10267, "fx":[133.82545,76.08707,-17.93322,64.69797], "fy":[-111.60224,-154.77802,-173.27426,-162.11962]}, - {"t":0.76711, "x":5.15302, "y":6.12028, "heading":-1.95894, "vx":-3.47134, "vy":0.39239, "omega":2.04789, "ax":3.98636, "ay":-8.89239, "alpha":11.01112, "fx":[132.3216,73.94449,-5.1065,70.06811], "fy":[-113.74586,-156.81724,-174.39881,-160.0664]}, - {"t":0.79779, "x":5.04838, "y":6.12813, "heading":-1.8961, "vx":-3.34902, "vy":0.11953, "omega":2.38576, "ax":4.2696, "ay":-8.91622, "alpha":9.57994, "fx":[129.303,73.81799,11.19992,76.17805], "fy":[-117.40838,-157.55864,-174.3019,-157.38051]}, - {"t":0.82848, "x":4.94763, "y":6.1276, "heading":-1.82289, "vx":-3.21801, "vy":-0.15405, "omega":2.67971, "ax":5.095, "ay":-8.78742, "alpha":5.38612, "fx":[117.68797,87.23676,53.79621,87.93711], "fy":[-129.21499,-151.20398,-166.29411,-151.17285]}, - {"t":0.85916, "x":4.85129, "y":6.11874, "heading":-1.74067, "vx":-3.06167, "vy":-0.42369, "omega":2.84498, "ax":5.8498, "ay":-8.45274, "alpha":0.70895, "fx":[103.48705,99.47138,95.484,99.57145], "fy":[-140.98086,-143.82168,-146.51994,-143.79247]}, - {"t":0.88984, "x":4.76009, "y":6.10176, "heading":-1.65337, "vx":-2.88218, "vy":-0.68305, "omega":2.86674, "ax":6.3159, "ay":-8.08132, "alpha":-2.75299, "fx":[92.30566,108.31135,122.06187,107.04772], "fy":[-148.65409,-137.4862,-125.37063,-138.33305]}, - {"t":0.92053, "x":4.67463, "y":6.07699, "heading":-1.56541, "vx":-2.68838, "vy":-0.93102, "omega":2.78226, "ax":6.60718, "ay":-7.74543, "alpha":-5.23735, "fx":[84.53579,115.61045,138.84363,110.55528], "fy":[-153.29562,-131.53204,-106.59857,-135.56404]}, - {"t":0.95121, "x":4.59525, "y":6.04478, "heading":-1.48004, "vx":-2.48564, "vy":-1.16869, "omega":2.62156, "ax":6.79252, "ay":-7.46335, "alpha":-7.07763, "fx":[79.71605,121.93483,149.82986,110.67487], "fy":[-155.92716,-125.77725,-90.6069,-135.48645]}, - {"t":0.9819, "x":4.52218, "y":6.00541, "heading":-1.3996, "vx":-2.27722, "vy":-1.39769, "omega":2.40439, "ax":6.90739, "ay":-7.23303, "alpha":-8.53649, "fx":[77.14547,127.4702,157.28205,108.07286], "fy":[-157.27057,-120.22726,-77.03504,-137.59402]}, - {"t":1.01258, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-2.06527, "vy":-1.61963, "omega":2.14245, "ax":7.24385, "ay":-6.93702, "alpha":-8.08334, "fx":[87.23039,133.18243,159.0245,113.4259], "fy":[-151.8483,-113.79229,-73.21839,-133.1276]}, - {"t":1.03929, "x":4.40297, "y":5.91338, "heading":-1.26859, "vx":-1.87178, "vy":-1.80493, "omega":1.92654, "ax":7.76481, "ay":-6.29081, "alpha":-8.5487, "fx":[96.51442,141.7468,165.70724,124.34052], "fy":[-146.05269,-102.85139,-56.29285,-122.8224]}, - {"t":1.066, "x":4.35575, "y":5.86292, "heading":-1.21713, "vx":-1.66437, "vy":-1.97296, "omega":1.69819, "ax":8.40442, "ay":-5.32833, "alpha":-9.02793, "fx":[108.5581,151.3525,171.75897,140.15781], "fy":[-137.23316,-87.99533,-33.18332,-104.1216]}, - {"t":1.09271, "x":4.31429, "y":5.80832, "heading":-1.17177, "vx":-1.43988, "vy":-2.11529, "omega":1.45704, "ax":9.13437, "ay":-3.82651, "alpha":-9.52044, "fx":[124.63209,161.62721,174.84382,160.38899], "fy":[-122.65209,-67.10112,-2.19871,-68.39948]}, - {"t":1.11943, "x":4.27908, "y":5.75045, "heading":-1.13285, "vx":-1.19589, "vy":-2.2175, "omega":1.20274, "ax":9.7265, "ay":-1.47567, "alpha":-10.29159, "fx":[145.83771,170.9269,170.92351,174.09174], "fy":[-96.16818,-37.15055,36.56734,-3.65182]}, - {"t":1.14614, "x":4.25061, "y":5.6907, "heading":-1.10072, "vx":-0.93609, "vy":-2.25692, "omega":0.92784, "ax":9.62861, "ay":1.70184, "alpha":-11.2268, "fx":[168.49805,174.7683,156.08706,155.7662], "fy":[-45.25193,4.62688,78.59996,77.81611]}, - {"t":1.17285, "x":4.22904, "y":5.63102, "heading":-1.07594, "vx":-0.6789, "vy":-2.21146, "omega":0.62796, "ax":8.51973, "ay":5.03535, "alpha":-9.8329, "fx":[170.14207,165.48807,131.01371,113.02863], "fy":[37.95471,56.19719,115.68423,132.763]}, - {"t":1.19956, "x":4.21395, "y":5.57374, "heading":-1.05917, "vx":-0.45132, "vy":-2.07696, "omega":0.36531, "ax":6.58679, "ay":7.64388, "alpha":-6.73979, "fx":[131.38838,138.98863,101.74012,76.0402], "fy":[114.75106,105.963,142.18281,157.18412]}, - {"t":1.22627, "x":4.20424, "y":5.52099, "heading":-1.04941, "vx":-0.27538, "vy":-1.87278, "omega":0.18529, "ax":4.60717, "ay":9.10195, "alpha":-4.34654, "fx":[85.32557,102.97456,74.26101,50.90569], "fy":[152.41486,141.29724,158.35709,167.21723]}, - {"t":1.25298, "x":4.19853, "y":5.47422, "heading":-1.04446, "vx":-0.15232, "vy":-1.62966, "omega":0.06919, "ax":3.03867, "ay":9.79508, "alpha":-2.65021, "fx":[52.62082,68.83792,51.18174,34.10683], "fy":[166.75009,160.81192,167.32036,171.56394]}, - {"t":1.27969, "x":4.19554, "y":5.43418, "heading":-1.04261, "vx":-0.07115, "vy":-1.36802, "omega":-0.0016, "ax":1.87907, "ay":10.10762, "alpha":-1.41432, "fx":[31.2394,41.502,32.65639,22.45205], "fy":[172.1702,170.0155,171.96012,173.56533]}, - {"t":1.3064, "x":4.19431, "y":5.40124, "heading":-1.04266, "vx":-0.02096, "vy":-1.09804, "omega":-0.03938, "ax":1.02363, "ay":10.24068, "alpha":-0.50546, "fx":[16.85905,20.83402,17.94637,14.00737], "fy":[174.25472,173.83117,174.16191,174.51647]}, - {"t":1.33311, "x":4.19412, "y":5.37557, "heading":-1.04371, "vx":0.00638, "vy":-0.8245, "omega":-0.05288, "ax":0.38056, "ay":10.28864, "alpha":0.17504, "fx":[6.73556,5.30176,6.20739,7.64798], "fy":[175.0013,175.04881,175.01614,174.96095]}, - {"t":1.35983, "x":4.19443, "y":5.35721, "heading":-1.04512, "vx":0.01654, "vy":-0.54968, "omega":-0.04821, "ax":-0.11458, "ay":10.29564, "alpha":0.69712, "fx":[-0.69975,-6.53241,-3.26632,2.7026], "fy":[175.17607,175.05004,175.13075,175.14653]}, - {"t":1.38654, "x":4.19483, "y":5.3462, "heading":-1.04641, "vx":0.01348, "vy":-0.27467, "omega":-0.02959, "ax":-0.50482, "ay":10.28288, "alpha":1.10768, "fx":[-6.36118,-15.72988,-11.01114,-1.24548], "fy":[175.09785,174.49868,174.84413,175.1945]}, - {"t":1.41325, "x":4.19501, "y":5.34254, "heading":-1.0472, "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.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.86629, "ay":1.39146, "alpha":44.45758, "fx":[-31.87881,-7.19172,-155.9394,-196.1433], "fy":[-195.58974,198.97892,123.96842,-34.57787]}, + {"t":0.03282, "x":7.09773, "y":5.07643, "heading":3.14159, "vx":-0.19256, "vy":0.04567, "omega":1.45931, "ax":-6.53541, "ay":1.62439, "alpha":41.41147, "fx":[-69.21768,-13.79279,-155.78571,-196.97244], "fy":[-185.20002,198.5714,124.10152,-29.16179]}, + {"t":0.06565, "x":7.08789, "y":5.0788, "heading":-3.09369, "vx":-0.40708, "vy":0.09899, "omega":2.81864, "ax":-7.93554, "ay":2.37656, "alpha":33.32614, "fx":[-143.06971,-29.98897,-158.51402,-197.55375], "fy":[-134.49679,196.69599,120.49904,-24.23346]}, + {"t":0.09847, "x":7.07025, "y":5.08333, "heading":-3.00117, "vx":-0.66757, "vy":0.17701, "omega":3.91257, "ax":-9.01388, "ay":5.41878, "alpha":18.7973, "fx":[-183.56978,-56.20484,-162.8063,-198.44755], "fy":[69.07975,190.73029,114.445,-12.94108]}, + {"t":0.1313, "x":7.04348, "y":5.09206, "heading":-2.87274, "vx":-0.96345, "vy":0.35488, "omega":4.52959, "ax":-9.08258, "ay":6.68279, "alpha":12.96599, "fx":[-147.09074,-97.19322,-165.1023,-196.22258], "fy":[131.87489,173.15285,110.63795,29.92978]}, + {"t":0.16412, "x":7.00696, "y":5.10731, "heading":-2.72406, "vx":-1.26158, "vy":0.57424, "omega":4.95519, "ax":-8.79865, "ay":7.71345, "alpha":-6.81199, "fx":[-157.36724,-175.07231,-134.34651,-119.89113], "fy":[120.32069,91.88087,144.59749,157.51921]}, + {"t":0.19695, "x":6.96081, "y":5.13032, "heading":-2.5614, "vx":-1.5504, "vy":0.82743, "omega":4.73159, "ax":-7.83505, "ay":7.24662, "alpha":-18.65473, "fx":[-170.35289,-195.60644,-65.11047,-91.35602], "fy":[100.81855,22.47234,184.46455,175.43507]}, + {"t":0.22977, "x":6.9057, "y":5.16138, "heading":-2.40609, "vx":-1.80758, "vy":1.0653, "omega":4.11925, "ax":-8.50982, "ay":6.32065, "alpha":-19.04918, "fx":[-180.97427,-196.04597,-78.93588,-111.46252], "fy":[79.13072,1.44587,178.00423,162.86809]}, + {"t":0.2626, "x":6.84178, "y":5.19975, "heading":-2.27088, "vx":-2.08691, "vy":1.27277, "omega":3.49396, "ax":-9.21507, "ay":4.63545, "alpha":-20.70408, "fx":[-191.46788,-189.56842,-99.94318,-133.46337], "fy":[45.67302,-45.09189,164.19781,144.30404]}, + {"t":0.29542, "x":6.76831, "y":5.24403, "heading":-2.15619, "vx":-2.3894, "vy":1.42493, "omega":2.81435, "ax":-9.72284, "ay":1.36486, "alpha":-22.30126, "fx":[-195.52187,-150.56762,-141.92653,-160.28409], "fy":[-10.78497,-121.89816,112.71937,110.96974]}, + {"t":0.32825, "x":6.68464, "y":5.29154, "heading":-2.06381, "vx":-2.70855, "vy":1.46973, "omega":2.08232, "ax":-8.83467, "ay":2.04797, "alpha":-26.25103, "fx":[-192.695,-147.55374,-94.47901,-154.35125], "fy":[-8.62906,-116.35321,147.48351,114.05317]}, + {"t":0.36107, "x":6.59097, "y":5.34089, "heading":-1.99545, "vx":-2.99855, "vy":1.53696, "omega":1.22063, "ax":-7.67277, "ay":3.64483, "alpha":-25.26723, "fx":[-182.26923,-151.46338,-46.29581,-131.57713], "fy":[19.30644,-63.05664,158.97239,127.80788]}, + {"t":0.3939, "x":6.48841, "y":5.3933, "heading":-1.95539, "vx":-3.2504, "vy":1.6566, "omega":0.39124, "ax":-2.2739, "ay":2.76327, "alpha":-9.78345, "fx":[-70.36097,-27.34281,-5.27046,-48.64501], "fy":[31.95606,15.05909,63.12142,74.11267]}, + {"t":0.42672, "x":6.3805, "y":5.44917, "heading":-1.94254, "vx":-3.32504, "vy":1.7473, "omega":0.0701, "ax":0.07387, "ay":0.28408, "alpha":-0.20464, "fx":[0.58567,1.515,1.87687,0.94775], "fy":[4.45262,4.09042,5.01851,5.3806]}, + {"t":0.45955, "x":6.27139, "y":5.50667, "heading":-1.94024, "vx":-3.32262, "vy":1.75663, "omega":0.06338, "ax":0.01823, "ay":0.04273, "alpha":-0.01471, "fx":[0.25753,0.32436,0.35023,0.28341], "fy":[0.69181,0.66594,0.73276,0.75864]}, + {"t":0.49237, "x":6.16234, "y":5.56436, "heading":-1.93816, "vx":-3.32202, "vy":1.75803, "omega":0.0629, "ax":0.00181, "ay":0.00767, "alpha":-0.00867, "fx":[0.00297,0.04236,0.05752,0.01813], "fy":[0.11578,0.10062,0.14,0.15516]}, + {"t":0.5252, "x":6.05329, "y":5.62207, "heading":-1.9361, "vx":-3.32196, "vy":1.75828, "omega":0.06261, "ax":-0.00101, "ay":0.00185, "alpha":-0.00789, "fx":[-0.04166,-0.00578,0.00794,-0.02794], "fy":[0.01975,0.00603,0.04191,0.05563]}, + {"t":0.55802, "x":5.94425, "y":5.67979, "heading":-1.93404, "vx":-3.32199, "vy":1.75834, "omega":0.06235, "ax":-0.00229, "ay":-0.00081, "alpha":-0.00748, "fx":[-0.06163,-0.02757,-0.01462,-0.04868], "fy":[-0.02399,-0.03694,-0.00288,0.01007]}, + {"t":0.59085, "x":5.8352, "y":5.7375, "heading":-1.932, "vx":-3.32207, "vy":1.75832, "omega":0.06211, "ax":-0.00783, "ay":-0.01208, "alpha":-0.00622, "fx":[-0.15011,-0.12178,-0.11107,-0.13941], "fy":[-0.21018,-0.22088,-0.19255,-0.18184]}, + {"t":0.62367, "x":5.72615, "y":5.79521, "heading":-1.92996, "vx":-3.32233, "vy":1.75792, "omega":0.06191, "ax":-0.04215, "ay":-0.08182, "alpha":0.00129, "fx":[-0.69853,-0.7044,-0.7066,-0.70073], "fy":[-1.36215,-1.35995,-1.36581,-1.36801]}, + {"t":0.6565, "x":5.61708, "y":5.85287, "heading":-1.92793, "vx":-3.32371, "vy":1.75523, "omega":0.06195, "ax":-0.24545, "ay":-0.52161, "alpha":0.07742, "fx":[-3.84877,-4.20283,-4.33417,-3.98018], "fy":[-8.58486,-8.4526,-8.80511,-8.93736]}, + {"t":0.68932, "x":5.50784, "y":5.91021, "heading":-1.92589, "vx":-3.33177, "vy":1.73811, "omega":0.06449, "ax":-0.78666, "ay":-3.28516, "alpha":2.33627, "fx":[-5.31255,-17.06323,-20.73466,-9.34231], "fy":[-51.94817,-47.80924,-57.65343,-61.63723]}, + {"t":0.72215, "x":5.39805, "y":5.96549, "heading":-1.92378, "vx":-3.35759, "vy":1.63028, "omega":0.14118, "ax":2.75987, "ay":-8.00364, "alpha":17.68871, "fx":[132.69168,20.48512,-33.4244,64.27072], "fy":[-97.23321,-110.70199,-163.61092,-162.12137]}, + {"t":0.75497, "x":5.28933, "y":6.01469, "heading":-1.91914, "vx":-3.267, "vy":1.36756, "omega":0.72181, "ax":3.5832, "ay":-8.61755, "alpha":19.83604, "fx":[161.0045,42.47074,-41.40998,76.85561], "fy":[-90.31281,-131.44015,-180.33266,-172.51591]}, + {"t":0.7878, "x":5.18402, "y":6.05494, "heading":-1.89545, "vx":-3.14938, "vy":1.08469, "omega":1.37292, "ax":3.13453, "ay":-9.98717, "alpha":15.46977, "fx":[146.26212,24.03809,-31.49297,70.19708], "fy":[-121.63444,-175.82861,-188.64089,-179.82118]}, + {"t":0.82062, "x":5.08233, "y":6.08516, "heading":-1.85038, "vx":-3.04649, "vy":0.75686, "omega":1.88071, "ax":1.8874, "ay":-11.31566, "alpha":8.54037, "fx":[87.85631,6.3539,-15.7869,47.42458], "fy":[-174.47526,-194.06509,-195.3757,-190.59017]}, + {"t":0.85345, "x":4.98335, "y":6.10391, "heading":-1.78865, "vx":-2.98453, "vy":0.38543, "omega":2.16105, "ax":2.14018, "ay":-11.53476, "alpha":5.39405, "fx":[70.7087,19.66163,4.77809,47.55499], "fy":[-184.05514,-195.88011,-197.39049,-191.78957]}, + {"t":0.88627, "x":4.88653, "y":6.11035, "heading":-1.71771, "vx":-2.91428, "vy":0.0068, "omega":2.33811, "ax":2.72288, "ay":-11.53075, "alpha":2.98084, "fx":[64.16033,36.80184,27.92301,52.67117], "fy":[-187.29128,-194.4352,-196.10673,-191.01516]}, + {"t":0.9191, "x":4.79234, "y":6.10436, "heading":-1.64096, "vx":-2.8249, "vy":-0.3717, "omega":2.43596, "ax":3.38256, "ay":-11.41022, "alpha":0.74089, "fx":[60.8537,54.32095,51.99899,58.36871], "fy":[-188.84457,-190.81295,-191.49317,-189.66082]}, + {"t":0.95192, "x":4.70143, "y":6.08601, "heading":-1.561, "vx":-2.71387, "vy":-0.74623, "omega":2.46028, "ax":4.03737, "ay":-11.20434, "alpha":-1.445, "fx":[59.07138,71.21501,75.84151,63.07612], "fy":[-189.68161,-185.48064,-183.58672,-188.33475]}, + {"t":0.98475, "x":4.61453, "y":6.05548, "heading":-1.48025, "vx":-2.58135, "vy":-1.11402, "omega":2.41284, "ax":4.66342, "ay":-10.92385, "alpha":-3.68928, "fx":[58.22862,87.45538,99.25116,66.01253], "fy":[-190.11946,-178.59838,-172.21805,-187.44551]}, + {"t":1.01757, "x":4.53231, "y":6.01303, "heading":-1.40104, "vx":-2.42827, "vy":-1.47259, "omega":2.29174, "ax":5.48423, "ay":-10.35021, "alpha":-7.87215, "fx":[54.88429,109.91174,135.92241,64.95927], "fy":[-191.24381,-165.88881,-145.15229,-187.8471]}, + {"t":1.0504, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-2.24825, "vy":-1.81233, "omega":2.03334, "ax":6.36759, "ay":-9.87174, "alpha":-7.09366, "fx":[73.88508,122.11045,144.14765,84.43569], "fy":[-184.62076,-156.99966,-136.8145,-179.79375]}, + {"t":1.07699, "x":4.39802, "y":5.90743, "heading":-1.27175, "vx":-2.07893, "vy":-2.07483, "omega":1.84471, "ax":7.22974, "ay":-9.20487, "alpha":-7.75367, "fx":[86.8659,135.99852,159.40302,99.79771], "fy":[-178.7597,-145.01015,-118.4688,-171.52468]}, + {"t":1.10358, "x":4.3453, "y":5.84901, "heading":-1.2227, "vx":-1.88668, "vy":-2.3196, "omega":1.63853, "ax":8.41854, "ay":-8.04224, "alpha":-8.60241, "fx":[105.14054,153.66043,178.01045,124.52065], "fy":[-168.47194,-125.95178,-87.68901,-154.1286]}, + {"t":1.13017, "x":4.2981, "y":5.78448, "heading":-1.17913, "vx":-1.66283, "vy":-2.53345, "omega":1.40979, "ax":9.97042, "ay":-5.85868, "alpha":-9.48117, "fx":[132.02858,174.73517,194.77441,163.27051], "fy":[-148.0148,-94.24029,-36.97828,-111.41195]}, + {"t":1.15676, "x":4.25741, "y":5.71504, "heading":-1.14164, "vx":-1.3977, "vy":-2.68924, "omega":1.15767, "ax":11.33457, "ay":-1.78472, "alpha":-10.51358, "fx":[169.72208,193.83062,195.23461,196.98022], "fy":[-101.92854,-42.14438,33.84761,-8.77663]}, + {"t":1.18335, "x":4.22425, "y":5.6429, "heading":-1.11085, "vx":-1.0963, "vy":-2.7367, "omega":0.87811, "ax":10.88301, "ay":3.60988, "alpha":-10.89736, "fx":[197.59417,195.87734,169.44636,162.74024], "fy":[-4.57685,30.64044,102.8347,111.80187]}, + {"t":1.20994, "x":4.19895, "y":5.57141, "heading":-1.08751, "vx":-0.80691, "vy":-2.64071, "omega":0.58833, "ax":8.66549, "ay":7.81879, "alpha":-7.82967, "fx":[167.11713,170.25003,131.86014,108.57074], "fy":[105.83856,101.70261,148.21581,165.58505]}, + {"t":1.23653, "x":4.18056, "y":5.50395, "heading":-1.07186, "vx":-0.57649, "vy":-2.4328, "omega":0.38014, "ax":6.29656, "ay":9.98797, "alpha":-5.17122, "fx":[115.36193,131.67165,98.75315,74.05552], "fy":[161.18279,148.50176,172.25589,184.0381]}, + {"t":1.26312, "x":4.16745, "y":5.44279, "heading":-1.06175, "vx":-0.40906, "vy":-2.16721, "omega":0.24263, "ax":4.55067, "ay":10.96435, "alpha":-3.48718, "fx":[79.02991,97.23224,73.83764,53.33041], "fy":[182.09836,173.20333,184.4666,191.3137]}, + {"t":1.28972, "x":4.15818, "y":5.38904, "heading":-1.0553, "vx":-0.28805, "vy":-1.87565, "omega":0.1499, "ax":3.3418, "ay":11.42462, "alpha":-2.35877, "fx":[56.03252,71.13899,55.60093,40.0526], "fy":[190.63475,185.59029,190.86835,194.67848]}, + {"t":1.31631, "x":4.15171, "y":5.3432, "heading":-1.05131, "vx":-0.19919, "vy":-1.57186, "omega":0.08718, "ax":2.48782, "ay":11.65803, "alpha":-1.56738, "fx":[40.87462,51.98544,42.06351,30.95929], "fy":[194.57881,191.93954,194.38359,196.43332]}, + {"t":1.3429, "x":4.14729, "y":5.30553, "heading":-1.049, "vx":-0.13303, "vy":-1.26186, "omega":0.0455, "ax":1.86309, "ay":11.7839, "alpha":-0.99058, "fx":[30.32095,37.7574,31.76159,24.38723], "fy":[196.58991,195.31281,196.39341,197.43172]}, + {"t":1.36949, "x":4.14441, "y":5.27614, "heading":-1.04779, "vx":-0.08349, "vy":-0.94852, "omega":0.01916, "ax":1.39034, "ay":11.85483, "alpha":-0.55545, "fx":[22.61641,26.93672,23.71946,19.43275], "fy":[197.69,197.15444,197.57575,198.03712]}, + {"t":1.39608, "x":4.14268, "y":5.25511, "heading":-1.04728, "vx":-0.04652, "vy":-0.63328, "omega":0.00439, "ax":1.02193, "ay":11.89587, "alpha":-0.21729, "fx":[16.77226,18.50228,17.29455,15.57133], "fy":[198.32104,198.16928,198.28136,198.42202]}, + {"t":1.42267, "x":4.14181, "y":5.24248, "heading":-1.04716, "vx":-0.01935, "vy":-0.31696, "omega":-0.00139, "ax":0.72761, "ay":11.9198, "alpha":0.05224, "fx":[12.20042,11.77773,12.05732,12.48048], "fy":[198.69363,198.71869,198.70125,198.67556]}, + {"t":1.44926, "x":4.14155, "y":5.23826, "heading":-1.0472, "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/startToL.traj b/src/main/deploy/choreo/startToL.traj index 0c7aa6fe..8dcdc7be 100644 --- a/src/main/deploy/choreo/startToL.traj +++ b/src/main/deploy/choreo/startToL.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.9103645244834646, "y":5.178198486719822, "heading":-1.0471975511965976, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "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}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"L.x", "val":3.9103645244834646}, "y":{"exp":"L.y", "val":5.178198486719822}, "heading":{"exp":"L.heading", "val":-1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "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}, @@ -28,58 +28,56 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.05456,1.44688], + "waypoints":[0.0,1.10537,1.49745], "samples":[ - {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.64413, "ay":2.82147, "alpha":23.98294, "fx":[-160.82196,-43.21148,-141.31688,-174.7473], "fy":[-67.84429,169.81674,103.77169,-13.77469]}, - {"t":0.03102, "x":7.09721, "y":5.07704, "heading":3.14159, "vx":-0.23709, "vy":0.08751, "omega":0.74386, "ax":-7.76433, "ay":3.03317, "alpha":22.74526, "fx":[-165.70153,-46.29545,-141.39475,-174.88417], "fy":[-54.65195,168.97294,103.63552,-11.58314]}, - {"t":0.06203, "x":7.08612, "y":5.08121, "heading":-3.11852, "vx":-0.47791, "vy":0.18159, "omega":1.44934, "ax":-7.9525, "ay":3.34407, "alpha":20.64828, "fx":[-171.48302,-52.07415,-142.57478,-174.94674], "fy":[-31.60985,167.24981,101.9671,-10.0807]}, - {"t":0.09305, "x":7.06747, "y":5.08845, "heading":-3.07357, "vx":-0.72457, "vy":0.28531, "omega":2.08977, "ax":-8.14654, "ay":3.78224, "alpha":17.90866, "fx":[-174.23429,-60.40271,-144.65957,-174.98487], "fy":[2.5824,164.38672,98.93428,-8.56393]}, - {"t":0.12407, "x":7.04108, "y":5.09912, "heading":-3.00875, "vx":-0.97725, "vy":0.40262, "omega":2.64524, "ax":-8.26473, "ay":4.2893, "alpha":15.19585, "fx":[-168.84798,-71.10826,-147.32972,-175.03671], "fy":[42.81475,159.99702,94.83596,-5.80858]}, - {"t":0.15508, "x":7.0068, "y":5.11367, "heading":-2.9267, "vx":-1.23359, "vy":0.53566, "omega":3.11656, "ax":-8.31652, "ay":4.71341, "alpha":13.05545, "fx":[-156.44657,-84.20845,-150.14964,-175.04153], "fy":[76.66999,153.43473,90.18677,0.4032]}, - {"t":0.1861, "x":6.96453, "y":5.13255, "heading":-2.83004, "vx":-1.49154, "vy":0.68185, "omega":3.52149, "ax":-8.41507, "ay":4.99139, "alpha":10.90916, "fx":[-145.50472,-100.23226,-152.49963,-174.31494], "fy":[95.97688,143.36288,85.95292,14.31568]}, - {"t":0.21711, "x":6.91422, "y":5.1561, "heading":-2.72082, "vx":-1.75254, "vy":0.83667, "omega":3.85985, "ax":-8.60109, "ay":5.22453, "alpha":7.16574, "fx":[-142.28598,-120.70363,-153.10515,-169.11357], "fy":[100.82773,126.37894,84.48675,43.77744]}, - {"t":0.24813, "x":6.85573, "y":5.18456, "heading":-2.6011, "vx":-2.01932, "vy":0.99871, "omega":4.08211, "ax":-8.65633, "ay":5.49631, "alpha":-0.42033, "fx":[-147.74245,-148.70035,-146.74147,-145.78233], "fy":[92.75007,91.16527,94.26504,95.78213]}, - {"t":0.27915, "x":6.78893, "y":5.21818, "heading":-2.47449, "vx":-2.28781, "vy":1.16919, "omega":4.06907, "ax":-7.55649, "ay":5.45555, "alpha":-15.52247, "fx":[-158.55358,-173.70793,-83.62116,-98.25215], "fy":[72.75896,3.23907,151.18977,144.00146]}, - {"t":0.31016, "x":6.71434, "y":5.25707, "heading":-2.34828, "vx":-2.52218, "vy":1.3384, "omega":3.58762, "ax":-7.62067, "ay":5.05061, "alpha":-17.05583, "fx":[-163.34837,-172.92744,-76.4881,-105.73789], "fy":[60.54455,-9.58446,154.36602,138.31162]}, - {"t":0.34118, "x":6.63244, "y":5.30101, "heading":-2.237, "vx":-2.75855, "vy":1.49505, "omega":3.05861, "ax":-7.76789, "ay":4.46673, "alpha":-18.36542, "fx":[-167.61875,-170.3519,-76.04522,-114.50272], "fy":[46.05689,-26.39666,153.60422,130.64703]}, - {"t":0.3722, "x":6.54315, "y":5.34953, "heading":-2.14214, "vx":-2.99948, "vy":1.63359, "omega":2.48898, "ax":-7.85641, "ay":3.5891, "alpha":-20.40125, "fx":[-171.11021,-162.24673,-77.38394,-123.80022], "fy":[26.89183,-54.39428,150.78618,120.91427]}, - {"t":0.40321, "x":6.44634, "y":5.40193, "heading":-2.06494, "vx":-3.24316, "vy":1.74492, "omega":1.85621, "ax":-7.35128, "ay":3.00614, "alpha":-24.43011, "fx":[-171.57416,-147.47259,-55.44407,-125.68195], "fy":[12.40133,-81.42507,156.40663,117.15172]}, - {"t":0.43423, "x":6.34221, "y":5.4575, "heading":-2.00736, "vx":-3.47117, "vy":1.83816, "omega":1.09847, "ax":-7.10095, "ay":3.19695, "alpha":-23.31839, "fx":[-167.53309,-145.55687,-48.06949,-121.98108], "fy":[15.65174,-65.6322,151.66981,115.8277]}, - {"t":0.46525, "x":6.23113, "y":5.51605, "heading":-1.97329, "vx":-3.69141, "vy":1.93731, "omega":0.37522, "ax":-3.39846, "ay":1.63877, "alpha":-11.31774, "fx":[-91.01414,-49.76212,-21.89174,-68.55938], "fy":[11.44978,-13.14631,50.8023,62.39394]}, - {"t":0.49626, "x":6.115, "y":5.57692, "heading":-1.96165, "vx":-3.79682, "vy":1.98814, "omega":0.02419, "ax":-0.08369, "ay":-0.11534, "alpha":-0.06316, "fx":[-1.62425,-1.33984,-1.22267,-1.50713], "fy":[-2.04538,-2.16269,-1.8783,-1.761]}, - {"t":0.52728, "x":5.9972, "y":5.63853, "heading":-1.9609, "vx":-3.79942, "vy":1.98456, "omega":0.02223, "ax":-0.20044, "ay":-0.386, "alpha":0.0017, "fx":[-3.40405,-3.4117,-3.41484,-3.40719], "fy":[-6.56345,-6.5603,-6.56793,-6.57108]}, - {"t":0.55829, "x":5.87926, "y":5.6999, "heading":-1.96022, "vx":-3.80563, "vy":1.97259, "omega":0.02228, "ax":-0.73926, "ay":-1.4515, "alpha":0.00873, "fx":[-12.54664,-12.58687,-12.60246,-12.56222], "fy":[-24.67878,-24.66208,-24.7005,-24.7172]}, - {"t":0.58931, "x":5.76086, "y":5.76039, "heading":-1.95952, "vx":-3.82856, "vy":1.92757, "omega":0.02255, "ax":-2.13867, "ay":-4.46287, "alpha":0.03114, "fx":[-36.27245,-36.44343,-36.48395,-36.31312], "fy":[-75.89444,-75.82003,-75.92988,-76.00422]}, - {"t":0.62033, "x":5.64109, "y":5.81802, "heading":-1.95882, "vx":-3.8949, "vy":1.78915, "omega":0.02352, "ax":-3.11273, "ay":-7.55202, "alpha":0.26504, "fx":[-51.88094,-53.83448,-54.00102,-52.07009], "fy":[-128.6358,-127.83211,-128.2844,-129.07872]}, - {"t":0.65134, "x":5.51878, "y":5.86989, "heading":-1.9581, "vx":-3.99144, "vy":1.55491, "omega":0.03174, "ax":-1.42995, "ay":-9.20031, "alpha":5.14418, "fx":[5.36199,-43.37589,-48.79522,-10.48306], "fy":[-158.63234,-151.21336,-154.14559,-161.98758]}, - {"t":0.68236, "x":5.3943, "y":5.91369, "heading":-1.95711, "vx":-4.03579, "vy":1.26955, "omega":0.19129, "ax":1.96896, "ay":-9.0953, "alpha":12.09475, "fx":[111.57017,8.78088,-34.1168,47.73183], "fy":[-126.66652,-161.96641,-166.18831,-164.01252]}, - {"t":0.71338, "x":5.27007, "y":5.94869, "heading":-1.95118, "vx":-3.97472, "vy":0.98745, "omega":0.56643, "ax":3.15655, "ay":-8.8378, "alpha":12.87639, "fx":[132.59736,45.81508,-26.21913,62.57451], "fy":[-109.59353,-160.26046,-170.17481,-161.28505]}, - {"t":0.74439, "x":5.14831, "y":5.97507, "heading":-1.93361, "vx":-3.87682, "vy":0.71333, "omega":0.96581, "ax":3.74537, "ay":-8.73981, "alpha":12.47705, "fx":[137.44168,64.1586,-16.4573,69.6874], "fy":[-105.50855,-157.33904,-172.51535,-159.28396]}, - {"t":0.77541, "x":5.02986, "y":5.99299, "heading":-1.90365, "vx":-3.76065, "vy":0.44226, "omega":1.3528, "ax":4.11545, "ay":-8.71588, "alpha":11.67612, "fx":[138.03758,71.81682,-4.96971,75.12576], "fy":[-105.75548,-156.14546,-173.83481,-157.28254]}, - {"t":0.80643, "x":4.9152, "y":6.00251, "heading":-1.86169, "vx":-3.633, "vy":0.17192, "omega":1.71495, "ax":4.40685, "ay":-8.71237, "alpha":10.6775, "fx":[136.9781,75.0444,7.59261,80.22166], "fy":[-107.71619,-155.90755,-174.115,-155.04084]}, - {"t":0.83744, "x":4.80464, "y":6.00365, "heading":-1.8085, "vx":-3.49632, "vy":-0.09831, "omega":2.04613, "ax":4.68216, "ay":-8.70129, "alpha":9.53197, "fx":[134.93333,77.18089,21.1017,85.35293], "fy":[-110.64158,-155.66387,-173.24966,-152.47076]}, - {"t":0.86846, "x":4.69845, "y":5.99642, "heading":-1.74504, "vx":-3.3511, "vy":-0.36819, "omega":2.34177, "ax":5.061, "ay":-8.64822, "alpha":7.79496, "fx":[130.84684,82.41997,39.68354,91.39397], "fy":[-115.69805,-153.52852,-170.13274,-149.05538]}, - {"t":0.89948, "x":4.59694, "y":5.98084, "heading":-1.67241, "vx":-3.19412, "vy":-0.63642, "omega":2.58355, "ax":5.99502, "ay":-8.30946, "alpha":2.74975, "fx":[117.03869,101.21071,86.51684,103.12775], "fy":[-129.83368,-142.41702,-151.89682,-141.21888]}, - {"t":0.93049, "x":4.50075, "y":5.9571, "heading":-1.59227, "vx":-3.00818, "vy":-0.89415, "omega":2.66883, "ax":6.65556, "ay":-7.82259, "alpha":-1.7646, "fx":[103.90591,113.87853,122.32009,112.73239], "fy":[-140.71171,-132.81343,-125.02968,-133.68509]}, - {"t":0.96151, "x":4.41065, "y":5.92561, "heading":-1.5095, "vx":-2.80175, "vy":-1.13678, "omega":2.6141, "ax":7.04043, "ay":-7.36615, "alpha":-4.95685, "fx":[94.62115,122.72709,143.45682,118.21796], "fy":[-147.23042,-124.85974,-100.22397,-128.87011]}, - {"t":0.99252, "x":4.32714, "y":5.8868, "heading":-1.42842, "vx":-2.58338, "vy":-1.36525, "omega":2.46036, "ax":7.26758, "ay":-6.98619, "alpha":-7.22194, "fx":[88.81652,129.69643,155.98253,119.98238], "fy":[-150.8912,-117.7203,-79.47814,-127.24306]}, - {"t":1.02354, "x":4.25051, "y":5.8411, "heading":-1.3521, "vx":-2.35796, "vy":-1.58194, "omega":2.23636, "ax":7.39863, "ay":-6.68399, "alpha":-8.9277, "fx":[85.6409,135.4397,163.63011,118.68342], "fy":[-152.78355,-111.14761,-62.36555,-128.47414]}, - {"t":1.05456, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-2.12849, "vy":-1.78925, "omega":1.95946, "ax":7.78396, "ay":-6.28799, "alpha":-8.37466, "fx":[97.0894,141.39826,165.37952,125.74447], "fy":[-145.69425,-103.35875,-57.32977,-121.44482]}, - {"t":1.08071, "x":4.12792, "y":5.73987, "heading":-1.23149, "vx":-1.9249, "vy":-1.95372, "omega":1.74042, "ax":8.35372, "ay":-5.45045, "alpha":-8.74523, "fx":[108.12702,150.0745,170.88598,139.28991], "fy":[-137.61704,-90.20927,-37.60221,-105.4138]}, - {"t":1.10687, "x":4.08043, "y":5.6869, "heading":-1.18597, "vx":-1.7064, "vy":-2.09627, "omega":1.51168, "ax":8.99951, "ay":-4.22167, "alpha":-9.10519, "fx":[122.19128,159.29699,174.50021,156.32814], "fy":[-125.16976,-72.55668,-11.95853,-77.55288]}, - {"t":1.13302, "x":4.03888, "y":5.63063, "heading":-1.14643, "vx":-1.47102, "vy":-2.20669, "omega":1.27354, "ax":9.60576, "ay":-2.39934, "alpha":-9.55174, "fx":[140.02812,168.14236,173.7058,171.68839], "fy":[-104.6263,-48.41859,19.96372,-30.16733]}, - {"t":1.15918, "x":4.00369, "y":5.5721, "heading":-1.11312, "vx":-1.21978, "vy":-2.26945, "omega":1.02371, "ax":9.85479, "ay":0.12313, "alpha":-10.20631, "fx":[160.4787,174.1988,165.50757,170.32383], "fy":[-68.87837,-15.65439,56.25877,36.65191]}, - {"t":1.18533, "x":3.97516, "y":5.51278, "heading":-1.08635, "vx":-0.96203, "vy":-2.26623, "omega":0.75676, "ax":9.39224, "ay":3.06073, "alpha":-10.08482, "fx":[174.23235,172.85964,148.57024,143.37483], "fy":[-9.20955,26.20931,92.09635,99.15233]}, - {"t":1.21149, "x":3.95321, "y":5.45455, "heading":-1.06655, "vx":-0.71637, "vy":-2.18617, "omega":0.49299, "ax":8.13434, "ay":5.84711, "alpha":-8.06267, "fx":[161.51639,159.01812,125.17399,107.74266], "fy":[65.90197,72.59189,122.04751,137.28961]}, - {"t":1.23764, "x":3.93726, "y":5.39937, "heading":-1.05366, "vx":-0.50362, "vy":-2.03324, "omega":0.28211, "ax":6.38891, "ay":7.88701, "alpha":-5.59214, "fx":[123.84147,132.67515,99.83888,78.3386], "fy":[123.04976,113.84352,143.57491,156.15476]}, - {"t":1.2638, "x":3.92627, "y":5.34889, "heading":-1.04628, "vx":-0.33652, "vy":-1.82696, "omega":0.13585, "ax":4.70607, "ay":9.07855, "alpha":-3.68163, "fx":[86.01569,100.8828,76.20148,57.09587], "fy":[152.12028,142.85014,157.46529,165.25848]}, - {"t":1.28995, "x":3.91908, "y":5.30421, "heading":-1.04273, "vx":-0.21343, "vy":-1.58951, "omega":0.03956, "ax":3.33454, "ay":9.70832, "alpha":-2.25089, "fx":[57.99386,71.09676,55.8528,41.93508], "fy":[165.00195,159.85629,165.83845,169.84654]}, - {"t":1.31611, "x":3.91464, "y":5.26596, "heading":-1.04169, "vx":-0.12622, "vy":-1.33558, "omega":-0.01931, "ax":2.27472, "ay":10.0294, "alpha":-1.16862, "fx":[38.43479,46.50908,38.96647,30.85921], "fy":[170.72776,168.72964,170.65012,172.28169]}, - {"t":1.34226, "x":3.91211, "y":5.23446, "heading":-1.0422, "vx":-0.06672, "vy":-1.07326, "omega":-0.04988, "ax":1.4611, "ay":10.18832, "alpha":-0.34592, "fx":[24.58105,27.1932,25.11978,22.51765], "fy":[173.34187,172.95622,173.27542,173.62808]}, - {"t":1.36842, "x":3.91087, "y":5.20987, "heading":-1.0435, "vx":-0.0285, "vy":-0.80679, "omega":-0.05893, "ax":0.82946, "ay":10.262, "alpha":0.28743, "fx":[14.45349,12.17387,13.75744,16.05079], "fy":[174.53493,174.7056,174.58339,174.39076]}, - {"t":1.39457, "x":3.91041, "y":5.19228, "heading":-1.04504, "vx":-0.00681, "vy":-0.53838, "omega":-0.05141, "ax":0.33068, "ay":10.29027, "alpha":0.78411, "fx":[6.80676,0.41128,4.3725,10.90879], "fy":[175.04321,175.16823,175.10222,174.82438]}, - {"t":1.42073, "x":3.91034, "y":5.18172, "heading":-1.04639, "vx":0.00184, "vy":-0.26924, "omega":-0.0309, "ax":-0.07031, "ay":10.29406, "alpha":1.18142, "fx":[0.86228,-8.93043,-3.4473,6.73166], "fy":[175.2073,174.97332,175.15037,175.06482]}, - {"t":1.44688, "x":3.91036, "y":5.1782, "heading":-1.0472, "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.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.72765, "ay":1.05979, "alpha":45.60884, "fx":[-20.40347,-7.01096,-161.16336,-193.3312], "fy":[-197.49548,198.97143,117.09875,-47.91016]}, + {"t":0.0335, "x":7.09767, "y":5.07627, "heading":3.14159, "vx":-0.19185, "vy":0.0355, "omega":1.52772, "ax":-6.24476, "ay":1.1967, "alpha":43.42892, "fx":[-46.27794,-14.49748,-161.17053,-194.44284], "fy":[-192.77535,198.49962,117.02164,-42.95195]}, + {"t":0.06699, "x":7.08774, "y":5.07813, "heading":-3.09042, "vx":-0.40103, "vy":0.07558, "omega":2.98242, "ax":-7.27619, "ay":1.42797, "alpha":38.37283, "fx":[-92.69361,-33.06614,-164.25745,-195.14522], "fy":[-174.29214,196.16872,112.53435,-39.19674]}, + {"t":0.10049, "x":7.07023, "y":5.08147, "heading":-2.99052, "vx":-0.64475, "vy":0.12341, "omega":4.26775, "ax":-9.37106, "ay":3.62815, "alpha":22.3557, "fx":[-194.75012,-64.19966,-169.12948,-196.76486], "fy":[-22.24437,188.12902,104.83592,-28.80256]}, + {"t":0.13398, "x":7.04338, "y":5.08764, "heading":-2.84757, "vx":-0.95865, "vy":0.24494, "omega":5.01658, "ax":-9.74415, "ay":5.85512, "alpha":11.91269, "fx":[-165.48149,-115.02862,-171.9935,-197.21753], "fy":[107.73817,161.6872,99.39489,21.58771]}, + {"t":0.16748, "x":7.0058, "y":5.09913, "heading":-2.67953, "vx":-1.28504, "vy":0.44107, "omega":5.41561, "ax":-8.27501, "ay":6.99416, "alpha":-16.93947, "fx":[-169.38127,-196.65523,-94.59338,-91.13202], "fy":[102.77219,16.78938,171.13576,175.65968]}, + {"t":0.20098, "x":6.95811, "y":5.11782, "heading":-2.49813, "vx":-1.56222, "vy":0.67534, "omega":4.84821, "ax":-8.42921, "ay":6.49415, "alpha":-18.75189, "fx":[-178.14532,-196.70348,-82.29165,-104.9034], "fy":[86.03637,2.54253,176.93069,167.50813]}, + {"t":0.23447, "x":6.90106, "y":5.14409, "heading":-2.33573, "vx":-1.84456, "vy":0.89287, "omega":4.22009, "ax":-9.04295, "ay":5.51776, "alpha":-18.93632, "fx":[-186.90023,-195.08369,-96.22513,-124.75748], "fy":[63.39659,-16.85266,168.69285,152.67656]}, + {"t":0.26797, "x":6.8342, "y":5.17709, "heading":-2.19438, "vx":-2.14747, "vy":1.0777, "omega":3.5858, "ax":-9.61987, "ay":3.72774, "alpha":-20.45976, "fx":[-194.56235,-184.13311,-116.94239,-145.79684], "fy":[28.30778,-62.69257,151.52971,131.41331]}, + {"t":0.30146, "x":6.75687, "y":5.21528, "heading":-2.07427, "vx":-2.46969, "vy":1.20256, "omega":2.90048, "ax":-9.84603, "ay":0.17041, "alpha":-21.65097, "fx":[-193.06931,-136.42574,-156.59344,-170.42593], "fy":[-30.64096,-136.96374,85.32109,93.64625]}, + {"t":0.33496, "x":6.66862, "y":5.25566, "heading":-1.97711, "vx":-2.7995, "vy":1.20827, "omega":2.17525, "ax":-9.08969, "ay":-0.19465, "alpha":-24.62674, "fx":[-187.06988,-117.38366,-130.19827,-171.43136], "fy":[-44.98784,-147.19546,95.63626,83.56804]}, + {"t":0.36846, "x":6.56975, "y":5.29602, "heading":-1.90425, "vx":-3.10397, "vy":1.20175, "omega":1.35036, "ax":-8.03166, "ay":2.22523, "alpha":-24.82627, "fx":[-180.99335,-136.28269,-71.36922,-146.89036], "fy":[-7.91647,-88.59347,139.7737,105.1103]}, + {"t":0.40195, "x":6.46127, "y":5.33752, "heading":-1.85902, "vx":-3.373, "vy":1.27628, "omega":0.51877, "ax":-3.34735, "ay":2.90194, "alpha":-12.47764, "fx":[-94.81057,-41.28195,-15.96901,-71.13315], "fy":[25.60712,9.08388,77.17693,81.62782]}, + {"t":0.43545, "x":6.34641, "y":5.3819, "heading":-1.84164, "vx":-3.48512, "vy":1.37349, "omega":0.10082, "ax":-0.01345, "ay":0.40002, "alpha":-0.48449, "fx":[-1.67879,0.59815,1.2296,-1.04578], "fy":[5.84801,5.21701,7.4887,8.11868]}, + {"t":0.46895, "x":6.22967, "y":5.42813, "heading":-1.83827, "vx":-3.48557, "vy":1.38689, "omega":0.08459, "ax":0.01305, "ay":0.05915, "alpha":-0.0349, "fx":[0.11302,0.27697,0.3219,0.15795], "fy":[0.92653,0.8816,1.04555,1.09048]}, + {"t":0.50244, "x":6.11292, "y":5.47462, "heading":-1.83543, "vx":-3.48513, "vy":1.38887, "omega":0.08342, "ax":0.00006, "ay":0.01144, "alpha":-0.01751, "fx":[-0.05134,0.03095,0.05325,-0.02904], "fy":[0.16073,0.13843,0.22072,0.24302]}, + {"t":0.53594, "x":5.99618, "y":5.52115, "heading":-1.83264, "vx":-3.48513, "vy":1.38925, "omega":0.08284, "ax":-0.00223, "ay":0.00414, "alpha":-0.01557, "fx":[-0.08359,-0.01034,0.00929,-0.06396], "fy":[0.04222,0.02258,0.09583,0.11546]}, + {"t":0.56943, "x":5.87944, "y":5.56768, "heading":-1.82986, "vx":-3.48521, "vy":1.38939, "omega":0.08232, "ax":-0.00252, "ay":0.00295, "alpha":-0.01494, "fx":[-0.08646,-0.01612,0.00252,-0.06782], "fy":[0.02325,0.00461,0.07494,0.09358]}, + {"t":0.60293, "x":5.7627, "y":5.61423, "heading":-1.82711, "vx":-3.48529, "vy":1.38949, "omega":0.08181, "ax":-0.00263, "ay":0.0023, "alpha":-0.01443, "fx":[-0.08674,-0.01873,-0.00091,-0.06892], "fy":[0.01324,-0.00458,0.06342,0.08125]}, + {"t":0.63643, "x":5.64596, "y":5.66077, "heading":-1.82437, "vx":-3.48538, "vy":1.38957, "omega":0.08133, "ax":-0.00358, "ay":-0.00079, "alpha":-0.01354, "fx":[-0.09995,-0.03611,-0.01957,-0.08341], "fy":[-0.03688,-0.05343,0.01042,0.02696]}, + {"t":0.66992, "x":5.52921, "y":5.70731, "heading":-1.82164, "vx":-3.4855, "vy":1.38954, "omega":0.08088, "ax":-0.01023, "ay":-0.02056, "alpha":-0.0098, "fx":[-0.19956,-0.15331,-0.14146,-0.18771], "fy":[-0.35996,-0.37181,-0.32556,-0.3137]}, + {"t":0.70342, "x":5.41245, "y":5.75385, "heading":-1.81893, "vx":-3.48584, "vy":1.38885, "omega":0.08055, "ax":-0.05164, "ay":-0.14837, "alpha":0.0176, "fx":[-0.80872,-0.89182,-0.91287,-0.82977], "fy":[-2.44219,-2.42113,-2.5042,-2.52526]}, + {"t":0.73691, "x":5.29566, "y":5.80028, "heading":-1.81623, "vx":-3.48757, "vy":1.38388, "omega":0.08114, "ax":-0.22542, "ay":-0.99283, "alpha":0.45311, "fx":[-2.41196,-4.57041,-5.10142,-2.94702], "fy":[-15.7575,-15.21753,-17.34315,-17.88202]}, + {"t":0.77041, "x":5.17871, "y":5.84608, "heading":-1.81352, "vx":-3.49512, "vy":1.35063, "omega":0.09632, "ax":1.3661, "ay":-5.38128, "alpha":9.30156, "fx":[58.59854,3.80507,-9.50058,38.18587], "fy":[-74.0312,-68.56769,-106.3117,-109.90279]}, + {"t":0.80391, "x":5.06241, "y":5.8883, "heading":-1.81029, "vx":-3.44936, "vy":1.17037, "omega":0.40788, "ax":4.17116, "ay":-7.6073, "alpha":18.86725, "fx":[154.9357,47.76738,-14.31404,89.73565], "fy":[-74.26433,-105.47016,-172.09914,-155.4067]}, + {"t":0.8374, "x":4.94921, "y":5.92324, "heading":-1.79663, "vx":-3.30964, "vy":0.91556, "omega":1.03986, "ax":4.06322, "ay":-8.67308, "alpha":18.34432, "fx":[162.71767,37.40347,-19.60963,90.41645], "fy":[-84.78346,-144.54474,-183.78346,-165.19274]}, + {"t":0.8709, "x":4.84063, "y":5.94904, "heading":-1.7618, "vx":-3.17354, "vy":0.62504, "omega":1.65432, "ax":2.92028, "ay":-10.63646, "alpha":10.49739, "fx":[114.02322,18.40862,-5.20323,67.49005], "fy":[-151.9385,-185.16992,-191.61055,-180.49987]}, + {"t":0.90439, "x":4.73596, "y":5.96401, "heading":-1.70638, "vx":-3.07572, "vy":0.26876, "omega":2.00594, "ax":2.15455, "ay":-11.49864, "alpha":4.24309, "fx":[62.33027,21.56139,12.49098,47.2788], "fy":[-185.70379,-194.40885,-195.90561,-190.68882]}, + {"t":0.93789, "x":4.63415, "y":5.96656, "heading":-1.63919, "vx":-3.00356, "vy":-0.11639, "omega":2.14807, "ax":2.69844, "ay":-11.529, "alpha":1.69419, "fx":[55.1467,39.4352,35.31317,50.03185], "fy":[-189.64341,-193.47076,-194.41733,-191.20018]}, + {"t":0.97139, "x":4.53506, "y":5.9562, "heading":-1.56724, "vx":-2.91317, "vy":-0.50257, "omega":2.20482, "ax":3.41999, "ay":-11.38563, "alpha":-0.33778, "fx":[55.07472,58.0598,58.96398,55.93998], "fy":[-190.37757,-189.49428,-189.19513,-190.10519]}, + {"t":1.00488, "x":4.43939, "y":5.93297, "heading":-1.49339, "vx":-2.79861, "vy":-0.88394, "omega":2.19351, "ax":4.13336, "ay":-11.14891, "alpha":-2.266, "fx":[56.65914,75.59884,81.99308,61.35345], "fy":[-190.29065,-183.61505,-180.75384,-188.7283]}, + {"t":1.03838, "x":4.34797, "y":5.89711, "heading":-1.41991, "vx":-2.66016, "vy":-1.25739, "omega":2.1176, "ax":4.78867, "ay":-10.84897, "alpha":-4.18126, "fx":[58.80075,91.62955,103.63319,65.23565], "fy":[-189.8678,-176.41369,-169.50091,-187.60597]}, + {"t":1.07187, "x":4.26155, "y":5.84891, "heading":-1.34898, "vx":-2.49976, "vy":-1.62079, "omega":1.97755, "ax":5.54694, "ay":-10.33967, "alpha":-7.44376, "fx":[59.06032,111.38961,133.68383,65.72587], "fy":[-189.94416,-164.8376,-147.12626,-187.52153]}, + {"t":1.10537, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-2.31396, "vy":-1.96713, "omega":1.72821, "ax":6.68578, "ay":-9.68924, "alpha":-6.51318, "fx":[82.30925,126.28405,145.40446,91.79725], "fy":[-180.96708,-153.59762,-135.39862,-176.09629]}, + {"t":1.13338, "x":4.11875, "y":5.72993, "heading":-1.23434, "vx":-2.12672, "vy":-2.23848, "omega":1.5458, "ax":7.8407, "ay":-8.72933, "alpha":-7.10148, "fx":[100.33269,143.68449,164.46914,114.31666], "fy":[-171.46833,-137.29176,-111.1903,-162.10466]}, + {"t":1.16138, "x":4.06226, "y":5.66381, "heading":-1.19105, "vx":-1.90713, "vy":-2.48296, "omega":1.34692, "ax":9.4219, "ay":-6.89615, "alpha":-7.88261, "fx":[126.47944,165.91196,186.33044,149.51216], "fy":[-152.92761,-109.12396,-67.97058,-129.8002]}, + {"t":1.18939, "x":4.01255, "y":5.59157, "heading":-1.15333, "vx":-1.64326, "vy":-2.67609, "omega":1.12616, "ax":11.12025, "ay":-3.33476, "alpha":-8.68475, "fx":[163.3375,188.93819,198.19235,191.00889], "fy":[-112.16482,-60.57923,0.75461,-50.3657]}, + {"t":1.21739, "x":3.97089, "y":5.51532, "heading":-1.12179, "vx":-1.33182, "vy":-2.76948, "omega":0.88293, "ax":11.39095, "ay":2.0782, "alpha":-9.24489, "fx":[196.22077,197.95181,182.05152,183.30247], "fy":[-25.18757,11.73571,78.39399,73.62831]}, + {"t":1.2454, "x":3.93805, "y":5.43857, "heading":-1.09706, "vx":-1.01281, "vy":-2.71128, "omega":0.62402, "ax":9.50304, "ay":6.83162, "alpha":-7.25986, "fx":[178.43174,178.14965,146.02395,131.03926], "fy":[85.60982,87.18711,134.27855,148.44408]}, + {"t":1.27341, "x":3.91342, "y":5.36531, "heading":-1.07958, "vx":-0.74667, "vy":-2.51996, "omega":0.4207, "ax":7.10866, "ay":9.44, "alpha":-4.92478, "fx":[129.92301,141.68037,111.26614,91.12235], "fy":[149.74614,139.01726,164.45817,176.21943]}, + {"t":1.30141, "x":3.89529, "y":5.29844, "heading":-1.0678, "vx":-0.54758, "vy":-2.25558, "omega":0.28278, "ax":5.26892, "ay":10.64176, "alpha":-3.39156, "fx":[92.23712,107.65497,84.83427,66.59488], "fy":[175.80652,166.95145,179.69016,187.12401]}, + {"t":1.32942, "x":3.88202, "y":5.23945, "heading":-1.05988, "vx":-0.40002, "vy":-1.95754, "omega":0.18779, "ax":3.98991, "ay":11.21567, "alpha":-2.37474, "fx":[67.73132,81.64588,65.67842,50.9842], "fy":[186.82063,181.23773,187.65844,192.1227]}, + {"t":1.35742, "x":3.87239, "y":5.18902, "heading":-1.05462, "vx":-0.28828, "vy":-1.64344, "omega":0.12128, "ax":3.09067, "ay":11.51286, "alpha":-1.66904, "fx":[51.47309,62.55634,51.64311,40.40739], "fy":[192.06608,188.78069,192.08114,194.72762]}, + {"t":1.38543, "x":3.86552, "y":5.14751, "heading":-1.05123, "vx":-0.20172, "vy":-1.32101, "omega":0.07454, "ax":2.43672, "ay":11.67858, "alpha":-1.15883, "fx":[40.14317,48.40375,41.08955,32.83953], "fy":[194.83825,192.9699,194.67625,196.22104]}, + {"t":1.41344, "x":3.86083, "y":5.11509, "heading":-1.04914, "vx":-0.13348, "vy":-0.99394, "omega":0.04209, "ax":1.94452, "ay":11.77662, "alpha":-0.77635, "fx":[31.87747,37.66194,32.93435,27.18325], "fy":[196.42437,195.40738,196.27032,197.1404]}, + {"t":1.44144, "x":3.85785, "y":5.09188, "heading":-1.04796, "vx":-0.07902, "vy":-0.66412, "omega":0.02034, "ax":1.56269, "ay":11.83731, "alpha":-0.4806, "fx":[25.61419,29.3027,26.4739,22.8067], "fy":[197.3875,196.87867,197.2851,197.73793]}, + {"t":1.46945, "x":3.85625, "y":5.07792, "heading":-1.04739, "vx":-0.03525, "vy":-0.3326, "omega":0.00689, "ax":1.25882, "ay":11.87618, "alpha":-0.24584, "fx":[20.71986,22.64655,21.24421,19.32487], "fy":[197.99909,197.79013,197.94855,198.14304]}, + {"t":1.49745, "x":3.85576, "y":5.07326, "heading":-1.0472, "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/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 40f7bb24..8369db74 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -95,22 +95,21 @@ public final class VisionConstants { public static final int kCircularBufferSize = 1000; // Poses public static final Pose3d kCam1Pose = - new Pose3d(new Translation3d(0.28, 0.03, 0.30), new Rotation3d()); - + new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d()); public static final Pose3d kCam2Pose = new Pose3d( - new Translation3d(0.236, -0.108, 0.932), - new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(22.5))); + new Translation3d(0.236, 0.108, 0.932), + new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(22.5))); public static final Pose3d kCam3Pose = - new Pose3d(new Translation3d(0.12, -0.28, 0.30), new Rotation3d()); + new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( - new Translation3d(0.20, 0.035, 0.94), - new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(-22.5))); + new Translation3d(0.236, -.038, 0.932), + new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(-22.5))); public static final Pose3d kCam5Pose = new Pose3d( - new Translation3d(-0.229, -0.073, 0.934), - new Rotation3d(0, Units.degreesToRadians(-10.0), Units.degreesToRadians(180.0))); + new Translation3d(-0.229, 0.073, 0.934), + new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(180))); // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is // in the form [theta], with units in radians. public static Matrix kLocalMeasurementStdDevs = diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index c17fa192..4777cbfe 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -1,5 +1,12 @@ package frc.robot.subsystems.tagAlign; +import java.util.Set; + +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -10,12 +17,7 @@ import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; -import java.util.Set; import net.jafama.FastMath; -import org.littletonrobotics.junction.Logger; -import org.slf4j.LoggerFactory; -import org.strykeforce.telemetry.measurable.MeasurableSubsystem; -import org.strykeforce.telemetry.measurable.Measure; public class TagAlignSubsystem extends MeasurableSubsystem { private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class); @@ -77,19 +79,19 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu Logger.recordOutput("TagAlignSubsystem/Hexant", -1); - // driveRadius = 1.253823; - // for (int i = 0; i < 6; i++) { - // logger.info("Hexant {}, left and right", i); - - // logger.info( - // "{}, {}", - // getTargetDrivePose(Alliance.Blue, true, i).getX(), - // getTargetDrivePose(Alliance.Blue, true, i).getY()); - // logger.info( - // "{}, {}", - // getTargetDrivePose(Alliance.Blue, false, i).getX(), - // getTargetDrivePose(Alliance.Blue, false, i).getY()); - // } + driveRadius = 1.223823; + for (int i = 0; i < 6; i++) { + logger.info("Hexant {}, left and right", i); + + logger.info( + "{}, {}", + getTargetDrivePose(Alliance.Blue, true, i).getX(), + getTargetDrivePose(Alliance.Blue, true, i).getY()); + logger.info( + "{}, {}", + getTargetDrivePose(Alliance.Blue, false, i).getX(), + getTargetDrivePose(Alliance.Blue, false, i).getY()); + } } public void setProceedToAlign(boolean proceed) { @@ -153,7 +155,6 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft, int hexant) ? TagServoingConstants.kBlueReefPose : TagServoingConstants.kRedReefPose; - logger.info("Using {}", driveRadius); Translation2d offset = new Translation2d(driveRadius, Rotation2d.fromDegrees(hexant * 60 + 180)); @@ -219,7 +220,6 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); Logger.recordOutput("TagAlignSubsystem/GettingAlgae", algae); - driveX.reset(); driveY.reset(); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); From c2c377468e12f1205093426f56748e8631168133 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 2 Mar 2025 17:17:14 -0500 Subject: [PATCH 43/48] fix up so files build --- .../frc/robot/constants/ClimbConstants.java | 28 ++-- .../frc/robot/subsystems/climb/ClimbIO.java | 17 ++- .../subsystems/climb/ClimbIOServoFX.java | 56 ++++---- .../subsystems/climb/ClimbSubsystem.java | 120 ++++++------------ 4 files changed, 92 insertions(+), 129 deletions(-) diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 0985d8b3..38f672c7 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -1,7 +1,6 @@ package frc.robot.constants; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.DegreesPerSecond; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; @@ -17,25 +16,33 @@ import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; import edu.wpi.first.units.measure.Angle; -//import edu.wpi.first.units.measure.AngularVelocity; +// import edu.wpi.first.units.measure.AngularVelocity; public class ClimbConstants { public static int kPivotArmFrontFxId = 45; - public static int kPivotArmFollowFxId = 46; - public static int kCANcoderId = 47; - public static int kDeployServoId = 1; //the servo to release the pin - public static int kRatchetServoId = 2; + // public static int kPivotArmFollowFxId = 46; + public static int kCANcoderId = 46; + public static int kDeployServoId = 1; // the servo to release the pin + public static int kRatchetServoId = 2; public static int kCageAlignedDIOId = 11; - public static final Angle kPivotArmCloseEnough = Degrees.of(5.0); //all of these arbitrary numbers + public static final Angle kPivotArmCloseEnough = + Degrees.of(5.0); // all of these arbitrary numbers public static final Angle kArmMaxFwd = Degrees.of(100); public static final Angle kArmMaxRev = Degrees.of(-100); public static final Angle kArmZeroTicks = Degrees.of(1530); - public static final Angle kPinDeployedPosition = Degrees.of(500); - public static final Angle kPinRetractedPosition = Degrees.of(0); - //Climb positions + // Deploy Servo + public static final double kPinDeployedPosition = 1.0; + public static final double kPinRetractedPosition = 0.0; + + // Ratchet Servo + public static final double kRatchetEngatedPos = 1.0; + public static final double kRatchetDisengagedPos = 0.0; + + // Climb positions public static final Angle kClimbCagePos = Degrees.of(3); + public static TalonFXConfiguration getPivotArmFxConfig() { TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); @@ -109,5 +116,4 @@ public static CurrentLimitsConfigs getRunCurrentLimit() { CurrentLimitsConfigs config = new CurrentLimitsConfigs(); return config; } - } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index 04671880..7172b014 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -1,10 +1,11 @@ package frc.robot.subsystems.climb; + import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; public interface ClimbIO { @@ -17,18 +18,20 @@ static class ClimbIOInputs { } public default void setPosition(Angle position) {} - - public default void setRatchetServoPosition(double position) {} //needed? since it's double, not angle - - public default void setPinServoPosition(double position) {} //may make a method to stick these both into one method + + public default void setRatchetServoPosition( + double position) {} // needed? since it's double, not angle + + public default void setPinServoPosition( + double position) {} // may make a method to stick these both into one method public default void updateInputs(ClimbIOInputs inputs) {} - public default void zero() {} + public default void zero() {} public default void registerWith(TelemetryService telemetryService) {} public default void setSoftLimitsEnabled(boolean enable) {} public default void setCurrentLimit(CurrentLimitsConfigs config) {} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java index ab56e7ae..a10f961c 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java @@ -8,16 +8,15 @@ import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.controls.Follower; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Servo; import frc.robot.constants.ClimbConstants; +import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; import org.slf4j.Logger; import org.slf4j.LoggerFactory; -import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; public class ClimbIOServoFX implements ClimbIO { - //will prob need healthchecks eventually + // will prob need healthchecks eventually private Logger logger; private TalonFX talonFxPivotArmFront; private TalonFX talonFxPivotArmBack; @@ -32,30 +31,28 @@ public class ClimbIOServoFX implements ClimbIO { TalonFXConfigurator configuratorBack; private MotionMagicDutyCycle positionRequestMain = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); - private Follower positionRequestFollower = - new Follower(ClimbConstants.kPivotArmFollowFxId, true); - + StatusSignal currPosition; public ClimbIOServoFX() { logger = LoggerFactory.getLogger(this.getClass()); talonFxPivotArmFront = new TalonFX(ClimbConstants.kPivotArmFrontFxId); - talonFxPivotArmBack = new TalonFX(ClimbConstants.kPivotArmFollowFxId); ratchetServo = new Servo(ClimbConstants.kRatchetServoId); pinServo = new Servo(ClimbConstants.kDeployServoId); - absPivotArmFrontSensorInitial = talonFxPivotArmFront.getPosition().getValue(); //only for logging - configuratorFront = talonFxPivotArmFront.getConfigurator(); + absPivotArmFrontSensorInitial = + talonFxPivotArmFront.getPosition().getValue(); // only for logging + configuratorFront = talonFxPivotArmFront.getConfigurator(); configuratorBack = talonFxPivotArmBack.getConfigurator(); - configuratorFront.apply(new TalonFXConfiguration()); + configuratorFront.apply(new TalonFXConfiguration()); configuratorFront.apply(ClimbConstants.getPivotArmFxConfig()); - configuratorBack.apply(new TalonFXConfiguration()); + configuratorBack.apply(new TalonFXConfiguration()); configuratorBack.apply(ClimbConstants.getPivotArmFxConfig()); currPosition = talonFxPivotArmFront.getPosition(); } - //@Override + // @Override public String getName() { return "Climb"; } @@ -67,22 +64,21 @@ public void setPosition(Angle position) { logger.info("Setting position to {} rotations", pivotArmSetpoint.in(Rotations)); talonFxPivotArmFront.setControl(positionRequestMain.withPosition(pivotArmSetpoint)); - talonFxPivotArmBack.setControl(positionRequestFollower); } @Override - public void setRatchetServoPosition(double position) { - //ratchetServo.setPosition(position); - ratchetServo.set(position); - //could be wrong but I'm assuming set rather than setPosition - } + public void setRatchetServoPosition(double position) { + // ratchetServo.setPosition(position); + ratchetServo.set(position); + // could be wrong but I'm assuming set rather than setPosition + } @Override - public void setPinServoPosition(double position) { - //pinServo.setPosition(position); - pinServo.set(position); - } - + public void setPinServoPosition(double position) { + // pinServo.setPosition(position); + pinServo.set(position); + } + @Override public void updateInputs(ClimbIOInputs inputs) { inputs.position = currPosition.refresh().getValue(); @@ -93,7 +89,7 @@ public void updateInputs(ClimbIOInputs inputs) { @Override public void zero() { relSetpointOffset = ClimbConstants.kArmZeroTicks; - /* + /* logger.info( "Abs: {}, Zero Pos: {}, Offset: {}" absPivotArmFrontSensorInitial, @@ -109,10 +105,13 @@ public void zero() { @Override public void setSoftLimitsEnabled(boolean enable) { - configuratorFront.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable) - .withReverseSoftLimitEnable()); //fixme - //configuratorBack.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable) - //.withReverseSoftLimitEnable()); + configuratorFront.apply( + ClimbConstants.getPivotArmFxConfig() + .SoftwareLimitSwitch + .withForwardSoftLimitEnable(enable) + .withReverseSoftLimitEnable(enable)); // fixme + // configuratorBack.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable) + // .withReverseSoftLimitEnable()); } @Override @@ -122,5 +121,4 @@ public void setCurrentLimit(CurrentLimitsConfigs config) { } public void goToZero() {} - } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 2d10f5f2..2076f653 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -1,36 +1,24 @@ -//still have a non-negligible amount of stuff to do on the state machine - +// still have a non-negligible amount of stuff to do on the state machine package frc.robot.subsystems.climb; import static edu.wpi.first.units.Units.Rotations; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.ClimbConstants; import frc.robot.standards.ClosedLoopPosSubsystem; import frc.robot.subsystems.climb.ClimbSubsystem.ClimbState; - import java.util.Set; -//import org.littletonrobotics.junction.Logger; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; +import org.littletonrobotics.junction.Logger; import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem { - private final ClimbIO climbIo; - private final ClimbIO pivotArmIo; //will probably not need all these individual ios - private final ClimbIO ratchetIo; - private final ClimbIO pinIo; - - private final ClimbIOInputsAutoLogged climbInputs = new ClimbIOInputsAutoLogged(); - private final ClimbIOInputsAutoLogged pivotArmInputs = new ClimbIOInputsAutoLogged(); - private final ClimbIOInputsAutoLogged ratchetInputs = new ClimbIOInputsAutoLogged(); - private final ClimbIOInputsAutoLogged pinInputs = new ClimbIOInputsAutoLogged(); + private final ClimbIO io; - private Logger logger = LoggerFactory.getLogger(this.getClass()); + private ClimbIOInputsAutoLogged climbInputs = new ClimbIOInputsAutoLogged(); private boolean isRotatingToPosition = false; private boolean isClimbZeroed = false; @@ -39,21 +27,17 @@ public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPos private boolean isBeamBroken = false; private boolean isPinDeployed = false; private boolean proceedToClimb = false; - private int climbZeroStableCounts; //idk what this means - //more + private int climbZeroStableCounts; // idk what this means + // more private Angle setpoints = Rotations.of(0.0); private ClimbState curState = ClimbState.INIT; private Timer hangTimer = new Timer(); - public ClimbSubsystem(ClimbIO climbIo, ClimbIO pivotArmIo, ClimbIO ratchetIo, ClimbIO pinIo) { - this.climbIo = climbIo; - this.pivotArmIo = pivotArmIo; - this.ratchetIo = ratchetIo; - this.pinIo = pinIo; - + public ClimbSubsystem(ClimbIO io) { + this.io = io; enableRatchet(false); - + zero(); } @Override @@ -63,9 +47,8 @@ public Angle getPosition() { @Override public void setPosition(Angle position) { - climbIo.setPosition(position); + io.setPosition(position); setpoints = position; - } @Override @@ -76,25 +59,25 @@ public boolean isFinished() { @Override public void zero() { - climbIo.zero(); + io.zero(); + } - curState = ClimbState.ZEROED; + private void setClimbDebugMsg(String msg) { + Logger.recordOutput("Climb/DebugMsg", msg); } public void toggleClimbRatchet() { - if (isRatchetOn) enableRatchet(false); - else enableRatchet(true); + enableRatchet(!isRatchetOn); } public void enableRatchet(boolean enable) { if (enable) { - ratchetIo.setPosition(); //fixme - logger.info("Engaging Ratchet"); + io.setRatchetServoPosition(ClimbConstants.kRatchetEngatedPos); // fixme + setClimbDebugMsg("Engaging Ratchet"); isRatchetOn = true; - } - else { - ratchetIo.setPosition(); //fixme - logger.info("Disengaging Ratchet"); + } else { + io.setRatchetServoPosition(ClimbConstants.kRatchetDisengagedPos); + setClimbDebugMsg("Disengaging Ratchet"); isRatchetOn = false; } } @@ -113,75 +96,52 @@ public void climb() {} public void deployPin(boolean enable) { if (enable) { - pinIo.setPosition(); //fixme - logger.info("Deploying Pin"); + io.setPinServoPosition(ClimbConstants.kPinDeployedPosition); + setClimbDebugMsg("Deploying Pin"); isPinDeployed = true; - } - else { - pinIo.setPosition(); //fixme - logger.info("Retracting Pin"); + } else { + io.setPinServoPosition(ClimbConstants.kPinRetractedPosition); + setClimbDebugMsg("Retracting Pin"); isPinDeployed = true; } - } - public void togglePin() { - if (isPinDeployed) deployPin(false); - else enableRatchet(true); + public void toggleDeployState() { + deployPin(!isPinDeployed); } - public boolean isClimbFinished() {} + public boolean isClimbFinished() { + return curState == ClimbState.CLIMBED; + } public void descend() {} - public void stow() { - - } + public void stow() {} @Override public void periodic() { // Read Inputs - climbIo.updateInputs(climbInputs); - pivotArmIo.updateInputs(pivotArmInputs); - pinIo.updateInputs(pinInputs); - ratchetIo.updateInputs(ratchetInputs); + io.updateInputs(climbInputs); Logger.processInputs(getName(), climbInputs); - //climbInputs.ratchetPosition = ratchetServo.getPosition(); + // climbInputs.ratchetPosition = ratchetServo.getPosition(); // State Machine switch (curState) { case INIT: break; - case ZEROING: - if () { - - } - else { - - } - break; default: break; - case ZEROED: - if (proceedToClimb) { - - } - else - - } // Log Outputs - Logger.recordOutput("Coral/curState", curState.ordinal()); //works not - Logger.recordOutput("Coral/setpoint", setpoints.in(Rotations)); + Logger.recordOutput("Climb/curState", curState.ordinal()); // works not + Logger.recordOutput("Climb/setpoint", setpoints.in(Rotations)); } @Override public void registerWith(TelemetryService telemetryService) { super.registerWith(telemetryService); - pivotArmIo.registerWith(telemetryService); - pinIo.registerWith(telemetryService); - ratchetIo.registerWith(telemetryService); + io.registerWith(telemetryService); } @Override @@ -192,13 +152,9 @@ public Set getMeasures() { public enum ClimbState { INIT, STOWED, - ZEROING, - ZEROED, PREP_CLIMBING, PREPPED, CLIMBING, - CLIMBED, - DESCENDING, - DOWN + CLIMBED } } From 958bf0209871e88bf4d1c4a9fe7b8cde16ecf412 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 2 Mar 2025 18:42:14 -0500 Subject: [PATCH 44/48] add in initial climb test --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++- .../frc/robot/constants/ClimbConstants.java | 38 ++++++++----- .../robot/subsystems/biscuit/BiscuitIOFX.java | 4 +- .../frc/robot/subsystems/climb/ClimbIO.java | 8 ++- .../subsystems/climb/ClimbIOServoFX.java | 35 +++++++++--- .../subsystems/climb/ClimbSubsystem.java | 55 ++++++++++++++----- .../subsystems/drive/DriveSubsystem.java | 9 +++ .../frc/robot/subsystems/drive/Swerve.java | 13 +++++ .../frc/robot/subsystems/drive/SwerveIO.java | 2 + .../robotState/RobotStateSubsystem.java | 6 +- .../tagAlign/TagAlignSubsystem.java | 12 ++-- 11 files changed, 154 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95bb7359..f5b73fb8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,6 +60,8 @@ import frc.robot.subsystems.battMon.BattMonSubsystem; import frc.robot.subsystems.biscuit.BiscuitIOFX; import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.climb.ClimbIO; +import frc.robot.subsystems.climb.ClimbIOServoFX; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.coral.CoralIO; import frc.robot.subsystems.coral.CoralIOFX; @@ -93,6 +95,7 @@ public class RobotContainer { private final BiscuitIOFX biscuitIO; private final BiscuitSubsystem biscuitSubsystem; + private final ClimbIO climbIO; private final ClimbSubsystem climbSubsystem; private final CoralIO coralIO; @@ -132,7 +135,8 @@ public RobotContainer() { biscuitIO = new BiscuitIOFX(); biscuitSubsystem = new BiscuitSubsystem(biscuitIO); - climbSubsystem = new ClimbSubsystem(); + climbIO = new ClimbIOServoFX(); + climbSubsystem = new ClimbSubsystem(climbIO); coralIO = new CoralIOFX(); coralSubsystem = new CoralSubsystem(coralIO); @@ -186,6 +190,7 @@ private void configureTelemetry() { funnelSubsystem.registerWith(telemetryService); biscuitSubsystem.registerWith(telemetryService); ledSubsystem.registerWith(telemetryService); + climbSubsystem.registerWith(telemetryService); telemetryService.start(); } @@ -264,6 +269,10 @@ private void configureDriverBindings() { .onFalse( new FloorAlgaeCommand( robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); + + // climb + new JoystickButton(driveJoystick, Button.SWA.id) + .onTrue(new InstantCommand(() -> robotStateSubsystem.toClimb())); } private void configureOperatorBindings() { @@ -311,6 +320,10 @@ private void configureOperatorBindings() { .onTrue(new SetScoreSideRightCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) .onTrue(new setScoreSideLeftCommand(robotStateSubsystem)); + + // Prep Climb + new JoystickButton(xboxController, XboxController.Button.kStart.value) + .onTrue(new InstantCommand(() -> robotStateSubsystem.toPrepClimb())); } private void configureTestOperatorBindings() { @@ -473,6 +486,15 @@ public void configurePitDashboard() { .withPosition(1, 0) .withSize(1, 1); + Shuffleboard.getTab("Pit") + .add("Prep Climb", new InstantCommand(() -> robotStateSubsystem.toPrepClimb())) + .withPosition(3, 0) + .withSize(1, 1); + Shuffleboard.getTab("Pit") + .add("Climb", new InstantCommand(() -> robotStateSubsystem.toClimb())) + .withPosition(4, 0) + .withSize(1, 1); + // Shuffleboard.getTab("Pit") // .add( // "Start Auton", diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 38f672c7..62786f43 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -1,14 +1,17 @@ package frc.robot.constants; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -26,22 +29,24 @@ public class ClimbConstants { public static int kRatchetServoId = 2; public static int kCageAlignedDIOId = 11; - public static final Angle kPivotArmCloseEnough = - Degrees.of(5.0); // all of these arbitrary numbers - public static final Angle kArmMaxFwd = Degrees.of(100); - public static final Angle kArmMaxRev = Degrees.of(-100); + public static final double kPivotArmCloseEnough = 0.01; // FIXME + public static final double kArmMaxFwd = 0.260; + public static final double kArmMaxRev = 0.03; public static final Angle kArmZeroTicks = Degrees.of(1530); // Deploy Servo - public static final double kPinDeployedPosition = 1.0; - public static final double kPinRetractedPosition = 0.0; + public static final double kPinDeployedPosition = 0.75; + public static final double kPinRetractedPosition = 0.18; // Ratchet Servo - public static final double kRatchetEngatedPos = 1.0; - public static final double kRatchetDisengagedPos = 0.0; + public static final double kRatchetEngagedPos = 0.0; + public static final double kRatchetDisengagedPos = 1.0; // Climb positions - public static final Angle kClimbCagePos = Degrees.of(3); + public static final Angle kClimbCagePos = Rotations.of(4); // fixme + public static final Double kClimbRatchedEngage = 0.1; + public static final double kFullyClimbed = 0.260; + public static final double kClimbOpenLoopSpeed = 4.0; public static TalonFXConfiguration getPivotArmFxConfig() { TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); @@ -50,10 +55,9 @@ public static TalonFXConfiguration getPivotArmFxConfig() { new CurrentLimitsConfigs() .withStatorCurrentLimit(10) .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) - .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimit(30) + .withSupplyCurrentLowerLimit(30) + .withSupplyCurrentLowerTime(1) .withSupplyCurrentLimitEnable(true); armFxConfig.CurrentLimits = current; @@ -101,9 +105,15 @@ public static TalonFXConfiguration getPivotArmFxConfig() { MotorOutputConfigs motorOut = new MotorOutputConfigs() .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); + .withNeutralMode(NeutralModeValue.Brake); armFxConfig.MotorOutput = motorOut; + FeedbackConfigs feedbackConfigs = + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) + .withFeedbackRemoteSensorID(kCANcoderId); + armFxConfig.Feedback = feedbackConfigs; + return armFxConfig; } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index a46bfc27..3fb167ea 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -55,9 +55,9 @@ public BiscuitIOFX() { velocity = talon.getVelocity(); position = talon.getPosition(); rawQuadrature = talon.getRawQuadraturePosition(); - rawQuadrature.setUpdateFrequency(200); + rawQuadrature.setUpdateFrequency(20); rawPulseWidth = talon.getRawPulseWidthPosition(); - rawPulseWidth.setUpdateFrequency(20); + rawPulseWidth.setUpdateFrequency(200); zero(); } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index 7172b014..129e1caf 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.climb; -import static edu.wpi.first.units.Units.Rotations; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import edu.wpi.first.units.measure.Angle; import org.littletonrobotics.junction.AutoLog; @@ -11,7 +9,7 @@ public interface ClimbIO { @AutoLog static class ClimbIOInputs { - public Angle position = Rotations.of(0.0); + public double position = 0.0; public double velocity = 0.0; public double ratchetServoPosition = 0.0; public double pinServoPosition = 0.0; @@ -34,4 +32,8 @@ public default void registerWith(TelemetryService telemetryService) {} public default void setSoftLimitsEnabled(boolean enable) {} public default void setCurrentLimit(CurrentLimitsConfigs config) {} + + public default void setPercent(double percent) {} + + public default void setCoastMode(boolean coast) {} } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java index a10f961c..571044c1 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java @@ -7,19 +7,23 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Servo; import frc.robot.constants.ClimbConstants; import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; public class ClimbIOServoFX implements ClimbIO { // will prob need healthchecks eventually private Logger logger; private TalonFX talonFxPivotArmFront; - private TalonFX talonFxPivotArmBack; + private CANcoder canCoder; private Servo ratchetServo; private Servo pinServo; @@ -28,9 +32,9 @@ public class ClimbIOServoFX implements ClimbIO { private Angle pivotArmSetpoint; TalonFXConfigurator configuratorFront; - TalonFXConfigurator configuratorBack; private MotionMagicDutyCycle positionRequestMain = new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); + private VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false); StatusSignal currPosition; @@ -38,16 +42,14 @@ public ClimbIOServoFX() { logger = LoggerFactory.getLogger(this.getClass()); talonFxPivotArmFront = new TalonFX(ClimbConstants.kPivotArmFrontFxId); ratchetServo = new Servo(ClimbConstants.kRatchetServoId); + canCoder = new CANcoder(ClimbConstants.kCANcoderId); pinServo = new Servo(ClimbConstants.kDeployServoId); absPivotArmFrontSensorInitial = talonFxPivotArmFront.getPosition().getValue(); // only for logging configuratorFront = talonFxPivotArmFront.getConfigurator(); - configuratorBack = talonFxPivotArmBack.getConfigurator(); configuratorFront.apply(new TalonFXConfiguration()); configuratorFront.apply(ClimbConstants.getPivotArmFxConfig()); - configuratorBack.apply(new TalonFXConfiguration()); - configuratorBack.apply(ClimbConstants.getPivotArmFxConfig()); currPosition = talonFxPivotArmFront.getPosition(); } @@ -81,7 +83,7 @@ public void setPinServoPosition(double position) { @Override public void updateInputs(ClimbIOInputs inputs) { - inputs.position = currPosition.refresh().getValue(); + inputs.position = currPosition.refresh().getValueAsDouble(); inputs.ratchetServoPosition = ratchetServo.getPosition(); inputs.pinServoPosition = pinServo.getPosition(); } @@ -99,7 +101,6 @@ public void zero() { ); */ - talonFxPivotArmBack.setPosition(0.0); talonFxPivotArmFront.setPosition(0.0); } @@ -117,8 +118,26 @@ public void setSoftLimitsEnabled(boolean enable) { @Override public void setCurrentLimit(CurrentLimitsConfigs config) { configuratorFront.apply(config); - configuratorBack.apply(config); + } + + @Override + public void setPercent(double percent) { + talonFxPivotArmFront.setControl(voltageRequest.withOutput(percent)); + } + + @Override + public void setCoastMode(boolean coast) { + configuratorFront.apply( + ClimbConstants.getPivotArmFxConfig() + .MotorOutput + .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake)); } public void goToZero() {} + + @Override + public void registerWith(TelemetryService telemetryService) { + telemetryService.register(talonFxPivotArmFront, false); + telemetryService.register(canCoder); + } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 2076f653..35c8083a 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -30,31 +30,31 @@ public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPos private int climbZeroStableCounts; // idk what this means // more - private Angle setpoints = Rotations.of(0.0); + private double setpoints = 0.0; private ClimbState curState = ClimbState.INIT; private Timer hangTimer = new Timer(); public ClimbSubsystem(ClimbIO io) { this.io = io; enableRatchet(false); + deployClimb(false); zero(); } @Override public Angle getPosition() { - return climbInputs.position; + return Rotations.of(climbInputs.position); } @Override public void setPosition(Angle position) { io.setPosition(position); - setpoints = position; + setpoints = position.in(Rotations); } @Override public boolean isFinished() { - return setpoints.minus(climbInputs.position).abs(Rotations) - <= ClimbConstants.kPivotArmCloseEnough.in(Rotations); + return (Math.abs(setpoints - climbInputs.position) < ClimbConstants.kPivotArmCloseEnough); } @Override @@ -72,7 +72,7 @@ public void toggleClimbRatchet() { public void enableRatchet(boolean enable) { if (enable) { - io.setRatchetServoPosition(ClimbConstants.kRatchetEngatedPos); // fixme + io.setRatchetServoPosition(ClimbConstants.kRatchetEngagedPos); // fixme setClimbDebugMsg("Engaging Ratchet"); isRatchetOn = true; } else { @@ -82,7 +82,7 @@ public void enableRatchet(boolean enable) { } } - public Angle getSetpoints() { + public double getSetpoints() { return setpoints; } @@ -90,11 +90,21 @@ public ClimbState getState() { return curState; } - public void prepClimb() {} + public void prepClimb() { + enableRatchet(false); + deployClimb(true); + setState(ClimbState.PREPPED); + } - public void climb() {} + public void climb() { + if (curState == ClimbState.PREPPED) { + // setPosition(ClimbConstants.kClimbCagePos); + io.setPercent(ClimbConstants.kClimbOpenLoopSpeed); + setState(ClimbState.CLIMBING); + } + } - public void deployPin(boolean enable) { + private void deployClimb(boolean enable) { if (enable) { io.setPinServoPosition(ClimbConstants.kPinDeployedPosition); setClimbDebugMsg("Deploying Pin"); @@ -107,17 +117,21 @@ public void deployPin(boolean enable) { } public void toggleDeployState() { - deployPin(!isPinDeployed); + deployClimb(!isPinDeployed); } public boolean isClimbFinished() { return curState == ClimbState.CLIMBED; } - public void descend() {} - public void stow() {} + private void setState(ClimbState state) { + setClimbDebugMsg(curState + "->" + state); + setpoints = ClimbConstants.kFullyClimbed; + curState = state; + } + @Override public void periodic() { // Read Inputs @@ -129,13 +143,24 @@ public void periodic() { switch (curState) { case INIT: break; + case PREPPED: + break; + case CLIMBING: + if (climbInputs.position >= ClimbConstants.kClimbRatchedEngage && !isRatchetOn) { + enableRatchet(true); + } + if (isFinished()) { + io.setCoastMode(true); + setState(ClimbState.CLIMBED); + } + break; default: break; } // Log Outputs - Logger.recordOutput("Climb/curState", curState.ordinal()); // works not - Logger.recordOutput("Climb/setpoint", setpoints.in(Rotations)); + Logger.recordOutput("Climb/curState", curState); + Logger.recordOutput("Climb/setpoint", setpoints); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 25940553..dbec072a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -279,6 +279,15 @@ public void setEnableHolo(boolean enabled) { logger.info("Holonomic Controller Enabled: {}", enabled); } + public void prepClimb() { + io.setDriveCoast(true); + io.setSwerveModuleAngles( + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90), + Rotation2d.fromDegrees(90)); + } + public void teleResetGyro() { logger.info("Driver Joystick: Reset Gyro"); double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0; diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index 1635424e..ddf9d27d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -209,6 +210,18 @@ public void setBothGyroOffset(Rotation2d rotation) { navxOffset = rotation; } + @Override + public void setDriveCoast(boolean coast) { + for (int i = 0; i < 4; i++) { + drives[i] + .getConfigurator() + .apply( + DriveConstants.getDriveTalonConfig() + .MotorOutput + .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake)); + } + } + @Override public void resetGyro() { swerveDrive.resetGyro(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index c555ff59..f6456ff6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -66,6 +66,8 @@ public default SwerveDriveKinematics getKinematics() { return null; } + public default void setDriveCoast(boolean coast) {} + public default void setOdometry(OdometryStrategy Odom) {} public default void setPigeonGyroOffset(Rotation2d rotation) {} diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index 7e3c3ce1..74e98eaf 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -559,16 +559,18 @@ public void toPrepClimb() { } climbSubsystem.prepClimb(); + driveSubsystem.prepClimb(); - setState(RobotStates.PREP_CLIMB, true); + setState(RobotStates.PREP_CLIMB, false); } public void toClimb() { if (curState == RobotStates.PREP_CLIMB) { climbSubsystem.climb(); + driveSubsystem.prepClimb(); - setState(RobotStates.CLIMB, true); + setState(RobotStates.CLIMB, false); } } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 4777cbfe..d06e29cf 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -1,12 +1,5 @@ package frc.robot.subsystems.tagAlign; -import java.util.Set; - -import org.littletonrobotics.junction.Logger; -import org.slf4j.LoggerFactory; -import org.strykeforce.telemetry.measurable.MeasurableSubsystem; -import org.strykeforce.telemetry.measurable.Measure; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -17,7 +10,12 @@ import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.Set; import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; public class TagAlignSubsystem extends MeasurableSubsystem { private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class); From 580deb6510c6a9eb5e205ac9d6ca8a329dffe9a3 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Sun, 2 Mar 2025 20:20:12 -0500 Subject: [PATCH 45/48] constants tweak --- src/main/java/frc/robot/constants/ClimbConstants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 62786f43..1d5e1f8f 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -30,8 +30,8 @@ public class ClimbConstants { public static int kCageAlignedDIOId = 11; public static final double kPivotArmCloseEnough = 0.01; // FIXME - public static final double kArmMaxFwd = 0.260; - public static final double kArmMaxRev = 0.03; + public static final double kArmMaxFwd = 0.12; + public static final double kArmMaxRev = -0.14; public static final Angle kArmZeroTicks = Degrees.of(1530); // Deploy Servo @@ -44,8 +44,8 @@ public class ClimbConstants { // Climb positions public static final Angle kClimbCagePos = Rotations.of(4); // fixme - public static final Double kClimbRatchedEngage = 0.1; - public static final double kFullyClimbed = 0.260; + public static final Double kClimbRatchedEngage = 0.0; + public static final double kFullyClimbed = kArmMaxFwd; public static final double kClimbOpenLoopSpeed = 4.0; public static TalonFXConfiguration getPivotArmFxConfig() { From 3f5bd10ab74883c247a8436ab234d99e841c9af7 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Mon, 3 Mar 2025 00:50:47 -0500 Subject: [PATCH 46/48] add zero check to modules --- src/main/java/frc/robot/Robot.java | 4 +++- src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ .../robot/subsystems/drive/DriveSubsystem.java | 8 ++++++++ .../java/frc/robot/subsystems/drive/Swerve.java | 10 ++++++++++ .../frc/robot/subsystems/drive/SwerveFXS.java | 16 ++++++++++++++-- .../frc/robot/subsystems/drive/SwerveIO.java | 3 +++ vendordeps/thirdcoast.json | 4 ++-- 7 files changed, 48 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f60eb8e..27e36de3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -57,7 +57,9 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + if (!m_robotContainer.hasSwerveZeroed()) m_robotContainer.zeroSwerve(); + } @Override public void disabledExit() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5b73fb8..3129e8e9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -194,6 +194,14 @@ private void configureTelemetry() { telemetryService.start(); } + public boolean hasSwerveZeroed() { + return driveSubsystem.hasZeroed(); + } + + public void zeroSwerve() { + driveSubsystem.zeroModules(); + } + private void configureDriverBindings() { driveSubsystem.setDefaultCommand( new DriveTeleopCommand( diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index dbec072a..cd7e1a00 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -83,6 +83,14 @@ public DriveSubsystem(SwerveIO io) { holonomicController.setEnabled(true); } + public boolean hasZeroed() { + return inputs.didZero; + } + + public void zeroModules() { + io.zeroModules(); + } + // Open-Loop Swerve Movements public void drive(double vXmps, double vYmps, double vOmegaRadps) { if (!ignoreSticks) { diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java index ddf9d27d..eded81d2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -53,6 +53,7 @@ public class Swerve implements SwerveIO, Checkable { private SwerveDriveKinematics kinematics; private double fieldY = 0.0; private double fieldX = 0.0; + private boolean didZero = false; public Swerve() { @@ -94,6 +95,7 @@ public Swerve() { .build(); swerveModules[i].loadAndSetAzimuthZeroReference(); } + didZero = true; pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "rio"); pigeon.applyConfig(DriveConstants.getPigeon2Configuration()); @@ -270,6 +272,13 @@ public void configDriveCurrents(CurrentLimitsConfigs config) { } } + @Override + public void zeroModules() { + for (int i = 0; i < 4; i++) { + swerveModules[i].loadAndSetAzimuthZeroReference(); + } + } + @Override public void updateInputs(SwerveIOInputs inputs) { swerveDrive.updateInputs(); // Call before swerveDrive.periodic() @@ -299,6 +308,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; inputs.fieldX = fieldX; + inputs.didZero = didZero; } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java index b1be18e4..0582d2c6 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java @@ -53,6 +53,7 @@ public class SwerveFXS implements SwerveIO, Checkable { private SwerveDriveKinematics kinematics; private double fieldY = 0.0; private double fieldX = 0.0; + private boolean didZero = false; public SwerveFXS() { @@ -65,7 +66,7 @@ public SwerveFXS() { .encoderOpposed(false); swerveModules = new FXSwerveModule[4]; Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters(); - + didZero = true; for (int i = 0; i < 4; i++) { var azimuthFXS = new TalonFXS(i, "*"); configuratorFXS = azimuthFXS.getConfigurator(); @@ -89,7 +90,8 @@ public SwerveFXS() { .wheelLocationMeters(wheelLocations[i]) .closedLoopUnits(ClosedLoopUnits.VOLTAGE) .build(); - swerveModules[i].loadAndSetAzimuthZeroReference(); + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; } pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*"); @@ -120,6 +122,15 @@ public String getName() { return "Swerve"; } + @Override + public void zeroModules() { + didZero = true; + for (int i = 0; i < 4; i++) { + boolean zeroCheck = swerveModules[i].zeroAndCheck(); + didZero = zeroCheck && didZero; + } + } + private SwerveModule[] getSwerveModules() { return swerveDrive.getSwerveModules(); } @@ -284,6 +295,7 @@ public void updateInputs(SwerveIOInputs inputs) { inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed); inputs.fieldY = fieldY; inputs.fieldX = fieldX; + inputs.didZero = didZero; } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java index f6456ff6..0bc6bfce 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java @@ -41,6 +41,7 @@ public static class SwerveIOInputs { public double[] azimuthCurrent = {0, 0, 0, 0}; public double avgDriveCurrent = 0; public double avgRearDriveVel = 0; + public boolean didZero = false; } private SwerveModule[] getSwerveModules() { @@ -101,4 +102,6 @@ public default void configDriveCurrents(CurrentLimitsConfigs config) {} public default BooleanSupplier getAzimuth1FwdLimitSwitch() { return () -> false; } + + public default void zeroModules() {} } diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index d9f6ff20..9f049a00 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.6", + "version": "25.0.7", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.6" + "version": "25.0.7" } ], "jniDependencies": [], From a5a42f7f723c16f5f37dfef94216b8e6516c9489 Mon Sep 17 00:00:00 2001 From: KaydenLee456 Date: Mon, 3 Mar 2025 19:48:39 -0500 Subject: [PATCH 47/48] Changed a few constants and stuff after testing --- .../java/frc/robot/commands/climb/Climb.java | 20 +++++++++++++++++++ .../frc/robot/commands/climb/PrepClimb.java | 20 +++++++++++++++++++ .../frc/robot/constants/ClimbConstants.java | 12 +++++------ .../subsystems/climb/ClimbSubsystem.java | 1 + 4 files changed, 47 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/commands/climb/Climb.java create mode 100644 src/main/java/frc/robot/commands/climb/PrepClimb.java diff --git a/src/main/java/frc/robot/commands/climb/Climb.java b/src/main/java/frc/robot/commands/climb/Climb.java new file mode 100644 index 00000000..08c2a4c4 --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/Climb.java @@ -0,0 +1,20 @@ +package frc.robot.commands.climb; + +import frc.robot.subsystems.climb.ClimbSubsystem; + +public class Climb { + + ClimbSubsystem climbSubsystem; + /* + @Override + public void initialize() { + + } + + @Override + public void isFinished() { + + } + */ + +} diff --git a/src/main/java/frc/robot/commands/climb/PrepClimb.java b/src/main/java/frc/robot/commands/climb/PrepClimb.java new file mode 100644 index 00000000..2f9fba93 --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/PrepClimb.java @@ -0,0 +1,20 @@ +package frc.robot.commands.climb; + +import frc.robot.subsystems.climb.ClimbSubsystem; + +public class PrepClimb { + + ClimbSubsystem climbSubsystem; + /* + @Override + public void initialize() { + + } + + @Override + public void isFinished() { + + } + */ + +} diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 1d5e1f8f..51db5235 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -25,13 +25,13 @@ public class ClimbConstants { public static int kPivotArmFrontFxId = 45; // public static int kPivotArmFollowFxId = 46; public static int kCANcoderId = 46; - public static int kDeployServoId = 1; // the servo to release the pin + public static int kDeployServoId = 1; public static int kRatchetServoId = 2; public static int kCageAlignedDIOId = 11; - public static final double kPivotArmCloseEnough = 0.01; // FIXME - public static final double kArmMaxFwd = 0.12; - public static final double kArmMaxRev = -0.14; + public static final double kPivotArmCloseEnough = 0.01; // fixme + public static final double kArmMaxFwd = 0; + public static final double kArmMaxRev = -0.29; public static final Angle kArmZeroTicks = Degrees.of(1530); // Deploy Servo @@ -43,8 +43,8 @@ public class ClimbConstants { public static final double kRatchetDisengagedPos = 1.0; // Climb positions - public static final Angle kClimbCagePos = Rotations.of(4); // fixme - public static final Double kClimbRatchedEngage = 0.0; + //public static final Angle kClimbCagePos = Rotations.of(); // will test + public static final Double kClimbRatchedEngage = -0.12; public static final double kFullyClimbed = kArmMaxFwd; public static final double kClimbOpenLoopSpeed = 4.0; diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 35c8083a..8c6e4ec4 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -152,6 +152,7 @@ public void periodic() { if (isFinished()) { io.setCoastMode(true); setState(ClimbState.CLIMBED); + io.setPercent(0.0); } break; default: From 783f95d4163883f4c32f2f49d73bae705873e963 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Mon, 3 Mar 2025 23:02:48 -0500 Subject: [PATCH 48/48] spotless --- src/main/java/frc/robot/constants/ClimbConstants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 51db5235..8255b126 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -1,7 +1,6 @@ package frc.robot.constants; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -43,7 +42,7 @@ public class ClimbConstants { public static final double kRatchetDisengagedPos = 1.0; // Climb positions - //public static final Angle kClimbCagePos = Rotations.of(); // will test + // public static final Angle kClimbCagePos = Rotations.of(); // will test public static final Double kClimbRatchedEngage = -0.12; public static final double kFullyClimbed = kArmMaxFwd; public static final double kClimbOpenLoopSpeed = 4.0;