From beb0c522792b96fabd80b6defd683ee762ed80c8 Mon Sep 17 00:00:00 2001 From: sneha Date: Sun, 7 Jan 2024 15:46:05 -0800 Subject: [PATCH 01/19] added code for pivot, kicker, and shooter --- .../frc/robot/subsystems/kicker/KickerIO.java | 27 +++++++++ .../robot/subsystems/kicker/KickerIOReal.java | 46 +++++++++++++++ .../subsystems/kicker/KickerSubsystem.java | 43 ++++++++++++++ .../frc/robot/subsystems/pivot/PivotIO.java | 27 +++++++++ .../robot/subsystems/pivot/PivotIOReal.java | 57 +++++++++++++++++++ .../robot/subsystems/pivot/PivotIOSim.java | 55 ++++++++++++++++++ .../subsystems/pivot/PivotSubsystem.java | 45 +++++++++++++++ .../robot/subsystems/shooter/ShooterIO.java | 25 ++++++++ .../subsystems/shooter/ShooterIOReal.java | 41 +++++++++++++ .../subsystems/shooter/ShooterSubsystem.java | 41 +++++++++++++ 10 files changed, 407 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/kicker/KickerIO.java create mode 100644 src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIO.java create mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java new file mode 100644 index 00000000..1009bf90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.kicker; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface KickerIO { + @AutoLog + public class KickerIOInputs { + + // Motor values + public double velocityRPM; + public double currentDrawAmps; + public double temperatureCelsius; + public double motorOutputVolts; + public double positionRotations; + } + + public abstract void setPosition(double degrees); + + public abstract void reset(double degrees); + + public abstract KickerIOInputsAutoLogged updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java new file mode 100644 index 00000000..c77f361e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.kicker; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + +public class KickerIOReal { + + private TalonFX kickerMotor = new TalonFX(Constants.KICKER_MOTOR_ID); + private PositionVoltage motorRequest = new PositionVoltage(Constants.KICKER_MOTOR_ID); + + private StatusSignal supplyVoltageSignal = kickerMotor.getDutyCycle(); + private StatusSignal position = kickerMotor.getRotorPosition(); + private StatusSignal velocity = kickerMotor.getRotorVelocity(); + private StatusSignal currentDraw = kickerMotor.getStatorCurrent(); + + public KickerIOReal (){ + TalonFXConfiguration kickerConfig = new TalonFXConfiguration(); + kickerConfig.Slot0.kP = 1; + kickerConfig.Slot0.kD = 0.01; + kickerConfig.Slot0.kI = 0; + } + + public void setPosition(double degrees) { + kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.KICKER_GEAR_RATIO)); + } + + public void reset(double degrees){ + kickerMotor.setPosition((Units.degreesToRotations(degrees))*Constants.KICKER_GEAR_RATIO); + } + + public KickerIOInputsAutoLogged updateInputs() { + KickerIOInputsAutoLogged current = new KickerIOInputsAutoLogged(); + + current.currentDrawAmps = currentDraw.refresh().getValue(); + current.positionRotations = position.refresh().getValue() / Constants.KICKER_GEAR_RATIO; + current.velocityRPM = velocity.refresh().getValue() / Constants.KICKER_GEAR_RATIO; + current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + + return(current); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java new file mode 100644 index 00000000..7fd0b1d5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.kicker; + + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class KickerSubsystem extends SubsystemBase { + KickerIO io; + KickerIOInputsAutoLogged inputs; + + + public KickerSubsystem(KickerIO io) { + this.io = io; + inputs = new KickerIOInputsAutoLogged(); + + SmartDashboard.putData("reset Kicker value", new InstantCommand( + ()->{io.reset(0);} + )); + } + + public InstantCommand reset (){ + return new InstantCommand( + ()->{io.reset(0);} + ); + } + + public RunCommand run(double degrees) { + return new RunCommand(() -> { + io.setPosition(degrees); + }, this); + } + + @Override + public void periodic() { + inputs = io.updateInputs(); + Logger.processInputs("Kicker", inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java new file mode 100644 index 00000000..fef806c2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface PivotIO { + @AutoLog + public class PivotIOInputs { + + // Motor values + public double velocityRPM; + public double currentDrawAmps; + public double temperatureCelsius; + public double motorOutputVolts; + public double positionRotations; + } + + public abstract void setPosition(double degrees); + + public abstract void reset(double degrees); + + public abstract PivotIOInputsAutoLogged updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java new file mode 100644 index 00000000..0d03ebac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.pivot; +import com.ctre.phoenix6.StatusSignal; + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + + + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + +public class PivotIOReal implements PivotIO{ + + private TalonFX pivotMotor = new TalonFX(Constants.PIVOT_MOTOR_ID); + private PositionVoltage motorRequest = new PositionVoltage(Constants.PIVOT_MOTOR_ID); + + private StatusSignal supplyVoltageSignal = pivotMotor.getDutyCycle(); + private StatusSignal position = pivotMotor.getRotorPosition(); + private StatusSignal velocity = pivotMotor.getRotorVelocity(); + private StatusSignal currentDraw = pivotMotor.getStatorCurrent(); + + + public PivotIOReal (){ + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotConfig.Slot0.kP = 1; + pivotConfig.Slot0.kD = 0.01; + pivotConfig.Slot0.kI = 0; + } + + @Override + public void setPosition(double degrees) { + pivotMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.PIVOT_GEAR_RATIO)); + } + + @Override + public void reset(double degrees){ + pivotMotor.setPosition((Units.degreesToRotations(degrees))*Constants.PIVOT_GEAR_RATIO); + } + + + @Override + public PivotIOInputsAutoLogged updateInputs() { + PivotIOInputsAutoLogged current = new PivotIOInputsAutoLogged(); + + current.currentDrawAmps = currentDraw.refresh().getValue(); + current.positionRotations = position.refresh().getValue() / Constants.PIVOT_GEAR_RATIO; + current.velocityRPM = velocity.refresh().getValue() / Constants.PIVOT_GEAR_RATIO; + current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + + return(current); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java new file mode 100644 index 00000000..6816511c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +/** Add your docs here. */ +public class PivotIOSim implements PivotIO { + + SingleJointedArmSim arm = + new SingleJointedArmSim( + DCMotor.getFalcon500(1), + 27 * (24 / 18), + SingleJointedArmSim.estimateMOI(Units.inchesToMeters(30), 3), + Units.inchesToMeters(30), + 0, + 3.5, + true, + 0); + PIDController pid = new PIDController(1, 0, 0); + + public PivotIOInputsAutoLogged updateInputs() { + + PivotIOInputsAutoLogged input = new PivotIOInputsAutoLogged(); + + // var motorSimState = motor.getSimState(); + + input.velocityRPM = arm.getVelocityRadPerSec() * 60; + input.currentDrawAmps = arm.getCurrentDrawAmps(); + input.temperatureCelsius = 0; + input.motorOutputVolts = arm.getOutput(0); + input.positionRotations = Units.radiansToDegrees(arm.getAngleRads()); + + arm.update(0.020); + + return input; + } + + @Override + public void setPosition(double degrees) { + double outputVolts = pid.calculate(Units.radiansToDegrees(arm.getAngleRads()), degrees); + arm.setInput(outputVolts); + } + + @Override + public void reset(double degrees) { + // TODO Auto-generated method stub + + } +} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java new file mode 100644 index 00000000..ddda259b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.pivot; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Add your docs here. */ +public class PivotSubsystem extends SubsystemBase { + PivotIO io; + PivotIOInputsAutoLogged inputs; + + public PivotSubsystem(PivotIO io) { + this.io = io; + inputs = new PivotIOInputsAutoLogged(); + + SmartDashboard.putData("reset Pivot value", new InstantCommand( + ()->{io.reset(0);} + )); + } + + public InstantCommand reset (){ + return new InstantCommand( + ()->{io.reset(0);} + ); + } + + public RunCommand run(double degrees) { + return new RunCommand(() -> { + io.setPosition(degrees); + }, this); + } + + @Override + public void periodic() { + inputs = io.updateInputs(); + Logger.processInputs("Pivot", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 00000000..ad4a7b28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +/** Add your docs here. */ +public interface ShooterIO { + @AutoLog + public class ShooterIOInputs { + + // Motor values + public double velocityRPM; + public double currentDrawAmps; + public double temperatureCelsius; + public double motorOutputVolts; } + + public abstract void setVelocity(double velocity); + + public abstract void reset(double velocity); + + public abstract ShooterIOInputsAutoLogged updateInputs(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java new file mode 100644 index 00000000..2602db85 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import frc.robot.Constants; + +public class ShooterIOReal { + + private TalonFX bottomShooterMotor = new TalonFX(Constants.BOTTOM_SHOOTER_MOTOR_ID); + private VelocityVoltage bottomShooterMotorVelocity = new VelocityVoltage(0); + + + private StatusSignal supplyVoltageSignal = bottomShooterMotor.getDutyCycle(); + private StatusSignal velocity = bottomShooterMotor.getRotorVelocity(); + private StatusSignal currentDraw = bottomShooterMotor.getStatorCurrent(); + + public ShooterIOReal(){ + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotConfig.Slot0.kP = 1; + pivotConfig.Slot0.kD = 0.01; + pivotConfig.Slot0.kI = 0; + } + + public void setVelocity(double velocity) { + bottomShooterMotor.setControl(bottomShooterMotorVelocity.withVelocity(velocity)); + } + + + public ShooterIOInputsAutoLogged updateInputs() { + ShooterIOInputsAutoLogged current = new ShooterIOInputsAutoLogged(); + + current.currentDrawAmps = currentDraw.refresh().getValue(); + current.velocityRPM = velocity.refresh().getValue() / Constants.BOTTOM_SHOOTER_GEAR_RATIO; + current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + + return(current); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 00000000..5880f11a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase{ + + ShooterIO io; + ShooterIOInputsAutoLogged inputs; + + public ShooterSubsystem(ShooterIO io) { + this.io = io; + inputs = new ShooterIOInputsAutoLogged(); + + SmartDashboard.putData("reset Shooter value", new InstantCommand( + ()->{io.reset(0);} + )); + } + + public InstantCommand reset (){ + return new InstantCommand( + ()->{io.reset(0);} + ); + } + + public RunCommand run(double velocity) { + return new RunCommand(() -> { + io.setVelocity(velocity); + }, this); + } + + @Override + public void periodic() { + inputs = io.updateInputs(); + Logger.processInputs("Shooter", inputs); + } +} \ No newline at end of file From 25519fdcdd173ab41d8a6eccc46ecc41ac66c3c9 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 9 Jan 2024 14:36:36 -0800 Subject: [PATCH 02/19] transfered module constants from offseason robot --- .../java/frc/robot/subsystems/swerve/Module.java | 6 +++--- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 12 ++++++------ 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index 98219268..e5e1c5cc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -28,11 +28,11 @@ public record ModuleConstants( public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); public static final double ODOMETRY_FREQUENCY_HZ = 250.0; - // Gear ratios for SDS MK4i L2, adjust as necessary + // Gear ratios for SDS MK3 Fast, adjust as necessary // These numbers are taken from SDS's website // They are the gear tooth counts for each stage of the modules' gearboxes - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + public static final double DRIVE_GEAR_RATIO = (48.0 / 16.0) * (16.0 / 28.0) * (60.0 / 15.0); + public static final double TURN_GEAR_RATIO = 12.8 / 1.0; public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed public static final double TURN_STATOR_CURRENT_LIMIT = 40.0; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 7109411a..a1d17e2e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -37,8 +37,8 @@ public class SwerveSubsystem extends SubsystemBase { // Drivebase constants public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(20.5); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.5); public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; @@ -46,13 +46,13 @@ public class SwerveSubsystem extends SubsystemBase { public static final int PigeonID = 0; public static final ModuleConstants frontLeft = - new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Left", 6, 5, 21, Rotation2d.fromRotations(0.735107)); public static final ModuleConstants frontRight = - new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Front Right", 8, 7, 23, Rotation2d.fromRotations(0.076172)); public static final ModuleConstants backLeft = - new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Left", 4, 3, 20, Rotation2d.fromRotations(0.85)); public static final ModuleConstants backRight = - new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0)); + new ModuleConstants("Back Right", 2, 1, 11, Rotation2d.fromRotations(0.245361)); public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; From ef0885229f2bcb8723702aea0718c86781b99b41 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 9 Jan 2024 18:15:44 -0800 Subject: [PATCH 03/19] bumped versions and fixed configs w real robot --- build.gradle | 160 +++++++++--------- src/main/java/frc/robot/Robot.java | 9 +- .../frc/robot/subsystems/swerve/GyroIO.java | 2 + .../subsystems/swerve/GyroIOPigeon2.java | 10 +- .../subsystems/swerve/ModuleIOTalonFX.java | 35 ++-- .../subsystems/swerve/SwerveSubsystem.java | 14 +- vendordeps/AdvantageKit.json | 10 +- vendordeps/Phoenix6.json | 50 +++--- 8 files changed, 154 insertions(+), 136 deletions(-) diff --git a/build.gradle b/build.gradle index db5948dd..fdafc12e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" + id "edu.wpi.first.GradleRIO" version "2024.1.1" id "com.peterabeles.gversion" version "1.10" - id 'com.diffplug.spotless' version '6.12.0' + // id "com.diffplug.spotless" version "6.12.0" } java { @@ -49,8 +49,30 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true +// Configuration for AdvantageKit +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 5. +// Also defines JUnit 4. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -69,9 +91,10 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' - testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' - testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + implementation 'gov.nist.math:jama:1.0.3' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" @@ -79,23 +102,13 @@ dependencies { implementation("com.google.guava:guava:33.0.0-jre") } -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/Los_Angeles" // Use preferred time zone - indent = " " -} - test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true +wpi.sim.addGui() wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') @@ -118,67 +131,56 @@ tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } -repositories { - maven { - url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") - credentials { - username = "Mechanical-Advantage-Bot" - password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" - } - } - mavenLocal() -} - -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" - classpath = sourceSets.main.runtimeClasspath +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " } -compileJava.finalizedBy checkAkitInstall -spotless { - enforceCheck false - java { - target fileTree('.') { - include '**/*.java' - exclude '**/build/**', '**/build-*/**', '**/BuildConstants.java' - } - toggleOffOn() - googleJavaFormat() - removeUnusedImports() - trimTrailingWhitespace() - endWithNewline() - } - groovyGradle { - target fileTree('.') { - include '**/*.gradle' - exclude '**/build/**', '**/build-*/**' - } - greclipse() - indentWithSpaces(4) - trimTrailingWhitespace() - endWithNewline() - } - format 'xml', { - target fileTree('.') { - include '**/*.xml' - exclude '**/build/**', '**/build-*/**' - } - eclipseWtp('xml') - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } - format 'misc', { - target fileTree('.') { - include '**/*.md', '**/.gitignore' - exclude '**/build/**', '**/build-*/**' - } - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } -} +// Spotless formatting +// project.compileJava.dependsOn(spotlessApply) +// spotless { +// enforceCheck false +// java { +// target fileTree(".") { +// include "**/*.java" +// exclude "**/build/**", "**/build-*/**" +// } +// toggleOffOn() +// googleJavaFormat() +// removeUnusedImports() +// trimTrailingWhitespace() +// endWithNewline() +// } +// groovyGradle { +// target fileTree(".") { +// include "**/*.gradle" +// exclude "**/build/**", "**/build-*/**" +// } +// greclipse() +// indentWithSpaces(4) +// trimTrailingWhitespace() +// endWithNewline() +// } +// json { +// target fileTree(".") { +// include "**/*.json" +// exclude "**/build/**", "**/build-*/**" +// } +// gson().indentWithSpaces(2) +// } +// format "misc", { +// target fileTree(".") { +// include "**/*.md", "**/.gitignore" +// exclude "**/build/**", "**/build-*/**" +// } +// trimTrailingWhitespace() +// indentWithSpaces(2) +// endWithNewline() +// } +// } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2381f406..d0eca0f7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; @@ -62,7 +63,7 @@ public void robotInit() { switch (mode) { case REAL: - Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick + // Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging break; @@ -89,9 +90,11 @@ public void robotInit() { swerve.runVelocityFieldRelative( () -> new ChassisSpeeds( - -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, - -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, + controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, + controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); + + controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java index f114108d..42198db4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIO.java @@ -26,4 +26,6 @@ public static class GyroIOInputs { } public default void updateInputs(GyroIOInputs inputs) {} + + public default void setYaw(Rotation2d yaw) {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java index 084a61ac..1d91209e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java @@ -24,13 +24,14 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(20); + private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); public GyroIOPigeon2() { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); + var config = new Pigeon2Configuration(); + pigeon.getConfigurator().apply(config); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY_HZ); yawVelocity.setUpdateFrequency(100.0); @@ -50,4 +51,9 @@ public void updateInputs(GyroIOInputs inputs) { .toArray(Rotation2d[]::new); yawPositionQueue.clear(); } + + @Override + public void setYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index 4af58c16..6f3efbcc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -44,7 +45,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { // Constants - private static final boolean IS_TURN_MOTOR_INVERTED = true; + private static final boolean IS_TURN_MOTOR_INVERTED = false; private final String name; @@ -71,8 +72,8 @@ public class ModuleIOTalonFX implements ModuleIO { private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true); private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage drivePIDF = new VelocityVoltage(0.0).withEnableFOC(true); - private final MotionMagicExpoVoltage turnMotionMagic = - new MotionMagicExpoVoltage(0.0).withEnableFOC(true); + private final PositionVoltage turnPID = + new PositionVoltage(0.0).withEnableFOC(true); public ModuleIOTalonFX(ModuleConstants constants) { name = constants.prefix(); @@ -92,14 +93,14 @@ public ModuleIOTalonFX(ModuleConstants constants) { // Sensor // Meters per second driveConfig.Feedback.SensorToMechanismRatio = - (1.0 / Module.DRIVE_GEAR_RATIO) * Module.WHEEL_RADIUS * 2 * Math.PI; + (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI)); // Controls Gains - driveConfig.Slot0.kV = - (5800.0 * (1.0 / Module.DRIVE_GEAR_RATIO) * Module.WHEEL_RADIUS * 2 * Math.PI) - / 12.0; // Hypothetical based on free speed + driveConfig.Slot0.kV = 2.5; + // (5800.0 * (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI))) + // / 12.0; // Hypothetical based on free speed driveConfig.Slot0.kA = 0.0; // TODO: Find using sysid or hand tuning driveConfig.Slot0.kS = 0.0; - driveConfig.Slot0.kP = 0.0; + driveConfig.Slot0.kP = 0.25; // Guess driveConfig.Slot0.kD = 0.0; driveTalon.getConfigurator().apply(driveConfig); @@ -122,18 +123,18 @@ public ModuleIOTalonFX(ModuleConstants constants) { turnConfig.Feedback.FeedbackRotorOffset = 0.0; // Is this correct? Cancoder config should handle it // Controls Gains - turnConfig.Slot0.kV = - (5800.0 / Module.TURN_GEAR_RATIO) - / 12.0; // Free speed over voltage, should find empirically - turnConfig.Slot0.kA = - Module.TURN_GEAR_RATIO - * (9.37 / 483.0) - / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life + turnConfig.Slot0.kV = 0.0; + // (5800.0 / Module.TURN_GEAR_RATIO) + // / 12.0; // Free speed over voltage, should find empirically + turnConfig.Slot0.kA = 0.0; + // Module.TURN_GEAR_RATIO + // * (9.37 / 483.0) + // / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life // gearing * Kt (torque per amp) / (intertia * resistance (nominal voltage / stall current)) // Yeah its messy and should be found using sysid later but its worth trying as a first guess // If this works we can use a similar technique on future mechanisms turnConfig.Slot0.kS = 0.0; // TODO: Find empirically - turnConfig.Slot0.kP = 0.0; + turnConfig.Slot0.kP = 50.0; turnConfig.Slot0.kD = 0.0; turnConfig.ClosedLoopGeneral.ContinuousWrap = true; @@ -225,7 +226,7 @@ public void setDriveSetpoint(final double metersPerSecond) { @Override public void setTurnSetpoint(final Rotation2d rotation) { - turnTalon.setControl(turnMotionMagic.withPosition(rotation.getRotations())); + turnTalon.setControl(turnPID.withPosition(rotation.getRotations())); } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index a1d17e2e..b3e546dc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -43,16 +43,16 @@ public class SwerveSubsystem extends SubsystemBase { Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; // Hardware constants - public static final int PigeonID = 0; + public static final int PIGEON_ID = 0; public static final ModuleConstants frontLeft = - new ModuleConstants("Front Left", 6, 5, 21, Rotation2d.fromRotations(0.735107)); + new ModuleConstants("Front Left", 6, 5, 21, Rotation2d.fromRotations(0.125732)); public static final ModuleConstants frontRight = - new ModuleConstants("Front Right", 8, 7, 23, Rotation2d.fromRotations(0.076172)); + new ModuleConstants("Front Right", 8, 7, 23, Rotation2d.fromRotations(0.461426)); public static final ModuleConstants backLeft = - new ModuleConstants("Back Left", 4, 3, 20, Rotation2d.fromRotations(0.85)); + new ModuleConstants("Back Left", 4, 3, 20, Rotation2d.fromRotations(0.152344)); public static final ModuleConstants backRight = - new ModuleConstants("Back Right", 2, 1, 11, Rotation2d.fromRotations(0.245361)); + new ModuleConstants("Back Right", 2, 1, 22, Rotation2d.fromRotations(-0.238281)); public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -255,6 +255,10 @@ public void setPose(Pose2d pose) { this.pose = pose; } + public void setYaw(Rotation2d yaw) { + gyroIO.setYaw(yaw); + } + /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { return new Translation2d[] { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 114ee222..cca06ae6 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.0-beta-6", + "version": "3.0.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.0-beta-6" + "version": "3.0.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.0-beta-6" + "version": "3.0.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.0-beta-6" + "version": "3.0.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.0-beta-6", + "version": "3.0.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 7ea073a5..69a40798 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-5", + "version": "24.1.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-5" + "version": "24.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-5", + "version": "24.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-5", + "version": "24.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, From be684fdd3e36a47c498de509d60160b23225970e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 9 Jan 2024 18:47:35 -0800 Subject: [PATCH 04/19] added shooter --- src/main/java/frc/robot/Robot.java | 8 ++++++++ .../frc/robot/subsystems/kicker/KickerIOReal.java | 13 ++++++------- .../frc/robot/subsystems/pivot/PivotIOReal.java | 13 ++++++------- .../frc/robot/subsystems/shooter/ShooterIO.java | 2 -- .../robot/subsystems/shooter/ShooterIOReal.java | 14 ++++++-------- .../robot/subsystems/shooter/ShooterSubsystem.java | 10 ---------- 6 files changed, 26 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d0eca0f7..b6d0d496 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.shooter.ShooterIOReal; +import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.GyroIO; import frc.robot.subsystems.swerve.GyroIOPigeon2; import frc.robot.subsystems.swerve.SwerveSubsystem; @@ -40,6 +42,7 @@ public static enum RobotMode { mode == RobotMode.REAL ? SwerveSubsystem.createTalonFXModules() : SwerveSubsystem.createSimModules()); + private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOReal()); @Override public void robotInit() { @@ -94,7 +97,12 @@ public void robotInit() { controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); + shooter.setDefaultCommand(shooter.run(0.0)); + controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); + + controller.leftTrigger().whileTrue(shooter.run(5.0)); + controller.rightTrigger().whileTrue(shooter.run(-10.0)); } @Override diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java index c77f361e..19a00d1f 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java @@ -6,12 +6,11 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; public class KickerIOReal { - private TalonFX kickerMotor = new TalonFX(Constants.KICKER_MOTOR_ID); - private PositionVoltage motorRequest = new PositionVoltage(Constants.KICKER_MOTOR_ID); + private TalonFX kickerMotor = new TalonFX(12); + private PositionVoltage motorRequest = new PositionVoltage(0.0); private StatusSignal supplyVoltageSignal = kickerMotor.getDutyCycle(); private StatusSignal position = kickerMotor.getRotorPosition(); @@ -26,19 +25,19 @@ public KickerIOReal (){ } public void setPosition(double degrees) { - kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.KICKER_GEAR_RATIO)); + kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); } public void reset(double degrees){ - kickerMotor.setPosition((Units.degreesToRotations(degrees))*Constants.KICKER_GEAR_RATIO); + kickerMotor.setPosition((Units.degreesToRotations(degrees))); } public KickerIOInputsAutoLogged updateInputs() { KickerIOInputsAutoLogged current = new KickerIOInputsAutoLogged(); current.currentDrawAmps = currentDraw.refresh().getValue(); - current.positionRotations = position.refresh().getValue() / Constants.KICKER_GEAR_RATIO; - current.velocityRPM = velocity.refresh().getValue() / Constants.KICKER_GEAR_RATIO; + current.positionRotations = position.refresh().getValue(); + current.velocityRPM = velocity.refresh().getValue(); current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); return(current); diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index 0d03ebac..43340925 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -12,12 +12,11 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; public class PivotIOReal implements PivotIO{ - private TalonFX pivotMotor = new TalonFX(Constants.PIVOT_MOTOR_ID); - private PositionVoltage motorRequest = new PositionVoltage(Constants.PIVOT_MOTOR_ID); + private TalonFX pivotMotor = new TalonFX(10); + private PositionVoltage motorRequest = new PositionVoltage(0.0); private StatusSignal supplyVoltageSignal = pivotMotor.getDutyCycle(); private StatusSignal position = pivotMotor.getRotorPosition(); @@ -34,12 +33,12 @@ public PivotIOReal (){ @Override public void setPosition(double degrees) { - pivotMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees)*Constants.PIVOT_GEAR_RATIO)); + pivotMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); } @Override public void reset(double degrees){ - pivotMotor.setPosition((Units.degreesToRotations(degrees))*Constants.PIVOT_GEAR_RATIO); + pivotMotor.setPosition((Units.degreesToRotations(degrees))); } @@ -48,8 +47,8 @@ public PivotIOInputsAutoLogged updateInputs() { PivotIOInputsAutoLogged current = new PivotIOInputsAutoLogged(); current.currentDrawAmps = currentDraw.refresh().getValue(); - current.positionRotations = position.refresh().getValue() / Constants.PIVOT_GEAR_RATIO; - current.velocityRPM = velocity.refresh().getValue() / Constants.PIVOT_GEAR_RATIO; + current.positionRotations = position.refresh().getValue(); + current.velocityRPM = velocity.refresh().getValue(); current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); return(current); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index ad4a7b28..7247743c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -19,7 +19,5 @@ public class ShooterIOInputs { public abstract void setVelocity(double velocity); - public abstract void reset(double velocity); - public abstract ShooterIOInputsAutoLogged updateInputs(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 2602db85..3bb93d8c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -3,15 +3,14 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import frc.robot.Constants; - -public class ShooterIOReal { +public class ShooterIOReal implements ShooterIO { - private TalonFX bottomShooterMotor = new TalonFX(Constants.BOTTOM_SHOOTER_MOTOR_ID); + private TalonFX bottomShooterMotor = new TalonFX(27); private VelocityVoltage bottomShooterMotorVelocity = new VelocityVoltage(0); - + private VoltageOut voltageOut = new VoltageOut(0.0); private StatusSignal supplyVoltageSignal = bottomShooterMotor.getDutyCycle(); private StatusSignal velocity = bottomShooterMotor.getRotorVelocity(); @@ -25,15 +24,14 @@ public ShooterIOReal(){ } public void setVelocity(double velocity) { - bottomShooterMotor.setControl(bottomShooterMotorVelocity.withVelocity(velocity)); + bottomShooterMotor.setControl(voltageOut.withOutput(velocity)); } - public ShooterIOInputsAutoLogged updateInputs() { ShooterIOInputsAutoLogged current = new ShooterIOInputsAutoLogged(); current.currentDrawAmps = currentDraw.refresh().getValue(); - current.velocityRPM = velocity.refresh().getValue() / Constants.BOTTOM_SHOOTER_GEAR_RATIO; + current.velocityRPM = velocity.refresh().getValue(); current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); return(current); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5880f11a..760bba42 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -15,18 +15,8 @@ public class ShooterSubsystem extends SubsystemBase{ public ShooterSubsystem(ShooterIO io) { this.io = io; inputs = new ShooterIOInputsAutoLogged(); - - SmartDashboard.putData("reset Shooter value", new InstantCommand( - ()->{io.reset(0);} - )); } - public InstantCommand reset (){ - return new InstantCommand( - ()->{io.reset(0);} - ); - } - public RunCommand run(double velocity) { return new RunCommand(() -> { io.setVelocity(velocity); From 819f257f912240d5d730119aa0a4e5a6a412446c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 9 Jan 2024 19:40:42 -0800 Subject: [PATCH 05/19] added kicker --- src/main/java/frc/robot/Robot.java | 14 ++++++++++++-- .../frc/robot/subsystems/kicker/KickerIOReal.java | 12 ++++++++---- .../frc/robot/subsystems/pivot/PivotIOReal.java | 11 +++++++++-- .../frc/robot/subsystems/pivot/PivotSubsystem.java | 3 ++- .../robot/subsystems/shooter/ShooterSubsystem.java | 3 ++- 5 files changed, 33 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b6d0d496..514e0c7f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,10 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.kicker.KickerIOReal; +import frc.robot.subsystems.kicker.KickerSubsystem; +import frc.robot.subsystems.pivot.PivotIOReal; +import frc.robot.subsystems.pivot.PivotSubsystem; import frc.robot.subsystems.shooter.ShooterIOReal; import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.GyroIO; @@ -43,6 +47,8 @@ public static enum RobotMode { ? SwerveSubsystem.createTalonFXModules() : SwerveSubsystem.createSimModules()); private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOReal()); + private final PivotSubsystem pivot = new PivotSubsystem(new PivotIOReal()); + private final KickerSubsystem kicker = new KickerSubsystem(new KickerIOReal()); @Override public void robotInit() { @@ -98,11 +104,15 @@ public void robotInit() { controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); shooter.setDefaultCommand(shooter.run(0.0)); + pivot.setDefaultCommand(pivot.run(0.0)); + kicker.setDefaultCommand(kicker.run(0.0)); controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); - controller.leftTrigger().whileTrue(shooter.run(5.0)); - controller.rightTrigger().whileTrue(shooter.run(-10.0)); + controller.leftTrigger().whileTrue(Commands.parallel(shooter.run(5.0), pivot.run(100.0))); + controller.rightTrigger().whileTrue(Commands.parallel(shooter.run(-10.0), pivot.run(-15.0), Commands.waitSeconds(0.5).andThen(kicker.run(-25.0)))); + + } @Override diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java index 19a00d1f..217cfe87 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.util.Units; -public class KickerIOReal { +public class KickerIOReal implements KickerIO { private TalonFX kickerMotor = new TalonFX(12); private PositionVoltage motorRequest = new PositionVoltage(0.0); @@ -19,9 +19,13 @@ public class KickerIOReal { public KickerIOReal (){ TalonFXConfiguration kickerConfig = new TalonFXConfiguration(); - kickerConfig.Slot0.kP = 1; - kickerConfig.Slot0.kD = 0.01; - kickerConfig.Slot0.kI = 0; + kickerConfig.Slot0.kP = 10.0; + kickerConfig.Slot0.kD = 0.0; + kickerConfig.Slot0.kI = 0.0; + kickerConfig.CurrentLimits.StatorCurrentLimit = 20.0; + kickerConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kickerConfig.Feedback.SensorToMechanismRatio = 125.0; + kickerMotor.getConfigurator().apply(kickerConfig); } public void setPosition(double degrees) { diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index 43340925..dd2d508c 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -26,9 +26,16 @@ public class PivotIOReal implements PivotIO{ public PivotIOReal (){ TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); - pivotConfig.Slot0.kP = 1; - pivotConfig.Slot0.kD = 0.01; + pivotConfig.Slot0.kP = 120; + pivotConfig.Slot0.kD = 0.0; pivotConfig.Slot0.kI = 0; + + pivotConfig.Feedback.SensorToMechanismRatio = 58.9; + + pivotConfig.CurrentLimits.StatorCurrentLimit = 40.0; + pivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + pivotMotor.getConfigurator().apply(pivotConfig); } @Override diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java index ddda259b..df7eb026 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -7,6 +7,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -31,7 +32,7 @@ public InstantCommand reset (){ ); } - public RunCommand run(double degrees) { + public Command run(double degrees) { return new RunCommand(() -> { io.setPosition(degrees); }, this); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 760bba42..8100e569 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -17,7 +18,7 @@ public ShooterSubsystem(ShooterIO io) { inputs = new ShooterIOInputsAutoLogged(); } - public RunCommand run(double velocity) { + public Command run(double velocity) { return new RunCommand(() -> { io.setVelocity(velocity); }, this); From 8069e75b31ba14c7a0658bc7fbfca07ea95e2080 Mon Sep 17 00:00:00 2001 From: sneha Date: Wed, 10 Jan 2024 00:36:18 -0800 Subject: [PATCH 06/19] edited kicker, shooter, and pivot subsystems --- build.gradle | 158 +++++++++--------- src/main/java/frc/robot/Constants.java | 5 + .../frc/robot/subsystems/kicker/KickerIO.java | 4 +- .../robot/subsystems/kicker/KickerIOReal.java | 21 ++- .../subsystems/kicker/KickerSubsystem.java | 5 +- .../frc/robot/subsystems/pivot/PivotIO.java | 2 +- .../robot/subsystems/pivot/PivotIOReal.java | 18 +- .../robot/subsystems/pivot/PivotIOSim.java | 2 +- .../subsystems/pivot/PivotSubsystem.java | 4 +- .../robot/subsystems/shooter/ShooterIO.java | 4 +- .../subsystems/shooter/ShooterIOReal.java | 34 ++-- .../subsystems/shooter/ShooterSubsystem.java | 2 - .../subsystems/swerve/SwerveSubsystem.java | 3 +- 13 files changed, 138 insertions(+), 124 deletions(-) create mode 100644 src/main/java/frc/robot/Constants.java diff --git a/build.gradle b/build.gradle index fdafc12e..3b1d22dc 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.1.1" id "com.peterabeles.gversion" version "1.10" - // id "com.diffplug.spotless" version "6.12.0" + id 'com.diffplug.spotless' version '6.12.0' } java { @@ -49,30 +49,8 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true -// Configuration for AdvantageKit -repositories { - maven { - url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") - credentials { - username = "Mechanical-Advantage-Bot" - password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" - } - } - mavenLocal() -} - -configurations.all { - exclude group: "edu.wpi.first.wpilibj" -} - -task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { - mainClass = "org.littletonrobotics.junction.CheckInstall" - classpath = sourceSets.main.runtimeClasspath -} -compileJava.finalizedBy checkAkitInstall - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. +// Also defines JUnit 5. dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -91,10 +69,9 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - - implementation 'gov.nist.math:jama:1.0.3' + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" @@ -102,13 +79,23 @@ dependencies { implementation("com.google.guava:guava:33.0.0-jre") } +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Los_Angeles" // Use preferred time zone + indent = " " +} + test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui() +wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') @@ -131,56 +118,67 @@ tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } -// Create version file -project.compileJava.dependsOn(createVersionFile) -gversion { - srcDir = "src/main/java/" - classPackage = "frc.robot" - className = "BuildConstants" - dateFormat = "yyyy-MM-dd HH:mm:ss z" - timeZone = "America/New_York" - indent = " " +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() } -// Spotless formatting -// project.compileJava.dependsOn(spotlessApply) -// spotless { -// enforceCheck false -// java { -// target fileTree(".") { -// include "**/*.java" -// exclude "**/build/**", "**/build-*/**" -// } -// toggleOffOn() -// googleJavaFormat() -// removeUnusedImports() -// trimTrailingWhitespace() -// endWithNewline() -// } -// groovyGradle { -// target fileTree(".") { -// include "**/*.gradle" -// exclude "**/build/**", "**/build-*/**" -// } -// greclipse() -// indentWithSpaces(4) -// trimTrailingWhitespace() -// endWithNewline() -// } -// json { -// target fileTree(".") { -// include "**/*.json" -// exclude "**/build/**", "**/build-*/**" -// } -// gson().indentWithSpaces(2) -// } -// format "misc", { -// target fileTree(".") { -// include "**/*.md", "**/.gitignore" -// exclude "**/build/**", "**/build-*/**" -// } -// trimTrailingWhitespace() -// indentWithSpaces(2) -// endWithNewline() -// } -// } +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + +spotless { + enforceCheck false + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**', '**/BuildConstants.java' + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree('.') { + include '**/*.gradle' + exclude '**/build/**', '**/build-*/**' + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + format 'xml', { + target fileTree('.') { + include '**/*.xml' + exclude '**/build/**', '**/build-*/**' + } + eclipseWtp('xml') + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } + format 'misc', { + target fileTree('.') { + include '**/*.md', '**/.gitignore' + exclude '**/build/**', '**/build-*/**' + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 00000000..d48cb538 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,5 @@ +package frc.robot; + +public class Constants { + +} diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java index 1009bf90..f3f80fd0 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIO.java @@ -6,13 +6,15 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Rotation2d; + /** Add your docs here. */ public interface KickerIO { @AutoLog public class KickerIOInputs { // Motor values - public double velocityRPM; + public double velocityRPS; public double currentDrawAmps; public double temperatureCelsius; public double motorOutputVolts; diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java index 217cfe87..23b8ed71 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java @@ -8,20 +8,23 @@ import edu.wpi.first.math.util.Units; public class KickerIOReal implements KickerIO { - + public static final int KICKER_MOTOR_ID = 12; + private TalonFX kickerMotor = new TalonFX(12); private PositionVoltage motorRequest = new PositionVoltage(0.0); - private StatusSignal supplyVoltageSignal = kickerMotor.getDutyCycle(); + private StatusSignal supplyVoltageSignal = kickerMotor.getMotorVoltage(); private StatusSignal position = kickerMotor.getRotorPosition(); private StatusSignal velocity = kickerMotor.getRotorVelocity(); private StatusSignal currentDraw = kickerMotor.getStatorCurrent(); public KickerIOReal (){ TalonFXConfiguration kickerConfig = new TalonFXConfiguration(); + kickerConfig.Feedback.SensorToMechanismRatio = 125/1; kickerConfig.Slot0.kP = 10.0; kickerConfig.Slot0.kD = 0.0; kickerConfig.Slot0.kI = 0.0; + kickerConfig.CurrentLimits.StatorCurrentLimit = 20.0; kickerConfig.CurrentLimits.StatorCurrentLimitEnable = true; kickerConfig.Feedback.SensorToMechanismRatio = 125.0; @@ -29,7 +32,7 @@ public KickerIOReal (){ } public void setPosition(double degrees) { - kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); + kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); } public void reset(double degrees){ @@ -37,13 +40,13 @@ public void reset(double degrees){ } public KickerIOInputsAutoLogged updateInputs() { - KickerIOInputsAutoLogged current = new KickerIOInputsAutoLogged(); + KickerIOInputsAutoLogged updated = new KickerIOInputsAutoLogged(); //new values - current.currentDrawAmps = currentDraw.refresh().getValue(); - current.positionRotations = position.refresh().getValue(); - current.velocityRPM = velocity.refresh().getValue(); - current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + updated.currentDrawAmps = currentDraw.getValue(); + updated.positionRotations = position.getValue(); + updated.velocityRPS = velocity.getValue(); + updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); - return(current); + return(updated); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java index 7fd0b1d5..9376f22a 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java @@ -4,6 +4,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,8 +24,8 @@ public KickerSubsystem(KickerIO io) { )); } - public InstantCommand reset (){ - return new InstantCommand( + public Command reset (){ + return this.runOnce( ()->{io.reset(0);} ); } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java index fef806c2..f50514e2 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java @@ -12,7 +12,7 @@ public interface PivotIO { public class PivotIOInputs { // Motor values - public double velocityRPM; + public double velocityRPS; public double currentDrawAmps; public double temperatureCelsius; public double motorOutputVolts; diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index dd2d508c..b354f581 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -15,15 +15,17 @@ public class PivotIOReal implements PivotIO{ + public static final int PIVOT_MOTOR_ID = 10; + public static final double PIVOT_GEAR_RATIO = (27.0 / 1) * (48.0 / 22); //check if this is the correct gear ratio + private TalonFX pivotMotor = new TalonFX(10); private PositionVoltage motorRequest = new PositionVoltage(0.0); - private StatusSignal supplyVoltageSignal = pivotMotor.getDutyCycle(); + private StatusSignal supplyVoltageSignal = pivotMotor.getMotorVoltage(); private StatusSignal position = pivotMotor.getRotorPosition(); private StatusSignal velocity = pivotMotor.getRotorVelocity(); private StatusSignal currentDraw = pivotMotor.getStatorCurrent(); - public PivotIOReal (){ TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); pivotConfig.Slot0.kP = 120; @@ -51,13 +53,13 @@ public void reset(double degrees){ @Override public PivotIOInputsAutoLogged updateInputs() { - PivotIOInputsAutoLogged current = new PivotIOInputsAutoLogged(); + PivotIOInputsAutoLogged updated = new PivotIOInputsAutoLogged(); - current.currentDrawAmps = currentDraw.refresh().getValue(); - current.positionRotations = position.refresh().getValue(); - current.velocityRPM = velocity.refresh().getValue(); - current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + updated.currentDrawAmps = currentDraw.getValue(); + updated.positionRotations = position.getValue(); + updated.velocityRPS = velocity.getValue(); + updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); - return(current); + return(updated); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java index 6816511c..4e17ac51 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java @@ -30,7 +30,7 @@ public PivotIOInputsAutoLogged updateInputs() { // var motorSimState = motor.getSimState(); - input.velocityRPM = arm.getVelocityRadPerSec() * 60; + input.velocityRPS = arm.getVelocityRadPerSec() * 60; input.currentDrawAmps = arm.getCurrentDrawAmps(); input.temperatureCelsius = 0; input.motorOutputVolts = arm.getOutput(0); diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java index df7eb026..2202ba34 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotSubsystem.java @@ -26,8 +26,8 @@ public PivotSubsystem(PivotIO io) { )); } - public InstantCommand reset (){ - return new InstantCommand( + public Command reset (){ + return this.runOnce( ()->{io.reset(0);} ); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 7247743c..9fd46988 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -12,12 +12,12 @@ public interface ShooterIO { public class ShooterIOInputs { // Motor values - public double velocityRPM; + public double velocityRPS; public double currentDrawAmps; public double temperatureCelsius; public double motorOutputVolts; } - public abstract void setVelocity(double velocity); + public abstract void setVelocity(double rotationsPerSecond); public abstract ShooterIOInputsAutoLogged updateInputs(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 3bb93d8c..dfee2437 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -7,33 +7,37 @@ import com.ctre.phoenix6.hardware.TalonFX; public class ShooterIOReal implements ShooterIO { + + public static final int SHOOTER_MOTOR_ID = 27; + public static final double SHOOTER_GEAR_RATIO = 1; - private TalonFX bottomShooterMotor = new TalonFX(27); - private VelocityVoltage bottomShooterMotorVelocity = new VelocityVoltage(0); + private TalonFX shooterMotor = new TalonFX(27); + private VelocityVoltage shooterMotorVelocity = new VelocityVoltage(0); private VoltageOut voltageOut = new VoltageOut(0.0); - private StatusSignal supplyVoltageSignal = bottomShooterMotor.getDutyCycle(); - private StatusSignal velocity = bottomShooterMotor.getRotorVelocity(); - private StatusSignal currentDraw = bottomShooterMotor.getStatorCurrent(); + private StatusSignal supplyVoltageSignal = shooterMotor.getMotorVoltage(); + private StatusSignal velocity = shooterMotor.getRotorVelocity(); + private StatusSignal currentDraw = shooterMotor.getStatorCurrent(); public ShooterIOReal(){ - TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); - pivotConfig.Slot0.kP = 1; - pivotConfig.Slot0.kD = 0.01; - pivotConfig.Slot0.kI = 0; + TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); + shooterConfig.Slot0.kP = 0; + shooterConfig.Slot0.kD = 0; + shooterConfig.Slot0.kI = 0; + shooterConfig.Slot0.kV = 0; } public void setVelocity(double velocity) { - bottomShooterMotor.setControl(voltageOut.withOutput(velocity)); + shooterMotor.setControl(voltageOut.withOutput(velocity)); } public ShooterIOInputsAutoLogged updateInputs() { - ShooterIOInputsAutoLogged current = new ShooterIOInputsAutoLogged(); + ShooterIOInputsAutoLogged updated = new ShooterIOInputsAutoLogged(); - current.currentDrawAmps = currentDraw.refresh().getValue(); - current.velocityRPM = velocity.refresh().getValue(); - current.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); + updated.currentDrawAmps = currentDraw.getValue(); + updated.velocityRPS = velocity.getValue(); + updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); - return(current); + return(updated); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 8100e569..048b4304 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -2,9 +2,7 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index b3e546dc..c155bf18 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -123,6 +123,7 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } + // Update odometry int deltaCount = Math.min( @@ -268,4 +269,4 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) }; } -} +} \ No newline at end of file From 0b5dacef5f577d1f302cdc7426429e3d1702dd62 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 10 Jan 2024 14:38:55 -0800 Subject: [PATCH 07/19] sim generated files --- ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat | Bin 0 -> 2048 bytes simgui.json | 7 +++++-- src/main/java/frc/robot/Robot.java | 2 +- 5 files changed, 6 insertions(+), 3 deletions(-) create mode 100644 ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..92f98fe3e03d9703b372abe2f7bff5e9d23e3ce0 GIT binary patch literal 2048 zcmcgtT}abW6u)z`&>{>XX0EwJ3>3s9*b2(gOFk40e2_jY>7hYJjOgnj+JY#HhzS<1 z6=aYt)6#tF$p@jAiAu0V_0Sg)1hF#dV-M@zbIvd4f3ydK?t|x^bAIP{&OPV+=Zvb4 zE0x)%{^Pk3>(comEyYqsZOT#14_}2`yoahwvSFoGh~9Jf)Q4YMpMDU|8EHd+Nc#oG z2U5q#|MCdqtvH00;3BeFsb<65RNC{ZLpy^R59L>?|C7-#eK`<&qp=?GYdjqzO&s(| zRhe5OzQu^7a^;+h36z+?&wi>Ob#4&5wGXv+epkT8GG{sM=eeJkMgBucMMG;EIDcPD zt@NAW*aJ_|=t8G+mukd{$7y_WI>W_g})PHpADF!0frq&a7m<}; z`TeNp$y3y`kl)H{p`qrQZ**Gv=@-D>?G9=!?0@3qT}!Gu=sUIAq2JzgpZ0jDku8bM z3-zJ(XM*Ot!+ASeLvx*7x_^hMCz2VUc)tKp&xr6ed71CE)ZJQyKY#9sZ>{6}=nwdk zsmDVXuj%}&Pt)&}65i+a*=MHSA3n}IE_UQcys6jZLprQb0#8ECX^o{p6wOgQsDbw-A$7Jn+|(bx@S%=~YnXVFagHx@ l4QH=v%vf)#?&Gq?mDrQ+hl7Nh`rh{LZhXQs;O75#zX8Q+RRjP4 literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..81bb04da5a12182051881224b1e7e17b30975346 GIT binary patch literal 2048 zcmcgtOK4M35S=EeD3nq}B=!3&MZt*FA|?uLo^H&dU_hm6De209xCr8>3!zy^t1hG! zl}Z(?V2L%T?NVH1QLu}IrU2-2Cv>$wTn?Mo*!Ks8;lW6X!n8J9_>>Pf^J#07QCwIX*gb zO8Z}(=36J$62B9#kCF-pJxBG$ zHR4-}$kMu^epyyf~jkT$Il5-;(Z%aU%)bm()nu5&tM(U2Z@R!dW_{e6?kN$u!J2Rf{ zyG~@U?$+Nc65bbE3(r-(ze1chBX;BmUe&Ae0gwAg+w^T-21|bMYlriLd;d+}>E-#N zAK-^{a9x76Tl}j)Qz1juw`bMfVm{^bu$^edufE^NgMOKLZq0JPkl@J5aXq)trZe0> zany)ReCNGQ_&7h}tsmHD4*YPM@xUpm>V=MYTX@mm$bD{ehYJ!UGDFg)b|J9Vc}aYb<$PtPq*}G>gU1t_l72{ Jkq!KR_Xo6pSTO(q literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..5087592ae586e4373e89b1bb5378c46ebacf7acc GIT binary patch literal 2048 zcmcgtO-R#W7=PR*6hsV)n3nZVL?IRBkumy2&P zA_Hskp~VDBjQ69D-mmZ6BzC(GRer9^D`ARQ%&2ZfV zr)cy-XJ`+!6k5&btWj4`9`7^HQ=KD?oL}t#j8m?u*#2&uzkRA<1)1)Q@xIhR1?iUr z7<@5B^O%d{diP~@zW{K*(xDXX8Tc6)6gli;yYi)%32u&%Zs#mr@oHYG7m;P3xPIO5 zhmZ9=^Z9LFrFq`Lc zfo=2kWX-7#295mq`>lDjUv{4B6Vflq z;MU!vxkc8ubN|G?T4ekS?`=H5`4#Vc)qQ5*+v6OsaVYUUsu$o}g&MHl#FZXW2Wxr0 zQHxixMh@bpx#qIl)SBt#d_(OYeqJ=T?Qzy^z5(HDtUJN=$sF^t%Wijy>Q}y$@GUW% ac-->)+@CD>Y2@49H@A9+o&L4_fA Date: Wed, 10 Jan 2024 17:25:18 -0800 Subject: [PATCH 08/19] follows path --- .pathplanner/settings.json | 2 +- .vscode/settings.json | 3 +- choreo.chor | 2386 +++++++++++++--- .../deploy/choreo/amp 4 local sgmt 1.traj | 494 ++-- .../deploy/choreo/amp 4 local sgmt 2.traj | 284 +- .../deploy/choreo/amp 4 local sgmt 3.traj | 392 +-- src/main/deploy/choreo/amp 4 local.traj | 2479 ++++++++++++++--- src/main/deploy/choreo/test.traj | 1084 +++++++ .../deploy/pathplanner/autos/local 4.auto | 12 +- .../pathplanner/autos/triangle test.auto | 25 + src/main/java/frc/robot/Robot.java | 33 +- .../robot/subsystems/kicker/KickerIOReal.java | 5 +- .../subsystems/kicker/KickerSubsystem.java | 2 + .../subsystems/shooter/ShooterIOReal.java | 4 + .../frc/robot/subsystems/swerve/Module.java | 5 +- .../subsystems/swerve/ModuleIOTalonFX.java | 6 +- .../subsystems/swerve/SwerveSubsystem.java | 96 +- 17 files changed, 5985 insertions(+), 1327 deletions(-) create mode 100644 src/main/deploy/choreo/test.traj create mode 100644 src/main/deploy/pathplanner/autos/triangle test.auto diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab015..4d7f432f 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,7 +5,7 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/.vscode/settings.json b/.vscode/settings.json index 2f1b9b39..dfce6909 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.compile.nullAnalysis.mode": "automatic" + "java.compile.nullAnalysis.mode": "automatic", + "wpilib.autoStartRioLog": false } diff --git a/choreo.chor b/choreo.chor index eff8a597..fc912d34 100644 --- a/choreo.chor +++ b/choreo.chor @@ -3,9 +3,9 @@ "robotConfiguration": { "mass": 37.04398850154597, "rotationalInertia": 2.8, - "motorMaxTorque": 1.1526315789473685, + "motorMaxTorque": 0.3, "motorMaxVelocity": 4864, - "gearing": 6.86, + "gearing": 8.16, "wheelbase": 0.5206997188221518, "trackWidth": 0.5206997188221518, "bumperLength": 0.8508995405142481, @@ -22,16 +22,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 16 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 }, { - "x": 2.2610747814178467, - "y": 6.9050798416137695, + "x": 2.7185182571411133, + "y": 6.8797807693481445, "heading": 0.15617120010566338, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 13 }, { "x": 1.3818649053573608, @@ -40,16 +49,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 11 }, { - "x": 2.3054046630859375, - "y": 5.538240909576416, + "x": 2.59291672706604, + "y": 5.461223602294922, "heading": -0.020267736871838693, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 10 }, { "x": 1.3892531394958496, @@ -58,21 +67,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 15 }, { - "x": 2.4143476486206055, - "y": 4.51963472366333, + "x": 2.541198492050171, + "y": 4.256927967071533, "heading": -0.6391760227970993, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, + "x": 1.4994455575942993, + "y": 4.286480903625488, + "heading": -0.7254698893067673, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -81,454 +90,760 @@ ], "trajectory": [ { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, - "velocityX": 0, - "velocityY": 0, + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, - "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 }, { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 }, { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 - }, - { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 - }, - { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 - }, - { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 - }, - { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 - }, - { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, - "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 }, { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 }, { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 }, { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 }, { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 }, { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, - "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": -9.422070435540628e-32, - "velocityX": 1.9369819218763315e-32, - "velocityY": -1.162688749272308e-31, - "timestamp": 3.4297275902633078 + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 + }, + { + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 + }, + { + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 + }, + { + "x": 1.4994455567524874, + "y": 4.286480903049883, + "heading": -0.7254698893413497, + "angularVelocity": 3.8000232610425283e-11, + "velocityX": 2.8946950656209713e-9, + "velocityY": -5.415769760526697e-10, + "timestamp": 9.654617059422403 } ], "constraints": [ @@ -537,42 +852,1201 @@ "first" ], "type": "StopPoint", - "uuid": "97aec77a-72c9-452d-b090-247d1b380727" + "uuid": "7bca14ee-a4d0-4baa-be1f-60ad68299d67" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "2e8488aa-d4fb-46f3-93c2-d32a1ab6c3d0" + "uuid": "1379bd6e-531b-4e56-9d4c-3c18bbd998e5" }, { "scope": [ - 2 + 3 ], "type": "StopPoint", - "uuid": "daeb5c61-2b20-430d-951f-f2bdd4f2e4c0" + "uuid": "2791092d-310f-40e0-8550-a29b6dbd16cd" }, { "scope": [ - 4 + 5 ], "type": "StopPoint", - "uuid": "96ea1bb1-66cd-462e-bee5-77ef8d7233f0" + "uuid": "1a44440f-9b6c-4036-96ad-1df3035de21c" }, { "scope": [ - 5 + 6 ], "type": "WptVelocityDirection", - "uuid": "889873f8-2ca0-445b-af07-7765055f2cb4", + "uuid": "b403510d-5383-4de4-b88e-ab6ebe0ef535", "direction": -1 + }, + { + "scope": [ + 1, + 2 + ], + "type": "ZeroAngularVelocity", + "uuid": "19d90aa7-0ff4-4439-9ef6-74adff29337b" } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "test": { + "waypoints": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 8.74417024799509 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "a518fb1e-39f4-45cd-8a81-de94f49f26ef" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "0a3f85ed-c0e4-4302-83c8-516bcd4bb4cb" + } + ], + "usesControlIntervalGuessing": false, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } } } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj index ddf10560..1dbcc825 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 1.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 1.traj @@ -1,175 +1,337 @@ { "samples": [ { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, - "velocityX": 0, - "velocityY": 0, + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, - "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 - }, - { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 + }, + { + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 + }, + { + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 + }, + { + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 + }, + { + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 + }, + { + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 + }, + { + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 + }, + { + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj index 88093dbe..87607fce 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 2.traj @@ -1,103 +1,193 @@ { "samples": [ { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 - }, - { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 - }, - { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 - }, - { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 - }, - { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 - }, - { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, - "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 - }, - { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 - }, - { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 - }, - { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 - }, - { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj index 3c66e297..db814883 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 3.traj @@ -1,175 +1,229 @@ { "samples": [ { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, - "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj index 2864d9ec..d949b53a 100644 --- a/src/main/deploy/choreo/amp 4 local.traj +++ b/src/main/deploy/choreo/amp 4 local.traj @@ -4,451 +4,2170 @@ "x": 0.7470832467079163, "y": 6.625458717346191, "heading": 1.056495497952334, - "angularVelocity": 1.8216849722605286e-31, + "angularVelocity": 0, "velocityX": 0, - "velocityY": 0, + "velocityY": 2.778590006377356e-31, "timestamp": 0 }, { - "x": 0.7875510590073889, - "y": 6.647513625582565, - "heading": 0.9955054911508737, - "angularVelocity": -1.1259767405457384, - "velocityX": 0.7471029727592392, - "velocityY": 0.4071702069138493, - "timestamp": 0.05416631143899068 - }, - { - "x": 0.8691098883744304, - "y": 6.691702168377954, - "heading": 0.8785332104893699, - "angularVelocity": -2.159502420489772, - "velocityX": 1.505711339767041, - "velocityY": 0.8157938323926509, - "timestamp": 0.10833262287798136 - }, - { - "x": 0.9931586796399057, - "y": 6.758536729383142, - "heading": 0.7242571551382772, - "angularVelocity": -2.848192008142514, - "velocityX": 2.290146549949145, - "velocityY": 1.2338769103830776, - "timestamp": 0.16249893431697204 - }, - { - "x": 1.1542844409320208, - "y": 6.847841004747936, - "heading": 0.6582721283096681, - "angularVelocity": -1.2181930996525292, - "velocityX": 2.974648947133801, - "velocityY": 1.6487051267166433, - "timestamp": 0.21666524575596272 - }, - { - "x": 1.3362429767095947, - "y": 6.9407596540671515, - "heading": 0.6582720385538592, - "angularVelocity": -0.0000016570411839532803, - "velocityX": 3.3592565368332274, - "velocityY": 1.7154324680917685, - "timestamp": 0.2708315571949534 - }, - { - "x": 1.5182021069161247, - "y": 7.033677139333042, - "heading": 0.6582719487984567, - "angularVelocity": -0.0000016570336808247504, - "velocityX": 3.359267510978228, - "velocityY": 1.7154109777356332, - "timestamp": 0.3249978686339441 - }, - { - "x": 1.7157951910460691, - "y": 7.085636206134739, - "heading": 0.6582717690726799, - "angularVelocity": -0.0000033180361009936695, - "velocityX": 3.647896245482769, - "velocityY": 0.9592506009979895, - "timestamp": 0.3791641800729348 - }, - { - "x": 1.8967940454930174, - "y": 7.095126843263903, - "heading": 0.5939474144774982, - "angularVelocity": -1.1875343342814295, - "velocityX": 3.3415392268460664, - "velocityY": 0.1752129114394957, - "timestamp": 0.4333304915119255 - }, - { - "x": 2.0516314923077155, - "y": 7.067103472452243, - "heading": 0.46543090394591263, - "angularVelocity": -2.3726280619336353, - "velocityX": 2.8585562262089095, - "velocityY": -0.5173579309202905, - "timestamp": 0.4874968029509162 - }, - { - "x": 2.1749652200442315, - "y": 7.00220729212556, - "heading": 0.30618928904490067, - "angularVelocity": -2.9398644779490204, - "velocityX": 2.2769452905322995, - "velocityY": -1.198091186249191, - "timestamp": 0.5416631143899069 + "x": 0.7490480926070738, + "y": 6.626378384939511, + "heading": 1.0288300799302914, + "angularVelocity": -0.7154916906166144, + "velocityX": 0.05081545896286759, + "velocityY": 0.023784730837073272, + "timestamp": 0.03866630232728455 + }, + { + "x": 0.7533109049583806, + "y": 6.629023111131428, + "heading": 0.9756962016369112, + "angularVelocity": -1.3741649729945935, + "velocityX": 0.11024618581902172, + "velocityY": 0.06839873566218808, + "timestamp": 0.0773326046545691 + }, + { + "x": 0.7612619802970353, + "y": 6.634108872295573, + "heading": 0.9038556504632858, + "angularVelocity": -1.8579627957574294, + "velocityX": 0.2056331963516693, + "velocityY": 0.13152954531570296, + "timestamp": 0.11599890698185367 + }, + { + "x": 0.7743286281835381, + "y": 6.6414096178619975, + "heading": 0.8200810534595926, + "angularVelocity": -2.1666048202532666, + "velocityX": 0.3379337329933604, + "velocityY": 0.18881416445328386, + "timestamp": 0.1546652093091382 + }, + { + "x": 0.7933808300169274, + "y": 6.65119855442881, + "heading": 0.7366646644009647, + "angularVelocity": -2.1573407343826863, + "velocityX": 0.4927340005809824, + "velocityY": 0.25316453805060635, + "timestamp": 0.19333151163642276 + }, + { + "x": 0.8173469969355076, + "y": 6.663336254760928, + "heading": 0.6665724862917816, + "angularVelocity": -1.8127458249272754, + "velocityX": 0.6198205019898244, + "velocityY": 0.3139090009015357, + "timestamp": 0.2319978139637073 + }, + { + "x": 0.8463280476277008, + "y": 6.677302965953696, + "heading": 0.6104261741684412, + "angularVelocity": -1.4520734785570149, + "velocityX": 0.7495169940713614, + "velocityY": 0.3612114516290285, + "timestamp": 0.27066411629099185 + }, + { + "x": 0.8803563088296403, + "y": 6.692538823127676, + "heading": 0.5688472333280339, + "angularVelocity": -1.0753275678766911, + "velocityX": 0.8800495303098976, + "velocityY": 0.39403450179995414, + "timestamp": 0.3093304186182764 + }, + { + "x": 0.9194008243749598, + "y": 6.708497582198662, + "heading": 0.5422592705741602, + "angularVelocity": -0.687626205598872, + "velocityX": 1.0097814684952597, + "velocityY": 0.41273041667921895, + "timestamp": 0.347996720945561 + }, + { + "x": 0.9633926969788928, + "y": 6.724681738794303, + "heading": 0.5307405695170507, + "angularVelocity": -0.2979002481174931, + "velocityX": 1.1377315635609189, + "velocityY": 0.4185597179335494, + "timestamp": 0.3866630232728456 + }, + { + "x": 1.0120758029948176, + "y": 6.737784321546207, + "heading": 0.5307403964315477, + "angularVelocity": -0.00000447639139680952, + "velocityX": 1.2590577087992194, + "velocityY": 0.33886309171704404, + "timestamp": 0.42532932560013015 + }, + { + "x": 1.0608121926714902, + "y": 6.750687310320991, + "heading": 0.5307402334549555, + "angularVelocity": -0.000004214951581791169, + "velocityX": 1.2604357474927872, + "velocityY": 0.33370112987702966, + "timestamp": 0.4639956279274147 + }, + { + "x": 1.1095485839791375, + "y": 6.763590292935407, + "heading": 0.5307400704784038, + "angularVelocity": -0.0000042149505359662004, + "velocityX": 1.2604357896735676, + "velocityY": 0.33370097055568565, + "timestamp": 0.5026619302546993 + }, + { + "x": 1.1582849752868287, + "y": 6.7764932755496385, + "heading": 0.5307399075018228, + "angularVelocity": -0.000004214951290774306, + "velocityX": 1.2604357896747036, + "velocityY": 0.33370097055091935, + "timestamp": 0.5413282325819838 + }, + { + "x": 1.2070213665945133, + "y": 6.789396258163877, + "heading": 0.5307397445252127, + "angularVelocity": -0.000004214952045657554, + "velocityX": 1.2604357896745395, + "velocityY": 0.33370097055106207, + "timestamp": 0.5799945349092683 + }, + { + "x": 1.255757757902192, + "y": 6.80229924077812, + "heading": 0.5307395815485734, + "angularVelocity": -0.000004214952801397565, + "velocityX": 1.2604357896743759, + "velocityY": 0.3337009705512057, + "timestamp": 0.6186608372365529 + }, + { + "x": 1.304494149209864, + "y": 6.815202223392369, + "heading": 0.5307394185719048, + "angularVelocity": -0.0000042149535559010885, + "velocityX": 1.2604357896742118, + "velocityY": 0.3337009705513488, + "timestamp": 0.6573271395638374 + }, + { + "x": 1.35323054051753, + "y": 6.828105206006622, + "heading": 0.5307392555952071, + "angularVelocity": -0.000004214954311531878, + "velocityX": 1.2604357896740477, + "velocityY": 0.333700970551492, + "timestamp": 0.6959934418911219 + }, + { + "x": 1.4019669318251895, + "y": 6.841008188620883, + "heading": 0.5307390926184801, + "angularVelocity": -0.000004214955066684376, + "velocityX": 1.260435789673884, + "velocityY": 0.3337009705516355, + "timestamp": 0.7346597442184064 + }, + { + "x": 1.4507033231328428, + "y": 6.853911171235148, + "heading": 0.530738929641724, + "angularVelocity": -0.000004214955821682156, + "velocityX": 1.26043578967372, + "velocityY": 0.3337009705517787, + "timestamp": 0.7733260465456909 + }, + { + "x": 1.4994397144404896, + "y": 6.866814153849419, + "heading": 0.5307387666649387, + "angularVelocity": -0.000004214956576941562, + "velocityX": 1.2604357896735556, + "velocityY": 0.3337009705519219, + "timestamp": 0.8119923488729754 + }, + { + "x": 1.5481761057481302, + "y": 6.879717136463695, + "heading": 0.5307386036881241, + "angularVelocity": -0.000004214957331767799, + "velocityX": 1.260435789673392, + "velocityY": 0.3337009705520652, + "timestamp": 0.85065865120026 + }, + { + "x": 1.5969124970557644, + "y": 6.892620119077977, + "heading": 0.5307384407112804, + "angularVelocity": -0.000004214958086684387, + "velocityX": 1.260435789673228, + "velocityY": 0.3337009705522084, + "timestamp": 0.8893249535275445 + }, + { + "x": 1.6456488883633922, + "y": 6.905523101692264, + "heading": 0.5307382777344075, + "angularVelocity": -0.000004214958842439668, + "velocityX": 1.2604357896730638, + "velocityY": 0.3337009705523516, + "timestamp": 0.927991255854829 + }, + { + "x": 1.6943852796710137, + "y": 6.9184260843065575, + "heading": 0.5307381147575053, + "angularVelocity": -0.000004214959597145177, + "velocityX": 1.2604357896729, + "velocityY": 0.33370097055249487, + "timestamp": 0.9666575581821135 + }, + { + "x": 1.7431216709786288, + "y": 6.931329066920855, + "heading": 0.530737951780574, + "angularVelocity": -0.000004214960352296469, + "velocityX": 1.2604357896727358, + "velocityY": 0.3337009705526381, + "timestamp": 1.0053238605093981 + }, + { + "x": 1.7918580622862375, + "y": 6.9442320495351595, + "heading": 0.5307377888036136, + "angularVelocity": -0.000004214961107568621, + "velocityX": 1.2604357896725717, + "velocityY": 0.33370097055278125, + "timestamp": 1.0439901628366828 + }, + { + "x": 1.8405944535938399, + "y": 6.957135032149469, + "heading": 0.5307376258266238, + "angularVelocity": -0.000004214961862680116, + "velocityX": 1.2604357896724079, + "velocityY": 0.33370097055292464, + "timestamp": 1.0826564651639674 + }, + { + "x": 1.889330844901439, + "y": 6.970038014763773, + "heading": 0.5307374628496049, + "angularVelocity": -0.000004214962617743034, + "velocityX": 1.260435789672316, + "velocityY": 0.3337009705527953, + "timestamp": 1.121322767491252 + }, + { + "x": 1.9380672362995741, + "y": 6.982940997036096, + "heading": 0.5307372998725607, + "angularVelocity": -0.00000421496327361953, + "velocityX": 1.260435792013792, + "velocityY": 0.3337009617083353, + "timestamp": 1.1599890698185367 + }, + { + "x": 1.986806564448301, + "y": 6.995832881166175, + "heading": 0.5307371364965603, + "angularVelocity": -0.000004225281203502036, + "velocityX": 1.2605117431752726, + "velocityY": 0.33341393808569914, + "timestamp": 1.1986553721458213 + }, + { + "x": 2.0317973138853715, + "y": 7.00490530051565, + "heading": 0.5171429760348675, + "angularVelocity": -0.3515764281428815, + "velocityX": 1.1635648285228963, + "velocityY": 0.23463374575309978, + "timestamp": 1.237321674473106 + }, + { + "x": 2.0728058947189574, + "y": 7.0102031606644015, + "heading": 0.4906124312582534, + "angularVelocity": -0.6861412439204636, + "velocityX": 1.0605767390546776, + "velocityY": 0.13701491556938833, + "timestamp": 1.2759879768003906 + }, + { + "x": 2.110279120553285, + "y": 7.0109088277839495, + "heading": 0.45382754800463343, + "angularVelocity": -0.9513421516826277, + "velocityX": 0.9691442827175547, + "velocityY": 0.01825018367609023, + "timestamp": 1.3146542791276752 + }, + { + "x": 2.1444753266925427, + "y": 7.0064516078188515, + "heading": 0.4096540058788496, + "angularVelocity": -1.1424299575353483, + "velocityX": 0.8843929747871832, + "velocityY": -0.11527401630940219, + "timestamp": 1.3533205814549598 + }, + { + "x": 2.175331590133971, + "y": 6.996634649413561, + "heading": 0.3604548445442151, + "angularVelocity": -1.2724040927993665, + "velocityX": 0.7980143324862465, + "velocityY": -0.2538892475985945, + "timestamp": 1.3919868837822444 + }, + { + "x": 2.202658354652599, + "y": 6.981442157383057, + "heading": 0.3084300196662748, + "angularVelocity": -1.3454822868139975, + "velocityX": 0.7067333278293896, + "velocityY": -0.3929129789011957, + "timestamp": 1.430653186109529 + }, + { + "x": 2.226219612980993, + "y": 6.960976280761605, + "heading": 0.2557056738235528, + "angularVelocity": -1.363573516713794, + "velocityX": 0.6093486294333386, + "velocityY": -0.5292948999422463, + "timestamp": 1.4693194884368137 + }, + { + "x": 2.24576982847862, + "y": 6.9354320182780045, + "heading": 0.20431672210316915, + "angularVelocity": -1.329037136404176, + "velocityX": 0.505613785671528, + "velocityY": -0.6606337028917001, + "timestamp": 1.5079857907640983 }, { "x": 2.2610747814178467, "y": 6.9050798416137695, "heading": 0.15617120010566338, - "angularVelocity": -2.7695828819396024, - "velocityX": 1.5897254047029348, - "velocityY": -1.7931339227554435, - "timestamp": 0.5958294258288975 - }, - { - "x": 2.3021420961741277, - "y": 6.690129168178249, - "heading": -0.009663328888242836, - "angularVelocity": -2.0165575251161725, - "velocityX": 0.4993809377969598, - "velocityY": -2.6138127003762723, - "timestamp": 0.6780658742953757 - }, - { - "x": 2.248466001407306, - "y": 6.41546750147008, - "heading": -0.10873685342712126, - "angularVelocity": -1.204739825057738, - "velocityX": -0.6527044366306908, - "velocityY": -3.3399018541045957, - "timestamp": 0.7603023227618538 - }, - { - "x": 2.085165640858422, - "y": 6.151744237063873, - "heading": -0.10873683701731249, - "angularVelocity": 1.9954422992985155e-7, - "velocityX": -1.9857418893211336, - "velocityY": -3.2068902454330503, - "timestamp": 0.842538771228332 - }, - { - "x": 1.8574534262453974, - "y": 5.941116320042222, - "heading": -0.10873681487910887, - "angularVelocity": 2.692018447413834e-7, - "velocityX": -2.7689937838918954, - "velocityY": -2.5612477307736583, - "timestamp": 0.9247752196948101 - }, - { - "x": 1.629741023592729, - "y": 5.730488606312599, - "heading": -0.10873679274080142, - "angularVelocity": 2.6920310704244503e-7, - "velocityX": -2.76899607046491, - "velocityY": -2.5612452587307564, - "timestamp": 1.0070116681612884 - }, - { - "x": 1.4644902813281564, - "y": 5.577695792451846, - "heading": 0.0012781207724603802, - "angularVelocity": 1.337787751839788, - "velocityX": -2.009458644507651, - "velocityY": -1.8579695099921079, - "timestamp": 1.0892481166277665 + "angularVelocity": -1.245154542836593, + "velocityX": 0.39582147808373047, + "velocityY": -0.7849774826495736, + "timestamp": 1.546652093091383 + }, + { + "x": 2.271660182847978, + "y": 6.871430398890197, + "heading": 0.11471436587906376, + "angularVelocity": -1.100460804827315, + "velocityX": 0.2809867080916391, + "velocityY": -0.8932156425445383, + "timestamp": 1.5843243427932703 + }, + { + "x": 2.277809261221615, + "y": 6.833910424701081, + "heading": 0.07939066205351579, + "angularVelocity": -0.9376584649197209, + "velocityX": 0.1632256746625241, + "velocityY": -0.9959578864023372, + "timestamp": 1.6219965924951576 + }, + { + "x": 2.2793609405244357, + "y": 6.792889887349777, + "heading": 0.05132731412073156, + "angularVelocity": -0.7449342196140949, + "velocityX": 0.041188920627471544, + "velocityY": -1.0888794185616628, + "timestamp": 1.659668842197045 + }, + { + "x": 2.27611913107799, + "y": 6.749248560060934, + "heading": 0.03326209599426278, + "angularVelocity": -0.4795364829398642, + "velocityX": -0.08605298255589845, + "velocityY": -1.158447600931467, + "timestamp": 1.6973410918989322 + }, + { + "x": 2.2678771229472154, + "y": 6.703362743956978, + "heading": 0.025221677462375882, + "angularVelocity": -0.21343080372209108, + "velocityX": -0.21878194681729676, + "velocityY": -1.2180269685793952, + "timestamp": 1.7350133416008195 + }, + { + "x": 2.2540333522614104, + "y": 6.656234544077909, + "heading": 0.0252216304618494, + "angularVelocity": -0.000001247616663515529, + "velocityX": -0.36747926644535317, + "velocityY": -1.2510057204444232, + "timestamp": 1.7726855913027069 + }, + { + "x": 2.2344053991573594, + "y": 6.61120719259268, + "heading": 0.025221637193402757, + "angularVelocity": 1.7868732048261226e-7, + "velocityX": -0.5210188735571781, + "velocityY": -1.1952392501522764, + "timestamp": 1.8103578410045942 + }, + { + "x": 2.209301413350582, + "y": 6.568987423658516, + "heading": 0.02522164328516559, + "angularVelocity": 1.617042486976944e-7, + "velocityX": -0.6663787271910937, + "velocityY": -1.1207127067872833, + "timestamp": 1.8480300907064815 + }, + { + "x": 2.1792162284525647, + "y": 6.530159590050397, + "heading": 0.025221649180709897, + "angularVelocity": 1.5649567921023276e-7, + "velocityX": -0.7986033522312032, + "velocityY": -1.0306746720829034, + "timestamp": 1.8857023404083688 + }, + { + "x": 2.1491271012258317, + "y": 6.491334811418837, + "heading": 0.025221655076174288, + "angularVelocity": 1.5649355793983052e-7, + "velocityX": -0.7987080003142876, + "velocityY": -1.0305935785304006, + "timestamp": 1.9233745901102561 + }, + { + "x": 2.119037973867113, + "y": 6.452510032889566, + "heading": 0.025221660971638672, + "angularVelocity": 1.564935577840148e-7, + "velocityX": -0.7987080038178083, + "velocityY": -1.0305935758151787, + "timestamp": 1.9610468398121434 + }, + { + "x": 2.08894884650839, + "y": 6.413685254360297, + "heading": 0.02522166686710306, + "angularVelocity": 1.5649355776196318e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.0305935758150877, + "timestamp": 1.9987190895140308 + }, + { + "x": 2.058859719149667, + "y": 6.374860475831029, + "heading": 0.025221672762567447, + "angularVelocity": 1.5649355789908895e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150877, + "timestamp": 2.036391339215918 + }, + { + "x": 2.0287705917909435, + "y": 6.33603569730176, + "heading": 0.02522167865803183, + "angularVelocity": 1.5649355774113446e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150877, + "timestamp": 2.0740635889178054 + }, + { + "x": 1.9986814644322204, + "y": 6.297210918772492, + "heading": 0.02522168455349622, + "angularVelocity": 1.5649355783955773e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.0305935758150877, + "timestamp": 2.1117358386196927 + }, + { + "x": 1.9685923370734972, + "y": 6.258386140243222, + "heading": 0.02522169044896061, + "angularVelocity": 1.5649355788803578e-7, + "velocityX": -0.7987080038179256, + "velocityY": -1.0305935758150877, + "timestamp": 2.14940808832158 + }, + { + "x": 1.9385032097147739, + "y": 6.219561361713954, + "heading": 0.025221696344425008, + "angularVelocity": 1.5649355808145546e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.0305935758150877, + "timestamp": 2.1870803380234674 + }, + { + "x": 1.9084140823560507, + "y": 6.180736583184685, + "heading": 0.0252217022398894, + "angularVelocity": 1.5649355798785538e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.0305935758150877, + "timestamp": 2.2247525877253547 + }, + { + "x": 1.8783249549973273, + "y": 6.141911804655416, + "heading": 0.025221708135353786, + "angularVelocity": 1.5649355785902612e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.030593575815088, + "timestamp": 2.262424837427242 + }, + { + "x": 1.8482358276386042, + "y": 6.103087026126148, + "heading": 0.02522171403081817, + "angularVelocity": 1.5649355767080312e-7, + "velocityX": -0.7987080038179256, + "velocityY": -1.030593575815088, + "timestamp": 2.3000970871291293 + }, + { + "x": 1.8181467002798808, + "y": 6.064262247596879, + "heading": 0.025221719926282554, + "angularVelocity": 1.5649355777865092e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150877, + "timestamp": 2.3377693368310166 + }, + { + "x": 1.7880575729211576, + "y": 6.025437469067611, + "heading": 0.02522172582174694, + "angularVelocity": 1.5649355789868648e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.030593575815088, + "timestamp": 2.375441586532904 + }, + { + "x": 1.7579684455624343, + "y": 5.986612690538342, + "heading": 0.025221731717211322, + "angularVelocity": 1.5649355766897273e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.030593575815088, + "timestamp": 2.4131138362347913 + }, + { + "x": 1.727879318203711, + "y": 5.9477879120090735, + "heading": 0.025221737612675703, + "angularVelocity": 1.5649355766619197e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150877, + "timestamp": 2.4507860859366786 + }, + { + "x": 1.6977901908449877, + "y": 5.908963133479805, + "heading": 0.025221743508140076, + "angularVelocity": 1.5649355754994554e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150875, + "timestamp": 2.488458335638566 + }, + { + "x": 1.6677010634862643, + "y": 5.870138354950536, + "heading": 0.025221749403604464, + "angularVelocity": 1.564935577800695e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.0305935758150877, + "timestamp": 2.526130585340453 + }, + { + "x": 1.6376119361275412, + "y": 5.831313576421268, + "heading": 0.025221755299068848, + "angularVelocity": 1.5649355775129745e-7, + "velocityX": -0.7987080038179257, + "velocityY": -1.0305935758150877, + "timestamp": 2.5638028350423405 + }, + { + "x": 1.6075228087688178, + "y": 5.792488797891999, + "heading": 0.025221761194533236, + "angularVelocity": 1.5649355776446108e-7, + "velocityX": -0.7987080038179256, + "velocityY": -1.030593575815088, + "timestamp": 2.601475084744228 + }, + { + "x": 1.5774336814100947, + "y": 5.753664019362731, + "heading": 0.025221767089997616, + "angularVelocity": 1.5649355770162784e-7, + "velocityX": -0.7987080038179258, + "velocityY": -1.030593575815088, + "timestamp": 2.639147334446115 + }, + { + "x": 1.5473445540513797, + "y": 5.714839240833456, + "heading": 0.025221772985461997, + "angularVelocity": 1.564935575497616e-7, + "velocityX": -0.7987080038177026, + "velocityY": -1.0305935758152607, + "timestamp": 2.6768195841480025 + }, + { + "x": 1.5172554269437706, + "y": 5.676014462109576, + "heading": 0.02522177888092892, + "angularVelocity": 1.5649362515357016e-7, + "velocityX": -0.7987079971521701, + "velocityY": -1.0305935809809978, + "timestamp": 2.71449183384989 + }, + { + "x": 1.4872404490214195, + "y": 5.6372698779844646, + "heading": 0.02551879992466325, + "angularVelocity": 0.007884345800487103, + "velocityX": -0.7967397264530625, + "velocityY": -1.0284648363638766, + "timestamp": 2.752164083551777 + }, + { + "x": 1.4608873504024076, + "y": 5.603265426982504, + "heading": 0.030651229646237198, + "angularVelocity": 0.1362390025068593, + "velocityX": -0.69953609958403, + "velocityY": -0.9026392442991357, + "timestamp": 2.7898363332536644 + }, + { + "x": 1.4383001181401514, + "y": 5.574120228098155, + "heading": 0.03673673574367204, + "angularVelocity": 0.1615381652434539, + "velocityX": -0.599572163621708, + "velocityY": -0.7736516697299336, + "timestamp": 2.8275085829555517 + }, + { + "x": 1.4194824287204242, + "y": 5.549838992967363, + "heading": 0.04263007064344353, + "angularVelocity": 0.15643703114119317, + "velocityX": -0.49951063630760206, + "velocityY": -0.6445390260188326, + "timestamp": 2.865180832657439 + }, + { + "x": 1.4044322017294746, + "y": 5.530419035921232, + "heading": 0.04779341900668404, + "angularVelocity": 0.13705972975070876, + "velocityX": -0.39950433303155547, + "velocityY": -0.5154976726849608, + "timestamp": 2.9028530823593264 + }, + { + "x": 1.3931471151595585, + "y": 5.515857373000764, + "heading": 0.05191554418164246, + "angularVelocity": 0.10942073296867788, + "velocityX": -0.29955966684281865, + "velocityY": -0.38653552776111066, + "timestamp": 2.9405253320612137 + }, + { + "x": 1.3856252119872758, + "y": 5.506151497285298, + "heading": 0.054793759356887295, + "angularVelocity": 0.07640146787149468, + "velocityX": -0.1996669493275965, + "velocityY": -0.2576399283895094, + "timestamp": 2.978197581763101 }, { "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 0.6688946289455666, - "velocityX": -1.0047293810903806, - "velocityY": -0.9289848068631428, - "timestamp": 1.1714845650942447 + "angularVelocity": 0.03960156640941446, + "velocityX": -0.09981635446971436, + "velocityY": -0.12879814897149688, + "timestamp": 3.0158698314649883 }, { "x": 1.3818649053573608, "y": 5.5012993812561035, "heading": 0.05628563945524819, - "angularVelocity": 8.108539434054397e-29, - "velocityX": -3.383351472481295e-31, - "velocityY": 2.5568610146599268e-30, - "timestamp": 1.2537210135607229 + "angularVelocity": -1.4628158600346964e-29, + "velocityX": 1.6054548136189337e-31, + "velocityY": -2.7912282615412047e-30, + "timestamp": 3.0535420811668756 + }, + { + "x": 1.3845786591625213, + "y": 5.501429733098524, + "heading": 0.05582872936035501, + "angularVelocity": -0.01824281215259517, + "velocityX": 0.10835063932598839, + "velocityY": 0.005204490339855129, + "timestamp": 3.0785881152469945 + }, + { + "x": 1.3900063822638737, + "y": 5.501690432475359, + "heading": 0.05493041969316394, + "angularVelocity": -0.035866343721856854, + "velocityX": 0.2167098824505046, + "velocityY": 0.01040880867619674, + "timestamp": 3.1036341493271133 + }, + { + "x": 1.3981483243528032, + "y": 5.502081473730299, + "heading": 0.05360938694536658, + "angularVelocity": -0.052744188703298736, + "velocityX": 0.32507909487334297, + "velocityY": 0.015612901176014845, + "timestamp": 3.128680183407232 + }, + { + "x": 1.4090047772653698, + "y": 5.502602849235542, + "heading": 0.05188855400955324, + "angularVelocity": -0.06870680325276785, + "velocityX": 0.43345995928302505, + "velocityY": 0.02081668912451226, + "timestamp": 3.153726217487351 + }, + { + "x": 1.4225760848465896, + "y": 5.5032545483338176, + "heading": 0.04979672584047775, + "angularVelocity": -0.08351933732794915, + "velocityX": 0.5418545522141726, + "velocityY": 0.026020051565707466, + "timestamp": 3.17877225156747 + }, + { + "x": 1.4388626543213987, + "y": 5.504036555432711, + "heading": 0.04737119018132143, + "angularVelocity": -0.09684310303916191, + "velocityX": 0.6502654042039197, + "velocityY": 0.031222791456693683, + "timestamp": 3.2038182856475887 + }, + { + "x": 1.4578649660996903, + "y": 5.504948846300725, + "heading": 0.044662119929733506, + "angularVelocity": -0.1081636415133266, + "velocityX": 0.7586954372700782, + "velocityY": 0.03642456386927063, + "timestamp": 3.2288643197277076 + }, + { + "x": 1.479583568415963, + "y": 5.505991380099472, + "heading": 0.04174066948746936, + "angularVelocity": -0.11664323512860124, + "velocityX": 0.8671473594104407, + "velocityY": 0.04162470574871449, + "timestamp": 3.2539103538078264 }, { - "x": 1.4659695573316451, - "y": 5.505361506655373, - "heading": 0.050154371824367786, - "angularVelocity": -0.0866134750258778, - "velocityX": 1.188106051128092, - "velocityY": 0.05738369583633677, - "timestamp": 1.3245098559325195 + "x": 1.5040189973726636, + "y": 5.507164079613842, + "heading": 0.038715690708052176, + "angularVelocity": -0.12077675729898374, + "velocityX": 0.9756206862348644, + "velocityY": 0.04682176470053254, + "timestamp": 3.2789563878879453 }, { - "x": 1.6341788550204006, - "y": 5.5134857557639885, - "heading": 0.03789188378734141, - "angularVelocity": -0.17322628293059847, - "velocityX": 2.3762120138268115, - "velocityY": 0.11476736779992304, - "timestamp": 1.395298698304316 + "x": 1.5311713165435374, + "y": 5.508466770219786, + "heading": 0.035774832832580886, + "angularVelocity": -0.11741810563861127, + "velocityX": 1.0840965513352385, + "velocityY": 0.05201185152863736, + "timestamp": 3.304002421968064 }, { - "x": 1.8864927796012139, - "y": 5.525672125057411, - "heading": 0.019498324963128963, - "angularVelocity": -0.25983697724008453, - "velocityX": 3.5643177106303305, - "velocityY": 0.1721509899740557, - "timestamp": 1.4660875406761127 + "x": 1.5610369807434235, + "y": 5.509898901262541, + "heading": 0.03332144972686085, + "angularVelocity": -0.09795495358045375, + "velocityX": 1.1924308696681545, + "velocityY": 0.057179952649896545, + "timestamp": 3.329048456048183 }, { - "x": 2.1102347064408926, - "y": 5.533923842143074, - "heading": 0.00011174721946555298, - "angularVelocity": -0.2738648789005671, - "velocityX": 3.160694812108141, - "velocityY": 0.11656804673148825, - "timestamp": 1.5368763830479093 + "x": 1.593555387085345, + "y": 5.5114558804639, + "heading": 0.03294001206773026, + "angularVelocity": -0.015229463390271427, + "velocityX": 1.2983455279948946, + "velocityY": 0.06216470026327949, + "timestamp": 3.354094490128302 }, { - "x": 2.249872007587195, - "y": 5.538113436672261, - "heading": -0.013143603106743272, - "angularVelocity": -0.18725197195045482, - "velocityX": 1.9725891322378306, - "velocityY": 0.0591843910539215, - "timestamp": 1.607665225419706 + "x": 1.6261742711277745, + "y": 5.51302455812895, + "heading": 0.032940008328990356, + "angularVelocity": -1.4927472709571015e-7, + "velocityX": 1.3023572489794273, + "velocityY": 0.0626317787497518, + "timestamp": 3.3791405242084207 + }, + { + "x": 1.6587931551326383, + "y": 5.514593236575243, + "heading": 0.03294000459026413, + "angularVelocity": -1.4927418075968276e-7, + "velocityX": 1.302357247479563, + "velocityY": 0.062631809941977, + "timestamp": 3.4041865582885396 + }, + { + "x": 1.6914120391374976, + "y": 5.516161915021623, + "heading": 0.03294000085153791, + "angularVelocity": -1.4927418086717312e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552323, + "timestamp": 3.4292325923686584 + }, + { + "x": 1.7240309231423572, + "y": 5.517730593468005, + "heading": 0.03293999711281168, + "angularVelocity": -1.4927418091774954e-7, + "velocityX": 1.3023572474793925, + "velocityY": 0.06263180994552363, + "timestamp": 3.4542786264487773 + }, + { + "x": 1.7566498071472165, + "y": 5.519299271914385, + "heading": 0.032939993374085465, + "angularVelocity": -1.4927418089977704e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552363, + "timestamp": 3.479324660528896 + }, + { + "x": 1.789268691152076, + "y": 5.520867950360767, + "heading": 0.03293998963535924, + "angularVelocity": -1.492741809585475e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552364, + "timestamp": 3.504370694609015 + }, + { + "x": 1.8218875751569354, + "y": 5.522436628807148, + "heading": 0.03293998589663304, + "angularVelocity": -1.4927418013494575e-7, + "velocityX": 1.3023572474793925, + "velocityY": 0.06263180994552363, + "timestamp": 3.529416728689134 + }, + { + "x": 1.854506459161795, + "y": 5.524005307253529, + "heading": 0.03293998215790683, + "angularVelocity": -1.4927418024687253e-7, + "velocityX": 1.3023572474793925, + "velocityY": 0.06263180994552364, + "timestamp": 3.5544627627692527 + }, + { + "x": 1.8871253431666544, + "y": 5.52557398569991, + "heading": 0.03293997841918063, + "angularVelocity": -1.4927418036897545e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552363, + "timestamp": 3.5795087968493715 + }, + { + "x": 1.919744227171514, + "y": 5.527142664146291, + "heading": 0.03293997468045441, + "angularVelocity": -1.492741807507e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552363, + "timestamp": 3.6045548309294904 + }, + { + "x": 1.9523631111763733, + "y": 5.528711342592672, + "heading": 0.032939970941728194, + "angularVelocity": -1.4927418060860102e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552363, + "timestamp": 3.629600865009609 + }, + { + "x": 1.9849819951812326, + "y": 5.5302800210390535, + "heading": 0.03293996720300198, + "angularVelocity": -1.4927418068263622e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552363, + "timestamp": 3.654646899089728 + }, + { + "x": 2.017600879186092, + "y": 5.531848699485434, + "heading": 0.03293996346427576, + "angularVelocity": -1.492741805805833e-7, + "velocityX": 1.3023572474793927, + "velocityY": 0.06263180994552273, + "timestamp": 3.679692933169847 + }, + { + "x": 2.0502197631909613, + "y": 5.533417377931615, + "heading": 0.03293995972554955, + "angularVelocity": -1.492741805487614e-7, + "velocityX": 1.3023572474797764, + "velocityY": 0.06263180993754675, + "timestamp": 3.7047389672499658 + }, + { + "x": 2.0828386472803335, + "y": 5.534986054620662, + "heading": 0.03293995598682334, + "angularVelocity": -1.4927418043775344e-7, + "velocityX": 1.302357250853668, + "velocityY": 0.06263173978131052, + "timestamp": 3.7297850013300846 + }, + { + "x": 2.115458270958098, + "y": 5.536539275955716, + "heading": 0.03293995220626054, + "angularVelocity": -1.509445682565159e-7, + "velocityX": 1.3023867800148246, + "velocityY": 0.06201466189978957, + "timestamp": 3.7548310354102035 + }, + { + "x": 2.1462101718049076, + "y": 5.5373923966618745, + "heading": 0.02588090552477964, + "angularVelocity": -0.2818428921324181, + "velocityX": 1.2278151801765511, + "velocityY": 0.03406210753032908, + "timestamp": 3.7798770694903223 + }, + { + "x": 2.1742457798850925, + "y": 5.538117943233322, + "heading": 0.019050828435002092, + "angularVelocity": -0.2727009421129447, + "velocityX": 1.119363169054274, + "velocityY": 0.02896852128537856, + "timestamp": 3.804923103570441 + }, + { + "x": 2.199567234111099, + "y": 5.538714457409619, + "heading": 0.012652728722647373, + "angularVelocity": -0.25545360562588787, + "velocityX": 1.010996557178806, + "velocityY": 0.023816711832265754, + "timestamp": 3.82996913765056 + }, + { + "x": 2.2221755025244465, + "y": 5.539181482835929, + "heading": 0.006752605462386223, + "angularVelocity": -0.23557115834836043, + "velocityX": 0.9026685957945435, + "velocityY": 0.018646681736272153, + "timestamp": 3.855015171730679 + }, + { + "x": 2.2420711141918233, + "y": 5.539518797286996, + "heading": 0.0013831641278171489, + "angularVelocity": -0.21438289660576354, + "velocityX": 0.7943617581812826, + "velocityY": 0.013467778967862453, + "timestamp": 3.8800612058107977 + }, + { + "x": 2.2592544005969666, + "y": 5.539726269571517, + "heading": -0.0034360662843230887, + "angularVelocity": -0.19241491074908987, + "velocityX": 0.686068155547121, + "velocityY": 0.008283638192281886, + "timestamp": 3.9051072398909166 + }, + { + "x": 2.273725588479032, + "y": 5.5398038132440925, + "heading": -0.00769210681421661, + "angularVelocity": -0.16992872070189774, + "velocityX": 0.5777836058095082, + "velocityY": 0.003096045956056925, + "timestamp": 3.9301532739710354 + }, + { + "x": 2.285484842574345, + "y": 5.539751367109182, + "heading": -0.011375707178688507, + "angularVelocity": -0.1470731994007397, + "velocityX": 0.4695056334136347, + "velocityY": -0.0020939896027525593, + "timestamp": 3.9551993080511543 + }, + { + "x": 2.2945322879434182, + "y": 5.539568885584673, + "heading": -0.014479940949098854, + "angularVelocity": -0.12394113017918651, + "velocityX": 0.36123265424391854, + "velocityY": -0.007285845094921426, + "timestamp": 3.980245342131273 + }, + { + "x": 2.3008680227496536, + "y": 5.539256333392346, + "heading": -0.016999427321887948, + "angularVelocity": -0.1005942244073929, + "velocityX": 0.25296359439339106, + "velocityY": -0.012479109122301884, + "timestamp": 4.005291376211392 + }, + { + "x": 2.3044921260944244, + "y": 5.538813682391967, + "heading": -0.01892986514732486, + "angularVelocity": -0.07707558886326937, + "velocityX": 0.14469769278233818, + "velocityY": -0.017673496690129843, + "timestamp": 4.030337410291511 }, { "x": 2.3054046630859375, "y": 5.538240909576416, "heading": -0.020267736871838693, - "angularVelocity": -0.10063921836266371, - "velocityX": 0.784483170484331, - "velocityY": 0.0018007485344283746, - "timestamp": 1.6784540677915025 + "angularVelocity": -0.053416509783886956, + "velocityX": 0.036434390712835496, + "velocityY": -0.022868802851364584, + "timestamp": 4.05538344437163 }, { - "x": 2.206836259942782, - "y": 5.529745448091516, - "heading": -0.017578724891835627, - "angularVelocity": 0.025983583153922206, - "velocityX": -0.9524540308729255, - "velocityY": -0.08209057139403513, - "timestamp": 1.7819429501397848 + "x": 2.3035545118219214, + "y": 5.537528590892998, + "heading": -0.021011691067101653, + "angularVelocity": -0.029372452797134904, + "velocityX": -0.07304680988823148, + "velocityY": -0.028123434260638402, + "timestamp": 4.080711740936763 }, { - "x": 1.9285141868544358, - "y": 5.512568166846942, - "heading": -0.0017855827044546864, - "angularVelocity": 0.15260713836130316, - "velocityX": -2.689391041558251, - "velocityY": -0.1659818992610695, - "timestamp": 1.885431832488067 + "x": 2.298931156655876, + "y": 5.536683300929956, + "heading": -0.021159077287045792, + "angularVelocity": -0.005819034043431043, + "velocityX": -0.1825371538163915, + "velocityY": -0.03337334434767621, + "timestamp": 4.106040037501895 }, { - "x": 1.5690068282385219, - "y": 5.495204505355553, - "heading": 0.024422680398419737, - "angularVelocity": 0.25324713638971313, - "velocityX": -3.4738742023131053, - "velocityY": -0.16778286804715686, - "timestamp": 1.9889207148363492 + "x": 2.2915343251541973, + "y": 5.535705183945715, + "heading": -0.020724840944498453, + "angularVelocity": 0.017144316886782557, + "velocityX": -0.2920382538410566, + "velocityY": -0.03861755889085621, + "timestamp": 4.1313683340670275 + }, + { + "x": 2.2813636924749554, + "y": 5.534594417495688, + "heading": -0.01972730019355094, + "angularVelocity": 0.039384438994890546, + "velocityX": -0.4015521791234612, + "velocityY": -0.043854763273898215, + "timestamp": 4.15669663063216 + }, + { + "x": 2.268418865246587, + "y": 5.533351225535465, + "heading": -0.01818943546874751, + "angularVelocity": 0.06071725829704457, + "velocityX": -0.5110816353185899, + "velocityY": -0.04908312554685771, + "timestamp": 4.1820249271972925 + }, + { + "x": 2.252699358665988, + "y": 5.531975899350418, + "heading": -0.01614093194994636, + "angularVelocity": 0.08087806116370898, + "velocityX": -0.6206302322838587, + "velocityY": -0.05429998742641995, + "timestamp": 4.207353223762425 + }, + { + "x": 2.2342045636549908, + "y": 5.530468833172859, + "heading": -0.013621619170245854, + "angularVelocity": 0.09946633296833651, + "velocityX": -0.7302028765895126, + "velocityY": -0.05950128440959299, + "timestamp": 4.232681520327557 + }, + { + "x": 2.212933700942747, + "y": 5.5288305900830315, + "heading": -0.010687751449041242, + "angularVelocity": 0.11583359795362039, + "velocityX": -0.839806287703566, + "velocityY": -0.06468035012099213, + "timestamp": 4.25800981689269 + }, + { + "x": 2.188885768109178, + "y": 5.527062038952629, + "heading": -0.00742483553755111, + "angularVelocity": 0.12882492524028982, + "velocityX": -0.9494492758999686, + "velocityY": -0.06982511144524438, + "timestamp": 4.283338113457822 + }, + { + "x": 2.162059564630152, + "y": 5.52516469289038, + "heading": -0.003978602963118568, + "angularVelocity": 0.13606254828697195, + "velocityX": -1.0591396626306326, + "velocityY": -0.07491013291587634, + "timestamp": 4.308666410022955 + }, + { + "x": 2.132454659152142, + "y": 5.523141831263589, + "heading": -0.0006543637194032241, + "angularVelocity": 0.13124606446293755, + "velocityX": -1.1688470798602, + "velocityY": -0.07986567993502292, + "timestamp": 4.333994706588087 + }, + { + "x": 2.100091910087291, + "y": 5.521005809692814, + "heading": 0.0014695168260145277, + "angularVelocity": 0.08385406180119828, + "velocityX": -1.277730974983244, + "velocityY": -0.08433340810204847, + "timestamp": 4.35932300315322 + }, + { + "x": 2.067106097490617, + "y": 5.5194054139110715, + "heading": 0.0014695211546497217, + "angularVelocity": 1.7090115724079902e-7, + "velocityX": -1.3023304789506625, + "velocityY": -0.06318608034459623, + "timestamp": 4.384651299718352 + }, + { + "x": 2.0341202814051282, + "y": 5.517805090038329, + "heading": 0.0014695254831924606, + "angularVelocity": 1.7089750697357396e-7, + "velocityX": -1.3023306166944215, + "velocityY": -0.06318324126698588, + "timestamp": 4.409979596283485 + }, + { + "x": 2.001134465319256, + "y": 5.516204766173488, + "heading": 0.0014695298117352064, + "angularVelocity": 1.7089750724047756e-7, + "velocityX": -1.3023306167095552, + "velocityY": -0.06318324095504856, + "timestamp": 4.435307892848617 + }, + { + "x": 1.968148649233384, + "y": 5.514604442308648, + "heading": 0.0014695341402779503, + "angularVelocity": 1.70897507174197e-7, + "velocityX": -1.3023306167095567, + "velocityY": -0.06318324095501428, + "timestamp": 4.46063618941375 + }, + { + "x": 1.935162833147512, + "y": 5.513004118443807, + "heading": 0.0014695384688206946, + "angularVelocity": 1.7089750718946717e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501428, + "timestamp": 4.485964485978882 + }, + { + "x": 1.9021770170616399, + "y": 5.511403794578967, + "heading": 0.0014695427973634258, + "angularVelocity": 1.7089750667103753e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501429, + "timestamp": 4.511292782544015 + }, + { + "x": 1.8691912009757676, + "y": 5.509803470714126, + "heading": 0.0014695471259061786, + "angularVelocity": 1.7089750752194645e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501429, + "timestamp": 4.536621079109147 + }, + { + "x": 1.8362053848898956, + "y": 5.5082031468492865, + "heading": 0.0014695514544489196, + "angularVelocity": 1.7089750705816044e-7, + "velocityX": -1.3023306167095567, + "velocityY": -0.0631832409550143, + "timestamp": 4.5619493756742795 + }, + { + "x": 1.8032195688040236, + "y": 5.506602822984446, + "heading": 0.0014695557829916591, + "angularVelocity": 1.7089750699961795e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501429, + "timestamp": 4.587277672239412 + }, + { + "x": 1.7702337527181515, + "y": 5.505002499119605, + "heading": 0.0014695601115344013, + "angularVelocity": 1.708975070971006e-7, + "velocityX": -1.3023306167095567, + "velocityY": -0.06318324095501428, + "timestamp": 4.612605968804544 + }, + { + "x": 1.7372479366322795, + "y": 5.503402175254765, + "heading": 0.0014695644400771505, + "angularVelocity": 1.7089750737689966e-7, + "velocityX": -1.3023306167095567, + "velocityY": -0.06318324095501429, + "timestamp": 4.637934265369677 + }, + { + "x": 1.7042621205464072, + "y": 5.501801851389924, + "heading": 0.001469568768619884, + "angularVelocity": 1.708975067545602e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501429, + "timestamp": 4.663262561934809 + }, + { + "x": 1.6712763044605352, + "y": 5.500201527525085, + "heading": 0.0014695730971626293, + "angularVelocity": 1.708975072324485e-7, + "velocityX": -1.302330616709557, + "velocityY": -0.06318324095501379, + "timestamp": 4.688590858499942 + }, + { + "x": 1.6382904883746574, + "y": 5.498601203660361, + "heading": 0.0014695774257053702, + "angularVelocity": 1.7089750704976447e-7, + "velocityX": -1.3023306167097823, + "velocityY": -0.06318324095036792, + "timestamp": 4.713919155065074 + }, + { + "x": 1.6053046722368207, + "y": 5.497000880866617, + "heading": 0.0014695817542481073, + "angularVelocity": 1.7089750689937978e-7, + "velocityX": -1.3023306187612051, + "velocityY": -0.06318319866650293, + "timestamp": 4.739247451630207 + }, + { + "x": 1.5723183905912428, + "y": 5.495410305750672, + "heading": 0.0014696021551505508, + "angularVelocity": 8.054589217739563e-7, + "velocityX": -1.3023489977208753, + "velocityY": -0.06279834539482758, + "timestamp": 4.764575748195339 + }, + { + "x": 1.5417971049968995, + "y": 5.493927935781562, + "heading": 0.006291317242065446, + "angularVelocity": 0.19036870776507275, + "velocityX": -1.2050271725090151, + "velocityY": -0.05852624021955814, + "timestamp": 4.789904044760472 + }, + { + "x": 1.5140534309467035, + "y": 5.492580807122346, + "heading": 0.011350728528281598, + "angularVelocity": 0.19975331831943247, + "velocityX": -1.0953628081090867, + "velocityY": -0.05318670585541525, + "timestamp": 4.815232341325604 + }, + { + "x": 1.489087675281974, + "y": 5.49136872045318, + "heading": 0.01621727097300913, + "angularVelocity": 0.19213856061115964, + "velocityX": -0.9856863291432186, + "velocityY": -0.0478550409444552, + "timestamp": 4.840560637890737 + }, + { + "x": 1.4668984505481375, + "y": 5.490291524858565, + "heading": 0.020724096649122396, + "angularVelocity": 0.17793639080867357, + "velocityX": -0.8760646290121439, + "velocityY": -0.04252933440885621, + "timestamp": 4.865888934455869 + }, + { + "x": 1.4474846758288984, + "y": 5.489349123131251, + "heading": 0.024782341761516204, + "angularVelocity": 0.16022574206553616, + "velocityX": -0.7664856051144657, + "velocityY": -0.037207465764595535, + "timestamp": 4.891217231021002 + }, + { + "x": 1.4308455599117629, + "y": 5.488541449109029, + "heading": 0.028336770165514075, + "angularVelocity": 0.14033428560278258, + "velocityX": -0.6569378194997122, + "velocityY": -0.031888209305836225, + "timestamp": 4.916545527586134 + }, + { + "x": 1.41698051088131, + "y": 5.487868455273427, + "heading": 0.03134971257468342, + "angularVelocity": 0.11895558793002604, + "velocityX": -0.5474134036137117, + "velocityY": -0.026570828949275312, + "timestamp": 4.9418738241512665 + }, + { + "x": 1.4058890726702549, + "y": 5.48733010598215, + "heading": 0.03379383297497233, + "angularVelocity": 0.09649762249126285, + "velocityX": -0.4379069939636957, + "velocityY": -0.02125485580511678, + "timestamp": 4.967202120716399 + }, + { + "x": 1.3975708843453332, + "y": 5.486926373543047, + "heading": 0.035648388488984245, + "angularVelocity": 0.07322069643498604, + "velocityX": -0.32841483451260217, + "velocityY": -0.015939975989693458, + "timestamp": 4.9925304172815315 + }, + { + "x": 1.392025653680069, + "y": 5.486657235798119, + "heading": 0.03689710038622943, + "angularVelocity": 0.04930106112871753, + "velocityX": -0.21893421261101836, + "velocityY": -0.010625971005922226, + "timestamp": 5.017858713846664 }, { "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 0.12662396350062077, - "velocityX": -1.7369371923230166, - "velocityY": -0.08389143450007014, - "timestamp": 2.0924095971846315 + "angularVelocity": 0.024863593639583376, + "velocityX": -0.10946311281102968, + "velocityY": -0.005312684065681727, + "timestamp": 5.043187010411796 }, { "x": 1.3892531394958496, "y": 5.486522674560547, "heading": 0.03752685285960867, - "angularVelocity": 2.746210792067846e-31, - "velocityX": -7.082594657438039e-35, - "velocityY": 0, - "timestamp": 2.195898479532914 - }, - { - "x": 1.4239336560949027, - "y": 5.45678487802077, - "heading": -0.004824083437490689, - "angularVelocity": -0.7980299015185681, - "velocityX": 0.6534941530454559, - "velocityY": -0.5603571707963146, - "timestamp": 2.248967839736724 - }, - { - "x": 1.4936387312915242, - "y": 5.397060158439441, - "heading": -0.08394241765705696, - "angularVelocity": -1.4908477116686103, - "velocityX": 1.3134711805253199, - "velocityY": -1.1254086982009737, - "timestamp": 2.3020371999405342 - }, - { - "x": 1.5989756629525658, - "y": 5.306833356180067, - "heading": -0.18501626934212806, - "angularVelocity": -1.9045613381601734, - "velocityX": 1.9848916824416323, - "velocityY": -1.7001675149815771, - "timestamp": 2.3551065601443444 - }, - { - "x": 1.7389822594115079, - "y": 5.188268009617622, - "heading": -0.23253583212093973, - "angularVelocity": -0.8954236982755346, - "velocityX": 2.6381813521258652, - "velocityY": -2.234158205546495, - "timestamp": 2.4081759203481545 - }, - { - "x": 1.8903539316186226, - "y": 5.05728781912084, - "heading": -0.23253589895082247, - "angularVelocity": -0.0000012592931680036756, - "velocityX": 2.85233648240303, - "velocityY": -2.4680943955939854, - "timestamp": 2.4612452805519647 - }, - { - "x": 2.041725559967012, - "y": 4.926307577937187, - "heading": -0.23253596578056132, - "angularVelocity": -0.000001259290456718511, - "velocityX": 2.8523356559614563, - "velocityY": -2.468095350700103, - "timestamp": 2.514314640755775 - }, - { - "x": 2.186666927649336, - "y": 4.788245829195716, - "heading": -0.23253707199818868, - "angularVelocity": -0.000020844751532055486, - "velocityX": 2.731168552356477, - "velocityY": -2.601534071849594, - "timestamp": 2.567384000959585 - }, - { - "x": 2.299267843418341, - "y": 4.670864736575426, - "heading": -0.33873841006148575, - "angularVelocity": -2.0011799210587196, - "velocityX": 2.1217688575209404, - "velocityY": -2.2118429950821876, - "timestamp": 2.620453361163395 - }, - { - "x": 2.37528105951498, - "y": 4.58047733131235, - "heading": -0.49175658993248267, - "angularVelocity": -2.883362062088883, - "velocityX": 1.4323371490576546, - "velocityY": -1.7031937998865565, - "timestamp": 2.6735227213672053 + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -3.7734192506152025e-32, + "timestamp": 5.068515306976929 + }, + { + "x": 1.3924224488650907, + "y": 5.483383635076441, + "heading": 0.02924366746771374, + "angularVelocity": -0.24133382912300758, + "velocityX": 0.09233906155267109, + "velocityY": -0.09145713667218072, + "timestamp": 5.102837827868408 + }, + { + "x": 1.3988725362064904, + "y": 5.477161526594317, + "heading": 0.012911519796096444, + "angularVelocity": -0.47584347674393, + "velocityX": 0.18792580422031321, + "velocityY": -0.18128355145583694, + "timestamp": 5.1371603487598865 + }, + { + "x": 1.408682240841783, + "y": 5.467866567883204, + "heading": -0.011184409535748709, + "angularVelocity": -0.7020442760609589, + "velocityX": 0.28580956120061807, + "velocityY": -0.2708122384280062, + "timestamp": 5.171482869651365 + }, + { + "x": 1.4219108303772576, + "y": 5.455441331497704, + "heading": -0.04252483753751102, + "angularVelocity": -0.9131155634184035, + "velocityX": 0.38542010294934864, + "velocityY": -0.36201409636507, + "timestamp": 5.205805390542844 + }, + { + "x": 1.4386294081873154, + "y": 5.439746841342612, + "heading": -0.07997828832221507, + "angularVelocity": -1.0912208605866973, + "velocityX": 0.4871022691753612, + "velocityY": -0.4572650769072472, + "timestamp": 5.240127911434323 + }, + { + "x": 1.45897718646536, + "y": 5.420573005762799, + "heading": -0.12053700236214591, + "angularVelocity": -1.1816939136892084, + "velocityX": 0.5928404368193222, + "velocityY": -0.5586371595617143, + "timestamp": 5.274450432325802 + }, + { + "x": 1.48279681598405, + "y": 5.398288119279197, + "heading": -0.15391427405348196, + "angularVelocity": -0.9724597967867469, + "velocityX": 0.6939941735050115, + "velocityY": -0.6492788380568949, + "timestamp": 5.3087729532172805 + }, + { + "x": 1.5099325470579765, + "y": 5.373593666238814, + "heading": -0.17626410102219708, + "angularVelocity": -0.651170904356972, + "velocityX": 0.7906100825089313, + "velocityY": -0.7194824971761627, + "timestamp": 5.343095474108759 + }, + { + "x": 1.54027922460392, + "y": 5.34641384751758, + "heading": -0.18746082036459932, + "angularVelocity": -0.32622077433659935, + "velocityX": 0.88416225725069, + "velocityY": -0.7918945932663884, + "timestamp": 5.377417995000238 + }, + { + "x": 1.5736954265024226, + "y": 5.316646807539857, + "heading": -0.1874611197510539, + "angularVelocity": -0.000008722740836805637, + "velocityX": 0.9735940435190712, + "velocityY": -0.867274291181608, + "timestamp": 5.411740515891717 + }, + { + "x": 1.6060049814196613, + "y": 5.285681999275841, + "heading": -0.18746118384049648, + "angularVelocity": -0.0000018672708450361788, + "velocityX": 0.9413514531579913, + "velocityY": -0.9021717362171962, + "timestamp": 5.446063036783196 + }, + { + "x": 1.6383144805702523, + "y": 5.254717132823409, + "heading": -0.1874612479298642, + "angularVelocity": -0.0000018672686635293044, + "velocityX": 0.9413498283750139, + "velocityY": -0.902173431559325, + "timestamp": 5.4803855576746745 + }, + { + "x": 1.6706239797183724, + "y": 5.223752266368399, + "heading": -0.1874613120192331, + "angularVelocity": -0.0000018672686984763428, + "velocityX": 0.9413498283030138, + "velocityY": -0.9021734316344456, + "timestamp": 5.514708078566153 + }, + { + "x": 1.7029334788664925, + "y": 5.192787399913389, + "heading": -0.18746137610860322, + "angularVelocity": -0.0000018672687334879554, + "velocityX": 0.9413498283030205, + "velocityY": -0.9021734316344329, + "timestamp": 5.549030599457632 + }, + { + "x": 1.7352429780146128, + "y": 5.16182253345838, + "heading": -0.18746144019797456, + "angularVelocity": -0.0000018672687685683143, + "velocityX": 0.9413498283030303, + "velocityY": -0.9021734316344164, + "timestamp": 5.583353120349111 + }, + { + "x": 1.7675524771627336, + "y": 5.130857667003371, + "heading": -0.18746150428734706, + "angularVelocity": -0.000001867268803540895, + "velocityX": 0.9413498283030403, + "velocityY": -0.9021734316344002, + "timestamp": 5.61767564124059 + }, + { + "x": 1.7998619763108545, + "y": 5.099892800548363, + "heading": -0.1874615683767208, + "angularVelocity": -0.0000018672688385171224, + "velocityX": 0.94134982830305, + "velocityY": -0.902173431634384, + "timestamp": 5.6519981621320685 + }, + { + "x": 1.8321714754589762, + "y": 5.068927934093356, + "heading": -0.1874616324660957, + "angularVelocity": -0.000001867268873360854, + "velocityX": 0.94134982830306, + "velocityY": -0.9021734316343675, + "timestamp": 5.686320683023547 + }, + { + "x": 1.864480974607098, + "y": 5.037963067638349, + "heading": -0.18746169655547182, + "angularVelocity": -0.000001867268908538558, + "velocityX": 0.94134982830307, + "velocityY": -0.9021734316343514, + "timestamp": 5.720643203915026 + }, + { + "x": 1.8967904737552201, + "y": 5.006998201183343, + "heading": -0.18746176064484915, + "angularVelocity": -0.0000018672689434492718, + "velocityX": 0.94134982830308, + "velocityY": -0.9021734316343349, + "timestamp": 5.754965724806505 + }, + { + "x": 1.9290999729033427, + "y": 4.976033334728337, + "heading": -0.18746182473422768, + "angularVelocity": -0.0000018672689784928927, + "velocityX": 0.9413498283030898, + "velocityY": -0.9021734316343186, + "timestamp": 5.789288245697984 + }, + { + "x": 1.9614094720514654, + "y": 4.945068468273332, + "heading": -0.1874618888236074, + "angularVelocity": -0.0000018672690133920361, + "velocityX": 0.9413498283030998, + "velocityY": -0.9021734316343023, + "timestamp": 5.8236107665894625 + }, + { + "x": 1.9937189711995889, + "y": 4.9141036018183275, + "heading": -0.1874619529129883, + "angularVelocity": -0.0000018672690484597244, + "velocityX": 0.9413498283031098, + "velocityY": -0.9021734316342861, + "timestamp": 5.857933287480941 + }, + { + "x": 2.0260284703477125, + "y": 4.883138735363323, + "heading": -0.18746201700237045, + "angularVelocity": -0.0000018672690833977735, + "velocityX": 0.9413498283031192, + "velocityY": -0.9021734316342699, + "timestamp": 5.89225580837242 + }, + { + "x": 2.0583379694958364, + "y": 4.85217386890832, + "heading": -0.18746208109175377, + "angularVelocity": -0.000001867269118277379, + "velocityX": 0.9413498283031293, + "velocityY": -0.9021734316342535, + "timestamp": 5.926578329263899 + }, + { + "x": 2.0906474686439607, + "y": 4.821209002453316, + "heading": -0.18746214518113832, + "angularVelocity": -0.0000018672691534530088, + "velocityX": 0.9413498283031392, + "velocityY": -0.9021734316342372, + "timestamp": 5.960900850155378 + }, + { + "x": 2.122956967792085, + "y": 4.790244135998314, + "heading": -0.18746220927052404, + "angularVelocity": -0.0000018672691879651852, + "velocityX": 0.9413498283031491, + "velocityY": -0.902173431634221, + "timestamp": 5.9952233710468565 + }, + { + "x": 2.1552664669402097, + "y": 4.759279269543312, + "heading": -0.18746227335991097, + "angularVelocity": -0.000001867269223375388, + "velocityX": 0.9413498283031604, + "velocityY": -0.9021734316342032, + "timestamp": 6.029545891938335 + }, + { + "x": 2.1875759660893364, + "y": 4.728314403089357, + "heading": -0.18746233744929908, + "angularVelocity": -0.0000018672692579471589, + "velocityX": 0.9413498283323467, + "velocityY": -0.9021734316037436, + "timestamp": 6.063868412829814 + }, + { + "x": 2.2198854878335275, + "y": 4.697349560211671, + "heading": -0.1874624015387177, + "angularVelocity": -0.000001867270147103653, + "velocityX": 0.9413504866483311, + "velocityY": -0.9021727446999435, + "timestamp": 6.098190933721293 + }, + { + "x": 2.252690273002457, + "y": 4.666909917394811, + "heading": -0.18746249439904436, + "angularVelocity": -0.0000027055217456866627, + "velocityX": 0.9557801792197065, + "velocityY": -0.8868708365887329, + "timestamp": 6.132513454612772 + }, + { + "x": 2.28400682991543, + "y": 4.6403322606682735, + "heading": -0.19781896983491082, + "angularVelocity": -0.3017399412068747, + "velocityX": 0.9124200699590284, + "velocityY": -0.7743503692683428, + "timestamp": 6.1668359755042506 + }, + { + "x": 2.3122327500910567, + "y": 4.616324863842612, + "heading": -0.21952477619155, + "angularVelocity": -0.6324071132557241, + "velocityX": 0.8223731661456952, + "velocityY": -0.6994648470480529, + "timestamp": 6.201158496395729 + }, + { + "x": 2.3372936813760514, + "y": 4.59483038719477, + "heading": -0.2527061616816108, + "angularVelocity": -0.966752576099356, + "velocityX": 0.7301599834182618, + "velocityY": -0.6262499399680785, + "timestamp": 6.235481017287208 + }, + { + "x": 2.3591158146883524, + "y": 4.575789635608326, + "heading": -0.29749234621539733, + "angularVelocity": -1.3048629113051509, + "velocityX": 0.6357963443681244, + "velocityY": -0.5547597056360541, + "timestamp": 6.269803538178687 + }, + { + "x": 2.3776154934613634, + "y": 4.5591361519017175, + "heading": -0.35396586299822075, + "angularVelocity": -1.6453778835587947, + "velocityX": 0.5389953387020591, + "velocityY": -0.48520572714527554, + "timestamp": 6.304126059070166 + }, + { + "x": 2.3926892972627702, + "y": 4.54479439277693, + "heading": -0.4220731577487357, + "angularVelocity": -1.9843325309891349, + "velocityX": 0.4391811385028281, + "velocityY": -0.4178527320337051, + "timestamp": 6.338448579961645 + }, + { + "x": 2.4038429340082574, + "y": 4.533232670068245, + "heading": -0.4964329248800265, + "angularVelocity": -2.1665007464458306, + "velocityX": 0.32496554611338146, + "velocityY": -0.3368552894247085, + "timestamp": 6.372771100853123 + }, + { + "x": 2.4110020440418207, + "y": 4.52484519407797, + "heading": -0.5699375243350002, + "angularVelocity": -2.1415851034771523, + "velocityX": 0.20858345621521887, + "velocityY": -0.2443723762830769, + "timestamp": 6.407093621744602 }, { "x": 2.4143476486206055, "y": 4.51963472366333, "heading": -0.6391760227970993, - "angularVelocity": -2.777863390447102, - "velocityX": 0.7361420781330721, - "velocityY": -1.146473358927959, - "timestamp": 2.7265920815710154 - }, - { - "x": 2.3701336666128094, - "y": 4.4395971700859445, - "heading": -0.8245153775471664, - "angularVelocity": -2.372308285003151, - "velocityX": -0.5659305114745719, - "velocityY": -1.0244653744427692, - "timestamp": 2.8047182492034923 - }, - { - "x": 2.2242072743116044, - "y": 4.369198833257615, - "heading": -0.9780363443258114, - "angularVelocity": -1.9650390058915468, - "velocityX": -1.8678299054380199, - "velocityY": -0.901085244056695, - "timestamp": 2.8828444168359693 - }, - { - "x": 1.9766118523863694, - "y": 4.308771886473381, - "heading": -1.099290083952399, - "angularVelocity": -1.5520246711313, - "velocityX": -3.169174035132216, - "velocityY": -0.7734533590395138, - "timestamp": 2.960970584468446 - }, - { - "x": 1.6829782303645726, - "y": 4.333642341587589, - "heading": -1.099290070338212, - "angularVelocity": 1.7425898802224296e-7, - "velocityX": -3.758454189166382, - "velocityY": 0.31833706769275627, - "timestamp": 3.039096752100923 - }, - { - "x": 1.3895177021759386, - "y": 4.36047817582874, - "heading": -1.0992900564673602, - "angularVelocity": 1.7754424736252278e-7, - "velocityX": -3.7562386212150933, - "velocityY": 0.34349354453672953, - "timestamp": 3.1172229197334 - }, - { - "x": 1.0960571742019716, - "y": 4.38731401241607, - "heading": -1.0992900425963372, - "angularVelocity": 1.7754644295782002e-7, - "velocityX": -3.756238618467393, - "velocityY": 0.34349357456735763, - "timestamp": 3.195349087365877 - }, - { - "x": 0.8925519822862651, - "y": 4.405915634394149, - "heading": -1.0363057213411215, - "angularVelocity": 0.8061872630368462, - "velocityX": -2.6048275255614968, - "velocityY": 0.23809720279105512, - "timestamp": 3.273475254998354 + "angularVelocity": -2.017290591242367, + "velocityX": 0.09747549107370657, + "velocityY": -0.15180908276272057, + "timestamp": 6.441416142636081 + }, + { + "x": 2.411550419309736, + "y": 4.5135312748194405, + "heading": -0.7146746576956277, + "angularVelocity": -1.9134707003445537, + "velocityX": -0.07089421332832456, + "velocityY": -0.15468849932890996, + "timestamp": 6.480872526726484 + }, + { + "x": 2.402130176421324, + "y": 4.5073270767652875, + "heading": -0.7857083432117212, + "angularVelocity": -1.8003090540009719, + "velocityX": -0.2387507904127206, + "velocityY": -0.15724193174765805, + "timestamp": 6.520328910816888 + }, + { + "x": 2.3861176361049883, + "y": 4.50103756567989, + "heading": -0.8517701759248951, + "angularVelocity": -1.674300223806899, + "velocityX": -0.4058288838543077, + "velocityY": -0.15940414283750848, + "timestamp": 6.559785294907291 + }, + { + "x": 2.3635630141416817, + "y": 4.494680207849731, + "heading": -0.9121294134589305, + "angularVelocity": -1.529771136547598, + "velocityX": -0.5716342863965691, + "velocityY": -0.16112368065944074, + "timestamp": 6.599241678997695 + }, + { + "x": 2.334558035905885, + "y": 4.488268138139215, + "heading": -0.9656544357627823, + "angularVelocity": -1.3565617716315246, + "velocityX": -0.735114960593953, + "velocityY": -0.16251032268502744, + "timestamp": 6.638698063088098 + }, + { + "x": 2.299306014374734, + "y": 4.481764383153815, + "heading": -1.0103454671131291, + "angularVelocity": -1.1326692088142007, + "velocityX": -0.8934427810308345, + "velocityY": -0.16483403472802036, + "timestamp": 6.6781544471785015 + }, + { + "x": 2.258684856589374, + "y": 4.474594280720458, + "heading": -1.0401966260593063, + "angularVelocity": -0.7565609377124255, + "velocityX": -1.0295205382299486, + "velocityY": -0.18172223833104606, + "timestamp": 6.717610831268905 + }, + { + "x": 2.21277726427517, + "y": 4.4668464349379295, + "heading": -1.0547000810066136, + "angularVelocity": -0.3675819587034771, + "velocityX": -1.1635022664271168, + "velocityY": -0.19636482057697183, + "timestamp": 6.757067215359308 + }, + { + "x": 2.161777655764477, + "y": 4.46008626841952, + "heading": -1.0547000906528792, + "angularVelocity": -2.4447920514853427e-7, + "velocityX": -1.292556570663997, + "velocityY": -0.17133264170686324, + "timestamp": 6.796523599449712 + }, + { + "x": 2.1103594690305147, + "y": 4.458403877945663, + "heading": -1.0547000861825677, + "angularVelocity": 1.132975479004823e-7, + "velocityX": -1.3031652017617956, + "velocityY": -0.04263924616108742, + "timestamp": 6.835979983540115 + }, + { + "x": 2.058941271027739, + "y": 4.456721831910964, + "heading": -1.0547000817122596, + "angularVelocity": 1.1329746134753013e-7, + "velocityX": -1.3031654873635976, + "velocityY": -0.04263051654316007, + "timestamp": 6.875436367630519 + }, + { + "x": 2.0075230730246374, + "y": 4.4550397858862105, + "heading": -1.0547000772419513, + "angularVelocity": 1.1329746168009402e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.042630516291088466, + "timestamp": 6.914892751720922 + }, + { + "x": 1.9561048750215357, + "y": 4.453357739861458, + "heading": -1.0547000727716431, + "angularVelocity": 1.1329746154059279e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.042630516291081035, + "timestamp": 6.954349135811325 + }, + { + "x": 1.904686677018434, + "y": 4.451675693836704, + "heading": -1.054700068301335, + "angularVelocity": 1.1329746125974681e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108111, + "timestamp": 6.993805519901729 + }, + { + "x": 1.8532684790153326, + "y": 4.449993647811951, + "heading": -1.0547000638310269, + "angularVelocity": 1.1329746137667254e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108123, + "timestamp": 7.033261903992132 + }, + { + "x": 1.801850281012231, + "y": 4.448311601787198, + "heading": -1.0547000593607188, + "angularVelocity": 1.132974614247932e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108126, + "timestamp": 7.072718288082536 + }, + { + "x": 1.7504320830091293, + "y": 4.446629555762445, + "heading": -1.0547000548904106, + "angularVelocity": 1.1329746142979229e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.0426305162910812, + "timestamp": 7.112174672172939 + }, + { + "x": 1.699013885006028, + "y": 4.444947509737691, + "heading": -1.0547000504201025, + "angularVelocity": 1.1329746122002105e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108106, + "timestamp": 7.1516310562633425 + }, + { + "x": 1.6475956870029262, + "y": 4.443265463712938, + "heading": -1.0547000459497944, + "angularVelocity": 1.1329746123173152e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108124, + "timestamp": 7.191087440353746 + }, + { + "x": 1.5961774889998248, + "y": 4.441583417688185, + "heading": -1.054700041479486, + "angularVelocity": 1.1329746152886358e-7, + "velocityX": -1.3031654873718441, + "velocityY": -0.04263051629108131, + "timestamp": 7.230543824444149 + }, + { + "x": 1.5447592909967232, + "y": 4.4399013716634315, + "heading": -1.054700037009178, + "angularVelocity": 1.1329746155492088e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.042630516291081264, + "timestamp": 7.270000208534553 + }, + { + "x": 1.4933410929936215, + "y": 4.438219325638678, + "heading": -1.0547000325388698, + "angularVelocity": 1.1329746160632969e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.042630516291081166, + "timestamp": 7.309456592624956 + }, + { + "x": 1.44192289499052, + "y": 4.436537279613925, + "heading": -1.0547000280685617, + "angularVelocity": 1.1329746145163e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.0426305162910812, + "timestamp": 7.34891297671536 + }, + { + "x": 1.3905046969874184, + "y": 4.434855233589172, + "heading": -1.0547000235982535, + "angularVelocity": 1.1329746128786673e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.042630516291081215, + "timestamp": 7.388369360805763 + }, + { + "x": 1.3390864989843168, + "y": 4.433173187564418, + "heading": -1.0547000191279454, + "angularVelocity": 1.1329746138407028e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108108, + "timestamp": 7.427825744896166 + }, + { + "x": 1.2876683009812153, + "y": 4.431491141539665, + "heading": -1.0547000146576373, + "angularVelocity": 1.1329746123221407e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108107, + "timestamp": 7.46728212898657 + }, + { + "x": 1.2362501029781137, + "y": 4.429809095514912, + "heading": -1.0547000101873292, + "angularVelocity": 1.1329746116894446e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108118, + "timestamp": 7.506738513076973 + }, + { + "x": 1.1848319049750122, + "y": 4.428127049490159, + "heading": -1.0547000057170208, + "angularVelocity": 1.1329746119416889e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108113, + "timestamp": 7.546194897167377 + }, + { + "x": 1.1334137069719106, + "y": 4.426445003465405, + "heading": -1.0547000012467127, + "angularVelocity": 1.132974610235854e-7, + "velocityX": -1.303165487371844, + "velocityY": -0.04263051629108119, + "timestamp": 7.58565128125778 + }, + { + "x": 1.081995508968809, + "y": 4.424762957440652, + "heading": -1.0546999967764046, + "angularVelocity": 1.132974612359749e-7, + "velocityX": -1.3031654873718441, + "velocityY": -0.04263051629108153, + "timestamp": 7.6251076653481835 + }, + { + "x": 1.0305773109657206, + "y": 4.423080911415498, + "heading": -1.0546999923060965, + "angularVelocity": 1.1329746154357785e-7, + "velocityX": -1.3031654873715115, + "velocityY": -0.04263051630124593, + "timestamp": 7.664564049438587 + }, + { + "x": 0.9791591134170966, + "y": 4.421398851501261, + "heading": -1.0546999878355032, + "angularVelocity": 1.1330468776623308e-7, + "velocityX": -1.3031654758533633, + "velocityY": -0.04263086831227212, + "timestamp": 7.70402043352899 + }, + { + "x": 0.9320637074618211, + "y": 4.419851540523593, + "heading": -1.0424883288696292, + "angularVelocity": 0.3094976705897423, + "velocityX": -1.1936067392128267, + "velocityY": -0.03921573183500874, + "timestamp": 7.743476817619394 + }, + { + "x": 0.8917002364780792, + "y": 4.4185265531626925, + "heading": -1.0318261905546944, + "angularVelocity": 0.2702259358208133, + "velocityX": -1.0229896102810612, + "velocityY": -0.033581064039327264, + "timestamp": 7.782933201709797 + }, + { + "x": 0.8580655701196472, + "y": 4.4174228713893, + "heading": -1.0228692034037898, + "angularVelocity": 0.22700983268973754, + "velocityX": -0.8524518182245786, + "velocityY": -0.027972197626213308, + "timestamp": 7.822389585800201 + }, + { + "x": 0.8311586102834976, + "y": 4.416540158262108, + "heading": -1.0156685912652759, + "angularVelocity": 0.1824954897543525, + "velocityX": -0.6819418569755298, + "velocityY": -0.022371870802177205, + "timestamp": 7.861845969890604 + }, + { + "x": 0.8109787988166595, + "y": 4.41587824490326, + "heading": -1.0102497931206222, + "angularVelocity": 0.13733641005313718, + "velocityX": -0.511446041801538, + "velocityY": -0.016775824093046718, + "timestamp": 7.901302353981007 + }, + { + "x": 0.797525797662573, + "y": 4.415437029865442, + "heading": -1.0066280322412158, + "angularVelocity": 0.09179150504790234, + "velocityX": -0.34095879448209143, + "velocityY": -0.011182348509336195, + "timestamp": 7.940758738071411 }, { "x": 0.7907993793487549, "y": 4.415216445922852, "heading": -1.0048134836944498, - "angularVelocity": 0.4030946173477082, - "velocityX": -1.302413852119022, - "velocityY": 0.11904860830311666, - "timestamp": 3.351601422630831 + "angularVelocity": 0.04598871864711492, + "velocityX": -0.17047731232558744, + "velocityY": -0.0055905767260737965, + "timestamp": 7.980215122161814 }, { "x": 0.7907993793487549, "y": 4.415216445922852, "heading": -1.0048134836944498, - "angularVelocity": -9.422070435540628e-32, - "velocityX": 1.9369819218763315e-32, - "velocityY": -1.162688749272308e-31, - "timestamp": 3.4297275902633078 + "angularVelocity": 2.6076874199065633e-34, + "velocityX": 4.4246114702257314e-36, + "velocityY": -1.2923319814607663e-34, + "timestamp": 8.019671506252218 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj new file mode 100644 index 00000000..9ee66c72 --- /dev/null +++ b/src/main/deploy/choreo/test.traj @@ -0,0 +1,1084 @@ +{ + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto index 67fcef43..e486ea19 100644 --- a/src/main/deploy/pathplanner/autos/local 4.auto +++ b/src/main/deploy/pathplanner/autos/local 4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7470832467079163, - "y": 6.625458717346191 + "x": 0.7470832465334709, + "y": 6.625458718233556 }, - "rotation": 60.53273310724105 + "rotation": 60.53273310463171 }, "command": { "type": "sequential", @@ -18,7 +18,7 @@ } }, { - "type": "parallel", + "type": "race", "data": { "commands": [ { @@ -43,7 +43,7 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { @@ -68,7 +68,7 @@ } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/autos/triangle test.auto b/src/main/deploy/pathplanner/autos/triangle test.auto new file mode 100644 index 00000000..afbf7a14 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/triangle test.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.1199185848236084, + "y": 5.957952976226807 + }, + "rotation": 2.7794689858351906e-30 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df00d051..9f7a4133 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.PowerDistribution; @@ -101,9 +102,9 @@ public void robotInit() { swerve.runVelocityFieldRelative( () -> new ChassisSpeeds( - controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, - controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, - controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); + -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED, + -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED, + -controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); shooter.setDefaultCommand(shooter.run(0.0)); pivot.setDefaultCommand(pivot.run(0.0)); @@ -119,15 +120,27 @@ public void robotInit() { Commands.parallel( shooter.run(-10.0), pivot.run(-15.0), - Commands.waitSeconds(0.5).andThen(kicker.run(-25.0)))); + Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360)))); + controller + .leftBumper() + .whileTrue( + Commands.parallel( + pivot.run(10.0), + shooter.run(-2.0), + Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360)))); + controller + .a() + .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here NamedCommands.registerCommand( - "fender", - Commands.deadline( - Commands.sequence( - Commands.print("fender shot"), Commands.waitSeconds(1.0), Commands.print("pew!")), - swerve.stopCmd())); - NamedCommands.registerCommand("intake", Commands.print("intake")); + "fender", Commands.none() + // Commands.race(Commands.waitSeconds(1.0), swerve.stopWithXCmd(), pivot.run(0)) + // .asProxy() + // .withTimeout(2.0) + ); + NamedCommands.registerCommand( + "intake", + Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(100.0)).asProxy()); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java index ee82c6f2..eea9d62c 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java @@ -19,14 +19,13 @@ public class KickerIOReal implements KickerIO { public KickerIOReal() { TalonFXConfiguration kickerConfig = new TalonFXConfiguration(); - kickerConfig.Feedback.SensorToMechanismRatio = 125 / 1; - kickerConfig.Slot0.kP = 10.0; + kickerConfig.Slot0.kP = 50.0; kickerConfig.Slot0.kD = 0.0; kickerConfig.Slot0.kI = 0.0; kickerConfig.CurrentLimits.StatorCurrentLimit = 20.0; kickerConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerConfig.Feedback.SensorToMechanismRatio = 125.0; + kickerConfig.Feedback.SensorToMechanismRatio = 1.0; kickerMotor.getConfigurator().apply(kickerConfig); } diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java index 2ff39463..e8baccde 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.kicker; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -33,6 +34,7 @@ public Command reset() { public RunCommand run(double degrees) { return new RunCommand( () -> { + Logger.recordOutput("Kicker/Target", Units.degreesToRotations(degrees)); io.setPosition(degrees); }, this); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 963f61e0..eff5a551 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -25,6 +25,10 @@ public ShooterIOReal() { shooterConfig.Slot0.kD = 0; shooterConfig.Slot0.kI = 0; shooterConfig.Slot0.kV = 0; + + shooterConfig.CurrentLimits.StatorCurrentLimit = 100.0; + shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; + shooterMotor.getConfigurator().apply(shooterConfig); } public void setVelocity(double velocity) { diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java index e5e1c5cc..cde771ca 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/Module.java @@ -31,10 +31,11 @@ public record ModuleConstants( // Gear ratios for SDS MK3 Fast, adjust as necessary // These numbers are taken from SDS's website // They are the gear tooth counts for each stage of the modules' gearboxes - public static final double DRIVE_GEAR_RATIO = (48.0 / 16.0) * (16.0 / 28.0) * (60.0 / 15.0); + public static final double DRIVE_GEAR_RATIO = + 8.16; // (48.0 / 16.0) * (16.0 / 28.0) * (60.0 / 15.0); public static final double TURN_GEAR_RATIO = 12.8 / 1.0; - public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed + public static final double DRIVE_STATOR_CURRENT_LIMIT = 50.0; // TODO bump as needed public static final double TURN_STATOR_CURRENT_LIMIT = 40.0; private final ModuleIO io; diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java index 09ee3bd0..3c178e0c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOTalonFX.java @@ -86,7 +86,7 @@ public ModuleIOTalonFX(ModuleConstants constants) { driveConfig.CurrentLimits.StatorCurrentLimit = Module.DRIVE_STATOR_CURRENT_LIMIT; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Inverts - driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; // Sensor // Meters per second @@ -186,8 +186,8 @@ public void updateInputs(ModuleIOInputs inputs) { turnAppliedVolts, turnCurrent); - inputs.drivePositionMeters = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityMetersPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.drivePositionMeters = drivePosition.getValueAsDouble(); + inputs.driveVelocityMetersPerSec = driveVelocity.getValueAsDouble(); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2b93fe9f..73f45e63 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -22,9 +22,9 @@ 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; @@ -67,6 +67,7 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); + private SwerveDriveOdometry odometry; public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { this.gyroIO = gyroIO; @@ -86,16 +87,28 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { // your Constants class new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - 4.5, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + MAX_LINEAR_SPEED, // Max module speed, in m/s + DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options // here ), - () -> true, + () -> false, this // Reference to this subsystem to set requirements ); PathPlannerLogging.setLogTargetPoseCallback( - (pose) -> Logger.recordOutput("PathPlanner/Target", pose)); + (pose) -> { + Logger.recordOutput("PathPlanner/Target", pose); + Logger.recordOutput( + "PathPlanner/Absolute Translation Error", + pose.minus(getPose()).getTranslation().getNorm()); + }); + PathPlannerLogging.setLogActivePathCallback( + (path) -> Logger.recordOutput("PathPlanner/Active Path", path.toArray(Pose2d[]::new))); + Logger.recordOutput("PathPlanner/Target", new Pose2d()); + Logger.recordOutput("PathPlanner/Absolute Translation Error", 0.0); + + odometry = new SwerveDriveOdometry(kinematics, getRotation(), getModulePositions()); } /** @@ -151,34 +164,36 @@ public void periodic() { } // Update odometry - int deltaCount = - Math.min( - gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE, - Arrays.stream(modules) - .map((m) -> m.getPositionDeltas().length) - .min(Integer::compare) - .get()); - for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { - // Read wheel deltas from each module - SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; - } - - // The twist represents the motion of the robot since the last - // sample in x, y, and theta based only on the modules, without - // the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); - if (gyroInputs.connected) { - // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last sample. - Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; - twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians()); - lastGyroRotation = gyroRotation; - } - // Apply the twist (change since last sample) to the current pose - pose = pose.exp(twist); - } + // int deltaCount = + // Math.min( + // gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE, + // Arrays.stream(modules) + // .map((m) -> m.getPositionDeltas().length) + // .min(Integer::compare) + // .get()); + // for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { + // // Read wheel deltas from each module + // SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; + // for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + // wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; + // } + + // // The twist represents the motion of the robot since the last + // // sample in x, y, and theta based only on the modules, without + // // the gyro. The gyro is always disconnected in simulation. + // var twist = kinematics.toTwist2d(wheelDeltas); + // if (gyroInputs.connected) { + // // If the gyro is connected, replace the theta component of the twist + // // with the change in angle since the last sample. + // Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; + // twist = new Twist2d(twist.dx, twist.dy, + // gyroRotation.minus(lastGyroRotation).getRadians()); + // lastGyroRotation = gyroRotation; + // } + // Apply the twist (change since last sample) to the current pose + // pose = pose.exp(twist); + // } + pose = odometry.update(gyroInputs.yawPosition, getModulePositions()); } private void runVelocity(ChassisSpeeds speeds) { @@ -187,6 +202,9 @@ private void runVelocity(ChassisSpeeds speeds) { SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + Logger.recordOutput("Swerve/Target Speeds", discreteSpeeds); + Logger.recordOutput("Swerve/Speed Error", discreteSpeeds.minus(getVelocity())); + // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = Streams.zip( @@ -247,6 +265,16 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + private SwerveModulePosition[] getModulePositions() { + + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getPosition(); + } + + return positions; + } + /** Returns the module states (turn angles and drive velocitoes) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -288,10 +316,12 @@ public Rotation2d getRotation() { /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { this.pose = pose; + odometry.resetPosition(gyroInputs.yawPosition, getModulePositions(), pose); } public void setYaw(Rotation2d yaw) { gyroIO.setYaw(yaw); + setPose(new Pose2d(getPose().getTranslation(), yaw)); } /** Returns an array of module translations. */ From f7d98b1c6f4fa9f4c0a895960dc342b45cf35c85 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 10 Jan 2024 17:53:39 -0800 Subject: [PATCH 09/19] more auto debugging --- .vscode/settings.json | 2 +- .../deploy/pathplanner/autos/commandless.auto | 37 +++++++++++++++++++ src/main/deploy/pathplanner/autos/single.auto | 25 +++++++++++++ src/main/java/frc/robot/Robot.java | 19 ++++++---- .../subsystems/swerve/SwerveSubsystem.java | 4 +- 5 files changed, 77 insertions(+), 10 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/commandless.auto create mode 100644 src/main/deploy/pathplanner/autos/single.auto diff --git a/.vscode/settings.json b/.vscode/settings.json index dfce6909..6e590aac 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,5 +27,5 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "java.compile.nullAnalysis.mode": "automatic", - "wpilib.autoStartRioLog": false + "wpilib.autoStartRioLog": true } diff --git a/src/main/deploy/pathplanner/autos/commandless.auto b/src/main/deploy/pathplanner/autos/commandless.auto new file mode 100644 index 00000000..1b28b59b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/commandless.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7470832465334709, + "y": 6.625458718233556 + }, + "rotation": 60.53273310463171 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 1" + } + }, + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 2" + } + }, + { + "type": "path", + "data": { + "pathName": "amp 4 local sgmt 3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/single.auto b/src/main/deploy/pathplanner/autos/single.auto new file mode 100644 index 00000000..e800b224 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/single.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7470832467079163, + "y": 6.625458717346191 + }, + "rotation": 60.53273310724105 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "amp 4 local" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9f7a4133..afeb5469 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,8 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -133,14 +133,17 @@ public void robotInit() { .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here NamedCommands.registerCommand( - "fender", Commands.none() - // Commands.race(Commands.waitSeconds(1.0), swerve.stopWithXCmd(), pivot.run(0)) - // .asProxy() - // .withTimeout(2.0) + "fender", Commands.print("pew") + // Commands.parallel( + // swerve.stopCmd(), + // shooter.run(-10.0), + // pivot.run(-30.0), + // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + // .withTimeout(1.5) + // .andThen(Commands.print("done shooting")) ); NamedCommands.registerCommand( - "intake", - Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(100.0)).asProxy()); + "intake", Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -160,7 +163,7 @@ public void disabledExit() {} @Override public void autonomousInit() { - autonomousCommand = new PathPlannerAuto("local 4"); + autonomousCommand = AutoBuilder.buildAuto("local 4"); if (autonomousCommand != null) { autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 73f45e63..a2bb02bb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -90,12 +90,14 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) { MAX_LINEAR_SPEED, // Max module speed, in m/s DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to // furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options + new ReplanningConfig( + false, false) // Default path replanning config. See the API for the options // here ), () -> false, this // Reference to this subsystem to set requirements ); + PathPlannerLogging.setLogTargetPoseCallback( (pose) -> { Logger.recordOutput("PathPlanner/Target", pose); From fe8dfd72a4eaae497a09b3b33f0bd2f7b374a462 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 10 Jan 2024 18:12:46 -0800 Subject: [PATCH 10/19] auto works, waiting on plate repair --- choreo.chor | 16 +- .../deploy/choreo/amp 4 local sgmt 2.traj | 42 +- .../deploy/choreo/amp 4 local sgmt 3.traj | 50 +- src/main/deploy/choreo/amp 4 local.traj | 2915 +++++------------ src/main/java/frc/robot/Robot.java | 90 +- .../subsystems/swerve/SwerveSubsystem.java | 2 +- vendordeps/ChoreoLib.json | 43 + 7 files changed, 929 insertions(+), 2229 deletions(-) create mode 100644 vendordeps/ChoreoLib.json diff --git a/choreo.chor b/choreo.chor index fc912d34..4f09fee3 100644 --- a/choreo.chor +++ b/choreo.chor @@ -852,35 +852,35 @@ "first" ], "type": "StopPoint", - "uuid": "7bca14ee-a4d0-4baa-be1f-60ad68299d67" + "uuid": "ce158810-b71e-4150-8960-1f702cb58f8b" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "1379bd6e-531b-4e56-9d4c-3c18bbd998e5" + "uuid": "2e31556e-985d-4335-80eb-6bd55dee43a9" }, { "scope": [ 3 ], "type": "StopPoint", - "uuid": "2791092d-310f-40e0-8550-a29b6dbd16cd" + "uuid": "b3fbeb72-3318-42db-a9ff-31d7d4d299e1" }, { "scope": [ 5 ], "type": "StopPoint", - "uuid": "1a44440f-9b6c-4036-96ad-1df3035de21c" + "uuid": "7c8e9570-1690-46b0-9b6c-ed87935178fe" }, { "scope": [ 6 ], "type": "WptVelocityDirection", - "uuid": "b403510d-5383-4de4-b88e-ab6ebe0ef535", + "uuid": "d09e9560-c87d-48d3-b775-9c5bf170fe8e", "direction": -1 }, { @@ -889,7 +889,7 @@ 2 ], "type": "ZeroAngularVelocity", - "uuid": "19d90aa7-0ff4-4439-9ef6-74adff29337b" + "uuid": "eae272b7-8811-4fc6-b7bc-f0ddad8551be" } ], "usesControlIntervalGuessing": true, @@ -2033,14 +2033,14 @@ "first" ], "type": "StopPoint", - "uuid": "a518fb1e-39f4-45cd-8a81-de94f49f26ef" + "uuid": "fbed2953-b55c-457e-8134-9066a5dd1534" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "0a3f85ed-c0e4-4302-83c8-516bcd4bb4cb" + "uuid": "09730991-f6ed-48ac-a05f-16390ee0b1ab" } ], "usesControlIntervalGuessing": false, diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj index 87607fce..ad2ef04b 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 2.traj @@ -7,7 +7,7 @@ "angularVelocity": -8.20711890061584e-12, "velocityX": -2.918186949691226e-10, "velocityY": -8.516677331367793e-11, - "timestamp": 4.176746950310032 + "timestamp": 0 }, { "x": 1.4254749911225055, @@ -16,7 +16,7 @@ "angularVelocity": -0.03818204740477268, "velocityX": 0.41634256351427257, "velocityY": -0.015487297221703413, - "timestamp": 4.281492632082638 + "timestamp": 0.10474568177260579 }, { "x": 1.5054277602383004, @@ -25,7 +25,7 @@ "angularVelocity": -0.06631224378557163, "velocityX": 0.763303725485265, "velocityY": -0.030711609603001202, - "timestamp": 4.386238313855244 + "timestamp": 0.20949136354521158 }, { "x": 1.614706352195534, @@ -34,7 +34,7 @@ "angularVelocity": -0.08322684370600701, "velocityX": 1.0432753896642522, "velocityY": -0.045042652489690874, - "timestamp": 4.4909839956278494 + "timestamp": 0.3142370453178174 }, { "x": 1.7468754648697162, @@ -43,7 +43,7 @@ "angularVelocity": -0.08740652985101428, "velocityX": 1.2618096559898304, "velocityY": -0.05733479469253905, - "timestamp": 4.595729677400455 + "timestamp": 0.41898272709042317 }, { "x": 1.8974421226741423, @@ -52,7 +52,7 @@ "angularVelocity": -0.07841370510897432, "velocityX": 1.43744978567254, "velocityY": -0.06608562544064354, - "timestamp": 4.700475359173061 + "timestamp": 0.523728408863029 }, { "x": 2.0478771487383725, @@ -61,7 +61,7 @@ "angularVelocity": -0.07075498604111852, "velocityX": 1.436193106275662, "velocityY": -0.06487716815402857, - "timestamp": 4.805221040945667 + "timestamp": 0.6284740906356348 }, { "x": 2.1991998312667134, @@ -70,7 +70,7 @@ "angularVelocity": -0.04317608251475354, "velocityX": 1.4446675030153622, "velocityY": -0.06417008055763802, - "timestamp": 4.909966722718273 + "timestamp": 0.7332197724082405 }, { "x": 2.337086103631991, @@ -79,7 +79,7 @@ "angularVelocity": -0.04962641198543019, "velocityX": 1.316390996187784, "velocityY": -0.007313001468104138, - "timestamp": 5.014712404490878 + "timestamp": 0.8379654541808463 }, { "x": 2.45341531313919, @@ -88,7 +88,7 @@ "angularVelocity": -0.06298504924092899, "velocityX": 1.1105871626237722, "velocityY": -0.0003703621622037101, - "timestamp": 5.119458086263484 + "timestamp": 0.9427111359534521 }, { "x": 2.541111145262716, @@ -97,7 +97,7 @@ "angularVelocity": -0.07311289048562652, "velocityX": 0.8372262287970834, "velocityY": -0.008339593435811936, - "timestamp": 5.22420376803609 + "timestamp": 1.047456817726058 }, { "x": 2.592916727068705, @@ -106,7 +106,7 @@ "angularVelocity": -0.07765316033563659, "velocityX": 0.4945844156915881, "velocityY": -0.022868582319897673, - "timestamp": 5.328949449808696 + "timestamp": 1.1522024994986637 }, { "x": 2.573983182200651, @@ -115,7 +115,7 @@ "angularVelocity": -0.07029420270393813, "velocityX": -0.14547032678251723, "velocityY": -0.03872138453206129, - "timestamp": 5.459103449457666 + "timestamp": 1.2823564991476335 }, { "x": 2.4879155076147748, @@ -124,7 +124,7 @@ "angularVelocity": -0.05584378637679435, "velocityX": -0.6612756796636089, "velocityY": -0.04951457856894505, - "timestamp": 5.589257449106635 + "timestamp": 1.4125104987966033 }, { "x": 2.35046961428428, @@ -133,7 +133,7 @@ "angularVelocity": -0.02375059815495546, "velocityX": -1.056025121733672, "velocityY": -0.046833099245860266, - "timestamp": 5.719411448755605 + "timestamp": 1.542664498445573 }, { "x": 2.1764781877651598, @@ -142,7 +142,7 @@ "angularVelocity": 0.05771615319957162, "velocityX": -1.336811984121226, "velocityY": -0.0033152797571240995, - "timestamp": 5.849565448404575 + "timestamp": 1.6728184980945429 }, { "x": 1.9752868983122995, @@ -151,7 +151,7 @@ "angularVelocity": 0.174948516049147, "velocityX": -1.5457941360881122, "velocityY": 0.07881309585526655, - "timestamp": 5.979719448053545 + "timestamp": 1.8029724977435126 }, { "x": 1.7725080662079153, @@ -160,7 +160,7 @@ "angularVelocity": 0.11035868346800264, "velocityX": -1.5579915534229911, "velocityY": 0.07689970624561784, - "timestamp": 6.1098734477025145 + "timestamp": 1.9331264973924824 }, { "x": 1.5970859275969194, @@ -169,7 +169,7 @@ "angularVelocity": 0.11248892189634652, "velocityX": -1.347804439948399, "velocityY": 0.07899436394267238, - "timestamp": 6.240027447351484 + "timestamp": 2.063280497041452 }, { "x": 1.463998994938607, @@ -178,7 +178,7 @@ "angularVelocity": 0.0889948258110792, "velocityX": -1.0225343286428261, "velocityY": 0.06298609118419185, - "timestamp": 6.370181447000454 + "timestamp": 2.193434496690422 }, { "x": 1.3892531402609951, @@ -187,7 +187,7 @@ "angularVelocity": 0.049429229542960515, "velocityX": -0.5742878042328909, "velocityY": 0.03506906174326123, - "timestamp": 6.500335446649424 + "timestamp": 2.323588496339392 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj index db814883..104394ca 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 3.traj @@ -7,7 +7,7 @@ "angularVelocity": 5.018347137854837e-11, "velocityX": -8.981333628450582e-10, "velocityY": 1.1863234183511445e-9, - "timestamp": 6.630489446298394 + "timestamp": 0 }, { "x": 1.410785208000178, @@ -16,7 +16,7 @@ "angularVelocity": -0.12074981514388294, "velocityX": 0.21418676347263918, "velocityY": -0.1962009963268651, - "timestamp": 6.731018848208977 + "timestamp": 0.1005294019105829 }, { "x": 1.4512665581597533, @@ -25,7 +25,7 @@ "angularVelocity": -0.2282188250282345, "velocityX": 0.4026816906907998, "velocityY": -0.36885249450974994, - "timestamp": 6.8315482501195595 + "timestamp": 0.2010588038211658 }, { "x": 1.5081514675148215, @@ -34,7 +34,7 @@ "angularVelocity": -0.3226138782330449, "velocityX": 0.5658534482437384, "velocityY": -0.5179951130124598, - "timestamp": 6.932077652030142 + "timestamp": 0.3015882057317487 }, { "x": 1.578941487701644, @@ -43,7 +43,7 @@ "angularVelocity": -0.4039659933419245, "velocityX": 0.7041722939494359, "velocityY": -0.6435568078574709, - "timestamp": 7.032607053940725 + "timestamp": 0.4021176076423316 }, { "x": 1.6611946188865518, @@ -52,7 +52,7 @@ "angularVelocity": -0.4715405656932785, "velocityX": 0.8181997404499256, "velocityY": -0.7452396506196601, - "timestamp": 7.133136455851308 + "timestamp": 0.5026470095529145 }, { "x": 1.7524893345353523, @@ -61,7 +61,7 @@ "angularVelocity": -0.521676197753198, "velocityX": 0.9081394425873396, "velocityY": -0.8223581237255294, - "timestamp": 7.233665857761891 + "timestamp": 0.6031764114634974 }, { "x": 1.8499500532222615, @@ -70,7 +70,7 @@ "angularVelocity": -0.5426683689609221, "velocityX": 0.9694747626122975, "velocityY": -0.8746392520744162, - "timestamp": 7.334195259672474 + "timestamp": 0.7037058133740803 }, { "x": 1.9473638091809122, @@ -79,7 +79,7 @@ "angularVelocity": -0.5453636534436577, "velocityX": 0.9690076084526664, "velocityY": -0.8789016988127862, - "timestamp": 7.434724661583057 + "timestamp": 0.8042352152846632 }, { "x": 2.05818821772957, @@ -88,7 +88,7 @@ "angularVelocity": -0.6527316496725523, "velocityX": 1.1024079106043232, "velocityY": -1.2360033993617228, - "timestamp": 7.53525406349364 + "timestamp": 0.9047646171952461 }, { "x": 2.1664425710584037, @@ -97,7 +97,7 @@ "angularVelocity": -0.5838740711290391, "velocityX": 1.076842701113489, "velocityY": -1.2194448247922516, - "timestamp": 7.635783465404223 + "timestamp": 1.005294019105829 }, { "x": 2.2666201121554423, @@ -106,7 +106,7 @@ "angularVelocity": -0.557726794034664, "velocityX": 0.996499915039965, "velocityY": -1.153502450677793, - "timestamp": 7.736312867314806 + "timestamp": 1.105823421016412 }, { "x": 2.356480498307365, @@ -115,7 +115,7 @@ "angularVelocity": -0.5201316047807618, "velocityX": 0.8938716814183492, "velocityY": -1.0690974663909212, - "timestamp": 7.8368422692253885 + "timestamp": 1.2063528229269949 }, { "x": 2.433798199341425, @@ -124,7 +124,7 @@ "angularVelocity": -0.47501310796660834, "velocityX": 0.7691053456052521, "velocityY": -0.9659173928845837, - "timestamp": 7.937371671135971 + "timestamp": 1.3068822248375778 }, { "x": 2.4961951429135376, @@ -133,7 +133,7 @@ "angularVelocity": -0.4225460918867537, "velocityX": 0.620683518971687, "velocityY": -0.8422905312450144, - "timestamp": 8.037901073046553 + "timestamp": 1.4074116267481598 }, { "x": 2.541198492257727, @@ -142,7 +142,7 @@ "angularVelocity": -0.3625720221944257, "velocityX": 0.4476635477259144, "velocityY": -0.6971946668803632, - "timestamp": 8.138430474957136 + "timestamp": 1.5079410286587427 }, { "x": 2.5331758624705114, @@ -151,7 +151,7 @@ "angularVelocity": -0.2449990301925222, "velocityX": -0.052913214713167914, "velocityY": -0.426119279886695, - "timestamp": 8.290049133403663 + "timestamp": 1.6595596871052694 }, { "x": 2.46411262676954, @@ -160,7 +160,7 @@ "angularVelocity": -0.16853709734646205, "velocityX": -0.45550618406914944, "velocityY": -0.21004248027074415, - "timestamp": 8.44166779185019 + "timestamp": 1.811178345551796 }, { "x": 2.348779581051416, @@ -169,7 +169,7 @@ "angularVelocity": -0.121314567375086, "velocityX": -0.7606784530447123, "velocityY": -0.04726417363495295, - "timestamp": 8.593286450296716 + "timestamp": 1.9627970039983227 }, { "x": 2.201596779448434, @@ -178,7 +178,7 @@ "angularVelocity": -0.0863946406365267, "velocityX": -0.9707433369527995, "velocityY": 0.06416850879734587, - "timestamp": 8.744905108743243 + "timestamp": 2.1144156624448494 }, { "x": 2.03723470076713, @@ -187,7 +187,7 @@ "angularVelocity": -0.11028538710126731, "velocityX": -1.0840491612778367, "velocityY": 0.13276026521150996, - "timestamp": 8.89652376718977 + "timestamp": 2.266034320891376 }, { "x": 1.8800877783569956, @@ -196,7 +196,7 @@ "angularVelocity": 0.13911819814698503, "velocityX": -1.0364616391016628, "velocityY": 0.08249012434493122, - "timestamp": 9.048142425636296 + "timestamp": 2.4176529793379027 }, { "x": 1.7046442157621393, @@ -205,7 +205,7 @@ "angularVelocity": 0.03403847932720532, "velocityX": -1.1571370254440554, "velocityY": 0.24830844691554524, - "timestamp": 9.199761084082823 + "timestamp": 2.5692716377844294 }, { "x": 1.572815192920781, @@ -214,7 +214,7 @@ "angularVelocity": -0.002191897597681109, "velocityX": -0.8694775764003041, "velocityY": 0.21430393183203963, - "timestamp": 9.35137974252935 + "timestamp": 2.720890296230956 }, { "x": 1.4994455554717883, @@ -223,7 +223,7 @@ "angularVelocity": -0.00858476582372283, "velocityX": -0.4839090323218549, "velocityY": 0.13631083596916982, - "timestamp": 9.502998400975876 + "timestamp": 2.8725089546774827 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj index d949b53a..6dfc86f9 100644 --- a/src/main/deploy/choreo/amp 4 local.traj +++ b/src/main/deploy/choreo/amp 4 local.traj @@ -1,2173 +1,760 @@ { "samples": [ { - "x": 0.7470832467079163, - "y": 6.625458717346191, - "heading": 1.056495497952334, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 2.778590006377356e-31, + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, "timestamp": 0 }, { - "x": 0.7490480926070738, - "y": 6.626378384939511, - "heading": 1.0288300799302914, - "angularVelocity": -0.7154916906166144, - "velocityX": 0.05081545896286759, - "velocityY": 0.023784730837073272, - "timestamp": 0.03866630232728455 - }, - { - "x": 0.7533109049583806, - "y": 6.629023111131428, - "heading": 0.9756962016369112, - "angularVelocity": -1.3741649729945935, - "velocityX": 0.11024618581902172, - "velocityY": 0.06839873566218808, - "timestamp": 0.0773326046545691 - }, - { - "x": 0.7612619802970353, - "y": 6.634108872295573, - "heading": 0.9038556504632858, - "angularVelocity": -1.8579627957574294, - "velocityX": 0.2056331963516693, - "velocityY": 0.13152954531570296, - "timestamp": 0.11599890698185367 - }, - { - "x": 0.7743286281835381, - "y": 6.6414096178619975, - "heading": 0.8200810534595926, - "angularVelocity": -2.1666048202532666, - "velocityX": 0.3379337329933604, - "velocityY": 0.18881416445328386, - "timestamp": 0.1546652093091382 - }, - { - "x": 0.7933808300169274, - "y": 6.65119855442881, - "heading": 0.7366646644009647, - "angularVelocity": -2.1573407343826863, - "velocityX": 0.4927340005809824, - "velocityY": 0.25316453805060635, - "timestamp": 0.19333151163642276 - }, - { - "x": 0.8173469969355076, - "y": 6.663336254760928, - "heading": 0.6665724862917816, - "angularVelocity": -1.8127458249272754, - "velocityX": 0.6198205019898244, - "velocityY": 0.3139090009015357, - "timestamp": 0.2319978139637073 - }, - { - "x": 0.8463280476277008, - "y": 6.677302965953696, - "heading": 0.6104261741684412, - "angularVelocity": -1.4520734785570149, - "velocityX": 0.7495169940713614, - "velocityY": 0.3612114516290285, - "timestamp": 0.27066411629099185 - }, - { - "x": 0.8803563088296403, - "y": 6.692538823127676, - "heading": 0.5688472333280339, - "angularVelocity": -1.0753275678766911, - "velocityX": 0.8800495303098976, - "velocityY": 0.39403450179995414, - "timestamp": 0.3093304186182764 - }, - { - "x": 0.9194008243749598, - "y": 6.708497582198662, - "heading": 0.5422592705741602, - "angularVelocity": -0.687626205598872, - "velocityX": 1.0097814684952597, - "velocityY": 0.41273041667921895, - "timestamp": 0.347996720945561 - }, - { - "x": 0.9633926969788928, - "y": 6.724681738794303, - "heading": 0.5307405695170507, - "angularVelocity": -0.2979002481174931, - "velocityX": 1.1377315635609189, - "velocityY": 0.4185597179335494, - "timestamp": 0.3866630232728456 - }, - { - "x": 1.0120758029948176, - "y": 6.737784321546207, - "heading": 0.5307403964315477, - "angularVelocity": -0.00000447639139680952, - "velocityX": 1.2590577087992194, - "velocityY": 0.33886309171704404, - "timestamp": 0.42532932560013015 - }, - { - "x": 1.0608121926714902, - "y": 6.750687310320991, - "heading": 0.5307402334549555, - "angularVelocity": -0.000004214951581791169, - "velocityX": 1.2604357474927872, - "velocityY": 0.33370112987702966, - "timestamp": 0.4639956279274147 - }, - { - "x": 1.1095485839791375, - "y": 6.763590292935407, - "heading": 0.5307400704784038, - "angularVelocity": -0.0000042149505359662004, - "velocityX": 1.2604357896735676, - "velocityY": 0.33370097055568565, - "timestamp": 0.5026619302546993 - }, - { - "x": 1.1582849752868287, - "y": 6.7764932755496385, - "heading": 0.5307399075018228, - "angularVelocity": -0.000004214951290774306, - "velocityX": 1.2604357896747036, - "velocityY": 0.33370097055091935, - "timestamp": 0.5413282325819838 - }, - { - "x": 1.2070213665945133, - "y": 6.789396258163877, - "heading": 0.5307397445252127, - "angularVelocity": -0.000004214952045657554, - "velocityX": 1.2604357896745395, - "velocityY": 0.33370097055106207, - "timestamp": 0.5799945349092683 - }, - { - "x": 1.255757757902192, - "y": 6.80229924077812, - "heading": 0.5307395815485734, - "angularVelocity": -0.000004214952801397565, - "velocityX": 1.2604357896743759, - "velocityY": 0.3337009705512057, - "timestamp": 0.6186608372365529 - }, - { - "x": 1.304494149209864, - "y": 6.815202223392369, - "heading": 0.5307394185719048, - "angularVelocity": -0.0000042149535559010885, - "velocityX": 1.2604357896742118, - "velocityY": 0.3337009705513488, - "timestamp": 0.6573271395638374 - }, - { - "x": 1.35323054051753, - "y": 6.828105206006622, - "heading": 0.5307392555952071, - "angularVelocity": -0.000004214954311531878, - "velocityX": 1.2604357896740477, - "velocityY": 0.333700970551492, - "timestamp": 0.6959934418911219 - }, - { - "x": 1.4019669318251895, - "y": 6.841008188620883, - "heading": 0.5307390926184801, - "angularVelocity": -0.000004214955066684376, - "velocityX": 1.260435789673884, - "velocityY": 0.3337009705516355, - "timestamp": 0.7346597442184064 - }, - { - "x": 1.4507033231328428, - "y": 6.853911171235148, - "heading": 0.530738929641724, - "angularVelocity": -0.000004214955821682156, - "velocityX": 1.26043578967372, - "velocityY": 0.3337009705517787, - "timestamp": 0.7733260465456909 - }, - { - "x": 1.4994397144404896, - "y": 6.866814153849419, - "heading": 0.5307387666649387, - "angularVelocity": -0.000004214956576941562, - "velocityX": 1.2604357896735556, - "velocityY": 0.3337009705519219, - "timestamp": 0.8119923488729754 - }, - { - "x": 1.5481761057481302, - "y": 6.879717136463695, - "heading": 0.5307386036881241, - "angularVelocity": -0.000004214957331767799, - "velocityX": 1.260435789673392, - "velocityY": 0.3337009705520652, - "timestamp": 0.85065865120026 - }, - { - "x": 1.5969124970557644, - "y": 6.892620119077977, - "heading": 0.5307384407112804, - "angularVelocity": -0.000004214958086684387, - "velocityX": 1.260435789673228, - "velocityY": 0.3337009705522084, - "timestamp": 0.8893249535275445 - }, - { - "x": 1.6456488883633922, - "y": 6.905523101692264, - "heading": 0.5307382777344075, - "angularVelocity": -0.000004214958842439668, - "velocityX": 1.2604357896730638, - "velocityY": 0.3337009705523516, - "timestamp": 0.927991255854829 - }, - { - "x": 1.6943852796710137, - "y": 6.9184260843065575, - "heading": 0.5307381147575053, - "angularVelocity": -0.000004214959597145177, - "velocityX": 1.2604357896729, - "velocityY": 0.33370097055249487, - "timestamp": 0.9666575581821135 - }, - { - "x": 1.7431216709786288, - "y": 6.931329066920855, - "heading": 0.530737951780574, - "angularVelocity": -0.000004214960352296469, - "velocityX": 1.2604357896727358, - "velocityY": 0.3337009705526381, - "timestamp": 1.0053238605093981 - }, - { - "x": 1.7918580622862375, - "y": 6.9442320495351595, - "heading": 0.5307377888036136, - "angularVelocity": -0.000004214961107568621, - "velocityX": 1.2604357896725717, - "velocityY": 0.33370097055278125, - "timestamp": 1.0439901628366828 - }, - { - "x": 1.8405944535938399, - "y": 6.957135032149469, - "heading": 0.5307376258266238, - "angularVelocity": -0.000004214961862680116, - "velocityX": 1.2604357896724079, - "velocityY": 0.33370097055292464, - "timestamp": 1.0826564651639674 - }, - { - "x": 1.889330844901439, - "y": 6.970038014763773, - "heading": 0.5307374628496049, - "angularVelocity": -0.000004214962617743034, - "velocityX": 1.260435789672316, - "velocityY": 0.3337009705527953, - "timestamp": 1.121322767491252 - }, - { - "x": 1.9380672362995741, - "y": 6.982940997036096, - "heading": 0.5307372998725607, - "angularVelocity": -0.00000421496327361953, - "velocityX": 1.260435792013792, - "velocityY": 0.3337009617083353, - "timestamp": 1.1599890698185367 - }, - { - "x": 1.986806564448301, - "y": 6.995832881166175, - "heading": 0.5307371364965603, - "angularVelocity": -0.000004225281203502036, - "velocityX": 1.2605117431752726, - "velocityY": 0.33341393808569914, - "timestamp": 1.1986553721458213 - }, - { - "x": 2.0317973138853715, - "y": 7.00490530051565, - "heading": 0.5171429760348675, - "angularVelocity": -0.3515764281428815, - "velocityX": 1.1635648285228963, - "velocityY": 0.23463374575309978, - "timestamp": 1.237321674473106 - }, - { - "x": 2.0728058947189574, - "y": 7.0102031606644015, - "heading": 0.4906124312582534, - "angularVelocity": -0.6861412439204636, - "velocityX": 1.0605767390546776, - "velocityY": 0.13701491556938833, - "timestamp": 1.2759879768003906 - }, - { - "x": 2.110279120553285, - "y": 7.0109088277839495, - "heading": 0.45382754800463343, - "angularVelocity": -0.9513421516826277, - "velocityX": 0.9691442827175547, - "velocityY": 0.01825018367609023, - "timestamp": 1.3146542791276752 - }, - { - "x": 2.1444753266925427, - "y": 7.0064516078188515, - "heading": 0.4096540058788496, - "angularVelocity": -1.1424299575353483, - "velocityX": 0.8843929747871832, - "velocityY": -0.11527401630940219, - "timestamp": 1.3533205814549598 - }, - { - "x": 2.175331590133971, - "y": 6.996634649413561, - "heading": 0.3604548445442151, - "angularVelocity": -1.2724040927993665, - "velocityX": 0.7980143324862465, - "velocityY": -0.2538892475985945, - "timestamp": 1.3919868837822444 - }, - { - "x": 2.202658354652599, - "y": 6.981442157383057, - "heading": 0.3084300196662748, - "angularVelocity": -1.3454822868139975, - "velocityX": 0.7067333278293896, - "velocityY": -0.3929129789011957, - "timestamp": 1.430653186109529 - }, - { - "x": 2.226219612980993, - "y": 6.960976280761605, - "heading": 0.2557056738235528, - "angularVelocity": -1.363573516713794, - "velocityX": 0.6093486294333386, - "velocityY": -0.5292948999422463, - "timestamp": 1.4693194884368137 - }, - { - "x": 2.24576982847862, - "y": 6.9354320182780045, - "heading": 0.20431672210316915, - "angularVelocity": -1.329037136404176, - "velocityX": 0.505613785671528, - "velocityY": -0.6606337028917001, - "timestamp": 1.5079857907640983 - }, - { - "x": 2.2610747814178467, - "y": 6.9050798416137695, - "heading": 0.15617120010566338, - "angularVelocity": -1.245154542836593, - "velocityX": 0.39582147808373047, - "velocityY": -0.7849774826495736, - "timestamp": 1.546652093091383 - }, - { - "x": 2.271660182847978, - "y": 6.871430398890197, - "heading": 0.11471436587906376, - "angularVelocity": -1.100460804827315, - "velocityX": 0.2809867080916391, - "velocityY": -0.8932156425445383, - "timestamp": 1.5843243427932703 - }, - { - "x": 2.277809261221615, - "y": 6.833910424701081, - "heading": 0.07939066205351579, - "angularVelocity": -0.9376584649197209, - "velocityX": 0.1632256746625241, - "velocityY": -0.9959578864023372, - "timestamp": 1.6219965924951576 - }, - { - "x": 2.2793609405244357, - "y": 6.792889887349777, - "heading": 0.05132731412073156, - "angularVelocity": -0.7449342196140949, - "velocityX": 0.041188920627471544, - "velocityY": -1.0888794185616628, - "timestamp": 1.659668842197045 - }, - { - "x": 2.27611913107799, - "y": 6.749248560060934, - "heading": 0.03326209599426278, - "angularVelocity": -0.4795364829398642, - "velocityX": -0.08605298255589845, - "velocityY": -1.158447600931467, - "timestamp": 1.6973410918989322 - }, - { - "x": 2.2678771229472154, - "y": 6.703362743956978, - "heading": 0.025221677462375882, - "angularVelocity": -0.21343080372209108, - "velocityX": -0.21878194681729676, - "velocityY": -1.2180269685793952, - "timestamp": 1.7350133416008195 - }, - { - "x": 2.2540333522614104, - "y": 6.656234544077909, - "heading": 0.0252216304618494, - "angularVelocity": -0.000001247616663515529, - "velocityX": -0.36747926644535317, - "velocityY": -1.2510057204444232, - "timestamp": 1.7726855913027069 - }, - { - "x": 2.2344053991573594, - "y": 6.61120719259268, - "heading": 0.025221637193402757, - "angularVelocity": 1.7868732048261226e-7, - "velocityX": -0.5210188735571781, - "velocityY": -1.1952392501522764, - "timestamp": 1.8103578410045942 - }, - { - "x": 2.209301413350582, - "y": 6.568987423658516, - "heading": 0.02522164328516559, - "angularVelocity": 1.617042486976944e-7, - "velocityX": -0.6663787271910937, - "velocityY": -1.1207127067872833, - "timestamp": 1.8480300907064815 - }, - { - "x": 2.1792162284525647, - "y": 6.530159590050397, - "heading": 0.025221649180709897, - "angularVelocity": 1.5649567921023276e-7, - "velocityX": -0.7986033522312032, - "velocityY": -1.0306746720829034, - "timestamp": 1.8857023404083688 - }, - { - "x": 2.1491271012258317, - "y": 6.491334811418837, - "heading": 0.025221655076174288, - "angularVelocity": 1.5649355793983052e-7, - "velocityX": -0.7987080003142876, - "velocityY": -1.0305935785304006, - "timestamp": 1.9233745901102561 - }, - { - "x": 2.119037973867113, - "y": 6.452510032889566, - "heading": 0.025221660971638672, - "angularVelocity": 1.564935577840148e-7, - "velocityX": -0.7987080038178083, - "velocityY": -1.0305935758151787, - "timestamp": 1.9610468398121434 - }, - { - "x": 2.08894884650839, - "y": 6.413685254360297, - "heading": 0.02522166686710306, - "angularVelocity": 1.5649355776196318e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.0305935758150877, - "timestamp": 1.9987190895140308 - }, - { - "x": 2.058859719149667, - "y": 6.374860475831029, - "heading": 0.025221672762567447, - "angularVelocity": 1.5649355789908895e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150877, - "timestamp": 2.036391339215918 - }, - { - "x": 2.0287705917909435, - "y": 6.33603569730176, - "heading": 0.02522167865803183, - "angularVelocity": 1.5649355774113446e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150877, - "timestamp": 2.0740635889178054 - }, - { - "x": 1.9986814644322204, - "y": 6.297210918772492, - "heading": 0.02522168455349622, - "angularVelocity": 1.5649355783955773e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.0305935758150877, - "timestamp": 2.1117358386196927 - }, - { - "x": 1.9685923370734972, - "y": 6.258386140243222, - "heading": 0.02522169044896061, - "angularVelocity": 1.5649355788803578e-7, - "velocityX": -0.7987080038179256, - "velocityY": -1.0305935758150877, - "timestamp": 2.14940808832158 - }, - { - "x": 1.9385032097147739, - "y": 6.219561361713954, - "heading": 0.025221696344425008, - "angularVelocity": 1.5649355808145546e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.0305935758150877, - "timestamp": 2.1870803380234674 - }, - { - "x": 1.9084140823560507, - "y": 6.180736583184685, - "heading": 0.0252217022398894, - "angularVelocity": 1.5649355798785538e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.0305935758150877, - "timestamp": 2.2247525877253547 - }, - { - "x": 1.8783249549973273, - "y": 6.141911804655416, - "heading": 0.025221708135353786, - "angularVelocity": 1.5649355785902612e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.030593575815088, - "timestamp": 2.262424837427242 - }, - { - "x": 1.8482358276386042, - "y": 6.103087026126148, - "heading": 0.02522171403081817, - "angularVelocity": 1.5649355767080312e-7, - "velocityX": -0.7987080038179256, - "velocityY": -1.030593575815088, - "timestamp": 2.3000970871291293 - }, - { - "x": 1.8181467002798808, - "y": 6.064262247596879, - "heading": 0.025221719926282554, - "angularVelocity": 1.5649355777865092e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150877, - "timestamp": 2.3377693368310166 - }, - { - "x": 1.7880575729211576, - "y": 6.025437469067611, - "heading": 0.02522172582174694, - "angularVelocity": 1.5649355789868648e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.030593575815088, - "timestamp": 2.375441586532904 - }, - { - "x": 1.7579684455624343, - "y": 5.986612690538342, - "heading": 0.025221731717211322, - "angularVelocity": 1.5649355766897273e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.030593575815088, - "timestamp": 2.4131138362347913 - }, - { - "x": 1.727879318203711, - "y": 5.9477879120090735, - "heading": 0.025221737612675703, - "angularVelocity": 1.5649355766619197e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150877, - "timestamp": 2.4507860859366786 - }, - { - "x": 1.6977901908449877, - "y": 5.908963133479805, - "heading": 0.025221743508140076, - "angularVelocity": 1.5649355754994554e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150875, - "timestamp": 2.488458335638566 - }, - { - "x": 1.6677010634862643, - "y": 5.870138354950536, - "heading": 0.025221749403604464, - "angularVelocity": 1.564935577800695e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.0305935758150877, - "timestamp": 2.526130585340453 - }, - { - "x": 1.6376119361275412, - "y": 5.831313576421268, - "heading": 0.025221755299068848, - "angularVelocity": 1.5649355775129745e-7, - "velocityX": -0.7987080038179257, - "velocityY": -1.0305935758150877, - "timestamp": 2.5638028350423405 - }, - { - "x": 1.6075228087688178, - "y": 5.792488797891999, - "heading": 0.025221761194533236, - "angularVelocity": 1.5649355776446108e-7, - "velocityX": -0.7987080038179256, - "velocityY": -1.030593575815088, - "timestamp": 2.601475084744228 - }, - { - "x": 1.5774336814100947, - "y": 5.753664019362731, - "heading": 0.025221767089997616, - "angularVelocity": 1.5649355770162784e-7, - "velocityX": -0.7987080038179258, - "velocityY": -1.030593575815088, - "timestamp": 2.639147334446115 - }, - { - "x": 1.5473445540513797, - "y": 5.714839240833456, - "heading": 0.025221772985461997, - "angularVelocity": 1.564935575497616e-7, - "velocityX": -0.7987080038177026, - "velocityY": -1.0305935758152607, - "timestamp": 2.6768195841480025 - }, - { - "x": 1.5172554269437706, - "y": 5.676014462109576, - "heading": 0.02522177888092892, - "angularVelocity": 1.5649362515357016e-7, - "velocityX": -0.7987079971521701, - "velocityY": -1.0305935809809978, - "timestamp": 2.71449183384989 - }, - { - "x": 1.4872404490214195, - "y": 5.6372698779844646, - "heading": 0.02551879992466325, - "angularVelocity": 0.007884345800487103, - "velocityX": -0.7967397264530625, - "velocityY": -1.0284648363638766, - "timestamp": 2.752164083551777 - }, - { - "x": 1.4608873504024076, - "y": 5.603265426982504, - "heading": 0.030651229646237198, - "angularVelocity": 0.1362390025068593, - "velocityX": -0.69953609958403, - "velocityY": -0.9026392442991357, - "timestamp": 2.7898363332536644 - }, - { - "x": 1.4383001181401514, - "y": 5.574120228098155, - "heading": 0.03673673574367204, - "angularVelocity": 0.1615381652434539, - "velocityX": -0.599572163621708, - "velocityY": -0.7736516697299336, - "timestamp": 2.8275085829555517 - }, - { - "x": 1.4194824287204242, - "y": 5.549838992967363, - "heading": 0.04263007064344353, - "angularVelocity": 0.15643703114119317, - "velocityX": -0.49951063630760206, - "velocityY": -0.6445390260188326, - "timestamp": 2.865180832657439 - }, - { - "x": 1.4044322017294746, - "y": 5.530419035921232, - "heading": 0.04779341900668404, - "angularVelocity": 0.13705972975070876, - "velocityX": -0.39950433303155547, - "velocityY": -0.5154976726849608, - "timestamp": 2.9028530823593264 - }, - { - "x": 1.3931471151595585, - "y": 5.515857373000764, - "heading": 0.05191554418164246, - "angularVelocity": 0.10942073296867788, - "velocityX": -0.29955966684281865, - "velocityY": -0.38653552776111066, - "timestamp": 2.9405253320612137 - }, - { - "x": 1.3856252119872758, - "y": 5.506151497285298, - "heading": 0.054793759356887295, - "angularVelocity": 0.07640146787149468, - "velocityX": -0.1996669493275965, - "velocityY": -0.2576399283895094, - "timestamp": 2.978197581763101 - }, - { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": 0.03960156640941446, - "velocityX": -0.09981635446971436, - "velocityY": -0.12879814897149688, - "timestamp": 3.0158698314649883 - }, - { - "x": 1.3818649053573608, - "y": 5.5012993812561035, - "heading": 0.05628563945524819, - "angularVelocity": -1.4628158600346964e-29, - "velocityX": 1.6054548136189337e-31, - "velocityY": -2.7912282615412047e-30, - "timestamp": 3.0535420811668756 - }, - { - "x": 1.3845786591625213, - "y": 5.501429733098524, - "heading": 0.05582872936035501, - "angularVelocity": -0.01824281215259517, - "velocityX": 0.10835063932598839, - "velocityY": 0.005204490339855129, - "timestamp": 3.0785881152469945 - }, - { - "x": 1.3900063822638737, - "y": 5.501690432475359, - "heading": 0.05493041969316394, - "angularVelocity": -0.035866343721856854, - "velocityX": 0.2167098824505046, - "velocityY": 0.01040880867619674, - "timestamp": 3.1036341493271133 - }, - { - "x": 1.3981483243528032, - "y": 5.502081473730299, - "heading": 0.05360938694536658, - "angularVelocity": -0.052744188703298736, - "velocityX": 0.32507909487334297, - "velocityY": 0.015612901176014845, - "timestamp": 3.128680183407232 - }, - { - "x": 1.4090047772653698, - "y": 5.502602849235542, - "heading": 0.05188855400955324, - "angularVelocity": -0.06870680325276785, - "velocityX": 0.43345995928302505, - "velocityY": 0.02081668912451226, - "timestamp": 3.153726217487351 - }, - { - "x": 1.4225760848465896, - "y": 5.5032545483338176, - "heading": 0.04979672584047775, - "angularVelocity": -0.08351933732794915, - "velocityX": 0.5418545522141726, - "velocityY": 0.026020051565707466, - "timestamp": 3.17877225156747 - }, - { - "x": 1.4388626543213987, - "y": 5.504036555432711, - "heading": 0.04737119018132143, - "angularVelocity": -0.09684310303916191, - "velocityX": 0.6502654042039197, - "velocityY": 0.031222791456693683, - "timestamp": 3.2038182856475887 - }, - { - "x": 1.4578649660996903, - "y": 5.504948846300725, - "heading": 0.044662119929733506, - "angularVelocity": -0.1081636415133266, - "velocityX": 0.7586954372700782, - "velocityY": 0.03642456386927063, - "timestamp": 3.2288643197277076 - }, - { - "x": 1.479583568415963, - "y": 5.505991380099472, - "heading": 0.04174066948746936, - "angularVelocity": -0.11664323512860124, - "velocityX": 0.8671473594104407, - "velocityY": 0.04162470574871449, - "timestamp": 3.2539103538078264 - }, - { - "x": 1.5040189973726636, - "y": 5.507164079613842, - "heading": 0.038715690708052176, - "angularVelocity": -0.12077675729898374, - "velocityX": 0.9756206862348644, - "velocityY": 0.04682176470053254, - "timestamp": 3.2789563878879453 - }, - { - "x": 1.5311713165435374, - "y": 5.508466770219786, - "heading": 0.035774832832580886, - "angularVelocity": -0.11741810563861127, - "velocityX": 1.0840965513352385, - "velocityY": 0.05201185152863736, - "timestamp": 3.304002421968064 - }, - { - "x": 1.5610369807434235, - "y": 5.509898901262541, - "heading": 0.03332144972686085, - "angularVelocity": -0.09795495358045375, - "velocityX": 1.1924308696681545, - "velocityY": 0.057179952649896545, - "timestamp": 3.329048456048183 - }, - { - "x": 1.593555387085345, - "y": 5.5114558804639, - "heading": 0.03294001206773026, - "angularVelocity": -0.015229463390271427, - "velocityX": 1.2983455279948946, - "velocityY": 0.06216470026327949, - "timestamp": 3.354094490128302 - }, - { - "x": 1.6261742711277745, - "y": 5.51302455812895, - "heading": 0.032940008328990356, - "angularVelocity": -1.4927472709571015e-7, - "velocityX": 1.3023572489794273, - "velocityY": 0.0626317787497518, - "timestamp": 3.3791405242084207 - }, - { - "x": 1.6587931551326383, - "y": 5.514593236575243, - "heading": 0.03294000459026413, - "angularVelocity": -1.4927418075968276e-7, - "velocityX": 1.302357247479563, - "velocityY": 0.062631809941977, - "timestamp": 3.4041865582885396 - }, - { - "x": 1.6914120391374976, - "y": 5.516161915021623, - "heading": 0.03294000085153791, - "angularVelocity": -1.4927418086717312e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552323, - "timestamp": 3.4292325923686584 - }, - { - "x": 1.7240309231423572, - "y": 5.517730593468005, - "heading": 0.03293999711281168, - "angularVelocity": -1.4927418091774954e-7, - "velocityX": 1.3023572474793925, - "velocityY": 0.06263180994552363, - "timestamp": 3.4542786264487773 - }, - { - "x": 1.7566498071472165, - "y": 5.519299271914385, - "heading": 0.032939993374085465, - "angularVelocity": -1.4927418089977704e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552363, - "timestamp": 3.479324660528896 - }, - { - "x": 1.789268691152076, - "y": 5.520867950360767, - "heading": 0.03293998963535924, - "angularVelocity": -1.492741809585475e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552364, - "timestamp": 3.504370694609015 - }, - { - "x": 1.8218875751569354, - "y": 5.522436628807148, - "heading": 0.03293998589663304, - "angularVelocity": -1.4927418013494575e-7, - "velocityX": 1.3023572474793925, - "velocityY": 0.06263180994552363, - "timestamp": 3.529416728689134 - }, - { - "x": 1.854506459161795, - "y": 5.524005307253529, - "heading": 0.03293998215790683, - "angularVelocity": -1.4927418024687253e-7, - "velocityX": 1.3023572474793925, - "velocityY": 0.06263180994552364, - "timestamp": 3.5544627627692527 - }, - { - "x": 1.8871253431666544, - "y": 5.52557398569991, - "heading": 0.03293997841918063, - "angularVelocity": -1.4927418036897545e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552363, - "timestamp": 3.5795087968493715 - }, - { - "x": 1.919744227171514, - "y": 5.527142664146291, - "heading": 0.03293997468045441, - "angularVelocity": -1.492741807507e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552363, - "timestamp": 3.6045548309294904 - }, - { - "x": 1.9523631111763733, - "y": 5.528711342592672, - "heading": 0.032939970941728194, - "angularVelocity": -1.4927418060860102e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552363, - "timestamp": 3.629600865009609 - }, - { - "x": 1.9849819951812326, - "y": 5.5302800210390535, - "heading": 0.03293996720300198, - "angularVelocity": -1.4927418068263622e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552363, - "timestamp": 3.654646899089728 - }, - { - "x": 2.017600879186092, - "y": 5.531848699485434, - "heading": 0.03293996346427576, - "angularVelocity": -1.492741805805833e-7, - "velocityX": 1.3023572474793927, - "velocityY": 0.06263180994552273, - "timestamp": 3.679692933169847 - }, - { - "x": 2.0502197631909613, - "y": 5.533417377931615, - "heading": 0.03293995972554955, - "angularVelocity": -1.492741805487614e-7, - "velocityX": 1.3023572474797764, - "velocityY": 0.06263180993754675, - "timestamp": 3.7047389672499658 - }, - { - "x": 2.0828386472803335, - "y": 5.534986054620662, - "heading": 0.03293995598682334, - "angularVelocity": -1.4927418043775344e-7, - "velocityX": 1.302357250853668, - "velocityY": 0.06263173978131052, - "timestamp": 3.7297850013300846 - }, - { - "x": 2.115458270958098, - "y": 5.536539275955716, - "heading": 0.03293995220626054, - "angularVelocity": -1.509445682565159e-7, - "velocityX": 1.3023867800148246, - "velocityY": 0.06201466189978957, - "timestamp": 3.7548310354102035 - }, - { - "x": 2.1462101718049076, - "y": 5.5373923966618745, - "heading": 0.02588090552477964, - "angularVelocity": -0.2818428921324181, - "velocityX": 1.2278151801765511, - "velocityY": 0.03406210753032908, - "timestamp": 3.7798770694903223 - }, - { - "x": 2.1742457798850925, - "y": 5.538117943233322, - "heading": 0.019050828435002092, - "angularVelocity": -0.2727009421129447, - "velocityX": 1.119363169054274, - "velocityY": 0.02896852128537856, - "timestamp": 3.804923103570441 - }, - { - "x": 2.199567234111099, - "y": 5.538714457409619, - "heading": 0.012652728722647373, - "angularVelocity": -0.25545360562588787, - "velocityX": 1.010996557178806, - "velocityY": 0.023816711832265754, - "timestamp": 3.82996913765056 - }, - { - "x": 2.2221755025244465, - "y": 5.539181482835929, - "heading": 0.006752605462386223, - "angularVelocity": -0.23557115834836043, - "velocityX": 0.9026685957945435, - "velocityY": 0.018646681736272153, - "timestamp": 3.855015171730679 - }, - { - "x": 2.2420711141918233, - "y": 5.539518797286996, - "heading": 0.0013831641278171489, - "angularVelocity": -0.21438289660576354, - "velocityX": 0.7943617581812826, - "velocityY": 0.013467778967862453, - "timestamp": 3.8800612058107977 - }, - { - "x": 2.2592544005969666, - "y": 5.539726269571517, - "heading": -0.0034360662843230887, - "angularVelocity": -0.19241491074908987, - "velocityX": 0.686068155547121, - "velocityY": 0.008283638192281886, - "timestamp": 3.9051072398909166 - }, - { - "x": 2.273725588479032, - "y": 5.5398038132440925, - "heading": -0.00769210681421661, - "angularVelocity": -0.16992872070189774, - "velocityX": 0.5777836058095082, - "velocityY": 0.003096045956056925, - "timestamp": 3.9301532739710354 - }, - { - "x": 2.285484842574345, - "y": 5.539751367109182, - "heading": -0.011375707178688507, - "angularVelocity": -0.1470731994007397, - "velocityX": 0.4695056334136347, - "velocityY": -0.0020939896027525593, - "timestamp": 3.9551993080511543 - }, - { - "x": 2.2945322879434182, - "y": 5.539568885584673, - "heading": -0.014479940949098854, - "angularVelocity": -0.12394113017918651, - "velocityX": 0.36123265424391854, - "velocityY": -0.007285845094921426, - "timestamp": 3.980245342131273 - }, - { - "x": 2.3008680227496536, - "y": 5.539256333392346, - "heading": -0.016999427321887948, - "angularVelocity": -0.1005942244073929, - "velocityX": 0.25296359439339106, - "velocityY": -0.012479109122301884, - "timestamp": 4.005291376211392 - }, - { - "x": 2.3044921260944244, - "y": 5.538813682391967, - "heading": -0.01892986514732486, - "angularVelocity": -0.07707558886326937, - "velocityX": 0.14469769278233818, - "velocityY": -0.017673496690129843, - "timestamp": 4.030337410291511 - }, - { - "x": 2.3054046630859375, - "y": 5.538240909576416, - "heading": -0.020267736871838693, - "angularVelocity": -0.053416509783886956, - "velocityX": 0.036434390712835496, - "velocityY": -0.022868802851364584, - "timestamp": 4.05538344437163 - }, - { - "x": 2.3035545118219214, - "y": 5.537528590892998, - "heading": -0.021011691067101653, - "angularVelocity": -0.029372452797134904, - "velocityX": -0.07304680988823148, - "velocityY": -0.028123434260638402, - "timestamp": 4.080711740936763 - }, - { - "x": 2.298931156655876, - "y": 5.536683300929956, - "heading": -0.021159077287045792, - "angularVelocity": -0.005819034043431043, - "velocityX": -0.1825371538163915, - "velocityY": -0.03337334434767621, - "timestamp": 4.106040037501895 - }, - { - "x": 2.2915343251541973, - "y": 5.535705183945715, - "heading": -0.020724840944498453, - "angularVelocity": 0.017144316886782557, - "velocityX": -0.2920382538410566, - "velocityY": -0.03861755889085621, - "timestamp": 4.1313683340670275 - }, - { - "x": 2.2813636924749554, - "y": 5.534594417495688, - "heading": -0.01972730019355094, - "angularVelocity": 0.039384438994890546, - "velocityX": -0.4015521791234612, - "velocityY": -0.043854763273898215, - "timestamp": 4.15669663063216 - }, - { - "x": 2.268418865246587, - "y": 5.533351225535465, - "heading": -0.01818943546874751, - "angularVelocity": 0.06071725829704457, - "velocityX": -0.5110816353185899, - "velocityY": -0.04908312554685771, - "timestamp": 4.1820249271972925 - }, - { - "x": 2.252699358665988, - "y": 5.531975899350418, - "heading": -0.01614093194994636, - "angularVelocity": 0.08087806116370898, - "velocityX": -0.6206302322838587, - "velocityY": -0.05429998742641995, - "timestamp": 4.207353223762425 - }, - { - "x": 2.2342045636549908, - "y": 5.530468833172859, - "heading": -0.013621619170245854, - "angularVelocity": 0.09946633296833651, - "velocityX": -0.7302028765895126, - "velocityY": -0.05950128440959299, - "timestamp": 4.232681520327557 - }, - { - "x": 2.212933700942747, - "y": 5.5288305900830315, - "heading": -0.010687751449041242, - "angularVelocity": 0.11583359795362039, - "velocityX": -0.839806287703566, - "velocityY": -0.06468035012099213, - "timestamp": 4.25800981689269 - }, - { - "x": 2.188885768109178, - "y": 5.527062038952629, - "heading": -0.00742483553755111, - "angularVelocity": 0.12882492524028982, - "velocityX": -0.9494492758999686, - "velocityY": -0.06982511144524438, - "timestamp": 4.283338113457822 - }, - { - "x": 2.162059564630152, - "y": 5.52516469289038, - "heading": -0.003978602963118568, - "angularVelocity": 0.13606254828697195, - "velocityX": -1.0591396626306326, - "velocityY": -0.07491013291587634, - "timestamp": 4.308666410022955 - }, - { - "x": 2.132454659152142, - "y": 5.523141831263589, - "heading": -0.0006543637194032241, - "angularVelocity": 0.13124606446293755, - "velocityX": -1.1688470798602, - "velocityY": -0.07986567993502292, - "timestamp": 4.333994706588087 - }, - { - "x": 2.100091910087291, - "y": 5.521005809692814, - "heading": 0.0014695168260145277, - "angularVelocity": 0.08385406180119828, - "velocityX": -1.277730974983244, - "velocityY": -0.08433340810204847, - "timestamp": 4.35932300315322 - }, - { - "x": 2.067106097490617, - "y": 5.5194054139110715, - "heading": 0.0014695211546497217, - "angularVelocity": 1.7090115724079902e-7, - "velocityX": -1.3023304789506625, - "velocityY": -0.06318608034459623, - "timestamp": 4.384651299718352 - }, - { - "x": 2.0341202814051282, - "y": 5.517805090038329, - "heading": 0.0014695254831924606, - "angularVelocity": 1.7089750697357396e-7, - "velocityX": -1.3023306166944215, - "velocityY": -0.06318324126698588, - "timestamp": 4.409979596283485 - }, - { - "x": 2.001134465319256, - "y": 5.516204766173488, - "heading": 0.0014695298117352064, - "angularVelocity": 1.7089750724047756e-7, - "velocityX": -1.3023306167095552, - "velocityY": -0.06318324095504856, - "timestamp": 4.435307892848617 - }, - { - "x": 1.968148649233384, - "y": 5.514604442308648, - "heading": 0.0014695341402779503, - "angularVelocity": 1.70897507174197e-7, - "velocityX": -1.3023306167095567, - "velocityY": -0.06318324095501428, - "timestamp": 4.46063618941375 - }, - { - "x": 1.935162833147512, - "y": 5.513004118443807, - "heading": 0.0014695384688206946, - "angularVelocity": 1.7089750718946717e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501428, - "timestamp": 4.485964485978882 - }, - { - "x": 1.9021770170616399, - "y": 5.511403794578967, - "heading": 0.0014695427973634258, - "angularVelocity": 1.7089750667103753e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501429, - "timestamp": 4.511292782544015 - }, - { - "x": 1.8691912009757676, - "y": 5.509803470714126, - "heading": 0.0014695471259061786, - "angularVelocity": 1.7089750752194645e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501429, - "timestamp": 4.536621079109147 - }, - { - "x": 1.8362053848898956, - "y": 5.5082031468492865, - "heading": 0.0014695514544489196, - "angularVelocity": 1.7089750705816044e-7, - "velocityX": -1.3023306167095567, - "velocityY": -0.0631832409550143, - "timestamp": 4.5619493756742795 - }, - { - "x": 1.8032195688040236, - "y": 5.506602822984446, - "heading": 0.0014695557829916591, - "angularVelocity": 1.7089750699961795e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501429, - "timestamp": 4.587277672239412 - }, - { - "x": 1.7702337527181515, - "y": 5.505002499119605, - "heading": 0.0014695601115344013, - "angularVelocity": 1.708975070971006e-7, - "velocityX": -1.3023306167095567, - "velocityY": -0.06318324095501428, - "timestamp": 4.612605968804544 - }, - { - "x": 1.7372479366322795, - "y": 5.503402175254765, - "heading": 0.0014695644400771505, - "angularVelocity": 1.7089750737689966e-7, - "velocityX": -1.3023306167095567, - "velocityY": -0.06318324095501429, - "timestamp": 4.637934265369677 - }, - { - "x": 1.7042621205464072, - "y": 5.501801851389924, - "heading": 0.001469568768619884, - "angularVelocity": 1.708975067545602e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501429, - "timestamp": 4.663262561934809 - }, - { - "x": 1.6712763044605352, - "y": 5.500201527525085, - "heading": 0.0014695730971626293, - "angularVelocity": 1.708975072324485e-7, - "velocityX": -1.302330616709557, - "velocityY": -0.06318324095501379, - "timestamp": 4.688590858499942 - }, - { - "x": 1.6382904883746574, - "y": 5.498601203660361, - "heading": 0.0014695774257053702, - "angularVelocity": 1.7089750704976447e-7, - "velocityX": -1.3023306167097823, - "velocityY": -0.06318324095036792, - "timestamp": 4.713919155065074 - }, - { - "x": 1.6053046722368207, - "y": 5.497000880866617, - "heading": 0.0014695817542481073, - "angularVelocity": 1.7089750689937978e-7, - "velocityX": -1.3023306187612051, - "velocityY": -0.06318319866650293, - "timestamp": 4.739247451630207 - }, - { - "x": 1.5723183905912428, - "y": 5.495410305750672, - "heading": 0.0014696021551505508, - "angularVelocity": 8.054589217739563e-7, - "velocityX": -1.3023489977208753, - "velocityY": -0.06279834539482758, - "timestamp": 4.764575748195339 - }, - { - "x": 1.5417971049968995, - "y": 5.493927935781562, - "heading": 0.006291317242065446, - "angularVelocity": 0.19036870776507275, - "velocityX": -1.2050271725090151, - "velocityY": -0.05852624021955814, - "timestamp": 4.789904044760472 - }, - { - "x": 1.5140534309467035, - "y": 5.492580807122346, - "heading": 0.011350728528281598, - "angularVelocity": 0.19975331831943247, - "velocityX": -1.0953628081090867, - "velocityY": -0.05318670585541525, - "timestamp": 4.815232341325604 - }, - { - "x": 1.489087675281974, - "y": 5.49136872045318, - "heading": 0.01621727097300913, - "angularVelocity": 0.19213856061115964, - "velocityX": -0.9856863291432186, - "velocityY": -0.0478550409444552, - "timestamp": 4.840560637890737 - }, - { - "x": 1.4668984505481375, - "y": 5.490291524858565, - "heading": 0.020724096649122396, - "angularVelocity": 0.17793639080867357, - "velocityX": -0.8760646290121439, - "velocityY": -0.04252933440885621, - "timestamp": 4.865888934455869 - }, - { - "x": 1.4474846758288984, - "y": 5.489349123131251, - "heading": 0.024782341761516204, - "angularVelocity": 0.16022574206553616, - "velocityX": -0.7664856051144657, - "velocityY": -0.037207465764595535, - "timestamp": 4.891217231021002 - }, - { - "x": 1.4308455599117629, - "y": 5.488541449109029, - "heading": 0.028336770165514075, - "angularVelocity": 0.14033428560278258, - "velocityX": -0.6569378194997122, - "velocityY": -0.031888209305836225, - "timestamp": 4.916545527586134 - }, - { - "x": 1.41698051088131, - "y": 5.487868455273427, - "heading": 0.03134971257468342, - "angularVelocity": 0.11895558793002604, - "velocityX": -0.5474134036137117, - "velocityY": -0.026570828949275312, - "timestamp": 4.9418738241512665 - }, - { - "x": 1.4058890726702549, - "y": 5.48733010598215, - "heading": 0.03379383297497233, - "angularVelocity": 0.09649762249126285, - "velocityX": -0.4379069939636957, - "velocityY": -0.02125485580511678, - "timestamp": 4.967202120716399 - }, - { - "x": 1.3975708843453332, - "y": 5.486926373543047, - "heading": 0.035648388488984245, - "angularVelocity": 0.07322069643498604, - "velocityX": -0.32841483451260217, - "velocityY": -0.015939975989693458, - "timestamp": 4.9925304172815315 - }, - { - "x": 1.392025653680069, - "y": 5.486657235798119, - "heading": 0.03689710038622943, - "angularVelocity": 0.04930106112871753, - "velocityX": -0.21893421261101836, - "velocityY": -0.010625971005922226, - "timestamp": 5.017858713846664 - }, - { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 0.024863593639583376, - "velocityX": -0.10946311281102968, - "velocityY": -0.005312684065681727, - "timestamp": 5.043187010411796 - }, - { - "x": 1.3892531394958496, - "y": 5.486522674560547, - "heading": 0.03752685285960867, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -3.7734192506152025e-32, - "timestamp": 5.068515306976929 - }, - { - "x": 1.3924224488650907, - "y": 5.483383635076441, - "heading": 0.02924366746771374, - "angularVelocity": -0.24133382912300758, - "velocityX": 0.09233906155267109, - "velocityY": -0.09145713667218072, - "timestamp": 5.102837827868408 - }, - { - "x": 1.3988725362064904, - "y": 5.477161526594317, - "heading": 0.012911519796096444, - "angularVelocity": -0.47584347674393, - "velocityX": 0.18792580422031321, - "velocityY": -0.18128355145583694, - "timestamp": 5.1371603487598865 - }, - { - "x": 1.408682240841783, - "y": 5.467866567883204, - "heading": -0.011184409535748709, - "angularVelocity": -0.7020442760609589, - "velocityX": 0.28580956120061807, - "velocityY": -0.2708122384280062, - "timestamp": 5.171482869651365 - }, - { - "x": 1.4219108303772576, - "y": 5.455441331497704, - "heading": -0.04252483753751102, - "angularVelocity": -0.9131155634184035, - "velocityX": 0.38542010294934864, - "velocityY": -0.36201409636507, - "timestamp": 5.205805390542844 - }, - { - "x": 1.4386294081873154, - "y": 5.439746841342612, - "heading": -0.07997828832221507, - "angularVelocity": -1.0912208605866973, - "velocityX": 0.4871022691753612, - "velocityY": -0.4572650769072472, - "timestamp": 5.240127911434323 - }, - { - "x": 1.45897718646536, - "y": 5.420573005762799, - "heading": -0.12053700236214591, - "angularVelocity": -1.1816939136892084, - "velocityX": 0.5928404368193222, - "velocityY": -0.5586371595617143, - "timestamp": 5.274450432325802 - }, - { - "x": 1.48279681598405, - "y": 5.398288119279197, - "heading": -0.15391427405348196, - "angularVelocity": -0.9724597967867469, - "velocityX": 0.6939941735050115, - "velocityY": -0.6492788380568949, - "timestamp": 5.3087729532172805 - }, - { - "x": 1.5099325470579765, - "y": 5.373593666238814, - "heading": -0.17626410102219708, - "angularVelocity": -0.651170904356972, - "velocityX": 0.7906100825089313, - "velocityY": -0.7194824971761627, - "timestamp": 5.343095474108759 - }, - { - "x": 1.54027922460392, - "y": 5.34641384751758, - "heading": -0.18746082036459932, - "angularVelocity": -0.32622077433659935, - "velocityX": 0.88416225725069, - "velocityY": -0.7918945932663884, - "timestamp": 5.377417995000238 - }, - { - "x": 1.5736954265024226, - "y": 5.316646807539857, - "heading": -0.1874611197510539, - "angularVelocity": -0.000008722740836805637, - "velocityX": 0.9735940435190712, - "velocityY": -0.867274291181608, - "timestamp": 5.411740515891717 - }, - { - "x": 1.6060049814196613, - "y": 5.285681999275841, - "heading": -0.18746118384049648, - "angularVelocity": -0.0000018672708450361788, - "velocityX": 0.9413514531579913, - "velocityY": -0.9021717362171962, - "timestamp": 5.446063036783196 - }, - { - "x": 1.6383144805702523, - "y": 5.254717132823409, - "heading": -0.1874612479298642, - "angularVelocity": -0.0000018672686635293044, - "velocityX": 0.9413498283750139, - "velocityY": -0.902173431559325, - "timestamp": 5.4803855576746745 - }, - { - "x": 1.6706239797183724, - "y": 5.223752266368399, - "heading": -0.1874613120192331, - "angularVelocity": -0.0000018672686984763428, - "velocityX": 0.9413498283030138, - "velocityY": -0.9021734316344456, - "timestamp": 5.514708078566153 - }, - { - "x": 1.7029334788664925, - "y": 5.192787399913389, - "heading": -0.18746137610860322, - "angularVelocity": -0.0000018672687334879554, - "velocityX": 0.9413498283030205, - "velocityY": -0.9021734316344329, - "timestamp": 5.549030599457632 - }, - { - "x": 1.7352429780146128, - "y": 5.16182253345838, - "heading": -0.18746144019797456, - "angularVelocity": -0.0000018672687685683143, - "velocityX": 0.9413498283030303, - "velocityY": -0.9021734316344164, - "timestamp": 5.583353120349111 - }, - { - "x": 1.7675524771627336, - "y": 5.130857667003371, - "heading": -0.18746150428734706, - "angularVelocity": -0.000001867268803540895, - "velocityX": 0.9413498283030403, - "velocityY": -0.9021734316344002, - "timestamp": 5.61767564124059 - }, - { - "x": 1.7998619763108545, - "y": 5.099892800548363, - "heading": -0.1874615683767208, - "angularVelocity": -0.0000018672688385171224, - "velocityX": 0.94134982830305, - "velocityY": -0.902173431634384, - "timestamp": 5.6519981621320685 - }, - { - "x": 1.8321714754589762, - "y": 5.068927934093356, - "heading": -0.1874616324660957, - "angularVelocity": -0.000001867268873360854, - "velocityX": 0.94134982830306, - "velocityY": -0.9021734316343675, - "timestamp": 5.686320683023547 - }, - { - "x": 1.864480974607098, - "y": 5.037963067638349, - "heading": -0.18746169655547182, - "angularVelocity": -0.000001867268908538558, - "velocityX": 0.94134982830307, - "velocityY": -0.9021734316343514, - "timestamp": 5.720643203915026 - }, - { - "x": 1.8967904737552201, - "y": 5.006998201183343, - "heading": -0.18746176064484915, - "angularVelocity": -0.0000018672689434492718, - "velocityX": 0.94134982830308, - "velocityY": -0.9021734316343349, - "timestamp": 5.754965724806505 - }, - { - "x": 1.9290999729033427, - "y": 4.976033334728337, - "heading": -0.18746182473422768, - "angularVelocity": -0.0000018672689784928927, - "velocityX": 0.9413498283030898, - "velocityY": -0.9021734316343186, - "timestamp": 5.789288245697984 - }, - { - "x": 1.9614094720514654, - "y": 4.945068468273332, - "heading": -0.1874618888236074, - "angularVelocity": -0.0000018672690133920361, - "velocityX": 0.9413498283030998, - "velocityY": -0.9021734316343023, - "timestamp": 5.8236107665894625 - }, - { - "x": 1.9937189711995889, - "y": 4.9141036018183275, - "heading": -0.1874619529129883, - "angularVelocity": -0.0000018672690484597244, - "velocityX": 0.9413498283031098, - "velocityY": -0.9021734316342861, - "timestamp": 5.857933287480941 - }, - { - "x": 2.0260284703477125, - "y": 4.883138735363323, - "heading": -0.18746201700237045, - "angularVelocity": -0.0000018672690833977735, - "velocityX": 0.9413498283031192, - "velocityY": -0.9021734316342699, - "timestamp": 5.89225580837242 - }, - { - "x": 2.0583379694958364, - "y": 4.85217386890832, - "heading": -0.18746208109175377, - "angularVelocity": -0.000001867269118277379, - "velocityX": 0.9413498283031293, - "velocityY": -0.9021734316342535, - "timestamp": 5.926578329263899 - }, - { - "x": 2.0906474686439607, - "y": 4.821209002453316, - "heading": -0.18746214518113832, - "angularVelocity": -0.0000018672691534530088, - "velocityX": 0.9413498283031392, - "velocityY": -0.9021734316342372, - "timestamp": 5.960900850155378 - }, - { - "x": 2.122956967792085, - "y": 4.790244135998314, - "heading": -0.18746220927052404, - "angularVelocity": -0.0000018672691879651852, - "velocityX": 0.9413498283031491, - "velocityY": -0.902173431634221, - "timestamp": 5.9952233710468565 - }, - { - "x": 2.1552664669402097, - "y": 4.759279269543312, - "heading": -0.18746227335991097, - "angularVelocity": -0.000001867269223375388, - "velocityX": 0.9413498283031604, - "velocityY": -0.9021734316342032, - "timestamp": 6.029545891938335 - }, - { - "x": 2.1875759660893364, - "y": 4.728314403089357, - "heading": -0.18746233744929908, - "angularVelocity": -0.0000018672692579471589, - "velocityX": 0.9413498283323467, - "velocityY": -0.9021734316037436, - "timestamp": 6.063868412829814 - }, - { - "x": 2.2198854878335275, - "y": 4.697349560211671, - "heading": -0.1874624015387177, - "angularVelocity": -0.000001867270147103653, - "velocityX": 0.9413504866483311, - "velocityY": -0.9021727446999435, - "timestamp": 6.098190933721293 - }, - { - "x": 2.252690273002457, - "y": 4.666909917394811, - "heading": -0.18746249439904436, - "angularVelocity": -0.0000027055217456866627, - "velocityX": 0.9557801792197065, - "velocityY": -0.8868708365887329, - "timestamp": 6.132513454612772 - }, - { - "x": 2.28400682991543, - "y": 4.6403322606682735, - "heading": -0.19781896983491082, - "angularVelocity": -0.3017399412068747, - "velocityX": 0.9124200699590284, - "velocityY": -0.7743503692683428, - "timestamp": 6.1668359755042506 - }, - { - "x": 2.3122327500910567, - "y": 4.616324863842612, - "heading": -0.21952477619155, - "angularVelocity": -0.6324071132557241, - "velocityX": 0.8223731661456952, - "velocityY": -0.6994648470480529, - "timestamp": 6.201158496395729 - }, - { - "x": 2.3372936813760514, - "y": 4.59483038719477, - "heading": -0.2527061616816108, - "angularVelocity": -0.966752576099356, - "velocityX": 0.7301599834182618, - "velocityY": -0.6262499399680785, - "timestamp": 6.235481017287208 - }, - { - "x": 2.3591158146883524, - "y": 4.575789635608326, - "heading": -0.29749234621539733, - "angularVelocity": -1.3048629113051509, - "velocityX": 0.6357963443681244, - "velocityY": -0.5547597056360541, - "timestamp": 6.269803538178687 - }, - { - "x": 2.3776154934613634, - "y": 4.5591361519017175, - "heading": -0.35396586299822075, - "angularVelocity": -1.6453778835587947, - "velocityX": 0.5389953387020591, - "velocityY": -0.48520572714527554, - "timestamp": 6.304126059070166 - }, - { - "x": 2.3926892972627702, - "y": 4.54479439277693, - "heading": -0.4220731577487357, - "angularVelocity": -1.9843325309891349, - "velocityX": 0.4391811385028281, - "velocityY": -0.4178527320337051, - "timestamp": 6.338448579961645 - }, - { - "x": 2.4038429340082574, - "y": 4.533232670068245, - "heading": -0.4964329248800265, - "angularVelocity": -2.1665007464458306, - "velocityX": 0.32496554611338146, - "velocityY": -0.3368552894247085, - "timestamp": 6.372771100853123 - }, - { - "x": 2.4110020440418207, - "y": 4.52484519407797, - "heading": -0.5699375243350002, - "angularVelocity": -2.1415851034771523, - "velocityX": 0.20858345621521887, - "velocityY": -0.2443723762830769, - "timestamp": 6.407093621744602 - }, - { - "x": 2.4143476486206055, - "y": 4.51963472366333, - "heading": -0.6391760227970993, - "angularVelocity": -2.017290591242367, - "velocityX": 0.09747549107370657, - "velocityY": -0.15180908276272057, - "timestamp": 6.441416142636081 - }, - { - "x": 2.411550419309736, - "y": 4.5135312748194405, - "heading": -0.7146746576956277, - "angularVelocity": -1.9134707003445537, - "velocityX": -0.07089421332832456, - "velocityY": -0.15468849932890996, - "timestamp": 6.480872526726484 - }, - { - "x": 2.402130176421324, - "y": 4.5073270767652875, - "heading": -0.7857083432117212, - "angularVelocity": -1.8003090540009719, - "velocityX": -0.2387507904127206, - "velocityY": -0.15724193174765805, - "timestamp": 6.520328910816888 - }, - { - "x": 2.3861176361049883, - "y": 4.50103756567989, - "heading": -0.8517701759248951, - "angularVelocity": -1.674300223806899, - "velocityX": -0.4058288838543077, - "velocityY": -0.15940414283750848, - "timestamp": 6.559785294907291 - }, - { - "x": 2.3635630141416817, - "y": 4.494680207849731, - "heading": -0.9121294134589305, - "angularVelocity": -1.529771136547598, - "velocityX": -0.5716342863965691, - "velocityY": -0.16112368065944074, - "timestamp": 6.599241678997695 - }, - { - "x": 2.334558035905885, - "y": 4.488268138139215, - "heading": -0.9656544357627823, - "angularVelocity": -1.3565617716315246, - "velocityX": -0.735114960593953, - "velocityY": -0.16251032268502744, - "timestamp": 6.638698063088098 - }, - { - "x": 2.299306014374734, - "y": 4.481764383153815, - "heading": -1.0103454671131291, - "angularVelocity": -1.1326692088142007, - "velocityX": -0.8934427810308345, - "velocityY": -0.16483403472802036, - "timestamp": 6.6781544471785015 - }, - { - "x": 2.258684856589374, - "y": 4.474594280720458, - "heading": -1.0401966260593063, - "angularVelocity": -0.7565609377124255, - "velocityX": -1.0295205382299486, - "velocityY": -0.18172223833104606, - "timestamp": 6.717610831268905 - }, - { - "x": 2.21277726427517, - "y": 4.4668464349379295, - "heading": -1.0547000810066136, - "angularVelocity": -0.3675819587034771, - "velocityX": -1.1635022664271168, - "velocityY": -0.19636482057697183, - "timestamp": 6.757067215359308 - }, - { - "x": 2.161777655764477, - "y": 4.46008626841952, - "heading": -1.0547000906528792, - "angularVelocity": -2.4447920514853427e-7, - "velocityX": -1.292556570663997, - "velocityY": -0.17133264170686324, - "timestamp": 6.796523599449712 - }, - { - "x": 2.1103594690305147, - "y": 4.458403877945663, - "heading": -1.0547000861825677, - "angularVelocity": 1.132975479004823e-7, - "velocityX": -1.3031652017617956, - "velocityY": -0.04263924616108742, - "timestamp": 6.835979983540115 - }, - { - "x": 2.058941271027739, - "y": 4.456721831910964, - "heading": -1.0547000817122596, - "angularVelocity": 1.1329746134753013e-7, - "velocityX": -1.3031654873635976, - "velocityY": -0.04263051654316007, - "timestamp": 6.875436367630519 - }, - { - "x": 2.0075230730246374, - "y": 4.4550397858862105, - "heading": -1.0547000772419513, - "angularVelocity": 1.1329746168009402e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.042630516291088466, - "timestamp": 6.914892751720922 - }, - { - "x": 1.9561048750215357, - "y": 4.453357739861458, - "heading": -1.0547000727716431, - "angularVelocity": 1.1329746154059279e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.042630516291081035, - "timestamp": 6.954349135811325 - }, - { - "x": 1.904686677018434, - "y": 4.451675693836704, - "heading": -1.054700068301335, - "angularVelocity": 1.1329746125974681e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108111, - "timestamp": 6.993805519901729 - }, - { - "x": 1.8532684790153326, - "y": 4.449993647811951, - "heading": -1.0547000638310269, - "angularVelocity": 1.1329746137667254e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108123, - "timestamp": 7.033261903992132 - }, - { - "x": 1.801850281012231, - "y": 4.448311601787198, - "heading": -1.0547000593607188, - "angularVelocity": 1.132974614247932e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108126, - "timestamp": 7.072718288082536 - }, - { - "x": 1.7504320830091293, - "y": 4.446629555762445, - "heading": -1.0547000548904106, - "angularVelocity": 1.1329746142979229e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.0426305162910812, - "timestamp": 7.112174672172939 - }, - { - "x": 1.699013885006028, - "y": 4.444947509737691, - "heading": -1.0547000504201025, - "angularVelocity": 1.1329746122002105e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108106, - "timestamp": 7.1516310562633425 - }, - { - "x": 1.6475956870029262, - "y": 4.443265463712938, - "heading": -1.0547000459497944, - "angularVelocity": 1.1329746123173152e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108124, - "timestamp": 7.191087440353746 - }, - { - "x": 1.5961774889998248, - "y": 4.441583417688185, - "heading": -1.054700041479486, - "angularVelocity": 1.1329746152886358e-7, - "velocityX": -1.3031654873718441, - "velocityY": -0.04263051629108131, - "timestamp": 7.230543824444149 - }, - { - "x": 1.5447592909967232, - "y": 4.4399013716634315, - "heading": -1.054700037009178, - "angularVelocity": 1.1329746155492088e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.042630516291081264, - "timestamp": 7.270000208534553 - }, - { - "x": 1.4933410929936215, - "y": 4.438219325638678, - "heading": -1.0547000325388698, - "angularVelocity": 1.1329746160632969e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.042630516291081166, - "timestamp": 7.309456592624956 - }, - { - "x": 1.44192289499052, - "y": 4.436537279613925, - "heading": -1.0547000280685617, - "angularVelocity": 1.1329746145163e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.0426305162910812, - "timestamp": 7.34891297671536 - }, - { - "x": 1.3905046969874184, - "y": 4.434855233589172, - "heading": -1.0547000235982535, - "angularVelocity": 1.1329746128786673e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.042630516291081215, - "timestamp": 7.388369360805763 - }, - { - "x": 1.3390864989843168, - "y": 4.433173187564418, - "heading": -1.0547000191279454, - "angularVelocity": 1.1329746138407028e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108108, - "timestamp": 7.427825744896166 - }, - { - "x": 1.2876683009812153, - "y": 4.431491141539665, - "heading": -1.0547000146576373, - "angularVelocity": 1.1329746123221407e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108107, - "timestamp": 7.46728212898657 - }, - { - "x": 1.2362501029781137, - "y": 4.429809095514912, - "heading": -1.0547000101873292, - "angularVelocity": 1.1329746116894446e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108118, - "timestamp": 7.506738513076973 - }, - { - "x": 1.1848319049750122, - "y": 4.428127049490159, - "heading": -1.0547000057170208, - "angularVelocity": 1.1329746119416889e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108113, - "timestamp": 7.546194897167377 - }, - { - "x": 1.1334137069719106, - "y": 4.426445003465405, - "heading": -1.0547000012467127, - "angularVelocity": 1.132974610235854e-7, - "velocityX": -1.303165487371844, - "velocityY": -0.04263051629108119, - "timestamp": 7.58565128125778 - }, - { - "x": 1.081995508968809, - "y": 4.424762957440652, - "heading": -1.0546999967764046, - "angularVelocity": 1.132974612359749e-7, - "velocityX": -1.3031654873718441, - "velocityY": -0.04263051629108153, - "timestamp": 7.6251076653481835 - }, - { - "x": 1.0305773109657206, - "y": 4.423080911415498, - "heading": -1.0546999923060965, - "angularVelocity": 1.1329746154357785e-7, - "velocityX": -1.3031654873715115, - "velocityY": -0.04263051630124593, - "timestamp": 7.664564049438587 - }, - { - "x": 0.9791591134170966, - "y": 4.421398851501261, - "heading": -1.0546999878355032, - "angularVelocity": 1.1330468776623308e-7, - "velocityX": -1.3031654758533633, - "velocityY": -0.04263086831227212, - "timestamp": 7.70402043352899 - }, - { - "x": 0.9320637074618211, - "y": 4.419851540523593, - "heading": -1.0424883288696292, - "angularVelocity": 0.3094976705897423, - "velocityX": -1.1936067392128267, - "velocityY": -0.03921573183500874, - "timestamp": 7.743476817619394 - }, - { - "x": 0.8917002364780792, - "y": 4.4185265531626925, - "heading": -1.0318261905546944, - "angularVelocity": 0.2702259358208133, - "velocityX": -1.0229896102810612, - "velocityY": -0.033581064039327264, - "timestamp": 7.782933201709797 - }, - { - "x": 0.8580655701196472, - "y": 4.4174228713893, - "heading": -1.0228692034037898, - "angularVelocity": 0.22700983268973754, - "velocityX": -0.8524518182245786, - "velocityY": -0.027972197626213308, - "timestamp": 7.822389585800201 - }, - { - "x": 0.8311586102834976, - "y": 4.416540158262108, - "heading": -1.0156685912652759, - "angularVelocity": 0.1824954897543525, - "velocityX": -0.6819418569755298, - "velocityY": -0.022371870802177205, - "timestamp": 7.861845969890604 - }, - { - "x": 0.8109787988166595, - "y": 4.41587824490326, - "heading": -1.0102497931206222, - "angularVelocity": 0.13733641005313718, - "velocityX": -0.511446041801538, - "velocityY": -0.016775824093046718, - "timestamp": 7.901302353981007 - }, - { - "x": 0.797525797662573, - "y": 4.415437029865442, - "heading": -1.0066280322412158, - "angularVelocity": 0.09179150504790234, - "velocityX": -0.34095879448209143, - "velocityY": -0.011182348509336195, - "timestamp": 7.940758738071411 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 0.04598871864711492, - "velocityX": -0.17047731232558744, - "velocityY": -0.0055905767260737965, - "timestamp": 7.980215122161814 - }, - { - "x": 0.7907993793487549, - "y": 4.415216445922852, - "heading": -1.0048134836944498, - "angularVelocity": 2.6076874199065633e-34, - "velocityX": 4.4246114702257314e-36, - "velocityY": -1.2923319814607663e-34, - "timestamp": 8.019671506252218 + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 + }, + { + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 + }, + { + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 + }, + { + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 + }, + { + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 + }, + { + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 + }, + { + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 + }, + { + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 + }, + { + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 + }, + { + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 + }, + { + "x": 1.4994455567524874, + "y": 4.286480903049883, + "heading": -0.7254698893413497, + "angularVelocity": 3.8000232610425283e-11, + "velocityX": 2.8946950656209713e-9, + "velocityY": -5.415769760526697e-10, + "timestamp": 9.654617059422403 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index afeb5469..4108f756 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -133,17 +133,17 @@ public void robotInit() { .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here NamedCommands.registerCommand( - "fender", Commands.print("pew") - // Commands.parallel( - // swerve.stopCmd(), - // shooter.run(-10.0), - // pivot.run(-30.0), - // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) - // .withTimeout(1.5) - // .andThen(Commands.print("done shooting")) - ); + "fender", // Commands.print("pew") + Commands.parallel( + swerve.stopCmd(), + // shooter.run(-10.0), + pivot.run(-60.0), + Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + .withTimeout(1.5) + .andThen(Commands.print("done shooting"))); NamedCommands.registerCommand( - "intake", Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))); + "intake", + Commands.parallel(Commands.print("intake"), /*shooter.run(5.0),*/ pivot.run(100.0))); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -164,6 +164,76 @@ public void disabledExit() {} @Override public void autonomousInit() { autonomousCommand = AutoBuilder.buildAuto("local 4"); + // autonomousCommand = + // Commands.sequence( + // swerve.runOnce(() -> swerve.setPose()) + // Commands.parallel( + // swerve.stopCmd(), + // shooter.run(-10.0), + // pivot.run(-30.0), + // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + // .withTimeout(1.5) + // .andThen(Commands.print("done shooting")), + // Choreo.choreoSwerveCommand( + // Choreo.getTrajectory("amp 4 local sgmt 1"), + // swerve::getPose, + // Choreo.choreoSwerveController( + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0)), + // swerve::runVelocity, + // () -> false, + // swerve) + // .alongWith( + // Commands.parallel( + // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), + // Commands.parallel( + // swerve.stopCmd(), + // shooter.run(-10.0), + // pivot.run(-30.0), + // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + // .withTimeout(1.5) + // .andThen(Commands.print("done shooting")), + // Choreo.choreoSwerveCommand( + // Choreo.getTrajectory("amp 4 local sgmt 2"), + // swerve::getPose, + // Choreo.choreoSwerveController( + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0)), + // swerve::runVelocity, + // () -> false, + // swerve) + // .alongWith( + // Commands.parallel( + // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), + // Commands.parallel( + // swerve.stopCmd(), + // shooter.run(-10.0), + // pivot.run(-30.0), + // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + // .withTimeout(1.5) + // .andThen(Commands.print("done shooting")), + // Choreo.choreoSwerveCommand( + // Choreo.getTrajectory("amp 4 local sgmt 3"), + // swerve::getPose, + // Choreo.choreoSwerveController( + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0), + // new PIDController(5.0, 0.0, 0.0)), + // swerve::runVelocity, + // () -> false, + // swerve) + // .alongWith( + // Commands.parallel( + // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), + // Commands.parallel( + // swerve.stopCmd(), + // shooter.run(-10.0), + // pivot.run(-30.0), + // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + // .withTimeout(1.5) + // .andThen(Commands.print("done shooting"))); if (autonomousCommand != null) { autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index a2bb02bb..fd767bff 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -198,7 +198,7 @@ public void periodic() { pose = odometry.update(gyroInputs.yawPosition, getModulePositions()); } - private void runVelocity(ChassisSpeeds speeds) { + public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json new file mode 100644 index 00000000..e08fc121 --- /dev/null +++ b/vendordeps/ChoreoLib.json @@ -0,0 +1,43 @@ +{ + "fileName": "ChoreoLib.json", + "name": "ChoreoLib", + "version": "2024.1.0", + "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", + "frcYear": "2024", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "javaDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-java", + "version": "2024.1.0" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.10.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.choreo.lib", + "artifactId": "ChoreoLib-cpp", + "version": "2024.1.0", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From 1fef3add2993103cd7464a4e4939581390ac7af7 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 11 Jan 2024 13:07:53 -0800 Subject: [PATCH 11/19] adjusted command bindings --- src/main/java/frc/robot/Robot.java | 87 +++--------------------------- 1 file changed, 8 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4108f756..7bb42b19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,8 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -113,13 +113,13 @@ public void robotInit() { // Controller bindings here controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); - controller.leftTrigger().whileTrue(Commands.parallel(shooter.run(5.0), pivot.run(100.0))); + controller.leftTrigger().whileTrue(Commands.parallel(shooter.run(5.0), pivot.run(103.0))); controller .rightTrigger() .whileTrue( Commands.parallel( shooter.run(-10.0), - pivot.run(-15.0), + pivot.run(-63.0), Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360)))); controller .leftBumper() @@ -136,14 +136,13 @@ public void robotInit() { "fender", // Commands.print("pew") Commands.parallel( swerve.stopCmd(), - // shooter.run(-10.0), - pivot.run(-60.0), - Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) + shooter.run(-10.0), + pivot.run(-63.0), + Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) .withTimeout(1.5) .andThen(Commands.print("done shooting"))); NamedCommands.registerCommand( - "intake", - Commands.parallel(Commands.print("intake"), /*shooter.run(5.0),*/ pivot.run(100.0))); + "intake", Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(103.0))); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -163,77 +162,7 @@ public void disabledExit() {} @Override public void autonomousInit() { - autonomousCommand = AutoBuilder.buildAuto("local 4"); - // autonomousCommand = - // Commands.sequence( - // swerve.runOnce(() -> swerve.setPose()) - // Commands.parallel( - // swerve.stopCmd(), - // shooter.run(-10.0), - // pivot.run(-30.0), - // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) - // .withTimeout(1.5) - // .andThen(Commands.print("done shooting")), - // Choreo.choreoSwerveCommand( - // Choreo.getTrajectory("amp 4 local sgmt 1"), - // swerve::getPose, - // Choreo.choreoSwerveController( - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0)), - // swerve::runVelocity, - // () -> false, - // swerve) - // .alongWith( - // Commands.parallel( - // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), - // Commands.parallel( - // swerve.stopCmd(), - // shooter.run(-10.0), - // pivot.run(-30.0), - // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) - // .withTimeout(1.5) - // .andThen(Commands.print("done shooting")), - // Choreo.choreoSwerveCommand( - // Choreo.getTrajectory("amp 4 local sgmt 2"), - // swerve::getPose, - // Choreo.choreoSwerveController( - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0)), - // swerve::runVelocity, - // () -> false, - // swerve) - // .alongWith( - // Commands.parallel( - // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), - // Commands.parallel( - // swerve.stopCmd(), - // shooter.run(-10.0), - // pivot.run(-30.0), - // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) - // .withTimeout(1.5) - // .andThen(Commands.print("done shooting")), - // Choreo.choreoSwerveCommand( - // Choreo.getTrajectory("amp 4 local sgmt 3"), - // swerve::getPose, - // Choreo.choreoSwerveController( - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0), - // new PIDController(5.0, 0.0, 0.0)), - // swerve::runVelocity, - // () -> false, - // swerve) - // .alongWith( - // Commands.parallel( - // Commands.print("intake"), shooter.run(5.0), pivot.run(100.0))), - // Commands.parallel( - // swerve.stopCmd(), - // shooter.run(-10.0), - // pivot.run(-30.0), - // Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360).asProxy())) - // .withTimeout(1.5) - // .andThen(Commands.print("done shooting"))); + autonomousCommand = new PathPlannerAuto("local 4"); if (autonomousCommand != null) { autonomousCommand.schedule(); From aad568370f4cac400d6521fae747c38faa2b91d1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 11 Jan 2024 13:13:54 -0800 Subject: [PATCH 12/19] moved command bindings to separate methods for consistency --- .../deploy/pathplanner/autos/local 4.auto | 51 ++++++++++++++++--- src/main/java/frc/robot/Robot.java | 51 +++++++------------ 2 files changed, 62 insertions(+), 40 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto index e486ea19..c5819b58 100644 --- a/src/main/deploy/pathplanner/autos/local 4.auto +++ b/src/main/deploy/pathplanner/autos/local 4.auto @@ -28,9 +28,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] } } ] @@ -53,9 +66,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] } } ] @@ -78,9 +104,22 @@ } }, { - "type": "named", + "type": "race", "data": { - "name": "intake" + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] } } ] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7bb42b19..fb92882a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -113,14 +113,10 @@ public void robotInit() { // Controller bindings here controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); - controller.leftTrigger().whileTrue(Commands.parallel(shooter.run(5.0), pivot.run(103.0))); + controller.leftTrigger().whileTrue(intake()); controller .rightTrigger() - .whileTrue( - Commands.parallel( - shooter.run(-10.0), - pivot.run(-63.0), - Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360)))); + .whileTrue(shootFender()); controller .leftBumper() .whileTrue( @@ -133,16 +129,10 @@ public void robotInit() { .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here NamedCommands.registerCommand( - "fender", // Commands.print("pew") - Commands.parallel( - swerve.stopCmd(), - shooter.run(-10.0), - pivot.run(-63.0), - Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) - .withTimeout(1.5) - .andThen(Commands.print("done shooting"))); + "fender", shootFender() + ); NamedCommands.registerCommand( - "intake", Commands.parallel(Commands.print("intake"), shooter.run(5.0), pivot.run(103.0))); + "intake", intake()); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -151,15 +141,9 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); } - @Override - public void disabledInit() {} - @Override public void disabledPeriodic() {} - @Override - public void disabledExit() {} - @Override public void autonomousInit() { autonomousCommand = new PathPlannerAuto("local 4"); @@ -169,12 +153,6 @@ public void autonomousInit() { } } - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - @Override public void teleopInit() { if (autonomousCommand != null) { @@ -185,17 +163,22 @@ public void teleopInit() { @Override public void teleopPeriodic() {} - @Override - public void teleopExit() {} - @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); } - @Override - public void testPeriodic() {} + private Command intake() { + return Commands.parallel(shooter.run(5.0), pivot.run(103.0)); + } - @Override - public void testExit() {} + private Command shootFender() { + return Commands.parallel( + swerve.stopCmd(), + shooter.run(-10.0), + pivot.run(-63.0), + Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) + .withTimeout(1.5) + .andThen(Commands.print("done shooting")); + } } From c06d8889268452853c89956c2c01e3f5404fb53c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 11 Jan 2024 13:14:40 -0800 Subject: [PATCH 13/19] removed choreolib --- src/main/java/frc/robot/Robot.java | 23 +++++++--------- vendordeps/ChoreoLib.json | 43 ------------------------------ 2 files changed, 9 insertions(+), 57 deletions(-) delete mode 100644 vendordeps/ChoreoLib.json diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fb92882a..c53affa3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,9 +114,7 @@ public void robotInit() { controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); controller.leftTrigger().whileTrue(intake()); - controller - .rightTrigger() - .whileTrue(shootFender()); + controller.rightTrigger().whileTrue(shootFender()); controller .leftBumper() .whileTrue( @@ -128,11 +126,8 @@ public void robotInit() { .a() .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); // Auto Bindings here - NamedCommands.registerCommand( - "fender", shootFender() - ); - NamedCommands.registerCommand( - "intake", intake()); + NamedCommands.registerCommand("fender", shootFender()); + NamedCommands.registerCommand("intake", intake()); NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy()); } @@ -174,11 +169,11 @@ private Command intake() { private Command shootFender() { return Commands.parallel( - swerve.stopCmd(), - shooter.run(-10.0), - pivot.run(-63.0), - Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) - .withTimeout(1.5) - .andThen(Commands.print("done shooting")); + swerve.stopCmd(), + shooter.run(-10.0), + pivot.run(-63.0), + Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) + .withTimeout(1.5) + .andThen(Commands.print("done shooting")); } } diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json deleted file mode 100644 index e08fc121..00000000 --- a/vendordeps/ChoreoLib.json +++ /dev/null @@ -1,43 +0,0 @@ -{ - "fileName": "ChoreoLib.json", - "name": "ChoreoLib", - "version": "2024.1.0", - "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", - "frcYear": "2024", - "mavenUrls": [ - "https://SleipnirGroup.github.io/ChoreoLib/dep" - ], - "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", - "javaDependencies": [ - { - "groupId": "com.choreo.lib", - "artifactId": "ChoreoLib-java", - "version": "2024.1.0" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.10.1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.choreo.lib", - "artifactId": "ChoreoLib-cpp", - "version": "2024.1.0", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file From 3c1cde922bfd50ff7eed40bcd47d8e09a7c331d2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 11 Jan 2024 23:13:18 -0800 Subject: [PATCH 14/19] final attempts at 4 note on alpha --- choreo.chor | 1475 ++++++++--------- ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat | Bin 2048 -> 2048 bytes ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat | Bin 2048 -> 2048 bytes deploy/choreo/amp 4 local.traj | 760 +++++++++ deploy/choreo/test.traj | 1093 ++++++++++++ .../deploy/choreo/amp 4 local sgmt 1.traj | 602 +++---- .../deploy/choreo/amp 4 local sgmt 2.traj | 332 ++-- .../deploy/choreo/amp 4 local sgmt 3.traj | 440 ++--- .../deploy/pathplanner/autos/local 4.auto | 12 +- src/main/java/frc/robot/Robot.java | 10 +- .../robot/subsystems/pivot/PivotIOReal.java | 11 +- 12 files changed, 3207 insertions(+), 1528 deletions(-) create mode 100644 deploy/choreo/amp 4 local.traj create mode 100644 deploy/choreo/test.traj diff --git a/choreo.chor b/choreo.chor index 4f09fee3..6d6f1b3a 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1,15 +1,15 @@ { - "version": "v0.2", + "version": "v0.2.1", "robotConfiguration": { - "mass": 37.04398850154597, - "rotationalInertia": 2.8, - "motorMaxTorque": 0.3, - "motorMaxVelocity": 4864, - "gearing": 8.16, - "wheelbase": 0.5206997188221518, - "trackWidth": 0.5206997188221518, - "bumperLength": 0.8508995405142481, - "bumperWidth": 0.8508995405142481, + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 1.162295081967213, + "motorMaxVelocity": 4800, + "gearing": 6.75, + "wheelbase": 0.5778496879611685, + "trackWidth": 0.5778496879611685, + "bumperLength": 0.8762995267982555, + "bumperWidth": 0.8762995267982555, "wheelRadius": 0.050799972568014815 }, "paths": { @@ -22,7 +22,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 14 }, { "x": 2.1557445526123047, @@ -31,7 +31,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 8 + "controlIntervalCount": 6 }, { "x": 2.7185182571411133, @@ -40,7 +40,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 11 }, { "x": 1.3818649053573608, @@ -49,7 +49,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 9 }, { "x": 2.59291672706604, @@ -58,7 +58,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 8 }, { "x": 1.3892531394958496, @@ -67,21 +67,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 13 }, { - "x": 2.541198492050171, - "y": 4.256927967071533, + "x": 2.764192819595337, + "y": 4.415045738220215, "heading": -0.6391760227970993, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 12 }, { - "x": 1.4994455575942993, - "y": 4.286480903625488, - "heading": -0.7254698893067673, + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071235, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -90,760 +90,670 @@ ], "trajectory": [ { - "x": 0.7470832465334709, - "y": 6.625458718233556, - "heading": 1.0564954979067924, - "angularVelocity": -5.684301329861994e-12, - "velocityX": -3.602456269610706e-10, - "velocityY": 4.1137821262407583e-10, + "x": 0.7470832467079163, + "y": 6.625458717346191, + "heading": 1.056495497952334, + "angularVelocity": -4.3186177103351096e-19, + "velocityX": 7.079282317781465e-18, + "velocityY": -1.220945161386867e-18, "timestamp": 0 }, { - "x": 0.7589569772775385, - "y": 6.629936126338657, - "heading": 1.0434265097431823, - "angularVelocity": -0.12778392744309977, - "velocityX": 0.11609712676013798, - "velocityY": 0.04377849867759475, - "timestamp": 0.10227411521599863 - }, - { - "x": 0.7822022838432713, - "y": 6.638629979602176, - "heading": 1.0183346599429852, - "angularVelocity": -0.2453392014361959, - "velocityX": 0.2272843592055021, - "velocityY": 0.08500540295845989, - "timestamp": 0.20454823043199727 - }, - { - "x": 0.816345318524229, - "y": 6.6512661070932495, - "heading": 0.9822355978698938, - "angularVelocity": -0.35296381641965074, - "velocityX": 0.33383847695279084, - "velocityY": 0.12355156118457596, - "timestamp": 0.30682234564799593 - }, - { - "x": 0.8609432271767217, - "y": 6.667555740683275, - "heading": 0.9361120278121537, - "angularVelocity": -0.45097989764859636, - "velocityX": 0.4360625240585109, - "velocityY": 0.15927424713723962, - "timestamp": 0.40909646086399454 - }, - { - "x": 0.9155883048461597, - "y": 6.687191502411657, - "heading": 0.8809291072748399, - "angularVelocity": -0.5395590113414345, - "velocityX": 0.5343001768187209, - "velocityY": 0.19199150048411856, - "timestamp": 0.5113705760799931 - }, - { - "x": 0.9799143577111046, - "y": 6.709840447255909, - "heading": 0.817662056557047, - "angularVelocity": -0.6186027670698295, - "velocityX": 0.6289573163604619, - "velocityY": 0.22145333556813593, - "timestamp": 0.6136446912959918 - }, - { - "x": 1.0536065024635548, - "y": 6.735133328172977, - "heading": 0.7473295527982347, - "angularVelocity": -0.687686259272288, - "velocityX": 0.7205356386732681, - "velocityY": 0.24730480411634378, - "timestamp": 0.7159188065119904 - }, - { - "x": 1.1364169197016813, - "y": 6.762648211314195, - "heading": 0.6710254557943266, - "angularVelocity": -0.7460743786173882, - "velocityX": 0.8096908708297301, - "velocityY": 0.2690307532427302, - "timestamp": 0.818192921727989 - }, - { - "x": 1.2281932228229642, - "y": 6.7918845460908575, - "heading": 0.5899378478266369, - "angularVelocity": -0.7928458510838684, - "velocityX": 0.8973561208709863, - "velocityY": 0.28586249636547506, - "timestamp": 0.9204670369439876 - }, - { - "x": 1.3289409008958384, - "y": 6.822217824239773, - "heading": 0.5053285615301966, - "angularVelocity": -0.8272795718879136, - "velocityX": 0.985075041075245, - "velocityY": 0.296588019337818, - "timestamp": 1.0227411521599863 - }, - { - "x": 1.4390031913745853, - "y": 6.852792823991799, - "heading": 0.4183893727045359, - "angularVelocity": -0.8500605319021087, - "velocityX": 1.076150015287458, - "velocityY": 0.29895148738224403, - "timestamp": 1.125015267375985 - }, - { - "x": 1.5597885498551032, - "y": 6.882059112916124, - "heading": 0.32979940511385675, - "angularVelocity": -0.8662012607776253, - "velocityX": 1.1809963684348201, - "velocityY": 0.28615537738935176, - "timestamp": 1.2272893825919837 - }, - { - "x": 1.700662064049805, - "y": 6.9025972281907375, - "heading": 0.2406449492427404, - "angularVelocity": -0.8717206268168672, - "velocityX": 1.3774112254273, - "velocityY": 0.20081439319892427, - "timestamp": 1.3295634978079824 - }, - { - "x": 1.8510164004103928, - "y": 6.913603640101383, - "heading": 0.18435642050200896, - "angularVelocity": -0.5503692556839603, - "velocityX": 1.4701113396826178, - "velocityY": 0.10761678064700267, - "timestamp": 1.4318376130239812 - }, - { - "x": 2.0039488544462833, - "y": 6.918843013673937, - "heading": 0.15617120081925578, - "angularVelocity": -0.2755850738942481, - "velocityX": 1.4953192592999736, - "velocityY": 0.051228726583383746, - "timestamp": 1.5341117282399799 - }, - { - "x": 2.1557445524327394, - "y": 6.919295783765447, - "heading": 0.15617120073466711, - "angularVelocity": -8.01993499988118e-11, - "velocityX": 1.484204462099971, - "velocityY": 0.0044270165813428694, - "timestamp": 1.6363858434559786 - }, - { - "x": 2.232501376452394, - "y": 6.918644981823818, - "heading": 0.15617120066422163, - "angularVelocity": -2.7881719949913897e-12, - "velocityX": 1.4631774376072004, - "velocityY": -0.012405855819009865, - "timestamp": 1.6888448428916174 - }, - { - "x": 2.3078780299226054, - "y": 6.916920762093343, - "heading": 0.1561712005937278, - "angularVelocity": -3.698975819730093e-12, - "velocityX": 1.436867928003674, - "velocityY": -0.032867889033463694, - "timestamp": 1.7413038423272562 - }, - { - "x": 2.381659462849091, - "y": 6.91403766609857, - "heading": 0.15617120052323402, - "angularVelocity": -3.687857737825465e-12, - "velocityX": 1.4064590265589882, - "velocityY": -0.05495897496281437, - "timestamp": 1.793762841762895 - }, - { - "x": 2.453627858185915, - "y": 6.909923503320222, - "heading": 0.15617120045273983, - "angularVelocity": -3.68806627753873e-12, - "velocityX": 1.3718979863339387, - "velocityY": -0.07842619311071317, - "timestamp": 1.8462218411985338 - }, - { - "x": 2.5235476514141553, - "y": 6.90450568335544, - "heading": 0.15617120038224525, - "angularVelocity": -3.688143550349055e-12, - "velocityX": 1.3328464960153679, - "velocityY": -0.10327716626043287, - "timestamp": 1.8986808406341726 - }, - { - "x": 2.5911718313337544, - "y": 6.897715977103231, - "heading": 0.1561712003117504, - "angularVelocity": -3.687560329325201e-12, - "velocityX": 1.2890863531735766, - "velocityY": -0.12942875706821344, - "timestamp": 1.9511398400698114 - }, - { - "x": 2.65624736130467, - "y": 6.889492702059127, - "heading": 0.15617120024125528, - "angularVelocity": -3.68944362324301e-12, - "velocityX": 1.240502697821755, - "velocityY": -0.1567561709974853, - "timestamp": 2.0035988395054503 - }, - { - "x": 2.7185182573768354, - "y": 6.879780772610566, - "heading": 0.1561712001707086, - "angularVelocity": -4.669569996251489e-12, - "velocityX": 1.187039346844362, - "velocityY": -0.1851336547556834, - "timestamp": 2.056057838941089 - }, - { - "x": 2.844727229024539, - "y": 6.826831262995891, - "heading": 0.36522912798339696, - "angularVelocity": 1.2815424227266763, - "velocityX": 0.7736714562084378, - "velocityY": -0.32458488264739044, - "timestamp": 2.219187770584854 - }, - { - "x": 2.905891887894138, - "y": 6.746880408353911, - "heading": 0.5636352419280429, - "angularVelocity": 1.2162459209838357, - "velocityX": 0.3749444284785087, - "velocityY": -0.490105365340026, - "timestamp": 2.382317702228619 - }, - { - "x": 2.909090532344001, - "y": 6.6442122814929245, - "heading": 0.7377210107974022, - "angularVelocity": 1.0671601901772823, - "velocityX": 0.0196079562833389, - "velocityY": -0.6293641266849203, - "timestamp": 2.5454476338723837 - }, - { - "x": 2.8528015320306115, - "y": 6.520167648469734, - "heading": 0.8617887991289785, - "angularVelocity": 0.7605458243522792, - "velocityX": -0.34505623601923857, - "velocityY": -0.7604038809886424, - "timestamp": 2.7085775655161486 - }, - { - "x": 2.6673775887021587, - "y": 6.358333521535233, - "heading": 0.8824380102910534, - "angularVelocity": 0.1265813757217392, - "velocityX": -1.1366641384675562, - "velocityY": -0.9920566103835737, - "timestamp": 2.8717074971599135 - }, - { - "x": 2.433334548523251, - "y": 6.191208743378355, - "heading": 0.7641593769894643, - "angularVelocity": -0.7250578241718747, - "velocityX": -1.4347032313583656, - "velocityY": -1.0244887408273557, - "timestamp": 3.0348374288036783 - }, - { - "x": 2.190845809692594, - "y": 6.024313186516238, - "heading": 0.6297541443819642, - "angularVelocity": -0.823915214372992, - "velocityX": -1.486476064011715, - "velocityY": -1.0230835952941921, - "timestamp": 3.1979673604474432 - }, - { - "x": 1.9580404786548902, - "y": 5.870547470854263, - "heading": 0.47066977289213696, - "angularVelocity": -0.9752003809508628, - "velocityX": -1.4271159717504125, - "velocityY": -0.9425965806313563, - "timestamp": 3.361097292091208 - }, - { - "x": 1.7498273467354928, - "y": 5.73486661370809, - "heading": 0.32338313207816166, - "angularVelocity": -0.9028793141986351, - "velocityX": -1.2763637531323493, - "velocityY": -0.831734899187721, - "timestamp": 3.524227223734973 - }, - { - "x": 1.577014249967444, - "y": 5.624033816048722, - "heading": 0.19882585300104294, - "angularVelocity": -0.7635464431083702, - "velocityX": -1.0593586039568081, - "velocityY": -0.6794142353068856, - "timestamp": 3.687357155378738 - }, - { - "x": 1.4506486184747536, - "y": 5.5441849844920235, - "heading": 0.1067515452077215, - "angularVelocity": -0.5644231371867715, - "velocityX": -0.7746317925903302, - "velocityY": -0.4894799553714506, - "timestamp": 3.8504870870225028 - }, - { - "x": 1.3818649056319274, - "y": 5.50129938101995, - "heading": 0.05628563946839357, - "angularVelocity": -0.30936018439404245, - "velocityX": -0.4216498592955241, - "velocityY": -0.2628923039688523, - "timestamp": 4.013617018666268 - }, - { - "x": 1.3818649054659777, - "y": 5.501299381110093, - "heading": 0.05628563946121549, - "angularVelocity": -8.20711890061584e-12, - "velocityX": -2.918186949691226e-10, - "velocityY": -8.516677331367793e-11, - "timestamp": 4.176746950310032 - }, - { - "x": 1.4254749911225055, - "y": 5.4996771535618185, - "heading": 0.052286234874463405, - "angularVelocity": -0.03818204740477268, - "velocityX": 0.41634256351427257, - "velocityY": -0.015487297221703413, - "timestamp": 4.281492632082638 - }, - { - "x": 1.5054277602383004, - "y": 5.496460245033646, - "heading": 0.04534031368940726, - "angularVelocity": -0.06631224378557163, - "velocityX": 0.763303725485265, - "velocityY": -0.030711609603001202, - "timestamp": 4.386238313855244 - }, - { - "x": 1.614706352195534, - "y": 5.491742221647794, - "heading": 0.036622661203774276, - "angularVelocity": -0.08322684370600701, - "velocityX": 1.0432753896642522, - "velocityY": -0.045042652489690874, - "timestamp": 4.4909839956278494 - }, - { - "x": 1.7468754648697162, - "y": 5.485736649446459, - "heading": 0.02746720464328704, - "angularVelocity": -0.08740652985101428, - "velocityX": 1.2618096559898304, - "velocityY": -0.05733479469253905, - "timestamp": 4.595729677400455 - }, - { - "x": 1.8974421226741423, - "y": 5.478814465512338, - "heading": 0.0192537076414664, - "angularVelocity": -0.07841370510897432, - "velocityX": 1.43744978567254, - "velocityY": -0.06608562544064354, - "timestamp": 4.700475359173061 - }, - { - "x": 2.0478771487383725, - "y": 5.472018862260595, - "heading": 0.011842428389913316, - "angularVelocity": -0.07075498604111852, - "velocityX": 1.436193106275662, - "velocityY": -0.06487716815402857, - "timestamp": 4.805221040945667 - }, - { - "x": 2.1991998312667134, - "y": 5.465297323381209, - "heading": 0.007319920190768939, - "angularVelocity": -0.04317608251475354, - "velocityX": 1.4446675030153622, - "velocityY": -0.06417008055763802, - "timestamp": 4.909966722718273 - }, - { - "x": 2.337086103631991, - "y": 5.4645313180146555, - "heading": 0.0021217678335590726, - "angularVelocity": -0.04962641198543019, - "velocityX": 1.316390996187784, - "velocityY": -0.007313001468104138, - "timestamp": 5.014712404490878 - }, - { - "x": 2.45341531313919, - "y": 5.464492524135499, - "heading": -0.0044756440905307565, - "angularVelocity": -0.06298504924092899, - "velocityX": 1.1105871626237722, - "velocityY": -0.0003703621622037101, - "timestamp": 5.119458086263484 - }, - { - "x": 2.541111145262716, - "y": 5.463618987693384, - "heading": -0.012133903650681083, - "angularVelocity": -0.07311289048562652, - "velocityX": 0.8372262287970834, - "velocityY": -0.008339593435811936, - "timestamp": 5.22420376803609 - }, - { - "x": 2.592916727068705, - "y": 5.461223602405139, - "heading": -0.020267736871702392, - "angularVelocity": -0.07765316033563659, - "velocityX": 0.4945844156915881, - "velocityY": -0.022868582319897673, - "timestamp": 5.328949449808696 - }, - { - "x": 2.573983182200651, - "y": 5.456183859404588, - "heading": -0.02941680850548689, - "angularVelocity": -0.07029420270393813, - "velocityX": -0.14547032678251723, - "velocityY": -0.03872138453206129, - "timestamp": 5.459103449457666 - }, - { - "x": 2.4879155076147748, - "y": 5.449739339031148, - "heading": -0.03668510065770142, - "angularVelocity": -0.05584378637679435, - "velocityX": -0.6612756796636089, - "velocityY": -0.04951457856894505, - "timestamp": 5.589257449106635 - }, - { - "x": 2.35046961428428, - "y": 5.4436438239165845, - "heading": -0.03977633600135786, - "angularVelocity": -0.02375059815495546, - "velocityX": -1.056025121733672, - "velocityY": -0.046833099245860266, - "timestamp": 5.719411448755605 - }, - { - "x": 2.1764781877651598, - "y": 5.443212327064482, - "heading": -0.032264347817817784, - "angularVelocity": 0.05771615319957162, - "velocityX": -1.336811984121226, - "velocityY": -0.0033152797571240995, - "timestamp": 5.849565448404575 - }, - { - "x": 1.9752868983122995, - "y": 5.4534701667830054, - "heading": -0.009494098721117665, - "angularVelocity": 0.174948516049147, - "velocityX": -1.5457941360881122, - "velocityY": 0.07881309585526655, - "timestamp": 5.979719448053545 - }, - { - "x": 1.7725080662079153, - "y": 5.4634789711909475, - "heading": 0.004869525328448035, - "angularVelocity": 0.11035868346800264, - "velocityX": -1.5579915534229911, - "velocityY": 0.07689970624561784, - "timestamp": 6.1098734477025145 - }, - { - "x": 1.5970859275969194, - "y": 5.473760403676058, - "heading": 0.019510408429697427, - "angularVelocity": 0.11248892189634652, - "velocityX": -1.347804439948399, - "velocityY": 0.07899436394267238, - "timestamp": 6.240027447351484 - }, - { - "x": 1.463998994938607, - "y": 5.481958295434183, - "heading": 0.031093440957313423, - "angularVelocity": 0.0889948258110792, - "velocityX": -1.0225343286428261, - "velocityY": 0.06298609118419185, - "timestamp": 6.370181447000454 - }, - { - "x": 1.3892531402609951, - "y": 5.486522674152253, - "heading": 0.03752685288213878, - "angularVelocity": 0.049429229542960515, - "velocityX": -0.5742878042328909, - "velocityY": 0.03506906174326123, - "timestamp": 6.500335446649424 - }, - { - "x": 1.3892531401370203, - "y": 5.486522674374907, - "heading": 0.03752685288891443, - "angularVelocity": 5.018347137854837e-11, - "velocityX": -8.981333628450582e-10, - "velocityY": 1.1863234183511445e-9, - "timestamp": 6.630489446298394 - }, - { - "x": 1.410785208000178, - "y": 5.466798705442517, - "heading": 0.02538794622123859, - "angularVelocity": -0.12074981514388294, - "velocityX": 0.21418676347263918, - "velocityY": -0.1962009963268651, - "timestamp": 6.731018848208977 - }, - { - "x": 1.4512665581597533, - "y": 5.429718184658836, - "heading": 0.002445244265967334, - "angularVelocity": -0.2282188250282345, - "velocityX": 0.4026816906907998, - "velocityY": -0.36885249450974994, - "timestamp": 6.8315482501195595 - }, - { - "x": 1.5081514675148215, - "y": 5.3776444456377, - "heading": -0.029986935931297812, - "angularVelocity": -0.3226138782330449, - "velocityX": 0.5658534482437384, - "velocityY": -0.5179951130124598, - "timestamp": 6.932077652030142 - }, - { - "x": 1.578941487701644, - "y": 5.312948064530918, - "heading": -0.07059739560458454, - "angularVelocity": -0.4039659933419245, - "velocityX": 0.7041722939494359, - "velocityY": -0.6435568078574709, - "timestamp": 7.032607053940725 - }, - { - "x": 1.6611946188865518, - "y": 5.238029568056684, - "heading": -0.11800108662062794, - "angularVelocity": -0.4715405656932785, - "velocityX": 0.8181997404499256, - "velocityY": -0.7452396506196601, - "timestamp": 7.133136455851308 - }, - { - "x": 1.7524893345353523, - "y": 5.155358397604862, - "heading": -0.17044488274189246, - "angularVelocity": -0.521676197753198, - "velocityX": 0.9081394425873396, - "velocityY": -0.8223581237255294, - "timestamp": 7.233665857761891 - }, - { - "x": 1.8499500532222615, - "y": 5.067431436588914, - "heading": -0.22499900927916092, - "angularVelocity": -0.5426683689609221, - "velocityX": 0.9694747626122975, - "velocityY": -0.8746392520744162, - "timestamp": 7.334195259672474 - }, - { - "x": 1.9473638091809122, - "y": 4.979075974351683, - "heading": -0.27982409115325835, - "angularVelocity": -0.5453636534436577, - "velocityX": 0.9690076084526664, - "velocityY": -0.8789016988127862, - "timestamp": 7.434724661583057 - }, - { - "x": 2.05818821772957, - "y": 4.854821291737014, - "heading": -0.34544281347225186, - "angularVelocity": -0.6527316496725523, - "velocityX": 1.1024079106043232, - "velocityY": -1.2360033993617228, - "timestamp": 7.53525406349364 - }, - { - "x": 2.1664425710584037, - "y": 4.732231232720307, - "heading": -0.4041393246178968, - "angularVelocity": -0.5838740711290391, - "velocityX": 1.076842701113489, - "velocityY": -1.2194448247922516, - "timestamp": 7.635783465404223 - }, - { - "x": 2.2666201121554423, - "y": 4.61627032113389, - "heading": -0.4602072656334145, - "angularVelocity": -0.557726794034664, - "velocityX": 0.996499915039965, - "velocityY": -1.153502450677793, - "timestamp": 7.736312867314806 - }, - { - "x": 2.356480498307365, - "y": 4.5087945921361055, - "heading": -0.5124957847583047, - "angularVelocity": -0.5201316047807618, - "velocityX": 0.8938716814183492, - "velocityY": -1.0690974663909212, - "timestamp": 7.8368422692253885 - }, - { - "x": 2.433798199341425, - "y": 4.411691494217003, - "heading": -0.5602485683833155, - "angularVelocity": -0.47501310796660834, - "velocityX": 0.7691053456052521, - "velocityY": -0.9659173928845837, - "timestamp": 7.937371671135971 - }, - { - "x": 2.4961951429135376, - "y": 4.327016530758609, - "heading": -0.6027268742617791, - "angularVelocity": -0.4225460918867537, - "velocityX": 0.620683518971687, - "velocityY": -0.8422905312450144, - "timestamp": 8.037901073046553 - }, - { - "x": 2.541198492257727, - "y": 4.256927967764492, - "heading": -0.6391760227839379, - "angularVelocity": -0.3625720221944257, - "velocityX": 0.4476635477259144, - "velocityY": -0.6971946668803632, - "timestamp": 8.138430474957136 - }, - { - "x": 2.5331758624705114, - "y": 4.192320334785442, - "heading": -0.6763224470307224, - "angularVelocity": -0.2449990301925222, - "velocityX": -0.052913214713167914, - "velocityY": -0.426119279886695, - "timestamp": 8.290049133403663 - }, - { - "x": 2.46411262676954, - "y": 4.160473976285577, - "heading": -0.7018758155970488, - "angularVelocity": -0.16853709734646205, - "velocityX": -0.45550618406914944, - "velocityY": -0.21004248027074415, - "timestamp": 8.44166779185019 - }, - { - "x": 2.348779581051416, - "y": 4.153307846262027, - "heading": -0.7202693675207003, - "angularVelocity": -0.121314567375086, - "velocityX": -0.7606784530447123, - "velocityY": -0.04726417363495295, - "timestamp": 8.593286450296716 - }, - { - "x": 2.201596779448434, - "y": 4.163036990055964, - "heading": -0.7333684069992377, - "angularVelocity": -0.0863946406365267, - "velocityX": -0.9707433369527995, - "velocityY": 0.06416850879734587, - "timestamp": 8.744905108743243 - }, - { - "x": 2.03723470076713, - "y": 4.1831659239379135, - "heading": -0.7500897294060416, - "angularVelocity": -0.11028538710126731, - "velocityX": -1.0840491612778367, - "velocityY": 0.13276026521150996, - "timestamp": 8.89652376718977 - }, - { - "x": 1.8800877783569956, - "y": 4.195672966501764, - "heading": -0.7289968148061016, - "angularVelocity": 0.13911819814698503, - "velocityX": -1.0364616391016628, - "velocityY": 0.08249012434493122, - "timestamp": 9.048142425636296 - }, - { - "x": 1.7046442157621393, - "y": 4.233321160679635, - "heading": -0.7238359462014516, - "angularVelocity": 0.03403847932720532, - "velocityX": -1.1571370254440554, - "velocityY": 0.24830844691554524, - "timestamp": 9.199761084082823 - }, - { - "x": 1.572815192920781, - "y": 4.2658136358994225, - "heading": -0.7241682787394389, - "angularVelocity": -0.002191897597681109, - "velocityX": -0.8694775764003041, - "velocityY": 0.21430393183203963, - "timestamp": 9.35137974252935 - }, - { - "x": 1.4994455554717883, - "y": 4.286480902556393, - "heading": -0.7254698893818832, - "angularVelocity": -0.00858476582372283, - "velocityX": -0.4839090323218549, - "velocityY": 0.13631083596916982, - "timestamp": 9.502998400975876 - }, - { - "x": 1.4994455567524874, - "y": 4.286480903049883, - "heading": -0.7254698893413497, - "angularVelocity": 3.8000232610425283e-11, - "velocityX": 2.8946950656209713e-9, - "velocityY": -5.415769760526697e-10, - "timestamp": 9.654617059422403 + "x": 0.7613714426488418, + "y": 6.628614589829769, + "heading": 1.0345345122436784, + "angularVelocity": -0.5024975798156311, + "velocityX": 0.32693358920665033, + "velocityY": 0.07221070612415084, + "timestamp": 0.04370366463598273 + }, + { + "x": 0.7899910246485671, + "y": 6.635101738465351, + "heading": 0.9912579578255775, + "angularVelocity": -0.9902271303461743, + "velocityX": 0.6548554277565509, + "velocityY": 0.14843488960512216, + "timestamp": 0.08740732927196546 + }, + { + "x": 0.8329823079677272, + "y": 6.6450825712167525, + "heading": 0.9272173132517384, + "angularVelocity": -1.465338092520316, + "velocityX": 0.9836997349591596, + "velocityY": 0.22837519083431007, + "timestamp": 0.13111099390794817 + }, + { + "x": 0.8904118185530745, + "y": 6.658650997634064, + "heading": 0.8429907149485981, + "angularVelocity": -1.927220497518482, + "velocityX": 1.3140662473888705, + "velocityY": 0.3104642718253751, + "timestamp": 0.1748146585439309 + }, + { + "x": 0.962426123217969, + "y": 6.675814256462545, + "heading": 0.7397363294626891, + "angularVelocity": -2.362602457847324, + "velocityX": 1.6477864102408268, + "velocityY": 0.39271898527129734, + "timestamp": 0.21851832317991365 + }, + { + "x": 1.0493031568120121, + "y": 6.696537442602017, + "heading": 0.6202474925482772, + "angularVelocity": -2.7340690514092785, + "velocityX": 1.9878661049974005, + "velocityY": 0.47417502198223827, + "timestamp": 0.2622219878158964 + }, + { + "x": 1.1514382857503924, + "y": 6.720845269246716, + "heading": 0.4905752358986126, + "angularVelocity": -2.9670797112720706, + "velocityX": 2.3369923275104343, + "velocityY": 0.5561965305921692, + "timestamp": 0.30592565245187914 + }, + { + "x": 1.269055326004296, + "y": 6.748845004507445, + "heading": 0.3629054511567606, + "angularVelocity": -2.921260397846258, + "velocityX": 2.6912397675014597, + "velocityY": 0.6406724812196789, + "timestamp": 0.3496293170878619 + }, + { + "x": 1.400011708453663, + "y": 6.779996215891522, + "heading": 0.2672618294840015, + "angularVelocity": -2.188457706450791, + "velocityX": 2.996462277022566, + "velocityY": 0.7127825925706782, + "timestamp": 0.3933329817238446 + }, + { + "x": 1.5437969816492523, + "y": 6.812790254409381, + "heading": 0.2092081541239441, + "angularVelocity": -1.3283479965261176, + "velocityX": 3.2900049548066255, + "velocityY": 0.7503727385565244, + "timestamp": 0.43703664635982736 + }, + { + "x": 1.6999373892672376, + "y": 6.847694256712915, + "heading": 0.1915003494540967, + "angularVelocity": -0.40517894362723794, + "velocityX": 3.572707435829763, + "velocityY": 0.7986516140981349, + "timestamp": 0.4807403109958101 + }, + { + "x": 1.8624433107711578, + "y": 6.878112633593708, + "heading": 0.1915002067487222, + "angularVelocity": -0.0000032652953862886492, + "velocityX": 3.7183591549465618, + "velocityY": 0.6960143304721129, + "timestamp": 0.5244439756317928 + }, + { + "x": 2.015535426159542, + "y": 6.900266962585095, + "heading": 0.1561712001056634, + "angularVelocity": -0.8083762983569384, + "velocityX": 3.502958313988489, + "velocityY": 0.5069215402397582, + "timestamp": 0.5681476402677755 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0.15617120010566338, + "angularVelocity": 8.052388519770604e-18, + "velocityX": 3.208177795171024, + "velocityY": 0.4354057121910402, + "timestamp": 0.6118513049037583 + }, + { + "x": 2.2979527331593355, + "y": 6.936286453257653, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442958129174e-19, + "velocityX": 2.796432452004192, + "velocityY": 0.33411051356290106, + "timestamp": 0.6627047344028709 + }, + { + "x": 2.4202451824897016, + "y": 6.945011127796279, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959053082e-19, + "velocityX": 2.404802400445729, + "velocityY": 0.1715651161497845, + "timestamp": 0.7135581639019836 + }, + { + "x": 2.52317206188046, + "y": 6.94425224667938, + "heading": 0.15617120010566338, + "angularVelocity": -1.128244295905312e-19, + "velocityX": 2.0239909167296966, + "velocityY": -0.014922909317535413, + "timestamp": 0.7644115934010962 + }, + { + "x": 2.6070605614479327, + "y": 6.933368973549914, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959052985e-19, + "velocityX": 1.6496134163171314, + "velocityY": -0.21401256978465483, + "timestamp": 0.8152650229002089 + }, + { + "x": 2.672125506615784, + "y": 6.911967284292252, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442956919517e-19, + "velocityX": 1.2794603197604928, + "velocityY": -0.4208504611874918, + "timestamp": 0.8661184523993215 + }, + { + "x": 2.7185182571411133, + "y": 6.8797807693481445, + "heading": 0.15617120010566338, + "angularVelocity": -1.0748383020939867e-19, + "velocityX": 0.9122836155256804, + "velocityY": -0.6329271252919907, + "timestamp": 0.9169718818984341 + }, + { + "x": 2.742166614394153, + "y": 6.787220144570856, + "heading": 0.15109105455160673, + "angularVelocity": -0.05590353076450862, + "velocityX": 0.26023401360416243, + "velocityY": -1.0185664327447723, + "timestamp": 1.0078453112611694 + }, + { + "x": 2.7063747461891747, + "y": 6.659930157895132, + "heading": 0.14109258501335736, + "angularVelocity": -0.11002632571879596, + "velocityX": -0.39386505446064113, + "velocityY": -1.4007393312694816, + "timestamp": 1.0987187406239047 + }, + { + "x": 2.6108392240840024, + "y": 6.498432494186991, + "heading": 0.12644294397632044, + "angularVelocity": -0.16120929010568202, + "velocityX": -1.0513031452112058, + "velocityY": -1.7771714442897992, + "timestamp": 1.18959216998664 + }, + { + "x": 2.4549786573639274, + "y": 6.303757740845773, + "heading": 0.10766682633359297, + "angularVelocity": -0.20661834569682533, + "velocityX": -1.715139043536396, + "velocityY": -2.1422626471390678, + "timestamp": 1.2804655993493754 + }, + { + "x": 2.237238983313492, + "y": 6.078890290802275, + "heading": 0.08626225710840621, + "angularVelocity": -0.23554265944719108, + "velocityX": -2.396076340217028, + "velocityY": -2.4745126448998374, + "timestamp": 1.3713390287121108 + }, + { + "x": 1.9563226558789764, + "y": 5.880741504295724, + "heading": 0.08626219926626796, + "angularVelocity": -6.365132100594054e-7, + "velocityX": -3.0912922446581543, + "velocityY": -2.1804920084573034, + "timestamp": 1.4622124580748461 + }, + { + "x": 1.7274737983405954, + "y": 5.727569361833653, + "heading": 0.0766627418429575, + "angularVelocity": -0.10563547002277199, + "velocityX": -2.5183253140463755, + "velocityY": -1.6855547714685755, + "timestamp": 1.5530858874375815 + }, + { + "x": 1.5549556420919104, + "y": 5.613995417389302, + "heading": 0.06722021492045382, + "angularVelocity": -0.10390855708563887, + "velocityX": -1.8984444348419232, + "velocityY": -1.2498036581298453, + "timestamp": 1.6439593168003168 + }, + { + "x": 1.4396298957459885, + "y": 5.538759046250953, + "heading": 0.06010941807852643, + "angularVelocity": -0.07824946072569235, + "velocityX": -1.269081041121286, + "velocityY": -0.8279248584097305, + "timestamp": 1.7348327461630522 + }, + { + "x": 1.3818649053573608, + "y": 5.5012993812561035, + "heading": 0.05628563945524819, + "angularVelocity": -0.0420780711159847, + "velocityX": -0.6356642507464983, + "velocityY": -0.4122180185951169, + "timestamp": 1.8257061755257875 + }, + { + "x": 1.3818649053573608, + "y": 5.5012993812561035, + "heading": 0.05628563945524819, + "angularVelocity": 1.3994093358769211e-21, + "velocityX": -9.902038153093951e-18, + "velocityY": -5.706702901855187e-18, + "timestamp": 1.9165796048885229 + }, + { + "x": 1.4353793564287938, + "y": 5.499850139119754, + "heading": 0.05330774673729568, + "angularVelocity": -0.037162247354032396, + "velocityX": 0.6678270361261427, + "velocityY": -0.018085639694878305, + "timestamp": 1.9967118070554994 + }, + { + "x": 1.5424082559070336, + "y": 5.49695165466464, + "heading": 0.04735195633823623, + "angularVelocity": -0.07432455664514218, + "velocityX": 1.335654038999412, + "velocityY": -0.036171281666197166, + "timestamp": 2.076844009222476 + }, + { + "x": 1.7029515984698416, + "y": 5.492603927569323, + "heading": 0.03841826071733368, + "angularVelocity": -0.11148696003994531, + "velocityX": 2.003480975454455, + "velocityY": -0.05425692764886357, + "timestamp": 2.156976211389453 + }, + { + "x": 1.9170093681982305, + "y": 5.486806957092212, + "heading": 0.026506650941055532, + "angularVelocity": -0.1486494749196728, + "velocityX": 2.671307713250448, + "velocityY": -0.07234258288609449, + "timestamp": 2.2371084135564296 + }, + { + "x": 2.1592197171124052, + "y": 5.478791804392635, + "heading": 0.011195918948050757, + "angularVelocity": -0.19106840419911095, + "velocityX": 3.0226343762457084, + "velocityY": -0.10002411618334738, + "timestamp": 2.3172406157234064 + }, + { + "x": 2.3479156387807323, + "y": 5.472225891398255, + "heading": -0.0011368905306301479, + "angularVelocity": -0.1539057850049126, + "velocityX": 2.3548076374480407, + "velocityY": -0.08193850682774677, + "timestamp": 2.3973728178903833 + }, + { + "x": 2.483097117399865, + "y": 5.467109219705175, + "heading": -0.010491771187470106, + "angularVelocity": -0.11674308709682761, + "velocityX": 1.6869807014344356, + "velocityY": -0.06385287755374197, + "timestamp": 2.47750502005736 + }, + { + "x": 2.564764147705788, + "y": 5.463441789903776, + "heading": -0.01686872035025771, + "angularVelocity": -0.0795803558412077, + "velocityX": 1.0191536997292066, + "velocityY": -0.04576724091218098, + "timestamp": 2.557637222224337 + }, + { + "x": 2.59291672706604, + "y": 5.461223602294922, + "heading": -0.020267736871838693, + "angularVelocity": -0.042417610269816795, + "velocityX": 0.3513266651724994, + "velocityY": -0.027681600516002228, + "timestamp": 2.6377694243913137 + }, + { + "x": 2.5441490516673415, + "y": 5.460719866140607, + "heading": -0.019850376221801108, + "angularVelocity": 0.004155972098076838, + "velocityX": -0.48561621280493705, + "velocityY": -0.0050160775864576445, + "timestamp": 2.7381937391336377 + }, + { + "x": 2.4113319639164366, + "y": 5.462492299555472, + "heading": -0.014755896628636694, + "angularVelocity": 0.050729543001973076, + "velocityX": -1.3225590644227596, + "velocityY": 0.017649444951779485, + "timestamp": 2.8386180538759618 + }, + { + "x": 2.194465469101637, + "y": 5.466540902467684, + "heading": -0.004984302193896615, + "angularVelocity": 0.09730307306365722, + "velocityX": -2.1595018633809024, + "velocityY": 0.040314966774733244, + "timestamp": 2.939042368618286 + }, + { + "x": 1.8935495831026135, + "y": 5.472865675007542, + "heading": 0.009464396467489124, + "angularVelocity": 0.14387649742454536, + "velocityX": -2.9964445042133008, + "velocityY": 0.0629804898951702, + "timestamp": 3.03946668336061 + }, + { + "x": 1.641401371888256, + "y": 5.479694173969964, + "heading": 0.023495626181635927, + "angularVelocity": 0.13971944693024915, + "velocityX": -2.510828297522737, + "velocityY": 0.06799647057530608, + "timestamp": 3.139890998102934 + }, + { + "x": 1.473302552055367, + "y": 5.484246507552045, + "heading": 0.03284977683623274, + "angularVelocity": 0.09314627317695277, + "velocityX": -1.6738856547262355, + "velocityY": 0.04533098974846174, + "timestamp": 3.240315312845258 + }, + { + "x": 1.3892531394958496, + "y": 5.486522674560547, + "heading": 0.03752685285960867, + "angularVelocity": 0.04657314351984077, + "velocityX": -0.8369428536821739, + "velocityY": 0.02266549703966028, + "timestamp": 3.340739627587582 + }, + { + "x": 1.3892531394958496, + "y": 5.486522674560547, + "heading": 0.03752685285960867, + "angularVelocity": -2.6599524768911785e-21, + "velocityX": -1.3251439086588674e-20, + "velocityY": 5.237013291945624e-20, + "timestamp": 3.441163942329906 + }, + { + "x": 1.4165016264324646, + "y": 5.468726018878958, + "heading": 0.027669365250431073, + "angularVelocity": -0.15749827520188686, + "velocityX": 0.4353634378786766, + "velocityY": -0.2843465480598295, + "timestamp": 3.503751850651708 + }, + { + "x": 1.4710112662474708, + "y": 5.4331263299672, + "heading": 0.008563925885950884, + "angularVelocity": -0.3052576747931523, + "velocityX": 0.8709292461850596, + "velocityY": -0.5687949935747745, + "timestamp": 3.56633975897351 + }, + { + "x": 1.5527987232870373, + "y": 5.379715427763577, + "heading": -0.018924667024813987, + "angularVelocity": -0.4391997375823718, + "velocityX": 1.306761309533597, + "velocityY": -0.8533741362469829, + "timestamp": 3.628927667295312 + }, + { + "x": 1.6618865667976872, + "y": 5.308482777774649, + "heading": -0.05347261278820921, + "angularVelocity": -0.5519907389421553, + "velocityX": 1.7429539736296065, + "velocityY": -1.1381215940733804, + "timestamp": 3.691515575617114 + }, + { + "x": 1.7983053814757264, + "y": 5.219415946962974, + "heading": -0.09280768567650903, + "angularVelocity": -0.6284771922086734, + "velocityX": 2.1796353055390356, + "velocityY": -1.4230677010921848, + "timestamp": 3.754103483938916 + }, + { + "x": 1.9620858254779319, + "y": 5.112511494418258, + "heading": -0.13216035126284406, + "angularVelocity": -0.6287582800179152, + "velocityX": 2.6168064789785364, + "velocityY": -1.708068785348386, + "timestamp": 3.8166913922607177 + }, + { + "x": 2.1529952387272515, + "y": 4.987995425318902, + "heading": -0.15523721329619666, + "angularVelocity": -0.3687111880253399, + "velocityX": 3.0502603197368576, + "velocityY": -1.9894588657468077, + "timestamp": 3.8792793005825197 + }, + { + "x": 2.323053130467604, + "y": 4.850242258645187, + "heading": -0.20104196583241465, + "angularVelocity": -0.7318466739726868, + "velocityX": 2.7171045701988246, + "velocityY": -2.200954950682231, + "timestamp": 3.9418672089043216 + }, + { + "x": 2.466345532790285, + "y": 4.728187893870888, + "heading": -0.2844490729613583, + "angularVelocity": -1.3326393126943619, + "velocityX": 2.2894582382579274, + "velocityY": -1.9501269182338685, + "timestamp": 4.004455117226123 + }, + { + "x": 2.582053624940948, + "y": 4.6234869751422165, + "heading": -0.37568976157943984, + "angularVelocity": -1.4578005730589176, + "velocityX": 1.848729175542, + "velocityY": -1.6728617641340675, + "timestamp": 4.067043025547925 + }, + { + "x": 2.670225613669648, + "y": 4.536352850675129, + "heading": -0.4673001281858147, + "angularVelocity": -1.463707113127209, + "velocityX": 1.4087703374804497, + "velocityY": -1.3921878331366975, + "timestamp": 4.129630933869726 + }, + { + "x": 2.730923003974787, + "y": 4.4668604060655035, + "heading": -0.555803694848379, + "angularVelocity": -1.4140681328974087, + "velocityX": 0.9697941971963301, + "velocityY": -1.1103174155033886, + "timestamp": 4.192218842191528 + }, + { + "x": 2.764192819595337, + "y": 4.415045738220215, + "heading": -0.6391760227970993, + "angularVelocity": -1.3320836274005747, + "velocityX": 0.5315693799749596, + "velocityY": -0.8278702585630189, + "timestamp": 4.254806750513329 + }, + { + "x": 2.7480635723388045, + "y": 4.353298917368432, + "heading": -0.7461336511354502, + "angularVelocity": -1.226587505939294, + "velocityX": -0.18496981909961455, + "velocityY": -0.7081110544885852, + "timestamp": 4.3420060965204 + }, + { + "x": 2.669473842343889, + "y": 4.302035156263182, + "heading": -0.8427126143522478, + "angularVelocity": -1.1075652242731957, + "velocityX": -0.9012651309167219, + "velocityY": -0.5878915777773497, + "timestamp": 4.429205442527471 + }, + { + "x": 2.528459101717774, + "y": 4.261314542628117, + "heading": -0.927127909191312, + "angularVelocity": -0.9680725682531954, + "velocityX": -1.6171536494629242, + "velocityY": -0.46698301649834584, + "timestamp": 4.516404788534542 + }, + { + "x": 2.3250882453572066, + "y": 4.231236836483453, + "heading": -0.996366799797096, + "angularVelocity": -0.7940299299970593, + "velocityX": -2.3322520829924085, + "velocityY": -0.34493040971001, + "timestamp": 4.603604134541613 + }, + { + "x": 2.0595373245634314, + "y": 4.211999318818496, + "heading": -1.044279386733433, + "angularVelocity": -0.5494603931140937, + "velocityX": -3.0453315644390497, + "velocityY": -0.22061538928740462, + "timestamp": 4.690803480548684 + }, + { + "x": 1.7326671393645254, + "y": 4.204157960465748, + "heading": -1.0516982080702795, + "angularVelocity": -0.08507886442456468, + "velocityX": -3.7485394118942086, + "velocityY": -0.08992450874702791, + "timestamp": 4.778002826555755 + }, + { + "x": 1.419863969855018, + "y": 4.255364813771041, + "heading": -1.034285504034905, + "angularVelocity": 0.19968847053007022, + "velocityX": -3.587219214742072, + "velocityY": 0.5872389604979419, + "timestamp": 4.865202172562826 + }, + { + "x": 1.1696215071966642, + "y": 4.29633086503019, + "heading": -1.02035490887151, + "angularVelocity": 0.15975572984532924, + "velocityX": -2.8697745352133857, + "velocityY": 0.4697976892605051, + "timestamp": 4.952401518569897 + }, + { + "x": 0.9819396842688768, + "y": 4.327055591018277, + "heading": -1.009906819806218, + "angularVelocity": 0.11981843378096674, + "velocityX": -2.152330625421988, + "velocityY": 0.35235041769230013, + "timestamp": 5.039600864576968 + }, + { + "x": 0.8568184786877541, + "y": 4.347538817267099, + "heading": -1.002941368423543, + "angularVelocity": 0.07987962870856867, + "velocityX": -1.434886972328629, + "velocityY": 0.2349011453269456, + "timestamp": 5.126800210584039 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": 0.03994005776301401, + "velocityX": -0.7174434476318076, + "velocityY": 0.11745087256777795, + "timestamp": 5.21399955659111 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": -9.407654269861187e-21, + "velocityX": -1.531911723578323e-20, + "velocityY": 3.960115739846757e-19, + "timestamp": 5.301198902598181 } ], "constraints": [ @@ -852,35 +762,35 @@ "first" ], "type": "StopPoint", - "uuid": "ce158810-b71e-4150-8960-1f702cb58f8b" + "uuid": "f5e2f7dc-14a2-456d-a6fe-7d777b7ff67b" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "2e31556e-985d-4335-80eb-6bd55dee43a9" + "uuid": "ae918eb2-5f0e-4193-9c60-52e3bb50a1ba" }, { "scope": [ 3 ], "type": "StopPoint", - "uuid": "b3fbeb72-3318-42db-a9ff-31d7d4d299e1" + "uuid": "cf8e15e8-0cb0-4e98-8b57-a1df1fe7f549" }, { "scope": [ 5 ], "type": "StopPoint", - "uuid": "7c8e9570-1690-46b0-9b6c-ed87935178fe" + "uuid": "3fc2d624-09ad-44f8-b3e5-99fb97f324b6" }, { "scope": [ 6 ], "type": "WptVelocityDirection", - "uuid": "d09e9560-c87d-48d3-b775-9c5bf170fe8e", + "uuid": "b6b3c210-d1c4-4502-b48c-cda9e58be917", "direction": -1 }, { @@ -889,7 +799,7 @@ 2 ], "type": "ZeroAngularVelocity", - "uuid": "eae272b7-8811-4fc6-b7bc-f0ddad8551be" + "uuid": "203093d8-08ae-40dd-a312-c914dbd9735b" } ], "usesControlIntervalGuessing": true, @@ -2033,14 +1943,14 @@ "first" ], "type": "StopPoint", - "uuid": "fbed2953-b55c-457e-8134-9066a5dd1534" + "uuid": "8bd4a7b9-3f58-4fe1-9239-10051f881fd7" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "09730991-f6ed-48ac-a05f-16390ee0b1ab" + "uuid": "d9bfd8e0-e225-46e1-8b92-adfae7d70f38" } ], "usesControlIntervalGuessing": false, @@ -2048,5 +1958,6 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] } - } + }, + "splitTrajectoriesAtStopPoints": true } \ No newline at end of file diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat index 92f98fe3e03d9703b372abe2f7bff5e9d23e3ce0..4e4eb2dc1c31296a19154fddfae7708e5cc8e9c8 100644 GIT binary patch delta 55 zcmZn=Xb_lifa4N_=4=Lr4^9&WlP4;0PAu`^I?Ay5PX_}KFijLR<2u5y?N1www}^=i E002i2y8r+H delta 200 zcmZn=Xb_liAW(n-0?gConSR$ZFz`Y76SK;n{Q~kYtginLq8Xv$Fp9YiqCVhS5m^3q z4n#fUL_zV%1uQI)Fohu62#7$27_d&f6U)TFkp4jdY#9T?!q*^v=g0V&4}t0wpz=WV fU;v^)`e6LC43ht)CJI`!o?(#rF9BpMVqyaTJasTZ diff --git a/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 012 - 0 - ext.dat index 81bb04da5a12182051881224b1e7e17b30975346..7b2f31b3e63e14254e9cb65c5318e6a9099ef739 100644 GIT binary patch delta 65 zcmZn=Xb_lifa4N_oFxN;htovC)QJk5Yz_7d3=Q@Z1!X4A(BL}Cu=!610}wDx6g1;H O!m#a68;rMzi46c|8WHLM delta 200 zcmZn=Xb_liAW(n-0?gConSR$ZFz`Y76SK;n{Q~kYtginLq8Xv$Fp9YiqCVhS5m^3q z4n#fUL_zV%1uQI)Fohu62#7$27_d&f6U)TFkp4jdY#9T?!q*^v=g0V&4}t0wpz=WV fU;v^)`e6LC43ht)CJI`!o?(#rF9BpMVqyaTJasTZ diff --git a/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 027 - 0 - ext.dat index 5087592ae586e4373e89b1bb5378c46ebacf7acc..980135a562c72d7df03fc5fff1a766a96b7ec865 100644 GIT binary patch delta 66 zcmZn=Xb_lifaMZ{Tg61d)QJkbYz_7d3=Q@Z1!Y)XFf*`B6jb3l%CPxQ2Lli=O%yca PI>NB+PaBN4h=~mVimeha delta 200 zcmZn=Xb_liAW(n-0?gConSR$ZFz`Y76SK;n{Q~kYtginLq8Xv$Fp9YiqCVhS5m^3q z4n#fUL_zUbCYUNHZN$J}1Tw^cb>bb6WcmjMuw@Jk3txlyogd?8J_M>&fXV~ag8_&J b>4WjlGD!ZHnkZ<^dWJ#fzXXu6h=~mV&nquH diff --git a/deploy/choreo/amp 4 local.traj b/deploy/choreo/amp 4 local.traj new file mode 100644 index 00000000..6dfc86f9 --- /dev/null +++ b/deploy/choreo/amp 4 local.traj @@ -0,0 +1,760 @@ +{ + "samples": [ + { + "x": 0.7470832465334709, + "y": 6.625458718233556, + "heading": 1.0564954979067924, + "angularVelocity": -5.684301329861994e-12, + "velocityX": -3.602456269610706e-10, + "velocityY": 4.1137821262407583e-10, + "timestamp": 0 + }, + { + "x": 0.7589569772775385, + "y": 6.629936126338657, + "heading": 1.0434265097431823, + "angularVelocity": -0.12778392744309977, + "velocityX": 0.11609712676013798, + "velocityY": 0.04377849867759475, + "timestamp": 0.10227411521599863 + }, + { + "x": 0.7822022838432713, + "y": 6.638629979602176, + "heading": 1.0183346599429852, + "angularVelocity": -0.2453392014361959, + "velocityX": 0.2272843592055021, + "velocityY": 0.08500540295845989, + "timestamp": 0.20454823043199727 + }, + { + "x": 0.816345318524229, + "y": 6.6512661070932495, + "heading": 0.9822355978698938, + "angularVelocity": -0.35296381641965074, + "velocityX": 0.33383847695279084, + "velocityY": 0.12355156118457596, + "timestamp": 0.30682234564799593 + }, + { + "x": 0.8609432271767217, + "y": 6.667555740683275, + "heading": 0.9361120278121537, + "angularVelocity": -0.45097989764859636, + "velocityX": 0.4360625240585109, + "velocityY": 0.15927424713723962, + "timestamp": 0.40909646086399454 + }, + { + "x": 0.9155883048461597, + "y": 6.687191502411657, + "heading": 0.8809291072748399, + "angularVelocity": -0.5395590113414345, + "velocityX": 0.5343001768187209, + "velocityY": 0.19199150048411856, + "timestamp": 0.5113705760799931 + }, + { + "x": 0.9799143577111046, + "y": 6.709840447255909, + "heading": 0.817662056557047, + "angularVelocity": -0.6186027670698295, + "velocityX": 0.6289573163604619, + "velocityY": 0.22145333556813593, + "timestamp": 0.6136446912959918 + }, + { + "x": 1.0536065024635548, + "y": 6.735133328172977, + "heading": 0.7473295527982347, + "angularVelocity": -0.687686259272288, + "velocityX": 0.7205356386732681, + "velocityY": 0.24730480411634378, + "timestamp": 0.7159188065119904 + }, + { + "x": 1.1364169197016813, + "y": 6.762648211314195, + "heading": 0.6710254557943266, + "angularVelocity": -0.7460743786173882, + "velocityX": 0.8096908708297301, + "velocityY": 0.2690307532427302, + "timestamp": 0.818192921727989 + }, + { + "x": 1.2281932228229642, + "y": 6.7918845460908575, + "heading": 0.5899378478266369, + "angularVelocity": -0.7928458510838684, + "velocityX": 0.8973561208709863, + "velocityY": 0.28586249636547506, + "timestamp": 0.9204670369439876 + }, + { + "x": 1.3289409008958384, + "y": 6.822217824239773, + "heading": 0.5053285615301966, + "angularVelocity": -0.8272795718879136, + "velocityX": 0.985075041075245, + "velocityY": 0.296588019337818, + "timestamp": 1.0227411521599863 + }, + { + "x": 1.4390031913745853, + "y": 6.852792823991799, + "heading": 0.4183893727045359, + "angularVelocity": -0.8500605319021087, + "velocityX": 1.076150015287458, + "velocityY": 0.29895148738224403, + "timestamp": 1.125015267375985 + }, + { + "x": 1.5597885498551032, + "y": 6.882059112916124, + "heading": 0.32979940511385675, + "angularVelocity": -0.8662012607776253, + "velocityX": 1.1809963684348201, + "velocityY": 0.28615537738935176, + "timestamp": 1.2272893825919837 + }, + { + "x": 1.700662064049805, + "y": 6.9025972281907375, + "heading": 0.2406449492427404, + "angularVelocity": -0.8717206268168672, + "velocityX": 1.3774112254273, + "velocityY": 0.20081439319892427, + "timestamp": 1.3295634978079824 + }, + { + "x": 1.8510164004103928, + "y": 6.913603640101383, + "heading": 0.18435642050200896, + "angularVelocity": -0.5503692556839603, + "velocityX": 1.4701113396826178, + "velocityY": 0.10761678064700267, + "timestamp": 1.4318376130239812 + }, + { + "x": 2.0039488544462833, + "y": 6.918843013673937, + "heading": 0.15617120081925578, + "angularVelocity": -0.2755850738942481, + "velocityX": 1.4953192592999736, + "velocityY": 0.051228726583383746, + "timestamp": 1.5341117282399799 + }, + { + "x": 2.1557445524327394, + "y": 6.919295783765447, + "heading": 0.15617120073466711, + "angularVelocity": -8.01993499988118e-11, + "velocityX": 1.484204462099971, + "velocityY": 0.0044270165813428694, + "timestamp": 1.6363858434559786 + }, + { + "x": 2.232501376452394, + "y": 6.918644981823818, + "heading": 0.15617120066422163, + "angularVelocity": -2.7881719949913897e-12, + "velocityX": 1.4631774376072004, + "velocityY": -0.012405855819009865, + "timestamp": 1.6888448428916174 + }, + { + "x": 2.3078780299226054, + "y": 6.916920762093343, + "heading": 0.1561712005937278, + "angularVelocity": -3.698975819730093e-12, + "velocityX": 1.436867928003674, + "velocityY": -0.032867889033463694, + "timestamp": 1.7413038423272562 + }, + { + "x": 2.381659462849091, + "y": 6.91403766609857, + "heading": 0.15617120052323402, + "angularVelocity": -3.687857737825465e-12, + "velocityX": 1.4064590265589882, + "velocityY": -0.05495897496281437, + "timestamp": 1.793762841762895 + }, + { + "x": 2.453627858185915, + "y": 6.909923503320222, + "heading": 0.15617120045273983, + "angularVelocity": -3.68806627753873e-12, + "velocityX": 1.3718979863339387, + "velocityY": -0.07842619311071317, + "timestamp": 1.8462218411985338 + }, + { + "x": 2.5235476514141553, + "y": 6.90450568335544, + "heading": 0.15617120038224525, + "angularVelocity": -3.688143550349055e-12, + "velocityX": 1.3328464960153679, + "velocityY": -0.10327716626043287, + "timestamp": 1.8986808406341726 + }, + { + "x": 2.5911718313337544, + "y": 6.897715977103231, + "heading": 0.1561712003117504, + "angularVelocity": -3.687560329325201e-12, + "velocityX": 1.2890863531735766, + "velocityY": -0.12942875706821344, + "timestamp": 1.9511398400698114 + }, + { + "x": 2.65624736130467, + "y": 6.889492702059127, + "heading": 0.15617120024125528, + "angularVelocity": -3.68944362324301e-12, + "velocityX": 1.240502697821755, + "velocityY": -0.1567561709974853, + "timestamp": 2.0035988395054503 + }, + { + "x": 2.7185182573768354, + "y": 6.879780772610566, + "heading": 0.1561712001707086, + "angularVelocity": -4.669569996251489e-12, + "velocityX": 1.187039346844362, + "velocityY": -0.1851336547556834, + "timestamp": 2.056057838941089 + }, + { + "x": 2.844727229024539, + "y": 6.826831262995891, + "heading": 0.36522912798339696, + "angularVelocity": 1.2815424227266763, + "velocityX": 0.7736714562084378, + "velocityY": -0.32458488264739044, + "timestamp": 2.219187770584854 + }, + { + "x": 2.905891887894138, + "y": 6.746880408353911, + "heading": 0.5636352419280429, + "angularVelocity": 1.2162459209838357, + "velocityX": 0.3749444284785087, + "velocityY": -0.490105365340026, + "timestamp": 2.382317702228619 + }, + { + "x": 2.909090532344001, + "y": 6.6442122814929245, + "heading": 0.7377210107974022, + "angularVelocity": 1.0671601901772823, + "velocityX": 0.0196079562833389, + "velocityY": -0.6293641266849203, + "timestamp": 2.5454476338723837 + }, + { + "x": 2.8528015320306115, + "y": 6.520167648469734, + "heading": 0.8617887991289785, + "angularVelocity": 0.7605458243522792, + "velocityX": -0.34505623601923857, + "velocityY": -0.7604038809886424, + "timestamp": 2.7085775655161486 + }, + { + "x": 2.6673775887021587, + "y": 6.358333521535233, + "heading": 0.8824380102910534, + "angularVelocity": 0.1265813757217392, + "velocityX": -1.1366641384675562, + "velocityY": -0.9920566103835737, + "timestamp": 2.8717074971599135 + }, + { + "x": 2.433334548523251, + "y": 6.191208743378355, + "heading": 0.7641593769894643, + "angularVelocity": -0.7250578241718747, + "velocityX": -1.4347032313583656, + "velocityY": -1.0244887408273557, + "timestamp": 3.0348374288036783 + }, + { + "x": 2.190845809692594, + "y": 6.024313186516238, + "heading": 0.6297541443819642, + "angularVelocity": -0.823915214372992, + "velocityX": -1.486476064011715, + "velocityY": -1.0230835952941921, + "timestamp": 3.1979673604474432 + }, + { + "x": 1.9580404786548902, + "y": 5.870547470854263, + "heading": 0.47066977289213696, + "angularVelocity": -0.9752003809508628, + "velocityX": -1.4271159717504125, + "velocityY": -0.9425965806313563, + "timestamp": 3.361097292091208 + }, + { + "x": 1.7498273467354928, + "y": 5.73486661370809, + "heading": 0.32338313207816166, + "angularVelocity": -0.9028793141986351, + "velocityX": -1.2763637531323493, + "velocityY": -0.831734899187721, + "timestamp": 3.524227223734973 + }, + { + "x": 1.577014249967444, + "y": 5.624033816048722, + "heading": 0.19882585300104294, + "angularVelocity": -0.7635464431083702, + "velocityX": -1.0593586039568081, + "velocityY": -0.6794142353068856, + "timestamp": 3.687357155378738 + }, + { + "x": 1.4506486184747536, + "y": 5.5441849844920235, + "heading": 0.1067515452077215, + "angularVelocity": -0.5644231371867715, + "velocityX": -0.7746317925903302, + "velocityY": -0.4894799553714506, + "timestamp": 3.8504870870225028 + }, + { + "x": 1.3818649056319274, + "y": 5.50129938101995, + "heading": 0.05628563946839357, + "angularVelocity": -0.30936018439404245, + "velocityX": -0.4216498592955241, + "velocityY": -0.2628923039688523, + "timestamp": 4.013617018666268 + }, + { + "x": 1.3818649054659777, + "y": 5.501299381110093, + "heading": 0.05628563946121549, + "angularVelocity": -8.20711890061584e-12, + "velocityX": -2.918186949691226e-10, + "velocityY": -8.516677331367793e-11, + "timestamp": 4.176746950310032 + }, + { + "x": 1.4254749911225055, + "y": 5.4996771535618185, + "heading": 0.052286234874463405, + "angularVelocity": -0.03818204740477268, + "velocityX": 0.41634256351427257, + "velocityY": -0.015487297221703413, + "timestamp": 4.281492632082638 + }, + { + "x": 1.5054277602383004, + "y": 5.496460245033646, + "heading": 0.04534031368940726, + "angularVelocity": -0.06631224378557163, + "velocityX": 0.763303725485265, + "velocityY": -0.030711609603001202, + "timestamp": 4.386238313855244 + }, + { + "x": 1.614706352195534, + "y": 5.491742221647794, + "heading": 0.036622661203774276, + "angularVelocity": -0.08322684370600701, + "velocityX": 1.0432753896642522, + "velocityY": -0.045042652489690874, + "timestamp": 4.4909839956278494 + }, + { + "x": 1.7468754648697162, + "y": 5.485736649446459, + "heading": 0.02746720464328704, + "angularVelocity": -0.08740652985101428, + "velocityX": 1.2618096559898304, + "velocityY": -0.05733479469253905, + "timestamp": 4.595729677400455 + }, + { + "x": 1.8974421226741423, + "y": 5.478814465512338, + "heading": 0.0192537076414664, + "angularVelocity": -0.07841370510897432, + "velocityX": 1.43744978567254, + "velocityY": -0.06608562544064354, + "timestamp": 4.700475359173061 + }, + { + "x": 2.0478771487383725, + "y": 5.472018862260595, + "heading": 0.011842428389913316, + "angularVelocity": -0.07075498604111852, + "velocityX": 1.436193106275662, + "velocityY": -0.06487716815402857, + "timestamp": 4.805221040945667 + }, + { + "x": 2.1991998312667134, + "y": 5.465297323381209, + "heading": 0.007319920190768939, + "angularVelocity": -0.04317608251475354, + "velocityX": 1.4446675030153622, + "velocityY": -0.06417008055763802, + "timestamp": 4.909966722718273 + }, + { + "x": 2.337086103631991, + "y": 5.4645313180146555, + "heading": 0.0021217678335590726, + "angularVelocity": -0.04962641198543019, + "velocityX": 1.316390996187784, + "velocityY": -0.007313001468104138, + "timestamp": 5.014712404490878 + }, + { + "x": 2.45341531313919, + "y": 5.464492524135499, + "heading": -0.0044756440905307565, + "angularVelocity": -0.06298504924092899, + "velocityX": 1.1105871626237722, + "velocityY": -0.0003703621622037101, + "timestamp": 5.119458086263484 + }, + { + "x": 2.541111145262716, + "y": 5.463618987693384, + "heading": -0.012133903650681083, + "angularVelocity": -0.07311289048562652, + "velocityX": 0.8372262287970834, + "velocityY": -0.008339593435811936, + "timestamp": 5.22420376803609 + }, + { + "x": 2.592916727068705, + "y": 5.461223602405139, + "heading": -0.020267736871702392, + "angularVelocity": -0.07765316033563659, + "velocityX": 0.4945844156915881, + "velocityY": -0.022868582319897673, + "timestamp": 5.328949449808696 + }, + { + "x": 2.573983182200651, + "y": 5.456183859404588, + "heading": -0.02941680850548689, + "angularVelocity": -0.07029420270393813, + "velocityX": -0.14547032678251723, + "velocityY": -0.03872138453206129, + "timestamp": 5.459103449457666 + }, + { + "x": 2.4879155076147748, + "y": 5.449739339031148, + "heading": -0.03668510065770142, + "angularVelocity": -0.05584378637679435, + "velocityX": -0.6612756796636089, + "velocityY": -0.04951457856894505, + "timestamp": 5.589257449106635 + }, + { + "x": 2.35046961428428, + "y": 5.4436438239165845, + "heading": -0.03977633600135786, + "angularVelocity": -0.02375059815495546, + "velocityX": -1.056025121733672, + "velocityY": -0.046833099245860266, + "timestamp": 5.719411448755605 + }, + { + "x": 2.1764781877651598, + "y": 5.443212327064482, + "heading": -0.032264347817817784, + "angularVelocity": 0.05771615319957162, + "velocityX": -1.336811984121226, + "velocityY": -0.0033152797571240995, + "timestamp": 5.849565448404575 + }, + { + "x": 1.9752868983122995, + "y": 5.4534701667830054, + "heading": -0.009494098721117665, + "angularVelocity": 0.174948516049147, + "velocityX": -1.5457941360881122, + "velocityY": 0.07881309585526655, + "timestamp": 5.979719448053545 + }, + { + "x": 1.7725080662079153, + "y": 5.4634789711909475, + "heading": 0.004869525328448035, + "angularVelocity": 0.11035868346800264, + "velocityX": -1.5579915534229911, + "velocityY": 0.07689970624561784, + "timestamp": 6.1098734477025145 + }, + { + "x": 1.5970859275969194, + "y": 5.473760403676058, + "heading": 0.019510408429697427, + "angularVelocity": 0.11248892189634652, + "velocityX": -1.347804439948399, + "velocityY": 0.07899436394267238, + "timestamp": 6.240027447351484 + }, + { + "x": 1.463998994938607, + "y": 5.481958295434183, + "heading": 0.031093440957313423, + "angularVelocity": 0.0889948258110792, + "velocityX": -1.0225343286428261, + "velocityY": 0.06298609118419185, + "timestamp": 6.370181447000454 + }, + { + "x": 1.3892531402609951, + "y": 5.486522674152253, + "heading": 0.03752685288213878, + "angularVelocity": 0.049429229542960515, + "velocityX": -0.5742878042328909, + "velocityY": 0.03506906174326123, + "timestamp": 6.500335446649424 + }, + { + "x": 1.3892531401370203, + "y": 5.486522674374907, + "heading": 0.03752685288891443, + "angularVelocity": 5.018347137854837e-11, + "velocityX": -8.981333628450582e-10, + "velocityY": 1.1863234183511445e-9, + "timestamp": 6.630489446298394 + }, + { + "x": 1.410785208000178, + "y": 5.466798705442517, + "heading": 0.02538794622123859, + "angularVelocity": -0.12074981514388294, + "velocityX": 0.21418676347263918, + "velocityY": -0.1962009963268651, + "timestamp": 6.731018848208977 + }, + { + "x": 1.4512665581597533, + "y": 5.429718184658836, + "heading": 0.002445244265967334, + "angularVelocity": -0.2282188250282345, + "velocityX": 0.4026816906907998, + "velocityY": -0.36885249450974994, + "timestamp": 6.8315482501195595 + }, + { + "x": 1.5081514675148215, + "y": 5.3776444456377, + "heading": -0.029986935931297812, + "angularVelocity": -0.3226138782330449, + "velocityX": 0.5658534482437384, + "velocityY": -0.5179951130124598, + "timestamp": 6.932077652030142 + }, + { + "x": 1.578941487701644, + "y": 5.312948064530918, + "heading": -0.07059739560458454, + "angularVelocity": -0.4039659933419245, + "velocityX": 0.7041722939494359, + "velocityY": -0.6435568078574709, + "timestamp": 7.032607053940725 + }, + { + "x": 1.6611946188865518, + "y": 5.238029568056684, + "heading": -0.11800108662062794, + "angularVelocity": -0.4715405656932785, + "velocityX": 0.8181997404499256, + "velocityY": -0.7452396506196601, + "timestamp": 7.133136455851308 + }, + { + "x": 1.7524893345353523, + "y": 5.155358397604862, + "heading": -0.17044488274189246, + "angularVelocity": -0.521676197753198, + "velocityX": 0.9081394425873396, + "velocityY": -0.8223581237255294, + "timestamp": 7.233665857761891 + }, + { + "x": 1.8499500532222615, + "y": 5.067431436588914, + "heading": -0.22499900927916092, + "angularVelocity": -0.5426683689609221, + "velocityX": 0.9694747626122975, + "velocityY": -0.8746392520744162, + "timestamp": 7.334195259672474 + }, + { + "x": 1.9473638091809122, + "y": 4.979075974351683, + "heading": -0.27982409115325835, + "angularVelocity": -0.5453636534436577, + "velocityX": 0.9690076084526664, + "velocityY": -0.8789016988127862, + "timestamp": 7.434724661583057 + }, + { + "x": 2.05818821772957, + "y": 4.854821291737014, + "heading": -0.34544281347225186, + "angularVelocity": -0.6527316496725523, + "velocityX": 1.1024079106043232, + "velocityY": -1.2360033993617228, + "timestamp": 7.53525406349364 + }, + { + "x": 2.1664425710584037, + "y": 4.732231232720307, + "heading": -0.4041393246178968, + "angularVelocity": -0.5838740711290391, + "velocityX": 1.076842701113489, + "velocityY": -1.2194448247922516, + "timestamp": 7.635783465404223 + }, + { + "x": 2.2666201121554423, + "y": 4.61627032113389, + "heading": -0.4602072656334145, + "angularVelocity": -0.557726794034664, + "velocityX": 0.996499915039965, + "velocityY": -1.153502450677793, + "timestamp": 7.736312867314806 + }, + { + "x": 2.356480498307365, + "y": 4.5087945921361055, + "heading": -0.5124957847583047, + "angularVelocity": -0.5201316047807618, + "velocityX": 0.8938716814183492, + "velocityY": -1.0690974663909212, + "timestamp": 7.8368422692253885 + }, + { + "x": 2.433798199341425, + "y": 4.411691494217003, + "heading": -0.5602485683833155, + "angularVelocity": -0.47501310796660834, + "velocityX": 0.7691053456052521, + "velocityY": -0.9659173928845837, + "timestamp": 7.937371671135971 + }, + { + "x": 2.4961951429135376, + "y": 4.327016530758609, + "heading": -0.6027268742617791, + "angularVelocity": -0.4225460918867537, + "velocityX": 0.620683518971687, + "velocityY": -0.8422905312450144, + "timestamp": 8.037901073046553 + }, + { + "x": 2.541198492257727, + "y": 4.256927967764492, + "heading": -0.6391760227839379, + "angularVelocity": -0.3625720221944257, + "velocityX": 0.4476635477259144, + "velocityY": -0.6971946668803632, + "timestamp": 8.138430474957136 + }, + { + "x": 2.5331758624705114, + "y": 4.192320334785442, + "heading": -0.6763224470307224, + "angularVelocity": -0.2449990301925222, + "velocityX": -0.052913214713167914, + "velocityY": -0.426119279886695, + "timestamp": 8.290049133403663 + }, + { + "x": 2.46411262676954, + "y": 4.160473976285577, + "heading": -0.7018758155970488, + "angularVelocity": -0.16853709734646205, + "velocityX": -0.45550618406914944, + "velocityY": -0.21004248027074415, + "timestamp": 8.44166779185019 + }, + { + "x": 2.348779581051416, + "y": 4.153307846262027, + "heading": -0.7202693675207003, + "angularVelocity": -0.121314567375086, + "velocityX": -0.7606784530447123, + "velocityY": -0.04726417363495295, + "timestamp": 8.593286450296716 + }, + { + "x": 2.201596779448434, + "y": 4.163036990055964, + "heading": -0.7333684069992377, + "angularVelocity": -0.0863946406365267, + "velocityX": -0.9707433369527995, + "velocityY": 0.06416850879734587, + "timestamp": 8.744905108743243 + }, + { + "x": 2.03723470076713, + "y": 4.1831659239379135, + "heading": -0.7500897294060416, + "angularVelocity": -0.11028538710126731, + "velocityX": -1.0840491612778367, + "velocityY": 0.13276026521150996, + "timestamp": 8.89652376718977 + }, + { + "x": 1.8800877783569956, + "y": 4.195672966501764, + "heading": -0.7289968148061016, + "angularVelocity": 0.13911819814698503, + "velocityX": -1.0364616391016628, + "velocityY": 0.08249012434493122, + "timestamp": 9.048142425636296 + }, + { + "x": 1.7046442157621393, + "y": 4.233321160679635, + "heading": -0.7238359462014516, + "angularVelocity": 0.03403847932720532, + "velocityX": -1.1571370254440554, + "velocityY": 0.24830844691554524, + "timestamp": 9.199761084082823 + }, + { + "x": 1.572815192920781, + "y": 4.2658136358994225, + "heading": -0.7241682787394389, + "angularVelocity": -0.002191897597681109, + "velocityX": -0.8694775764003041, + "velocityY": 0.21430393183203963, + "timestamp": 9.35137974252935 + }, + { + "x": 1.4994455554717883, + "y": 4.286480902556393, + "heading": -0.7254698893818832, + "angularVelocity": -0.00858476582372283, + "velocityX": -0.4839090323218549, + "velocityY": 0.13631083596916982, + "timestamp": 9.502998400975876 + }, + { + "x": 1.4994455567524874, + "y": 4.286480903049883, + "heading": -0.7254698893413497, + "angularVelocity": 3.8000232610425283e-11, + "velocityX": 2.8946950656209713e-9, + "velocityY": -5.415769760526697e-10, + "timestamp": 9.654617059422403 + } + ] +} \ No newline at end of file diff --git a/deploy/choreo/test.traj b/deploy/choreo/test.traj new file mode 100644 index 00000000..ddadedba --- /dev/null +++ b/deploy/choreo/test.traj @@ -0,0 +1,1093 @@ +{ + "samples": [ + { + "x": 2.1199185848236084, + "y": 5.957952976226807, + "heading": -6.25322258455275e-30, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.157285981558088, + "y": 5.958401136866338, + "heading": 0.034313098562848664, + "angularVelocity": 0.4715592535645141, + "velocityX": 0.5135339695272734, + "velocityY": 0.006158997744474669, + "timestamp": 0.07276518974757916 + }, + { + "x": 2.2137097634050407, + "y": 5.959057625247641, + "heading": 0.03431310711765102, + "angularVelocity": 1.1756723768181273e-7, + "velocityX": 0.7754227267555416, + "velocityY": 0.009022011535733282, + "timestamp": 0.14553037949515832 + }, + { + "x": 2.270133545252147, + "y": 5.959714113615942, + "heading": 0.03431311567244465, + "angularVelocity": 1.1756711762076828e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058456, + "timestamp": 0.21829556924273746 + }, + { + "x": 2.3265573270992537, + "y": 5.960370601984243, + "heading": 0.03431312422723829, + "angularVelocity": 1.1756711788910916e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058367, + "timestamp": 0.29106075899031664 + }, + { + "x": 2.38298110894636, + "y": 5.961027090352544, + "heading": 0.0343131327820319, + "angularVelocity": 1.175671175035012e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058381, + "timestamp": 0.3638259487378958 + }, + { + "x": 2.4394048907934667, + "y": 5.961683578720844, + "heading": 0.034313141336825545, + "angularVelocity": 1.1756711790299388e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058397, + "timestamp": 0.436591138485475 + }, + { + "x": 2.495828672640573, + "y": 5.962340067089145, + "heading": 0.03431314989161917, + "angularVelocity": 1.1756711758579864e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.0090220113570584, + "timestamp": 0.5093563282330541 + }, + { + "x": 2.5522524544876797, + "y": 5.962996555457446, + "heading": 0.0343131584464128, + "angularVelocity": 1.175671177574276e-7, + "velocityX": 0.7754227267576639, + "velocityY": 0.009022011357058414, + "timestamp": 0.5821215179806333 + }, + { + "x": 2.6086762363347864, + "y": 5.963653043825747, + "heading": 0.034313167001206415, + "angularVelocity": 1.1756711751242314e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.6548867077282124 + }, + { + "x": 2.6651000181818927, + "y": 5.964309532194048, + "heading": 0.03431317555600002, + "angularVelocity": 1.1756711746464553e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058426, + "timestamp": 0.7276518974757916 + }, + { + "x": 2.7215238000289994, + "y": 5.964966020562349, + "heading": 0.03431318411079364, + "angularVelocity": 1.1756711761494635e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058443, + "timestamp": 0.8004170872233708 + }, + { + "x": 2.7779475818761057, + "y": 5.965622508930649, + "heading": 0.034313192665587286, + "angularVelocity": 1.1756711789914241e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058452, + "timestamp": 0.87318227697095 + }, + { + "x": 2.8343713637232124, + "y": 5.96627899729895, + "heading": 0.03431320122038093, + "angularVelocity": 1.1756711786542428e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058466, + "timestamp": 0.9459474667185291 + }, + { + "x": 2.8907951455703187, + "y": 5.966935485667251, + "heading": 0.03431320977517455, + "angularVelocity": 1.1756711758478168e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058475, + "timestamp": 1.0187126564661082 + }, + { + "x": 2.9472189274174254, + "y": 5.967591974035552, + "heading": 0.03431321832996816, + "angularVelocity": 1.1756711747039204e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058483, + "timestamp": 1.0914778462136874 + }, + { + "x": 3.0036427092645317, + "y": 5.968248462403853, + "heading": 0.034313226884761766, + "angularVelocity": 1.1756711737664598e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058489, + "timestamp": 1.1642430359612665 + }, + { + "x": 3.0600664911116384, + "y": 5.968904950772154, + "heading": 0.03431323543955538, + "angularVelocity": 1.1756711759331021e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058496, + "timestamp": 1.2370082257088457 + }, + { + "x": 3.116490272958745, + "y": 5.969561439140454, + "heading": 0.034313243994349, + "angularVelocity": 1.1756711756317827e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058518, + "timestamp": 1.309773415456425 + }, + { + "x": 3.1729140548058514, + "y": 5.970217927508755, + "heading": 0.03431325254914262, + "angularVelocity": 1.1756711755305748e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058523, + "timestamp": 1.382538605204004 + }, + { + "x": 3.229337836652958, + "y": 5.970874415877057, + "heading": 0.03431326110393624, + "angularVelocity": 1.175671175226974e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705854, + "timestamp": 1.4553037949515832 + }, + { + "x": 3.2857616185000644, + "y": 5.971530904245357, + "heading": 0.03431326965872984, + "angularVelocity": 1.1756711738766866e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058546, + "timestamp": 1.5280689846991624 + }, + { + "x": 3.342185400347171, + "y": 5.972187392613658, + "heading": 0.03431327821352346, + "angularVelocity": 1.1756711754575316e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058565, + "timestamp": 1.6008341744467416 + }, + { + "x": 3.3986091821942774, + "y": 5.972843880981959, + "heading": 0.03431328676831708, + "angularVelocity": 1.1756711769845501e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058572, + "timestamp": 1.6735993641943208 + }, + { + "x": 3.455032964041384, + "y": 5.973500369350259, + "heading": 0.034313295323110705, + "angularVelocity": 1.175671175699485e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058579, + "timestamp": 1.7463645539419 + }, + { + "x": 3.5114567458884904, + "y": 5.97415685771856, + "heading": 0.03431330387790433, + "angularVelocity": 1.1756711766849268e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058603, + "timestamp": 1.819129743689479 + }, + { + "x": 3.567880527735597, + "y": 5.9748133460868615, + "heading": 0.03431331243269794, + "angularVelocity": 1.1756711756878859e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058601, + "timestamp": 1.8918949334370583 + }, + { + "x": 3.624304309582704, + "y": 5.975469834455162, + "heading": 0.03431332098749156, + "angularVelocity": 1.1756711758891969e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058617, + "timestamp": 1.9646601231846375 + }, + { + "x": 3.68072809142981, + "y": 5.976126322823463, + "heading": 0.03431332954228517, + "angularVelocity": 1.1756711749627294e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058617, + "timestamp": 2.0374253129322164 + }, + { + "x": 3.737151873276917, + "y": 5.976782811191764, + "heading": 0.03431333809707879, + "angularVelocity": 1.1756711754302844e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058624, + "timestamp": 2.1101905026797954 + }, + { + "x": 3.793575655124023, + "y": 5.977439299560064, + "heading": 0.03431334665187242, + "angularVelocity": 1.175671176839478e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058646, + "timestamp": 2.1829556924273743 + }, + { + "x": 3.84999943697113, + "y": 5.978095787928365, + "heading": 0.034313355206666014, + "angularVelocity": 1.1756711730438892e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.00902201135705865, + "timestamp": 2.2557208821749533 + }, + { + "x": 3.906423218818236, + "y": 5.9787522762966665, + "heading": 0.03431336376145962, + "angularVelocity": 1.1756711741363398e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.009022011357058672, + "timestamp": 2.328486071922532 + }, + { + "x": 3.962847000665343, + "y": 5.979408764664967, + "heading": 0.03431337231625322, + "angularVelocity": 1.1756711736937194e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058674, + "timestamp": 2.401251261670111 + }, + { + "x": 4.019270782512449, + "y": 5.980065253033268, + "heading": 0.034313380871046816, + "angularVelocity": 1.17567117272878e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705869, + "timestamp": 2.47401645141769 + }, + { + "x": 4.075694564359556, + "y": 5.9807217414015685, + "heading": 0.034313389425840425, + "angularVelocity": 1.1756711748988721e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058704, + "timestamp": 2.546781641165269 + }, + { + "x": 4.132118346206663, + "y": 5.981378229769869, + "heading": 0.034313397980634035, + "angularVelocity": 1.1756711734690936e-7, + "velocityX": 0.7754227267576638, + "velocityY": 0.00902201135705871, + "timestamp": 2.619546830912848 + }, + { + "x": 4.188542128053769, + "y": 5.98203471813817, + "heading": 0.03431340653542763, + "angularVelocity": 1.1756711733140912e-7, + "velocityX": 0.7754227267576637, + "velocityY": 0.009022011357058714, + "timestamp": 2.692312020660427 + }, + { + "x": 4.244965909900876, + "y": 5.982691206506428, + "heading": 0.03431341509022124, + "angularVelocity": 1.1756711743485013e-7, + "velocityX": 0.7754227267576708, + "velocityY": 0.00902201135645878, + "timestamp": 2.765077210408006 + }, + { + "x": 4.30138969265204, + "y": 5.983347617167627, + "heading": 0.03431342364501558, + "angularVelocity": 1.1756712748463029e-7, + "velocityX": 0.7754227391819724, + "velocityY": 0.009020943441183957, + "timestamp": 2.837842400155585 + }, + { + "x": 4.337240695953369, + "y": 5.957952976226807, + "heading": -3.4992233566514893e-26, + "angularVelocity": -0.47156372111511136, + "velocityX": 0.4926944247063125, + "velocityY": -0.3489943615747365, + "timestamp": 2.910607589903164 + }, + { + "x": 4.337075774441293, + "y": 5.930386954424772, + "heading": -0.09015124251095946, + "angularVelocity": -1.224439774519884, + "velocityX": -0.002239974219267059, + "velocityY": -0.37440342007034494, + "timestamp": 2.9842341123234344 + }, + { + "x": 4.333264434450346, + "y": 5.912061346555676, + "heading": -0.20505051130417343, + "angularVelocity": -1.5605690044322955, + "velocityX": -0.05176585645578975, + "velocityY": -0.24889954416819945, + "timestamp": 3.057860634743705 + }, + { + "x": 4.325553986362593, + "y": 5.887325303672255, + "heading": -0.3021610086195407, + "angularVelocity": -1.31896080546962, + "velocityX": -0.10472378477610225, + "velocityY": -0.33596647064523955, + "timestamp": 3.1314871571639755 + }, + { + "x": 4.313628651056446, + "y": 5.857761645119009, + "heading": -0.3834128542416627, + "angularVelocity": -1.1035676132892005, + "velocityX": -0.16197064473690057, + "velocityY": -0.40153544648615047, + "timestamp": 3.205113679584246 + }, + { + "x": 4.297606686426737, + "y": 5.824666184248557, + "heading": -0.4508559142750475, + "angularVelocity": -0.9160158298447155, + "velocityX": -0.2176113186258289, + "velocityY": -0.4495046048968324, + "timestamp": 3.2787402020045167 + }, + { + "x": 4.277844157832715, + "y": 5.789044520818464, + "heading": -0.5065111263049437, + "angularVelocity": -0.7559125461910066, + "velocityX": -0.26841589069241406, + "velocityY": -0.4838156449486921, + "timestamp": 3.3523667244247872 + }, + { + "x": 4.254799811398479, + "y": 5.751649129927804, + "heading": -0.5522697074374715, + "angularVelocity": -0.6214958907244229, + "velocityX": -0.3129897444116032, + "velocityY": -0.5079065214733656, + "timestamp": 3.425993246845058 + }, + { + "x": 4.228951375108858, + "y": 5.713030269053256, + "heading": -0.5898366283888508, + "angularVelocity": -0.5102362534107197, + "velocityX": -0.35107506697211693, + "velocityY": -0.524523766776681, + "timestamp": 3.4996197692653284 + }, + { + "x": 4.2007493706625425, + "y": 5.673585962715614, + "heading": -0.6207093446974378, + "angularVelocity": -0.419315150216672, + "velocityX": -0.3830413758419039, + "velocityY": -0.5357350183197291, + "timestamp": 3.573246291685599 + }, + { + "x": 4.1705955025497214, + "y": 5.633604594294146, + "heading": -0.6461809126237779, + "angularVelocity": -0.34595641745707684, + "velocityX": -0.4095517093785753, + "velocityY": -0.5430294288958701, + "timestamp": 3.6468728141058695 + }, + { + "x": 4.138835739465679, + "y": 5.593298519659669, + "heading": -0.6673577962188777, + "angularVelocity": -0.28762574815389413, + "velocityX": -0.43136307460989803, + "velocityY": -0.5474396088464458, + "timestamp": 3.72049933652614 + }, + { + "x": 4.1057614035434415, + "y": 5.552829496334061, + "heading": -0.6851851845597893, + "angularVelocity": -0.2421326955950787, + "velocityX": -0.44921768453825217, + "velocityY": -0.549652788089118, + "timestamp": 3.7941258589464106 + }, + { + "x": 4.071614137002915, + "y": 5.512327581345269, + "heading": -0.700475200245641, + "angularVelocity": -0.20766994261353336, + "velocityX": -0.4637902948289247, + "velocityY": -0.5500995247011782, + "timestamp": 3.867752381366681 + }, + { + "x": 4.036592388923184, + "y": 5.471905240898815, + "heading": -0.7139354213530316, + "angularVelocity": -0.18281755901165134, + "velocityX": -0.47566755740304917, + "velocityY": -0.5490187383252648, + "timestamp": 3.9413789037869518 + }, + { + "x": 4.0008581978077, + "y": 5.431668194433558, + "heading": -0.7261965659955223, + "angularVelocity": -0.16653162799816065, + "velocityX": -0.4853440029600765, + "velocityY": -0.5465020639651851, + "timestamp": 4.015005426207223 + }, + { + "x": 3.964543737766948, + "y": 5.391724222217225, + "heading": -0.7378391147106159, + "angularVelocity": -0.15812981969508613, + "velocityX": -0.49322525153997593, + "velocityY": -0.5425215113152707, + "timestamp": 4.088631948627493 + }, + { + "x": 3.9277575195733885, + "y": 5.352190902296246, + "heading": -0.7494192294944804, + "angularVelocity": -0.15728183816374608, + "velocityX": -0.4996327000693894, + "velocityY": -0.536944006336704, + "timestamp": 4.162258471047764 + }, + { + "x": 3.89059042061129, + "y": 5.3132030416510085, + "heading": -0.761494708226674, + "angularVelocity": -0.16400990207394403, + "velocityX": -0.5048058463218383, + "velocityY": -0.5295355445784652, + "timestamp": 4.2358849934680345 + }, + { + "x": 3.8531219539867014, + "y": 5.274920426877835, + "heading": -0.7746519957947015, + "angularVelocity": -0.1787030968666938, + "velocityX": -0.5088990406298591, + "velocityY": -0.5199568513456381, + "timestamp": 4.309511515888305 + }, + { + "x": 3.8154274539330624, + "y": 5.23753642375436, + "heading": -0.7895355361070671, + "angularVelocity": -0.2021491688471734, + "velocityX": -0.5119690407007602, + "velocityY": -0.507751852112243, + "timestamp": 4.383138038308576 + }, + { + "x": 3.7775872299458775, + "y": 5.201287866945812, + "heading": -0.8068810412551409, + "angularVelocity": -0.23558772814317216, + "velocityX": -0.5139482722161929, + "velocityY": -0.49233014974734224, + "timestamp": 4.456764560728846 + }, + { + "x": 3.7396993230404707, + "y": 5.166466510716887, + "heading": -0.8275545372921488, + "angularVelocity": -0.2807887070776005, + "velocityX": -0.5145959045727775, + "velocityY": -0.4729458228402956, + "timestamp": 4.530391083149117 + }, + { + "x": 3.701898417706247, + "y": 5.133430805056498, + "heading": -0.8525964909440766, + "angularVelocity": -0.3401213697013267, + "velocityX": -0.5134142438298316, + "velocityY": -0.44869300592275085, + "timestamp": 4.604017605569387 + }, + { + "x": 3.664343129524347, + "y": 5.101026571041832, + "heading": -0.8792757408403825, + "angularVelocity": -0.36235922897480916, + "velocityX": -0.5100782564132129, + "velocityY": -0.44011631881374225, + "timestamp": 4.677644127989658 + }, + { + "x": 3.6270751479601078, + "y": 5.068774406382283, + "heading": -0.9064492634455257, + "angularVelocity": -0.3690724716024585, + "velocityX": -0.5061760400893075, + "velocityY": -0.4380509033884428, + "timestamp": 4.7512706504099285 + }, + { + "x": 3.590111595107405, + "y": 5.036682857394856, + "heading": -0.9341662862028731, + "angularVelocity": -0.37645432442313, + "velocityX": -0.5020412704230309, + "velocityY": -0.43586941135483154, + "timestamp": 4.824897172830199 + }, + { + "x": 3.55347222485786, + "y": 5.004762522170154, + "heading": -0.9624850330743058, + "angularVelocity": -0.38462697870999835, + "velocityX": -0.4976382021739686, + "velocityY": -0.4335439753964831, + "timestamp": 4.89852369525047 + }, + { + "x": 3.517180102788644, + "y": 4.973026350131823, + "heading": -0.9914744586380927, + "angularVelocity": -0.3937361783612594, + "velocityX": -0.49292185582330617, + "velocityY": -0.4310426595619462, + "timestamp": 4.97215021767074 + }, + { + "x": 3.4812625207112933, + "y": 4.941490315870616, + "heading": -1.0212172545129166, + "angularVelocity": -0.40396850071293, + "velocityX": -0.48783483039342795, + "velocityY": -0.42832437584372535, + "timestamp": 5.045776740091011 + }, + { + "x": 3.445752274995712, + "y": 4.9101744053392995, + "heading": -1.051814154831974, + "angularVelocity": -0.41556900031765764, + "velocityX": -0.4823023626307331, + "velocityY": -0.4253346416738305, + "timestamp": 5.119403262511281 + }, + { + "x": 3.4106895099559638, + "y": 4.879104067704057, + "heading": -1.0833902078498774, + "angularVelocity": -0.4288679130824993, + "velocityX": -0.4762246523013149, + "velocityY": -0.4219992553483426, + "timestamp": 5.193029784931552 + }, + { + "x": 3.376124474151927, + "y": 4.848312423976759, + "heading": -1.1161042305606605, + "angularVelocity": -0.44432388812345175, + "velocityX": -0.4694644629109969, + "velocityY": -0.4182140173826824, + "timestamp": 5.266656307351822 + }, + { + "x": 3.3421218274128583, + "y": 4.817843776695172, + "heading": -1.1501637008410697, + "angularVelocity": -0.4625978405715368, + "velocityX": -0.4618260597040901, + "velocityY": -0.4138270595977395, + "timestamp": 5.340282829772093 + }, + { + "x": 3.3087677521953345, + "y": 4.787759503770135, + "heading": -1.1858495459870186, + "angularVelocity": -0.48468736499938486, + "velocityX": -0.45301712101970637, + "velocityY": -0.40860646321595784, + "timestamp": 5.413909352192364 + }, + { + "x": 3.2761825599174124, + "y": 4.758148688152823, + "heading": -1.2235604524642407, + "angularVelocity": -0.5121918737647652, + "velocityX": -0.44257410518347007, + "velocityY": -0.4021759366589302, + "timestamp": 5.487535874612634 + }, + { + "x": 3.244545279648596, + "y": 4.729149218235016, + "heading": -1.2639000345369864, + "angularVelocity": -0.547894708953951, + "velocityX": -0.4296995054068451, + "velocityY": -0.3938726014013467, + "timestamp": 5.561162397032905 + }, + { + "x": 3.2141486367937797, + "y": 4.700995814255907, + "heading": -1.3078734515357762, + "angularVelocity": -0.5972496805944914, + "velocityX": -0.4128490910015826, + "velocityY": -0.382381281278044, + "timestamp": 5.634788919453175 + }, + { + "x": 3.185551521789466, + "y": 4.674155611334741, + "heading": -1.357437524753386, + "angularVelocity": -0.6731823205595795, + "velocityX": -0.38840779197850694, + "velocityY": -0.36454530295424753, + "timestamp": 5.708415441873446 + }, + { + "x": 3.161389111124608, + "y": 4.65130116728667, + "heading": -1.4227635315487523, + "angularVelocity": -0.8872618812888667, + "velocityX": -0.3281753622279693, + "velocityY": -0.3104104784090629, + "timestamp": 5.782041964293716 + }, + { + "x": 3.1613881587982178, + "y": 4.647716999053955, + "heading": -1.5707963267948966, + "angularVelocity": -2.010590618434374, + "velocityX": -0.00001293455617664302, + "velocityY": -0.048680395527252375, + "timestamp": 5.855668486713987 + }, + { + "x": 3.1590242751538056, + "y": 4.648068652143942, + "heading": -1.71822885135636, + "angularVelocity": -2.0416470093627317, + "velocityX": -0.032735083303029204, + "velocityY": 0.0048696953514198, + "timestamp": 5.927881030746015 + }, + { + "x": 3.147722843506569, + "y": 4.651261007949863, + "heading": -1.8460861775851984, + "angularVelocity": -1.7705694757427723, + "velocityX": -0.15650233347580403, + "velocityY": 0.04420777371456313, + "timestamp": 6.000093574778043 + }, + { + "x": 3.1294383579479246, + "y": 4.658664086881976, + "heading": -1.9555006668792347, + "angularVelocity": -1.5151728935835482, + "velocityX": -0.25320373078858216, + "velocityY": 0.10251790781432463, + "timestamp": 6.07230611881007 + }, + { + "x": 3.105950392035797, + "y": 4.670811298853881, + "heading": -2.048080530965875, + "angularVelocity": -1.2820468427975569, + "velocityX": -0.325261576461146, + "velocityY": 0.16821470749621126, + "timestamp": 6.144518662842098 + }, + { + "x": 3.078734489629141, + "y": 4.687681461713096, + "heading": -2.125691691683944, + "angularVelocity": -1.0747600954710448, + "velocityX": -0.37688607667090807, + "velocityY": 0.23361817652806605, + "timestamp": 6.216731206874126 + }, + { + "x": 3.048937913872785, + "y": 4.708930614799334, + "heading": -2.190285310241282, + "angularVelocity": -0.8944930472009076, + "velocityX": -0.41262326588495246, + "velocityY": 0.2942584750485111, + "timestamp": 6.288943750906154 + }, + { + "x": 3.0174119547024816, + "y": 4.734060139937329, + "heading": -2.243768219004051, + "angularVelocity": -0.7406318317638603, + "velocityX": -0.4365717839316291, + "velocityY": 0.347993904311833, + "timestamp": 6.361156294938182 + }, + { + "x": 2.9847672349131935, + "y": 4.762525988198673, + "heading": -2.2879169282753247, + "angularVelocity": -0.6113717479845765, + "velocityX": -0.4520643916770092, + "velocityY": 0.3941953388142518, + "timestamp": 6.4333688389702095 + }, + { + "x": 2.9514318737909195, + "y": 4.793802004717416, + "heading": -2.3243308683279778, + "angularVelocity": -0.5042605899122328, + "velocityX": -0.4616283994577112, + "velocityY": 0.4331105757037482, + "timestamp": 6.505581383002237 + }, + { + "x": 2.9177026495619596, + "y": 4.827411588267587, + "heading": -2.3544162803840454, + "angularVelocity": -0.4166230737242065, + "velocityX": -0.46708261952383673, + "velocityY": 0.46542583426037293, + "timestamp": 6.577793927034265 + }, + { + "x": 2.88378589769779, + "y": 4.862939604927141, + "heading": -2.379391061920221, + "angularVelocity": -0.3458510134347111, + "velocityX": -0.4696795040087044, + "velocityY": 0.4919923142964852, + "timestamp": 6.650006471066293 + }, + { + "x": 2.8498283989976017, + "y": 4.900033117168976, + "heading": -2.4003021659557313, + "angularVelocity": -0.289577168562792, + "velocityX": -0.47024376658337114, + "velocityY": 0.5136713120837215, + "timestamp": 6.722219015098321 + }, + { + "x": 2.8159399738054236, + "y": 4.938396448572123, + "heading": -2.4180494138871094, + "angularVelocity": -0.2457640589910084, + "velocityX": -0.46928723598420935, + "velocityY": 0.5312557799671519, + "timestamp": 6.794431559130349 + }, + { + "x": 2.7822098245267197, + "y": 4.977783840614619, + "heading": -2.4334117988726813, + "angularVelocity": -0.2127384541217516, + "velocityX": -0.4670954296215342, + "velocityY": 0.5454369814893605, + "timestamp": 6.866644103162376 + }, + { + "x": 2.7487184813842567, + "y": 5.0179914540575155, + "heading": -2.4470741047795617, + "angularVelocity": -0.18919574278978962, + "velocityX": -0.46378843996424124, + "velocityY": 0.5567954152821849, + "timestamp": 6.938856647194404 + }, + { + "x": 2.7155468563385656, + "y": 5.0588495367779895, + "heading": -2.459652889491748, + "angularVelocity": -0.17419113092882704, + "velocityX": -0.4593609807041143, + "velocityY": 0.5658031200556053, + "timestamp": 7.011069191226432 + }, + { + "x": 2.682783562068564, + "y": 5.100215023933873, + "heading": -2.471721683170927, + "angularVelocity": -0.16712877022898326, + "velocityX": -0.45370641221933317, + "velocityY": 0.5728296615271857, + "timestamp": 7.08328173525846 + }, + { + "x": 2.650531366839192, + "y": 5.141964497495229, + "heading": -2.4838357664761026, + "angularVelocity": -0.16775594140268155, + "velocityX": -0.44662870781934594, + "velocityY": 0.5781471089405039, + "timestamp": 7.155494279290488 + }, + { + "x": 2.618913440143494, + "y": 5.183987197473736, + "heading": -2.496557238457018, + "angularVelocity": -0.17616706559006176, + "velocityX": -0.4378453511023647, + "velocityY": 0.5819307509768665, + "timestamp": 7.2277068233225155 + }, + { + "x": 2.5880798824866296, + "y": 5.226177564560792, + "heading": -2.5104813496102354, + "angularVelocity": -0.1928212243435465, + "velocityX": -0.4269834011551906, + "velocityY": 0.5842526066986774, + "timestamp": 7.299919367354543 + }, + { + "x": 2.5582148956282778, + "y": 5.268426535906725, + "heading": -2.5262653286045555, + "angularVelocity": -0.21857669198470553, + "velocityX": -0.41357062347929907, + "velocityY": 0.585064159035808, + "timestamp": 7.372131911386571 + }, + { + "x": 2.5295447873325214, + "y": 5.31061044475372, + "heading": -2.5446612115646436, + "angularVelocity": -0.2547463630685689, + "velocityX": -0.39702393372321826, + "velocityY": 0.5841631729285931, + "timestamp": 7.444344455418599 + }, + { + "x": 2.5023467091367184, + "y": 5.352575812308809, + "heading": -2.566554449178874, + "angularVelocity": -0.3031777637486395, + "velocityX": -0.3766392468286359, + "velocityY": 0.5811368110293486, + "timestamp": 7.516556999450627 + }, + { + "x": 2.476956466389222, + "y": 5.394117632986223, + "heading": -2.593007817119188, + "angularVelocity": -0.36632649209230705, + "velocityX": -0.3516043242602833, + "velocityY": 0.5752715298188288, + "timestamp": 7.5887695434826545 + }, + { + "x": 2.4521989817386887, + "y": 5.435268589005595, + "heading": -2.621259955334882, + "angularVelocity": -0.39123588005934157, + "velocityX": -0.3428418840852991, + "velocityY": 0.5698588322981821, + "timestamp": 7.660982087514682 + }, + { + "x": 2.427559065081996, + "y": 5.476070836186979, + "heading": -2.6500802362174434, + "angularVelocity": -0.39910352514071706, + "velocityX": -0.34121380138282903, + "velocityY": 0.5650299089765801, + "timestamp": 7.73319463154671 + }, + { + "x": 2.403044029416898, + "y": 5.516501717159452, + "heading": -2.6795300674706377, + "angularVelocity": -0.407821544690809, + "velocityX": -0.3394844482175417, + "velocityY": 0.559887226165873, + "timestamp": 7.805407175578738 + }, + { + "x": 2.378663293884139, + "y": 5.556534603961008, + "heading": -2.7096829003029756, + "angularVelocity": -0.41755671727843513, + "velocityX": -0.33762465870120245, + "velocityY": 0.5543757990828, + "timestamp": 7.877619719610766 + }, + { + "x": 2.354428772799968, + "y": 5.59613781059984, + "heading": -2.74062708945702, + "angularVelocity": -0.42851542718561086, + "velocityX": -0.33559987961957327, + "velocityY": 0.5484255840822709, + "timestamp": 7.949832263642794 + }, + { + "x": 2.330355697409695, + "y": 5.635273029678329, + "heading": -2.77247058331099, + "angularVelocity": -0.4409690072662031, + "velocityX": -0.3333641781072918, + "velocityY": 0.5419448878733852, + "timestamp": 8.02204480767482 + }, + { + "x": 2.306463861338364, + "y": 5.673893057942382, + "heading": -2.8053478457859313, + "angularVelocity": -0.45528464501069815, + "velocityX": -0.33085437428619124, + "velocityY": 0.534810520550616, + "timestamp": 8.094257351706847 + }, + { + "x": 2.282779542094817, + "y": 5.711938365696433, + "heading": -2.839430372846894, + "angularVelocity": -0.47197516051846805, + "velocityX": -0.3279806792714956, + "velocityY": 0.5268517854346452, + "timestamp": 8.166469895738874 + }, + { + "x": 2.259338596376033, + "y": 5.749331681355029, + "heading": -2.8749434001407064, + "angularVelocity": -0.4917847414164131, + "velocityX": -0.3246104403742845, + "velocityY": 0.5178229926647014, + "timestamp": 8.238682439770901 + }, + { + "x": 2.2361917393720554, + "y": 5.785968937848207, + "heading": -2.9121940133772717, + "angularVelocity": -0.5158468481603996, + "velocityX": -0.3205378970405998, + "velocityY": 0.5073530781151929, + "timestamp": 8.310894983802928 + }, + { + "x": 2.2134142469672544, + "y": 5.821702966064213, + "heading": -2.9516221084722227, + "angularVelocity": -0.5460006377488126, + "velocityX": -0.3154229325406202, + "velocityY": 0.4948451642993865, + "timestamp": 8.383107527834955 + }, + { + "x": 2.191125686501144, + "y": 5.856311001786043, + "heading": -2.9939026154296866, + "angularVelocity": -0.5855008644856994, + "velocityX": -0.3086521983801695, + "velocityY": 0.4792524094772386, + "timestamp": 8.455320071866982 + }, + { + "x": 2.169536365545648, + "y": 5.8894197426408175, + "heading": -3.0401828782490323, + "angularVelocity": -0.6408895218927577, + "velocityX": -0.29896912295350214, + "velocityY": 0.4584901598272232, + "timestamp": 8.527532615899009 + }, + { + "x": 2.1490856813219974, + "y": 5.9202865649088325, + "heading": -3.092780352693255, + "angularVelocity": -0.7283703288571992, + "velocityX": -0.28320127060722483, + "velocityY": 0.42744404980837086, + "timestamp": 8.599745159931036 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": -0.9106411553901581, + "velocityX": -0.24882570833455084, + "velocityY": 0.3665276028197312, + "timestamp": 8.671957703963063 + }, + { + "x": 2.131117343902588, + "y": 5.946754455566406, + "heading": -3.158540067224243, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 8.74417024799509 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj index 1dbcc825..36ca3a7c 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 1.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 1.traj @@ -1,337 +1,283 @@ { "samples": [ { - "x": 0.7470832465334709, - "y": 6.625458718233556, - "heading": 1.0564954979067924, - "angularVelocity": -5.684301329861994e-12, - "velocityX": -3.602456269610706e-10, - "velocityY": 4.1137821262407583e-10, + "x": 0.7470832467079163, + "y": 6.625458717346191, + "heading": 1.056495497952334, + "angularVelocity": -4.3186177103351096e-19, + "velocityX": 7.079282317781465e-18, + "velocityY": -1.220945161386867e-18, "timestamp": 0 }, { - "x": 0.7589569772775385, - "y": 6.629936126338657, - "heading": 1.0434265097431823, - "angularVelocity": -0.12778392744309977, - "velocityX": 0.11609712676013798, - "velocityY": 0.04377849867759475, - "timestamp": 0.10227411521599863 - }, - { - "x": 0.7822022838432713, - "y": 6.638629979602176, - "heading": 1.0183346599429852, - "angularVelocity": -0.2453392014361959, - "velocityX": 0.2272843592055021, - "velocityY": 0.08500540295845989, - "timestamp": 0.20454823043199727 - }, - { - "x": 0.816345318524229, - "y": 6.6512661070932495, - "heading": 0.9822355978698938, - "angularVelocity": -0.35296381641965074, - "velocityX": 0.33383847695279084, - "velocityY": 0.12355156118457596, - "timestamp": 0.30682234564799593 - }, - { - "x": 0.8609432271767217, - "y": 6.667555740683275, - "heading": 0.9361120278121537, - "angularVelocity": -0.45097989764859636, - "velocityX": 0.4360625240585109, - "velocityY": 0.15927424713723962, - "timestamp": 0.40909646086399454 - }, - { - "x": 0.9155883048461597, - "y": 6.687191502411657, - "heading": 0.8809291072748399, - "angularVelocity": -0.5395590113414345, - "velocityX": 0.5343001768187209, - "velocityY": 0.19199150048411856, - "timestamp": 0.5113705760799931 - }, - { - "x": 0.9799143577111046, - "y": 6.709840447255909, - "heading": 0.817662056557047, - "angularVelocity": -0.6186027670698295, - "velocityX": 0.6289573163604619, - "velocityY": 0.22145333556813593, - "timestamp": 0.6136446912959918 - }, - { - "x": 1.0536065024635548, - "y": 6.735133328172977, - "heading": 0.7473295527982347, - "angularVelocity": -0.687686259272288, - "velocityX": 0.7205356386732681, - "velocityY": 0.24730480411634378, - "timestamp": 0.7159188065119904 - }, - { - "x": 1.1364169197016813, - "y": 6.762648211314195, - "heading": 0.6710254557943266, - "angularVelocity": -0.7460743786173882, - "velocityX": 0.8096908708297301, - "velocityY": 0.2690307532427302, - "timestamp": 0.818192921727989 - }, - { - "x": 1.2281932228229642, - "y": 6.7918845460908575, - "heading": 0.5899378478266369, - "angularVelocity": -0.7928458510838684, - "velocityX": 0.8973561208709863, - "velocityY": 0.28586249636547506, - "timestamp": 0.9204670369439876 - }, - { - "x": 1.3289409008958384, - "y": 6.822217824239773, - "heading": 0.5053285615301966, - "angularVelocity": -0.8272795718879136, - "velocityX": 0.985075041075245, - "velocityY": 0.296588019337818, - "timestamp": 1.0227411521599863 - }, - { - "x": 1.4390031913745853, - "y": 6.852792823991799, - "heading": 0.4183893727045359, - "angularVelocity": -0.8500605319021087, - "velocityX": 1.076150015287458, - "velocityY": 0.29895148738224403, - "timestamp": 1.125015267375985 - }, - { - "x": 1.5597885498551032, - "y": 6.882059112916124, - "heading": 0.32979940511385675, - "angularVelocity": -0.8662012607776253, - "velocityX": 1.1809963684348201, - "velocityY": 0.28615537738935176, - "timestamp": 1.2272893825919837 - }, - { - "x": 1.700662064049805, - "y": 6.9025972281907375, - "heading": 0.2406449492427404, - "angularVelocity": -0.8717206268168672, - "velocityX": 1.3774112254273, - "velocityY": 0.20081439319892427, - "timestamp": 1.3295634978079824 - }, - { - "x": 1.8510164004103928, - "y": 6.913603640101383, - "heading": 0.18435642050200896, - "angularVelocity": -0.5503692556839603, - "velocityX": 1.4701113396826178, - "velocityY": 0.10761678064700267, - "timestamp": 1.4318376130239812 - }, - { - "x": 2.0039488544462833, - "y": 6.918843013673937, - "heading": 0.15617120081925578, - "angularVelocity": -0.2755850738942481, - "velocityX": 1.4953192592999736, - "velocityY": 0.051228726583383746, - "timestamp": 1.5341117282399799 - }, - { - "x": 2.1557445524327394, - "y": 6.919295783765447, - "heading": 0.15617120073466711, - "angularVelocity": -8.01993499988118e-11, - "velocityX": 1.484204462099971, - "velocityY": 0.0044270165813428694, - "timestamp": 1.6363858434559786 - }, - { - "x": 2.232501376452394, - "y": 6.918644981823818, - "heading": 0.15617120066422163, - "angularVelocity": -2.7881719949913897e-12, - "velocityX": 1.4631774376072004, - "velocityY": -0.012405855819009865, - "timestamp": 1.6888448428916174 - }, - { - "x": 2.3078780299226054, - "y": 6.916920762093343, - "heading": 0.1561712005937278, - "angularVelocity": -3.698975819730093e-12, - "velocityX": 1.436867928003674, - "velocityY": -0.032867889033463694, - "timestamp": 1.7413038423272562 - }, - { - "x": 2.381659462849091, - "y": 6.91403766609857, - "heading": 0.15617120052323402, - "angularVelocity": -3.687857737825465e-12, - "velocityX": 1.4064590265589882, - "velocityY": -0.05495897496281437, - "timestamp": 1.793762841762895 - }, - { - "x": 2.453627858185915, - "y": 6.909923503320222, - "heading": 0.15617120045273983, - "angularVelocity": -3.68806627753873e-12, - "velocityX": 1.3718979863339387, - "velocityY": -0.07842619311071317, - "timestamp": 1.8462218411985338 - }, - { - "x": 2.5235476514141553, - "y": 6.90450568335544, - "heading": 0.15617120038224525, - "angularVelocity": -3.688143550349055e-12, - "velocityX": 1.3328464960153679, - "velocityY": -0.10327716626043287, - "timestamp": 1.8986808406341726 - }, - { - "x": 2.5911718313337544, - "y": 6.897715977103231, - "heading": 0.1561712003117504, - "angularVelocity": -3.687560329325201e-12, - "velocityX": 1.2890863531735766, - "velocityY": -0.12942875706821344, - "timestamp": 1.9511398400698114 - }, - { - "x": 2.65624736130467, - "y": 6.889492702059127, - "heading": 0.15617120024125528, - "angularVelocity": -3.68944362324301e-12, - "velocityX": 1.240502697821755, - "velocityY": -0.1567561709974853, - "timestamp": 2.0035988395054503 - }, - { - "x": 2.7185182573768354, - "y": 6.879780772610566, - "heading": 0.1561712001707086, - "angularVelocity": -4.669569996251489e-12, - "velocityX": 1.187039346844362, - "velocityY": -0.1851336547556834, - "timestamp": 2.056057838941089 - }, - { - "x": 2.844727229024539, - "y": 6.826831262995891, - "heading": 0.36522912798339696, - "angularVelocity": 1.2815424227266763, - "velocityX": 0.7736714562084378, - "velocityY": -0.32458488264739044, - "timestamp": 2.219187770584854 - }, - { - "x": 2.905891887894138, - "y": 6.746880408353911, - "heading": 0.5636352419280429, - "angularVelocity": 1.2162459209838357, - "velocityX": 0.3749444284785087, - "velocityY": -0.490105365340026, - "timestamp": 2.382317702228619 - }, - { - "x": 2.909090532344001, - "y": 6.6442122814929245, - "heading": 0.7377210107974022, - "angularVelocity": 1.0671601901772823, - "velocityX": 0.0196079562833389, - "velocityY": -0.6293641266849203, - "timestamp": 2.5454476338723837 - }, - { - "x": 2.8528015320306115, - "y": 6.520167648469734, - "heading": 0.8617887991289785, - "angularVelocity": 0.7605458243522792, - "velocityX": -0.34505623601923857, - "velocityY": -0.7604038809886424, - "timestamp": 2.7085775655161486 - }, - { - "x": 2.6673775887021587, - "y": 6.358333521535233, - "heading": 0.8824380102910534, - "angularVelocity": 0.1265813757217392, - "velocityX": -1.1366641384675562, - "velocityY": -0.9920566103835737, - "timestamp": 2.8717074971599135 - }, - { - "x": 2.433334548523251, - "y": 6.191208743378355, - "heading": 0.7641593769894643, - "angularVelocity": -0.7250578241718747, - "velocityX": -1.4347032313583656, - "velocityY": -1.0244887408273557, - "timestamp": 3.0348374288036783 - }, - { - "x": 2.190845809692594, - "y": 6.024313186516238, - "heading": 0.6297541443819642, - "angularVelocity": -0.823915214372992, - "velocityX": -1.486476064011715, - "velocityY": -1.0230835952941921, - "timestamp": 3.1979673604474432 - }, - { - "x": 1.9580404786548902, - "y": 5.870547470854263, - "heading": 0.47066977289213696, - "angularVelocity": -0.9752003809508628, - "velocityX": -1.4271159717504125, - "velocityY": -0.9425965806313563, - "timestamp": 3.361097292091208 - }, - { - "x": 1.7498273467354928, - "y": 5.73486661370809, - "heading": 0.32338313207816166, - "angularVelocity": -0.9028793141986351, - "velocityX": -1.2763637531323493, - "velocityY": -0.831734899187721, - "timestamp": 3.524227223734973 - }, - { - "x": 1.577014249967444, - "y": 5.624033816048722, - "heading": 0.19882585300104294, - "angularVelocity": -0.7635464431083702, - "velocityX": -1.0593586039568081, - "velocityY": -0.6794142353068856, - "timestamp": 3.687357155378738 - }, - { - "x": 1.4506486184747536, - "y": 5.5441849844920235, - "heading": 0.1067515452077215, - "angularVelocity": -0.5644231371867715, - "velocityX": -0.7746317925903302, - "velocityY": -0.4894799553714506, - "timestamp": 3.8504870870225028 - }, - { - "x": 1.3818649056319274, - "y": 5.50129938101995, - "heading": 0.05628563946839357, - "angularVelocity": -0.30936018439404245, - "velocityX": -0.4216498592955241, - "velocityY": -0.2628923039688523, - "timestamp": 4.013617018666268 + "x": 0.7613714426488418, + "y": 6.628614589829769, + "heading": 1.0345345122436784, + "angularVelocity": -0.5024975798156311, + "velocityX": 0.32693358920665033, + "velocityY": 0.07221070612415084, + "timestamp": 0.04370366463598273 + }, + { + "x": 0.7899910246485671, + "y": 6.635101738465351, + "heading": 0.9912579578255775, + "angularVelocity": -0.9902271303461743, + "velocityX": 0.6548554277565509, + "velocityY": 0.14843488960512216, + "timestamp": 0.08740732927196546 + }, + { + "x": 0.8329823079677272, + "y": 6.6450825712167525, + "heading": 0.9272173132517384, + "angularVelocity": -1.465338092520316, + "velocityX": 0.9836997349591596, + "velocityY": 0.22837519083431007, + "timestamp": 0.13111099390794817 + }, + { + "x": 0.8904118185530745, + "y": 6.658650997634064, + "heading": 0.8429907149485981, + "angularVelocity": -1.927220497518482, + "velocityX": 1.3140662473888705, + "velocityY": 0.3104642718253751, + "timestamp": 0.1748146585439309 + }, + { + "x": 0.962426123217969, + "y": 6.675814256462545, + "heading": 0.7397363294626891, + "angularVelocity": -2.362602457847324, + "velocityX": 1.6477864102408268, + "velocityY": 0.39271898527129734, + "timestamp": 0.21851832317991365 + }, + { + "x": 1.0493031568120121, + "y": 6.696537442602017, + "heading": 0.6202474925482772, + "angularVelocity": -2.7340690514092785, + "velocityX": 1.9878661049974005, + "velocityY": 0.47417502198223827, + "timestamp": 0.2622219878158964 + }, + { + "x": 1.1514382857503924, + "y": 6.720845269246716, + "heading": 0.4905752358986126, + "angularVelocity": -2.9670797112720706, + "velocityX": 2.3369923275104343, + "velocityY": 0.5561965305921692, + "timestamp": 0.30592565245187914 + }, + { + "x": 1.269055326004296, + "y": 6.748845004507445, + "heading": 0.3629054511567606, + "angularVelocity": -2.921260397846258, + "velocityX": 2.6912397675014597, + "velocityY": 0.6406724812196789, + "timestamp": 0.3496293170878619 + }, + { + "x": 1.400011708453663, + "y": 6.779996215891522, + "heading": 0.2672618294840015, + "angularVelocity": -2.188457706450791, + "velocityX": 2.996462277022566, + "velocityY": 0.7127825925706782, + "timestamp": 0.3933329817238446 + }, + { + "x": 1.5437969816492523, + "y": 6.812790254409381, + "heading": 0.2092081541239441, + "angularVelocity": -1.3283479965261176, + "velocityX": 3.2900049548066255, + "velocityY": 0.7503727385565244, + "timestamp": 0.43703664635982736 + }, + { + "x": 1.6999373892672376, + "y": 6.847694256712915, + "heading": 0.1915003494540967, + "angularVelocity": -0.40517894362723794, + "velocityX": 3.572707435829763, + "velocityY": 0.7986516140981349, + "timestamp": 0.4807403109958101 + }, + { + "x": 1.8624433107711578, + "y": 6.878112633593708, + "heading": 0.1915002067487222, + "angularVelocity": -0.0000032652953862886492, + "velocityX": 3.7183591549465618, + "velocityY": 0.6960143304721129, + "timestamp": 0.5244439756317928 + }, + { + "x": 2.015535426159542, + "y": 6.900266962585095, + "heading": 0.1561712001056634, + "angularVelocity": -0.8083762983569384, + "velocityX": 3.502958313988489, + "velocityY": 0.5069215402397582, + "timestamp": 0.5681476402677755 + }, + { + "x": 2.1557445526123047, + "y": 6.919295787811279, + "heading": 0.15617120010566338, + "angularVelocity": 8.052388519770604e-18, + "velocityX": 3.208177795171024, + "velocityY": 0.4354057121910402, + "timestamp": 0.6118513049037583 + }, + { + "x": 2.2979527331593355, + "y": 6.936286453257653, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442958129174e-19, + "velocityX": 2.796432452004192, + "velocityY": 0.33411051356290106, + "timestamp": 0.6627047344028709 + }, + { + "x": 2.4202451824897016, + "y": 6.945011127796279, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959053082e-19, + "velocityX": 2.404802400445729, + "velocityY": 0.1715651161497845, + "timestamp": 0.7135581639019836 + }, + { + "x": 2.52317206188046, + "y": 6.94425224667938, + "heading": 0.15617120010566338, + "angularVelocity": -1.128244295905312e-19, + "velocityX": 2.0239909167296966, + "velocityY": -0.014922909317535413, + "timestamp": 0.7644115934010962 + }, + { + "x": 2.6070605614479327, + "y": 6.933368973549914, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442959052985e-19, + "velocityX": 1.6496134163171314, + "velocityY": -0.21401256978465483, + "timestamp": 0.8152650229002089 + }, + { + "x": 2.672125506615784, + "y": 6.911967284292252, + "heading": 0.15617120010566338, + "angularVelocity": -1.1282442956919517e-19, + "velocityX": 1.2794603197604928, + "velocityY": -0.4208504611874918, + "timestamp": 0.8661184523993215 + }, + { + "x": 2.7185182571411133, + "y": 6.8797807693481445, + "heading": 0.15617120010566338, + "angularVelocity": -1.0748383020939867e-19, + "velocityX": 0.9122836155256804, + "velocityY": -0.6329271252919907, + "timestamp": 0.9169718818984341 + }, + { + "x": 2.742166614394153, + "y": 6.787220144570856, + "heading": 0.15109105455160673, + "angularVelocity": -0.05590353076450862, + "velocityX": 0.26023401360416243, + "velocityY": -1.0185664327447723, + "timestamp": 1.0078453112611694 + }, + { + "x": 2.7063747461891747, + "y": 6.659930157895132, + "heading": 0.14109258501335736, + "angularVelocity": -0.11002632571879596, + "velocityX": -0.39386505446064113, + "velocityY": -1.4007393312694816, + "timestamp": 1.0987187406239047 + }, + { + "x": 2.6108392240840024, + "y": 6.498432494186991, + "heading": 0.12644294397632044, + "angularVelocity": -0.16120929010568202, + "velocityX": -1.0513031452112058, + "velocityY": -1.7771714442897992, + "timestamp": 1.18959216998664 + }, + { + "x": 2.4549786573639274, + "y": 6.303757740845773, + "heading": 0.10766682633359297, + "angularVelocity": -0.20661834569682533, + "velocityX": -1.715139043536396, + "velocityY": -2.1422626471390678, + "timestamp": 1.2804655993493754 + }, + { + "x": 2.237238983313492, + "y": 6.078890290802275, + "heading": 0.08626225710840621, + "angularVelocity": -0.23554265944719108, + "velocityX": -2.396076340217028, + "velocityY": -2.4745126448998374, + "timestamp": 1.3713390287121108 + }, + { + "x": 1.9563226558789764, + "y": 5.880741504295724, + "heading": 0.08626219926626796, + "angularVelocity": -6.365132100594054e-7, + "velocityX": -3.0912922446581543, + "velocityY": -2.1804920084573034, + "timestamp": 1.4622124580748461 + }, + { + "x": 1.7274737983405954, + "y": 5.727569361833653, + "heading": 0.0766627418429575, + "angularVelocity": -0.10563547002277199, + "velocityX": -2.5183253140463755, + "velocityY": -1.6855547714685755, + "timestamp": 1.5530858874375815 + }, + { + "x": 1.5549556420919104, + "y": 5.613995417389302, + "heading": 0.06722021492045382, + "angularVelocity": -0.10390855708563887, + "velocityX": -1.8984444348419232, + "velocityY": -1.2498036581298453, + "timestamp": 1.6439593168003168 + }, + { + "x": 1.4396298957459885, + "y": 5.538759046250953, + "heading": 0.06010941807852643, + "angularVelocity": -0.07824946072569235, + "velocityX": -1.269081041121286, + "velocityY": -0.8279248584097305, + "timestamp": 1.7348327461630522 + }, + { + "x": 1.3818649053573608, + "y": 5.5012993812561035, + "heading": 0.05628563945524819, + "angularVelocity": -0.0420780711159847, + "velocityX": -0.6356642507464983, + "velocityY": -0.4122180185951169, + "timestamp": 1.8257061755257875 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj index ad2ef04b..822513bf 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 2.traj @@ -1,193 +1,157 @@ { "samples": [ { - "x": 1.3818649054659777, - "y": 5.501299381110093, - "heading": 0.05628563946121549, - "angularVelocity": -8.20711890061584e-12, - "velocityX": -2.918186949691226e-10, - "velocityY": -8.516677331367793e-11, + "x": 1.3818649053573608, + "y": 5.5012993812561035, + "heading": 0.05628563945524819, + "angularVelocity": 1.3994093358769211e-21, + "velocityX": -9.902038153093951e-18, + "velocityY": -5.706702901855187e-18, "timestamp": 0 }, { - "x": 1.4254749911225055, - "y": 5.4996771535618185, - "heading": 0.052286234874463405, - "angularVelocity": -0.03818204740477268, - "velocityX": 0.41634256351427257, - "velocityY": -0.015487297221703413, - "timestamp": 0.10474568177260579 - }, - { - "x": 1.5054277602383004, - "y": 5.496460245033646, - "heading": 0.04534031368940726, - "angularVelocity": -0.06631224378557163, - "velocityX": 0.763303725485265, - "velocityY": -0.030711609603001202, - "timestamp": 0.20949136354521158 - }, - { - "x": 1.614706352195534, - "y": 5.491742221647794, - "heading": 0.036622661203774276, - "angularVelocity": -0.08322684370600701, - "velocityX": 1.0432753896642522, - "velocityY": -0.045042652489690874, - "timestamp": 0.3142370453178174 - }, - { - "x": 1.7468754648697162, - "y": 5.485736649446459, - "heading": 0.02746720464328704, - "angularVelocity": -0.08740652985101428, - "velocityX": 1.2618096559898304, - "velocityY": -0.05733479469253905, - "timestamp": 0.41898272709042317 - }, - { - "x": 1.8974421226741423, - "y": 5.478814465512338, - "heading": 0.0192537076414664, - "angularVelocity": -0.07841370510897432, - "velocityX": 1.43744978567254, - "velocityY": -0.06608562544064354, - "timestamp": 0.523728408863029 - }, - { - "x": 2.0478771487383725, - "y": 5.472018862260595, - "heading": 0.011842428389913316, - "angularVelocity": -0.07075498604111852, - "velocityX": 1.436193106275662, - "velocityY": -0.06487716815402857, - "timestamp": 0.6284740906356348 - }, - { - "x": 2.1991998312667134, - "y": 5.465297323381209, - "heading": 0.007319920190768939, - "angularVelocity": -0.04317608251475354, - "velocityX": 1.4446675030153622, - "velocityY": -0.06417008055763802, - "timestamp": 0.7332197724082405 - }, - { - "x": 2.337086103631991, - "y": 5.4645313180146555, - "heading": 0.0021217678335590726, - "angularVelocity": -0.04962641198543019, - "velocityX": 1.316390996187784, - "velocityY": -0.007313001468104138, - "timestamp": 0.8379654541808463 - }, - { - "x": 2.45341531313919, - "y": 5.464492524135499, - "heading": -0.0044756440905307565, - "angularVelocity": -0.06298504924092899, - "velocityX": 1.1105871626237722, - "velocityY": -0.0003703621622037101, - "timestamp": 0.9427111359534521 - }, - { - "x": 2.541111145262716, - "y": 5.463618987693384, - "heading": -0.012133903650681083, - "angularVelocity": -0.07311289048562652, - "velocityX": 0.8372262287970834, - "velocityY": -0.008339593435811936, - "timestamp": 1.047456817726058 - }, - { - "x": 2.592916727068705, - "y": 5.461223602405139, - "heading": -0.020267736871702392, - "angularVelocity": -0.07765316033563659, - "velocityX": 0.4945844156915881, - "velocityY": -0.022868582319897673, - "timestamp": 1.1522024994986637 - }, - { - "x": 2.573983182200651, - "y": 5.456183859404588, - "heading": -0.02941680850548689, - "angularVelocity": -0.07029420270393813, - "velocityX": -0.14547032678251723, - "velocityY": -0.03872138453206129, - "timestamp": 1.2823564991476335 - }, - { - "x": 2.4879155076147748, - "y": 5.449739339031148, - "heading": -0.03668510065770142, - "angularVelocity": -0.05584378637679435, - "velocityX": -0.6612756796636089, - "velocityY": -0.04951457856894505, - "timestamp": 1.4125104987966033 - }, - { - "x": 2.35046961428428, - "y": 5.4436438239165845, - "heading": -0.03977633600135786, - "angularVelocity": -0.02375059815495546, - "velocityX": -1.056025121733672, - "velocityY": -0.046833099245860266, - "timestamp": 1.542664498445573 - }, - { - "x": 2.1764781877651598, - "y": 5.443212327064482, - "heading": -0.032264347817817784, - "angularVelocity": 0.05771615319957162, - "velocityX": -1.336811984121226, - "velocityY": -0.0033152797571240995, - "timestamp": 1.6728184980945429 - }, - { - "x": 1.9752868983122995, - "y": 5.4534701667830054, - "heading": -0.009494098721117665, - "angularVelocity": 0.174948516049147, - "velocityX": -1.5457941360881122, - "velocityY": 0.07881309585526655, - "timestamp": 1.8029724977435126 - }, - { - "x": 1.7725080662079153, - "y": 5.4634789711909475, - "heading": 0.004869525328448035, - "angularVelocity": 0.11035868346800264, - "velocityX": -1.5579915534229911, - "velocityY": 0.07689970624561784, - "timestamp": 1.9331264973924824 - }, - { - "x": 1.5970859275969194, - "y": 5.473760403676058, - "heading": 0.019510408429697427, - "angularVelocity": 0.11248892189634652, - "velocityX": -1.347804439948399, - "velocityY": 0.07899436394267238, - "timestamp": 2.063280497041452 - }, - { - "x": 1.463998994938607, - "y": 5.481958295434183, - "heading": 0.031093440957313423, - "angularVelocity": 0.0889948258110792, - "velocityX": -1.0225343286428261, - "velocityY": 0.06298609118419185, - "timestamp": 2.193434496690422 - }, - { - "x": 1.3892531402609951, - "y": 5.486522674152253, - "heading": 0.03752685288213878, - "angularVelocity": 0.049429229542960515, - "velocityX": -0.5742878042328909, - "velocityY": 0.03506906174326123, - "timestamp": 2.323588496339392 + "x": 1.4353793564287938, + "y": 5.499850139119754, + "heading": 0.05330774673729568, + "angularVelocity": -0.037162247354032396, + "velocityX": 0.6678270361261427, + "velocityY": -0.018085639694878305, + "timestamp": 0.08013220216697658 + }, + { + "x": 1.5424082559070336, + "y": 5.49695165466464, + "heading": 0.04735195633823623, + "angularVelocity": -0.07432455664514218, + "velocityX": 1.335654038999412, + "velocityY": -0.036171281666197166, + "timestamp": 0.16026440433395317 + }, + { + "x": 1.7029515984698416, + "y": 5.492603927569323, + "heading": 0.03841826071733368, + "angularVelocity": -0.11148696003994531, + "velocityX": 2.003480975454455, + "velocityY": -0.05425692764886357, + "timestamp": 0.24039660650092998 + }, + { + "x": 1.9170093681982305, + "y": 5.486806957092212, + "heading": 0.026506650941055532, + "angularVelocity": -0.1486494749196728, + "velocityX": 2.671307713250448, + "velocityY": -0.07234258288609449, + "timestamp": 0.3205288086679068 + }, + { + "x": 2.1592197171124052, + "y": 5.478791804392635, + "heading": 0.011195918948050757, + "angularVelocity": -0.19106840419911095, + "velocityX": 3.0226343762457084, + "velocityY": -0.10002411618334738, + "timestamp": 0.4006610108348836 + }, + { + "x": 2.3479156387807323, + "y": 5.472225891398255, + "heading": -0.0011368905306301479, + "angularVelocity": -0.1539057850049126, + "velocityX": 2.3548076374480407, + "velocityY": -0.08193850682774677, + "timestamp": 0.4807932130018604 + }, + { + "x": 2.483097117399865, + "y": 5.467109219705175, + "heading": -0.010491771187470106, + "angularVelocity": -0.11674308709682761, + "velocityX": 1.6869807014344356, + "velocityY": -0.06385287755374197, + "timestamp": 0.5609254151688372 + }, + { + "x": 2.564764147705788, + "y": 5.463441789903776, + "heading": -0.01686872035025771, + "angularVelocity": -0.0795803558412077, + "velocityX": 1.0191536997292066, + "velocityY": -0.04576724091218098, + "timestamp": 0.641057617335814 + }, + { + "x": 2.59291672706604, + "y": 5.461223602294922, + "heading": -0.020267736871838693, + "angularVelocity": -0.042417610269816795, + "velocityX": 0.3513266651724994, + "velocityY": -0.027681600516002228, + "timestamp": 0.7211898195027908 + }, + { + "x": 2.5441490516673415, + "y": 5.460719866140607, + "heading": -0.019850376221801108, + "angularVelocity": 0.004155972098076838, + "velocityX": -0.48561621280493705, + "velocityY": -0.0050160775864576445, + "timestamp": 0.8216141342451149 + }, + { + "x": 2.4113319639164366, + "y": 5.462492299555472, + "heading": -0.014755896628636694, + "angularVelocity": 0.050729543001973076, + "velocityX": -1.3225590644227596, + "velocityY": 0.017649444951779485, + "timestamp": 0.9220384489874389 + }, + { + "x": 2.194465469101637, + "y": 5.466540902467684, + "heading": -0.004984302193896615, + "angularVelocity": 0.09730307306365722, + "velocityX": -2.1595018633809024, + "velocityY": 0.040314966774733244, + "timestamp": 1.022462763729763 + }, + { + "x": 1.8935495831026135, + "y": 5.472865675007542, + "heading": 0.009464396467489124, + "angularVelocity": 0.14387649742454536, + "velocityX": -2.9964445042133008, + "velocityY": 0.0629804898951702, + "timestamp": 1.122887078472087 + }, + { + "x": 1.641401371888256, + "y": 5.479694173969964, + "heading": 0.023495626181635927, + "angularVelocity": 0.13971944693024915, + "velocityX": -2.510828297522737, + "velocityY": 0.06799647057530608, + "timestamp": 1.223311393214411 + }, + { + "x": 1.473302552055367, + "y": 5.484246507552045, + "heading": 0.03284977683623274, + "angularVelocity": 0.09314627317695277, + "velocityX": -1.6738856547262355, + "velocityY": 0.04533098974846174, + "timestamp": 1.3237357079567351 + }, + { + "x": 1.3892531394958496, + "y": 5.486522674560547, + "heading": 0.03752685285960867, + "angularVelocity": 0.04657314351984077, + "velocityX": -0.8369428536821739, + "velocityY": 0.02266549703966028, + "timestamp": 1.4241600226990592 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj index 104394ca..7833c650 100644 --- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj +++ b/src/main/deploy/choreo/amp 4 local sgmt 3.traj @@ -1,229 +1,229 @@ { "samples": [ { - "x": 1.3892531401370203, - "y": 5.486522674374907, - "heading": 0.03752685288891443, - "angularVelocity": 5.018347137854837e-11, - "velocityX": -8.981333628450582e-10, - "velocityY": 1.1863234183511445e-9, + "x": 1.3892531394958496, + "y": 5.486522674560547, + "heading": 0.03752685285960867, + "angularVelocity": -2.6599524768911785e-21, + "velocityX": -1.3251439086588674e-20, + "velocityY": 5.237013291945624e-20, "timestamp": 0 }, { - "x": 1.410785208000178, - "y": 5.466798705442517, - "heading": 0.02538794622123859, - "angularVelocity": -0.12074981514388294, - "velocityX": 0.21418676347263918, - "velocityY": -0.1962009963268651, - "timestamp": 0.1005294019105829 - }, - { - "x": 1.4512665581597533, - "y": 5.429718184658836, - "heading": 0.002445244265967334, - "angularVelocity": -0.2282188250282345, - "velocityX": 0.4026816906907998, - "velocityY": -0.36885249450974994, - "timestamp": 0.2010588038211658 - }, - { - "x": 1.5081514675148215, - "y": 5.3776444456377, - "heading": -0.029986935931297812, - "angularVelocity": -0.3226138782330449, - "velocityX": 0.5658534482437384, - "velocityY": -0.5179951130124598, - "timestamp": 0.3015882057317487 - }, - { - "x": 1.578941487701644, - "y": 5.312948064530918, - "heading": -0.07059739560458454, - "angularVelocity": -0.4039659933419245, - "velocityX": 0.7041722939494359, - "velocityY": -0.6435568078574709, - "timestamp": 0.4021176076423316 - }, - { - "x": 1.6611946188865518, - "y": 5.238029568056684, - "heading": -0.11800108662062794, - "angularVelocity": -0.4715405656932785, - "velocityX": 0.8181997404499256, - "velocityY": -0.7452396506196601, - "timestamp": 0.5026470095529145 - }, - { - "x": 1.7524893345353523, - "y": 5.155358397604862, - "heading": -0.17044488274189246, - "angularVelocity": -0.521676197753198, - "velocityX": 0.9081394425873396, - "velocityY": -0.8223581237255294, - "timestamp": 0.6031764114634974 - }, - { - "x": 1.8499500532222615, - "y": 5.067431436588914, - "heading": -0.22499900927916092, - "angularVelocity": -0.5426683689609221, - "velocityX": 0.9694747626122975, - "velocityY": -0.8746392520744162, - "timestamp": 0.7037058133740803 - }, - { - "x": 1.9473638091809122, - "y": 4.979075974351683, - "heading": -0.27982409115325835, - "angularVelocity": -0.5453636534436577, - "velocityX": 0.9690076084526664, - "velocityY": -0.8789016988127862, - "timestamp": 0.8042352152846632 - }, - { - "x": 2.05818821772957, - "y": 4.854821291737014, - "heading": -0.34544281347225186, - "angularVelocity": -0.6527316496725523, - "velocityX": 1.1024079106043232, - "velocityY": -1.2360033993617228, - "timestamp": 0.9047646171952461 - }, - { - "x": 2.1664425710584037, - "y": 4.732231232720307, - "heading": -0.4041393246178968, - "angularVelocity": -0.5838740711290391, - "velocityX": 1.076842701113489, - "velocityY": -1.2194448247922516, - "timestamp": 1.005294019105829 - }, - { - "x": 2.2666201121554423, - "y": 4.61627032113389, - "heading": -0.4602072656334145, - "angularVelocity": -0.557726794034664, - "velocityX": 0.996499915039965, - "velocityY": -1.153502450677793, - "timestamp": 1.105823421016412 - }, - { - "x": 2.356480498307365, - "y": 4.5087945921361055, - "heading": -0.5124957847583047, - "angularVelocity": -0.5201316047807618, - "velocityX": 0.8938716814183492, - "velocityY": -1.0690974663909212, - "timestamp": 1.2063528229269949 - }, - { - "x": 2.433798199341425, - "y": 4.411691494217003, - "heading": -0.5602485683833155, - "angularVelocity": -0.47501310796660834, - "velocityX": 0.7691053456052521, - "velocityY": -0.9659173928845837, - "timestamp": 1.3068822248375778 - }, - { - "x": 2.4961951429135376, - "y": 4.327016530758609, - "heading": -0.6027268742617791, - "angularVelocity": -0.4225460918867537, - "velocityX": 0.620683518971687, - "velocityY": -0.8422905312450144, - "timestamp": 1.4074116267481598 - }, - { - "x": 2.541198492257727, - "y": 4.256927967764492, - "heading": -0.6391760227839379, - "angularVelocity": -0.3625720221944257, - "velocityX": 0.4476635477259144, - "velocityY": -0.6971946668803632, - "timestamp": 1.5079410286587427 - }, - { - "x": 2.5331758624705114, - "y": 4.192320334785442, - "heading": -0.6763224470307224, - "angularVelocity": -0.2449990301925222, - "velocityX": -0.052913214713167914, - "velocityY": -0.426119279886695, - "timestamp": 1.6595596871052694 - }, - { - "x": 2.46411262676954, - "y": 4.160473976285577, - "heading": -0.7018758155970488, - "angularVelocity": -0.16853709734646205, - "velocityX": -0.45550618406914944, - "velocityY": -0.21004248027074415, - "timestamp": 1.811178345551796 - }, - { - "x": 2.348779581051416, - "y": 4.153307846262027, - "heading": -0.7202693675207003, - "angularVelocity": -0.121314567375086, - "velocityX": -0.7606784530447123, - "velocityY": -0.04726417363495295, - "timestamp": 1.9627970039983227 - }, - { - "x": 2.201596779448434, - "y": 4.163036990055964, - "heading": -0.7333684069992377, - "angularVelocity": -0.0863946406365267, - "velocityX": -0.9707433369527995, - "velocityY": 0.06416850879734587, - "timestamp": 2.1144156624448494 - }, - { - "x": 2.03723470076713, - "y": 4.1831659239379135, - "heading": -0.7500897294060416, - "angularVelocity": -0.11028538710126731, - "velocityX": -1.0840491612778367, - "velocityY": 0.13276026521150996, - "timestamp": 2.266034320891376 - }, - { - "x": 1.8800877783569956, - "y": 4.195672966501764, - "heading": -0.7289968148061016, - "angularVelocity": 0.13911819814698503, - "velocityX": -1.0364616391016628, - "velocityY": 0.08249012434493122, - "timestamp": 2.4176529793379027 - }, - { - "x": 1.7046442157621393, - "y": 4.233321160679635, - "heading": -0.7238359462014516, - "angularVelocity": 0.03403847932720532, - "velocityX": -1.1571370254440554, - "velocityY": 0.24830844691554524, - "timestamp": 2.5692716377844294 - }, - { - "x": 1.572815192920781, - "y": 4.2658136358994225, - "heading": -0.7241682787394389, - "angularVelocity": -0.002191897597681109, - "velocityX": -0.8694775764003041, - "velocityY": 0.21430393183203963, - "timestamp": 2.720890296230956 - }, - { - "x": 1.4994455554717883, - "y": 4.286480902556393, - "heading": -0.7254698893818832, - "angularVelocity": -0.00858476582372283, - "velocityX": -0.4839090323218549, - "velocityY": 0.13631083596916982, - "timestamp": 2.8725089546774827 + "x": 1.4165016264324646, + "y": 5.468726018878958, + "heading": 0.027669365250431073, + "angularVelocity": -0.15749827520188686, + "velocityX": 0.4353634378786766, + "velocityY": -0.2843465480598295, + "timestamp": 0.06258790832180194 + }, + { + "x": 1.4710112662474708, + "y": 5.4331263299672, + "heading": 0.008563925885950884, + "angularVelocity": -0.3052576747931523, + "velocityX": 0.8709292461850596, + "velocityY": -0.5687949935747745, + "timestamp": 0.1251758166436039 + }, + { + "x": 1.5527987232870373, + "y": 5.379715427763577, + "heading": -0.018924667024813987, + "angularVelocity": -0.4391997375823718, + "velocityX": 1.306761309533597, + "velocityY": -0.8533741362469829, + "timestamp": 0.18776372496540583 + }, + { + "x": 1.6618865667976872, + "y": 5.308482777774649, + "heading": -0.05347261278820921, + "angularVelocity": -0.5519907389421553, + "velocityX": 1.7429539736296065, + "velocityY": -1.1381215940733804, + "timestamp": 0.2503516332872078 + }, + { + "x": 1.7983053814757264, + "y": 5.219415946962974, + "heading": -0.09280768567650903, + "angularVelocity": -0.6284771922086734, + "velocityX": 2.1796353055390356, + "velocityY": -1.4230677010921848, + "timestamp": 0.3129395416090097 + }, + { + "x": 1.9620858254779319, + "y": 5.112511494418258, + "heading": -0.13216035126284406, + "angularVelocity": -0.6287582800179152, + "velocityX": 2.6168064789785364, + "velocityY": -1.708068785348386, + "timestamp": 0.37552744993081166 + }, + { + "x": 2.1529952387272515, + "y": 4.987995425318902, + "heading": -0.15523721329619666, + "angularVelocity": -0.3687111880253399, + "velocityX": 3.0502603197368576, + "velocityY": -1.9894588657468077, + "timestamp": 0.4381153582526136 + }, + { + "x": 2.323053130467604, + "y": 4.850242258645187, + "heading": -0.20104196583241465, + "angularVelocity": -0.7318466739726868, + "velocityX": 2.7171045701988246, + "velocityY": -2.200954950682231, + "timestamp": 0.5007032665744156 + }, + { + "x": 2.466345532790285, + "y": 4.728187893870888, + "heading": -0.2844490729613583, + "angularVelocity": -1.3326393126943619, + "velocityX": 2.2894582382579274, + "velocityY": -1.9501269182338685, + "timestamp": 0.563291174896217 + }, + { + "x": 2.582053624940948, + "y": 4.6234869751422165, + "heading": -0.37568976157943984, + "angularVelocity": -1.4578005730589176, + "velocityX": 1.848729175542, + "velocityY": -1.6728617641340675, + "timestamp": 0.6258790832180186 + }, + { + "x": 2.670225613669648, + "y": 4.536352850675129, + "heading": -0.4673001281858147, + "angularVelocity": -1.463707113127209, + "velocityX": 1.4087703374804497, + "velocityY": -1.3921878331366975, + "timestamp": 0.68846699153982 + }, + { + "x": 2.730923003974787, + "y": 4.4668604060655035, + "heading": -0.555803694848379, + "angularVelocity": -1.4140681328974087, + "velocityX": 0.9697941971963301, + "velocityY": -1.1103174155033886, + "timestamp": 0.7510548998616215 + }, + { + "x": 2.764192819595337, + "y": 4.415045738220215, + "heading": -0.6391760227970993, + "angularVelocity": -1.3320836274005747, + "velocityX": 0.5315693799749596, + "velocityY": -0.8278702585630189, + "timestamp": 0.813642808183423 + }, + { + "x": 2.7480635723388045, + "y": 4.353298917368432, + "heading": -0.7461336511354502, + "angularVelocity": -1.226587505939294, + "velocityX": -0.18496981909961455, + "velocityY": -0.7081110544885852, + "timestamp": 0.9008421541904941 + }, + { + "x": 2.669473842343889, + "y": 4.302035156263182, + "heading": -0.8427126143522478, + "angularVelocity": -1.1075652242731957, + "velocityX": -0.9012651309167219, + "velocityY": -0.5878915777773497, + "timestamp": 0.9880415001975651 + }, + { + "x": 2.528459101717774, + "y": 4.261314542628117, + "heading": -0.927127909191312, + "angularVelocity": -0.9680725682531954, + "velocityX": -1.6171536494629242, + "velocityY": -0.46698301649834584, + "timestamp": 1.0752408462046361 + }, + { + "x": 2.3250882453572066, + "y": 4.231236836483453, + "heading": -0.996366799797096, + "angularVelocity": -0.7940299299970593, + "velocityX": -2.3322520829924085, + "velocityY": -0.34493040971001, + "timestamp": 1.1624401922117071 + }, + { + "x": 2.0595373245634314, + "y": 4.211999318818496, + "heading": -1.044279386733433, + "angularVelocity": -0.5494603931140937, + "velocityX": -3.0453315644390497, + "velocityY": -0.22061538928740462, + "timestamp": 1.2496395382187782 + }, + { + "x": 1.7326671393645254, + "y": 4.204157960465748, + "heading": -1.0516982080702795, + "angularVelocity": -0.08507886442456468, + "velocityX": -3.7485394118942086, + "velocityY": -0.08992450874702791, + "timestamp": 1.3368388842258492 + }, + { + "x": 1.419863969855018, + "y": 4.255364813771041, + "heading": -1.034285504034905, + "angularVelocity": 0.19968847053007022, + "velocityX": -3.587219214742072, + "velocityY": 0.5872389604979419, + "timestamp": 1.4240382302329202 + }, + { + "x": 1.1696215071966642, + "y": 4.29633086503019, + "heading": -1.02035490887151, + "angularVelocity": 0.15975572984532924, + "velocityX": -2.8697745352133857, + "velocityY": 0.4697976892605051, + "timestamp": 1.5112375762399912 + }, + { + "x": 0.9819396842688768, + "y": 4.327055591018277, + "heading": -1.009906819806218, + "angularVelocity": 0.11981843378096674, + "velocityX": -2.152330625421988, + "velocityY": 0.35235041769230013, + "timestamp": 1.5984369222470622 + }, + { + "x": 0.8568184786877541, + "y": 4.347538817267099, + "heading": -1.002941368423543, + "angularVelocity": 0.07987962870856867, + "velocityX": -1.434886972328629, + "velocityY": 0.2349011453269456, + "timestamp": 1.6856362682541333 + }, + { + "x": 0.7942578792572021, + "y": 4.357780456542969, + "heading": -0.9994586215071236, + "angularVelocity": 0.03994005776301401, + "velocityX": -0.7174434476318076, + "velocityY": 0.11745087256777795, + "timestamp": 1.7728356142612043 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto index c5819b58..347fd4ae 100644 --- a/src/main/deploy/pathplanner/autos/local 4.auto +++ b/src/main/deploy/pathplanner/autos/local 4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7470832465334709, - "y": 6.625458718233556 + "x": 0.7470832467079163, + "y": 6.625458717346191 }, - "rotation": 60.53273310463171 + "rotation": 60.53273310724105 }, "command": { "type": "sequential", @@ -18,7 +18,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { @@ -56,7 +56,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { @@ -94,7 +94,7 @@ } }, { - "type": "race", + "type": "deadline", "data": { "commands": [ { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c53affa3..732c74f0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -107,7 +107,7 @@ public void robotInit() { -controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED))); shooter.setDefaultCommand(shooter.run(0.0)); - pivot.setDefaultCommand(pivot.run(0.0)); + pivot.setDefaultCommand(pivot.run(-100.0)); kicker.setDefaultCommand(kicker.run(0.0)); // Controller bindings here @@ -164,15 +164,15 @@ public void testInit() { } private Command intake() { - return Commands.parallel(shooter.run(5.0), pivot.run(103.0)); + return Commands.parallel(shooter.run(5.0), pivot.run(0.0)); } private Command shootFender() { return Commands.parallel( swerve.stopCmd(), - shooter.run(-10.0), - pivot.run(-63.0), - Commands.waitSeconds(0.75).andThen(kicker.run(-6.0 * 360).asProxy())) + Commands.waitSeconds(0.5).andThen(shooter.run(-10.0)), + pivot.run(-160.0), + Commands.waitSeconds(1.0).andThen(kicker.run(-6.0 * 360).asProxy())) .withTimeout(1.5) .andThen(Commands.print("done shooting")); } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java index f4d00058..f5da4caf 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java @@ -6,8 +6,9 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import edu.wpi.first.math.util.Units; public class PivotIOReal implements PivotIO { @@ -17,7 +18,7 @@ public class PivotIOReal implements PivotIO { (27.0 / 1) * (48.0 / 22); // check if this is the correct gear ratio private TalonFX pivotMotor = new TalonFX(10); - private PositionVoltage motorRequest = new PositionVoltage(0.0); + private MotionMagicVoltage motorRequest = new MotionMagicVoltage(0.0); private StatusSignal supplyVoltageSignal = pivotMotor.getMotorVoltage(); private StatusSignal position = pivotMotor.getRotorPosition(); @@ -29,10 +30,14 @@ public PivotIOReal() { pivotConfig.Slot0.kP = 120; pivotConfig.Slot0.kD = 0.0; pivotConfig.Slot0.kI = 0; + pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + pivotConfig.Slot0.kG = 0.4; + pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 11; + pivotConfig.MotionMagic.MotionMagicAcceleration = 6; pivotConfig.Feedback.SensorToMechanismRatio = 58.9; - pivotConfig.CurrentLimits.StatorCurrentLimit = 40.0; + pivotConfig.CurrentLimits.StatorCurrentLimit = 60.0; pivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; pivotMotor.getConfigurator().apply(pivotConfig); From 8ae87ed1f649abd6fcdafa9158da171a85df2270 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 24 Jan 2024 18:18:12 -0800 Subject: [PATCH 15/19] updated alpha code for new alpha --- src/main/java/frc/robot/Robot.java | 14 +++--- .../robot/subsystems/kicker/KickerIOReal.java | 50 ------------------- .../subsystems/kicker/KickerSubsystem.java | 48 ------------------ .../KickerIO.java => routing/RoutingIO.java} | 18 +++---- .../subsystems/routing/RoutingIOReal.java | 48 ++++++++++++++++++ .../subsystems/routing/RoutingSubsystem.java | 33 ++++++++++++ .../robot/subsystems/shooter/ShooterIO.java | 4 +- .../subsystems/shooter/ShooterIOReal.java | 16 +++--- .../subsystems/shooter/ShooterSubsystem.java | 8 ++- 9 files changed, 115 insertions(+), 124 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java rename src/main/java/frc/robot/subsystems/{kicker/KickerIO.java => routing/RoutingIO.java} (60%) create mode 100644 src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 732c74f0..7d895916 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,10 +15,10 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.kicker.KickerIOReal; -import frc.robot.subsystems.kicker.KickerSubsystem; import frc.robot.subsystems.pivot.PivotIOReal; import frc.robot.subsystems.pivot.PivotSubsystem; +import frc.robot.subsystems.routing.RoutingIOReal; +import frc.robot.subsystems.routing.RoutingSubsystem; import frc.robot.subsystems.shooter.ShooterIOReal; import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.GyroIO; @@ -51,7 +51,7 @@ public static enum RobotMode { : SwerveSubsystem.createSimModules()); private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOReal()); private final PivotSubsystem pivot = new PivotSubsystem(new PivotIOReal()); - private final KickerSubsystem kicker = new KickerSubsystem(new KickerIOReal()); + private final RoutingSubsystem routing = new RoutingSubsystem(new RoutingIOReal()); @Override public void robotInit() { @@ -108,7 +108,7 @@ public void robotInit() { shooter.setDefaultCommand(shooter.run(0.0)); pivot.setDefaultCommand(pivot.run(-100.0)); - kicker.setDefaultCommand(kicker.run(0.0)); + routing.setDefaultCommand(routing.run(0.0)); // Controller bindings here controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); @@ -121,7 +121,7 @@ public void robotInit() { Commands.parallel( pivot.run(10.0), shooter.run(-2.0), - Commands.waitSeconds(0.5).andThen(kicker.run(-6.0 * 360)))); + Commands.waitSeconds(0.5).andThen(routing.run(-6.0 * 360)))); controller .a() .onTrue(swerve.runOnce(() -> swerve.setPose(new Pose2d(2.0, 2.0, new Rotation2d())))); @@ -170,9 +170,9 @@ private Command intake() { private Command shootFender() { return Commands.parallel( swerve.stopCmd(), - Commands.waitSeconds(0.5).andThen(shooter.run(-10.0)), pivot.run(-160.0), - Commands.waitSeconds(1.0).andThen(kicker.run(-6.0 * 360).asProxy())) + Commands.waitSeconds(0.5).andThen(shooter.run(-40.0)), + Commands.waitSeconds(1.0).andThen(routing.run(-40.0).asProxy())) .withTimeout(1.5) .andThen(Commands.print("done shooting")); } diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java b/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java deleted file mode 100644 index eea9d62c..00000000 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIOReal.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.subsystems.kicker; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.util.Units; - -public class KickerIOReal implements KickerIO { - public static final int KICKER_MOTOR_ID = 12; - - private TalonFX kickerMotor = new TalonFX(12); - private PositionVoltage motorRequest = new PositionVoltage(0.0); - - private StatusSignal supplyVoltageSignal = kickerMotor.getMotorVoltage(); - private StatusSignal position = kickerMotor.getRotorPosition(); - private StatusSignal velocity = kickerMotor.getRotorVelocity(); - private StatusSignal currentDraw = kickerMotor.getStatorCurrent(); - - public KickerIOReal() { - TalonFXConfiguration kickerConfig = new TalonFXConfiguration(); - kickerConfig.Slot0.kP = 50.0; - kickerConfig.Slot0.kD = 0.0; - kickerConfig.Slot0.kI = 0.0; - - kickerConfig.CurrentLimits.StatorCurrentLimit = 20.0; - kickerConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerConfig.Feedback.SensorToMechanismRatio = 1.0; - kickerMotor.getConfigurator().apply(kickerConfig); - } - - public void setPosition(double degrees) { - kickerMotor.setControl(motorRequest.withPosition(Units.degreesToRotations(degrees))); - } - - public void reset(double degrees) { - kickerMotor.setPosition((Units.degreesToRotations(degrees))); - } - - public KickerIOInputsAutoLogged updateInputs() { - KickerIOInputsAutoLogged updated = new KickerIOInputsAutoLogged(); // new values - - updated.currentDrawAmps = currentDraw.getValue(); - updated.positionRotations = position.getValue(); - updated.velocityRPS = velocity.getValue(); - updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); - - return (updated); - } -} diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java deleted file mode 100644 index e8baccde..00000000 --- a/src/main/java/frc/robot/subsystems/kicker/KickerSubsystem.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.kicker; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - -public class KickerSubsystem extends SubsystemBase { - KickerIO io; - KickerIOInputsAutoLogged inputs; - - public KickerSubsystem(KickerIO io) { - this.io = io; - inputs = new KickerIOInputsAutoLogged(); - - SmartDashboard.putData( - "reset Kicker value", - new InstantCommand( - () -> { - io.reset(0); - })); - } - - public Command reset() { - return this.runOnce( - () -> { - io.reset(0); - }); - } - - public RunCommand run(double degrees) { - return new RunCommand( - () -> { - Logger.recordOutput("Kicker/Target", Units.degreesToRotations(degrees)); - io.setPosition(degrees); - }, - this); - } - - @Override - public void periodic() { - inputs = io.updateInputs(); - Logger.processInputs("Kicker", inputs); - } -} diff --git a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java similarity index 60% rename from src/main/java/frc/robot/subsystems/kicker/KickerIO.java rename to src/main/java/frc/robot/subsystems/routing/RoutingIO.java index 9a06741b..f41ff5cc 100644 --- a/src/main/java/frc/robot/subsystems/kicker/KickerIO.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java @@ -2,26 +2,24 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems.kicker; +package frc.robot.subsystems.routing; import org.littletonrobotics.junction.AutoLog; +import frc.robot.subsystems.routing.RoutingIOInputsAutoLogged; + /** Add your docs here. */ -public interface KickerIO { +public interface RoutingIO { @AutoLog - public class KickerIOInputs { - + public class RoutingIOInputs { // Motor values public double velocityRPS; public double currentDrawAmps; public double temperatureCelsius; public double motorOutputVolts; - public double positionRotations; } + + public abstract RoutingIOInputsAutoLogged updateInputs(); - public abstract void setPosition(double degrees); - - public abstract void reset(double degrees); - - public abstract KickerIOInputsAutoLogged updateInputs(); + public abstract void setVelocity(double rps); } diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java new file mode 100644 index 00000000..9060775a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.routing; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.routing.RoutingIOInputsAutoLogged; + +public class RoutingIOReal implements RoutingIO { + public static final int KICKER_MOTOR_ID = 12; + + private TalonFX motor = new TalonFX(12); + private VelocityVoltage velocityOut = new VelocityVoltage(0.0).withEnableFOC(true); + + private StatusSignal supplyVoltageSignal = motor.getMotorVoltage(); + private StatusSignal velocity = motor.getRotorVelocity(); + private StatusSignal currentDraw = motor.getStatorCurrent(); + + public RoutingIOReal() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 1.0; + config.Slot0.kD = 0.0; + config.Slot0.kI = 0.0; + config.Slot0.kV = 5.0; + + config.CurrentLimits.StatorCurrentLimit = 20.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.Feedback.SensorToMechanismRatio = 1.0; + motor.getConfigurator().apply(config); + } + + public RoutingIOInputsAutoLogged updateInputs() { + RoutingIOInputsAutoLogged updated = new RoutingIOInputsAutoLogged(); // new values + + updated.currentDrawAmps = currentDraw.getValue(); + updated.velocityRPS = velocity.getValue(); + updated.motorOutputVolts = supplyVoltageSignal.getValue(); + + return (updated); + } + + @Override + public void setVelocity(double rps) { + motor.setControl(velocityOut.withVelocity(rps)); + } +} diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java new file mode 100644 index 00000000..325552da --- /dev/null +++ b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.routing; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.littletonrobotics.junction.Logger; + +public class RoutingSubsystem extends SubsystemBase { + RoutingIO io; + RoutingIOInputsAutoLogged inputs; + + public RoutingSubsystem(RoutingIO io) { + this.io = io; + inputs = new RoutingIOInputsAutoLogged(); + } + + public Command run(double velocity) { + return this.run( + () -> { + io.setVelocity(velocity); + }); + } + + @Override + public void periodic() { + inputs = io.updateInputs(); + Logger.processInputs("Routing", inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 53a62373..95df9886 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -18,7 +18,9 @@ public class ShooterIOInputs { public double motorOutputVolts; } - public abstract void setVelocity(double rotationsPerSecond); + public abstract void setVoltage(double voltage); + + public abstract void setVelocity(double rps); public abstract ShooterIOInputsAutoLogged updateInputs(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index eff5a551..530d1f66 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -12,8 +12,8 @@ public class ShooterIOReal implements ShooterIO { public static final double SHOOTER_GEAR_RATIO = 1; private TalonFX shooterMotor = new TalonFX(27); - private VelocityVoltage shooterMotorVelocity = new VelocityVoltage(0); - private VoltageOut voltageOut = new VoltageOut(0.0); + private VelocityVoltage shooterMotorVelocity = new VelocityVoltage(0).withEnableFOC(true); + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private StatusSignal supplyVoltageSignal = shooterMotor.getMotorVoltage(); private StatusSignal velocity = shooterMotor.getRotorVelocity(); @@ -21,18 +21,22 @@ public class ShooterIOReal implements ShooterIO { public ShooterIOReal() { TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); - shooterConfig.Slot0.kP = 0; + shooterConfig.Slot0.kP = 1.0; shooterConfig.Slot0.kD = 0; shooterConfig.Slot0.kI = 0; - shooterConfig.Slot0.kV = 0; + shooterConfig.Slot0.kV = 5.0; shooterConfig.CurrentLimits.StatorCurrentLimit = 100.0; shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; shooterMotor.getConfigurator().apply(shooterConfig); } - public void setVelocity(double velocity) { - shooterMotor.setControl(voltageOut.withOutput(velocity)); + public void setVoltage(double voltage) { + shooterMotor.setControl(voltageOut.withOutput(voltage)); + } + + public void setVelocity(double rps) { + shooterMotor.setControl(shooterMotorVelocity.withVelocity(rps)); } public ShooterIOInputsAutoLogged updateInputs() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b377a4a1..4eb7d0f4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -15,14 +15,18 @@ public ShooterSubsystem(ShooterIO io) { inputs = new ShooterIOInputsAutoLogged(); } - public Command run(double velocity) { + public Command run(double voltage) { return new RunCommand( () -> { - io.setVelocity(velocity); + io.setVoltage(voltage); }, this); } + public Command runVelocity(double rps) { + return this.run(() -> io.setVelocity(rps)); + } + @Override public void periodic() { inputs = io.updateInputs(); From 0228daac66b769ba47fe109c19e261ef4bb50bcc Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 25 Jan 2024 13:21:42 -0800 Subject: [PATCH 16/19] tried code on test shooter, issue with reading status signals --- src/main/java/frc/robot/Robot.java | 4 ++- .../robot/subsystems/routing/RoutingIO.java | 16 +++++----- .../subsystems/routing/RoutingIOReal.java | 22 ++++++------- .../subsystems/routing/RoutingSubsystem.java | 7 +---- .../robot/subsystems/shooter/ShooterIO.java | 12 +++---- .../subsystems/shooter/ShooterIOReal.java | 31 +++++++++++-------- .../subsystems/shooter/ShooterSubsystem.java | 5 ++- 7 files changed, 46 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7d895916..1002d2df 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,7 +114,9 @@ public void robotInit() { controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); controller.leftTrigger().whileTrue(intake()); - controller.rightTrigger().whileTrue(shootFender()); + controller + .rightTrigger() + .whileTrue(Commands.parallel(shooter.runVelocity(50.0), routing.run(50.0))); controller .leftBumper() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIO.java b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java index f41ff5cc..d9636341 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIO.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java @@ -6,20 +6,18 @@ import org.littletonrobotics.junction.AutoLog; -import frc.robot.subsystems.routing.RoutingIOInputsAutoLogged; - /** Add your docs here. */ public interface RoutingIO { @AutoLog - public class RoutingIOInputs { + public static class RoutingIOInputs { // Motor values - public double velocityRPS; - public double currentDrawAmps; - public double temperatureCelsius; - public double motorOutputVolts; + public double velocityRPS = 0.0; + public double currentDrawAmps = 0.0; + public double temperatureCelsius = 0.0; + public double motorOutputVolts = 0.0; } - - public abstract RoutingIOInputsAutoLogged updateInputs(); + + public abstract void updateInputs(RoutingIOInputsAutoLogged inputs); public abstract void setVelocity(double rps); } diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java index 9060775a..3a28de82 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -2,11 +2,9 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.routing.RoutingIOInputsAutoLogged; +import com.ctre.phoenix6.signals.NeutralModeValue; public class RoutingIOReal implements RoutingIO { public static final int KICKER_MOTOR_ID = 12; @@ -15,30 +13,28 @@ public class RoutingIOReal implements RoutingIO { private VelocityVoltage velocityOut = new VelocityVoltage(0.0).withEnableFOC(true); private StatusSignal supplyVoltageSignal = motor.getMotorVoltage(); - private StatusSignal velocity = motor.getRotorVelocity(); + private StatusSignal velocity = motor.getVelocity(); private StatusSignal currentDraw = motor.getStatorCurrent(); public RoutingIOReal() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 1.0; + config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; config.Slot0.kI = 0.0; config.Slot0.kV = 5.0; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.CurrentLimits.StatorCurrentLimit = 20.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.SensorToMechanismRatio = 1.0; motor.getConfigurator().apply(config); } - public RoutingIOInputsAutoLogged updateInputs() { - RoutingIOInputsAutoLogged updated = new RoutingIOInputsAutoLogged(); // new values - - updated.currentDrawAmps = currentDraw.getValue(); - updated.velocityRPS = velocity.getValue(); - updated.motorOutputVolts = supplyVoltageSignal.getValue(); - - return (updated); + public void updateInputs(RoutingIOInputsAutoLogged inputs) { + inputs.currentDrawAmps = currentDraw.getValueAsDouble(); + inputs.velocityRPS = motor.getVelocity().getValueAsDouble(); + inputs.motorOutputVolts = supplyVoltageSignal.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java index 325552da..3698e8ef 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java @@ -1,12 +1,7 @@ package frc.robot.subsystems.routing; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import org.littletonrobotics.junction.Logger; public class RoutingSubsystem extends SubsystemBase { @@ -27,7 +22,7 @@ public Command run(double velocity) { @Override public void periodic() { - inputs = io.updateInputs(); + io.updateInputs(inputs); Logger.processInputs("Routing", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 95df9886..735ce4f7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -9,18 +9,18 @@ /** Add your docs here. */ public interface ShooterIO { @AutoLog - public class ShooterIOInputs { + public static class ShooterIOInputs { // Motor values - public double velocityRPS; - public double currentDrawAmps; - public double temperatureCelsius; - public double motorOutputVolts; + public double velocityRPS = 0.0; + public double currentDrawAmps = 0.0; + public double temperatureCelsius = 0.0; + public double motorOutputVolts = 0.0; } public abstract void setVoltage(double voltage); public abstract void setVelocity(double rps); - public abstract ShooterIOInputsAutoLogged updateInputs(); + public abstract void updateInputs(ShooterIOInputsAutoLogged inputs); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 530d1f66..5bf563e3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; public class ShooterIOReal implements ShooterIO { @@ -15,20 +16,26 @@ public class ShooterIOReal implements ShooterIO { private VelocityVoltage shooterMotorVelocity = new VelocityVoltage(0).withEnableFOC(true); private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private StatusSignal supplyVoltageSignal = shooterMotor.getMotorVoltage(); - private StatusSignal velocity = shooterMotor.getRotorVelocity(); - private StatusSignal currentDraw = shooterMotor.getStatorCurrent(); + private StatusSignal supplyVoltageSignal; + private StatusSignal velocity; + private StatusSignal currentDraw; public ShooterIOReal() { TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); - shooterConfig.Slot0.kP = 1.0; + shooterConfig.Slot0.kP = 0.1; shooterConfig.Slot0.kD = 0; shooterConfig.Slot0.kI = 0; shooterConfig.Slot0.kV = 5.0; - shooterConfig.CurrentLimits.StatorCurrentLimit = 100.0; + shooterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + shooterConfig.CurrentLimits.StatorCurrentLimit = 60.0; shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; shooterMotor.getConfigurator().apply(shooterConfig); + + supplyVoltageSignal = shooterMotor.getMotorVoltage(); + velocity = shooterMotor.getVelocity(); + currentDraw = shooterMotor.getStatorCurrent(); } public void setVoltage(double voltage) { @@ -39,13 +46,11 @@ public void setVelocity(double rps) { shooterMotor.setControl(shooterMotorVelocity.withVelocity(rps)); } - public ShooterIOInputsAutoLogged updateInputs() { - ShooterIOInputsAutoLogged updated = new ShooterIOInputsAutoLogged(); - - updated.currentDrawAmps = currentDraw.getValue(); - updated.velocityRPS = velocity.getValue(); - updated.motorOutputVolts = 12 * supplyVoltageSignal.getValue(); - - return (updated); + @Override + public void updateInputs(ShooterIOInputsAutoLogged inputs) { + inputs.currentDrawAmps = currentDraw.getValueAsDouble(); + inputs.velocityRPS = shooterMotor.getVelocity().getValueAsDouble(); + inputs.motorOutputVolts = supplyVoltageSignal.getValueAsDouble(); + inputs.temperatureCelsius = 20.0; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 4eb7d0f4..1c6693ea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -8,11 +8,10 @@ public class ShooterSubsystem extends SubsystemBase { ShooterIO io; - ShooterIOInputsAutoLogged inputs; + ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); public ShooterSubsystem(ShooterIO io) { this.io = io; - inputs = new ShooterIOInputsAutoLogged(); } public Command run(double voltage) { @@ -29,7 +28,7 @@ public Command runVelocity(double rps) { @Override public void periodic() { - inputs = io.updateInputs(); + io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); } } From 1f202a7f936af89a3e3f58239b1612b8b383fd7e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 25 Jan 2024 14:47:13 -0800 Subject: [PATCH 17/19] set update frequency for status signals --- .../subsystems/routing/RoutingIOReal.java | 10 ++-- .../subsystems/shooter/ShooterIOReal.java | 47 ++++++++++--------- 2 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java index 3a28de82..08cee04d 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.routing; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -12,7 +13,7 @@ public class RoutingIOReal implements RoutingIO { private TalonFX motor = new TalonFX(12); private VelocityVoltage velocityOut = new VelocityVoltage(0.0).withEnableFOC(true); - private StatusSignal supplyVoltageSignal = motor.getMotorVoltage(); + private StatusSignal voltage = motor.getMotorVoltage(); private StatusSignal velocity = motor.getVelocity(); private StatusSignal currentDraw = motor.getStatorCurrent(); @@ -29,12 +30,15 @@ public RoutingIOReal() { config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.SensorToMechanismRatio = 1.0; motor.getConfigurator().apply(config); + + BaseStatusSignal.setUpdateFrequencyForAll(50, voltage, velocity, currentDraw); + motor.optimizeBusUtilization(); } public void updateInputs(RoutingIOInputsAutoLogged inputs) { inputs.currentDrawAmps = currentDraw.getValueAsDouble(); - inputs.velocityRPS = motor.getVelocity().getValueAsDouble(); - inputs.motorOutputVolts = supplyVoltageSignal.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.motorOutputVolts = voltage.getValueAsDouble(); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 5bf563e3..26a61508 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -12,45 +13,49 @@ public class ShooterIOReal implements ShooterIO { public static final int SHOOTER_MOTOR_ID = 27; public static final double SHOOTER_GEAR_RATIO = 1; - private TalonFX shooterMotor = new TalonFX(27); - private VelocityVoltage shooterMotorVelocity = new VelocityVoltage(0).withEnableFOC(true); + private TalonFX motor = new TalonFX(27); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0).withEnableFOC(true); private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private StatusSignal supplyVoltageSignal; - private StatusSignal velocity; - private StatusSignal currentDraw; + private StatusSignal voltage = motor.getMotorVoltage(); + private StatusSignal velocity = motor.getVelocity(); + private StatusSignal currentDraw = motor.getStatorCurrent(); public ShooterIOReal() { - TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); - shooterConfig.Slot0.kP = 0.1; - shooterConfig.Slot0.kD = 0; - shooterConfig.Slot0.kI = 0; - shooterConfig.Slot0.kV = 5.0; + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Slot0.kP = 0.1; + config.Slot0.kD = 0; + config.Slot0.kI = 0; + config.Slot0.kV = 5.0; - shooterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - shooterConfig.CurrentLimits.StatorCurrentLimit = 60.0; - shooterConfig.CurrentLimits.StatorCurrentLimitEnable = true; - shooterMotor.getConfigurator().apply(shooterConfig); + config.CurrentLimits.StatorCurrentLimit = 60.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + motor.getConfigurator().apply(config); - supplyVoltageSignal = shooterMotor.getMotorVoltage(); - velocity = shooterMotor.getVelocity(); - currentDraw = shooterMotor.getStatorCurrent(); + voltage = motor.getMotorVoltage(); + velocity = motor.getVelocity(); + currentDraw = motor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll(50, voltage, velocity, currentDraw); + motor.optimizeBusUtilization(); } public void setVoltage(double voltage) { - shooterMotor.setControl(voltageOut.withOutput(voltage)); + motor.setControl(voltageOut.withOutput(voltage)); } public void setVelocity(double rps) { - shooterMotor.setControl(shooterMotorVelocity.withVelocity(rps)); + motor.setControl(velocityVoltage.withVelocity(rps)); } @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { + inputs.currentDrawAmps = currentDraw.getValueAsDouble(); - inputs.velocityRPS = shooterMotor.getVelocity().getValueAsDouble(); - inputs.motorOutputVolts = supplyVoltageSignal.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.motorOutputVolts = voltage.getValueAsDouble(); inputs.temperatureCelsius = 20.0; } } From 72e6bad486d6ad75481e9c2f2cae03a6b1425805 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 26 Jan 2024 09:57:38 -0800 Subject: [PATCH 18/19] retuned shooter and routing --- src/main/java/frc/robot/Robot.java | 4 ++-- .../frc/robot/subsystems/routing/RoutingIO.java | 2 ++ .../robot/subsystems/routing/RoutingIOReal.java | 14 +++++++++++--- .../robot/subsystems/routing/RoutingSubsystem.java | 4 ++++ .../robot/subsystems/shooter/ShooterIOReal.java | 4 ++-- 5 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1002d2df..215f4339 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -108,7 +108,7 @@ public void robotInit() { shooter.setDefaultCommand(shooter.run(0.0)); pivot.setDefaultCommand(pivot.run(-100.0)); - routing.setDefaultCommand(routing.run(0.0)); + routing.setDefaultCommand(routing.stop()); // Controller bindings here controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0)))); @@ -116,7 +116,7 @@ public void robotInit() { controller.leftTrigger().whileTrue(intake()); controller .rightTrigger() - .whileTrue(Commands.parallel(shooter.runVelocity(50.0), routing.run(50.0))); + .whileTrue(Commands.parallel(shooter.runVelocity(-100.0), routing.run(100.0))); controller .leftBumper() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIO.java b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java index d9636341..9a051933 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIO.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIO.java @@ -20,4 +20,6 @@ public static class RoutingIOInputs { public abstract void updateInputs(RoutingIOInputsAutoLogged inputs); public abstract void setVelocity(double rps); + + public abstract void setVoltage(double voltage); } diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java index 08cee04d..2640243e 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -12,6 +13,7 @@ public class RoutingIOReal implements RoutingIO { private TalonFX motor = new TalonFX(12); private VelocityVoltage velocityOut = new VelocityVoltage(0.0).withEnableFOC(true); + private VoltageOut voltageOut = new VoltageOut(0.0); private StatusSignal voltage = motor.getMotorVoltage(); private StatusSignal velocity = motor.getVelocity(); @@ -19,14 +21,14 @@ public class RoutingIOReal implements RoutingIO { public RoutingIOReal() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.1; + config.Slot0.kP = 1.0; config.Slot0.kD = 0.0; config.Slot0.kI = 0.0; - config.Slot0.kV = 5.0; + config.Slot0.kV = 0.12; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.CurrentLimits.StatorCurrentLimit = 20.0; + config.CurrentLimits.StatorCurrentLimit = 60.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.SensorToMechanismRatio = 1.0; motor.getConfigurator().apply(config); @@ -36,6 +38,7 @@ public RoutingIOReal() { } public void updateInputs(RoutingIOInputsAutoLogged inputs) { + BaseStatusSignal.refreshAll(currentDraw, velocity, voltage); inputs.currentDrawAmps = currentDraw.getValueAsDouble(); inputs.velocityRPS = velocity.getValueAsDouble(); inputs.motorOutputVolts = voltage.getValueAsDouble(); @@ -45,4 +48,9 @@ public void updateInputs(RoutingIOInputsAutoLogged inputs) { public void setVelocity(double rps) { motor.setControl(velocityOut.withVelocity(rps)); } + + @Override + public void setVoltage(double voltage) { + motor.setControl(voltageOut.withOutput(voltage)); + } } diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java index 3698e8ef..91ffc7ba 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingSubsystem.java @@ -20,6 +20,10 @@ public Command run(double velocity) { }); } + public Command stop() { + return this.run(() -> io.setVoltage(0.0)); + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 26a61508..2155919d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -26,7 +26,7 @@ public ShooterIOReal() { config.Slot0.kP = 0.1; config.Slot0.kD = 0; config.Slot0.kI = 0; - config.Slot0.kV = 5.0; + config.Slot0.kV = 0.12; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -52,7 +52,7 @@ public void setVelocity(double rps) { @Override public void updateInputs(ShooterIOInputsAutoLogged inputs) { - + BaseStatusSignal.refreshAll(currentDraw, velocity, voltage); inputs.currentDrawAmps = currentDraw.getValueAsDouble(); inputs.velocityRPS = velocity.getValueAsDouble(); inputs.motorOutputVolts = voltage.getValueAsDouble(); From f68d08811b23a0e023b29a0603b0b6af6c9155f6 Mon Sep 17 00:00:00 2001 From: team computer Date: Sat, 27 Jan 2024 17:19:52 -0800 Subject: [PATCH 19/19] adjusted shooter tuning for new prototype --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java | 2 +- src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java | 3 +++ 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 215f4339..9a6886b9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -116,7 +116,7 @@ public void robotInit() { controller.leftTrigger().whileTrue(intake()); controller .rightTrigger() - .whileTrue(Commands.parallel(shooter.runVelocity(-100.0), routing.run(100.0))); + .whileTrue(Commands.parallel(shooter.runVelocity(80.0), routing.run(80.0))); controller .leftBumper() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java index 2640243e..92f761e6 100644 --- a/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java +++ b/src/main/java/frc/robot/subsystems/routing/RoutingIOReal.java @@ -21,7 +21,7 @@ public class RoutingIOReal implements RoutingIO { public RoutingIOReal() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 1.0; + config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; config.Slot0.kI = 0.0; config.Slot0.kV = 0.12; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 2155919d..af959bc6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; public class ShooterIOReal implements ShooterIO { @@ -30,6 +31,8 @@ public ShooterIOReal() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.CurrentLimits.StatorCurrentLimit = 60.0; config.CurrentLimits.StatorCurrentLimitEnable = true; motor.getConfigurator().apply(config);