From 6ed184cc842c322f0ee4adf6c0d0489e0a0385fc Mon Sep 17 00:00:00 2001 From: Brayden N Date: Tue, 20 Feb 2024 20:21:03 -0500 Subject: [PATCH 01/22] Created lights subsystem and created the logging for lights. --- src/main/java/frc/robot/RobotContainer.java | 1 + .../robot/commands/intake/SetIntakeSpeed.java | 4 ++ .../commands/shooter/SetShooterVelocity.java | 4 +- .../robot/commands/shooter/StopShooter.java | 4 ++ .../frc/robot/constants/LightsConstants.java | 19 ++++++ .../java/frc/robot/subsystems/Intake.java | 8 +++ .../java/frc/robot/subsystems/Lights.java | 63 +++++++++++++++++++ 7 files changed, 102 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/constants/LightsConstants.java create mode 100644 src/main/java/frc/robot/subsystems/Lights.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f990b29..82c776e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,6 +59,7 @@ public class RobotContainer { private static RobotContainer instance = null; public static RobotContainer getInstance() { + if (instance == null) instance = new RobotContainer(); return instance; diff --git a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java index ac64a9c..c973c57 100644 --- a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java +++ b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java @@ -5,7 +5,9 @@ package frc.robot.commands.intake; import frc.robot.constants.IntakeConstants; +import frc.robot.constants.LightsConstants; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Lights; import edu.wpi.first.wpilibj2.command.Command; @@ -43,6 +45,7 @@ public void initialize() { // detect a note, don't run if (isIndexing || !intake.detectNote()) intake.setMotorSpeed(speed); + Lights.lightArray[2] = true; } // Called every time the scheduler runs while the command is scheduled. @@ -53,6 +56,7 @@ public void execute() {} @Override public void end(boolean interrupted) { intake.stopMotor(); + Lights.lightArray[LightsConstants.orderIntake] = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java index 98bc687..0579c76 100644 --- a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java +++ b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java @@ -1,5 +1,7 @@ package frc.robot.commands.shooter; +import frc.robot.constants.LightsConstants; +import frc.robot.subsystems.Lights; import frc.robot.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; @@ -18,7 +20,7 @@ public SetShooterVelocity(Shooter shooter, double velocity_rpm) { @Override public void initialize() { shooter.setVelocity(targetVelocity); - + Lights.lightArray[LightsConstants.shooterSpunUp] = true; } @Override diff --git a/src/main/java/frc/robot/commands/shooter/StopShooter.java b/src/main/java/frc/robot/commands/shooter/StopShooter.java index f084e4d..621c46c 100644 --- a/src/main/java/frc/robot/commands/shooter/StopShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StopShooter.java @@ -1,5 +1,7 @@ package frc.robot.commands.shooter; +import frc.robot.constants.LightsConstants; +import frc.robot.subsystems.Lights; import frc.robot.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; @@ -16,6 +18,8 @@ public StopShooter(Shooter shooter) { @Override public void initialize() { shooter.stop(); + Lights.lightArray[LightsConstants.shooterSpunUp] = false; + Lights.lightArray[LightsConstants.noteShot] = true; } diff --git a/src/main/java/frc/robot/constants/LightsConstants.java b/src/main/java/frc/robot/constants/LightsConstants.java new file mode 100644 index 0000000..3713588 --- /dev/null +++ b/src/main/java/frc/robot/constants/LightsConstants.java @@ -0,0 +1,19 @@ +// 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.constants; + +public class LightsConstants { + public static final char firstLightInputPin = 5000; + public static final char secondLightInputPin = 5000; + public static final char thirdLightInputPin = 5000; + + public static final char orderAmplify = 0; + public static final char orderCoop = 1; + public static final char orderIntake = 2; + public static final char shooterSpunUp = 3; + public static final char noteClaimed = 4; + public static final char noteShot = 5; + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 098d97e..2f85817 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,6 +2,8 @@ import static frc.robot.constants.IntakeConstants.*; +import frc.robot.constants.LightsConstants; + import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,6 +27,12 @@ public Intake() { public void periodic() { updateInputs(inputs); Logger.processInputs(getName(), inputs); + + if (inputs.noteSensor == true) { + Lights.lightArray[LightsConstants.noteClaimed] = true; + } else { + Lights.lightArray[LightsConstants.noteClaimed] = false; + } } public void setMotorSpeed(double speed) { diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java new file mode 100644 index 0000000..44e54a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import frc.robot.constants.LightsConstants; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Lights extends SubsystemBase { + private DigitalInput firstlightInput = new DigitalInput(LightsConstants.firstLightInputPin); + private DigitalInput secondlightInput = new DigitalInput(LightsConstants.secondLightInputPin); + private DigitalInput threeDigitalInput = new DigitalInput(LightsConstants.thirdLightInputPin); + + public static boolean[] lightArray = new boolean[8]; // The array that will be used for lights + + public Lights() {} + + @Override + public void periodic() { // optimise to escape as soon as it hits a true + if (Lights.lightArray[LightsConstants.orderAmplify] == true) { + // send arduino byte + } + if (Lights.lightArray[LightsConstants.orderCoop] == true) { + // send arduino byte + } + if (Lights.lightArray[LightsConstants.orderIntake] == true) { + // send arduino byte + } + if (Lights.lightArray[LightsConstants.shooterSpunUp] == true) { + // send arduino byte + } + if (Lights.lightArray[LightsConstants.noteClaimed] == true) { + // send arduino byte + } + if (Lights.lightArray[LightsConstants.noteShot] == true) { + // send arduino byte + } + + } + + public void sendByte(boolean firstDigit, boolean secondDigit, boolean thirdDigit) { + firstlightInput = firstDigit; // why doesn't this work + } + + public void callCoop(boolean called) { + if (called) { // no need to check since it's a boolean. It's automatically assumed true + Lights.lightArray[LightsConstants.orderCoop] = true; + } else { + Lights.lightArray[LightsConstants.orderCoop] = false; + } + } + + public void callAmplify(boolean called) { + if (called) { // no need to check since it's a boolean. It's automatically assumed true + Lights.lightArray[LightsConstants.orderAmplify] = true; + } else { + Lights.lightArray[LightsConstants.orderAmplify] = false; + } + } +} From 83a867066dd6abdcad7ddeeca19d55f3a52ab81d Mon Sep 17 00:00:00 2001 From: RushaliB Date: Mon, 11 Mar 2024 20:00:58 -0400 Subject: [PATCH 02/22] attempted to do lights --- src/main/java/frc/robot/commands/Autos.java | 2 + .../robot/commands/intake/SetIntakeSpeed.java | 8 ++- .../commands/shooter/SetShooterVelocity.java | 4 +- .../robot/commands/shooter/StopShooter.java | 4 -- .../frc/robot/constants/LightsConstants.java | 24 ++++--- .../java/frc/robot/subsystems/Intake.java | 8 --- .../java/frc/robot/subsystems/Lights.java | 68 ++++++------------- 7 files changed, 40 insertions(+), 78 deletions(-) diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index a0ad72a..d65b129 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,7 +4,9 @@ import edu.wpi.first.wpilibj2.command.Commands; public final class Autos { + public static Command deadlineSeconds(double time_s, Command... commands) { return Commands.deadline(Commands.waitSeconds(time_s), commands); + } } diff --git a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java index c973c57..f0c5357 100644 --- a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java +++ b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java @@ -5,7 +5,6 @@ package frc.robot.commands.intake; import frc.robot.constants.IntakeConstants; -import frc.robot.constants.LightsConstants; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Lights; @@ -45,7 +44,10 @@ public void initialize() { // detect a note, don't run if (isIndexing || !intake.detectNote()) intake.setMotorSpeed(speed); - Lights.lightArray[2] = true; + if (isIndexing) { + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Shooting); + } + } // Called every time the scheduler runs while the command is scheduled. @@ -56,7 +58,7 @@ public void execute() {} @Override public void end(boolean interrupted) { intake.stopMotor(); - Lights.lightArray[LightsConstants.orderIntake] = false; + } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java index 0579c76..98bc687 100644 --- a/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java +++ b/src/main/java/frc/robot/commands/shooter/SetShooterVelocity.java @@ -1,7 +1,5 @@ package frc.robot.commands.shooter; -import frc.robot.constants.LightsConstants; -import frc.robot.subsystems.Lights; import frc.robot.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; @@ -20,7 +18,7 @@ public SetShooterVelocity(Shooter shooter, double velocity_rpm) { @Override public void initialize() { shooter.setVelocity(targetVelocity); - Lights.lightArray[LightsConstants.shooterSpunUp] = true; + } @Override diff --git a/src/main/java/frc/robot/commands/shooter/StopShooter.java b/src/main/java/frc/robot/commands/shooter/StopShooter.java index 621c46c..f084e4d 100644 --- a/src/main/java/frc/robot/commands/shooter/StopShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StopShooter.java @@ -1,7 +1,5 @@ package frc.robot.commands.shooter; -import frc.robot.constants.LightsConstants; -import frc.robot.subsystems.Lights; import frc.robot.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.Command; @@ -18,8 +16,6 @@ public StopShooter(Shooter shooter) { @Override public void initialize() { shooter.stop(); - Lights.lightArray[LightsConstants.shooterSpunUp] = false; - Lights.lightArray[LightsConstants.noteShot] = true; } diff --git a/src/main/java/frc/robot/constants/LightsConstants.java b/src/main/java/frc/robot/constants/LightsConstants.java index 3713588..feaadf5 100644 --- a/src/main/java/frc/robot/constants/LightsConstants.java +++ b/src/main/java/frc/robot/constants/LightsConstants.java @@ -4,16 +4,18 @@ package frc.robot.constants; -public class LightsConstants { - public static final char firstLightInputPin = 5000; - public static final char secondLightInputPin = 5000; - public static final char thirdLightInputPin = 5000; +import lombok.Getter; - public static final char orderAmplify = 0; - public static final char orderCoop = 1; - public static final char orderIntake = 2; - public static final char shooterSpunUp = 3; - public static final char noteClaimed = 4; - public static final char noteShot = 5; +public final class LightsConstants { + public enum Mode { + Shooting(0), Intaking(1), Disabled(2), Auto(3), We_can_shoot(4), TeleopRED(5), TeleopBLUE(6), Climb( + 7), We_have_a_note(8); -} + @Getter + private final int code; + + private Mode(int code) { + this.code = code; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2f85817..098d97e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,8 +2,6 @@ import static frc.robot.constants.IntakeConstants.*; -import frc.robot.constants.LightsConstants; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,12 +25,6 @@ public Intake() { public void periodic() { updateInputs(inputs); Logger.processInputs(getName(), inputs); - - if (inputs.noteSensor == true) { - Lights.lightArray[LightsConstants.noteClaimed] = true; - } else { - Lights.lightArray[LightsConstants.noteClaimed] = false; - } } public void setMotorSpeed(double speed) { diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index 44e54a3..5ed7c0a 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -1,63 +1,33 @@ -// Copyright (c) FIRST and other WPILib contributors. + // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems; -import frc.robot.constants.LightsConstants; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Lights extends SubsystemBase { - private DigitalInput firstlightInput = new DigitalInput(LightsConstants.firstLightInputPin); - private DigitalInput secondlightInput = new DigitalInput(LightsConstants.secondLightInputPin); - private DigitalInput threeDigitalInput = new DigitalInput(LightsConstants.thirdLightInputPin); +import frc.robot.constants.LightsConstants.Mode; - public static boolean[] lightArray = new boolean[8]; // The array that will be used for lights +import edu.wpi.first.wpilibj.DigitalOutput; - public Lights() {} +import org.littletonrobotics.junction.Logger; - @Override - public void periodic() { // optimise to escape as soon as it hits a true - if (Lights.lightArray[LightsConstants.orderAmplify] == true) { - // send arduino byte - } - if (Lights.lightArray[LightsConstants.orderCoop] == true) { - // send arduino byte - } - if (Lights.lightArray[LightsConstants.orderIntake] == true) { - // send arduino byte - } - if (Lights.lightArray[LightsConstants.shooterSpunUp] == true) { - // send arduino byte - } - if (Lights.lightArray[LightsConstants.noteClaimed] == true) { - // send arduino byte - } - if (Lights.lightArray[LightsConstants.noteShot] == true) { - // send arduino byte - } +import java.util.BitSet; - } +public class Lights { + private static DigitalOutput[] pins = { + new DigitalOutput(1), + new DigitalOutput(2), + new DigitalOutput(3), + new DigitalOutput(4) + }; - public void sendByte(boolean firstDigit, boolean secondDigit, boolean thirdDigit) { - firstlightInput = firstDigit; // why doesn't this work - } + public Lights() {} - public void callCoop(boolean called) { - if (called) { // no need to check since it's a boolean. It's automatically assumed true - Lights.lightArray[LightsConstants.orderCoop] = true; - } else { - Lights.lightArray[LightsConstants.orderCoop] = false; + public static void setStatus(Mode mode) { + int code = mode.getCode(); + BitSet bits = BitSet.valueOf(new byte[] {(byte) code}); + for (int i = 0; i <= 3; i++) { + pins[i].set(bits.get(i)); } - } - public void callAmplify(boolean called) { - if (called) { // no need to check since it's a boolean. It's automatically assumed true - Lights.lightArray[LightsConstants.orderAmplify] = true; - } else { - Lights.lightArray[LightsConstants.orderAmplify] = false; - } + Logger.recordOutput("Lights/CurrentStatus", mode); } } From 829c1ffbae0a0bd42674cef5e9566befd3c6fc30 Mon Sep 17 00:00:00 2001 From: RushaliB Date: Mon, 11 Mar 2024 20:20:41 -0400 Subject: [PATCH 03/22] set lights --- src/main/java/frc/robot/Robot.java | 20 +++++++++++++++++-- .../robot/commands/intake/IntakeWithArm.java | 2 ++ 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4459afa..e3ca8c1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,12 +3,14 @@ import static frc.robot.constants.Constants.*; import frc.robot.constants.BuildConstants; +import frc.robot.subsystems.Lights; import frc.robot.util.AutoSetterTunableNumber.AutoSetterTunableNumberManager; import frc.robot.util.VirtualSubsystem; import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -28,6 +30,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import java.util.Optional; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -39,6 +43,8 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; + Optional alliance = DriverStation.getAlliance(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -129,6 +135,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { robotContainer.updateOIAlert(); + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Disabled); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @@ -144,7 +151,9 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Auto); + } @Override public void teleopInit() { @@ -160,7 +169,14 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if (alliance.get() == Alliance.Red) { + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.TeleopRED); + } else { + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.TeleopBLUE); + } + + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index ddae6b0..799a4ea 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -3,6 +3,7 @@ import frc.robot.constants.IntakeConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Lights; import edu.wpi.first.wpilibj2.command.Command; @@ -22,6 +23,7 @@ public void initialize() { if (!intake.inputs.noteSensor) { arm.setPosition(-1.5); intake.setMotorSpeed(IntakeConstants.intakeSpeed); + Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Intaking); } } From 425e103a3ed0fafcb5a1fb031674a37a5e23e592 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Tue, 12 Mar 2024 21:06:24 -0400 Subject: [PATCH 04/22] updated piecevision to not crash. Also maybe swerve half fix and limelight shooting ready for testing --- choreo.chor | 290 +++++++++--------- src/main/deploy/choreo/2piece.1.traj | 105 ++++--- src/main/deploy/choreo/2piece.2.traj | 105 ++++--- src/main/deploy/choreo/2piece.traj | 208 ++++++------- src/main/java/frc/robot/RobotContainer.java | 20 +- .../commands/scoring/SetScoringPosition.java | 66 ++++ .../frc/robot/constants/SwerveConstants.java | 4 +- src/main/java/frc/robot/io/PieceVisionIO.java | 21 +- src/main/java/frc/robot/subsystems/Arm.java | 2 +- 9 files changed, 448 insertions(+), 373 deletions(-) create mode 100644 src/main/java/frc/robot/commands/scoring/SetScoringPosition.java diff --git a/choreo.chor b/choreo.chor index 0c961ef..81ce503 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1,5 +1,5 @@ { - "version": "v0.2.1", + "version": "v0.2.2", "robotConfiguration": { "mass": 50, "rotationalInertia": 6, @@ -34,8 +34,17 @@ "controlIntervalCount": 7 }, { - "x": 1.3126474618911743, - "y": 5.567087650299072, + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + }, + { + "x": 15.239320755004883, + "y": 0.5401557087898254, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -45,139 +54,139 @@ ], "trajectory": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -9.427350845574357e-28, + "angularVelocity": -2.6622071589745317e-28, + "velocityX": -3.282424439471613e-20, + "velocityY": -1.4930668038031123e-26, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 - }, - { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 - }, - { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 - }, - { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 - }, - { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 - }, - { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 - }, - { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, - "timestamp": 0.7789710677542292 - }, - { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.9092346498434495 - }, - { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 1.0394982319326698 - }, - { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 1.16976181402189 - }, - { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 1.3000253961111101 - }, - { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 1.4302889782003303 - }, - { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 1.5605525602895505 - }, - { - "x": 1.3126474618909545, - "y": 5.56708765026724, - "heading": -1.4475075754332281e-12, - "angularVelocity": 4.040377540643928e-12, - "velocityX": 2.6238029906463054e-11, - "velocityY": 8.150524229191312e-12, - "timestamp": 1.6908161423787706 + "x": 1.3990259521113642, + "y": 5.567087650299072, + "heading": 2.2823746146776588e-21, + "angularVelocity": 2.4245762980861946e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.913697562586356e-18, + "timestamp": 0.09413506307087183 + }, + { + "x": 1.5717829272959243, + "y": 5.567087650299071, + "heading": 7.969574197540583e-21, + "angularVelocity": 6.041532430318509e-20, + "velocityX": 1.8352032659126796, + "velocityY": 1.7827387306366018e-17, + "timestamp": 0.18827012614174365 + }, + { + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 2.1798246445258308e-20, + "angularVelocity": 1.4690246561439573e-19, + "velocityX": 2.752804703454417, + "velocityY": 2.674104215464885e-17, + "timestamp": 0.2824051892126155 + }, + { + "x": 2.0900538160588726, + "y": 5.567087650299072, + "heading": 8.97766497460724e-21, + "angularVelocity": -1.3619346247394927e-19, + "velocityX": 2.7528047034544167, + "velocityY": 2.6750900782152162e-17, + "timestamp": 0.3765402522834873 + }, + { + "x": 2.262810791243433, + "y": 5.567087650299072, + "heading": 2.5676765945622405e-21, + "angularVelocity": -6.809351436893968e-20, + "velocityX": 1.8352032659126798, + "velocityY": 1.783723417840217e-17, + "timestamp": 0.47067531535435914 + }, + { + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 1.9544386221379647e-27, + "angularVelocity": -2.7276485190537467e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.923538165503086e-18, + "timestamp": 0.564810378425231 + }, + { + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 9.792832338957588e-28, + "angularVelocity": -3.4440052264109466e-28, + "velocityX": -1.0343178913428024e-19, + "velocityY": 9.836392156979254e-21, + "timestamp": 0.6589454414961028 + }, + { + "x": 2.324642160074082, + "y": 5.479331567651817, + "heading": -2.6812320806456912e-18, + "angularVelocity": -2.773115697648232e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327076, + "timestamp": 0.7556320686521809 + }, + { + "x": 2.275547918749188, + "y": 5.303819407556036, + "heading": -1.522107913528158e-17, + "angularVelocity": -1.2969577360167464e-16, + "velocityX": -0.507766614359639, + "velocityY": -1.815268204696575, + "timestamp": 0.852318695808259 + }, + { + "x": 2.201906561851502, + "y": 5.04055118560791, + "heading": -2.9497325867653756e-17, + "angularVelocity": -1.4765482111895074e-16, + "velocityX": -0.7616498688987154, + "velocityY": -2.7229021188539413, + "timestamp": 0.9490053229643372 + }, + { + "x": 2.1282652049538155, + "y": 4.7772829636597836, + "heading": -1.7518587052454428e-17, + "angularVelocity": 1.238924044843347e-16, + "velocityX": -0.7616498688987153, + "velocityY": -2.722902118853941, + "timestamp": 1.0456919501204154 + }, + { + "x": 2.079170963628921, + "y": 4.601770803564003, + "heading": -6.414309314069054e-18, + "angularVelocity": 1.1484812298678339e-16, + "velocityX": -0.5077666143596389, + "velocityY": -1.815268204696575, + "timestamp": 1.1423785772764936 + }, + { + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -7.445922412027734e-29, + "angularVelocity": 6.634122421282305e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327075, + "timestamp": 1.2390652044325718 + }, + { + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -3.654785275835865e-29, + "angularVelocity": 1.4102041741246665e-29, + "velocityX": 1.470589714203873e-20, + "velocityY": 5.257360934767921e-20, + "timestamp": 1.33575183158865 } ], "constraints": [ @@ -185,29 +194,25 @@ "scope": [ "first" ], - "type": "StopPoint", - "uuid": "bfe353f8-e8f8-4546-b813-3567c6958d6d" + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "uuid": "81b025c4-b2fe-4840-9a44-77be52e9b1f2" + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "WptZeroVelocity", - "uuid": "f8f97419-9800-4b8e-b0c8-7a9f550f0abb" + "type": "WptZeroVelocity" }, { "scope": [ 2 ], - "type": "StopPoint", - "uuid": "67dfed6f-c992-4f47-8847-3e0a2874a582" + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -216,5 +221,6 @@ "circleObstacles": [] } }, - "splitTrajectoriesAtStopPoints": true + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": false } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.1.traj b/src/main/deploy/choreo/2piece.1.traj index 358f5d8..62f4727 100644 --- a/src/main/deploy/choreo/2piece.1.traj +++ b/src/main/deploy/choreo/2piece.1.traj @@ -1,67 +1,76 @@ { "samples": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -9.427350845574357e-28, + "angularVelocity": -2.6622071589745317e-28, + "velocityX": -3.282424439471613e-20, + "velocityY": -1.4930668038031123e-26, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 + "x": 1.3990259521113642, + "y": 5.567087650299072, + "heading": 2.2823746146776588e-21, + "angularVelocity": 2.4245762980861946e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.913697562586356e-18, + "timestamp": 0.09413506307087183 }, { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 + "x": 1.5717829272959243, + "y": 5.567087650299071, + "heading": 7.969574197540583e-21, + "angularVelocity": 6.041532430318509e-20, + "velocityX": 1.8352032659126796, + "velocityY": 1.7827387306366018e-17, + "timestamp": 0.18827012614174365 }, { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 2.1798246445258308e-20, + "angularVelocity": 1.4690246561439573e-19, + "velocityX": 2.752804703454417, + "velocityY": 2.674104215464885e-17, + "timestamp": 0.2824051892126155 }, { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 + "x": 2.0900538160588726, + "y": 5.567087650299072, + "heading": 8.97766497460724e-21, + "angularVelocity": -1.3619346247394927e-19, + "velocityX": 2.7528047034544167, + "velocityY": 2.6750900782152162e-17, + "timestamp": 0.3765402522834873 }, { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 + "x": 2.262810791243433, + "y": 5.567087650299072, + "heading": 2.5676765945622405e-21, + "angularVelocity": -6.809351436893968e-20, + "velocityX": 1.8352032659126798, + "velocityY": 1.783723417840217e-17, + "timestamp": 0.47067531535435914 }, { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 1.9544386221379647e-27, + "angularVelocity": -2.7276485190537467e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.923538165503086e-18, + "timestamp": 0.564810378425231 + }, + { + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 9.792832338957588e-28, + "angularVelocity": -3.4440052264109466e-28, + "velocityX": -1.0343178913428024e-19, + "velocityY": 9.836392156979254e-21, + "timestamp": 0.6589454414961028 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.2.traj b/src/main/deploy/choreo/2piece.2.traj index 15af8cd..fff0bc0 100644 --- a/src/main/deploy/choreo/2piece.2.traj +++ b/src/main/deploy/choreo/2piece.2.traj @@ -1,67 +1,76 @@ { "samples": [ { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 9.792832338957588e-28, + "angularVelocity": -3.4440052264109466e-28, + "velocityX": -1.0343178913428024e-19, + "velocityY": 9.836392156979254e-21, "timestamp": 0 }, { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.1302635820892203 + "x": 2.324642160074082, + "y": 5.479331567651817, + "heading": -2.6812320806456912e-18, + "angularVelocity": -2.773115697648232e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327076, + "timestamp": 0.09668662715607812 }, { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 0.2605271641784406 + "x": 2.275547918749188, + "y": 5.303819407556036, + "heading": -1.522107913528158e-17, + "angularVelocity": -1.2969577360167464e-16, + "velocityX": -0.507766614359639, + "velocityY": -1.815268204696575, + "timestamp": 0.19337325431215624 }, { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 0.39079074626766075 + "x": 2.201906561851502, + "y": 5.04055118560791, + "heading": -2.9497325867653756e-17, + "angularVelocity": -1.4765482111895074e-16, + "velocityX": -0.7616498688987154, + "velocityY": -2.7229021188539413, + "timestamp": 0.29005988146823436 }, { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 0.5210543283568809 + "x": 2.1282652049538155, + "y": 4.7772829636597836, + "heading": -1.7518587052454428e-17, + "angularVelocity": 1.238924044843347e-16, + "velocityX": -0.7616498688987153, + "velocityY": -2.722902118853941, + "timestamp": 0.3867465086243126 }, { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 0.6513179104461011 + "x": 2.079170963628921, + "y": 4.601770803564003, + "heading": -6.414309314069054e-18, + "angularVelocity": 1.1484812298678339e-16, + "velocityX": -0.5077666143596389, + "velocityY": -1.815268204696575, + "timestamp": 0.4834331357803908 }, { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 0.7815814925353213 + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -7.445922412027734e-29, + "angularVelocity": 6.634122421282305e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327075, + "timestamp": 0.580119762936469 + }, + { + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -3.654785275835865e-29, + "angularVelocity": 1.4102041741246665e-29, + "velocityX": 1.470589714203873e-20, + "velocityY": 5.257360934767921e-20, + "timestamp": 0.6768063900925473 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.traj b/src/main/deploy/choreo/2piece.traj index 745f113..d781900 100644 --- a/src/main/deploy/choreo/2piece.traj +++ b/src/main/deploy/choreo/2piece.traj @@ -1,139 +1,139 @@ { "samples": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -9.427350845574357e-28, + "angularVelocity": -2.6622071589745317e-28, + "velocityX": -3.282424439471613e-20, + "velocityY": -1.4930668038031123e-26, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 + "x": 1.3990259521113642, + "y": 5.567087650299072, + "heading": 2.2823746146776588e-21, + "angularVelocity": 2.4245762980861946e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.913697562586356e-18, + "timestamp": 0.09413506307087183 }, { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 + "x": 1.5717829272959243, + "y": 5.567087650299071, + "heading": 7.969574197540583e-21, + "angularVelocity": 6.041532430318509e-20, + "velocityX": 1.8352032659126796, + "velocityY": 1.7827387306366018e-17, + "timestamp": 0.18827012614174365 }, { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 2.1798246445258308e-20, + "angularVelocity": 1.4690246561439573e-19, + "velocityX": 2.752804703454417, + "velocityY": 2.674104215464885e-17, + "timestamp": 0.2824051892126155 }, { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 + "x": 2.0900538160588726, + "y": 5.567087650299072, + "heading": 8.97766497460724e-21, + "angularVelocity": -1.3619346247394927e-19, + "velocityX": 2.7528047034544167, + "velocityY": 2.6750900782152162e-17, + "timestamp": 0.3765402522834873 }, { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 + "x": 2.262810791243433, + "y": 5.567087650299072, + "heading": 2.5676765945622405e-21, + "angularVelocity": -6.809351436893968e-20, + "velocityX": 1.8352032659126798, + "velocityY": 1.783723417840217e-17, + "timestamp": 0.47067531535435914 }, { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 1.9544386221379647e-27, + "angularVelocity": -2.7276485190537467e-20, + "velocityX": 0.9176016608727151, + "velocityY": 8.923538165503086e-18, + "timestamp": 0.564810378425231 }, { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, - "timestamp": 0.7789710677542292 + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 9.792832338957588e-28, + "angularVelocity": -3.4440052264109466e-28, + "velocityX": -1.0343178913428024e-19, + "velocityY": 9.836392156979254e-21, + "timestamp": 0.6589454414961028 }, { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.9092346498434495 + "x": 2.324642160074082, + "y": 5.479331567651817, + "heading": -2.6812320806456912e-18, + "angularVelocity": -2.773115697648232e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327076, + "timestamp": 0.7556320686521809 }, { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 1.0394982319326698 + "x": 2.275547918749188, + "y": 5.303819407556036, + "heading": -1.522107913528158e-17, + "angularVelocity": -1.2969577360167464e-16, + "velocityX": -0.507766614359639, + "velocityY": -1.815268204696575, + "timestamp": 0.852318695808259 }, { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 1.16976181402189 + "x": 2.201906561851502, + "y": 5.04055118560791, + "heading": -2.9497325867653756e-17, + "angularVelocity": -1.4765482111895074e-16, + "velocityX": -0.7616498688987154, + "velocityY": -2.7229021188539413, + "timestamp": 0.9490053229643372 }, { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 1.3000253961111101 + "x": 2.1282652049538155, + "y": 4.7772829636597836, + "heading": -1.7518587052454428e-17, + "angularVelocity": 1.238924044843347e-16, + "velocityX": -0.7616498688987153, + "velocityY": -2.722902118853941, + "timestamp": 1.0456919501204154 }, { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 1.4302889782003303 + "x": 2.079170963628921, + "y": 4.601770803564003, + "heading": -6.414309314069054e-18, + "angularVelocity": 1.1484812298678339e-16, + "velocityX": -0.5077666143596389, + "velocityY": -1.815268204696575, + "timestamp": 1.1423785772764936 }, { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 1.5605525602895505 + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -7.445922412027734e-29, + "angularVelocity": 6.634122421282305e-17, + "velocityX": -0.25388331469992637, + "velocityY": -0.9076341292327075, + "timestamp": 1.2390652044325718 }, { - "x": 1.3126474618909545, - "y": 5.56708765026724, - "heading": -1.4475075754332281e-12, - "angularVelocity": 4.040377540643928e-12, - "velocityX": 2.6238029906463054e-11, - "velocityY": 8.150524229191312e-12, - "timestamp": 1.6908161423787706 + "x": 2.05462384223938, + "y": 4.514014720916748, + "heading": -3.654785275835865e-29, + "angularVelocity": 1.4102041741246665e-29, + "velocityX": 1.470589714203873e-20, + "velocityY": 5.257360934767921e-20, + "timestamp": 1.33575183158865 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e3ea42..e37e598 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import frc.robot.constants.Constants.OperatorConstants; import frc.robot.constants.Constants.ShuffleboardConstants; import frc.robot.constants.IntakeConstants; +import frc.robot.constants.ScoringConstants.ScoringPosition; import frc.robot.constants.ShooterConstants; import frc.robot.io.GyroIO; import frc.robot.io.GyroIOPigeon2; @@ -23,7 +24,7 @@ import frc.robot.commands.arm.SetArmPosition; import frc.robot.commands.intake.IntakeWithArm; import frc.robot.commands.intake.SetIntakeSpeed; -import frc.robot.commands.shooter.SetShooterVelocity; +import frc.robot.commands.scoring.SetScoringPosition; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; @@ -98,7 +99,7 @@ public RobotContainer() { // named commands must be registered before any paths are created NamedCommands.registerCommand("XStance", new XStance(swerve)); NamedCommands.registerCommand("AimAndShoot", - Commands.print("aiming and shooting").andThen(Commands.waitSeconds(1))); + (new SetArmPosition(arm, 5)).andThen(Commands.print("arm moving to shooting position and shooting"))); NamedCommands.registerCommand("Intake", Commands.print("arm to intaking position & rollers running")); NamedCommands.registerCommand("WaitIntake", Commands.print("wait for intake note sensor").andThen(Commands.waitSeconds(1))); @@ -178,17 +179,18 @@ private void configureBindings() { // *OPERATOR CONTROLS* // operatorController.a() - .onTrue(Commands.parallel(new SetArmPosition(arm, 84), new SetShooterVelocity(shooter, 1750))); - operatorController.b().onTrue(Commands.parallel(new SetArmPosition(arm, -1.5), - new SetShooterVelocity(shooter, ShooterConstants.subwooferVelocity_rpm))); + .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); + operatorController.b() + .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); operatorController.x().onTrue(new StopShooter(shooter)); - // y -> set shooter speed and arm angle based on limelight + operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 84)); operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); - // dpad up -> climber up - // dpad down -> climber down + operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); + operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); + operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); @@ -218,7 +220,7 @@ private void configureBindings() { testController.a() .onTrue(new InstantCommand(() -> shooter.setPercentOutput(shooterSpeedEntry.getDouble(0)), shooter)); - testController.rightBumper() + testController.leftTrigger(boolTriggerThreshold) .whileTrue(Commands .runEnd(() -> intake.setMotorSpeed(intakeSpeedEntry.getDouble(0)), () -> intake.stopMotor(), intake) .until(() -> intake.inputs.noteSensor).unless(() -> intake.inputs.noteSensor)); diff --git a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java new file mode 100644 index 0000000..bfa231f --- /dev/null +++ b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java @@ -0,0 +1,66 @@ +package frc.robot.commands.scoring; + +import frc.robot.constants.ScoringConstants.ScoringPosition; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.Supplier; + +public class SetScoringPosition extends Command { + private Arm arm; + private Shooter shooter; + + private Supplier posSupplier; + + /** + * Create a set scoring position command that adjusts dynamically according to a method that provides a {@link ScoringPosition} object + * @param arm Arm subsystem + * @param shooter Shooter subsystem + * @param posSupplier Method that provides a {@link ScoringPosition}. This method is called on initialize + */ + public SetScoringPosition(Arm arm, Shooter shooter, Supplier posSupplier) { + addRequirements(arm, shooter); + this.arm = arm; + this.shooter = shooter; + this.posSupplier = posSupplier; + } + + /** + * Create a set scoring position command that goes to a static {@link ScoringPosition} + * @param arm Arm subsystem + * @param shooter Shooter subsyste, + * @param position ScoringPosition to go to + */ + public SetScoringPosition(Arm arm, Shooter shooter, ScoringPosition position) { + this(arm, shooter, () -> position); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + var targetPosition = posSupplier.get(); + arm.setPosition(targetPosition.armAngle_deg()); + shooter.setVelocity(targetPosition.shooterSpeed_rpm()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (interrupted) { + shooter.stop(); + arm.stop(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return arm.closeEnough(); + } +} diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 7e03242..2152456 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -42,9 +42,9 @@ public final class SwerveConstants { public static final double angleRampTime_s = 0.5; public static final boolean angleEncoderInverted = false; - public static final double maxVelocity_mps = 5676 /* NEO max RPM */ + public static final double maxVelocity_mps = 4704 /* NEO max RPM */ / 60.0 / driveGearRatio * wheelCircumference_m; - public static final double maxAngularVelocity_radps = 5676 /* NEO max RPM */ + public static final double maxAngularVelocity_radps = 4704 /* NEO max RPM */ / 60.0 / angleGearRatio / Math.hypot(trackWidth_m / 2.0, wheelBase_m / 2.0); // calculated with choreo, todo: see if empirical can match these diff --git a/src/main/java/frc/robot/io/PieceVisionIO.java b/src/main/java/frc/robot/io/PieceVisionIO.java index b349323..23feac8 100644 --- a/src/main/java/frc/robot/io/PieceVisionIO.java +++ b/src/main/java/frc/robot/io/PieceVisionIO.java @@ -55,12 +55,9 @@ public static class PieceVisionIOInputs { private NetworkTableEntry clEntry = table.getEntry("cl"); private NetworkTableEntry tlEntry = table.getEntry("tl"); - private NetworkTableEntry targetspaceEntry = table.getEntry("targetspace"); + private NetworkTableEntry targetspaceEntry = table.getEntry("botpose_targetspace"); private double[] targetspaceCache = new double[6]; // array that will hold all the positions - private NetworkTableEntry botpose_wpiblueEntry = table.getEntry("botpose_wpiblue"); - private double[] botpose_wpiblueCache = new double[7]; - public void updateInputs(PieceVisionIOInputs inputs) { inputs.pipeline = pipelineEntry.getNumber(inputs.pipeline).intValue(); inputs.hasTarget = tvEntry.getDouble(0) == 1; @@ -70,7 +67,7 @@ public void updateInputs(PieceVisionIOInputs inputs) { inputs.targetArea = taEntry.getDouble(inputs.targetArea); targetspaceCache = targetspaceEntry.getDoubleArray(targetspaceCache); - if (targetspaceCache.length > 0) { + if (targetspaceCache.length > 1) { inputs.targetspaceTranslationX_m = targetspaceCache[0]; inputs.targetspaceTranslationY_m = targetspaceCache[1]; inputs.targetspaceTranslationZ_m = targetspaceCache[2]; @@ -79,19 +76,5 @@ public void updateInputs(PieceVisionIOInputs inputs) { inputs.targetspaceRotationZ_rad = Math.toRadians(targetspaceCache[5]); } else DriverStation.reportWarning("invalid targetspace array from limelight", true); - - if (botpose_wpiblueCache.length > 0) { - botpose_wpiblueCache = botpose_wpiblueEntry.getDoubleArray(botpose_wpiblueCache); - inputs.fieldspaceTranslationX_m = botpose_wpiblueCache[0]; - inputs.fieldspaceTranslationY_m = botpose_wpiblueCache[1]; - inputs.fieldspaceTranslationZ_m = botpose_wpiblueCache[2]; - inputs.fieldspaceRotationX_rad = Math.toRadians(botpose_wpiblueCache[3]); - inputs.fieldspaceRotationY_rad = Math.toRadians(botpose_wpiblueCache[4]); - inputs.fieldspaceRotationZ_rad = Math.toRadians(botpose_wpiblueCache[5]); - inputs.fieldspaceTotalLatency_s = botpose_wpiblueCache[6] / 1000; - } else { - DriverStation.reportWarning("(Piece Vision)invalid wpiblue array from limelight", true); - inputs.fieldspaceTotalLatency_s = (tlEntry.getDouble(0) - clEntry.getDouble(0)) / 1000; - } } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 44644c3..0a837c1 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -165,6 +165,6 @@ public void stop() { } public boolean closeEnough() { - return MathUtil.isNear(targetPosition_deg, inputs.shaftPosition_deg, accuracyTolerance_deg); + return MathUtil.isNear(targetPosition_deg, inputs.shaftPosition_deg, 5); } } From 70d9d51b4376632a532c500e0803461159364aca Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Wed, 13 Mar 2024 20:32:51 -0400 Subject: [PATCH 05/22] Shooter speed control is voltage control, vision target pos log, made stop shooter raise arm --- src/main/java/frc/robot/RobotContainer.java | 12 ++-- .../java/frc/robot/commands/arm/AutoAim.java | 56 ------------------- .../robot/commands/shooter/StopShooter.java | 1 - .../frc/robot/constants/SwerveConstants.java | 8 +-- .../java/frc/robot/subsystems/Shooter.java | 4 +- .../java/frc/robot/subsystems/Vision.java | 4 ++ 6 files changed, 17 insertions(+), 68 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/arm/AutoAim.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e37e598..7f13214 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.commands.intake.IntakeWithArm; import frc.robot.commands.intake.SetIntakeSpeed; import frc.robot.commands.scoring.SetScoringPosition; +import frc.robot.commands.shooter.SetShooterVelocity; import frc.robot.commands.shooter.StopShooter; import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; @@ -118,7 +119,7 @@ public RobotContainer() { .withSize(4, 1).withPosition(0, 5); driveTab.add("alerts", Alert.getAlertsSendable()) .withSize(5, 4).withPosition(4, 5).withWidget(Alert.widgetName); - driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.11:5800/stream.mjpg")) + driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.12:5800/stream.mjpg")) .withProperties(Map.of("show crosshair", false, "show controls", false)) .withWidget(BuiltInWidgets.kCameraStream) .withSize(11, 5) @@ -138,6 +139,7 @@ public RobotContainer() { // shouldn't require tuningMode as we might run it randomly in comp // needs to run when disabled so motors don't run SmartDashboard.putData("swerve angle calibration", new SwerveAngleOffsetCalibration(swerve)); + SmartDashboard.putData("Clean Motor", new SetShooterVelocity(shooter, 1500)); } /** @@ -182,7 +184,7 @@ private void configureBindings() { .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); - operatorController.x().onTrue(new StopShooter(shooter)); + operatorController.x().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(4, 0))); operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 84)); @@ -250,11 +252,11 @@ public Command getAutonomousCommand() { private Command sysIdTestSet(SysIdRoutine routine) { return Commands.sequence( routine.quasistatic(SysIdRoutine.Direction.kForward), - Commands.waitSeconds(2), + Commands.waitSeconds(3), routine.quasistatic(SysIdRoutine.Direction.kReverse), - Commands.waitSeconds(2), + Commands.waitSeconds(3), routine.dynamic(SysIdRoutine.Direction.kForward), - Commands.waitSeconds(2), + Commands.waitSeconds(3), routine.dynamic(SysIdRoutine.Direction.kReverse)); } diff --git a/src/main/java/frc/robot/commands/arm/AutoAim.java b/src/main/java/frc/robot/commands/arm/AutoAim.java deleted file mode 100644 index 09241b6..0000000 --- a/src/main/java/frc/robot/commands/arm/AutoAim.java +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.arm; - -import frc.robot.constants.VisionConstants; -import frc.robot.io.FieldVisionIOInputsAutoLogged; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Vision; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; - -import java.util.Optional; - -public class AutoAim extends Command { - private Arm arm; - private Vision vision; // needed just to set the priority tag to stare at - private FieldVisionIOInputsAutoLogged fieldvisionio; // the data we take from - Optional alliance = DriverStation.getAlliance(); - - /** Creates a new AutoAim. */ - public AutoAim(Arm arm, Vision vision) { - this.arm = arm; - this.vision = vision; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - if (alliance.get() == Alliance.Blue) { - vision.setPriorityId(VisionConstants.blueSpeakerTag); - } - if (alliance.get() == Alliance.Red) { - vision.setPriorityId(VisionConstants.redSpeakerTag); - } - } - - @Override - public void execute() { - if (vision.findDistance() > 1) - ; - // add if statements for the range now - } - - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/shooter/StopShooter.java b/src/main/java/frc/robot/commands/shooter/StopShooter.java index f084e4d..8dd3dc8 100644 --- a/src/main/java/frc/robot/commands/shooter/StopShooter.java +++ b/src/main/java/frc/robot/commands/shooter/StopShooter.java @@ -16,7 +16,6 @@ public StopShooter(Shooter shooter) { @Override public void initialize() { shooter.stop(); - } @Override diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 2152456..234c231 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .722), - new SwerveModuleConstants(23, 24, 1.84), - new SwerveModuleConstants(25, 26, 3.648), - new SwerveModuleConstants(27, 28, 2.917) + new SwerveModuleConstants(21, 22, .705), + new SwerveModuleConstants(23, 24, 1.714), + new SwerveModuleConstants(25, 26, 3.406), + new SwerveModuleConstants(27, 28, 2.893) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5cae79e..b156671 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -69,11 +69,11 @@ public void setVelocity(double velocity_rpm) { } public void setPercentOutput(double speed) { - pidController.setReference(speed, ControlType.kDutyCycle, 0, 0); + pidController.setReference(speed * 12, ControlType.kVoltage, 0, 0); } public void stop() { - pidController.setReference(0, ControlType.kDutyCycle); + pidController.setReference(0, ControlType.kVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 292db90..47c2cb6 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -57,6 +57,8 @@ public ScoringPosition estimateScoringPosition_math() { double angle = -20 + (14.9 * distance) + (-.889 * Math.pow(distance, 2)); double speed = 2515 + (990 * distance) + (-473 * Math.pow(distance, 2)) + (79.4 * Math.pow(distance, 3)); + Logger.recordOutput("Vision/targetPosition", new double[] {angle, speed}); + return new ScoringPosition(angle, speed); } @@ -93,6 +95,8 @@ public ScoringPosition estimateScoringPosition_map() { double speed = MathUtil.interpolate(lowerPosition.shooterSpeed_rpm(), upperPosition.shooterSpeed_rpm(), interpolationDistance); + Logger.recordOutput("Vision/targetPosition", new double[] {angle, speed}); + return new ScoringPosition(angle, speed); } } From 9b814d8335e6d32a8c82a26b03090256faef93b3 Mon Sep 17 00:00:00 2001 From: Brayden N Date: Thu, 14 Mar 2024 19:10:57 -0400 Subject: [PATCH 06/22] moved dpad controls from test to operator --- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f13214..2a94130 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -196,12 +196,17 @@ private void configureBindings() { operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); + operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); + operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); + operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); // *TESTING CONTROLS* // // When in tuning mode, create multiple testing options on shuffleboard as well as bind commands to a unique // 'testing' controller - if (Constants.tuningMode) { + if (Constants.tuningMode) + + { var indexSpeedEntry = Shuffleboard.getTab("Test").add("Index Speed", 0) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("min", -1, "max", 1)).getEntry(); From d6bc1cd33ac5b6994d3650f9b2cccd1af861af01 Mon Sep 17 00:00:00 2001 From: RushaliB Date: Thu, 14 Mar 2024 20:30:33 -0400 Subject: [PATCH 07/22] moved dpad pov controls to operator driver align note --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f13214..379244e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; import frc.robot.commands.swerve.XStance; +import frc.robot.commands.vision.AprilTagVision; import frc.robot.commands.vision.GamePieceVision; import edu.wpi.first.wpilibj.DriverStation; @@ -174,6 +175,7 @@ private void configureBindings() { // b -> trap/climb align maybe? driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); + driverController.b().whileTrue(new AprilTagVision(vision, swerve)); // dpad left -> call for coopertition (lights) // dpad right -> call for amplify (lights) @@ -196,7 +198,9 @@ private void configureBindings() { operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); - + operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); + operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); + operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); // *TESTING CONTROLS* // // When in tuning mode, create multiple testing options on shuffleboard as well as bind commands to a unique From 51c0a5b207d7fae643432e1d641c54c7f59c779b Mon Sep 17 00:00:00 2001 From: RushaliB Date: Thu, 14 Mar 2024 20:37:35 -0400 Subject: [PATCH 08/22] change april tag vision to right bumper --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc2191a..5ecb17e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -175,7 +175,7 @@ private void configureBindings() { // b -> trap/climb align maybe? driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); - driverController.b().whileTrue(new AprilTagVision(vision, swerve)); + driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); // dpad left -> call for coopertition (lights) // dpad right -> call for amplify (lights) From 4edabb60b05eda92d8a4c375aed9a253e0f93da2 Mon Sep 17 00:00:00 2001 From: RushaliB Date: Thu, 14 Mar 2024 22:15:41 -0400 Subject: [PATCH 09/22] April tag vision blows up in the counstructor --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ecb17e..a18ff4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,7 +30,6 @@ import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; import frc.robot.commands.swerve.XStance; -import frc.robot.commands.vision.AprilTagVision; import frc.robot.commands.vision.GamePieceVision; import edu.wpi.first.wpilibj.DriverStation; @@ -175,7 +174,6 @@ private void configureBindings() { // b -> trap/climb align maybe? driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); - driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); // dpad left -> call for coopertition (lights) // dpad right -> call for amplify (lights) From 64f72f3bed783cb8765c7168f4fd2a09f330a710 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Fri, 15 Mar 2024 16:08:41 -0400 Subject: [PATCH 10/22] pp named command changes and intake command fixes --- src/main/java/frc/robot/Robot.java | 10 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 15 ++++++++------- .../frc/robot/commands/intake/IntakeWithArm.java | 4 ++-- .../frc/robot/commands/intake/SetIntakeSpeed.java | 2 +- 4 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8481188..5a792d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -39,6 +40,8 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; + Alliance alliance; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -158,6 +161,13 @@ public void teleopInit() { } robotContainer.arm.setPosition(5); + alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; + + // for limelight shooting targeting + switch (alliance) { + case Red -> robotContainer.vision.setPriorityId(4); + case Blue -> robotContainer.vision.setPriorityId(7); + } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a18ff4b..1d50852 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.robot.commands.swerve.XStance; import frc.robot.commands.vision.GamePieceVision; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Relay.Value; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -99,13 +100,13 @@ public RobotContainer() { // named commands must be registered before any paths are created NamedCommands.registerCommand("XStance", new XStance(swerve)); - NamedCommands.registerCommand("AimAndShoot", - (new SetArmPosition(arm, 5)).andThen(Commands.print("arm moving to shooting position and shooting"))); - NamedCommands.registerCommand("Intake", Commands.print("arm to intaking position & rollers running")); - NamedCommands.registerCommand("WaitIntake", - Commands.print("wait for intake note sensor").andThen(Commands.waitSeconds(1))); - NamedCommands.registerCommand("SafeArm", Commands.print("move arm to safe position inside frame")); - + NamedCommands.registerCommand("Speaker Setting", + new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); + NamedCommands.registerCommand("WaitIntake", new IntakeWithArm(intake, arm)); + NamedCommands.registerCommand("Arm Down", new SetArmPosition(arm, 4)); + NamedCommands.registerCommand("Amp Setting", new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); + NamedCommands.registerCommand("Slow Forward (DOES NOT CANCEL, DEADLINE WITH OTHER COMMANDS)", + Commands.startEnd(() -> swerve.drive(new Translation2d(.3, 0), 0, false), () -> swerve.stop(), swerve)); // create paths autoChooser.addOption("nothing", Commands.none()); final List autoNames = AutoBuilder.getAllAutoNames(); diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index ddae6b0..d7d71f7 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -19,7 +19,7 @@ public IntakeWithArm(Intake intake, Arm arm) { // Called when the command is initially scheduled. @Override public void initialize() { - if (!intake.inputs.noteSensor) { + if (!intake.isHoldingNote()) { arm.setPosition(-1.5); intake.setMotorSpeed(IntakeConstants.intakeSpeed); } @@ -39,6 +39,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return intake.inputs.noteSensor; + return intake.isHoldingNote(); } } diff --git a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java index 547faf9..3fa9aa6 100644 --- a/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java +++ b/src/main/java/frc/robot/commands/intake/SetIntakeSpeed.java @@ -61,6 +61,6 @@ public void end(boolean interrupted) { public boolean isFinished() { // If we are intaking and we detect a note, cancel automatically. If we are indexing, the command will only be // canceled on interrupt - return (!isIndexing && intake.inputs.noteSensor); + return (!isIndexing && intake.isHoldingNote()); } } From 3a375e3e362f0f392acfe735fdf298791e1efea1 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Fri, 15 Mar 2024 21:16:28 -0400 Subject: [PATCH 11/22] friday 3/15 comp changes! --- choreo.chor | 456 +++++++++++++----- src/main/deploy/choreo/2piece.1.traj | 110 ++--- src/main/deploy/choreo/2piece.2.traj | 110 ++--- src/main/deploy/choreo/2piece.traj | 208 ++++---- src/main/deploy/choreo/exit.1.traj | 157 ++++++ src/main/deploy/choreo/exit.traj | 157 ++++++ .../pathplanner/autos/1 piece choreo.auto | 49 ++ .../{example.auto => 1 piece no motion.auto} | 24 +- .../pathplanner/autos/2piece choreo.auto | 34 +- .../pathplanner/autos/no piece choreo.auto | 25 + src/main/java/frc/robot/Robot.java | 10 + src/main/java/frc/robot/RobotContainer.java | 50 +- .../robot/commands/intake/IntakeWithArm.java | 2 +- .../robot/commands/vision/AprilTagVision.java | 56 ++- .../commands/vision/GamePieceVision.java | 4 +- .../java/frc/robot/constants/Constants.java | 2 +- .../frc/robot/constants/ShooterConstants.java | 2 +- .../frc/robot/constants/SwerveConstants.java | 8 +- .../frc/robot/constants/VisionConstants.java | 9 +- .../java/frc/robot/subsystems/Intake.java | 9 + .../java/frc/robot/subsystems/Vision.java | 3 +- 21 files changed, 1067 insertions(+), 418 deletions(-) create mode 100644 src/main/deploy/choreo/exit.1.traj create mode 100644 src/main/deploy/choreo/exit.traj create mode 100644 src/main/deploy/pathplanner/autos/1 piece choreo.auto rename src/main/deploy/pathplanner/autos/{example.auto => 1 piece no motion.auto} (52%) create mode 100644 src/main/deploy/pathplanner/autos/no piece choreo.auto diff --git a/choreo.chor b/choreo.chor index 81ce503..d296798 100644 --- a/choreo.chor +++ b/choreo.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 50, "rotationalInertia": 6, - "motorMaxTorque": 0.9060773480662982, + "motorMaxTorque": 1.087292817679558, "motorMaxVelocity": 4704, "gearing": 6.74603175, "wheelbase": 0.57785, @@ -34,17 +34,8 @@ "controlIntervalCount": 7 }, { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - }, - { - "x": 15.239320755004883, - "y": 0.5401557087898254, + "x": 1.3126474618911743, + "y": 5.567087650299072, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -54,139 +45,139 @@ ], "trajectory": [ { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": -9.427350845574357e-28, - "angularVelocity": -2.6622071589745317e-28, - "velocityX": -3.282424439471613e-20, - "velocityY": -1.4930668038031123e-26, + "x": 1.3126474618832242, + "y": 5.567087649501998, + "heading": 5.676683302944745e-13, + "angularVelocity": -3.43321425200581e-14, + "velocityX": 4.004374450026349e-9, + "velocityY": -2.9137762377339615e-10, "timestamp": 0 }, { - "x": 1.3990259521113642, - "y": 5.567087650299072, - "heading": 2.2823746146776588e-21, - "angularVelocity": 2.4245762980861946e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.913697562586356e-18, - "timestamp": 0.09413506307087183 + "x": 1.4207148237590859, + "y": 5.56710281834894, + "heading": 0.0000017880981234128607, + "angularVelocity": 0.000016068220556833382, + "velocityX": 0.9711163411604299, + "velocityY": 0.000136317653921886, + "timestamp": 0.11128158110774704 }, { - "x": 1.5717829272959243, - "y": 5.567087650299071, - "heading": 7.969574197540583e-21, - "angularVelocity": 6.041532430318509e-20, - "velocityX": 1.8352032659126796, - "velocityY": 1.7827387306366018e-17, - "timestamp": 0.18827012614174365 + "x": 1.605588153249726, + "y": 5.56713209169265, + "heading": 0.000005179600595136428, + "angularVelocity": 0.000030476758799414847, + "velocityX": 1.6613111321605605, + "velocityY": 0.00026306366761914195, + "timestamp": 0.22256316221549408 }, { - "x": 1.8309183716773985, - "y": 5.567087650299072, - "heading": 2.1798246445258308e-20, - "angularVelocity": 1.4690246561439573e-19, - "velocityX": 2.752804703454417, - "velocityY": 2.674104215464885e-17, - "timestamp": 0.2824051892126155 + "x": 1.829753087040728, + "y": 5.567173626596197, + "heading": 0.000010321331376487245, + "angularVelocity": 0.00004620468334330266, + "velocityX": 2.0143938607602228, + "velocityY": 0.0003732486572643715, + "timestamp": 0.3338447433232411 }, { - "x": 2.0900538160588726, - "y": 5.567087650299072, - "heading": 8.97766497460724e-21, - "angularVelocity": -1.3619346247394927e-19, - "velocityX": 2.7528047034544167, - "velocityY": 2.6750900782152162e-17, - "timestamp": 0.3765402522834873 + "x": 2.05465908394277, + "y": 5.567129102744167, + "heading": 0.000005603711217800806, + "angularVelocity": -0.00004239354511836126, + "velocityX": 2.0210532117804934, + "velocityY": -0.0004000936587489705, + "timestamp": 0.44512632443098815 }, { - "x": 2.262810791243433, - "y": 5.567087650299072, - "heading": 2.5676765945622405e-21, - "angularVelocity": -6.809351436893968e-20, - "velocityX": 1.8352032659126798, - "velocityY": 1.783723417840217e-17, - "timestamp": 0.47067531535435914 + "x": 2.240632817341497, + "y": 5.567101155314737, + "heading": 0.000002024117024409385, + "angularVelocity": -0.00003216700129094262, + "velocityX": 1.6711995961531687, + "velocityY": -0.0002511343930962879, + "timestamp": 0.5564079055387352 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 1.9544386221379647e-27, - "angularVelocity": -2.7276485190537467e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.923538165503086e-18, - "timestamp": 0.564810378425231 + "x": 2.3491892819318387, + "y": 5.567087651960309, + "heading": 1.4919057496986763e-12, + "angularVelocity": -0.00001818913858902976, + "velocityX": 0.9755115223704687, + "velocityY": -0.00012133685742246452, + "timestamp": 0.6676894866464822 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 9.792832338957588e-28, - "angularVelocity": -3.4440052264109466e-28, - "velocityX": -1.0343178913428024e-19, - "velocityY": 9.836392156979254e-21, - "timestamp": 0.6589454414961028 - }, - { - "x": 2.324642160074082, - "y": 5.479331567651817, - "heading": -2.6812320806456912e-18, - "angularVelocity": -2.773115697648232e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327076, - "timestamp": 0.7556320686521809 - }, - { - "x": 2.275547918749188, - "y": 5.303819407556036, - "heading": -1.522107913528158e-17, - "angularVelocity": -1.2969577360167464e-16, - "velocityX": -0.507766614359639, - "velocityY": -1.815268204696575, - "timestamp": 0.852318695808259 - }, - { - "x": 2.201906561851502, - "y": 5.04055118560791, - "heading": -2.9497325867653756e-17, - "angularVelocity": -1.4765482111895074e-16, - "velocityX": -0.7616498688987154, - "velocityY": -2.7229021188539413, - "timestamp": 0.9490053229643372 - }, - { - "x": 2.1282652049538155, - "y": 4.7772829636597836, - "heading": -1.7518587052454428e-17, - "angularVelocity": 1.238924044843347e-16, - "velocityX": -0.7616498688987153, - "velocityY": -2.722902118853941, - "timestamp": 1.0456919501204154 - }, - { - "x": 2.079170963628921, - "y": 4.601770803564003, - "heading": -6.414309314069054e-18, - "angularVelocity": 1.1484812298678339e-16, - "velocityX": -0.5077666143596389, - "velocityY": -1.815268204696575, - "timestamp": 1.1423785772764936 - }, - { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -7.445922412027734e-29, - "angularVelocity": 6.634122421282305e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327075, - "timestamp": 1.2390652044325718 - }, - { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -3.654785275835865e-29, - "angularVelocity": 1.4102041741246665e-29, - "velocityX": 1.470589714203873e-20, - "velocityY": 5.257360934767921e-20, - "timestamp": 1.33575183158865 + "x": 2.3491892814717987, + "y": 5.5670876511279825, + "heading": 1.8498540134297734e-12, + "angularVelocity": -1.8838121801786534e-12, + "velocityX": -4.062924748919285e-9, + "velocityY": -3.168063995066893e-10, + "timestamp": 0.7789710677542292 + }, + { + "x": 2.185107777385966, + "y": 5.57843523155288, + "heading": 0.013437034302228146, + "angularVelocity": 0.10315265465985932, + "velocityX": -1.259611485069916, + "velocityY": 0.08711245469129487, + "timestamp": 0.9092346498434495 + }, + { + "x": 1.9922338624025642, + "y": 5.574648054868595, + "heading": -0.003357780196661172, + "angularVelocity": -0.12892946924959994, + "velocityX": -1.4806434146069372, + "velocityY": -0.029073181128942617, + "timestamp": 1.0394982319326698 + }, + { + "x": 1.816249994866529, + "y": 5.572880218482232, + "heading": -0.012742251484704396, + "angularVelocity": -0.07204217125930834, + "velocityX": -1.3509828665358827, + "velocityY": -0.013571225283608969, + "timestamp": 1.16976181402189 + }, + { + "x": 1.6467213961957556, + "y": 5.571788224133376, + "heading": -0.007226521308021137, + "angularVelocity": 0.04234284123533412, + "velocityX": -1.3014274285409717, + "velocityY": -0.008382959871005779, + "timestamp": 1.3000253961111101 + }, + { + "x": 1.4768616943564443, + "y": 5.569594921928596, + "heading": -0.007365237227012086, + "angularVelocity": -0.0010648864278731604, + "velocityX": -1.3039692223662636, + "velocityY": -0.01683741688944608, + "timestamp": 1.4302889782003303 + }, + { + "x": 1.3126474618873145, + "y": 5.567087650234358, + "heading": -3.6218014456567208e-12, + "angularVelocity": 0.056541030913362995, + "velocityX": -1.260630406715938, + "velocityY": -0.019247679867671577, + "timestamp": 1.5605525602895505 + }, + { + "x": 1.3126474618909545, + "y": 5.56708765026724, + "heading": -1.4475075754332281e-12, + "angularVelocity": 4.040377540643928e-12, + "velocityX": 2.6238029906463054e-11, + "velocityY": 8.150524229191312e-12, + "timestamp": 1.6908161423787706 } ], "constraints": [ @@ -194,24 +185,223 @@ "scope": [ "first" ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 1 ], - "type": "StopPoint" + "type": "StopPoint", + "direction": 0 }, { "scope": [ 2 ], - "type": "WptZeroVelocity" + "type": "WptZeroVelocity", + "direction": 0 }, { "scope": [ 2 ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "exit": { + "waypoints": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -1.6516987428817835e-31, + "angularVelocity": 3.365627186718647e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.3392991160266923, + "y": 5.578340379347365, + "heading": 0.05084527674264656, + "angularVelocity": 0.9301054243469516, + "velocityX": 0.48753492295327294, + "velocityY": 0.20584457391191183, + "timestamp": 0.05466614365607662 + }, + { + "x": 1.3926138209310608, + "y": 5.602095558464793, + "heading": 0.15090427909370982, + "angularVelocity": 1.8303651155744338, + "velocityX": 0.9752783229010918, + "velocityY": 0.4345501169221013, + "timestamp": 0.10933228731215323 + }, + { + "x": 1.4727419129112613, + "y": 5.6401417619413445, + "heading": 0.29625295514716404, + "angularVelocity": 2.6588426827378284, + "velocityX": 1.4657718035556655, + "velocityY": 0.6959737953332358, + "timestamp": 0.16399843096822986 + }, + { + "x": 1.5806463473229446, + "y": 5.695327975123219, + "heading": 0.473406200223283, + "angularVelocity": 3.2406391456959285, + "velocityX": 1.973880489733221, + "velocityY": 1.0095135579540684, + "timestamp": 0.21866457462430647 + }, + { + "x": 1.712426609279705, + "y": 5.771939883643632, + "heading": 0.6149480873197845, + "angularVelocity": 2.589205633142697, + "velocityX": 2.4106376112029215, + "velocityY": 1.4014507590366152, + "timestamp": 0.2733307182803831 + }, + { + "x": 1.8646157040757754, + "y": 5.861735120575505, + "heading": 0.6843254516381283, + "angularVelocity": 1.2691102696912473, + "velocityX": 2.7839734910430862, + "velocityY": 1.6426115128369911, + "timestamp": 0.3279968619364597 + }, + { + "x": 2.0394443509304283, + "y": 5.959373498780485, + "heading": 0.6843306271163396, + "angularVelocity": 0.00009467428768829868, + "velocityX": 3.1981156006642792, + "velocityY": 1.7860849819452436, + "timestamp": 0.38266300559253635 + }, + { + "x": 2.2170155057005516, + "y": 6.0519338232599695, + "heading": 0.6843311645173596, + "angularVelocity": 0.000009830600514719364, + "velocityX": 3.2482839083598782, + "velocityY": 1.6931928665357103, + "timestamp": 0.437329149248613 + }, + { + "x": 2.379893345636442, + "y": 6.14773810700836, + "heading": 0.7173523116229228, + "angularVelocity": 0.6040511530008493, + "velocityX": 2.97950118743716, + "velocityY": 1.7525341526032563, + "timestamp": 0.49199529290468963 + }, + { + "x": 2.5210272079944938, + "y": 6.230636536851746, + "heading": 0.8166805533527952, + "angularVelocity": 1.8169974153432211, + "velocityX": 2.581741694566448, + "velocityY": 1.5164492005312948, + "timestamp": 0.5466614365607663 + }, + { + "x": 2.6419948699380145, + "y": 6.295385989559359, + "heading": 0.9758437840679963, + "angularVelocity": 2.9115503686623883, + "velocityX": 2.2128442552042693, + "velocityY": 1.1844525400396748, + "timestamp": 0.6013275802168428 + }, + { + "x": 2.744635840082322, + "y": 6.338256745744779, + "heading": 1.187997716756794, + "angularVelocity": 3.8809017519788993, + "velocityX": 1.8775966856205815, + "velocityY": 0.7842286526581116, + "timestamp": 0.6559937238729194 + }, + { + "x": 2.819689210716326, + "y": 6.364168809782246, + "heading": 1.3690565756448358, + "angularVelocity": 3.312083984323915, + "velocityX": 1.3729406468872292, + "velocityY": 0.474005706356175, + "timestamp": 0.710659867528996 + }, + { + "x": 2.867851682057744, + "y": 6.37891492245815, + "heading": 1.5017329656570861, + "angularVelocity": 2.427030354417588, + "velocityX": 0.881029246262995, + "velocityY": 0.2697485443399293, + "timestamp": 0.7653260111850726 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.2633662541172024, + "velocityX": 0.4308550693218517, + "velocityY": 0.12416620423096118, + "timestamp": 0.8199921548411492 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.5542922184430967e-29, + "velocityX": -2.3304215196416068e-31, + "velocityY": 0, + "timestamp": 0.8746582984972258 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], "type": "StopPoint" } ], diff --git a/src/main/deploy/choreo/2piece.1.traj b/src/main/deploy/choreo/2piece.1.traj index 62f4727..ca8f8fe 100644 --- a/src/main/deploy/choreo/2piece.1.traj +++ b/src/main/deploy/choreo/2piece.1.traj @@ -1,76 +1,76 @@ { "samples": [ { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": -9.427350845574357e-28, - "angularVelocity": -2.6622071589745317e-28, - "velocityX": -3.282424439471613e-20, - "velocityY": -1.4930668038031123e-26, + "x": 1.3126474618832242, + "y": 5.567087649501998, + "heading": 5.676683302944745e-13, + "angularVelocity": -3.43321425200581e-14, + "velocityX": 4.004374450026349e-9, + "velocityY": -2.9137762377339615e-10, "timestamp": 0 }, { - "x": 1.3990259521113642, - "y": 5.567087650299072, - "heading": 2.2823746146776588e-21, - "angularVelocity": 2.4245762980861946e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.913697562586356e-18, - "timestamp": 0.09413506307087183 + "x": 1.4207148237590859, + "y": 5.56710281834894, + "heading": 0.0000017880981234128607, + "angularVelocity": 0.000016068220556833382, + "velocityX": 0.9711163411604299, + "velocityY": 0.000136317653921886, + "timestamp": 0.11128158110774704 }, { - "x": 1.5717829272959243, - "y": 5.567087650299071, - "heading": 7.969574197540583e-21, - "angularVelocity": 6.041532430318509e-20, - "velocityX": 1.8352032659126796, - "velocityY": 1.7827387306366018e-17, - "timestamp": 0.18827012614174365 + "x": 1.605588153249726, + "y": 5.56713209169265, + "heading": 0.000005179600595136428, + "angularVelocity": 0.000030476758799414847, + "velocityX": 1.6613111321605605, + "velocityY": 0.00026306366761914195, + "timestamp": 0.22256316221549408 }, { - "x": 1.8309183716773985, - "y": 5.567087650299072, - "heading": 2.1798246445258308e-20, - "angularVelocity": 1.4690246561439573e-19, - "velocityX": 2.752804703454417, - "velocityY": 2.674104215464885e-17, - "timestamp": 0.2824051892126155 + "x": 1.829753087040728, + "y": 5.567173626596197, + "heading": 0.000010321331376487245, + "angularVelocity": 0.00004620468334330266, + "velocityX": 2.0143938607602228, + "velocityY": 0.0003732486572643715, + "timestamp": 0.3338447433232411 }, { - "x": 2.0900538160588726, - "y": 5.567087650299072, - "heading": 8.97766497460724e-21, - "angularVelocity": -1.3619346247394927e-19, - "velocityX": 2.7528047034544167, - "velocityY": 2.6750900782152162e-17, - "timestamp": 0.3765402522834873 + "x": 2.05465908394277, + "y": 5.567129102744167, + "heading": 0.000005603711217800806, + "angularVelocity": -0.00004239354511836126, + "velocityX": 2.0210532117804934, + "velocityY": -0.0004000936587489705, + "timestamp": 0.44512632443098815 }, { - "x": 2.262810791243433, - "y": 5.567087650299072, - "heading": 2.5676765945622405e-21, - "angularVelocity": -6.809351436893968e-20, - "velocityX": 1.8352032659126798, - "velocityY": 1.783723417840217e-17, - "timestamp": 0.47067531535435914 + "x": 2.240632817341497, + "y": 5.567101155314737, + "heading": 0.000002024117024409385, + "angularVelocity": -0.00003216700129094262, + "velocityX": 1.6711995961531687, + "velocityY": -0.0002511343930962879, + "timestamp": 0.5564079055387352 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 1.9544386221379647e-27, - "angularVelocity": -2.7276485190537467e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.923538165503086e-18, - "timestamp": 0.564810378425231 + "x": 2.3491892819318387, + "y": 5.567087651960309, + "heading": 1.4919057496986763e-12, + "angularVelocity": -0.00001818913858902976, + "velocityX": 0.9755115223704687, + "velocityY": -0.00012133685742246452, + "timestamp": 0.6676894866464822 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 9.792832338957588e-28, - "angularVelocity": -3.4440052264109466e-28, - "velocityX": -1.0343178913428024e-19, - "velocityY": 9.836392156979254e-21, - "timestamp": 0.6589454414961028 + "x": 2.3491892814717987, + "y": 5.5670876511279825, + "heading": 1.8498540134297734e-12, + "angularVelocity": -1.8838121801786534e-12, + "velocityX": -4.062924748919285e-9, + "velocityY": -3.168063995066893e-10, + "timestamp": 0.7789710677542292 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.2.traj b/src/main/deploy/choreo/2piece.2.traj index fff0bc0..e6599d4 100644 --- a/src/main/deploy/choreo/2piece.2.traj +++ b/src/main/deploy/choreo/2piece.2.traj @@ -1,76 +1,76 @@ { "samples": [ { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 9.792832338957588e-28, - "angularVelocity": -3.4440052264109466e-28, - "velocityX": -1.0343178913428024e-19, - "velocityY": 9.836392156979254e-21, + "x": 2.3491892814717987, + "y": 5.5670876511279825, + "heading": 1.8498540134297734e-12, + "angularVelocity": -1.8838121801786534e-12, + "velocityX": -4.062924748919285e-9, + "velocityY": -3.168063995066893e-10, "timestamp": 0 }, { - "x": 2.324642160074082, - "y": 5.479331567651817, - "heading": -2.6812320806456912e-18, - "angularVelocity": -2.773115697648232e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327076, - "timestamp": 0.09668662715607812 + "x": 2.185107777385966, + "y": 5.57843523155288, + "heading": 0.013437034302228146, + "angularVelocity": 0.10315265465985932, + "velocityX": -1.259611485069916, + "velocityY": 0.08711245469129487, + "timestamp": 0.1302635820892203 }, { - "x": 2.275547918749188, - "y": 5.303819407556036, - "heading": -1.522107913528158e-17, - "angularVelocity": -1.2969577360167464e-16, - "velocityX": -0.507766614359639, - "velocityY": -1.815268204696575, - "timestamp": 0.19337325431215624 + "x": 1.9922338624025642, + "y": 5.574648054868595, + "heading": -0.003357780196661172, + "angularVelocity": -0.12892946924959994, + "velocityX": -1.4806434146069372, + "velocityY": -0.029073181128942617, + "timestamp": 0.2605271641784406 }, { - "x": 2.201906561851502, - "y": 5.04055118560791, - "heading": -2.9497325867653756e-17, - "angularVelocity": -1.4765482111895074e-16, - "velocityX": -0.7616498688987154, - "velocityY": -2.7229021188539413, - "timestamp": 0.29005988146823436 + "x": 1.816249994866529, + "y": 5.572880218482232, + "heading": -0.012742251484704396, + "angularVelocity": -0.07204217125930834, + "velocityX": -1.3509828665358827, + "velocityY": -0.013571225283608969, + "timestamp": 0.39079074626766075 }, { - "x": 2.1282652049538155, - "y": 4.7772829636597836, - "heading": -1.7518587052454428e-17, - "angularVelocity": 1.238924044843347e-16, - "velocityX": -0.7616498688987153, - "velocityY": -2.722902118853941, - "timestamp": 0.3867465086243126 + "x": 1.6467213961957556, + "y": 5.571788224133376, + "heading": -0.007226521308021137, + "angularVelocity": 0.04234284123533412, + "velocityX": -1.3014274285409717, + "velocityY": -0.008382959871005779, + "timestamp": 0.5210543283568809 }, { - "x": 2.079170963628921, - "y": 4.601770803564003, - "heading": -6.414309314069054e-18, - "angularVelocity": 1.1484812298678339e-16, - "velocityX": -0.5077666143596389, - "velocityY": -1.815268204696575, - "timestamp": 0.4834331357803908 + "x": 1.4768616943564443, + "y": 5.569594921928596, + "heading": -0.007365237227012086, + "angularVelocity": -0.0010648864278731604, + "velocityX": -1.3039692223662636, + "velocityY": -0.01683741688944608, + "timestamp": 0.6513179104461011 }, { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -7.445922412027734e-29, - "angularVelocity": 6.634122421282305e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327075, - "timestamp": 0.580119762936469 + "x": 1.3126474618873145, + "y": 5.567087650234358, + "heading": -3.6218014456567208e-12, + "angularVelocity": 0.056541030913362995, + "velocityX": -1.260630406715938, + "velocityY": -0.019247679867671577, + "timestamp": 0.7815814925353213 }, { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -3.654785275835865e-29, - "angularVelocity": 1.4102041741246665e-29, - "velocityX": 1.470589714203873e-20, - "velocityY": 5.257360934767921e-20, - "timestamp": 0.6768063900925473 + "x": 1.3126474618909545, + "y": 5.56708765026724, + "heading": -1.4475075754332281e-12, + "angularVelocity": 4.040377540643928e-12, + "velocityX": 2.6238029906463054e-11, + "velocityY": 8.150524229191312e-12, + "timestamp": 0.9118450746245415 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.traj b/src/main/deploy/choreo/2piece.traj index d781900..745f113 100644 --- a/src/main/deploy/choreo/2piece.traj +++ b/src/main/deploy/choreo/2piece.traj @@ -1,139 +1,139 @@ { "samples": [ { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": -9.427350845574357e-28, - "angularVelocity": -2.6622071589745317e-28, - "velocityX": -3.282424439471613e-20, - "velocityY": -1.4930668038031123e-26, + "x": 1.3126474618832242, + "y": 5.567087649501998, + "heading": 5.676683302944745e-13, + "angularVelocity": -3.43321425200581e-14, + "velocityX": 4.004374450026349e-9, + "velocityY": -2.9137762377339615e-10, "timestamp": 0 }, { - "x": 1.3990259521113642, - "y": 5.567087650299072, - "heading": 2.2823746146776588e-21, - "angularVelocity": 2.4245762980861946e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.913697562586356e-18, - "timestamp": 0.09413506307087183 + "x": 1.4207148237590859, + "y": 5.56710281834894, + "heading": 0.0000017880981234128607, + "angularVelocity": 0.000016068220556833382, + "velocityX": 0.9711163411604299, + "velocityY": 0.000136317653921886, + "timestamp": 0.11128158110774704 }, { - "x": 1.5717829272959243, - "y": 5.567087650299071, - "heading": 7.969574197540583e-21, - "angularVelocity": 6.041532430318509e-20, - "velocityX": 1.8352032659126796, - "velocityY": 1.7827387306366018e-17, - "timestamp": 0.18827012614174365 + "x": 1.605588153249726, + "y": 5.56713209169265, + "heading": 0.000005179600595136428, + "angularVelocity": 0.000030476758799414847, + "velocityX": 1.6613111321605605, + "velocityY": 0.00026306366761914195, + "timestamp": 0.22256316221549408 }, { - "x": 1.8309183716773985, - "y": 5.567087650299072, - "heading": 2.1798246445258308e-20, - "angularVelocity": 1.4690246561439573e-19, - "velocityX": 2.752804703454417, - "velocityY": 2.674104215464885e-17, - "timestamp": 0.2824051892126155 + "x": 1.829753087040728, + "y": 5.567173626596197, + "heading": 0.000010321331376487245, + "angularVelocity": 0.00004620468334330266, + "velocityX": 2.0143938607602228, + "velocityY": 0.0003732486572643715, + "timestamp": 0.3338447433232411 }, { - "x": 2.0900538160588726, - "y": 5.567087650299072, - "heading": 8.97766497460724e-21, - "angularVelocity": -1.3619346247394927e-19, - "velocityX": 2.7528047034544167, - "velocityY": 2.6750900782152162e-17, - "timestamp": 0.3765402522834873 + "x": 2.05465908394277, + "y": 5.567129102744167, + "heading": 0.000005603711217800806, + "angularVelocity": -0.00004239354511836126, + "velocityX": 2.0210532117804934, + "velocityY": -0.0004000936587489705, + "timestamp": 0.44512632443098815 }, { - "x": 2.262810791243433, - "y": 5.567087650299072, - "heading": 2.5676765945622405e-21, - "angularVelocity": -6.809351436893968e-20, - "velocityX": 1.8352032659126798, - "velocityY": 1.783723417840217e-17, - "timestamp": 0.47067531535435914 + "x": 2.240632817341497, + "y": 5.567101155314737, + "heading": 0.000002024117024409385, + "angularVelocity": -0.00003216700129094262, + "velocityX": 1.6711995961531687, + "velocityY": -0.0002511343930962879, + "timestamp": 0.5564079055387352 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 1.9544386221379647e-27, - "angularVelocity": -2.7276485190537467e-20, - "velocityX": 0.9176016608727151, - "velocityY": 8.923538165503086e-18, - "timestamp": 0.564810378425231 + "x": 2.3491892819318387, + "y": 5.567087651960309, + "heading": 1.4919057496986763e-12, + "angularVelocity": -0.00001818913858902976, + "velocityX": 0.9755115223704687, + "velocityY": -0.00012133685742246452, + "timestamp": 0.6676894866464822 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 9.792832338957588e-28, - "angularVelocity": -3.4440052264109466e-28, - "velocityX": -1.0343178913428024e-19, - "velocityY": 9.836392156979254e-21, - "timestamp": 0.6589454414961028 + "x": 2.3491892814717987, + "y": 5.5670876511279825, + "heading": 1.8498540134297734e-12, + "angularVelocity": -1.8838121801786534e-12, + "velocityX": -4.062924748919285e-9, + "velocityY": -3.168063995066893e-10, + "timestamp": 0.7789710677542292 }, { - "x": 2.324642160074082, - "y": 5.479331567651817, - "heading": -2.6812320806456912e-18, - "angularVelocity": -2.773115697648232e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327076, - "timestamp": 0.7556320686521809 + "x": 2.185107777385966, + "y": 5.57843523155288, + "heading": 0.013437034302228146, + "angularVelocity": 0.10315265465985932, + "velocityX": -1.259611485069916, + "velocityY": 0.08711245469129487, + "timestamp": 0.9092346498434495 }, { - "x": 2.275547918749188, - "y": 5.303819407556036, - "heading": -1.522107913528158e-17, - "angularVelocity": -1.2969577360167464e-16, - "velocityX": -0.507766614359639, - "velocityY": -1.815268204696575, - "timestamp": 0.852318695808259 + "x": 1.9922338624025642, + "y": 5.574648054868595, + "heading": -0.003357780196661172, + "angularVelocity": -0.12892946924959994, + "velocityX": -1.4806434146069372, + "velocityY": -0.029073181128942617, + "timestamp": 1.0394982319326698 }, { - "x": 2.201906561851502, - "y": 5.04055118560791, - "heading": -2.9497325867653756e-17, - "angularVelocity": -1.4765482111895074e-16, - "velocityX": -0.7616498688987154, - "velocityY": -2.7229021188539413, - "timestamp": 0.9490053229643372 + "x": 1.816249994866529, + "y": 5.572880218482232, + "heading": -0.012742251484704396, + "angularVelocity": -0.07204217125930834, + "velocityX": -1.3509828665358827, + "velocityY": -0.013571225283608969, + "timestamp": 1.16976181402189 }, { - "x": 2.1282652049538155, - "y": 4.7772829636597836, - "heading": -1.7518587052454428e-17, - "angularVelocity": 1.238924044843347e-16, - "velocityX": -0.7616498688987153, - "velocityY": -2.722902118853941, - "timestamp": 1.0456919501204154 + "x": 1.6467213961957556, + "y": 5.571788224133376, + "heading": -0.007226521308021137, + "angularVelocity": 0.04234284123533412, + "velocityX": -1.3014274285409717, + "velocityY": -0.008382959871005779, + "timestamp": 1.3000253961111101 }, { - "x": 2.079170963628921, - "y": 4.601770803564003, - "heading": -6.414309314069054e-18, - "angularVelocity": 1.1484812298678339e-16, - "velocityX": -0.5077666143596389, - "velocityY": -1.815268204696575, - "timestamp": 1.1423785772764936 + "x": 1.4768616943564443, + "y": 5.569594921928596, + "heading": -0.007365237227012086, + "angularVelocity": -0.0010648864278731604, + "velocityX": -1.3039692223662636, + "velocityY": -0.01683741688944608, + "timestamp": 1.4302889782003303 }, { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -7.445922412027734e-29, - "angularVelocity": 6.634122421282305e-17, - "velocityX": -0.25388331469992637, - "velocityY": -0.9076341292327075, - "timestamp": 1.2390652044325718 + "x": 1.3126474618873145, + "y": 5.567087650234358, + "heading": -3.6218014456567208e-12, + "angularVelocity": 0.056541030913362995, + "velocityX": -1.260630406715938, + "velocityY": -0.019247679867671577, + "timestamp": 1.5605525602895505 }, { - "x": 2.05462384223938, - "y": 4.514014720916748, - "heading": -3.654785275835865e-29, - "angularVelocity": 1.4102041741246665e-29, - "velocityX": 1.470589714203873e-20, - "velocityY": 5.257360934767921e-20, - "timestamp": 1.33575183158865 + "x": 1.3126474618909545, + "y": 5.56708765026724, + "heading": -1.4475075754332281e-12, + "angularVelocity": 4.040377540643928e-12, + "velocityX": 2.6238029906463054e-11, + "velocityY": 8.150524229191312e-12, + "timestamp": 1.6908161423787706 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/exit.1.traj b/src/main/deploy/choreo/exit.1.traj new file mode 100644 index 0000000..1f03f2c --- /dev/null +++ b/src/main/deploy/choreo/exit.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -1.6516987428817835e-31, + "angularVelocity": 3.365627186718647e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.3392991160266923, + "y": 5.578340379347365, + "heading": 0.05084527674264656, + "angularVelocity": 0.9301054243469516, + "velocityX": 0.48753492295327294, + "velocityY": 0.20584457391191183, + "timestamp": 0.05466614365607662 + }, + { + "x": 1.3926138209310608, + "y": 5.602095558464793, + "heading": 0.15090427909370982, + "angularVelocity": 1.8303651155744338, + "velocityX": 0.9752783229010918, + "velocityY": 0.4345501169221013, + "timestamp": 0.10933228731215323 + }, + { + "x": 1.4727419129112613, + "y": 5.6401417619413445, + "heading": 0.29625295514716404, + "angularVelocity": 2.6588426827378284, + "velocityX": 1.4657718035556655, + "velocityY": 0.6959737953332358, + "timestamp": 0.16399843096822986 + }, + { + "x": 1.5806463473229446, + "y": 5.695327975123219, + "heading": 0.473406200223283, + "angularVelocity": 3.2406391456959285, + "velocityX": 1.973880489733221, + "velocityY": 1.0095135579540684, + "timestamp": 0.21866457462430647 + }, + { + "x": 1.712426609279705, + "y": 5.771939883643632, + "heading": 0.6149480873197845, + "angularVelocity": 2.589205633142697, + "velocityX": 2.4106376112029215, + "velocityY": 1.4014507590366152, + "timestamp": 0.2733307182803831 + }, + { + "x": 1.8646157040757754, + "y": 5.861735120575505, + "heading": 0.6843254516381283, + "angularVelocity": 1.2691102696912473, + "velocityX": 2.7839734910430862, + "velocityY": 1.6426115128369911, + "timestamp": 0.3279968619364597 + }, + { + "x": 2.0394443509304283, + "y": 5.959373498780485, + "heading": 0.6843306271163396, + "angularVelocity": 0.00009467428768829868, + "velocityX": 3.1981156006642792, + "velocityY": 1.7860849819452436, + "timestamp": 0.38266300559253635 + }, + { + "x": 2.2170155057005516, + "y": 6.0519338232599695, + "heading": 0.6843311645173596, + "angularVelocity": 0.000009830600514719364, + "velocityX": 3.2482839083598782, + "velocityY": 1.6931928665357103, + "timestamp": 0.437329149248613 + }, + { + "x": 2.379893345636442, + "y": 6.14773810700836, + "heading": 0.7173523116229228, + "angularVelocity": 0.6040511530008493, + "velocityX": 2.97950118743716, + "velocityY": 1.7525341526032563, + "timestamp": 0.49199529290468963 + }, + { + "x": 2.5210272079944938, + "y": 6.230636536851746, + "heading": 0.8166805533527952, + "angularVelocity": 1.8169974153432211, + "velocityX": 2.581741694566448, + "velocityY": 1.5164492005312948, + "timestamp": 0.5466614365607663 + }, + { + "x": 2.6419948699380145, + "y": 6.295385989559359, + "heading": 0.9758437840679963, + "angularVelocity": 2.9115503686623883, + "velocityX": 2.2128442552042693, + "velocityY": 1.1844525400396748, + "timestamp": 0.6013275802168428 + }, + { + "x": 2.744635840082322, + "y": 6.338256745744779, + "heading": 1.187997716756794, + "angularVelocity": 3.8809017519788993, + "velocityX": 1.8775966856205815, + "velocityY": 0.7842286526581116, + "timestamp": 0.6559937238729194 + }, + { + "x": 2.819689210716326, + "y": 6.364168809782246, + "heading": 1.3690565756448358, + "angularVelocity": 3.312083984323915, + "velocityX": 1.3729406468872292, + "velocityY": 0.474005706356175, + "timestamp": 0.710659867528996 + }, + { + "x": 2.867851682057744, + "y": 6.37891492245815, + "heading": 1.5017329656570861, + "angularVelocity": 2.427030354417588, + "velocityX": 0.881029246262995, + "velocityY": 0.2697485443399293, + "timestamp": 0.7653260111850726 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.2633662541172024, + "velocityX": 0.4308550693218517, + "velocityY": 0.12416620423096118, + "timestamp": 0.8199921548411492 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.5542922184430967e-29, + "velocityX": -2.3304215196416068e-31, + "velocityY": 0, + "timestamp": 0.8746582984972258 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/exit.traj b/src/main/deploy/choreo/exit.traj new file mode 100644 index 0000000..1f03f2c --- /dev/null +++ b/src/main/deploy/choreo/exit.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -1.6516987428817835e-31, + "angularVelocity": 3.365627186718647e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.3392991160266923, + "y": 5.578340379347365, + "heading": 0.05084527674264656, + "angularVelocity": 0.9301054243469516, + "velocityX": 0.48753492295327294, + "velocityY": 0.20584457391191183, + "timestamp": 0.05466614365607662 + }, + { + "x": 1.3926138209310608, + "y": 5.602095558464793, + "heading": 0.15090427909370982, + "angularVelocity": 1.8303651155744338, + "velocityX": 0.9752783229010918, + "velocityY": 0.4345501169221013, + "timestamp": 0.10933228731215323 + }, + { + "x": 1.4727419129112613, + "y": 5.6401417619413445, + "heading": 0.29625295514716404, + "angularVelocity": 2.6588426827378284, + "velocityX": 1.4657718035556655, + "velocityY": 0.6959737953332358, + "timestamp": 0.16399843096822986 + }, + { + "x": 1.5806463473229446, + "y": 5.695327975123219, + "heading": 0.473406200223283, + "angularVelocity": 3.2406391456959285, + "velocityX": 1.973880489733221, + "velocityY": 1.0095135579540684, + "timestamp": 0.21866457462430647 + }, + { + "x": 1.712426609279705, + "y": 5.771939883643632, + "heading": 0.6149480873197845, + "angularVelocity": 2.589205633142697, + "velocityX": 2.4106376112029215, + "velocityY": 1.4014507590366152, + "timestamp": 0.2733307182803831 + }, + { + "x": 1.8646157040757754, + "y": 5.861735120575505, + "heading": 0.6843254516381283, + "angularVelocity": 1.2691102696912473, + "velocityX": 2.7839734910430862, + "velocityY": 1.6426115128369911, + "timestamp": 0.3279968619364597 + }, + { + "x": 2.0394443509304283, + "y": 5.959373498780485, + "heading": 0.6843306271163396, + "angularVelocity": 0.00009467428768829868, + "velocityX": 3.1981156006642792, + "velocityY": 1.7860849819452436, + "timestamp": 0.38266300559253635 + }, + { + "x": 2.2170155057005516, + "y": 6.0519338232599695, + "heading": 0.6843311645173596, + "angularVelocity": 0.000009830600514719364, + "velocityX": 3.2482839083598782, + "velocityY": 1.6931928665357103, + "timestamp": 0.437329149248613 + }, + { + "x": 2.379893345636442, + "y": 6.14773810700836, + "heading": 0.7173523116229228, + "angularVelocity": 0.6040511530008493, + "velocityX": 2.97950118743716, + "velocityY": 1.7525341526032563, + "timestamp": 0.49199529290468963 + }, + { + "x": 2.5210272079944938, + "y": 6.230636536851746, + "heading": 0.8166805533527952, + "angularVelocity": 1.8169974153432211, + "velocityX": 2.581741694566448, + "velocityY": 1.5164492005312948, + "timestamp": 0.5466614365607663 + }, + { + "x": 2.6419948699380145, + "y": 6.295385989559359, + "heading": 0.9758437840679963, + "angularVelocity": 2.9115503686623883, + "velocityX": 2.2128442552042693, + "velocityY": 1.1844525400396748, + "timestamp": 0.6013275802168428 + }, + { + "x": 2.744635840082322, + "y": 6.338256745744779, + "heading": 1.187997716756794, + "angularVelocity": 3.8809017519788993, + "velocityX": 1.8775966856205815, + "velocityY": 0.7842286526581116, + "timestamp": 0.6559937238729194 + }, + { + "x": 2.819689210716326, + "y": 6.364168809782246, + "heading": 1.3690565756448358, + "angularVelocity": 3.312083984323915, + "velocityX": 1.3729406468872292, + "velocityY": 0.474005706356175, + "timestamp": 0.710659867528996 + }, + { + "x": 2.867851682057744, + "y": 6.37891492245815, + "heading": 1.5017329656570861, + "angularVelocity": 2.427030354417588, + "velocityX": 0.881029246262995, + "velocityY": 0.2697485443399293, + "timestamp": 0.7653260111850726 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.2633662541172024, + "velocityX": 0.4308550693218517, + "velocityY": 0.12416620423096118, + "timestamp": 0.8199921548411492 + }, + { + "x": 2.891404867172241, + "y": 6.385702610015869, + "heading": 1.5707963267948966, + "angularVelocity": 1.5542922184430967e-29, + "velocityX": -2.3304215196416068e-31, + "velocityY": 0, + "timestamp": 0.8746582984972258 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece choreo.auto b/src/main/deploy/pathplanner/autos/1 piece choreo.auto new file mode 100644 index 0000000..f1c0620 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 piece choreo.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3126474618911743, + "y": 5.567087650299072 + }, + "rotation": -9.463536699418992e-30 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "OptDelay" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "exit.1" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/example.auto b/src/main/deploy/pathplanner/autos/1 piece no motion.auto similarity index 52% rename from src/main/deploy/pathplanner/autos/example.auto rename to src/main/deploy/pathplanner/autos/1 piece no motion.auto index 4bd766a..87f35e9 100644 --- a/src/main/deploy/pathplanner/autos/example.auto +++ b/src/main/deploy/pathplanner/autos/1 piece no motion.auto @@ -2,36 +2,42 @@ "version": 1.0, "startingPose": { "position": { - "x": 2.0, - "y": 7.0 + "x": 2, + "y": 2 }, - "rotation": 180.0 + "rotation": 0 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Example Path" + "name": "OptDelay" } }, { - "type": "wait", + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", "data": { - "waitTime": 5.0 + "name": "Index" } }, { "type": "named", "data": { - "name": "XStance" + "name": "ResetScoring" } } ] } }, "folder": null, - "choreoAuto": false + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2piece choreo.auto b/src/main/deploy/pathplanner/autos/2piece choreo.auto index 4ce01ff..25f3a6d 100644 --- a/src/main/deploy/pathplanner/autos/2piece choreo.auto +++ b/src/main/deploy/pathplanner/autos/2piece choreo.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3126474618911743, - "y": 5.567087650299072 + "x": 1.3126474618832242, + "y": 5.567087649501998 }, - "rotation": -3.948572916368557e-39 + "rotation": 3.2524999489111804e-11 }, "command": { "type": "sequential", @@ -14,13 +14,25 @@ { "type": "named", "data": { - "name": "AimAndShoot" + "name": "OptDelay" } }, { "type": "named", "data": { - "name": "Intake" + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" } }, { @@ -36,15 +48,21 @@ } }, { - "type": "path", + "type": "named", + "data": { + "name": "LimelightSetting" + } + }, + { + "type": "named", "data": { - "pathName": "2piece.2" + "name": "Index" } }, { "type": "named", "data": { - "name": "AimAndShoot" + "name": "ResetScoring" } } ] diff --git a/src/main/deploy/pathplanner/autos/no piece choreo.auto b/src/main/deploy/pathplanner/autos/no piece choreo.auto new file mode 100644 index 0000000..fe91be5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/no piece choreo.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3126474618911743, + "y": 5.567087650299072 + }, + "rotation": -9.463536699418992e-30 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "exit.1" + } + } + ] + } + }, + "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 5a792d6..172b972 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.Relay.Value; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.Command; @@ -143,6 +144,14 @@ public void autonomousInit() { if (autonomousCommand != null) { autonomousCommand.schedule(); } + + alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; + + // for limelight shooting targeting + switch (alliance) { + case Red -> robotContainer.vision.setPriorityId(4); + case Blue -> robotContainer.vision.setPriorityId(7); + } } /** This function is called periodically during autonomous. */ @@ -161,6 +170,7 @@ public void teleopInit() { } robotContainer.arm.setPosition(5); + robotContainer.arm.setClimberRelay(Value.kReverse); alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; // for limelight shooting targeting diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1d50852..fddbef2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc.robot.commands.swerve.SwerveAngleOffsetCalibration; import frc.robot.commands.swerve.TeleopSwerve; import frc.robot.commands.swerve.XStance; +import frc.robot.commands.vision.AprilTagVision; import frc.robot.commands.vision.GamePieceVision; import edu.wpi.first.math.geometry.Translation2d; @@ -98,24 +99,11 @@ public RobotContainer() { configureBindings(); - // named commands must be registered before any paths are created - NamedCommands.registerCommand("XStance", new XStance(swerve)); - NamedCommands.registerCommand("Speaker Setting", - new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); - NamedCommands.registerCommand("WaitIntake", new IntakeWithArm(intake, arm)); - NamedCommands.registerCommand("Arm Down", new SetArmPosition(arm, 4)); - NamedCommands.registerCommand("Amp Setting", new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); - NamedCommands.registerCommand("Slow Forward (DOES NOT CANCEL, DEADLINE WITH OTHER COMMANDS)", - Commands.startEnd(() -> swerve.drive(new Translation2d(.3, 0), 0, false), () -> swerve.stop(), swerve)); - // create paths autoChooser.addOption("nothing", Commands.none()); - final List autoNames = AutoBuilder.getAllAutoNames(); - for (final String autoName : autoNames) { - final Command auto = new PathPlannerAuto(autoName); - autoChooser.addOption(autoName, auto); - } final var driveTab = Shuffleboard.getTab(ShuffleboardConstants.driveTab); + var delayEntry = driveTab.add("Delay Auto?", false).withWidget(BuiltInWidgets.kToggleSwitch).withSize(3, 1) + .getEntry(); driveTab.add("auto routine", autoChooser.getSendableChooser()) .withSize(4, 1).withPosition(0, 5); driveTab.add("alerts", Alert.getAlertsSendable()) @@ -126,6 +114,29 @@ public RobotContainer() { .withSize(11, 5) .withPosition(0, 0); + // named commands must be registered before any paths are created + NamedCommands.registerCommand("OptDelay", Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0)); + NamedCommands.registerCommand("XStance", new XStance(swerve)); + NamedCommands.registerCommand("SpeakerAlign", new AprilTagVision(vision, swerve).withTimeout(1)); + NamedCommands.registerCommand("SpeakerSetting", + new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); + NamedCommands.registerCommand("LimelightSetting", + new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_map)); + NamedCommands.registerCommand("WaitIntake", new IntakeWithArm(intake, arm)); + NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(1)); + NamedCommands.registerCommand("ArmDown", new SetArmPosition(arm, 4)); + NamedCommands.registerCommand("AmpSetting", new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); + NamedCommands.registerCommand("SlowForward", + Commands.startEnd(() -> swerve.drive(new Translation2d(.3, 0), 0, false), () -> swerve.stop(), swerve) + .withTimeout(1.5)); + NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, new ScoringPosition(4, 0))); + // create paths + final List autoNames = AutoBuilder.getAllAutoNames(); + for (final String autoName : autoNames) { + final Command auto = new PathPlannerAuto(autoName); + autoChooser.addOption(autoName, auto); + } + if (Constants.tuningMode) { tuningModeAlert.set(true); Logger.recordOutput("SysId/testState", SysIdRoutineLog.State.kNone.toString()); // populate default @@ -168,7 +179,7 @@ private void configureBindings() { driverController.rightTrigger(OperatorConstants.boolTriggerThreshold) .whileTrue(teleopSwerve.holdToggleFieldRelative()); // right bumper -> rotate to speaker apriltag - // driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); + driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); // left bumper -> rotate to note driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve)); driverController.a().onTrue(teleopSwerve.toggleFieldRelative()); @@ -185,15 +196,16 @@ private void configureBindings() { .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); - operatorController.x().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(4, 0))); - operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); + operatorController.x().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0))); + operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_map)); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); - operatorController.rightBumper().onTrue(new SetArmPosition(arm, 84)); + operatorController.rightBumper().onTrue(new SetArmPosition(arm, 40)); operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); + operatorController.povRight().onTrue(Commands.runOnce(() -> intake.resetNoteDetection())); operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index d7d71f7..e556e35 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -32,7 +32,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - arm.setPosition(4); + arm.setPosition(40); intake.stopMotor(); } diff --git a/src/main/java/frc/robot/commands/vision/AprilTagVision.java b/src/main/java/frc/robot/commands/vision/AprilTagVision.java index 7a4c440..9dd187b 100644 --- a/src/main/java/frc/robot/commands/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/commands/vision/AprilTagVision.java @@ -8,6 +8,8 @@ import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Vision; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -23,11 +25,14 @@ public class AprilTagVision extends Command { private Swerve swerve; private boolean isAligned; private double robotRotate_radps; - Optional alliance = DriverStation.getAlliance(); - public double rotationalOffset = Math.atan2(VisionConstants.limelightXOffset, vision.findDistance()); // inverse - // tangent - // of - // X/y + Optional alliance; + private PIDController pidController = new PIDController(VisionConstants.visionRotateKp, 0, + VisionConstants.visionRotateKd); + + // public double rotationalOffset; // inverse + // tangent + // of + // X/y public AprilTagVision(Vision vision, Swerve swerve) { addRequirements(swerve); @@ -38,11 +43,13 @@ public AprilTagVision(Vision vision, Swerve swerve) { // Called when the command is initially scheduled. @Override public void initialize() { - if (alliance.get() == Alliance.Blue) { - vision.setPriorityId(VisionConstants.blueSpeakerTag); - } - if (alliance.get() == Alliance.Red) { - vision.setPriorityId(VisionConstants.redSpeakerTag); + alliance = DriverStation.getAlliance(); + // rotationalOffset = Math.atan2(VisionConstants.limelightXOffset, vision.findDistance()); + if (alliance.isPresent()) { + switch (alliance.get()) { + case Red -> vision.setPriorityId(VisionConstants.redSpeakerTag); + case Blue -> vision.setPriorityId(VisionConstants.blueSpeakerTag); + } } robotRotate_radps = 0; isAligned = false; @@ -59,19 +66,26 @@ public void execute() { } Logger.recordOutput("Fieldvision/isAligned", isAligned); - if (vision.fieldInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) { - robotRotate_radps = VisionConstants.AprilTagRotateKp - * vision.fieldInputs.crosshairToTargetErrorX_rad // why do we use Proportional limit * error - derivative - - VisionConstants.AprilTagRotateKd; - } else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.fieldInputs.crosshairToTargetErrorX_rad) { - robotRotate_radps = VisionConstants.AprilTagRotateKp - * vision.fieldInputs.crosshairToTargetErrorX_rad - + VisionConstants.AprilTagRotateKd; - } - robotRotate_radps *= -1; // Rotate opposite of error + // if (vision.fieldInputs.crosshairToTargetErrorX_rad < -VisionConstants.crosshairGamePieceBoundRotateX_rad) { + // robotRotate_radps = VisionConstants.AprilTagRotateKp + // * vision.fieldInputs.crosshairToTargetErrorX_rad // why do we use Proportional limit * error - derivative + // - VisionConstants.AprilTagRotateKd; + // } else if (VisionConstants.crosshairGamePieceBoundRotateX_rad < vision.fieldInputs.crosshairToTargetErrorX_rad) { + // robotRotate_radps = VisionConstants.AprilTagRotateKp + // * vision.fieldInputs.crosshairToTargetErrorX_rad + // + VisionConstants.AprilTagRotateKd; + // } + // robotRotate_radps *= -1; // Rotate opposite of error + + if (!MathUtil.isNear(0, vision.fieldInputs.crosshairToTargetErrorX_rad, + VisionConstants.crosshairGamePieceBoundRotateX_rad)) { + robotRotate_radps = pidController.calculate(vision.fieldInputs.crosshairToTargetErrorX_rad, 0); + } else + robotRotate_radps = 0; System.out.println("Swerve --> Shooting Speaker"); - swerve.drive(new Translation2d(0, 0), robotRotate_radps - rotationalOffset, false); + Logger.recordOutput("Vision/targetRotation", robotRotate_radps); + swerve.drive(new Translation2d(0, 0), robotRotate_radps, false); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/vision/GamePieceVision.java b/src/main/java/frc/robot/commands/vision/GamePieceVision.java index 44a2b7d..17cef89 100644 --- a/src/main/java/frc/robot/commands/vision/GamePieceVision.java +++ b/src/main/java/frc/robot/commands/vision/GamePieceVision.java @@ -20,8 +20,8 @@ public class GamePieceVision extends Command { private Swerve swerve; private double robotRotate_radps; private boolean isAligned; - private PIDController pidController = new PIDController(VisionConstants.gamePieceRotateKp, 0, - VisionConstants.gamePieceRotateKd); + private PIDController pidController = new PIDController(VisionConstants.visionRotateKp, 0, + VisionConstants.visionRotateKd); /** Creates a new GamePieceVision. */ public GamePieceVision(Vision vision, Swerve swerve) { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 9f3937d..e3faca4 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -10,7 +10,7 @@ public enum RobotMode { } /** publishes many constants to NT for hot editing & other non-competition functionality */ - public static final boolean tuningMode = true; + public static final boolean tuningMode = false; public static class OperatorConstants { public static final int driverControllerPort = 0; diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 749df28..a13cc24 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -8,7 +8,7 @@ public final class ShooterConstants { public static final double kD = .0000025; public static final double kS = 0.026257; - public static final double kV = .00224; + public static final double kV = .002092; public static final double kA = .008631; public static final double maxVelocity_rpm = 5000; diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 234c231..6ee240b 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -48,10 +48,10 @@ public final class SwerveConstants { / 60.0 / angleGearRatio / Math.hypot(trackWidth_m / 2.0, wheelBase_m / 2.0); // calculated with choreo, todo: see if empirical can match these - public static final double theoreticalMaxVelocity_mps = 4.420; - public static final double theoreticalMaxAcceleration_mps2 = 27.971; - public static final double theoreticalMaxAngularVelocity_radps = 10.817; - public static final double theoreticalMaxAngularAcceleration_radps2 = 95.242; + public static final double theoreticalMaxVelocity_mps = 3.663; + public static final double theoreticalMaxAcceleration_mps2 = 11.697; + public static final double theoreticalMaxAngularVelocity_radps = 8.865; + public static final double theoreticalMaxAngularAcceleration_radps2 = 39.829; /* drive motor PID values */ public static final double driveKp = 0.15138; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index ba22c6f..85568e0 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -1,15 +1,16 @@ package frc.robot.constants; public class VisionConstants { + /* AprilTagVision */ public static final double poseError_m = 1; // comparing visionPose to pose public static final double botpose_fieldOffsetX_m = 0.025; // realife offset to pathplanner app /* ObjectDetectionVision */ - public static final double gamePieceRotateKp = 1.75; - public static final double gamePieceRotateKd = 1.4; + public static final double visionRotateKp = 1.75; + public static final double visionRotateKd = 1.4; - public static final double AprilTagRotateKp = 1.2; - public static final double AprilTagRotateKd = 0.34; + // public static final double AprilTagRotateKp = 1.2; + // public static final double AprilTagRotateKd = 0.34; public static final double crosshairGamePieceBoundRotateX_rad = Math.toRadians(1.5); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1081caf..9b53502 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkBase.IdleMode; @@ -35,6 +37,8 @@ public Intake() { motor.enableVoltageCompensation(12); motor.burnFlash(); + Shuffleboard.getTab("drive").addBoolean("Holding Note?", () -> holdingNote).withWidget(BuiltInWidgets.kBooleanBox); + } @Override @@ -54,6 +58,11 @@ public void setMotorSpeed(double speed) { motor.setVoltage(speed * 12); } + public void resetNoteDetection() { + holdingNote = false; + cachedNoteSensor = false; + } + @AutoLog public static class IntakeIOInputs { public double current_A = 0.0; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 47c2cb6..bf2ccd5 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -57,7 +57,8 @@ public ScoringPosition estimateScoringPosition_math() { double angle = -20 + (14.9 * distance) + (-.889 * Math.pow(distance, 2)); double speed = 2515 + (990 * distance) + (-473 * Math.pow(distance, 2)) + (79.4 * Math.pow(distance, 3)); - Logger.recordOutput("Vision/targetPosition", new double[] {angle, speed}); + Logger.recordOutput("Vision/targetPosition_deg", angle); + Logger.recordOutput("Vision/targetPosition_rpm", speed); return new ScoringPosition(angle, speed); } From 460530a46e1aa959ff1ccd02d28231f1082ce9eb Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sat, 16 Mar 2024 10:00:22 -0400 Subject: [PATCH 12/22] Changes pre-quals 3/16 --- choreo.chor | 1533 ++++++++++++++--- src/main/deploy/choreo/2 piece left.1.traj | 130 ++ src/main/deploy/choreo/2 piece left.2.traj | 130 ++ src/main/deploy/choreo/2 piece left.3.traj | 157 ++ src/main/deploy/choreo/2 piece left.traj | 391 +++++ src/main/deploy/choreo/2 piece right.1.traj | 130 ++ src/main/deploy/choreo/2 piece right.2.traj | 130 ++ src/main/deploy/choreo/2 piece right.3.traj | 166 ++ src/main/deploy/choreo/2 piece right.traj | 400 +++++ src/main/deploy/choreo/2piece.1.traj | 105 +- src/main/deploy/choreo/2piece.2.traj | 105 +- src/main/deploy/choreo/2piece.traj | 198 +-- src/main/deploy/choreo/exit center.1.traj | 157 ++ src/main/deploy/choreo/exit center.traj | 157 ++ src/main/deploy/choreo/exit.1.traj | 157 -- src/main/deploy/choreo/exit.traj | 157 -- ... piece choreo.auto => 1 piece center.auto} | 6 +- .../autos/{2piece.auto => 1 piece left.auto} | 26 +- .../pathplanner/autos/1 piece no motion.auto | 2 +- .../pathplanner/autos/1 piece right.auto | 49 + ...2piece choreo.auto => 2 piece center.auto} | 2 +- .../pathplanner/autos/2 piece left.auto | 85 + .../pathplanner/autos/2 piece right.auto | 85 + ...piece choreo.auto => no piece center.auto} | 10 +- src/main/java/frc/robot/RobotContainer.java | 22 +- .../frc/robot/constants/SwerveConstants.java | 12 +- src/main/java/frc/robot/io/PieceVisionIO.java | 3 - .../java/frc/robot/subsystems/Intake.java | 18 +- 28 files changed, 3662 insertions(+), 861 deletions(-) create mode 100644 src/main/deploy/choreo/2 piece left.1.traj create mode 100644 src/main/deploy/choreo/2 piece left.2.traj create mode 100644 src/main/deploy/choreo/2 piece left.3.traj create mode 100644 src/main/deploy/choreo/2 piece left.traj create mode 100644 src/main/deploy/choreo/2 piece right.1.traj create mode 100644 src/main/deploy/choreo/2 piece right.2.traj create mode 100644 src/main/deploy/choreo/2 piece right.3.traj create mode 100644 src/main/deploy/choreo/2 piece right.traj create mode 100644 src/main/deploy/choreo/exit center.1.traj create mode 100644 src/main/deploy/choreo/exit center.traj delete mode 100644 src/main/deploy/choreo/exit.1.traj delete mode 100644 src/main/deploy/choreo/exit.traj rename src/main/deploy/pathplanner/autos/{1 piece choreo.auto => 1 piece center.auto} (87%) rename src/main/deploy/pathplanner/autos/{2piece.auto => 1 piece left.auto} (62%) create mode 100644 src/main/deploy/pathplanner/autos/1 piece right.auto rename src/main/deploy/pathplanner/autos/{2piece choreo.auto => 2 piece center.auto} (97%) create mode 100644 src/main/deploy/pathplanner/autos/2 piece left.auto create mode 100644 src/main/deploy/pathplanner/autos/2 piece right.auto rename src/main/deploy/pathplanner/autos/{no piece choreo.auto => no piece center.auto} (64%) diff --git a/choreo.chor b/choreo.chor index d296798..c6a1d2a 100644 --- a/choreo.chor +++ b/choreo.chor @@ -22,7 +22,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { "x": 2.349189281463623, @@ -31,7 +31,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { "x": 1.3126474618911743, @@ -45,139 +45,121 @@ ], "trajectory": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": 2.630502167570374e-32, + "angularVelocity": 6.912068173828048e-33, + "velocityX": -4.041414755979078e-16, + "velocityY": 2.1900742120129258e-39, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 - }, - { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 - }, - { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 - }, - { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 - }, - { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 - }, - { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 - }, - { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, - "timestamp": 0.7789710677542292 - }, - { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.9092346498434495 - }, - { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 1.0394982319326698 - }, - { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 1.16976181402189 - }, - { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 1.3000253961111101 - }, - { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 1.4302889782003303 - }, - { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 1.5605525602895505 - }, - { - "x": 1.3126474618909545, - "y": 5.56708765026724, - "heading": -1.4475075754332281e-12, - "angularVelocity": 4.040377540643928e-12, - "velocityX": 2.6238029906463054e-11, - "velocityY": 8.150524229191312e-12, - "timestamp": 1.6908161423787706 + "x": 1.4761905162579165, + "y": 5.567087650299072, + "heading": 5.081856100994774e-22, + "angularVelocity": 4.2976983347588495e-21, + "velocityX": 1.383114259492122, + "velocityY": 7.266238948856172e-27, + "timestamp": 0.11824262021257878 + }, + { + "x": 1.7126757532986059, + "y": 5.567087650299072, + "heading": 5.225371158203822e-19, + "angularVelocity": 4.415167063895265e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.427644052438308e-27, + "timestamp": 0.23648524042515756 + }, + { + "x": 1.9491609900561957, + "y": 5.567087650299072, + "heading": 2.6577036541275914e-19, + "angularVelocity": -2.1718062327712947e-18, + "velocityX": 1.9999999681274145, + "velocityY": 3.4263757854502784e-27, + "timestamp": 0.3547278606377363 + }, + { + "x": 2.1856462270968846, + "y": 5.567087650299072, + "heading": -1.0906583480085762e-21, + "angularVelocity": -2.2568820046674306e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.421212896814989e-27, + "timestamp": 0.4729704808503151 + }, + { + "x": 2.349189281463629, + "y": 5.567087650299072, + "heading": -3.2298015655990765e-32, + "angularVelocity": 9.223475207094912e-21, + "velocityX": 1.383114259492122, + "velocityY": -1.754147302356677e-26, + "timestamp": 0.5912131010628939 + }, + { + "x": 2.349189281463627, + "y": 5.567087650299072, + "heading": -3.648621613916173e-33, + "angularVelocity": 1.9825798352488026e-32, + "velocityX": -3.5425947355709975e-16, + "velocityY": -9.477719520863627e-38, + "timestamp": 0.7094557212754727 + }, + { + "x": 2.185646227096883, + "y": 5.567087650299072, + "heading": 3.738718946616064e-21, + "angularVelocity": 3.1617850562356095e-20, + "velocityX": -1.3831142594921222, + "velocityY": 5.863050032870948e-30, + "timestamp": 0.8276983414880514 + }, + { + "x": 1.9491609900561935, + "y": 5.567087650299072, + "heading": -1.3594265469642462e-18, + "angularVelocity": -1.1527879544319905e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.470823667011192e-30, + "timestamp": 0.9459409617006301 + }, + { + "x": 1.7126757532986037, + "y": 5.567087650299072, + "heading": -1.1949667812351735e-18, + "angularVelocity": 1.3904520102372167e-18, + "velocityX": -1.9999999681274145, + "velocityY": 8.47082157931736e-30, + "timestamp": 1.0641835819132088 + }, + { + "x": 1.4761905162579143, + "y": 5.567087650299072, + "heading": -5.013730755356182e-22, + "angularVelocity": 1.0101569711037546e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.47082075819828e-30, + "timestamp": 1.1824262021257876 + }, + { + "x": 1.3126474618911703, + "y": 5.567087650299072, + "heading": -4.6245267198088095e-32, + "angularVelocity": 4.2399724814725485e-21, + "velocityX": -1.3831142594921215, + "velocityY": 5.85686190815934e-30, + "timestamp": 1.3006688223383664 + }, + { + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": -2.2656479665287125e-32, + "angularVelocity": 7.883926885959527e-33, + "velocityX": 3.7971136148997106e-16, + "velocityY": -9.967559307460824e-45, + "timestamp": 1.4189114425509453 } ], "constraints": [ @@ -185,29 +167,33 @@ "scope": [ "first" ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 1 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" }, { "scope": [ 2 ], - "type": "WptZeroVelocity", - "direction": 0 + "type": "WptZeroVelocity" }, { "scope": [ 2 ], - "type": "StopPoint", - "direction": 0 + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 } ], "usesControlIntervalGuessing": true, @@ -215,7 +201,982 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "exit": { + "2 piece left": { + "waypoints": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 2.0283839438048332e-19, + "velocityX": 8.094691061889947e-19, + "velocityY": 1.7638426740730563e-19, + "timestamp": 0 + }, + { + "x": 0.8322802456836562, + "y": 6.653172699607564, + "heading": 1.0074798311324673, + "angularVelocity": -3.7400126935671097e-7, + "velocityX": 0.8646152527784429, + "velocityY": 0.18633427357388527, + "timestamp": 0.07561311115664361 + }, + { + "x": 0.9630327340876748, + "y": 6.681351325717821, + "heading": 1.007479762583371, + "angularVelocity": -9.065768535902331e-7, + "velocityX": 1.7292303729328837, + "velocityY": 0.3726685184515223, + "timestamp": 0.15122622231328722 + }, + { + "x": 1.1108648715080907, + "y": 6.713210807608451, + "heading": 0.935326621538057, + "angularVelocity": -0.9542411354538504, + "velocityX": 1.955112481936631, + "velocityY": 0.4213486444781865, + "timestamp": 0.22683933346993085 + }, + { + "x": 1.258697008933641, + "y": 6.745070289475266, + "heading": 0.8127259503436992, + "angularVelocity": -1.6214208001621926, + "velocityX": 1.9551124820045325, + "velocityY": 0.421348644163222, + "timestamp": 0.30245244462657445 + }, + { + "x": 1.406529146351303, + "y": 6.776929771378683, + "heading": 0.6640255812369789, + "angularVelocity": -1.966595036655294, + "velocityX": 1.9551124819002106, + "velocityY": 0.4213486446472832, + "timestamp": 0.37806555578321804 + }, + { + "x": 1.5543612837652037, + "y": 6.808789253299551, + "heading": 0.5044825425717008, + "angularVelocity": -2.1099917226625076, + "velocityX": 1.9551124818504668, + "velocityY": 0.42134864487809753, + "timestamp": 0.45367866693986164 + }, + { + "x": 1.702193421185673, + "y": 6.840648735189942, + "heading": 0.344692011554351, + "angularVelocity": -2.1132648633690048, + "velocityX": 1.9551124819373364, + "velocityY": 0.42134864447501086, + "timestamp": 0.5292917780965053 + }, + { + "x": 1.8500255586150136, + "y": 6.872508217039167, + "heading": 0.19547183240040739, + "angularVelocity": -1.9734696386822674, + "velocityX": 1.9551124820546628, + "velocityY": 0.42134864393060517, + "timestamp": 0.6049048892531489 + }, + { + "x": 1.9978576960460066, + "y": 6.904367698880726, + "heading": 0.07244487018227615, + "angularVelocity": -1.6270585925668755, + "velocityX": 1.9551124820765156, + "velocityY": 0.42134864382921067, + "timestamp": 0.6805180004097925 + }, + { + "x": 2.1456898334992545, + "y": 6.936227180619012, + "heading": 9.65887879992878e-8, + "angularVelocity": -0.9580980399471752, + "velocityX": 1.955112482370841, + "velocityY": 0.4213486424633956, + "timestamp": 0.7561311115664361 + }, + { + "x": 2.2764423218817926, + "y": 6.964405806828939, + "heading": 2.820501204963407e-8, + "angularVelocity": -9.043904534147509e-7, + "velocityX": 1.7292303726488056, + "velocityY": 0.37266851976968174, + "timestamp": 0.8317442227230797 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.9396360429134164e-18, + "angularVelocity": -3.7301747793182745e-7, + "velocityX": 0.8646152526837502, + "velocityY": 0.18633427401327182, + "timestamp": 0.9073573338797233 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.2932527092188034e-18, + "angularVelocity": 4.19600892926216e-20, + "velocityX": -8.058124746715222e-20, + "velocityY": -1.860414915840738e-20, + "timestamp": 0.9829704450363669 + }, + { + "x": 2.2764423218798027, + "y": 6.964405806838171, + "heading": 2.828141101686341e-8, + "angularVelocity": 3.7402787103507053e-7, + "velocityX": -0.8646152527100619, + "velocityY": -0.18633427389118332, + "timestamp": 1.0585835561930106 + }, + { + "x": 2.145689833491728, + "y": 6.936227180653932, + "heading": 9.68470451708674e-8, + "angularVelocity": 9.067955680869744e-7, + "velocityX": -1.7292303727220222, + "velocityY": -0.3726685194299493, + "timestamp": 1.1341966673496542 + }, + { + "x": 1.9978576960446424, + "y": 6.904367698887054, + "heading": 0.07260812222938247, + "angularVelocity": 0.960257080705476, + "velocityX": -1.9551124822893438, + "velocityY": -0.42134864284155416, + "timestamp": 1.2098097785062978 + }, + { + "x": 1.8500255586147867, + "y": 6.872508217040216, + "heading": 0.1958338387782673, + "angularVelocity": 1.6296871622383675, + "velocityX": -1.955112482061473, + "velocityY": -0.42134864389901044, + "timestamp": 1.2854228896629414 + }, + { + "x": 1.7021934211910146, + "y": 6.840648735165152, + "heading": 0.3451117743398479, + "angularVelocity": 1.9742334798568144, + "velocityX": -1.95511248198102, + "velocityY": -0.42134864427231705, + "timestamp": 1.361036000819585 + }, + { + "x": 1.5543612837761693, + "y": 6.8087892532486665, + "heading": 0.5047130791830543, + "angularVelocity": 2.110762305661104, + "velocityX": -1.9551124818629584, + "velocityY": -0.42134864482013473, + "timestamp": 1.4366491119762286 + }, + { + "x": 1.4065291463620155, + "y": 6.776929771328972, + "heading": 0.6639739239039864, + "angularVelocity": 2.106259645777567, + "velocityX": -1.9551124818538146, + "velocityY": -0.4213486448625627, + "timestamp": 1.5122622231328722 + }, + { + "x": 1.2586970089400131, + "y": 6.745070289445696, + "heading": 0.8125476250392986, + "angularVelocity": 1.9649198249166078, + "velocityX": -1.9551124819576147, + "velocityY": -0.4213486443809204, + "timestamp": 1.5878753342895158 + }, + { + "x": 1.1108648715115719, + "y": 6.713210807592297, + "heading": 0.9352215452290742, + "angularVelocity": 1.622389534212375, + "velocityX": -1.9551124820427679, + "velocityY": -0.42134864398580435, + "timestamp": 1.6634884454461594 + }, + { + "x": 0.9630327340807132, + "y": 6.681351325750123, + "heading": 1.0074797622855187, + "angularVelocity": 0.9556307887761831, + "velocityX": -1.9551124820747394, + "velocityY": -0.42134864383734444, + "timestamp": 1.739101556602803 + }, + { + "x": 0.8322802456820239, + "y": 6.653172699615138, + "heading": 1.0074798310413775, + "angularVelocity": 9.093113311976794e-7, + "velocityX": -1.7292303728624039, + "velocityY": -0.3726685187785591, + "timestamp": 1.8147146677594466 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 3.75205950615561e-7, + "velocityX": -0.8646152527568558, + "velocityY": -0.18633427367405322, + "timestamp": 1.8903277789160902 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": -2.242417201812226e-19, + "velocityX": -4.158054673626952e-19, + "velocityY": -9.844851544165139e-20, + "timestamp": 1.9659408900727338 + }, + { + "x": 0.867342073873612, + "y": 6.677517805054488, + "heading": 1.0074798401596599, + "angularVelocity": -2.0078718725992087e-7, + "velocityX": 1.0474996006171284, + "velocityY": 0.4008443829976782, + "timestamp": 2.061824532746057 + }, + { + "x": 1.0464438315164646, + "y": 6.746054283251552, + "heading": 0.9900885492460468, + "angularVelocity": -0.18137912190993252, + "velocityX": 1.8679073160899227, + "velocityY": 0.7147880106158334, + "timestamp": 2.1577081754193803 + }, + { + "x": 1.2255455891596982, + "y": 6.814590761447637, + "heading": 0.9294452786879966, + "angularVelocity": -0.6324673204653116, + "velocityX": 1.8679073160938997, + "velocityY": 0.7147880106056311, + "timestamp": 2.2535918180927035 + }, + { + "x": 1.4046473468026037, + "y": 6.883127239644582, + "heading": 0.8469789832587827, + "angularVelocity": -0.8600663588697559, + "velocityX": 1.8679073160904744, + "velocityY": 0.7147880106145814, + "timestamp": 2.3494754607660266 + }, + { + "x": 1.5837491044451752, + "y": 6.951663717842399, + "heading": 0.7537506929542891, + "angularVelocity": -0.9723065134490515, + "velocityX": 1.867907316086992, + "velocityY": 0.7147880106236818, + "timestamp": 2.44535910343935 + }, + { + "x": 1.762850862087631, + "y": 7.020200196040518, + "heading": 0.6553282675051919, + "angularVelocity": -1.0264777464121126, + "velocityX": 1.867907316085786, + "velocityY": 0.7147880106268334, + "timestamp": 2.541242746112673 + }, + { + "x": 1.941952619730239, + "y": 7.088736674238239, + "heading": 0.5545759098990335, + "angularVelocity": -1.0507773254862967, + "velocityX": 1.8679073160873736, + "velocityY": 0.7147880106226845, + "timestamp": 2.637126388785996 + }, + { + "x": 2.121054377373199, + "y": 7.157273152435041, + "heading": 0.453150200963735, + "angularVelocity": -1.0577999135979546, + "velocityX": 1.8679073160910444, + "velocityY": 0.7147880106130912, + "timestamp": 2.7330100314593193 + }, + { + "x": 2.3001561350165884, + "y": 7.225809630630721, + "heading": 0.3523691136343556, + "angularVelocity": -1.0510769566060587, + "velocityX": 1.8679073160955193, + "velocityY": 0.7147880106013975, + "timestamp": 2.8288936741326425 + }, + { + "x": 2.479257892660336, + "y": 7.294346108825463, + "heading": 0.2538978395296278, + "angularVelocity": -1.026987204065876, + "velocityX": 1.867907316099261, + "velocityY": 0.7147880105916198, + "timestamp": 2.9247773168059656 + }, + { + "x": 2.658359650304228, + "y": 7.362882587019828, + "heading": 0.16061512982409304, + "angularVelocity": -0.9728740701200732, + "velocityX": 1.8679073161007647, + "velocityY": 0.7147880105876907, + "timestamp": 3.020660959479289 + }, + { + "x": 2.8374614079479796, + "y": 7.431419065214562, + "heading": 0.07810324033722603, + "angularVelocity": -0.8605418733202074, + "velocityX": 1.867907316099298, + "velocityY": 0.7147880105915232, + "timestamp": 3.116544602152612 + }, + { + "x": 3.0165631655914407, + "y": 7.499955543410054, + "heading": 0.01742736740360304, + "angularVelocity": -0.6328073406675466, + "velocityX": 1.8679073160962676, + "velocityY": 0.714788010599443, + "timestamp": 3.212428244825935 + }, + { + "x": 3.195664923235554, + "y": 7.568492021603823, + "heading": 1.924276877277615e-8, + "angularVelocity": -0.18175517403118957, + "velocityX": 1.867907316103074, + "velocityY": 0.7147880105814662, + "timestamp": 3.3083118874992583 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -1.4869843514051715e-19, + "angularVelocity": -2.006887540420477e-7, + "velocityX": 1.0474996006097614, + "velocityY": 0.4008443830169303, + "timestamp": 3.4041955301725815 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -7.359523754309027e-20, + "angularVelocity": 1.5727023698046957e-20, + "velocityX": -4.856900753028092e-20, + "velocityY": -1.8631537844939477e-20, + "timestamp": 3.5000791728459046 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "2 piece right": { + "waypoints": [ + { + "x": 0.7823778390884399, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 0.7823778390884399, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7823778390884399, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 3.4836656833317357e-19, + "velocityX": 5.792784763551635e-18, + "velocityY": -1.1468055635760044e-18, + "timestamp": 0 + }, + { + "x": 0.848441149203722, + "y": 4.472500429335688, + "heading": -0.9928952478971514, + "angularVelocity": 5.064569480290326e-7, + "velocityX": 0.8673046782767408, + "velocityY": -0.20407170438519942, + "timestamp": 0.0761708218230117 + }, + { + "x": 0.9805677659026901, + "y": 4.4414118103682325, + "heading": -0.9928951486952988, + "angularVelocity": 0.0000013023602772620627, + "velocityX": 1.7346093101893227, + "velocityY": -0.40814341008019434, + "timestamp": 0.1523416436460234 + }, + { + "x": 1.1288597388405326, + "y": 4.406519593265519, + "heading": -0.9209894371787697, + "angularVelocity": 0.9440059828106777, + "velocityX": 1.9468343571559381, + "velocityY": -0.4580785170440767, + "timestamp": 0.2285124654690351 + }, + { + "x": 1.2771517113083277, + "y": 4.371627374165104, + "heading": -0.8000545061710758, + "angularVelocity": 1.587680533219069, + "velocityX": 1.9468343509849748, + "velocityY": -0.4580785432706628, + "timestamp": 0.3046832872920468 + }, + { + "x": 1.4254436833762532, + "y": 4.336735153365243, + "heading": -0.6538028446856889, + "angularVelocity": 1.9200483595308036, + "velocityX": 1.9468343457353292, + "velocityY": -0.45807856558166105, + "timestamp": 0.3808541091150585 + }, + { + "x": 1.57373565515521, + "y": 4.301842931337266, + "heading": -0.4970386957469823, + "angularVelocity": 2.058060359424233, + "velocityX": 1.9468343419416414, + "velocityY": -0.458078581704838, + "timestamp": 0.45702493093807023 + }, + { + "x": 1.7220276265458214, + "y": 4.26695070765882, + "heading": -0.3400744666794648, + "angularVelocity": 2.060687088715348, + "velocityX": 1.946834336843289, + "velocityY": -0.4580786033728385, + "timestamp": 0.5331957527610819 + }, + { + "x": 1.8703195977756357, + "y": 4.232058483296987, + "heading": -0.19340046314195958, + "angularVelocity": 1.9255930292876797, + "velocityX": 1.9468343347322845, + "velocityY": -0.45807861234460967, + "timestamp": 0.6093665745840936 + }, + { + "x": 2.0186115688161, + "y": 4.197166258130416, + "heading": -0.0721222371749164, + "angularVelocity": 1.592187442178866, + "velocityX": 1.946834332246427, + "velocityY": -0.45807862290950535, + "timestamp": 0.6855373964071053 + }, + { + "x": 2.1669035397570235, + "y": 4.162274032540797, + "heading": -1.3790420971273228e-7, + "angularVelocity": 0.946846804912877, + "velocityX": 1.9468343309396154, + "velocityY": -0.4580786284634526, + "timestamp": 0.761708218230117 + }, + { + "x": 2.299030158583636, + "y": 4.131185422616258, + "heading": -3.861728516505335e-8, + "angularVelocity": 0.0000013034771345390653, + "velocityX": 1.7346093381218648, + "velocityY": -0.4081432913612903, + "timestamp": 0.8378790400531287 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -2.2763209665573703e-18, + "angularVelocity": 5.069826509442209e-7, + "velocityX": 0.8673046903657866, + "velocityY": -0.2040716530068378, + "timestamp": 0.9140498618761405 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -1.5143047799084299e-18, + "angularVelocity": 5.84054737228804e-20, + "velocityX": -8.219525464434245e-19, + "velocityY": 2.1312698857170775e-19, + "timestamp": 0.9902206836991522 + }, + { + "x": 2.299030158572247, + "y": 4.131185422567857, + "heading": -3.8824131952933046e-8, + "angularVelocity": -5.096982155619529e-7, + "velocityX": -0.867304690515307, + "velocityY": 0.20407165237139407, + "timestamp": 1.0663915055221642 + }, + { + "x": 2.166903539754318, + "y": 4.162274032529242, + "heading": -1.3861601297880188e-7, + "angularVelocity": -0.000001310106398423391, + "velocityX": -1.7346093380078471, + "velocityY": 0.408143291845037, + "timestamp": 1.1425623273451764 + }, + { + "x": 2.018611568604941, + "y": 4.197166257232936, + "heading": -0.0722676107720337, + "angularVelocity": -0.948755316359051, + "velocityX": -1.9468343336762697, + "velocityY": 0.4580786168326712, + "timestamp": 1.2187331491681885 + }, + { + "x": 1.8703195973960691, + "y": 4.2320584816837785, + "heading": -0.19373209136885944, + "angularVelocity": -1.5946326649731632, + "velocityX": -1.946834334457335, + "velocityY": 0.458078613513146, + "timestamp": 1.2949039709912007 + }, + { + "x": 1.7220276260831988, + "y": 4.2669507056926275, + "heading": -0.34047859723492613, + "angularVelocity": -1.9265448678897275, + "velocityX": -1.9468343358226654, + "velocityY": 0.45807860771049036, + "timestamp": 1.3710747928142129 + }, + { + "x": 1.5737356547013517, + "y": 4.301842929408325, + "heading": -0.4972933443760097, + "angularVelocity": -2.058724632188595, + "velocityX": -1.946834336728219, + "velocityY": 0.4580786038618867, + "timestamp": 1.447245614637225 + }, + { + "x": 1.4254436831497614, + "y": 4.336735152402613, + "heading": -0.6538038889287485, + "angularVelocity": -2.0547309429902363, + "velocityX": -1.9468343389566751, + "velocityY": 0.458078594390947, + "timestamp": 1.5234164364602372 + }, + { + "x": 1.2771517114677216, + "y": 4.371627374842491, + "heading": -0.799921474135194, + "angularVelocity": -1.9182881543008596, + "velocityX": -1.946834340669268, + "velocityY": 0.45807858711242555, + "timestamp": 1.5995872582832493 + }, + { + "x": 1.1288597396330466, + "y": 4.406519596633671, + "heading": -0.9209030529092569, + "angularVelocity": -1.5882929431321022, + "velocityX": -1.9468343426731174, + "velocityY": 0.4580785785960651, + "timestamp": 1.6757580801062615 + }, + { + "x": 0.9805677676343831, + "y": 4.4414118177279, + "heading": -0.9928951480324381, + "angularVelocity": -0.945140060198636, + "velocityX": -1.946834344826023, + "velocityY": 0.45807856944623543, + "timestamp": 1.7519289019292736 + }, + { + "x": 0.8484411497365192, + "y": 4.472500431600075, + "heading": -0.9928952477030265, + "angularVelocity": -0.0000013085140236118818, + "velocityX": -1.734609325928881, + "velocityY": 0.40814334318738743, + "timestamp": 1.8280997237522858 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": -5.09005494945374e-7, + "velocityX": -0.8673046852715057, + "velocityY": 0.2040716746574537, + "timestamp": 1.904270545575298 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 1.8053835791622905e-18, + "velocityX": 3.8085009658841144e-17, + "velocityY": -4.441213712458509e-17, + "timestamp": 1.9804413673983101 + }, + { + "x": 0.8633075709529878, + "y": 4.398931081775789, + "heading": -0.9928954814598945, + "angularVelocity": -0.0000019220769098791852, + "velocityX": 0.7977678757913192, + "velocityY": -0.8784412255669002, + "timestamp": 2.0818865801229673 + }, + { + "x": 0.9997101217758085, + "y": 4.248734981396651, + "heading": -0.9705934228713969, + "angularVelocity": 0.21984338136319737, + "velocityX": 1.3445932751212721, + "velocityY": -1.480563708676927, + "timestamp": 2.1833317928476244 + }, + { + "x": 1.1361126726042652, + "y": 4.098538881022651, + "heading": -0.9135846602313676, + "angularVelocity": 0.5619660219429253, + "velocityX": 1.3445932751768286, + "velocityY": -1.4805637086262706, + "timestamp": 2.2847770055722814 + }, + { + "x": 1.2725152234348027, + "y": 3.948342780650542, + "heading": -0.8396943742506122, + "angularVelocity": 0.7283762732235443, + "velocityX": 1.3445932751973406, + "velocityY": -1.4805637086076424, + "timestamp": 2.3862222182969384 + }, + { + "x": 1.4089177742668983, + "y": 3.7981466802798476, + "heading": -0.7577094886902542, + "angularVelocity": 0.808169093034302, + "velocityX": 1.3445932752127017, + "velocityY": -1.4805637085936922, + "timestamp": 2.4876674310215954 + }, + { + "x": 1.5453203251000565, + "y": 3.6479505799101184, + "heading": -0.6718990052626181, + "angularVelocity": 0.8458800678997359, + "velocityX": 1.3445932752231742, + "velocityY": -1.4805637085841812, + "timestamp": 2.5891126437462524 + }, + { + "x": 1.6817228759337532, + "y": 3.4977544795408777, + "heading": -0.5843529548214927, + "angularVelocity": 0.8629884850135144, + "velocityX": 1.3445932752284844, + "velocityY": -1.4805637085793588, + "timestamp": 2.6905578564709094 + }, + { + "x": 1.8181254267674478, + "y": 3.3475583791716352, + "heading": -0.496152534626129, + "angularVelocity": 0.8694389594781329, + "velocityX": 1.3445932752284602, + "velocityY": -1.4805637085793808, + "timestamp": 2.7920030691955664 + }, + { + "x": 1.9545279776008493, + "y": 3.1973622788021268, + "heading": -0.4079812901328632, + "angularVelocity": 0.8691513588973434, + "velocityX": 1.3445932752255727, + "velocityY": -1.480563708582003, + "timestamp": 2.8934482819202234 + }, + { + "x": 2.0909305284339355, + "y": 3.0471661784323323, + "heading": -0.32051597252506797, + "angularVelocity": 0.8621926580724313, + "velocityX": 1.344593275222464, + "velocityY": -1.4805637085848262, + "timestamp": 2.9948934946448804 + }, + { + "x": 2.227333079266667, + "y": 2.896970078062216, + "heading": -0.234818517184951, + "angularVelocity": 0.8447658892757932, + "velocityX": 1.3445932752189704, + "velocityY": -1.4805637085879988, + "timestamp": 3.0963387073695374 + }, + { + "x": 2.3637356300989167, + "y": 2.7467739776916615, + "heading": -0.15295154788429333, + "angularVelocity": 0.8070067290692294, + "velocityX": 1.3445932752142213, + "velocityY": -1.480563708592312, + "timestamp": 3.1977839200941944 + }, + { + "x": 2.5001381809305556, + "y": 2.596577877320553, + "heading": -0.07915862654379939, + "angularVelocity": 0.7274164976200789, + "velocityX": 1.3445932752082013, + "velocityY": -1.4805637085977792, + "timestamp": 3.2992291328188514 + }, + { + "x": 2.6365407317614795, + "y": 2.4463817769487943, + "heading": -0.02222133180148921, + "angularVelocity": 0.5612615244531013, + "velocityX": 1.3445932752011478, + "velocityY": -1.480563708604185, + "timestamp": 3.4006743455435084 + }, + { + "x": 2.7729432825902887, + "y": 2.296185676575095, + "heading": 1.9540240946006604e-7, + "angularVelocity": 0.21904954021056072, + "velocityX": 1.3445932751803078, + "velocityY": -1.480563708623314, + "timestamp": 3.5021195582681655 + }, + { + "x": 2.8538730144500737, + "y": 2.207072019577026, + "heading": -5.5771741386668314e-18, + "angularVelocity": -0.0000019261866008177493, + "velocityX": 0.7977678757443631, + "velocityY": -0.8784412256095482, + "timestamp": 3.6035647709928225 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": -2.725395356358997e-18, + "angularVelocity": 1.2458293783490516e-18, + "velocityX": 3.57553609530669e-17, + "velocityY": -3.937144509458571e-17, + "timestamp": 3.7050099837174795 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "exit center": { "waypoints": [ { "x": 1.3126474618911743, @@ -227,8 +1188,8 @@ "controlIntervalCount": 16 }, { - "x": 2.891404867172241, - "y": 6.385702610015869, + "x": 2.8771493434906006, + "y": 5.000100135803223, "heading": 1.5707963267948966, "isInitialGuess": false, "translationConstrained": true, @@ -240,155 +1201,155 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -1.6516987428817835e-31, - "angularVelocity": 3.365627186718647e-31, + "heading": 0, + "angularVelocity": -4.047258922723054e-33, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 1.3392991160266923, - "y": 5.578340379347365, - "heading": 0.05084527674264656, - "angularVelocity": 0.9301054243469516, - "velocityX": 0.48753492295327294, - "velocityY": 0.20584457391191183, - "timestamp": 0.05466614365607662 - }, - { - "x": 1.3926138209310608, - "y": 5.602095558464793, - "heading": 0.15090427909370982, - "angularVelocity": 1.8303651155744338, - "velocityX": 0.9752783229010918, - "velocityY": 0.4345501169221013, - "timestamp": 0.10933228731215323 - }, - { - "x": 1.4727419129112613, - "y": 5.6401417619413445, - "heading": 0.29625295514716404, - "angularVelocity": 2.6588426827378284, - "velocityX": 1.4657718035556655, - "velocityY": 0.6959737953332358, - "timestamp": 0.16399843096822986 - }, - { - "x": 1.5806463473229446, - "y": 5.695327975123219, - "heading": 0.473406200223283, - "angularVelocity": 3.2406391456959285, - "velocityX": 1.973880489733221, - "velocityY": 1.0095135579540684, - "timestamp": 0.21866457462430647 - }, - { - "x": 1.712426609279705, - "y": 5.771939883643632, - "heading": 0.6149480873197845, - "angularVelocity": 2.589205633142697, - "velocityX": 2.4106376112029215, - "velocityY": 1.4014507590366152, - "timestamp": 0.2733307182803831 - }, - { - "x": 1.8646157040757754, - "y": 5.861735120575505, - "heading": 0.6843254516381283, - "angularVelocity": 1.2691102696912473, - "velocityX": 2.7839734910430862, - "velocityY": 1.6426115128369911, - "timestamp": 0.3279968619364597 - }, - { - "x": 2.0394443509304283, - "y": 5.959373498780485, - "heading": 0.6843306271163396, - "angularVelocity": 0.00009467428768829868, - "velocityX": 3.1981156006642792, - "velocityY": 1.7860849819452436, - "timestamp": 0.38266300559253635 - }, - { - "x": 2.2170155057005516, - "y": 6.0519338232599695, - "heading": 0.6843311645173596, - "angularVelocity": 0.000009830600514719364, - "velocityX": 3.2482839083598782, - "velocityY": 1.6931928665357103, - "timestamp": 0.437329149248613 - }, - { - "x": 2.379893345636442, - "y": 6.14773810700836, - "heading": 0.7173523116229228, - "angularVelocity": 0.6040511530008493, - "velocityX": 2.97950118743716, - "velocityY": 1.7525341526032563, - "timestamp": 0.49199529290468963 - }, - { - "x": 2.5210272079944938, - "y": 6.230636536851746, - "heading": 0.8166805533527952, - "angularVelocity": 1.8169974153432211, - "velocityX": 2.581741694566448, - "velocityY": 1.5164492005312948, - "timestamp": 0.5466614365607663 - }, - { - "x": 2.6419948699380145, - "y": 6.295385989559359, - "heading": 0.9758437840679963, - "angularVelocity": 2.9115503686623883, - "velocityX": 2.2128442552042693, - "velocityY": 1.1844525400396748, - "timestamp": 0.6013275802168428 - }, - { - "x": 2.744635840082322, - "y": 6.338256745744779, - "heading": 1.187997716756794, - "angularVelocity": 3.8809017519788993, - "velocityX": 1.8775966856205815, - "velocityY": 0.7842286526581116, - "timestamp": 0.6559937238729194 - }, - { - "x": 2.819689210716326, - "y": 6.364168809782246, - "heading": 1.3690565756448358, - "angularVelocity": 3.312083984323915, - "velocityX": 1.3729406468872292, - "velocityY": 0.474005706356175, - "timestamp": 0.710659867528996 - }, - { - "x": 2.867851682057744, - "y": 6.37891492245815, - "heading": 1.5017329656570861, - "angularVelocity": 2.427030354417588, - "velocityX": 0.881029246262995, - "velocityY": 0.2697485443399293, - "timestamp": 0.7653260111850726 - }, - { - "x": 2.891404867172241, - "y": 6.385702610015869, + "x": 1.3562741157973404, + "y": 5.551277015096179, + "heading": 5.1790019930154625e-8, + "angularVelocity": 8.222689572844604e-7, + "velocityX": 0.6926593812833762, + "velocityY": -0.2510250916993935, + "timestamp": 0.06298428215226594 + }, + { + "x": 1.4435274144279229, + "y": 5.519655747707938, + "heading": 1.8284200071758018e-7, + "angularVelocity": 0.0000020807092906136113, + "velocityX": 1.3853186167883211, + "velocityY": -0.5020501354892848, + "timestamp": 0.12596856430453188 + }, + { + "x": 1.561958503686593, + "y": 5.476735411831991, + "heading": 0.03758882643806014, + "angularVelocity": 0.5967940303771042, + "velocityX": 1.8803276819502406, + "velocityY": -0.6814451861527359, + "timestamp": 0.18895284645679783 + }, + { + "x": 1.6803895924155994, + "y": 5.433815074494518, + "heading": 0.1495361372904831, + "angularVelocity": 1.7773848812277921, + "velocityX": 1.8803276735407826, + "velocityY": -0.6814452093573423, + "timestamp": 0.25193712860906375 + }, + { + "x": 1.7988206811669072, + "y": 5.3908947372185825, + "heading": 0.3084692846412802, + "angularVelocity": 2.5233779273148262, + "velocityX": 1.8803276738948609, + "velocityY": -0.6814452083803143, + "timestamp": 0.3149214107613297 + }, + { + "x": 1.9172517698549563, + "y": 5.347974399768096, + "heading": 0.4903249170402497, + "angularVelocity": 2.887317695537583, + "velocityX": 1.880327672890501, + "velocityY": -0.6814452111516576, + "timestamp": 0.3779056929135956 + }, + { + "x": 2.0356828583945066, + "y": 5.305054061907854, + "heading": 0.6822100511004133, + "angularVelocity": 3.0465558628782476, + "velocityX": 1.880327670532791, + "velocityY": -0.6814452176573361, + "timestamp": 0.44088997506586153 + }, + { + "x": 2.1541139468698915, + "y": 5.26213372387056, + "heading": 0.8783830367201194, + "angularVelocity": 3.1146339835302603, + "velocityX": 1.880327669514046, + "velocityY": -0.6814452204683829, + "timestamp": 0.5038742572181275 + }, + { + "x": 2.272545035456946, + "y": 5.219213386141397, + "heading": 1.0737017740998906, + "angularVelocity": 3.1010711038602228, + "velocityX": 1.8803276712870123, + "velocityY": -0.6814452155761989, + "timestamp": 0.5668585393703934 + }, + { + "x": 2.390976124200322, + "y": 5.176293048843576, + "heading": 1.2594557238711797, + "angularVelocity": 2.949211190852737, + "velocityX": 1.8803276737689296, + "velocityY": -0.6814452087277921, + "timestamp": 0.6298428215226594 + }, + { + "x": 2.5094072129682594, + "y": 5.133372711613527, + "heading": 1.4204186158420298, + "angularVelocity": 2.5556041359925064, + "velocityX": 1.8803276741588917, + "velocityY": -0.6814452076517687, + "timestamp": 0.6928271036749253 + }, + { + "x": 2.627838301697845, + "y": 5.090452374277653, + "heading": 1.5329754550704864, + "angularVelocity": 1.787062349243704, + "velocityX": 1.880327673549982, + "velocityY": -0.6814452093319581, + "timestamp": 0.7558113858271912 + }, + { + "x": 2.746269390946397, + "y": 5.047532038373786, + "heading": 1.5707961445746523, + "angularVelocity": 0.6004782179263964, + "velocityX": 1.8803276817895906, + "velocityY": -0.6814451865960204, + "timestamp": 0.8187956679794571 + }, + { + "x": 2.8335226895825705, + "y": 5.015910771000973, + "heading": 1.570796275196268, + "angularVelocity": 0.000002073876396402678, + "velocityX": 1.3853186168770866, + "velocityY": -0.5020501352443532, + "timestamp": 0.8817799501317231 + }, + { + "x": 2.8771493434906006, + "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 1.2633662541172024, - "velocityX": 0.4308550693218517, - "velocityY": 0.12416620423096118, - "timestamp": 0.8199921548411492 + "angularVelocity": 8.19230237095319e-7, + "velocityX": 0.6926593813129686, + "velocityY": -0.25102509161774106, + "timestamp": 0.944764232283989 }, { - "x": 2.891404867172241, - "y": 6.385702610015869, + "x": 2.8771493434906006, + "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 1.5542922184430967e-29, - "velocityX": -2.3304215196416068e-31, + "angularVelocity": 0, + "velocityX": -1.88980553526586e-32, "velocityY": 0, - "timestamp": 0.8746582984972258 + "timestamp": 1.007748514436255 } ], "constraints": [ @@ -403,6 +1364,14 @@ "last" ], "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/2 piece left.1.traj b/src/main/deploy/choreo/2 piece left.1.traj new file mode 100644 index 0000000..509ec72 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.1.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 2.0283839438048332e-19, + "velocityX": 8.094691061889947e-19, + "velocityY": 1.7638426740730563e-19, + "timestamp": 0 + }, + { + "x": 0.8322802456836562, + "y": 6.653172699607564, + "heading": 1.0074798311324673, + "angularVelocity": -3.7400126935671097e-7, + "velocityX": 0.8646152527784429, + "velocityY": 0.18633427357388527, + "timestamp": 0.07561311115664361 + }, + { + "x": 0.9630327340876748, + "y": 6.681351325717821, + "heading": 1.007479762583371, + "angularVelocity": -9.065768535902331e-7, + "velocityX": 1.7292303729328837, + "velocityY": 0.3726685184515223, + "timestamp": 0.15122622231328722 + }, + { + "x": 1.1108648715080907, + "y": 6.713210807608451, + "heading": 0.935326621538057, + "angularVelocity": -0.9542411354538504, + "velocityX": 1.955112481936631, + "velocityY": 0.4213486444781865, + "timestamp": 0.22683933346993085 + }, + { + "x": 1.258697008933641, + "y": 6.745070289475266, + "heading": 0.8127259503436992, + "angularVelocity": -1.6214208001621926, + "velocityX": 1.9551124820045325, + "velocityY": 0.421348644163222, + "timestamp": 0.30245244462657445 + }, + { + "x": 1.406529146351303, + "y": 6.776929771378683, + "heading": 0.6640255812369789, + "angularVelocity": -1.966595036655294, + "velocityX": 1.9551124819002106, + "velocityY": 0.4213486446472832, + "timestamp": 0.37806555578321804 + }, + { + "x": 1.5543612837652037, + "y": 6.808789253299551, + "heading": 0.5044825425717008, + "angularVelocity": -2.1099917226625076, + "velocityX": 1.9551124818504668, + "velocityY": 0.42134864487809753, + "timestamp": 0.45367866693986164 + }, + { + "x": 1.702193421185673, + "y": 6.840648735189942, + "heading": 0.344692011554351, + "angularVelocity": -2.1132648633690048, + "velocityX": 1.9551124819373364, + "velocityY": 0.42134864447501086, + "timestamp": 0.5292917780965053 + }, + { + "x": 1.8500255586150136, + "y": 6.872508217039167, + "heading": 0.19547183240040739, + "angularVelocity": -1.9734696386822674, + "velocityX": 1.9551124820546628, + "velocityY": 0.42134864393060517, + "timestamp": 0.6049048892531489 + }, + { + "x": 1.9978576960460066, + "y": 6.904367698880726, + "heading": 0.07244487018227615, + "angularVelocity": -1.6270585925668755, + "velocityX": 1.9551124820765156, + "velocityY": 0.42134864382921067, + "timestamp": 0.6805180004097925 + }, + { + "x": 2.1456898334992545, + "y": 6.936227180619012, + "heading": 9.65887879992878e-8, + "angularVelocity": -0.9580980399471752, + "velocityX": 1.955112482370841, + "velocityY": 0.4213486424633956, + "timestamp": 0.7561311115664361 + }, + { + "x": 2.2764423218817926, + "y": 6.964405806828939, + "heading": 2.820501204963407e-8, + "angularVelocity": -9.043904534147509e-7, + "velocityX": 1.7292303726488056, + "velocityY": 0.37266851976968174, + "timestamp": 0.8317442227230797 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.9396360429134164e-18, + "angularVelocity": -3.7301747793182745e-7, + "velocityX": 0.8646152526837502, + "velocityY": 0.18633427401327182, + "timestamp": 0.9073573338797233 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.2932527092188034e-18, + "angularVelocity": 4.19600892926216e-20, + "velocityX": -8.058124746715222e-20, + "velocityY": -1.860414915840738e-20, + "timestamp": 0.9829704450363669 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.2.traj b/src/main/deploy/choreo/2 piece left.2.traj new file mode 100644 index 0000000..a50ba99 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.2.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.2932527092188034e-18, + "angularVelocity": 4.19600892926216e-20, + "velocityX": -8.058124746715222e-20, + "velocityY": -1.860414915840738e-20, + "timestamp": 0 + }, + { + "x": 2.2764423218798027, + "y": 6.964405806838171, + "heading": 2.828141101686341e-8, + "angularVelocity": 3.7402787103507053e-7, + "velocityX": -0.8646152527100619, + "velocityY": -0.18633427389118332, + "timestamp": 0.07561311115664371 + }, + { + "x": 2.145689833491728, + "y": 6.936227180653932, + "heading": 9.68470451708674e-8, + "angularVelocity": 9.067955680869744e-7, + "velocityX": -1.7292303727220222, + "velocityY": -0.3726685194299493, + "timestamp": 0.1512262223132873 + }, + { + "x": 1.9978576960446424, + "y": 6.904367698887054, + "heading": 0.07260812222938247, + "angularVelocity": 0.960257080705476, + "velocityX": -1.9551124822893438, + "velocityY": -0.42134864284155416, + "timestamp": 0.2268393334699309 + }, + { + "x": 1.8500255586147867, + "y": 6.872508217040216, + "heading": 0.1958338387782673, + "angularVelocity": 1.6296871622383675, + "velocityX": -1.955112482061473, + "velocityY": -0.42134864389901044, + "timestamp": 0.3024524446265745 + }, + { + "x": 1.7021934211910146, + "y": 6.840648735165152, + "heading": 0.3451117743398479, + "angularVelocity": 1.9742334798568144, + "velocityX": -1.95511248198102, + "velocityY": -0.42134864427231705, + "timestamp": 0.3780655557832181 + }, + { + "x": 1.5543612837761693, + "y": 6.8087892532486665, + "heading": 0.5047130791830543, + "angularVelocity": 2.110762305661104, + "velocityX": -1.9551124818629584, + "velocityY": -0.42134864482013473, + "timestamp": 0.4536786669398617 + }, + { + "x": 1.4065291463620155, + "y": 6.776929771328972, + "heading": 0.6639739239039864, + "angularVelocity": 2.106259645777567, + "velocityX": -1.9551124818538146, + "velocityY": -0.4213486448625627, + "timestamp": 0.5292917780965053 + }, + { + "x": 1.2586970089400131, + "y": 6.745070289445696, + "heading": 0.8125476250392986, + "angularVelocity": 1.9649198249166078, + "velocityX": -1.9551124819576147, + "velocityY": -0.4213486443809204, + "timestamp": 0.6049048892531489 + }, + { + "x": 1.1108648715115719, + "y": 6.713210807592297, + "heading": 0.9352215452290742, + "angularVelocity": 1.622389534212375, + "velocityX": -1.9551124820427679, + "velocityY": -0.42134864398580435, + "timestamp": 0.6805180004097925 + }, + { + "x": 0.9630327340807132, + "y": 6.681351325750123, + "heading": 1.0074797622855187, + "angularVelocity": 0.9556307887761831, + "velocityX": -1.9551124820747394, + "velocityY": -0.42134864383734444, + "timestamp": 0.7561311115664361 + }, + { + "x": 0.8322802456820239, + "y": 6.653172699615138, + "heading": 1.0074798310413775, + "angularVelocity": 9.093113311976794e-7, + "velocityX": -1.7292303728624039, + "velocityY": -0.3726685187785591, + "timestamp": 0.8317442227230797 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 3.75205950615561e-7, + "velocityX": -0.8646152527568558, + "velocityY": -0.18633427367405322, + "timestamp": 0.9073573338797233 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": -2.242417201812226e-19, + "velocityX": -4.158054673626952e-19, + "velocityY": -9.844851544165139e-20, + "timestamp": 0.9829704450363669 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.3.traj b/src/main/deploy/choreo/2 piece left.3.traj new file mode 100644 index 0000000..1ab8595 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.3.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": -2.242417201812226e-19, + "velocityX": -4.158054673626952e-19, + "velocityY": -9.844851544165139e-20, + "timestamp": 0 + }, + { + "x": 0.867342073873612, + "y": 6.677517805054488, + "heading": 1.0074798401596599, + "angularVelocity": -2.0078718725992087e-7, + "velocityX": 1.0474996006171284, + "velocityY": 0.4008443829976782, + "timestamp": 0.09588364267332339 + }, + { + "x": 1.0464438315164646, + "y": 6.746054283251552, + "heading": 0.9900885492460468, + "angularVelocity": -0.18137912190993252, + "velocityX": 1.8679073160899227, + "velocityY": 0.7147880106158334, + "timestamp": 0.19176728534664655 + }, + { + "x": 1.2255455891596982, + "y": 6.814590761447637, + "heading": 0.9294452786879966, + "angularVelocity": -0.6324673204653116, + "velocityX": 1.8679073160938997, + "velocityY": 0.7147880106056311, + "timestamp": 0.2876509280199697 + }, + { + "x": 1.4046473468026037, + "y": 6.883127239644582, + "heading": 0.8469789832587827, + "angularVelocity": -0.8600663588697559, + "velocityX": 1.8679073160904744, + "velocityY": 0.7147880106145814, + "timestamp": 0.3835345706932929 + }, + { + "x": 1.5837491044451752, + "y": 6.951663717842399, + "heading": 0.7537506929542891, + "angularVelocity": -0.9723065134490515, + "velocityX": 1.867907316086992, + "velocityY": 0.7147880106236818, + "timestamp": 0.47941821336661605 + }, + { + "x": 1.762850862087631, + "y": 7.020200196040518, + "heading": 0.6553282675051919, + "angularVelocity": -1.0264777464121126, + "velocityX": 1.867907316085786, + "velocityY": 0.7147880106268334, + "timestamp": 0.5753018560399392 + }, + { + "x": 1.941952619730239, + "y": 7.088736674238239, + "heading": 0.5545759098990335, + "angularVelocity": -1.0507773254862967, + "velocityX": 1.8679073160873736, + "velocityY": 0.7147880106226845, + "timestamp": 0.6711854987132624 + }, + { + "x": 2.121054377373199, + "y": 7.157273152435041, + "heading": 0.453150200963735, + "angularVelocity": -1.0577999135979546, + "velocityX": 1.8679073160910444, + "velocityY": 0.7147880106130912, + "timestamp": 0.7670691413865856 + }, + { + "x": 2.3001561350165884, + "y": 7.225809630630721, + "heading": 0.3523691136343556, + "angularVelocity": -1.0510769566060587, + "velocityX": 1.8679073160955193, + "velocityY": 0.7147880106013975, + "timestamp": 0.8629527840599087 + }, + { + "x": 2.479257892660336, + "y": 7.294346108825463, + "heading": 0.2538978395296278, + "angularVelocity": -1.026987204065876, + "velocityX": 1.867907316099261, + "velocityY": 0.7147880105916198, + "timestamp": 0.9588364267332319 + }, + { + "x": 2.658359650304228, + "y": 7.362882587019828, + "heading": 0.16061512982409304, + "angularVelocity": -0.9728740701200732, + "velocityX": 1.8679073161007647, + "velocityY": 0.7147880105876907, + "timestamp": 1.054720069406555 + }, + { + "x": 2.8374614079479796, + "y": 7.431419065214562, + "heading": 0.07810324033722603, + "angularVelocity": -0.8605418733202074, + "velocityX": 1.867907316099298, + "velocityY": 0.7147880105915232, + "timestamp": 1.1506037120798782 + }, + { + "x": 3.0165631655914407, + "y": 7.499955543410054, + "heading": 0.01742736740360304, + "angularVelocity": -0.6328073406675466, + "velocityX": 1.8679073160962676, + "velocityY": 0.714788010599443, + "timestamp": 1.2464873547532014 + }, + { + "x": 3.195664923235554, + "y": 7.568492021603823, + "heading": 1.924276877277615e-8, + "angularVelocity": -0.18175517403118957, + "velocityX": 1.867907316103074, + "velocityY": 0.7147880105814662, + "timestamp": 1.3423709974265245 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -1.4869843514051715e-19, + "angularVelocity": -2.006887540420477e-7, + "velocityX": 1.0474996006097614, + "velocityY": 0.4008443830169303, + "timestamp": 1.4382546400998477 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -7.359523754309027e-20, + "angularVelocity": 1.5727023698046957e-20, + "velocityX": -4.856900753028092e-20, + "velocityY": -1.8631537844939477e-20, + "timestamp": 1.5341382827731709 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.traj b/src/main/deploy/choreo/2 piece left.traj new file mode 100644 index 0000000..0f491e4 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.traj @@ -0,0 +1,391 @@ +{ + "samples": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 2.0283839438048332e-19, + "velocityX": 8.094691061889947e-19, + "velocityY": 1.7638426740730563e-19, + "timestamp": 0 + }, + { + "x": 0.8322802456836562, + "y": 6.653172699607564, + "heading": 1.0074798311324673, + "angularVelocity": -3.7400126935671097e-7, + "velocityX": 0.8646152527784429, + "velocityY": 0.18633427357388527, + "timestamp": 0.07561311115664361 + }, + { + "x": 0.9630327340876748, + "y": 6.681351325717821, + "heading": 1.007479762583371, + "angularVelocity": -9.065768535902331e-7, + "velocityX": 1.7292303729328837, + "velocityY": 0.3726685184515223, + "timestamp": 0.15122622231328722 + }, + { + "x": 1.1108648715080907, + "y": 6.713210807608451, + "heading": 0.935326621538057, + "angularVelocity": -0.9542411354538504, + "velocityX": 1.955112481936631, + "velocityY": 0.4213486444781865, + "timestamp": 0.22683933346993085 + }, + { + "x": 1.258697008933641, + "y": 6.745070289475266, + "heading": 0.8127259503436992, + "angularVelocity": -1.6214208001621926, + "velocityX": 1.9551124820045325, + "velocityY": 0.421348644163222, + "timestamp": 0.30245244462657445 + }, + { + "x": 1.406529146351303, + "y": 6.776929771378683, + "heading": 0.6640255812369789, + "angularVelocity": -1.966595036655294, + "velocityX": 1.9551124819002106, + "velocityY": 0.4213486446472832, + "timestamp": 0.37806555578321804 + }, + { + "x": 1.5543612837652037, + "y": 6.808789253299551, + "heading": 0.5044825425717008, + "angularVelocity": -2.1099917226625076, + "velocityX": 1.9551124818504668, + "velocityY": 0.42134864487809753, + "timestamp": 0.45367866693986164 + }, + { + "x": 1.702193421185673, + "y": 6.840648735189942, + "heading": 0.344692011554351, + "angularVelocity": -2.1132648633690048, + "velocityX": 1.9551124819373364, + "velocityY": 0.42134864447501086, + "timestamp": 0.5292917780965053 + }, + { + "x": 1.8500255586150136, + "y": 6.872508217039167, + "heading": 0.19547183240040739, + "angularVelocity": -1.9734696386822674, + "velocityX": 1.9551124820546628, + "velocityY": 0.42134864393060517, + "timestamp": 0.6049048892531489 + }, + { + "x": 1.9978576960460066, + "y": 6.904367698880726, + "heading": 0.07244487018227615, + "angularVelocity": -1.6270585925668755, + "velocityX": 1.9551124820765156, + "velocityY": 0.42134864382921067, + "timestamp": 0.6805180004097925 + }, + { + "x": 2.1456898334992545, + "y": 6.936227180619012, + "heading": 9.65887879992878e-8, + "angularVelocity": -0.9580980399471752, + "velocityX": 1.955112482370841, + "velocityY": 0.4213486424633956, + "timestamp": 0.7561311115664361 + }, + { + "x": 2.2764423218817926, + "y": 6.964405806828939, + "heading": 2.820501204963407e-8, + "angularVelocity": -9.043904534147509e-7, + "velocityX": 1.7292303726488056, + "velocityY": 0.37266851976968174, + "timestamp": 0.8317442227230797 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.9396360429134164e-18, + "angularVelocity": -3.7301747793182745e-7, + "velocityX": 0.8646152526837502, + "velocityY": 0.18633427401327182, + "timestamp": 0.9073573338797233 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": -1.2932527092188034e-18, + "angularVelocity": 4.19600892926216e-20, + "velocityX": -8.058124746715222e-20, + "velocityY": -1.860414915840738e-20, + "timestamp": 0.9829704450363669 + }, + { + "x": 2.2764423218798027, + "y": 6.964405806838171, + "heading": 2.828141101686341e-8, + "angularVelocity": 3.7402787103507053e-7, + "velocityX": -0.8646152527100619, + "velocityY": -0.18633427389118332, + "timestamp": 1.0585835561930106 + }, + { + "x": 2.145689833491728, + "y": 6.936227180653932, + "heading": 9.68470451708674e-8, + "angularVelocity": 9.067955680869744e-7, + "velocityX": -1.7292303727220222, + "velocityY": -0.3726685194299493, + "timestamp": 1.1341966673496542 + }, + { + "x": 1.9978576960446424, + "y": 6.904367698887054, + "heading": 0.07260812222938247, + "angularVelocity": 0.960257080705476, + "velocityX": -1.9551124822893438, + "velocityY": -0.42134864284155416, + "timestamp": 1.2098097785062978 + }, + { + "x": 1.8500255586147867, + "y": 6.872508217040216, + "heading": 0.1958338387782673, + "angularVelocity": 1.6296871622383675, + "velocityX": -1.955112482061473, + "velocityY": -0.42134864389901044, + "timestamp": 1.2854228896629414 + }, + { + "x": 1.7021934211910146, + "y": 6.840648735165152, + "heading": 0.3451117743398479, + "angularVelocity": 1.9742334798568144, + "velocityX": -1.95511248198102, + "velocityY": -0.42134864427231705, + "timestamp": 1.361036000819585 + }, + { + "x": 1.5543612837761693, + "y": 6.8087892532486665, + "heading": 0.5047130791830543, + "angularVelocity": 2.110762305661104, + "velocityX": -1.9551124818629584, + "velocityY": -0.42134864482013473, + "timestamp": 1.4366491119762286 + }, + { + "x": 1.4065291463620155, + "y": 6.776929771328972, + "heading": 0.6639739239039864, + "angularVelocity": 2.106259645777567, + "velocityX": -1.9551124818538146, + "velocityY": -0.4213486448625627, + "timestamp": 1.5122622231328722 + }, + { + "x": 1.2586970089400131, + "y": 6.745070289445696, + "heading": 0.8125476250392986, + "angularVelocity": 1.9649198249166078, + "velocityX": -1.9551124819576147, + "velocityY": -0.4213486443809204, + "timestamp": 1.5878753342895158 + }, + { + "x": 1.1108648715115719, + "y": 6.713210807592297, + "heading": 0.9352215452290742, + "angularVelocity": 1.622389534212375, + "velocityX": -1.9551124820427679, + "velocityY": -0.42134864398580435, + "timestamp": 1.6634884454461594 + }, + { + "x": 0.9630327340807132, + "y": 6.681351325750123, + "heading": 1.0074797622855187, + "angularVelocity": 0.9556307887761831, + "velocityX": -1.9551124820747394, + "velocityY": -0.42134864383734444, + "timestamp": 1.739101556602803 + }, + { + "x": 0.8322802456820239, + "y": 6.653172699615138, + "heading": 1.0074798310413775, + "angularVelocity": 9.093113311976794e-7, + "velocityX": -1.7292303728624039, + "velocityY": -0.3726685187785591, + "timestamp": 1.8147146677594466 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 3.75205950615561e-7, + "velocityX": -0.8646152527568558, + "velocityY": -0.18633427367405322, + "timestamp": 1.8903277789160902 + }, + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": -2.242417201812226e-19, + "velocityX": -4.158054673626952e-19, + "velocityY": -9.844851544165139e-20, + "timestamp": 1.9659408900727338 + }, + { + "x": 0.867342073873612, + "y": 6.677517805054488, + "heading": 1.0074798401596599, + "angularVelocity": -2.0078718725992087e-7, + "velocityX": 1.0474996006171284, + "velocityY": 0.4008443829976782, + "timestamp": 2.061824532746057 + }, + { + "x": 1.0464438315164646, + "y": 6.746054283251552, + "heading": 0.9900885492460468, + "angularVelocity": -0.18137912190993252, + "velocityX": 1.8679073160899227, + "velocityY": 0.7147880106158334, + "timestamp": 2.1577081754193803 + }, + { + "x": 1.2255455891596982, + "y": 6.814590761447637, + "heading": 0.9294452786879966, + "angularVelocity": -0.6324673204653116, + "velocityX": 1.8679073160938997, + "velocityY": 0.7147880106056311, + "timestamp": 2.2535918180927035 + }, + { + "x": 1.4046473468026037, + "y": 6.883127239644582, + "heading": 0.8469789832587827, + "angularVelocity": -0.8600663588697559, + "velocityX": 1.8679073160904744, + "velocityY": 0.7147880106145814, + "timestamp": 2.3494754607660266 + }, + { + "x": 1.5837491044451752, + "y": 6.951663717842399, + "heading": 0.7537506929542891, + "angularVelocity": -0.9723065134490515, + "velocityX": 1.867907316086992, + "velocityY": 0.7147880106236818, + "timestamp": 2.44535910343935 + }, + { + "x": 1.762850862087631, + "y": 7.020200196040518, + "heading": 0.6553282675051919, + "angularVelocity": -1.0264777464121126, + "velocityX": 1.867907316085786, + "velocityY": 0.7147880106268334, + "timestamp": 2.541242746112673 + }, + { + "x": 1.941952619730239, + "y": 7.088736674238239, + "heading": 0.5545759098990335, + "angularVelocity": -1.0507773254862967, + "velocityX": 1.8679073160873736, + "velocityY": 0.7147880106226845, + "timestamp": 2.637126388785996 + }, + { + "x": 2.121054377373199, + "y": 7.157273152435041, + "heading": 0.453150200963735, + "angularVelocity": -1.0577999135979546, + "velocityX": 1.8679073160910444, + "velocityY": 0.7147880106130912, + "timestamp": 2.7330100314593193 + }, + { + "x": 2.3001561350165884, + "y": 7.225809630630721, + "heading": 0.3523691136343556, + "angularVelocity": -1.0510769566060587, + "velocityX": 1.8679073160955193, + "velocityY": 0.7147880106013975, + "timestamp": 2.8288936741326425 + }, + { + "x": 2.479257892660336, + "y": 7.294346108825463, + "heading": 0.2538978395296278, + "angularVelocity": -1.026987204065876, + "velocityX": 1.867907316099261, + "velocityY": 0.7147880105916198, + "timestamp": 2.9247773168059656 + }, + { + "x": 2.658359650304228, + "y": 7.362882587019828, + "heading": 0.16061512982409304, + "angularVelocity": -0.9728740701200732, + "velocityX": 1.8679073161007647, + "velocityY": 0.7147880105876907, + "timestamp": 3.020660959479289 + }, + { + "x": 2.8374614079479796, + "y": 7.431419065214562, + "heading": 0.07810324033722603, + "angularVelocity": -0.8605418733202074, + "velocityX": 1.867907316099298, + "velocityY": 0.7147880105915232, + "timestamp": 3.116544602152612 + }, + { + "x": 3.0165631655914407, + "y": 7.499955543410054, + "heading": 0.01742736740360304, + "angularVelocity": -0.6328073406675466, + "velocityX": 1.8679073160962676, + "velocityY": 0.714788010599443, + "timestamp": 3.212428244825935 + }, + { + "x": 3.195664923235554, + "y": 7.568492021603823, + "heading": 1.924276877277615e-8, + "angularVelocity": -0.18175517403118957, + "velocityX": 1.867907316103074, + "velocityY": 0.7147880105814662, + "timestamp": 3.3083118874992583 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -1.4869843514051715e-19, + "angularVelocity": -2.006887540420477e-7, + "velocityX": 1.0474996006097614, + "velocityY": 0.4008443830169303, + "timestamp": 3.4041955301725815 + }, + { + "x": 3.296103000640869, + "y": 7.606926441192627, + "heading": -7.359523754309027e-20, + "angularVelocity": 1.5727023698046957e-20, + "velocityX": -4.856900753028092e-20, + "velocityY": -1.8631537844939477e-20, + "timestamp": 3.5000791728459046 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.1.traj b/src/main/deploy/choreo/2 piece right.1.traj new file mode 100644 index 0000000..a06f7df --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.1.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 0.7823778390884399, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 3.4836656833317357e-19, + "velocityX": 5.792784763551635e-18, + "velocityY": -1.1468055635760044e-18, + "timestamp": 0 + }, + { + "x": 0.848441149203722, + "y": 4.472500429335688, + "heading": -0.9928952478971514, + "angularVelocity": 5.064569480290326e-7, + "velocityX": 0.8673046782767408, + "velocityY": -0.20407170438519942, + "timestamp": 0.0761708218230117 + }, + { + "x": 0.9805677659026901, + "y": 4.4414118103682325, + "heading": -0.9928951486952988, + "angularVelocity": 0.0000013023602772620627, + "velocityX": 1.7346093101893227, + "velocityY": -0.40814341008019434, + "timestamp": 0.1523416436460234 + }, + { + "x": 1.1288597388405326, + "y": 4.406519593265519, + "heading": -0.9209894371787697, + "angularVelocity": 0.9440059828106777, + "velocityX": 1.9468343571559381, + "velocityY": -0.4580785170440767, + "timestamp": 0.2285124654690351 + }, + { + "x": 1.2771517113083277, + "y": 4.371627374165104, + "heading": -0.8000545061710758, + "angularVelocity": 1.587680533219069, + "velocityX": 1.9468343509849748, + "velocityY": -0.4580785432706628, + "timestamp": 0.3046832872920468 + }, + { + "x": 1.4254436833762532, + "y": 4.336735153365243, + "heading": -0.6538028446856889, + "angularVelocity": 1.9200483595308036, + "velocityX": 1.9468343457353292, + "velocityY": -0.45807856558166105, + "timestamp": 0.3808541091150585 + }, + { + "x": 1.57373565515521, + "y": 4.301842931337266, + "heading": -0.4970386957469823, + "angularVelocity": 2.058060359424233, + "velocityX": 1.9468343419416414, + "velocityY": -0.458078581704838, + "timestamp": 0.45702493093807023 + }, + { + "x": 1.7220276265458214, + "y": 4.26695070765882, + "heading": -0.3400744666794648, + "angularVelocity": 2.060687088715348, + "velocityX": 1.946834336843289, + "velocityY": -0.4580786033728385, + "timestamp": 0.5331957527610819 + }, + { + "x": 1.8703195977756357, + "y": 4.232058483296987, + "heading": -0.19340046314195958, + "angularVelocity": 1.9255930292876797, + "velocityX": 1.9468343347322845, + "velocityY": -0.45807861234460967, + "timestamp": 0.6093665745840936 + }, + { + "x": 2.0186115688161, + "y": 4.197166258130416, + "heading": -0.0721222371749164, + "angularVelocity": 1.592187442178866, + "velocityX": 1.946834332246427, + "velocityY": -0.45807862290950535, + "timestamp": 0.6855373964071053 + }, + { + "x": 2.1669035397570235, + "y": 4.162274032540797, + "heading": -1.3790420971273228e-7, + "angularVelocity": 0.946846804912877, + "velocityX": 1.9468343309396154, + "velocityY": -0.4580786284634526, + "timestamp": 0.761708218230117 + }, + { + "x": 2.299030158583636, + "y": 4.131185422616258, + "heading": -3.861728516505335e-8, + "angularVelocity": 0.0000013034771345390653, + "velocityX": 1.7346093381218648, + "velocityY": -0.4081432913612903, + "timestamp": 0.8378790400531287 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -2.2763209665573703e-18, + "angularVelocity": 5.069826509442209e-7, + "velocityX": 0.8673046903657866, + "velocityY": -0.2040716530068378, + "timestamp": 0.9140498618761405 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -1.5143047799084299e-18, + "angularVelocity": 5.84054737228804e-20, + "velocityX": -8.219525464434245e-19, + "velocityY": 2.1312698857170775e-19, + "timestamp": 0.9902206836991522 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.2.traj b/src/main/deploy/choreo/2 piece right.2.traj new file mode 100644 index 0000000..2477c36 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.2.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -1.5143047799084299e-18, + "angularVelocity": 5.84054737228804e-20, + "velocityX": -8.219525464434245e-19, + "velocityY": 2.1312698857170775e-19, + "timestamp": 0 + }, + { + "x": 2.299030158572247, + "y": 4.131185422567857, + "heading": -3.8824131952933046e-8, + "angularVelocity": -5.096982155619529e-7, + "velocityX": -0.867304690515307, + "velocityY": 0.20407165237139407, + "timestamp": 0.07617082182301205 + }, + { + "x": 2.166903539754318, + "y": 4.162274032529242, + "heading": -1.3861601297880188e-7, + "angularVelocity": -0.000001310106398423391, + "velocityX": -1.7346093380078471, + "velocityY": 0.408143291845037, + "timestamp": 0.1523416436460242 + }, + { + "x": 2.018611568604941, + "y": 4.197166257232936, + "heading": -0.0722676107720337, + "angularVelocity": -0.948755316359051, + "velocityX": -1.9468343336762697, + "velocityY": 0.4580786168326712, + "timestamp": 0.22851246546903636 + }, + { + "x": 1.8703195973960691, + "y": 4.2320584816837785, + "heading": -0.19373209136885944, + "angularVelocity": -1.5946326649731632, + "velocityX": -1.946834334457335, + "velocityY": 0.458078613513146, + "timestamp": 0.3046832872920485 + }, + { + "x": 1.7220276260831988, + "y": 4.2669507056926275, + "heading": -0.34047859723492613, + "angularVelocity": -1.9265448678897275, + "velocityX": -1.9468343358226654, + "velocityY": 0.45807860771049036, + "timestamp": 0.3808541091150607 + }, + { + "x": 1.5737356547013517, + "y": 4.301842929408325, + "heading": -0.4972933443760097, + "angularVelocity": -2.058724632188595, + "velocityX": -1.946834336728219, + "velocityY": 0.4580786038618867, + "timestamp": 0.45702493093807284 + }, + { + "x": 1.4254436831497614, + "y": 4.336735152402613, + "heading": -0.6538038889287485, + "angularVelocity": -2.0547309429902363, + "velocityX": -1.9468343389566751, + "velocityY": 0.458078594390947, + "timestamp": 0.533195752761085 + }, + { + "x": 1.2771517114677216, + "y": 4.371627374842491, + "heading": -0.799921474135194, + "angularVelocity": -1.9182881543008596, + "velocityX": -1.946834340669268, + "velocityY": 0.45807858711242555, + "timestamp": 0.6093665745840972 + }, + { + "x": 1.1288597396330466, + "y": 4.406519596633671, + "heading": -0.9209030529092569, + "angularVelocity": -1.5882929431321022, + "velocityX": -1.9468343426731174, + "velocityY": 0.4580785785960651, + "timestamp": 0.6855373964071093 + }, + { + "x": 0.9805677676343831, + "y": 4.4414118177279, + "heading": -0.9928951480324381, + "angularVelocity": -0.945140060198636, + "velocityX": -1.946834344826023, + "velocityY": 0.45807856944623543, + "timestamp": 0.7617082182301215 + }, + { + "x": 0.8484411497365192, + "y": 4.472500431600075, + "heading": -0.9928952477030265, + "angularVelocity": -0.0000013085140236118818, + "velocityX": -1.734609325928881, + "velocityY": 0.40814334318738743, + "timestamp": 0.8378790400531336 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": -5.09005494945374e-7, + "velocityX": -0.8673046852715057, + "velocityY": 0.2040716746574537, + "timestamp": 0.9140498618761458 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 1.8053835791622905e-18, + "velocityX": 3.8085009658841144e-17, + "velocityY": -4.441213712458509e-17, + "timestamp": 0.9902206836991579 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.3.traj b/src/main/deploy/choreo/2 piece right.3.traj new file mode 100644 index 0000000..b1995c1 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.3.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 1.8053835791622905e-18, + "velocityX": 3.8085009658841144e-17, + "velocityY": -4.441213712458509e-17, + "timestamp": 0 + }, + { + "x": 0.8633075709529878, + "y": 4.398931081775789, + "heading": -0.9928954814598945, + "angularVelocity": -0.0000019220769098791852, + "velocityX": 0.7977678757913192, + "velocityY": -0.8784412255669002, + "timestamp": 0.10144521272465723 + }, + { + "x": 0.9997101217758085, + "y": 4.248734981396651, + "heading": -0.9705934228713969, + "angularVelocity": 0.21984338136319737, + "velocityX": 1.3445932751212721, + "velocityY": -1.480563708676927, + "timestamp": 0.20289042544931424 + }, + { + "x": 1.1361126726042652, + "y": 4.098538881022651, + "heading": -0.9135846602313676, + "angularVelocity": 0.5619660219429253, + "velocityX": 1.3445932751768286, + "velocityY": -1.4805637086262706, + "timestamp": 0.30433563817397125 + }, + { + "x": 1.2725152234348027, + "y": 3.948342780650542, + "heading": -0.8396943742506122, + "angularVelocity": 0.7283762732235443, + "velocityX": 1.3445932751973406, + "velocityY": -1.4805637086076424, + "timestamp": 0.40578085089862825 + }, + { + "x": 1.4089177742668983, + "y": 3.7981466802798476, + "heading": -0.7577094886902542, + "angularVelocity": 0.808169093034302, + "velocityX": 1.3445932752127017, + "velocityY": -1.4805637085936922, + "timestamp": 0.5072260636232853 + }, + { + "x": 1.5453203251000565, + "y": 3.6479505799101184, + "heading": -0.6718990052626181, + "angularVelocity": 0.8458800678997359, + "velocityX": 1.3445932752231742, + "velocityY": -1.4805637085841812, + "timestamp": 0.6086712763479423 + }, + { + "x": 1.6817228759337532, + "y": 3.4977544795408777, + "heading": -0.5843529548214927, + "angularVelocity": 0.8629884850135144, + "velocityX": 1.3445932752284844, + "velocityY": -1.4805637085793588, + "timestamp": 0.7101164890725993 + }, + { + "x": 1.8181254267674478, + "y": 3.3475583791716352, + "heading": -0.496152534626129, + "angularVelocity": 0.8694389594781329, + "velocityX": 1.3445932752284602, + "velocityY": -1.4805637085793808, + "timestamp": 0.8115617017972563 + }, + { + "x": 1.9545279776008493, + "y": 3.1973622788021268, + "heading": -0.4079812901328632, + "angularVelocity": 0.8691513588973434, + "velocityX": 1.3445932752255727, + "velocityY": -1.480563708582003, + "timestamp": 0.9130069145219133 + }, + { + "x": 2.0909305284339355, + "y": 3.0471661784323323, + "heading": -0.32051597252506797, + "angularVelocity": 0.8621926580724313, + "velocityX": 1.344593275222464, + "velocityY": -1.4805637085848262, + "timestamp": 1.0144521272465703 + }, + { + "x": 2.227333079266667, + "y": 2.896970078062216, + "heading": -0.234818517184951, + "angularVelocity": 0.8447658892757932, + "velocityX": 1.3445932752189704, + "velocityY": -1.4805637085879988, + "timestamp": 1.1158973399712273 + }, + { + "x": 2.3637356300989167, + "y": 2.7467739776916615, + "heading": -0.15295154788429333, + "angularVelocity": 0.8070067290692294, + "velocityX": 1.3445932752142213, + "velocityY": -1.480563708592312, + "timestamp": 1.2173425526958843 + }, + { + "x": 2.5001381809305556, + "y": 2.596577877320553, + "heading": -0.07915862654379939, + "angularVelocity": 0.7274164976200789, + "velocityX": 1.3445932752082013, + "velocityY": -1.4805637085977792, + "timestamp": 1.3187877654205413 + }, + { + "x": 2.6365407317614795, + "y": 2.4463817769487943, + "heading": -0.02222133180148921, + "angularVelocity": 0.5612615244531013, + "velocityX": 1.3445932752011478, + "velocityY": -1.480563708604185, + "timestamp": 1.4202329781451983 + }, + { + "x": 2.7729432825902887, + "y": 2.296185676575095, + "heading": 1.9540240946006604e-7, + "angularVelocity": 0.21904954021056072, + "velocityX": 1.3445932751803078, + "velocityY": -1.480563708623314, + "timestamp": 1.5216781908698553 + }, + { + "x": 2.8538730144500737, + "y": 2.207072019577026, + "heading": -5.5771741386668314e-18, + "angularVelocity": -0.0000019261866008177493, + "velocityX": 0.7977678757443631, + "velocityY": -0.8784412256095482, + "timestamp": 1.6231234035945123 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": -2.725395356358997e-18, + "angularVelocity": 1.2458293783490516e-18, + "velocityX": 3.57553609530669e-17, + "velocityY": -3.937144509458571e-17, + "timestamp": 1.7245686163191694 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.traj b/src/main/deploy/choreo/2 piece right.traj new file mode 100644 index 0000000..489c053 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.traj @@ -0,0 +1,400 @@ +{ + "samples": [ + { + "x": 0.7823778390884399, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 3.4836656833317357e-19, + "velocityX": 5.792784763551635e-18, + "velocityY": -1.1468055635760044e-18, + "timestamp": 0 + }, + { + "x": 0.848441149203722, + "y": 4.472500429335688, + "heading": -0.9928952478971514, + "angularVelocity": 5.064569480290326e-7, + "velocityX": 0.8673046782767408, + "velocityY": -0.20407170438519942, + "timestamp": 0.0761708218230117 + }, + { + "x": 0.9805677659026901, + "y": 4.4414118103682325, + "heading": -0.9928951486952988, + "angularVelocity": 0.0000013023602772620627, + "velocityX": 1.7346093101893227, + "velocityY": -0.40814341008019434, + "timestamp": 0.1523416436460234 + }, + { + "x": 1.1288597388405326, + "y": 4.406519593265519, + "heading": -0.9209894371787697, + "angularVelocity": 0.9440059828106777, + "velocityX": 1.9468343571559381, + "velocityY": -0.4580785170440767, + "timestamp": 0.2285124654690351 + }, + { + "x": 1.2771517113083277, + "y": 4.371627374165104, + "heading": -0.8000545061710758, + "angularVelocity": 1.587680533219069, + "velocityX": 1.9468343509849748, + "velocityY": -0.4580785432706628, + "timestamp": 0.3046832872920468 + }, + { + "x": 1.4254436833762532, + "y": 4.336735153365243, + "heading": -0.6538028446856889, + "angularVelocity": 1.9200483595308036, + "velocityX": 1.9468343457353292, + "velocityY": -0.45807856558166105, + "timestamp": 0.3808541091150585 + }, + { + "x": 1.57373565515521, + "y": 4.301842931337266, + "heading": -0.4970386957469823, + "angularVelocity": 2.058060359424233, + "velocityX": 1.9468343419416414, + "velocityY": -0.458078581704838, + "timestamp": 0.45702493093807023 + }, + { + "x": 1.7220276265458214, + "y": 4.26695070765882, + "heading": -0.3400744666794648, + "angularVelocity": 2.060687088715348, + "velocityX": 1.946834336843289, + "velocityY": -0.4580786033728385, + "timestamp": 0.5331957527610819 + }, + { + "x": 1.8703195977756357, + "y": 4.232058483296987, + "heading": -0.19340046314195958, + "angularVelocity": 1.9255930292876797, + "velocityX": 1.9468343347322845, + "velocityY": -0.45807861234460967, + "timestamp": 0.6093665745840936 + }, + { + "x": 2.0186115688161, + "y": 4.197166258130416, + "heading": -0.0721222371749164, + "angularVelocity": 1.592187442178866, + "velocityX": 1.946834332246427, + "velocityY": -0.45807862290950535, + "timestamp": 0.6855373964071053 + }, + { + "x": 2.1669035397570235, + "y": 4.162274032540797, + "heading": -1.3790420971273228e-7, + "angularVelocity": 0.946846804912877, + "velocityX": 1.9468343309396154, + "velocityY": -0.4580786284634526, + "timestamp": 0.761708218230117 + }, + { + "x": 2.299030158583636, + "y": 4.131185422616258, + "heading": -3.861728516505335e-8, + "angularVelocity": 0.0000013034771345390653, + "velocityX": 1.7346093381218648, + "velocityY": -0.4081432913612903, + "timestamp": 0.8378790400531287 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -2.2763209665573703e-18, + "angularVelocity": 5.069826509442209e-7, + "velocityX": 0.8673046903657866, + "velocityY": -0.2040716530068378, + "timestamp": 0.9140498618761405 + }, + { + "x": 2.365093469619751, + "y": 4.115641117095947, + "heading": -1.5143047799084299e-18, + "angularVelocity": 5.84054737228804e-20, + "velocityX": -8.219525464434245e-19, + "velocityY": 2.1312698857170775e-19, + "timestamp": 0.9902206836991522 + }, + { + "x": 2.299030158572247, + "y": 4.131185422567857, + "heading": -3.8824131952933046e-8, + "angularVelocity": -5.096982155619529e-7, + "velocityX": -0.867304690515307, + "velocityY": 0.20407165237139407, + "timestamp": 1.0663915055221642 + }, + { + "x": 2.166903539754318, + "y": 4.162274032529242, + "heading": -1.3861601297880188e-7, + "angularVelocity": -0.000001310106398423391, + "velocityX": -1.7346093380078471, + "velocityY": 0.408143291845037, + "timestamp": 1.1425623273451764 + }, + { + "x": 2.018611568604941, + "y": 4.197166257232936, + "heading": -0.0722676107720337, + "angularVelocity": -0.948755316359051, + "velocityX": -1.9468343336762697, + "velocityY": 0.4580786168326712, + "timestamp": 1.2187331491681885 + }, + { + "x": 1.8703195973960691, + "y": 4.2320584816837785, + "heading": -0.19373209136885944, + "angularVelocity": -1.5946326649731632, + "velocityX": -1.946834334457335, + "velocityY": 0.458078613513146, + "timestamp": 1.2949039709912007 + }, + { + "x": 1.7220276260831988, + "y": 4.2669507056926275, + "heading": -0.34047859723492613, + "angularVelocity": -1.9265448678897275, + "velocityX": -1.9468343358226654, + "velocityY": 0.45807860771049036, + "timestamp": 1.3710747928142129 + }, + { + "x": 1.5737356547013517, + "y": 4.301842929408325, + "heading": -0.4972933443760097, + "angularVelocity": -2.058724632188595, + "velocityX": -1.946834336728219, + "velocityY": 0.4580786038618867, + "timestamp": 1.447245614637225 + }, + { + "x": 1.4254436831497614, + "y": 4.336735152402613, + "heading": -0.6538038889287485, + "angularVelocity": -2.0547309429902363, + "velocityX": -1.9468343389566751, + "velocityY": 0.458078594390947, + "timestamp": 1.5234164364602372 + }, + { + "x": 1.2771517114677216, + "y": 4.371627374842491, + "heading": -0.799921474135194, + "angularVelocity": -1.9182881543008596, + "velocityX": -1.946834340669268, + "velocityY": 0.45807858711242555, + "timestamp": 1.5995872582832493 + }, + { + "x": 1.1288597396330466, + "y": 4.406519596633671, + "heading": -0.9209030529092569, + "angularVelocity": -1.5882929431321022, + "velocityX": -1.9468343426731174, + "velocityY": 0.4580785785960651, + "timestamp": 1.6757580801062615 + }, + { + "x": 0.9805677676343831, + "y": 4.4414118177279, + "heading": -0.9928951480324381, + "angularVelocity": -0.945140060198636, + "velocityX": -1.946834344826023, + "velocityY": 0.45807856944623543, + "timestamp": 1.7519289019292736 + }, + { + "x": 0.8484411497365192, + "y": 4.472500431600075, + "heading": -0.9928952477030265, + "angularVelocity": -0.0000013085140236118818, + "velocityX": -1.734609325928881, + "velocityY": 0.40814334318738743, + "timestamp": 1.8280997237522858 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": -5.09005494945374e-7, + "velocityX": -0.8673046852715057, + "velocityY": 0.2040716746574537, + "timestamp": 1.904270545575298 + }, + { + "x": 0.7823778390884398, + "y": 4.488044738769531, + "heading": -0.9928952864743934, + "angularVelocity": 1.8053835791622905e-18, + "velocityX": 3.8085009658841144e-17, + "velocityY": -4.441213712458509e-17, + "timestamp": 1.9804413673983101 + }, + { + "x": 0.8633075709529878, + "y": 4.398931081775789, + "heading": -0.9928954814598945, + "angularVelocity": -0.0000019220769098791852, + "velocityX": 0.7977678757913192, + "velocityY": -0.8784412255669002, + "timestamp": 2.0818865801229673 + }, + { + "x": 0.9997101217758085, + "y": 4.248734981396651, + "heading": -0.9705934228713969, + "angularVelocity": 0.21984338136319737, + "velocityX": 1.3445932751212721, + "velocityY": -1.480563708676927, + "timestamp": 2.1833317928476244 + }, + { + "x": 1.1361126726042652, + "y": 4.098538881022651, + "heading": -0.9135846602313676, + "angularVelocity": 0.5619660219429253, + "velocityX": 1.3445932751768286, + "velocityY": -1.4805637086262706, + "timestamp": 2.2847770055722814 + }, + { + "x": 1.2725152234348027, + "y": 3.948342780650542, + "heading": -0.8396943742506122, + "angularVelocity": 0.7283762732235443, + "velocityX": 1.3445932751973406, + "velocityY": -1.4805637086076424, + "timestamp": 2.3862222182969384 + }, + { + "x": 1.4089177742668983, + "y": 3.7981466802798476, + "heading": -0.7577094886902542, + "angularVelocity": 0.808169093034302, + "velocityX": 1.3445932752127017, + "velocityY": -1.4805637085936922, + "timestamp": 2.4876674310215954 + }, + { + "x": 1.5453203251000565, + "y": 3.6479505799101184, + "heading": -0.6718990052626181, + "angularVelocity": 0.8458800678997359, + "velocityX": 1.3445932752231742, + "velocityY": -1.4805637085841812, + "timestamp": 2.5891126437462524 + }, + { + "x": 1.6817228759337532, + "y": 3.4977544795408777, + "heading": -0.5843529548214927, + "angularVelocity": 0.8629884850135144, + "velocityX": 1.3445932752284844, + "velocityY": -1.4805637085793588, + "timestamp": 2.6905578564709094 + }, + { + "x": 1.8181254267674478, + "y": 3.3475583791716352, + "heading": -0.496152534626129, + "angularVelocity": 0.8694389594781329, + "velocityX": 1.3445932752284602, + "velocityY": -1.4805637085793808, + "timestamp": 2.7920030691955664 + }, + { + "x": 1.9545279776008493, + "y": 3.1973622788021268, + "heading": -0.4079812901328632, + "angularVelocity": 0.8691513588973434, + "velocityX": 1.3445932752255727, + "velocityY": -1.480563708582003, + "timestamp": 2.8934482819202234 + }, + { + "x": 2.0909305284339355, + "y": 3.0471661784323323, + "heading": -0.32051597252506797, + "angularVelocity": 0.8621926580724313, + "velocityX": 1.344593275222464, + "velocityY": -1.4805637085848262, + "timestamp": 2.9948934946448804 + }, + { + "x": 2.227333079266667, + "y": 2.896970078062216, + "heading": -0.234818517184951, + "angularVelocity": 0.8447658892757932, + "velocityX": 1.3445932752189704, + "velocityY": -1.4805637085879988, + "timestamp": 3.0963387073695374 + }, + { + "x": 2.3637356300989167, + "y": 2.7467739776916615, + "heading": -0.15295154788429333, + "angularVelocity": 0.8070067290692294, + "velocityX": 1.3445932752142213, + "velocityY": -1.480563708592312, + "timestamp": 3.1977839200941944 + }, + { + "x": 2.5001381809305556, + "y": 2.596577877320553, + "heading": -0.07915862654379939, + "angularVelocity": 0.7274164976200789, + "velocityX": 1.3445932752082013, + "velocityY": -1.4805637085977792, + "timestamp": 3.2992291328188514 + }, + { + "x": 2.6365407317614795, + "y": 2.4463817769487943, + "heading": -0.02222133180148921, + "angularVelocity": 0.5612615244531013, + "velocityX": 1.3445932752011478, + "velocityY": -1.480563708604185, + "timestamp": 3.4006743455435084 + }, + { + "x": 2.7729432825902887, + "y": 2.296185676575095, + "heading": 1.9540240946006604e-7, + "angularVelocity": 0.21904954021056072, + "velocityX": 1.3445932751803078, + "velocityY": -1.480563708623314, + "timestamp": 3.5021195582681655 + }, + { + "x": 2.8538730144500737, + "y": 2.207072019577026, + "heading": -5.5771741386668314e-18, + "angularVelocity": -0.0000019261866008177493, + "velocityX": 0.7977678757443631, + "velocityY": -0.8784412256095482, + "timestamp": 3.6035647709928225 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": -2.725395356358997e-18, + "angularVelocity": 1.2458293783490516e-18, + "velocityX": 3.57553609530669e-17, + "velocityY": -3.937144509458571e-17, + "timestamp": 3.7050099837174795 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.1.traj b/src/main/deploy/choreo/2piece.1.traj index ca8f8fe..78c44a6 100644 --- a/src/main/deploy/choreo/2piece.1.traj +++ b/src/main/deploy/choreo/2piece.1.traj @@ -1,76 +1,67 @@ { "samples": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": 2.630502167570374e-32, + "angularVelocity": 6.912068173828048e-33, + "velocityX": -4.041414755979078e-16, + "velocityY": 2.1900742120129258e-39, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 + "x": 1.4761905162579165, + "y": 5.567087650299072, + "heading": 5.081856100994774e-22, + "angularVelocity": 4.2976983347588495e-21, + "velocityX": 1.383114259492122, + "velocityY": 7.266238948856172e-27, + "timestamp": 0.11824262021257878 }, { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 + "x": 1.7126757532986059, + "y": 5.567087650299072, + "heading": 5.225371158203822e-19, + "angularVelocity": 4.415167063895265e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.427644052438308e-27, + "timestamp": 0.23648524042515756 }, { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 + "x": 1.9491609900561957, + "y": 5.567087650299072, + "heading": 2.6577036541275914e-19, + "angularVelocity": -2.1718062327712947e-18, + "velocityX": 1.9999999681274145, + "velocityY": 3.4263757854502784e-27, + "timestamp": 0.3547278606377363 }, { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 + "x": 2.1856462270968846, + "y": 5.567087650299072, + "heading": -1.0906583480085762e-21, + "angularVelocity": -2.2568820046674306e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.421212896814989e-27, + "timestamp": 0.4729704808503151 }, { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 + "x": 2.349189281463629, + "y": 5.567087650299072, + "heading": -3.2298015655990765e-32, + "angularVelocity": 9.223475207094912e-21, + "velocityX": 1.383114259492122, + "velocityY": -1.754147302356677e-26, + "timestamp": 0.5912131010628939 }, { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 - }, - { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, - "timestamp": 0.7789710677542292 + "x": 2.349189281463627, + "y": 5.567087650299072, + "heading": -3.648621613916173e-33, + "angularVelocity": 1.9825798352488026e-32, + "velocityX": -3.5425947355709975e-16, + "velocityY": -9.477719520863627e-38, + "timestamp": 0.7094557212754727 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.2.traj b/src/main/deploy/choreo/2piece.2.traj index e6599d4..1aeff89 100644 --- a/src/main/deploy/choreo/2piece.2.traj +++ b/src/main/deploy/choreo/2piece.2.traj @@ -1,76 +1,67 @@ { "samples": [ { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, + "x": 2.349189281463627, + "y": 5.567087650299072, + "heading": -3.648621613916173e-33, + "angularVelocity": 1.9825798352488026e-32, + "velocityX": -3.5425947355709975e-16, + "velocityY": -9.477719520863627e-38, "timestamp": 0 }, { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.1302635820892203 + "x": 2.185646227096883, + "y": 5.567087650299072, + "heading": 3.738718946616064e-21, + "angularVelocity": 3.1617850562356095e-20, + "velocityX": -1.3831142594921222, + "velocityY": 5.863050032870948e-30, + "timestamp": 0.1182426202125787 }, { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 0.2605271641784406 + "x": 1.9491609900561935, + "y": 5.567087650299072, + "heading": -1.3594265469642462e-18, + "angularVelocity": -1.1527879544319905e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.470823667011192e-30, + "timestamp": 0.2364852404251574 }, { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 0.39079074626766075 + "x": 1.7126757532986037, + "y": 5.567087650299072, + "heading": -1.1949667812351735e-18, + "angularVelocity": 1.3904520102372167e-18, + "velocityX": -1.9999999681274145, + "velocityY": 8.47082157931736e-30, + "timestamp": 0.3547278606377361 }, { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 0.5210543283568809 + "x": 1.4761905162579143, + "y": 5.567087650299072, + "heading": -5.013730755356182e-22, + "angularVelocity": 1.0101569711037546e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.47082075819828e-30, + "timestamp": 0.4729704808503149 }, { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 0.6513179104461011 + "x": 1.3126474618911703, + "y": 5.567087650299072, + "heading": -4.6245267198088095e-32, + "angularVelocity": 4.2399724814725485e-21, + "velocityX": -1.3831142594921215, + "velocityY": 5.85686190815934e-30, + "timestamp": 0.5912131010628937 }, { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 0.7815814925353213 - }, - { - "x": 1.3126474618909545, - "y": 5.56708765026724, - "heading": -1.4475075754332281e-12, - "angularVelocity": 4.040377540643928e-12, - "velocityX": 2.6238029906463054e-11, - "velocityY": 8.150524229191312e-12, - "timestamp": 0.9118450746245415 + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": -2.2656479665287125e-32, + "angularVelocity": 7.883926885959527e-33, + "velocityX": 3.7971136148997106e-16, + "velocityY": -9.967559307460824e-45, + "timestamp": 0.7094557212754725 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.traj b/src/main/deploy/choreo/2piece.traj index 745f113..bbcbc6f 100644 --- a/src/main/deploy/choreo/2piece.traj +++ b/src/main/deploy/choreo/2piece.traj @@ -1,139 +1,121 @@ { "samples": [ { - "x": 1.3126474618832242, - "y": 5.567087649501998, - "heading": 5.676683302944745e-13, - "angularVelocity": -3.43321425200581e-14, - "velocityX": 4.004374450026349e-9, - "velocityY": -2.9137762377339615e-10, + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": 2.630502167570374e-32, + "angularVelocity": 6.912068173828048e-33, + "velocityX": -4.041414755979078e-16, + "velocityY": 2.1900742120129258e-39, "timestamp": 0 }, { - "x": 1.4207148237590859, - "y": 5.56710281834894, - "heading": 0.0000017880981234128607, - "angularVelocity": 0.000016068220556833382, - "velocityX": 0.9711163411604299, - "velocityY": 0.000136317653921886, - "timestamp": 0.11128158110774704 + "x": 1.4761905162579165, + "y": 5.567087650299072, + "heading": 5.081856100994774e-22, + "angularVelocity": 4.2976983347588495e-21, + "velocityX": 1.383114259492122, + "velocityY": 7.266238948856172e-27, + "timestamp": 0.11824262021257878 }, { - "x": 1.605588153249726, - "y": 5.56713209169265, - "heading": 0.000005179600595136428, - "angularVelocity": 0.000030476758799414847, - "velocityX": 1.6613111321605605, - "velocityY": 0.00026306366761914195, - "timestamp": 0.22256316221549408 + "x": 1.7126757532986059, + "y": 5.567087650299072, + "heading": 5.225371158203822e-19, + "angularVelocity": 4.415167063895265e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.427644052438308e-27, + "timestamp": 0.23648524042515756 }, { - "x": 1.829753087040728, - "y": 5.567173626596197, - "heading": 0.000010321331376487245, - "angularVelocity": 0.00004620468334330266, - "velocityX": 2.0143938607602228, - "velocityY": 0.0003732486572643715, - "timestamp": 0.3338447433232411 + "x": 1.9491609900561957, + "y": 5.567087650299072, + "heading": 2.6577036541275914e-19, + "angularVelocity": -2.1718062327712947e-18, + "velocityX": 1.9999999681274145, + "velocityY": 3.4263757854502784e-27, + "timestamp": 0.3547278606377363 }, { - "x": 2.05465908394277, - "y": 5.567129102744167, - "heading": 0.000005603711217800806, - "angularVelocity": -0.00004239354511836126, - "velocityX": 2.0210532117804934, - "velocityY": -0.0004000936587489705, - "timestamp": 0.44512632443098815 + "x": 2.1856462270968846, + "y": 5.567087650299072, + "heading": -1.0906583480085762e-21, + "angularVelocity": -2.2568820046674306e-18, + "velocityX": 1.9999999705215028, + "velocityY": 3.421212896814989e-27, + "timestamp": 0.4729704808503151 }, { - "x": 2.240632817341497, - "y": 5.567101155314737, - "heading": 0.000002024117024409385, - "angularVelocity": -0.00003216700129094262, - "velocityX": 1.6711995961531687, - "velocityY": -0.0002511343930962879, - "timestamp": 0.5564079055387352 + "x": 2.349189281463629, + "y": 5.567087650299072, + "heading": -3.2298015655990765e-32, + "angularVelocity": 9.223475207094912e-21, + "velocityX": 1.383114259492122, + "velocityY": -1.754147302356677e-26, + "timestamp": 0.5912131010628939 }, { - "x": 2.3491892819318387, - "y": 5.567087651960309, - "heading": 1.4919057496986763e-12, - "angularVelocity": -0.00001818913858902976, - "velocityX": 0.9755115223704687, - "velocityY": -0.00012133685742246452, - "timestamp": 0.6676894866464822 + "x": 2.349189281463627, + "y": 5.567087650299072, + "heading": -3.648621613916173e-33, + "angularVelocity": 1.9825798352488026e-32, + "velocityX": -3.5425947355709975e-16, + "velocityY": -9.477719520863627e-38, + "timestamp": 0.7094557212754727 }, { - "x": 2.3491892814717987, - "y": 5.5670876511279825, - "heading": 1.8498540134297734e-12, - "angularVelocity": -1.8838121801786534e-12, - "velocityX": -4.062924748919285e-9, - "velocityY": -3.168063995066893e-10, - "timestamp": 0.7789710677542292 + "x": 2.185646227096883, + "y": 5.567087650299072, + "heading": 3.738718946616064e-21, + "angularVelocity": 3.1617850562356095e-20, + "velocityX": -1.3831142594921222, + "velocityY": 5.863050032870948e-30, + "timestamp": 0.8276983414880514 }, { - "x": 2.185107777385966, - "y": 5.57843523155288, - "heading": 0.013437034302228146, - "angularVelocity": 0.10315265465985932, - "velocityX": -1.259611485069916, - "velocityY": 0.08711245469129487, - "timestamp": 0.9092346498434495 + "x": 1.9491609900561935, + "y": 5.567087650299072, + "heading": -1.3594265469642462e-18, + "angularVelocity": -1.1527879544319905e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.470823667011192e-30, + "timestamp": 0.9459409617006301 }, { - "x": 1.9922338624025642, - "y": 5.574648054868595, - "heading": -0.003357780196661172, - "angularVelocity": -0.12892946924959994, - "velocityX": -1.4806434146069372, - "velocityY": -0.029073181128942617, - "timestamp": 1.0394982319326698 + "x": 1.7126757532986037, + "y": 5.567087650299072, + "heading": -1.1949667812351735e-18, + "angularVelocity": 1.3904520102372167e-18, + "velocityX": -1.9999999681274145, + "velocityY": 8.47082157931736e-30, + "timestamp": 1.0641835819132088 }, { - "x": 1.816249994866529, - "y": 5.572880218482232, - "heading": -0.012742251484704396, - "angularVelocity": -0.07204217125930834, - "velocityX": -1.3509828665358827, - "velocityY": -0.013571225283608969, - "timestamp": 1.16976181402189 + "x": 1.4761905162579143, + "y": 5.567087650299072, + "heading": -5.013730755356182e-22, + "angularVelocity": 1.0101569711037546e-17, + "velocityX": -1.9999999705215028, + "velocityY": 8.47082075819828e-30, + "timestamp": 1.1824262021257876 }, { - "x": 1.6467213961957556, - "y": 5.571788224133376, - "heading": -0.007226521308021137, - "angularVelocity": 0.04234284123533412, - "velocityX": -1.3014274285409717, - "velocityY": -0.008382959871005779, - "timestamp": 1.3000253961111101 + "x": 1.3126474618911703, + "y": 5.567087650299072, + "heading": -4.6245267198088095e-32, + "angularVelocity": 4.2399724814725485e-21, + "velocityX": -1.3831142594921215, + "velocityY": 5.85686190815934e-30, + "timestamp": 1.3006688223383664 }, { - "x": 1.4768616943564443, - "y": 5.569594921928596, - "heading": -0.007365237227012086, - "angularVelocity": -0.0010648864278731604, - "velocityX": -1.3039692223662636, - "velocityY": -0.01683741688944608, - "timestamp": 1.4302889782003303 - }, - { - "x": 1.3126474618873145, - "y": 5.567087650234358, - "heading": -3.6218014456567208e-12, - "angularVelocity": 0.056541030913362995, - "velocityX": -1.260630406715938, - "velocityY": -0.019247679867671577, - "timestamp": 1.5605525602895505 - }, - { - "x": 1.3126474618909545, - "y": 5.56708765026724, - "heading": -1.4475075754332281e-12, - "angularVelocity": 4.040377540643928e-12, - "velocityX": 2.6238029906463054e-11, - "velocityY": 8.150524229191312e-12, - "timestamp": 1.6908161423787706 + "x": 1.3126474618911723, + "y": 5.567087650299072, + "heading": -2.2656479665287125e-32, + "angularVelocity": 7.883926885959527e-33, + "velocityX": 3.7971136148997106e-16, + "velocityY": -9.967559307460824e-45, + "timestamp": 1.4189114425509453 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/exit center.1.traj b/src/main/deploy/choreo/exit center.1.traj new file mode 100644 index 0000000..bffdbd3 --- /dev/null +++ b/src/main/deploy/choreo/exit center.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "angularVelocity": -4.047258922723054e-33, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.3562741157973404, + "y": 5.551277015096179, + "heading": 5.1790019930154625e-8, + "angularVelocity": 8.222689572844604e-7, + "velocityX": 0.6926593812833762, + "velocityY": -0.2510250916993935, + "timestamp": 0.06298428215226594 + }, + { + "x": 1.4435274144279229, + "y": 5.519655747707938, + "heading": 1.8284200071758018e-7, + "angularVelocity": 0.0000020807092906136113, + "velocityX": 1.3853186167883211, + "velocityY": -0.5020501354892848, + "timestamp": 0.12596856430453188 + }, + { + "x": 1.561958503686593, + "y": 5.476735411831991, + "heading": 0.03758882643806014, + "angularVelocity": 0.5967940303771042, + "velocityX": 1.8803276819502406, + "velocityY": -0.6814451861527359, + "timestamp": 0.18895284645679783 + }, + { + "x": 1.6803895924155994, + "y": 5.433815074494518, + "heading": 0.1495361372904831, + "angularVelocity": 1.7773848812277921, + "velocityX": 1.8803276735407826, + "velocityY": -0.6814452093573423, + "timestamp": 0.25193712860906375 + }, + { + "x": 1.7988206811669072, + "y": 5.3908947372185825, + "heading": 0.3084692846412802, + "angularVelocity": 2.5233779273148262, + "velocityX": 1.8803276738948609, + "velocityY": -0.6814452083803143, + "timestamp": 0.3149214107613297 + }, + { + "x": 1.9172517698549563, + "y": 5.347974399768096, + "heading": 0.4903249170402497, + "angularVelocity": 2.887317695537583, + "velocityX": 1.880327672890501, + "velocityY": -0.6814452111516576, + "timestamp": 0.3779056929135956 + }, + { + "x": 2.0356828583945066, + "y": 5.305054061907854, + "heading": 0.6822100511004133, + "angularVelocity": 3.0465558628782476, + "velocityX": 1.880327670532791, + "velocityY": -0.6814452176573361, + "timestamp": 0.44088997506586153 + }, + { + "x": 2.1541139468698915, + "y": 5.26213372387056, + "heading": 0.8783830367201194, + "angularVelocity": 3.1146339835302603, + "velocityX": 1.880327669514046, + "velocityY": -0.6814452204683829, + "timestamp": 0.5038742572181275 + }, + { + "x": 2.272545035456946, + "y": 5.219213386141397, + "heading": 1.0737017740998906, + "angularVelocity": 3.1010711038602228, + "velocityX": 1.8803276712870123, + "velocityY": -0.6814452155761989, + "timestamp": 0.5668585393703934 + }, + { + "x": 2.390976124200322, + "y": 5.176293048843576, + "heading": 1.2594557238711797, + "angularVelocity": 2.949211190852737, + "velocityX": 1.8803276737689296, + "velocityY": -0.6814452087277921, + "timestamp": 0.6298428215226594 + }, + { + "x": 2.5094072129682594, + "y": 5.133372711613527, + "heading": 1.4204186158420298, + "angularVelocity": 2.5556041359925064, + "velocityX": 1.8803276741588917, + "velocityY": -0.6814452076517687, + "timestamp": 0.6928271036749253 + }, + { + "x": 2.627838301697845, + "y": 5.090452374277653, + "heading": 1.5329754550704864, + "angularVelocity": 1.787062349243704, + "velocityX": 1.880327673549982, + "velocityY": -0.6814452093319581, + "timestamp": 0.7558113858271912 + }, + { + "x": 2.746269390946397, + "y": 5.047532038373786, + "heading": 1.5707961445746523, + "angularVelocity": 0.6004782179263964, + "velocityX": 1.8803276817895906, + "velocityY": -0.6814451865960204, + "timestamp": 0.8187956679794571 + }, + { + "x": 2.8335226895825705, + "y": 5.015910771000973, + "heading": 1.570796275196268, + "angularVelocity": 0.000002073876396402678, + "velocityX": 1.3853186168770866, + "velocityY": -0.5020501352443532, + "timestamp": 0.8817799501317231 + }, + { + "x": 2.8771493434906006, + "y": 5.000100135803223, + "heading": 1.5707963267948966, + "angularVelocity": 8.19230237095319e-7, + "velocityX": 0.6926593813129686, + "velocityY": -0.25102509161774106, + "timestamp": 0.944764232283989 + }, + { + "x": 2.8771493434906006, + "y": 5.000100135803223, + "heading": 1.5707963267948966, + "angularVelocity": 0, + "velocityX": -1.88980553526586e-32, + "velocityY": 0, + "timestamp": 1.007748514436255 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/exit center.traj b/src/main/deploy/choreo/exit center.traj new file mode 100644 index 0000000..bffdbd3 --- /dev/null +++ b/src/main/deploy/choreo/exit center.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "angularVelocity": -4.047258922723054e-33, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.3562741157973404, + "y": 5.551277015096179, + "heading": 5.1790019930154625e-8, + "angularVelocity": 8.222689572844604e-7, + "velocityX": 0.6926593812833762, + "velocityY": -0.2510250916993935, + "timestamp": 0.06298428215226594 + }, + { + "x": 1.4435274144279229, + "y": 5.519655747707938, + "heading": 1.8284200071758018e-7, + "angularVelocity": 0.0000020807092906136113, + "velocityX": 1.3853186167883211, + "velocityY": -0.5020501354892848, + "timestamp": 0.12596856430453188 + }, + { + "x": 1.561958503686593, + "y": 5.476735411831991, + "heading": 0.03758882643806014, + "angularVelocity": 0.5967940303771042, + "velocityX": 1.8803276819502406, + "velocityY": -0.6814451861527359, + "timestamp": 0.18895284645679783 + }, + { + "x": 1.6803895924155994, + "y": 5.433815074494518, + "heading": 0.1495361372904831, + "angularVelocity": 1.7773848812277921, + "velocityX": 1.8803276735407826, + "velocityY": -0.6814452093573423, + "timestamp": 0.25193712860906375 + }, + { + "x": 1.7988206811669072, + "y": 5.3908947372185825, + "heading": 0.3084692846412802, + "angularVelocity": 2.5233779273148262, + "velocityX": 1.8803276738948609, + "velocityY": -0.6814452083803143, + "timestamp": 0.3149214107613297 + }, + { + "x": 1.9172517698549563, + "y": 5.347974399768096, + "heading": 0.4903249170402497, + "angularVelocity": 2.887317695537583, + "velocityX": 1.880327672890501, + "velocityY": -0.6814452111516576, + "timestamp": 0.3779056929135956 + }, + { + "x": 2.0356828583945066, + "y": 5.305054061907854, + "heading": 0.6822100511004133, + "angularVelocity": 3.0465558628782476, + "velocityX": 1.880327670532791, + "velocityY": -0.6814452176573361, + "timestamp": 0.44088997506586153 + }, + { + "x": 2.1541139468698915, + "y": 5.26213372387056, + "heading": 0.8783830367201194, + "angularVelocity": 3.1146339835302603, + "velocityX": 1.880327669514046, + "velocityY": -0.6814452204683829, + "timestamp": 0.5038742572181275 + }, + { + "x": 2.272545035456946, + "y": 5.219213386141397, + "heading": 1.0737017740998906, + "angularVelocity": 3.1010711038602228, + "velocityX": 1.8803276712870123, + "velocityY": -0.6814452155761989, + "timestamp": 0.5668585393703934 + }, + { + "x": 2.390976124200322, + "y": 5.176293048843576, + "heading": 1.2594557238711797, + "angularVelocity": 2.949211190852737, + "velocityX": 1.8803276737689296, + "velocityY": -0.6814452087277921, + "timestamp": 0.6298428215226594 + }, + { + "x": 2.5094072129682594, + "y": 5.133372711613527, + "heading": 1.4204186158420298, + "angularVelocity": 2.5556041359925064, + "velocityX": 1.8803276741588917, + "velocityY": -0.6814452076517687, + "timestamp": 0.6928271036749253 + }, + { + "x": 2.627838301697845, + "y": 5.090452374277653, + "heading": 1.5329754550704864, + "angularVelocity": 1.787062349243704, + "velocityX": 1.880327673549982, + "velocityY": -0.6814452093319581, + "timestamp": 0.7558113858271912 + }, + { + "x": 2.746269390946397, + "y": 5.047532038373786, + "heading": 1.5707961445746523, + "angularVelocity": 0.6004782179263964, + "velocityX": 1.8803276817895906, + "velocityY": -0.6814451865960204, + "timestamp": 0.8187956679794571 + }, + { + "x": 2.8335226895825705, + "y": 5.015910771000973, + "heading": 1.570796275196268, + "angularVelocity": 0.000002073876396402678, + "velocityX": 1.3853186168770866, + "velocityY": -0.5020501352443532, + "timestamp": 0.8817799501317231 + }, + { + "x": 2.8771493434906006, + "y": 5.000100135803223, + "heading": 1.5707963267948966, + "angularVelocity": 8.19230237095319e-7, + "velocityX": 0.6926593813129686, + "velocityY": -0.25102509161774106, + "timestamp": 0.944764232283989 + }, + { + "x": 2.8771493434906006, + "y": 5.000100135803223, + "heading": 1.5707963267948966, + "angularVelocity": 0, + "velocityX": -1.88980553526586e-32, + "velocityY": 0, + "timestamp": 1.007748514436255 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/exit.1.traj b/src/main/deploy/choreo/exit.1.traj deleted file mode 100644 index 1f03f2c..0000000 --- a/src/main/deploy/choreo/exit.1.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": -1.6516987428817835e-31, - "angularVelocity": 3.365627186718647e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3392991160266923, - "y": 5.578340379347365, - "heading": 0.05084527674264656, - "angularVelocity": 0.9301054243469516, - "velocityX": 0.48753492295327294, - "velocityY": 0.20584457391191183, - "timestamp": 0.05466614365607662 - }, - { - "x": 1.3926138209310608, - "y": 5.602095558464793, - "heading": 0.15090427909370982, - "angularVelocity": 1.8303651155744338, - "velocityX": 0.9752783229010918, - "velocityY": 0.4345501169221013, - "timestamp": 0.10933228731215323 - }, - { - "x": 1.4727419129112613, - "y": 5.6401417619413445, - "heading": 0.29625295514716404, - "angularVelocity": 2.6588426827378284, - "velocityX": 1.4657718035556655, - "velocityY": 0.6959737953332358, - "timestamp": 0.16399843096822986 - }, - { - "x": 1.5806463473229446, - "y": 5.695327975123219, - "heading": 0.473406200223283, - "angularVelocity": 3.2406391456959285, - "velocityX": 1.973880489733221, - "velocityY": 1.0095135579540684, - "timestamp": 0.21866457462430647 - }, - { - "x": 1.712426609279705, - "y": 5.771939883643632, - "heading": 0.6149480873197845, - "angularVelocity": 2.589205633142697, - "velocityX": 2.4106376112029215, - "velocityY": 1.4014507590366152, - "timestamp": 0.2733307182803831 - }, - { - "x": 1.8646157040757754, - "y": 5.861735120575505, - "heading": 0.6843254516381283, - "angularVelocity": 1.2691102696912473, - "velocityX": 2.7839734910430862, - "velocityY": 1.6426115128369911, - "timestamp": 0.3279968619364597 - }, - { - "x": 2.0394443509304283, - "y": 5.959373498780485, - "heading": 0.6843306271163396, - "angularVelocity": 0.00009467428768829868, - "velocityX": 3.1981156006642792, - "velocityY": 1.7860849819452436, - "timestamp": 0.38266300559253635 - }, - { - "x": 2.2170155057005516, - "y": 6.0519338232599695, - "heading": 0.6843311645173596, - "angularVelocity": 0.000009830600514719364, - "velocityX": 3.2482839083598782, - "velocityY": 1.6931928665357103, - "timestamp": 0.437329149248613 - }, - { - "x": 2.379893345636442, - "y": 6.14773810700836, - "heading": 0.7173523116229228, - "angularVelocity": 0.6040511530008493, - "velocityX": 2.97950118743716, - "velocityY": 1.7525341526032563, - "timestamp": 0.49199529290468963 - }, - { - "x": 2.5210272079944938, - "y": 6.230636536851746, - "heading": 0.8166805533527952, - "angularVelocity": 1.8169974153432211, - "velocityX": 2.581741694566448, - "velocityY": 1.5164492005312948, - "timestamp": 0.5466614365607663 - }, - { - "x": 2.6419948699380145, - "y": 6.295385989559359, - "heading": 0.9758437840679963, - "angularVelocity": 2.9115503686623883, - "velocityX": 2.2128442552042693, - "velocityY": 1.1844525400396748, - "timestamp": 0.6013275802168428 - }, - { - "x": 2.744635840082322, - "y": 6.338256745744779, - "heading": 1.187997716756794, - "angularVelocity": 3.8809017519788993, - "velocityX": 1.8775966856205815, - "velocityY": 0.7842286526581116, - "timestamp": 0.6559937238729194 - }, - { - "x": 2.819689210716326, - "y": 6.364168809782246, - "heading": 1.3690565756448358, - "angularVelocity": 3.312083984323915, - "velocityX": 1.3729406468872292, - "velocityY": 0.474005706356175, - "timestamp": 0.710659867528996 - }, - { - "x": 2.867851682057744, - "y": 6.37891492245815, - "heading": 1.5017329656570861, - "angularVelocity": 2.427030354417588, - "velocityX": 0.881029246262995, - "velocityY": 0.2697485443399293, - "timestamp": 0.7653260111850726 - }, - { - "x": 2.891404867172241, - "y": 6.385702610015869, - "heading": 1.5707963267948966, - "angularVelocity": 1.2633662541172024, - "velocityX": 0.4308550693218517, - "velocityY": 0.12416620423096118, - "timestamp": 0.8199921548411492 - }, - { - "x": 2.891404867172241, - "y": 6.385702610015869, - "heading": 1.5707963267948966, - "angularVelocity": 1.5542922184430967e-29, - "velocityX": -2.3304215196416068e-31, - "velocityY": 0, - "timestamp": 0.8746582984972258 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/exit.traj b/src/main/deploy/choreo/exit.traj deleted file mode 100644 index 1f03f2c..0000000 --- a/src/main/deploy/choreo/exit.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": -1.6516987428817835e-31, - "angularVelocity": 3.365627186718647e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.3392991160266923, - "y": 5.578340379347365, - "heading": 0.05084527674264656, - "angularVelocity": 0.9301054243469516, - "velocityX": 0.48753492295327294, - "velocityY": 0.20584457391191183, - "timestamp": 0.05466614365607662 - }, - { - "x": 1.3926138209310608, - "y": 5.602095558464793, - "heading": 0.15090427909370982, - "angularVelocity": 1.8303651155744338, - "velocityX": 0.9752783229010918, - "velocityY": 0.4345501169221013, - "timestamp": 0.10933228731215323 - }, - { - "x": 1.4727419129112613, - "y": 5.6401417619413445, - "heading": 0.29625295514716404, - "angularVelocity": 2.6588426827378284, - "velocityX": 1.4657718035556655, - "velocityY": 0.6959737953332358, - "timestamp": 0.16399843096822986 - }, - { - "x": 1.5806463473229446, - "y": 5.695327975123219, - "heading": 0.473406200223283, - "angularVelocity": 3.2406391456959285, - "velocityX": 1.973880489733221, - "velocityY": 1.0095135579540684, - "timestamp": 0.21866457462430647 - }, - { - "x": 1.712426609279705, - "y": 5.771939883643632, - "heading": 0.6149480873197845, - "angularVelocity": 2.589205633142697, - "velocityX": 2.4106376112029215, - "velocityY": 1.4014507590366152, - "timestamp": 0.2733307182803831 - }, - { - "x": 1.8646157040757754, - "y": 5.861735120575505, - "heading": 0.6843254516381283, - "angularVelocity": 1.2691102696912473, - "velocityX": 2.7839734910430862, - "velocityY": 1.6426115128369911, - "timestamp": 0.3279968619364597 - }, - { - "x": 2.0394443509304283, - "y": 5.959373498780485, - "heading": 0.6843306271163396, - "angularVelocity": 0.00009467428768829868, - "velocityX": 3.1981156006642792, - "velocityY": 1.7860849819452436, - "timestamp": 0.38266300559253635 - }, - { - "x": 2.2170155057005516, - "y": 6.0519338232599695, - "heading": 0.6843311645173596, - "angularVelocity": 0.000009830600514719364, - "velocityX": 3.2482839083598782, - "velocityY": 1.6931928665357103, - "timestamp": 0.437329149248613 - }, - { - "x": 2.379893345636442, - "y": 6.14773810700836, - "heading": 0.7173523116229228, - "angularVelocity": 0.6040511530008493, - "velocityX": 2.97950118743716, - "velocityY": 1.7525341526032563, - "timestamp": 0.49199529290468963 - }, - { - "x": 2.5210272079944938, - "y": 6.230636536851746, - "heading": 0.8166805533527952, - "angularVelocity": 1.8169974153432211, - "velocityX": 2.581741694566448, - "velocityY": 1.5164492005312948, - "timestamp": 0.5466614365607663 - }, - { - "x": 2.6419948699380145, - "y": 6.295385989559359, - "heading": 0.9758437840679963, - "angularVelocity": 2.9115503686623883, - "velocityX": 2.2128442552042693, - "velocityY": 1.1844525400396748, - "timestamp": 0.6013275802168428 - }, - { - "x": 2.744635840082322, - "y": 6.338256745744779, - "heading": 1.187997716756794, - "angularVelocity": 3.8809017519788993, - "velocityX": 1.8775966856205815, - "velocityY": 0.7842286526581116, - "timestamp": 0.6559937238729194 - }, - { - "x": 2.819689210716326, - "y": 6.364168809782246, - "heading": 1.3690565756448358, - "angularVelocity": 3.312083984323915, - "velocityX": 1.3729406468872292, - "velocityY": 0.474005706356175, - "timestamp": 0.710659867528996 - }, - { - "x": 2.867851682057744, - "y": 6.37891492245815, - "heading": 1.5017329656570861, - "angularVelocity": 2.427030354417588, - "velocityX": 0.881029246262995, - "velocityY": 0.2697485443399293, - "timestamp": 0.7653260111850726 - }, - { - "x": 2.891404867172241, - "y": 6.385702610015869, - "heading": 1.5707963267948966, - "angularVelocity": 1.2633662541172024, - "velocityX": 0.4308550693218517, - "velocityY": 0.12416620423096118, - "timestamp": 0.8199921548411492 - }, - { - "x": 2.891404867172241, - "y": 6.385702610015869, - "heading": 1.5707963267948966, - "angularVelocity": 1.5542922184430967e-29, - "velocityX": -2.3304215196416068e-31, - "velocityY": 0, - "timestamp": 0.8746582984972258 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece choreo.auto b/src/main/deploy/pathplanner/autos/1 piece center.auto similarity index 87% rename from src/main/deploy/pathplanner/autos/1 piece choreo.auto rename to src/main/deploy/pathplanner/autos/1 piece center.auto index f1c0620..089b11e 100644 --- a/src/main/deploy/pathplanner/autos/1 piece choreo.auto +++ b/src/main/deploy/pathplanner/autos/1 piece center.auto @@ -5,7 +5,7 @@ "x": 1.3126474618911743, "y": 5.567087650299072 }, - "rotation": -9.463536699418992e-30 + "rotation": -8.345126428911687e-30 }, "command": { "type": "sequential", @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "OptDelay" + "name": "Options" } }, { @@ -38,7 +38,7 @@ { "type": "path", "data": { - "pathName": "exit.1" + "pathName": "exit center.1" } } ] diff --git a/src/main/deploy/pathplanner/autos/2piece.auto b/src/main/deploy/pathplanner/autos/1 piece left.auto similarity index 62% rename from src/main/deploy/pathplanner/autos/2piece.auto rename to src/main/deploy/pathplanner/autos/1 piece left.auto index ba9377f..b0179a4 100644 --- a/src/main/deploy/pathplanner/autos/2piece.auto +++ b/src/main/deploy/pathplanner/autos/1 piece left.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.29, - "y": 5.57 + "x": 0.7669039964675903, + "y": 6.639083385467529 }, - "rotation": 0.0 + "rotation": 57.72434388873349 }, "command": { "type": "sequential", @@ -14,42 +14,36 @@ { "type": "named", "data": { - "name": "AimAndShoot" + "name": "Options" } }, { "type": "named", "data": { - "name": "Intake" + "name": "SpeakerSetting" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "2piece.1" + "name": "Index" } }, { "type": "named", "data": { - "name": "WaitIntake" + "name": "ResetScoring" } }, { "type": "path", "data": { - "pathName": "2piece.2" - } - }, - { - "type": "named", - "data": { - "name": "AimAndShoot" + "pathName": "2 piece left.3" } } ] } }, "folder": null, - "choreoAuto": false + "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece no motion.auto b/src/main/deploy/pathplanner/autos/1 piece no motion.auto index 87f35e9..bed3990 100644 --- a/src/main/deploy/pathplanner/autos/1 piece no motion.auto +++ b/src/main/deploy/pathplanner/autos/1 piece no motion.auto @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "OptDelay" + "name": "Options" } }, { diff --git a/src/main/deploy/pathplanner/autos/1 piece right.auto b/src/main/deploy/pathplanner/autos/1 piece right.auto new file mode 100644 index 0000000..b75e784 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 piece right.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.78237783908844, + "y": 4.488044738769531 + }, + "rotation": -56.888709413415555 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2piece choreo.auto b/src/main/deploy/pathplanner/autos/2 piece center.auto similarity index 97% rename from src/main/deploy/pathplanner/autos/2piece choreo.auto rename to src/main/deploy/pathplanner/autos/2 piece center.auto index 25f3a6d..628279c 100644 --- a/src/main/deploy/pathplanner/autos/2piece choreo.auto +++ b/src/main/deploy/pathplanner/autos/2 piece center.auto @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "OptDelay" + "name": "Options" } }, { diff --git a/src/main/deploy/pathplanner/autos/2 piece left.auto b/src/main/deploy/pathplanner/autos/2 piece left.auto new file mode 100644 index 0000000..dac4146 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 piece left.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7669039964675903, + "y": 6.639083385467529 + }, + "rotation": 57.72434388873349 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.1" + } + }, + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.2" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 piece right.auto b/src/main/deploy/pathplanner/autos/2 piece right.auto new file mode 100644 index 0000000..7610586 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 piece right.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7823778390884399, + "y": 4.488044738769531 + }, + "rotation": -56.888709413415555 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.1" + } + }, + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.2" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/no piece choreo.auto b/src/main/deploy/pathplanner/autos/no piece center.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/no piece choreo.auto rename to src/main/deploy/pathplanner/autos/no piece center.auto index fe91be5..9ebf447 100644 --- a/src/main/deploy/pathplanner/autos/no piece choreo.auto +++ b/src/main/deploy/pathplanner/autos/no piece center.auto @@ -5,16 +5,22 @@ "x": 1.3126474618911743, "y": 5.567087650299072 }, - "rotation": -9.463536699418992e-30 + "rotation": -8.345126428911687e-30 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, { "type": "path", "data": { - "pathName": "exit.1" + "pathName": "exit center.1" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fddbef2..ea38e42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,7 +103,10 @@ public RobotContainer() { final var driveTab = Shuffleboard.getTab(ShuffleboardConstants.driveTab); var delayEntry = driveTab.add("Delay Auto?", false).withWidget(BuiltInWidgets.kToggleSwitch).withSize(3, 1) + .withPosition(0, 6) .getEntry(); + var noteStartEntry = driveTab.add("Starting with Note?", false).withWidget(BuiltInWidgets.kToggleSwitch) + .withSize(3, 1).withPosition(0, 7).getEntry(); driveTab.add("auto routine", autoChooser.getSendableChooser()) .withSize(4, 1).withPosition(0, 5); driveTab.add("alerts", Alert.getAlertsSendable()) @@ -114,22 +117,27 @@ public RobotContainer() { .withSize(11, 5) .withPosition(0, 0); + Command shuffleboardAutoOptions = Commands.parallel( + Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0), + Commands.runOnce(() -> intake.resetNoteDetection(true))); + // named commands must be registered before any paths are created - NamedCommands.registerCommand("OptDelay", Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0)); + NamedCommands.registerCommand("Options", shuffleboardAutoOptions); NamedCommands.registerCommand("XStance", new XStance(swerve)); NamedCommands.registerCommand("SpeakerAlign", new AprilTagVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("SpeakerSetting", new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); NamedCommands.registerCommand("LimelightSetting", - new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_map)); - NamedCommands.registerCommand("WaitIntake", new IntakeWithArm(intake, arm)); + new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); + NamedCommands.registerCommand("WaitIntake", + new IntakeWithArm(intake, arm).beforeStarting(() -> intake.resetNoteDetection(), intake)); NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(1)); NamedCommands.registerCommand("ArmDown", new SetArmPosition(arm, 4)); NamedCommands.registerCommand("AmpSetting", new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); NamedCommands.registerCommand("SlowForward", Commands.startEnd(() -> swerve.drive(new Translation2d(.3, 0), 0, false), () -> swerve.stop(), swerve) .withTimeout(1.5)); - NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, new ScoringPosition(4, 0))); + NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0))); // create paths final List autoNames = AutoBuilder.getAllAutoNames(); for (final String autoName : autoNames) { @@ -197,16 +205,18 @@ private void configureBindings() { operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); operatorController.x().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0))); - operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_map)); + operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 40)); operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); - operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); + // operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); operatorController.povRight().onTrue(Commands.runOnce(() -> intake.resetNoteDetection())); + operatorController.start().toggleOnTrue(Commands.startEnd(() -> arm.setMotorVoltage(-3.7), () -> arm.stop(), arm)); + operatorController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); operatorController.rightStick().whileTrue(new ManualArmControl(arm, operatorController::getLeftY)); diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 6ee240b..b7a6130 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .705), - new SwerveModuleConstants(23, 24, 1.714), - new SwerveModuleConstants(25, 26, 3.406), - new SwerveModuleConstants(27, 28, 2.893) + new SwerveModuleConstants(21, 22, .695), + new SwerveModuleConstants(23, 24, 1.775), + new SwerveModuleConstants(25, 26, 3.369), + new SwerveModuleConstants(27, 28, 2.863) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center @@ -42,9 +42,9 @@ public final class SwerveConstants { public static final double angleRampTime_s = 0.5; public static final boolean angleEncoderInverted = false; - public static final double maxVelocity_mps = 4704 /* NEO max RPM */ + public static final double maxVelocity_mps = 5650 /* NEO max RPM */ / 60.0 / driveGearRatio * wheelCircumference_m; - public static final double maxAngularVelocity_radps = 4704 /* NEO max RPM */ + public static final double maxAngularVelocity_radps = 5650 /* NEO max RPM */ / 60.0 / angleGearRatio / Math.hypot(trackWidth_m / 2.0, wheelBase_m / 2.0); // calculated with choreo, todo: see if empirical can match these diff --git a/src/main/java/frc/robot/io/PieceVisionIO.java b/src/main/java/frc/robot/io/PieceVisionIO.java index 23feac8..7e577e6 100644 --- a/src/main/java/frc/robot/io/PieceVisionIO.java +++ b/src/main/java/frc/robot/io/PieceVisionIO.java @@ -17,7 +17,6 @@ public class PieceVisionIO { public static class PieceVisionIOInputs { public int pipeline = 0; public boolean hasTarget = false; - public int targetAprilTag = 0; public double crosshairToTargetErrorX_rad = 0.0; public double crosshairToTargetErrorY_rad = 0.0; @@ -46,7 +45,6 @@ public static class PieceVisionIOInputs { private NetworkTableEntry pipelineEntry = table.getEntry("pipeline"); private NetworkTableEntry tvEntry = table.getEntry("tv"); - private NetworkTableEntry tidEntry = table.getEntry("tid"); private NetworkTableEntry txEntry = table.getEntry("tx"); private NetworkTableEntry tyEntry = table.getEntry("ty"); @@ -61,7 +59,6 @@ public static class PieceVisionIOInputs { public void updateInputs(PieceVisionIOInputs inputs) { inputs.pipeline = pipelineEntry.getNumber(inputs.pipeline).intValue(); inputs.hasTarget = tvEntry.getDouble(0) == 1; - inputs.targetAprilTag = tidEntry.getNumber(inputs.targetAprilTag).intValue(); inputs.crosshairToTargetErrorX_rad = Math.toRadians(txEntry.getDouble(inputs.crosshairToTargetErrorX_rad)); inputs.crosshairToTargetErrorY_rad = Math.toRadians(tyEntry.getDouble(inputs.crosshairToTargetErrorY_rad)); inputs.targetArea = taEntry.getDouble(inputs.targetArea); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 9b53502..340bcca 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -17,7 +17,6 @@ import org.littletonrobotics.junction.Logger; import lombok.Getter; -import lombok.Setter; public class Intake extends SubsystemBase { private final CANSparkMax motor = new CANSparkMax(motorId, MotorType.kBrushless); @@ -25,10 +24,8 @@ public class Intake extends SubsystemBase { private final DigitalInput noteSensor = new DigitalInput(sensorId); @Getter - @Setter @AutoLogOutput private boolean holdingNote = false; - private boolean cachedNoteSensor = false; public Intake() { @@ -58,9 +55,20 @@ public void setMotorSpeed(double speed) { motor.setVoltage(speed * 12); } + /** + * Manually set the note sensor and corresponding holding logic to a specific value + * @param value + */ + public void resetNoteDetection(boolean value) { + holdingNote = value; + cachedNoteSensor = value; + } + + /** + * Default version of {@link Intake#resetNoteDetection(boolean)} with false as the default value. + */ public void resetNoteDetection() { - holdingNote = false; - cachedNoteSensor = false; + resetNoteDetection(false); } @AutoLog From d966584d51b7284791287685ac6e93fe683101e4 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sat, 16 Mar 2024 10:00:42 -0400 Subject: [PATCH 13/22] forgor to stage this --- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/constants/VisionConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea38e42..126fa01 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,6 +125,7 @@ public RobotContainer() { NamedCommands.registerCommand("Options", shuffleboardAutoOptions); NamedCommands.registerCommand("XStance", new XStance(swerve)); NamedCommands.registerCommand("SpeakerAlign", new AprilTagVision(vision, swerve).withTimeout(1)); + NamedCommands.registerCommand("NoteAlign", new GamePieceVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("SpeakerSetting", new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); NamedCommands.registerCommand("LimelightSetting", diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 85568e0..e7b54bf 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -6,8 +6,8 @@ public class VisionConstants { public static final double poseError_m = 1; // comparing visionPose to pose public static final double botpose_fieldOffsetX_m = 0.025; // realife offset to pathplanner app /* ObjectDetectionVision */ - public static final double visionRotateKp = 1.75; - public static final double visionRotateKd = 1.4; + public static final double visionRotateKp = 1.90; + public static final double visionRotateKd = 1.5; // public static final double AprilTagRotateKp = 1.2; // public static final double AprilTagRotateKd = 0.34; From 6e6994aff345e84e9333618b72f7b4204a82a700 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sat, 16 Mar 2024 10:37:47 -0400 Subject: [PATCH 14/22] reset lights command and do it in init --- src/main/java/frc/robot/Robot.java | 29 ++++++++++--------- .../robot/commands/intake/IntakeWithArm.java | 4 ++- .../java/frc/robot/subsystems/Lights.java | 20 +++++++++++++ 3 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aa30441..d3c0548 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,6 +3,7 @@ import static frc.robot.constants.Constants.*; import frc.robot.constants.BuildConstants; +import frc.robot.constants.LightsConstants; import frc.robot.subsystems.Lights; import frc.robot.util.AutoSetterTunableNumber.AutoSetterTunableNumberManager; import frc.robot.util.VirtualSubsystem; @@ -30,8 +31,6 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import java.util.Optional; - /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -130,12 +129,13 @@ public void robotPeriodic() { @Override public void disabledInit() { robotContainer.swerve.stop(); + Lights.setStatus(LightsConstants.Mode.Disabled); } @Override public void disabledPeriodic() { robotContainer.updateOIAlert(); - Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Disabled); + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @@ -147,20 +147,22 @@ public void autonomousInit() { if (autonomousCommand != null) { autonomousCommand.schedule(); } - - alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; + + alliance = DriverStation.getAlliance().isPresent() ? DriverStation.getAlliance().get() : Alliance.Blue; // for limelight shooting targeting switch (alliance) { case Red -> robotContainer.vision.setPriorityId(4); case Blue -> robotContainer.vision.setPriorityId(7); } + + Lights.setStatus(LightsConstants.Mode.Auto); } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { - Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Auto); + } @Override @@ -179,19 +181,20 @@ public void teleopInit() { // for limelight shooting targeting switch (alliance) { - case Red -> robotContainer.vision.setPriorityId(4); - case Blue -> robotContainer.vision.setPriorityId(7); + case Red -> { + robotContainer.vision.setPriorityId(4); + Lights.setStatus(LightsConstants.Mode.TeleopRED); + } + case Blue -> { + robotContainer.vision.setPriorityId(7); + Lights.setStatus(LightsConstants.Mode.TeleopBLUE); + } } } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - if (alliance == Alliance.Red) { - Lights.setStatus(frc.robot.constants.LightsConstants.Mode.TeleopRED); - } else { - Lights.setStatus(frc.robot.constants.LightsConstants.Mode.TeleopBLUE); - } } diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index 4ebfb57..7421c4a 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -1,6 +1,7 @@ package frc.robot.commands.intake; import frc.robot.constants.IntakeConstants; +import frc.robot.constants.LightsConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Lights; @@ -23,7 +24,7 @@ public void initialize() { if (!intake.isHoldingNote()) { arm.setPosition(-1.5); intake.setMotorSpeed(IntakeConstants.intakeSpeed); - Lights.setStatus(frc.robot.constants.LightsConstants.Mode.Intaking); + Lights.setStatus(LightsConstants.Mode.Intaking); } } @@ -36,6 +37,7 @@ public void execute() {} public void end(boolean interrupted) { arm.setPosition(4); intake.stopMotor(); + Lights.setDefaultStatus(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index 5ed7c0a..ad1c70b 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -6,6 +6,7 @@ import frc.robot.constants.LightsConstants.Mode; import edu.wpi.first.wpilibj.DigitalOutput; +import edu.wpi.first.wpilibj.DriverStation; import org.littletonrobotics.junction.Logger; @@ -30,4 +31,23 @@ public static void setStatus(Mode mode) { Logger.recordOutput("Lights/CurrentStatus", mode); } + + public static void setDefaultStatus() { + if (DriverStation.isDisabled()) { + setStatus(Mode.Disabled); + return; + } + if (DriverStation.isAutonomous()) { + setStatus(Mode.Auto); + return; + } + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + switch (alliance.get()) { + case Red -> setStatus(Mode.TeleopRED); + case Blue -> setStatus(Mode.TeleopBLUE); + } + } + } } From eb45a63545d8e94b16d52002677eecf24c2c4d60 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sat, 16 Mar 2024 14:00:10 -0400 Subject: [PATCH 15/22] auto updates and yaw for red side (no time for testing lets hope!) --- choreo.chor | 272 +++++++ src/main/deploy/choreo/amp.1.traj | 220 ++++++ src/main/deploy/choreo/amp.traj | 220 ++++++ .../pathplanner/autos/1 piece amp shot.auto | 55 ++ ... piece left.auto => 1 piece amp side.auto} | 0 ...ce right.auto => 1 piece source side.auto} | 0 ... piece left.auto => 2 piece amp side.auto} | 0 ...ce right.auto => 2 piece source side.auto} | 0 src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/constants/SwerveConstants.java | 8 +- .../java/frc/robot/subsystems/Lights.java | 6 +- .../java/frc/robot/subsystems/Swerve.java | 15 +- vendordeps/PathplannerLib.json | 72 +- vendordeps/Phoenix6.json | 674 +++++++++--------- vendordeps/REVLib.json | 144 ++-- vendordeps/URCL.json | 126 ++-- 16 files changed, 1294 insertions(+), 520 deletions(-) create mode 100644 src/main/deploy/choreo/amp.1.traj create mode 100644 src/main/deploy/choreo/amp.traj create mode 100644 src/main/deploy/pathplanner/autos/1 piece amp shot.auto rename src/main/deploy/pathplanner/autos/{1 piece left.auto => 1 piece amp side.auto} (100%) rename src/main/deploy/pathplanner/autos/{1 piece right.auto => 1 piece source side.auto} (100%) rename src/main/deploy/pathplanner/autos/{2 piece left.auto => 2 piece amp side.auto} (100%) rename src/main/deploy/pathplanner/autos/{2 piece right.auto => 2 piece source side.auto} (100%) diff --git a/choreo.chor b/choreo.chor index c6a1d2a..86b7c69 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1378,6 +1378,278 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "amp": { + "waypoints": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 4.703190638042804e-18, + "velocityX": -1.208675917564465e-17, + "velocityY": -6.352810871367948e-18, + "timestamp": 0 + }, + { + "x": 0.8020378021280192, + "y": 6.657644636391979, + "heading": 1.007479841292087, + "angularVelocity": -3.1089087057766036e-7, + "velocityX": 0.6028069120245683, + "velocityY": 0.31846394498851993, + "timestamp": 0.05828368082645525 + }, + { + "x": 0.8723054048773754, + "y": 6.694767133707721, + "heading": 1.0074798016172224, + "angularVelocity": -6.807244191958373e-7, + "velocityX": 1.2056136769836088, + "velocityY": 0.6369278121994305, + "timestamp": 0.1165673616529105 + }, + { + "x": 0.9753734970192464, + "y": 6.749218186944615, + "heading": 0.9990535149100053, + "angularVelocity": -0.14457368832954978, + "velocityX": 1.7683868053695166, + "velocityY": 0.9342418403365317, + "timestamp": 0.17485104247936575 + }, + { + "x": 1.078441589183816, + "y": 6.803669240138584, + "heading": 0.9195149817064242, + "angularVelocity": -1.3646793077415271, + "velocityX": 1.7683868057588992, + "velocityY": 0.9342418396000037, + "timestamp": 0.233134723305821 + }, + { + "x": 1.1815096813012231, + "y": 6.858120293421828, + "heading": 0.7885846802295383, + "angularVelocity": -2.2464315846153347, + "velocityX": 1.7683868049497107, + "velocityY": 0.9342418411317296, + "timestamp": 0.29141840413227627 + }, + { + "x": 1.2845777733691615, + "y": 6.912571346798711, + "heading": 0.6269133013543681, + "angularVelocity": -2.7738704313454483, + "velocityX": 1.7683868041009485, + "velocityY": 0.9342418427383741, + "timestamp": 0.3497020849587315 + }, + { + "x": 1.3876458655603612, + "y": 6.967022399942281, + "heading": 0.45022486052054533, + "angularVelocity": -3.03152509120432, + "velocityX": 1.7683868062158064, + "velocityY": 0.9342418387352904, + "timestamp": 0.40798576578518675 + }, + { + "x": 1.4907139579910005, + "y": 7.021473452632629, + "heading": 0.2680622315698304, + "angularVelocity": -3.125448262138626, + "velocityX": 1.768386810323986, + "velocityY": 0.9342418309591196, + "timestamp": 0.466269446611642 + }, + { + "x": 1.5937820505189422, + "y": 7.075924505138796, + "heading": 0.0852527280795436, + "angularVelocity": -3.136546987065588, + "velocityX": 1.7683868119934505, + "velocityY": 0.9342418277990836, + "timestamp": 0.5245531274380972 + }, + { + "x": 1.696850142939802, + "y": 7.130375557847656, + "heading": -0.09670059771549627, + "angularVelocity": -3.1218571513776574, + "velocityX": 1.7683868101561941, + "velocityY": 0.934241831276761, + "timestamp": 0.5828368082645525 + }, + { + "x": 1.7999182351302574, + "y": 7.184826610992639, + "heading": -0.27821616946416433, + "angularVelocity": -3.1143464032592627, + "velocityX": 1.7683868062030377, + "velocityY": 0.934241838759528, + "timestamp": 0.6411204890910078 + }, + { + "x": 1.9029863270693197, + "y": 7.239277664613472, + "heading": -0.4604343199403316, + "angularVelocity": -3.126400870599392, + "velocityX": 1.7683868018897635, + "velocityY": 0.9342418469239417, + "timestamp": 0.6994041699174631 + }, + { + "x": 2.0060544188463965, + "y": 7.293728718540922, + "heading": -0.6442150495894297, + "angularVelocity": -3.1532107623299397, + "velocityX": 1.7683867991104978, + "velocityY": 0.9342418521846886, + "timestamp": 0.7576878507439184 + }, + { + "x": 2.1091225106680933, + "y": 7.348179772383911, + "heading": -0.829167725791517, + "angularVelocity": -3.173318389980438, + "velocityX": 1.7683867998760676, + "velocityY": 0.9342418507355571, + "timestamp": 0.8159715315703737 + }, + { + "x": 2.212190602724116, + "y": 7.402630825783353, + "heading": -1.0127639833296274, + "angularVelocity": -3.1500456891898856, + "velocityX": 1.7683868038965105, + "velocityY": 0.9342418431254117, + "timestamp": 0.874255212396829 + }, + { + "x": 2.3152586950079552, + "y": 7.45708187875157, + "heading": -1.189780759847791, + "angularVelocity": -3.0371584980121242, + "velocityX": 1.7683868078052658, + "velocityY": 0.9342418357266746, + "timestamp": 0.9325388932232843 + }, + { + "x": 2.4183267873051926, + "y": 7.511532931694425, + "heading": -1.3512503965030376, + "angularVelocity": -2.770409047008454, + "velocityX": 1.768386808035141, + "velocityY": 0.934241835291512, + "timestamp": 0.9908225740497396 + }, + { + "x": 2.5213948795058583, + "y": 7.565983984820071, + "heading": -1.4820477254792255, + "angularVelocity": -2.244150114076672, + "velocityX": 1.7683868063782155, + "velocityY": 0.9342418384277741, + "timestamp": 1.049106254876195 + }, + { + "x": 2.6244629716588035, + "y": 7.620435038036042, + "heading": -1.5615658496268932, + "angularVelocity": -1.3643291401540225, + "velocityX": 1.7683868055594567, + "velocityY": 0.9342418399775199, + "timestamp": 1.1073899357026502 + }, + { + "x": 2.7275310638313552, + "y": 7.674886091214861, + "heading": -1.569999963079154, + "angularVelocity": -0.14470797540511007, + "velocityX": 1.7683868058959282, + "velocityY": 0.9342418393401073, + "timestamp": 1.1656736165291055 + }, + { + "x": 2.7977986665714587, + "y": 7.7120085885476914, + "heading": -1.5699999870889756, + "angularVelocity": -4.119521318427831e-7, + "velocityX": 1.2056136768248569, + "velocityY": 0.6369278124926107, + "timestamp": 1.2239572973555608 + }, + { + "x": 2.8329324722290035, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.215218740040656e-7, + "velocityX": 0.6028069119750795, + "velocityY": 0.3184639450811426, + "timestamp": 1.282240978182016 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": 2.8428255363446037e-18, + "velocityX": -6.6133223633086636e-18, + "velocityY": -3.51368640376029e-18, + "timestamp": 1.3405246590084714 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/amp.1.traj b/src/main/deploy/choreo/amp.1.traj new file mode 100644 index 0000000..89142ab --- /dev/null +++ b/src/main/deploy/choreo/amp.1.traj @@ -0,0 +1,220 @@ +{ + "samples": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 4.703190638042804e-18, + "velocityX": -1.208675917564465e-17, + "velocityY": -6.352810871367948e-18, + "timestamp": 0 + }, + { + "x": 0.8020378021280192, + "y": 6.657644636391979, + "heading": 1.007479841292087, + "angularVelocity": -3.1089087057766036e-7, + "velocityX": 0.6028069120245683, + "velocityY": 0.31846394498851993, + "timestamp": 0.05828368082645525 + }, + { + "x": 0.8723054048773754, + "y": 6.694767133707721, + "heading": 1.0074798016172224, + "angularVelocity": -6.807244191958373e-7, + "velocityX": 1.2056136769836088, + "velocityY": 0.6369278121994305, + "timestamp": 0.1165673616529105 + }, + { + "x": 0.9753734970192464, + "y": 6.749218186944615, + "heading": 0.9990535149100053, + "angularVelocity": -0.14457368832954978, + "velocityX": 1.7683868053695166, + "velocityY": 0.9342418403365317, + "timestamp": 0.17485104247936575 + }, + { + "x": 1.078441589183816, + "y": 6.803669240138584, + "heading": 0.9195149817064242, + "angularVelocity": -1.3646793077415271, + "velocityX": 1.7683868057588992, + "velocityY": 0.9342418396000037, + "timestamp": 0.233134723305821 + }, + { + "x": 1.1815096813012231, + "y": 6.858120293421828, + "heading": 0.7885846802295383, + "angularVelocity": -2.2464315846153347, + "velocityX": 1.7683868049497107, + "velocityY": 0.9342418411317296, + "timestamp": 0.29141840413227627 + }, + { + "x": 1.2845777733691615, + "y": 6.912571346798711, + "heading": 0.6269133013543681, + "angularVelocity": -2.7738704313454483, + "velocityX": 1.7683868041009485, + "velocityY": 0.9342418427383741, + "timestamp": 0.3497020849587315 + }, + { + "x": 1.3876458655603612, + "y": 6.967022399942281, + "heading": 0.45022486052054533, + "angularVelocity": -3.03152509120432, + "velocityX": 1.7683868062158064, + "velocityY": 0.9342418387352904, + "timestamp": 0.40798576578518675 + }, + { + "x": 1.4907139579910005, + "y": 7.021473452632629, + "heading": 0.2680622315698304, + "angularVelocity": -3.125448262138626, + "velocityX": 1.768386810323986, + "velocityY": 0.9342418309591196, + "timestamp": 0.466269446611642 + }, + { + "x": 1.5937820505189422, + "y": 7.075924505138796, + "heading": 0.0852527280795436, + "angularVelocity": -3.136546987065588, + "velocityX": 1.7683868119934505, + "velocityY": 0.9342418277990836, + "timestamp": 0.5245531274380972 + }, + { + "x": 1.696850142939802, + "y": 7.130375557847656, + "heading": -0.09670059771549627, + "angularVelocity": -3.1218571513776574, + "velocityX": 1.7683868101561941, + "velocityY": 0.934241831276761, + "timestamp": 0.5828368082645525 + }, + { + "x": 1.7999182351302574, + "y": 7.184826610992639, + "heading": -0.27821616946416433, + "angularVelocity": -3.1143464032592627, + "velocityX": 1.7683868062030377, + "velocityY": 0.934241838759528, + "timestamp": 0.6411204890910078 + }, + { + "x": 1.9029863270693197, + "y": 7.239277664613472, + "heading": -0.4604343199403316, + "angularVelocity": -3.126400870599392, + "velocityX": 1.7683868018897635, + "velocityY": 0.9342418469239417, + "timestamp": 0.6994041699174631 + }, + { + "x": 2.0060544188463965, + "y": 7.293728718540922, + "heading": -0.6442150495894297, + "angularVelocity": -3.1532107623299397, + "velocityX": 1.7683867991104978, + "velocityY": 0.9342418521846886, + "timestamp": 0.7576878507439184 + }, + { + "x": 2.1091225106680933, + "y": 7.348179772383911, + "heading": -0.829167725791517, + "angularVelocity": -3.173318389980438, + "velocityX": 1.7683867998760676, + "velocityY": 0.9342418507355571, + "timestamp": 0.8159715315703737 + }, + { + "x": 2.212190602724116, + "y": 7.402630825783353, + "heading": -1.0127639833296274, + "angularVelocity": -3.1500456891898856, + "velocityX": 1.7683868038965105, + "velocityY": 0.9342418431254117, + "timestamp": 0.874255212396829 + }, + { + "x": 2.3152586950079552, + "y": 7.45708187875157, + "heading": -1.189780759847791, + "angularVelocity": -3.0371584980121242, + "velocityX": 1.7683868078052658, + "velocityY": 0.9342418357266746, + "timestamp": 0.9325388932232843 + }, + { + "x": 2.4183267873051926, + "y": 7.511532931694425, + "heading": -1.3512503965030376, + "angularVelocity": -2.770409047008454, + "velocityX": 1.768386808035141, + "velocityY": 0.934241835291512, + "timestamp": 0.9908225740497396 + }, + { + "x": 2.5213948795058583, + "y": 7.565983984820071, + "heading": -1.4820477254792255, + "angularVelocity": -2.244150114076672, + "velocityX": 1.7683868063782155, + "velocityY": 0.9342418384277741, + "timestamp": 1.049106254876195 + }, + { + "x": 2.6244629716588035, + "y": 7.620435038036042, + "heading": -1.5615658496268932, + "angularVelocity": -1.3643291401540225, + "velocityX": 1.7683868055594567, + "velocityY": 0.9342418399775199, + "timestamp": 1.1073899357026502 + }, + { + "x": 2.7275310638313552, + "y": 7.674886091214861, + "heading": -1.569999963079154, + "angularVelocity": -0.14470797540511007, + "velocityX": 1.7683868058959282, + "velocityY": 0.9342418393401073, + "timestamp": 1.1656736165291055 + }, + { + "x": 2.7977986665714587, + "y": 7.7120085885476914, + "heading": -1.5699999870889756, + "angularVelocity": -4.119521318427831e-7, + "velocityX": 1.2056136768248569, + "velocityY": 0.6369278124926107, + "timestamp": 1.2239572973555608 + }, + { + "x": 2.8329324722290035, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.215218740040656e-7, + "velocityX": 0.6028069119750795, + "velocityY": 0.3184639450811426, + "timestamp": 1.282240978182016 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": 2.8428255363446037e-18, + "velocityX": -6.6133223633086636e-18, + "velocityY": -3.51368640376029e-18, + "timestamp": 1.3405246590084714 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.traj b/src/main/deploy/choreo/amp.traj new file mode 100644 index 0000000..89142ab --- /dev/null +++ b/src/main/deploy/choreo/amp.traj @@ -0,0 +1,220 @@ +{ + "samples": [ + { + "x": 0.7669039964675903, + "y": 6.639083385467529, + "heading": 1.0074798594118668, + "angularVelocity": 4.703190638042804e-18, + "velocityX": -1.208675917564465e-17, + "velocityY": -6.352810871367948e-18, + "timestamp": 0 + }, + { + "x": 0.8020378021280192, + "y": 6.657644636391979, + "heading": 1.007479841292087, + "angularVelocity": -3.1089087057766036e-7, + "velocityX": 0.6028069120245683, + "velocityY": 0.31846394498851993, + "timestamp": 0.05828368082645525 + }, + { + "x": 0.8723054048773754, + "y": 6.694767133707721, + "heading": 1.0074798016172224, + "angularVelocity": -6.807244191958373e-7, + "velocityX": 1.2056136769836088, + "velocityY": 0.6369278121994305, + "timestamp": 0.1165673616529105 + }, + { + "x": 0.9753734970192464, + "y": 6.749218186944615, + "heading": 0.9990535149100053, + "angularVelocity": -0.14457368832954978, + "velocityX": 1.7683868053695166, + "velocityY": 0.9342418403365317, + "timestamp": 0.17485104247936575 + }, + { + "x": 1.078441589183816, + "y": 6.803669240138584, + "heading": 0.9195149817064242, + "angularVelocity": -1.3646793077415271, + "velocityX": 1.7683868057588992, + "velocityY": 0.9342418396000037, + "timestamp": 0.233134723305821 + }, + { + "x": 1.1815096813012231, + "y": 6.858120293421828, + "heading": 0.7885846802295383, + "angularVelocity": -2.2464315846153347, + "velocityX": 1.7683868049497107, + "velocityY": 0.9342418411317296, + "timestamp": 0.29141840413227627 + }, + { + "x": 1.2845777733691615, + "y": 6.912571346798711, + "heading": 0.6269133013543681, + "angularVelocity": -2.7738704313454483, + "velocityX": 1.7683868041009485, + "velocityY": 0.9342418427383741, + "timestamp": 0.3497020849587315 + }, + { + "x": 1.3876458655603612, + "y": 6.967022399942281, + "heading": 0.45022486052054533, + "angularVelocity": -3.03152509120432, + "velocityX": 1.7683868062158064, + "velocityY": 0.9342418387352904, + "timestamp": 0.40798576578518675 + }, + { + "x": 1.4907139579910005, + "y": 7.021473452632629, + "heading": 0.2680622315698304, + "angularVelocity": -3.125448262138626, + "velocityX": 1.768386810323986, + "velocityY": 0.9342418309591196, + "timestamp": 0.466269446611642 + }, + { + "x": 1.5937820505189422, + "y": 7.075924505138796, + "heading": 0.0852527280795436, + "angularVelocity": -3.136546987065588, + "velocityX": 1.7683868119934505, + "velocityY": 0.9342418277990836, + "timestamp": 0.5245531274380972 + }, + { + "x": 1.696850142939802, + "y": 7.130375557847656, + "heading": -0.09670059771549627, + "angularVelocity": -3.1218571513776574, + "velocityX": 1.7683868101561941, + "velocityY": 0.934241831276761, + "timestamp": 0.5828368082645525 + }, + { + "x": 1.7999182351302574, + "y": 7.184826610992639, + "heading": -0.27821616946416433, + "angularVelocity": -3.1143464032592627, + "velocityX": 1.7683868062030377, + "velocityY": 0.934241838759528, + "timestamp": 0.6411204890910078 + }, + { + "x": 1.9029863270693197, + "y": 7.239277664613472, + "heading": -0.4604343199403316, + "angularVelocity": -3.126400870599392, + "velocityX": 1.7683868018897635, + "velocityY": 0.9342418469239417, + "timestamp": 0.6994041699174631 + }, + { + "x": 2.0060544188463965, + "y": 7.293728718540922, + "heading": -0.6442150495894297, + "angularVelocity": -3.1532107623299397, + "velocityX": 1.7683867991104978, + "velocityY": 0.9342418521846886, + "timestamp": 0.7576878507439184 + }, + { + "x": 2.1091225106680933, + "y": 7.348179772383911, + "heading": -0.829167725791517, + "angularVelocity": -3.173318389980438, + "velocityX": 1.7683867998760676, + "velocityY": 0.9342418507355571, + "timestamp": 0.8159715315703737 + }, + { + "x": 2.212190602724116, + "y": 7.402630825783353, + "heading": -1.0127639833296274, + "angularVelocity": -3.1500456891898856, + "velocityX": 1.7683868038965105, + "velocityY": 0.9342418431254117, + "timestamp": 0.874255212396829 + }, + { + "x": 2.3152586950079552, + "y": 7.45708187875157, + "heading": -1.189780759847791, + "angularVelocity": -3.0371584980121242, + "velocityX": 1.7683868078052658, + "velocityY": 0.9342418357266746, + "timestamp": 0.9325388932232843 + }, + { + "x": 2.4183267873051926, + "y": 7.511532931694425, + "heading": -1.3512503965030376, + "angularVelocity": -2.770409047008454, + "velocityX": 1.768386808035141, + "velocityY": 0.934241835291512, + "timestamp": 0.9908225740497396 + }, + { + "x": 2.5213948795058583, + "y": 7.565983984820071, + "heading": -1.4820477254792255, + "angularVelocity": -2.244150114076672, + "velocityX": 1.7683868063782155, + "velocityY": 0.9342418384277741, + "timestamp": 1.049106254876195 + }, + { + "x": 2.6244629716588035, + "y": 7.620435038036042, + "heading": -1.5615658496268932, + "angularVelocity": -1.3643291401540225, + "velocityX": 1.7683868055594567, + "velocityY": 0.9342418399775199, + "timestamp": 1.1073899357026502 + }, + { + "x": 2.7275310638313552, + "y": 7.674886091214861, + "heading": -1.569999963079154, + "angularVelocity": -0.14470797540511007, + "velocityX": 1.7683868058959282, + "velocityY": 0.9342418393401073, + "timestamp": 1.1656736165291055 + }, + { + "x": 2.7977986665714587, + "y": 7.7120085885476914, + "heading": -1.5699999870889756, + "angularVelocity": -4.119521318427831e-7, + "velocityX": 1.2056136768248569, + "velocityY": 0.6369278124926107, + "timestamp": 1.2239572973555608 + }, + { + "x": 2.8329324722290035, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": -2.215218740040656e-7, + "velocityX": 0.6028069119750795, + "velocityY": 0.3184639450811426, + "timestamp": 1.282240978182016 + }, + { + "x": 2.832932472229004, + "y": 7.730569839477539, + "heading": -1.57, + "angularVelocity": 2.8428255363446037e-18, + "velocityX": -6.6133223633086636e-18, + "velocityY": -3.51368640376029e-18, + "timestamp": 1.3405246590084714 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece amp shot.auto b/src/main/deploy/pathplanner/autos/1 piece amp shot.auto new file mode 100644 index 0000000..c0d6942 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 piece amp shot.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7669039964675903, + "y": 6.639083385467529 + }, + "rotation": 57.72434388873349 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.1" + } + }, + { + "type": "named", + "data": { + "name": "AmpSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece left.auto b/src/main/deploy/pathplanner/autos/1 piece amp side.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1 piece left.auto rename to src/main/deploy/pathplanner/autos/1 piece amp side.auto diff --git a/src/main/deploy/pathplanner/autos/1 piece right.auto b/src/main/deploy/pathplanner/autos/1 piece source side.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1 piece right.auto rename to src/main/deploy/pathplanner/autos/1 piece source side.auto diff --git a/src/main/deploy/pathplanner/autos/2 piece left.auto b/src/main/deploy/pathplanner/autos/2 piece amp side.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2 piece left.auto rename to src/main/deploy/pathplanner/autos/2 piece amp side.auto diff --git a/src/main/deploy/pathplanner/autos/2 piece right.auto b/src/main/deploy/pathplanner/autos/2 piece source side.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2 piece right.auto rename to src/main/deploy/pathplanner/autos/2 piece source side.auto diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8038ad9..c01eb39 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -101,6 +101,8 @@ public void robotInit() { if (!tuningMode) PPLibTelemetry.enableCompetitionMode(); + + // robotContainer.swerve.zeroYaw(); } /** diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index b7a6130..6a21f78 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .695), - new SwerveModuleConstants(23, 24, 1.775), - new SwerveModuleConstants(25, 26, 3.369), - new SwerveModuleConstants(27, 28, 2.863) + new SwerveModuleConstants(21, 22, .708), + new SwerveModuleConstants(23, 24, 1.763), + new SwerveModuleConstants(25, 26, 3.349), + new SwerveModuleConstants(27, 28, 2.838) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index ad1c70b..bfaeb6d 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -14,10 +14,10 @@ public class Lights { private static DigitalOutput[] pins = { - new DigitalOutput(1), - new DigitalOutput(2), + new DigitalOutput(4), new DigitalOutput(3), - new DigitalOutput(4) + new DigitalOutput(2), + new DigitalOutput(1) }; public Lights() {} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 00a7ff4..152b2cc 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -102,7 +102,10 @@ public Swerve(GyroIO gyroIO, Vision vision) { new ReplanningConfig()), () -> { var alliance = DriverStation.getAlliance(); - return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; }, this); Pathfinding.setPathfinder(new LocalADStarAK()); @@ -152,7 +155,7 @@ public SwerveModulePosition[] getPositions() { * (like at the start of an autonomous path) */ public void resetOdometry(Pose2d pose) { - poseEstimator.resetPosition(getYaw(), getPositions(), pose); + poseEstimator.resetPosition(getYaw().plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180)), getPositions(), pose); } public void resetModuleOffsets() { @@ -240,7 +243,8 @@ public void periodic() { Logger.recordOutput("Swerve/States", moduleStates); // update odometry - poseEstimator.updateWithTime(timer.get(), getYaw(), modulePositions); + poseEstimator.updateWithTime(timer.get(), getYaw().plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180)), + modulePositions); var pose = poseEstimator.getEstimatedPosition(); translationY = pose.getY(); translationX = pose.getX(); @@ -258,8 +262,9 @@ public void periodic() { if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, vision.fieldInputs.fieldspaceTranslationY_m, - new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad)); // continuation of lines 259-261. Create a new - // pose2d + new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad) + .plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180))); // continuation of lines 259-261. Create a new + // pose2d // using X,Y and rotation Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index ca26d34..3ec4c12 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,38 +1,38 @@ { - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2024.1.5", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2024.1.5" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2024.1.5", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.2.6", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.2.6" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.2.6", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 55db333..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,339 +1,339 @@ { - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "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-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "24.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "24.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.1.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.1.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.1.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.1.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.1.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.1.0", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.1.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.1.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.1.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.2.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-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "24.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "24.2.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "24.2.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "24.2.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "24.2.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "24.2.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "24.2.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "24.2.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "24.2.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "24.2.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] } \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index b40dab7..60eacf8 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,74 +1,74 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2024.2.0", - "frcYear": "2024", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2024.2.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2024.2.0", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2024.2.0", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "windowsx86", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.3", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] } \ No newline at end of file diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index 23ea26a..998c261 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,65 +1,65 @@ { - "fileName": "URCL.json", - "name": "URCL", - "version": "2024.0.0", - "frcYear": "2024", - "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", - "mavenUrls": [ - "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.0.0" - ], - "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-java", - "version": "2024.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-driver", - "version": "2024.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-cpp", - "version": "2024.0.0", - "libName": "URCL", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - }, - { - "groupId": "org.littletonrobotics.urcl", - "artifactId": "URCL-driver", - "version": "2024.0.0", - "libName": "URCLDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena", - "osxuniversal" - ] - } - ] + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] } \ No newline at end of file From 3a44001294b8574c1867a581c1bae3b1de25ccdf Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sun, 17 Mar 2024 12:37:05 -0400 Subject: [PATCH 16/22] end of quals --- choreo.chor | 3481 +++++++++++------ src/main/deploy/choreo/2 piece left.1.traj | 247 +- src/main/deploy/choreo/2 piece left.2.traj | 245 +- src/main/deploy/choreo/2 piece left.3.traj | 310 +- src/main/deploy/choreo/2 piece left.traj | 796 ++-- src/main/deploy/choreo/2 piece right.1.traj | 257 +- src/main/deploy/choreo/2 piece right.2.traj | 257 +- src/main/deploy/choreo/2 piece right.3.traj | 338 +- src/main/deploy/choreo/2 piece right.traj | 842 ++-- src/main/deploy/choreo/2piece.1.traj | 121 +- src/main/deploy/choreo/2piece.2.traj | 121 +- src/main/deploy/choreo/2piece.traj | 230 +- src/main/deploy/choreo/4 piece.1.traj | 94 + src/main/deploy/choreo/4 piece.2.traj | 94 + src/main/deploy/choreo/4 piece.3.traj | 121 + src/main/deploy/choreo/4 piece.4.traj | 121 + src/main/deploy/choreo/4 piece.5.traj | 121 + src/main/deploy/choreo/4 piece.6.traj | 121 + src/main/deploy/choreo/4 piece.traj | 607 +++ src/main/deploy/choreo/amp.1.traj | 440 ++- src/main/deploy/choreo/amp.traj | 440 ++- src/main/deploy/choreo/exit center.1.traj | 295 +- src/main/deploy/choreo/exit center.traj | 295 +- .../pathplanner/autos/2 piece amp side.auto | 44 +- .../pathplanner/autos/2 piece center.auto | 60 +- .../autos/2 piece source side.auto | 46 +- .../autos/3 piece (towards amp).auto | 179 + .../deploy/pathplanner/autos/4 piece.auto | 247 ++ src/main/java/frc/robot/RobotContainer.java | 27 +- .../robot/commands/intake/IntakeWithArm.java | 9 +- .../robot/commands/swerve/TeleopSwerve.java | 8 +- .../frc/robot/constants/ArmConstants.java | 2 +- .../frc/robot/constants/SwerveConstants.java | 12 +- src/main/java/frc/robot/io/FieldVisionIO.java | 4 +- src/main/java/frc/robot/subsystems/Arm.java | 6 +- .../java/frc/robot/subsystems/Intake.java | 13 +- .../java/frc/robot/subsystems/Swerve.java | 43 +- 37 files changed, 7096 insertions(+), 3598 deletions(-) create mode 100644 src/main/deploy/choreo/4 piece.1.traj create mode 100644 src/main/deploy/choreo/4 piece.2.traj create mode 100644 src/main/deploy/choreo/4 piece.3.traj create mode 100644 src/main/deploy/choreo/4 piece.4.traj create mode 100644 src/main/deploy/choreo/4 piece.5.traj create mode 100644 src/main/deploy/choreo/4 piece.6.traj create mode 100644 src/main/deploy/choreo/4 piece.traj create mode 100644 src/main/deploy/pathplanner/autos/3 piece (towards amp).auto create mode 100644 src/main/deploy/pathplanner/autos/4 piece.auto diff --git a/choreo.chor b/choreo.chor index 86b7c69..ed502c2 100644 --- a/choreo.chor +++ b/choreo.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 50, "rotationalInertia": 6, - "motorMaxTorque": 1.087292817679558, + "motorMaxTorque": 0.5, "motorMaxVelocity": 4704, "gearing": 6.74603175, "wheelbase": 0.57785, @@ -22,16 +22,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 9 }, { - "x": 2.349189281463623, - "y": 5.567087650299072, + "x": 2.3261709213256836, + "y": 5.586577892303467, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 6 + "controlIntervalCount": 9 }, { "x": 1.3126474618911743, @@ -45,121 +45,175 @@ ], "trajectory": [ { - "x": 1.3126474618911723, + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": 2.630502167570374e-32, - "angularVelocity": 6.912068173828048e-33, - "velocityX": -4.041414755979078e-16, - "velocityY": 2.1900742120129258e-39, + "heading": -3.275463440583649e-29, + "angularVelocity": -1.5542228939894143e-29, + "velocityX": -1.0432080618288646e-15, + "velocityY": -2.0061080376600202e-17, "timestamp": 0 }, { - "x": 1.4761905162579165, - "y": 5.567087650299072, - "heading": 5.081856100994774e-22, - "angularVelocity": 4.2976983347588495e-21, - "velocityX": 1.383114259492122, - "velocityY": 7.266238948856172e-27, - "timestamp": 0.11824262021257878 - }, - { - "x": 1.7126757532986059, - "y": 5.567087650299072, - "heading": 5.225371158203822e-19, - "angularVelocity": 4.415167063895265e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.427644052438308e-27, - "timestamp": 0.23648524042515756 - }, - { - "x": 1.9491609900561957, - "y": 5.567087650299072, - "heading": 2.6577036541275914e-19, - "angularVelocity": -2.1718062327712947e-18, - "velocityX": 1.9999999681274145, - "velocityY": 3.4263757854502784e-27, - "timestamp": 0.3547278606377363 - }, - { - "x": 2.1856462270968846, - "y": 5.567087650299072, - "heading": -1.0906583480085762e-21, - "angularVelocity": -2.2568820046674306e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.421212896814989e-27, - "timestamp": 0.4729704808503151 - }, - { - "x": 2.349189281463629, - "y": 5.567087650299072, - "heading": -3.2298015655990765e-32, - "angularVelocity": 9.223475207094912e-21, - "velocityX": 1.383114259492122, - "velocityY": -1.754147302356677e-26, - "timestamp": 0.5912131010628939 - }, - { - "x": 2.349189281463627, - "y": 5.567087650299072, - "heading": -3.648621613916173e-33, - "angularVelocity": 1.9825798352488026e-32, - "velocityX": -3.5425947355709975e-16, - "velocityY": -9.477719520863627e-38, - "timestamp": 0.7094557212754727 - }, - { - "x": 2.185646227096883, - "y": 5.567087650299072, - "heading": 3.738718946616064e-21, - "angularVelocity": 3.1617850562356095e-20, - "velocityX": -1.3831142594921222, - "velocityY": 5.863050032870948e-30, - "timestamp": 0.8276983414880514 - }, - { - "x": 1.9491609900561935, - "y": 5.567087650299072, - "heading": -1.3594265469642462e-18, - "angularVelocity": -1.1527879544319905e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.470823667011192e-30, - "timestamp": 0.9459409617006301 - }, - { - "x": 1.7126757532986037, - "y": 5.567087650299072, - "heading": -1.1949667812351735e-18, - "angularVelocity": 1.3904520102372167e-18, - "velocityX": -1.9999999681274145, - "velocityY": 8.47082157931736e-30, - "timestamp": 1.0641835819132088 - }, - { - "x": 1.4761905162579143, - "y": 5.567087650299072, - "heading": -5.013730755356182e-22, - "angularVelocity": 1.0101569711037546e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.47082075819828e-30, - "timestamp": 1.1824262021257876 - }, - { - "x": 1.3126474618911703, + "x": 1.3669642138871982, + "y": 5.568132171401609, + "heading": -5.88414271395013e-19, + "angularVelocity": -5.855040371370758e-18, + "velocityX": 0.5404810945104359, + "velocityY": 0.01039355057127695, + "timestamp": 0.10049704337061591 + }, + { + "x": 1.4755977148071626, + "y": 5.570221213547605, + "heading": -1.2948347856012382e-18, + "angularVelocity": -7.029266314478605e-18, + "velocityX": 1.080962158452135, + "velocityY": 0.020787100554711394, + "timestamp": 0.20099408674123181 + }, + { + "x": 1.6385479553988989, + "y": 5.57335477655914, + "heading": -2.2094750613970746e-18, + "angularVelocity": -9.101165621984674e-18, + "velocityX": 1.6214431303297006, + "velocityY": 0.03118064876773541, + "timestamp": 0.3014911301118477 + }, + { + "x": 1.8194091916084238, + "y": 5.5768327713012695, + "heading": -7.770058287246279e-19, + "angularVelocity": 1.4253844128634057e-17, + "velocityX": 1.7996672354084517, + "velocityY": 0.03460793099457427, + "timestamp": 0.40198817348246363 + }, + { + "x": 2.000270427817948, + "y": 5.580310766043398, + "heading": 1.3008360366019906e-18, + "angularVelocity": 2.0675650427630753e-17, + "velocityX": 1.7996672354084513, + "velocityY": 0.03460793099457139, + "timestamp": 0.5024852168530796 + }, + { + "x": 2.163220668409685, + "y": 5.583444329054934, + "heading": 7.353018561899664e-19, + "angularVelocity": -5.62737100639049e-18, + "velocityX": 1.6214431303297023, + "velocityY": 0.031180648767737112, + "timestamp": 0.6029822602236955 + }, + { + "x": 2.2718541693296492, + "y": 5.58553337120093, + "heading": 2.1533805611247358e-19, + "angularVelocity": -5.173921101276726e-18, + "velocityX": 1.080962158452136, + "velocityY": 0.02078710055471218, + "timestamp": 0.7034793035943114 + }, + { + "x": 2.3261709213256676, + "y": 5.586577892303466, + "heading": 1.773087403019019e-28, + "angularVelocity": -2.142730137028445e-18, + "velocityX": 0.5404810945104372, + "velocityY": 0.01039355057127728, + "timestamp": 0.8039763469649274 + }, + { + "x": 2.326170921325673, + "y": 5.586577892303467, + "heading": 1.4827806105172361e-28, + "angularVelocity": 3.750914457354201e-29, + "velocityX": 2.67021227320413e-16, + "velocityY": 5.134872532614327e-18, + "timestamp": 0.9044733903355433 + }, + { + "x": 2.271854169329654, + "y": 5.585533371200933, + "heading": 1.0512311117475804e-18, + "angularVelocity": 1.046031869151828e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571247395, + "timestamp": 1.0049704337061591 + }, + { + "x": 2.1632206684096897, + "y": 5.5834443290549425, + "heading": 3.191325466769564e-18, + "angularVelocity": 2.1295097409897457e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554652216, + "timestamp": 1.105467477076775 + }, + { + "x": 2.000270427817954, + "y": 5.580310766043416, + "heading": 6.075146753884222e-18, + "angularVelocity": 2.8695582894264616e-17, + "velocityX": -1.6214431303297012, + "velocityY": -0.031180648767646445, + "timestamp": 1.2059645204473908 + }, + { + "x": 1.8194091916084287, + "y": 5.576832771301296, + "heading": -3.924520679097354e-19, + "angularVelocity": -6.435610856432199e-17, + "velocityX": -1.7996672354084535, + "velocityY": -0.03460793099447712, + "timestamp": 1.3064615638180066 + }, + { + "x": 1.638547955398904, + "y": 5.573354776559178, + "heading": -7.054275205144732e-18, + "angularVelocity": -6.628874683251403e-17, + "velocityX": -1.7996672354084533, + "velocityY": -0.034607930994474186, + "timestamp": 1.4069586071886224 + }, + { + "x": 1.475597714807168, + "y": 5.570221213547637, + "heading": -3.521961615863548e-18, + "angularVelocity": 3.514843204208217e-17, + "velocityX": -1.6214431303297017, + "velocityY": -0.031180648767786465, + "timestamp": 1.5074556505592382 + }, + { + "x": 1.3669642138872036, + "y": 5.568132171401634, + "heading": -1.0343608078918469e-18, + "angularVelocity": 2.4752974455228876e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554791237, + "timestamp": 1.607952693929854 + }, + { + "x": 1.312647461891185, + "y": 5.5670876502990865, + "heading": -2.3633742772886086e-28, + "angularVelocity": 1.0292449890816985e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571385917, + "timestamp": 1.70844973730047 + }, + { + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": -4.6245267198088095e-32, - "angularVelocity": 4.2399724814725485e-21, - "velocityX": -1.3831142594921215, - "velocityY": 5.85686190815934e-30, - "timestamp": 1.3006688223383664 - }, - { - "x": 1.3126474618911723, - "y": 5.567087650299072, - "heading": -2.2656479665287125e-32, - "angularVelocity": 7.883926885959527e-33, - "velocityX": 3.7971136148997106e-16, - "velocityY": -9.967559307460824e-45, - "timestamp": 1.4189114425509453 + "heading": -1.1552046309606331e-28, + "angularVelocity": 5.267876884045709e-29, + "velocityX": 2.7782898002965743e-16, + "velocityY": -1.3818335759211866e-13, + "timestamp": 1.8089467806710857 } ], "constraints": [ @@ -193,7 +247,13 @@ "last" ], "type": "MaxVelocity", - "velocity": 2 + "velocity": 1.8 + }, + { + "scope": [ + 1 + ], + "type": "WptZeroVelocity" } ], "usesControlIntervalGuessing": true, @@ -210,7 +270,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { "x": 2.3418185710906982, @@ -219,7 +279,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { "x": 0.7669039964675903, @@ -228,7 +288,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 20 }, { "x": 3.296103000640869, @@ -245,388 +305,478 @@ "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 2.0283839438048332e-19, - "velocityX": 8.094691061889947e-19, - "velocityY": 1.7638426740730563e-19, + "angularVelocity": -7.087535479777707e-19, + "velocityX": 7.50401412133545e-18, + "velocityY": 1.4256736684497237e-18, "timestamp": 0 }, { - "x": 0.8322802456836562, - "y": 6.653172699607564, - "heading": 1.0074798311324673, - "angularVelocity": -3.7400126935671097e-7, - "velocityX": 0.8646152527784429, - "velocityY": 0.18633427357388527, - "timestamp": 0.07561311115664361 - }, - { - "x": 0.9630327340876748, - "y": 6.681351325717821, - "heading": 1.007479762583371, - "angularVelocity": -9.065768535902331e-7, - "velocityX": 1.7292303729328837, - "velocityY": 0.3726685184515223, - "timestamp": 0.15122622231328722 - }, - { - "x": 1.1108648715080907, - "y": 6.713210807608451, - "heading": 0.935326621538057, - "angularVelocity": -0.9542411354538504, - "velocityX": 1.955112481936631, - "velocityY": 0.4213486444781865, - "timestamp": 0.22683933346993085 - }, - { - "x": 1.258697008933641, - "y": 6.745070289475266, - "heading": 0.8127259503436992, - "angularVelocity": -1.6214208001621926, - "velocityX": 1.9551124820045325, - "velocityY": 0.421348644163222, - "timestamp": 0.30245244462657445 - }, - { - "x": 1.406529146351303, - "y": 6.776929771378683, - "heading": 0.6640255812369789, - "angularVelocity": -1.966595036655294, - "velocityX": 1.9551124819002106, - "velocityY": 0.4213486446472832, - "timestamp": 0.37806555578321804 - }, - { - "x": 1.5543612837652037, - "y": 6.808789253299551, - "heading": 0.5044825425717008, - "angularVelocity": -2.1099917226625076, - "velocityX": 1.9551124818504668, - "velocityY": 0.42134864487809753, - "timestamp": 0.45367866693986164 - }, - { - "x": 1.702193421185673, - "y": 6.840648735189942, - "heading": 0.344692011554351, - "angularVelocity": -2.1132648633690048, - "velocityX": 1.9551124819373364, - "velocityY": 0.42134864447501086, - "timestamp": 0.5292917780965053 - }, - { - "x": 1.8500255586150136, - "y": 6.872508217039167, - "heading": 0.19547183240040739, - "angularVelocity": -1.9734696386822674, - "velocityX": 1.9551124820546628, - "velocityY": 0.42134864393060517, - "timestamp": 0.6049048892531489 - }, - { - "x": 1.9978576960460066, - "y": 6.904367698880726, - "heading": 0.07244487018227615, - "angularVelocity": -1.6270585925668755, - "velocityX": 1.9551124820765156, - "velocityY": 0.42134864382921067, - "timestamp": 0.6805180004097925 - }, - { - "x": 2.1456898334992545, - "y": 6.936227180619012, - "heading": 9.65887879992878e-8, - "angularVelocity": -0.9580980399471752, - "velocityX": 1.955112482370841, - "velocityY": 0.4213486424633956, - "timestamp": 0.7561311115664361 - }, - { - "x": 2.2764423218817926, - "y": 6.964405806828939, - "heading": 2.820501204963407e-8, - "angularVelocity": -9.043904534147509e-7, - "velocityX": 1.7292303726488056, - "velocityY": 0.37266851976968174, - "timestamp": 0.8317442227230797 + "x": 0.7981646775700082, + "y": 6.645820413966075, + "heading": 1.0074794822217241, + "angularVelocity": -0.000004891992156933861, + "velocityX": 0.4054374425885426, + "velocityY": 0.08737633054596244, + "timestamp": 0.07710358693768382 + }, + { + "x": 0.8606860382778649, + "y": 6.659294470675445, + "heading": 1.0074786780539715, + "angularVelocity": -0.000010429706123137783, + "velocityX": 0.8108748657619178, + "velocityY": 0.17475265736028606, + "timestamp": 0.15420717387536764 + }, + { + "x": 0.9544680756755046, + "y": 6.679505554656715, + "heading": 1.00747734637885, + "angularVelocity": -0.000017271247347385705, + "velocityX": 1.2163122511205047, + "velocityY": 0.2621289719971706, + "timestamp": 0.23131076081305146 + }, + { + "x": 1.0795107812763696, + "y": 6.706453661799292, + "heading": 1.007475162920361, + "angularVelocity": -0.000028318507292526304, + "velocityX": 1.6217495264121258, + "velocityY": 0.3495052333214708, + "timestamp": 0.3084143477507353 + }, + { + "x": 1.2151823459896474, + "y": 6.735692436676619, + "heading": 0.9400813447765213, + "angularVelocity": -0.8740685202921675, + "velocityX": 1.7596012079558552, + "velocityY": 0.37921419791997063, + "timestamp": 0.3855179346884191 + }, + { + "x": 1.3508539216363087, + "y": 6.764931160821833, + "heading": 0.8025402930124232, + "angularVelocity": -1.7838476422019192, + "velocityX": 1.7596013497570884, + "velocityY": 0.37921353994654156, + "timestamp": 0.4626215216261029 + }, + { + "x": 1.4865254934430294, + "y": 6.794169902784878, + "heading": 0.6117409589886299, + "angularVelocity": -2.4745844078304686, + "velocityX": 1.7596012999547292, + "velocityY": 0.3792137710360449, + "timestamp": 0.5397251085637867 + }, + { + "x": 1.6221970645812445, + "y": 6.8234086478498766, + "heading": 0.3987348321748914, + "angularVelocity": -2.7625968553952354, + "velocityX": 1.7596012912844983, + "velocityY": 0.3792138112670414, + "timestamp": 0.6168286955014706 + }, + { + "x": 1.757868635365604, + "y": 6.852647394556811, + "heading": 0.20722880927427756, + "angularVelocity": -2.4837498553133672, + "velocityX": 1.7596012866951443, + "velocityY": 0.37921383256224117, + "timestamp": 0.6939322824391544 + }, + { + "x": 1.8935401957987985, + "y": 6.881886189294451, + "heading": 0.06863797850596423, + "angularVelocity": -1.7974628194706992, + "velocityX": 1.7596011524450381, + "velocityY": 0.3792144554995873, + "timestamp": 0.7710358693768382 + }, + { + "x": 2.029211813519273, + "y": 6.911124718210696, + "heading": 0.000004762891013022644, + "angularVelocity": -0.8901429666355407, + "velocityX": 1.7596018954361505, + "velocityY": 0.37921100791168616, + "timestamp": 0.848139456314522 + }, + { + "x": 2.1542545037959298, + "y": 6.938072896534997, + "heading": 0.00000255375105981851, + "angularVelocity": -0.00002865158471797654, + "velocityX": 1.6217493276638106, + "velocityY": 0.34950615651748473, + "timestamp": 0.9252430432522059 + }, + { + "x": 2.2480365342099393, + "y": 6.958284012920916, + "heading": 0.0000012010318378255, + "angularVelocity": -0.000017544180183003836, + "velocityX": 1.2163121605458629, + "velocityY": 0.26212939227137494, + "timestamp": 1.0023466301898898 + }, + { + "x": 2.3105578914207334, + "y": 6.971758085856952, + "heading": 3.834757266936798e-7, + "angularVelocity": -0.000010603347309739972, + "velocityX": 0.8108748204065306, + "velocityY": 0.1747528678130866, + "timestamp": 1.0794502171275737 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.9396360429134164e-18, - "angularVelocity": -3.7301747793182745e-7, - "velocityX": 0.8646152526837502, - "velocityY": 0.18633427401327182, - "timestamp": 0.9073573338797233 + "heading": 3.134962737955188e-18, + "angularVelocity": -0.000004973513450108379, + "velocityX": 0.405437424010253, + "velocityY": 0.08737641675075283, + "timestamp": 1.1565538040652577 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.2932527092188034e-18, - "angularVelocity": 4.19600892926216e-20, - "velocityX": -8.058124746715222e-20, - "velocityY": -1.860414915840738e-20, - "timestamp": 0.9829704450363669 - }, - { - "x": 2.2764423218798027, - "y": 6.964405806838171, - "heading": 2.828141101686341e-8, - "angularVelocity": 3.7402787103507053e-7, - "velocityX": -0.8646152527100619, - "velocityY": -0.18633427389118332, - "timestamp": 1.0585835561930106 - }, - { - "x": 2.145689833491728, - "y": 6.936227180653932, - "heading": 9.68470451708674e-8, - "angularVelocity": 9.067955680869744e-7, - "velocityX": -1.7292303727220222, - "velocityY": -0.3726685194299493, - "timestamp": 1.1341966673496542 - }, - { - "x": 1.9978576960446424, - "y": 6.904367698887054, - "heading": 0.07260812222938247, - "angularVelocity": 0.960257080705476, - "velocityX": -1.9551124822893438, - "velocityY": -0.42134864284155416, - "timestamp": 1.2098097785062978 - }, - { - "x": 1.8500255586147867, - "y": 6.872508217040216, - "heading": 0.1958338387782673, - "angularVelocity": 1.6296871622383675, - "velocityX": -1.955112482061473, - "velocityY": -0.42134864389901044, - "timestamp": 1.2854228896629414 - }, - { - "x": 1.7021934211910146, - "y": 6.840648735165152, - "heading": 0.3451117743398479, - "angularVelocity": 1.9742334798568144, - "velocityX": -1.95511248198102, - "velocityY": -0.42134864427231705, - "timestamp": 1.361036000819585 - }, - { - "x": 1.5543612837761693, - "y": 6.8087892532486665, - "heading": 0.5047130791830543, - "angularVelocity": 2.110762305661104, - "velocityX": -1.9551124818629584, - "velocityY": -0.42134864482013473, - "timestamp": 1.4366491119762286 - }, - { - "x": 1.4065291463620155, - "y": 6.776929771328972, - "heading": 0.6639739239039864, - "angularVelocity": 2.106259645777567, - "velocityX": -1.9551124818538146, - "velocityY": -0.4213486448625627, - "timestamp": 1.5122622231328722 - }, - { - "x": 1.2586970089400131, - "y": 6.745070289445696, - "heading": 0.8125476250392986, - "angularVelocity": 1.9649198249166078, - "velocityX": -1.9551124819576147, - "velocityY": -0.4213486443809204, - "timestamp": 1.5878753342895158 - }, - { - "x": 1.1108648715115719, - "y": 6.713210807592297, - "heading": 0.9352215452290742, - "angularVelocity": 1.622389534212375, - "velocityX": -1.9551124820427679, - "velocityY": -0.42134864398580435, - "timestamp": 1.6634884454461594 - }, - { - "x": 0.9630327340807132, - "y": 6.681351325750123, - "heading": 1.0074797622855187, - "angularVelocity": 0.9556307887761831, - "velocityX": -1.9551124820747394, - "velocityY": -0.42134864383734444, - "timestamp": 1.739101556602803 - }, - { - "x": 0.8322802456820239, - "y": 6.653172699615138, - "heading": 1.0074798310413775, - "angularVelocity": 9.093113311976794e-7, - "velocityX": -1.7292303728624039, - "velocityY": -0.3726685187785591, - "timestamp": 1.8147146677594466 + "heading": 2.0846959931491773e-18, + "angularVelocity": -8.557421751891334e-20, + "velocityX": -4.987723153383344e-19, + "velocityY": -9.652716908345432e-20, + "timestamp": 1.2336573910029416 + }, + { + "x": 2.3105578909850855, + "y": 6.971758087878447, + "heading": 3.828776753639658e-7, + "angularVelocity": 0.000004965756984393634, + "velocityX": -0.40543742966046686, + "velocityY": -0.08737639053285204, + "timestamp": 1.3107609779406149 + }, + { + "x": 2.2480365327877774, + "y": 6.958284019520034, + "heading": 0.0000011992649992501823, + "angularVelocity": 0.000010588188647324088, + "velocityX": -0.8108748332013094, + "velocityY": -0.17475280844332217, + "timestamp": 1.3878645648782881 + }, + { + "x": 2.154254500585808, + "y": 6.938072911430576, + "heading": 0.0000025503266632355084, + "angularVelocity": 0.000017522682376308707, + "velocityX": -1.2163121837350852, + "velocityY": -0.2621292846699298, + "timestamp": 1.4649681518159614 + }, + { + "x": 2.0292118068653178, + "y": 6.911124749085196, + "heading": 0.000004757557407174423, + "angularVelocity": 0.000028626823103883053, + "velocityX": -1.6217493723290508, + "velocityY": -0.34950594927788836, + "timestamp": 1.5420717387536347 + }, + { + "x": 1.8935401996566608, + "y": 6.881886171392507, + "heading": 0.06878997325201827, + "angularVelocity": 0.8921143415832743, + "velocityX": -1.7596017591026925, + "velocityY": -0.3792116405210038, + "timestamp": 1.619175325691308 + }, + { + "x": 1.7578686367778409, + "y": 6.852647388002959, + "heading": 0.20749385875571683, + "angularVelocity": 1.7989290902357107, + "velocityX": -1.7596011841639752, + "velocityY": -0.379214308319837, + "timestamp": 1.6962789126289812 + }, + { + "x": 1.6221970666934797, + "y": 6.823408638048033, + "heading": 0.39902661142653667, + "angularVelocity": 2.484096528811891, + "velocityX": -1.7596012776167158, + "velocityY": -0.3792138746873246, + "timestamp": 1.7733824995666545 + }, + { + "x": 1.4865254965870125, + "y": 6.794169888195682, + "heading": 0.6119135529013362, + "angularVelocity": 2.7610510733681806, + "velocityX": -1.7596012779034225, + "velocityY": -0.37921387335696644, + "timestamp": 1.8504860865043278 + }, + { + "x": 1.3508539260055112, + "y": 6.7649311405475485, + "heading": 0.8026194720400166, + "angularVelocity": 2.473372857385191, + "velocityX": -1.7596012840644037, + "velocityY": -0.37921384476922, + "timestamp": 1.927589673442001 + }, + { + "x": 1.2151823556967136, + "y": 6.735692391634036, + "heading": 0.9401132494355333, + "angularVelocity": 1.7832345141952992, + "velocityX": -1.7596012805275563, + "velocityY": -0.37921386118065376, + "timestamp": 2.0046932603796743 + }, + { + "x": 1.0795107810200666, + "y": 6.706453662988116, + "heading": 1.0074751648495315, + "angularVelocity": 0.873654755756699, + "velocityX": -1.7596013371766643, + "velocityY": -0.37921359831878204, + "timestamp": 2.0817968473173476 + }, + { + "x": 0.9544680762714443, + "y": 6.679505551891246, + "heading": 1.007477347596638, + "angularVelocity": 0.000028309280971946704, + "velocityX": -1.6217495153591333, + "velocityY": -0.34950528460696234, + "timestamp": 2.158900434255021 + }, + { + "x": 0.8606860387160242, + "y": 6.659294468642213, + "heading": 1.007478678720387, + "angularVelocity": 0.000017264096285479253, + "velocityX": -1.2163122531670139, + "velocityY": -0.26212896250040785, + "timestamp": 2.236004021192694 + }, + { + "x": 0.7981646777386805, + "y": 6.645820413183375, + "heading": 1.007479482462847, + "angularVelocity": 0.000010424190260479305, + "velocityX": -0.8108748692571571, + "velocityY": -0.17475264114145156, + "timestamp": 2.3131076081303674 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 3.75205950615561e-7, - "velocityX": -0.8646152527568558, - "velocityY": -0.18633427367405322, - "timestamp": 1.8903277789160902 + "angularVelocity": 0.0000048888648998734084, + "velocityX": -0.4054374447762054, + "velocityY": -0.08737632039468787, + "timestamp": 2.3902111950680407 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": -2.242417201812226e-19, - "velocityX": -4.158054673626952e-19, - "velocityY": -9.844851544165139e-20, - "timestamp": 1.9659408900727338 - }, - { - "x": 0.867342073873612, - "y": 6.677517805054488, - "heading": 1.0074798401596599, - "angularVelocity": -2.0078718725992087e-7, - "velocityX": 1.0474996006171284, - "velocityY": 0.4008443829976782, - "timestamp": 2.061824532746057 - }, - { - "x": 1.0464438315164646, - "y": 6.746054283251552, - "heading": 0.9900885492460468, - "angularVelocity": -0.18137912190993252, - "velocityX": 1.8679073160899227, - "velocityY": 0.7147880106158334, - "timestamp": 2.1577081754193803 - }, - { - "x": 1.2255455891596982, - "y": 6.814590761447637, - "heading": 0.9294452786879966, - "angularVelocity": -0.6324673204653116, - "velocityX": 1.8679073160938997, - "velocityY": 0.7147880106056311, - "timestamp": 2.2535918180927035 - }, - { - "x": 1.4046473468026037, - "y": 6.883127239644582, - "heading": 0.8469789832587827, - "angularVelocity": -0.8600663588697559, - "velocityX": 1.8679073160904744, - "velocityY": 0.7147880106145814, - "timestamp": 2.3494754607660266 - }, - { - "x": 1.5837491044451752, - "y": 6.951663717842399, - "heading": 0.7537506929542891, - "angularVelocity": -0.9723065134490515, - "velocityX": 1.867907316086992, - "velocityY": 0.7147880106236818, - "timestamp": 2.44535910343935 - }, - { - "x": 1.762850862087631, - "y": 7.020200196040518, - "heading": 0.6553282675051919, - "angularVelocity": -1.0264777464121126, - "velocityX": 1.867907316085786, - "velocityY": 0.7147880106268334, - "timestamp": 2.541242746112673 - }, - { - "x": 1.941952619730239, - "y": 7.088736674238239, - "heading": 0.5545759098990335, - "angularVelocity": -1.0507773254862967, - "velocityX": 1.8679073160873736, - "velocityY": 0.7147880106226845, - "timestamp": 2.637126388785996 - }, - { - "x": 2.121054377373199, - "y": 7.157273152435041, - "heading": 0.453150200963735, - "angularVelocity": -1.0577999135979546, - "velocityX": 1.8679073160910444, - "velocityY": 0.7147880106130912, - "timestamp": 2.7330100314593193 - }, - { - "x": 2.3001561350165884, - "y": 7.225809630630721, - "heading": 0.3523691136343556, - "angularVelocity": -1.0510769566060587, - "velocityX": 1.8679073160955193, - "velocityY": 0.7147880106013975, - "timestamp": 2.8288936741326425 - }, - { - "x": 2.479257892660336, - "y": 7.294346108825463, - "heading": 0.2538978395296278, - "angularVelocity": -1.026987204065876, - "velocityX": 1.867907316099261, - "velocityY": 0.7147880105916198, - "timestamp": 2.9247773168059656 - }, - { - "x": 2.658359650304228, - "y": 7.362882587019828, - "heading": 0.16061512982409304, - "angularVelocity": -0.9728740701200732, - "velocityX": 1.8679073161007647, - "velocityY": 0.7147880105876907, - "timestamp": 3.020660959479289 - }, - { - "x": 2.8374614079479796, - "y": 7.431419065214562, - "heading": 0.07810324033722603, - "angularVelocity": -0.8605418733202074, - "velocityX": 1.867907316099298, - "velocityY": 0.7147880105915232, - "timestamp": 3.116544602152612 - }, - { - "x": 3.0165631655914407, - "y": 7.499955543410054, - "heading": 0.01742736740360304, - "angularVelocity": -0.6328073406675466, - "velocityX": 1.8679073160962676, - "velocityY": 0.714788010599443, - "timestamp": 3.212428244825935 - }, - { - "x": 3.195664923235554, - "y": 7.568492021603823, - "heading": 1.924276877277615e-8, - "angularVelocity": -0.18175517403118957, - "velocityX": 1.867907316103074, - "velocityY": 0.7147880105814662, - "timestamp": 3.3083118874992583 + "angularVelocity": 6.720704922337583e-19, + "velocityX": -1.755081189133287e-18, + "velocityY": -1.4248543300928555e-19, + "timestamp": 2.467314782005714 + }, + { + "x": 0.8096592904256411, + "y": 6.655444460332552, + "heading": 1.0074798681868895, + "angularVelocity": 9.511953785975191e-8, + "velocityX": 0.4634590560851991, + "velocityY": 0.17735086375321324, + "timestamp": 2.5595673626742226 + }, + { + "x": 0.8951698755926256, + "y": 6.688166608994117, + "heading": 1.007479884316123, + "angularVelocity": 1.7483774726918832e-7, + "velocityX": 0.9269180823705058, + "velocityY": 0.3547017159243002, + "timestamp": 2.651819943342731 + }, + { + "x": 1.0234357437188477, + "y": 6.737249828252512, + "heading": 1.0074798953442166, + "angularVelocity": 1.1954238566332632e-7, + "velocityX": 1.3903770192307126, + "velocityY": 0.5320525334111323, + "timestamp": 2.74407252401124 + }, + { + "x": 1.1785230905174175, + "y": 6.796596764677482, + "heading": 0.989392624850058, + "angularVelocity": -0.19606248804195148, + "velocityX": 1.6811166221554836, + "velocityY": 0.6433092277192939, + "timestamp": 2.8363251046797484 + }, + { + "x": 1.3336104374264297, + "y": 6.855943700813832, + "heading": 0.9393084137910155, + "angularVelocity": -0.5429030894974105, + "velocityX": 1.68111662335266, + "velocityY": 0.6433092245907064, + "timestamp": 2.928577685348257 + }, + { + "x": 1.488697784324255, + "y": 6.9152906369794165, + "heading": 0.8650983768805, + "angularVelocity": -0.8044223410635456, + "velocityX": 1.681116623231394, + "velocityY": 0.6433092249076018, + "timestamp": 3.0208302660167656 + }, + { + "x": 1.6437851312260245, + "y": 6.974637573134693, + "heading": 0.7734662642227176, + "angularVelocity": -0.9932742476554033, + "velocityX": 1.6811166232741508, + "velocityY": 0.6433092247958684, + "timestamp": 3.1130828466852742 + }, + { + "x": 1.7988724781311176, + "y": 7.033984509281286, + "heading": 0.6701256962472372, + "angularVelocity": -1.120191622029676, + "velocityX": 1.6811166233101758, + "velocityY": 0.6433092247017268, + "timestamp": 3.205335427353783 + }, + { + "x": 1.9539598250398915, + "y": 7.093331445418259, + "heading": 0.5600545044204543, + "angularVelocity": -1.1931502731864139, + "velocityX": 1.6811166233500756, + "velocityY": 0.6433092245974589, + "timestamp": 3.2975880080222915 + }, + { + "x": 2.109047171953823, + "y": 7.152678381541754, + "heading": 0.4477833516576351, + "angularVelocity": -1.2169974210937597, + "velocityX": 1.6811166234059833, + "velocityY": 0.6433092244513593, + "timestamp": 3.3898405886908 + }, + { + "x": 2.2641345188735014, + "y": 7.21202531765023, + "heading": 0.33769407921933825, + "angularVelocity": -1.1933462634924075, + "velocityX": 1.6811166234682786, + "velocityY": 0.6433092242885672, + "timestamp": 3.4820931693593087 + }, + { + "x": 2.4192218657983178, + "y": 7.271372253745279, + "heading": 0.23431876445689956, + "angularVelocity": -1.120568270430258, + "velocityX": 1.6811166235239754, + "velocityY": 0.6433092241430183, + "timestamp": 3.5743457500278173 + }, + { + "x": 2.5743092127283593, + "y": 7.3307191898266755, + "heading": 0.14263672871411986, + "angularVelocity": -0.9938154041697848, + "velocityX": 1.6811166235806112, + "velocityY": 0.6433092239950159, + "timestamp": 3.666598330696326 + }, + { + "x": 2.7293965596646075, + "y": 7.390066125891852, + "heading": 0.06836140472618683, + "angularVelocity": -0.8051300402622511, + "velocityX": 1.681116623647891, + "velocityY": 0.6433092238191984, + "timestamp": 3.7588509113648345 + }, + { + "x": 2.8844839065819534, + "y": 7.449413062006423, + "heading": 0.0181937383834693, + "angularVelocity": -0.5438077285120623, + "velocityX": 1.6811166234429973, + "velocityY": 0.6433092243546334, + "timestamp": 3.851103492033343 + }, + { + "x": 3.0395712536852333, + "y": 7.5087599976351145, + "heading": -3.599554284111106e-8, + "angularVelocity": -0.19721696940259958, + "velocityX": 1.6811166254584862, + "velocityY": 0.64330921908778, + "timestamp": 3.9433560727018517 + }, + { + "x": 3.1678371216372656, + "y": 7.557843217348709, + "heading": -2.4922879509035407e-8, + "angularVelocity": 1.2002551312894157e-7, + "velocityX": 1.3903770173425265, + "velocityY": 0.5320525383454039, + "timestamp": 4.03560865337036 + }, + { + "x": 3.2533477067184124, + "y": 7.590565366234588, + "heading": -8.779782162288685e-9, + "angularVelocity": 1.7498802992739692e-7, + "velocityX": 0.926918081440039, + "velocityY": 0.35470171835582626, + "timestamp": 4.127861234038869 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -1.4869843514051715e-19, - "angularVelocity": -2.006887540420477e-7, - "velocityX": 1.0474996006097614, - "velocityY": 0.4008443830169303, - "timestamp": 3.4041955301725815 + "heading": 2.7720082526846182e-19, + "angularVelocity": 9.517112799549954e-8, + "velocityX": 0.4634590556993664, + "velocityY": 0.17735086476148368, + "timestamp": 4.2201138147073785 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -7.359523754309027e-20, - "angularVelocity": 1.5727023698046957e-20, - "velocityX": -4.856900753028092e-20, - "velocityY": -1.8631537844939477e-20, - "timestamp": 3.5000791728459046 + "heading": 1.3557196489765115e-19, + "angularVelocity": -6.565556495699838e-20, + "velocityX": 1.1847381270605898e-18, + "velocityY": 4.521287308945025e-19, + "timestamp": 4.3123663953758875 } ], "constraints": [ @@ -672,15 +822,13 @@ "last" ], "type": "MaxVelocity", - "velocity": 2 + "velocity": 1.8 }, { "scope": [ - "first", - "last" + 1 ], - "type": "MaxVelocity", - "velocity": 2 + "type": "WptZeroVelocity" } ], "usesControlIntervalGuessing": true, @@ -697,16 +845,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { - "x": 2.365093469619751, - "y": 4.115641117095947, + "x": 2.4236249923706055, + "y": 4.163747310638428, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { "x": 0.7823778390884399, @@ -715,7 +863,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 21 }, { "x": 2.8538730144500732, @@ -729,400 +877,490 @@ ], "trajectory": [ { - "x": 0.7823778390884399, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 3.4836656833317357e-19, - "velocityX": 5.792784763551635e-18, - "velocityY": -1.1468055635760044e-18, + "angularVelocity": -2.7495232446215487e-17, + "velocityX": -5.069420583996102e-17, + "velocityY": 1.192104631717783e-17, "timestamp": 0 }, { - "x": 0.848441149203722, - "y": 4.472500429335688, - "heading": -0.9928952478971514, - "angularVelocity": 5.064569480290326e-7, - "velocityX": 0.8673046782767408, - "velocityY": -0.20407170438519942, - "timestamp": 0.0761708218230117 - }, - { - "x": 0.9805677659026901, - "y": 4.4414118103682325, - "heading": -0.9928951486952988, - "angularVelocity": 0.0000013023602772620627, - "velocityX": 1.7346093101893227, - "velocityY": -0.40814341008019434, - "timestamp": 0.1523416436460234 - }, - { - "x": 1.1288597388405326, - "y": 4.406519593265519, - "heading": -0.9209894371787697, - "angularVelocity": 0.9440059828106777, - "velocityX": 1.9468343571559381, - "velocityY": -0.4580785170440767, - "timestamp": 0.2285124654690351 - }, - { - "x": 1.2771517113083277, - "y": 4.371627374165104, - "heading": -0.8000545061710758, - "angularVelocity": 1.587680533219069, - "velocityX": 1.9468343509849748, - "velocityY": -0.4580785432706628, - "timestamp": 0.3046832872920468 - }, - { - "x": 1.4254436833762532, - "y": 4.336735153365243, - "heading": -0.6538028446856889, - "angularVelocity": 1.9200483595308036, - "velocityX": 1.9468343457353292, - "velocityY": -0.45807856558166105, - "timestamp": 0.3808541091150585 - }, - { - "x": 1.57373565515521, - "y": 4.301842931337266, - "heading": -0.4970386957469823, - "angularVelocity": 2.058060359424233, - "velocityX": 1.9468343419416414, - "velocityY": -0.458078581704838, - "timestamp": 0.45702493093807023 - }, - { - "x": 1.7220276265458214, - "y": 4.26695070765882, - "heading": -0.3400744666794648, - "angularVelocity": 2.060687088715348, - "velocityX": 1.946834336843289, - "velocityY": -0.4580786033728385, - "timestamp": 0.5331957527610819 - }, - { - "x": 1.8703195977756357, - "y": 4.232058483296987, - "heading": -0.19340046314195958, - "angularVelocity": 1.9255930292876797, - "velocityX": 1.9468343347322845, - "velocityY": -0.45807861234460967, - "timestamp": 0.6093665745840936 - }, - { - "x": 2.0186115688161, - "y": 4.197166258130416, - "heading": -0.0721222371749164, - "angularVelocity": 1.592187442178866, - "velocityX": 1.946834332246427, - "velocityY": -0.45807862290950535, - "timestamp": 0.6855373964071053 - }, - { - "x": 2.1669035397570235, - "y": 4.162274032540797, - "heading": -1.3790420971273228e-7, - "angularVelocity": 0.946846804912877, - "velocityX": 1.9468343309396154, - "velocityY": -0.4580786284634526, - "timestamp": 0.761708218230117 - }, - { - "x": 2.299030158583636, - "y": 4.131185422616258, - "heading": -3.861728516505335e-8, - "angularVelocity": 0.0000013034771345390653, - "velocityX": 1.7346093381218648, - "velocityY": -0.4081432913612903, - "timestamp": 0.8378790400531287 - }, - { - "x": 2.365093469619751, + "x": 0.8155240483745178, + "y": 4.480245631878025, + "heading": -0.9928952427133843, + "angularVelocity": 5.500196348995766e-7, + "velocityX": 0.4166008785692591, + "velocityY": -0.09802372135573052, + "timestamp": 0.07956346467603777 + }, + { + "x": 0.8818164643488411, + "y": 4.46464741870627, + "heading": -0.9928951428202392, + "angularVelocity": 0.000001255529267463834, + "velocityX": 0.8332017244870482, + "velocityY": -0.19604743502871538, + "timestamp": 0.15912692935207554 + }, + { + "x": 0.9812550792231622, + "y": 4.441250101086857, + "heading": -0.9928949496672224, + "angularVelocity": 0.000002427691463657242, + "velocityX": 1.2498024725159502, + "velocityY": -0.2940711256682478, + "timestamp": 0.23869039402811332 + }, + { + "x": 1.1129174292533976, + "y": 4.410270729279662, + "heading": -0.988662240113345, + "angularVelocity": 0.05319916084717845, + "velocityX": 1.654809158529407, + "velocityY": -0.3893668021287818, + "timestamp": 0.31825385870415107 + }, + { + "x": 1.244579779283711, + "y": 4.379291357472773, + "heading": -0.9114902323862101, + "angularVelocity": 0.969942775132182, + "velocityX": 1.6548091585300193, + "velocityY": -0.3893668021248496, + "timestamp": 0.3978173233801888 + }, + { + "x": 1.3762421293125207, + "y": 4.3483119856594845, + "heading": -0.7738179037346776, + "angularVelocity": 1.7303460729208497, + "velocityX": 1.6548091585111195, + "velocityY": -0.3893668022052987, + "timestamp": 0.4773807880562266 + }, + { + "x": 1.507904479337117, + "y": 4.317332613828277, + "heading": -0.594124699134844, + "angularVelocity": 2.2584889349524437, + "velocityX": 1.6548091584581641, + "velocityY": -0.3893668024305141, + "timestamp": 0.5569442527322643 + }, + { + "x": 1.6395668293620107, + "y": 4.286353241998326, + "heading": -0.39886759130073074, + "angularVelocity": 2.4541051426819718, + "velocityX": 1.654809158461902, + "velocityY": -0.3893668024147043, + "timestamp": 0.6365077174083021 + }, + { + "x": 1.7712291793925206, + "y": 4.25537387019225, + "heading": -0.21914201405263553, + "angularVelocity": 2.258895813265647, + "velocityX": 1.6548091585324882, + "velocityY": -0.38936680211463054, + "timestamp": 0.71607118208434 + }, + { + "x": 1.9028915294236735, + "y": 4.2243944983889214, + "heading": -0.08144285462028061, + "angularVelocity": 1.7306832978144895, + "velocityX": 1.6548091585405724, + "velocityY": -0.3893668020801211, + "timestamp": 0.7956346467603778 + }, + { + "x": 2.034553879453989, + "y": 4.1934151265820425, + "heading": -0.004251710722878646, + "angularVelocity": 0.9701832896755161, + "velocityX": 1.6548091585300484, + "velocityY": -0.38936680212472563, + "timestamp": 0.8751981114364156 + }, + { + "x": 2.166216229487868, + "y": 4.162435754790337, + "heading": -3.367292673298042e-7, + "angularVelocity": 0.053433746406308845, + "velocityX": 1.654809158575206, + "velocityY": -0.3893668019341025, + "timestamp": 0.9547615761124534 + }, + { + "x": 2.2656548443604327, + "y": 4.139038437163738, + "heading": -1.4361728442139784e-7, + "angularVelocity": 0.000002427175717358418, + "velocityX": 1.2498024724938752, + "velocityY": -0.2940711257585697, + "timestamp": 1.0343250407884912 + }, + { + "x": 2.331947260333981, + "y": 4.123440223988747, + "heading": -4.37491568242842e-8, + "angularVelocity": 0.0000012552148287255357, + "velocityX": 0.8332017244772982, + "velocityY": -0.1960474350693968, + "timestamp": 1.1138885054645289 + }, + { + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -2.2763209665573703e-18, - "angularVelocity": 5.069826509442209e-7, - "velocityX": 0.8673046903657866, - "velocityY": -0.2040716530068378, - "timestamp": 0.9140498618761405 + "heading": 1.4952923019590338e-16, + "angularVelocity": 5.498706671572415e-7, + "velocityX": 0.41660087856538297, + "velocityY": -0.09802372137198472, + "timestamp": 1.1934519701405666 }, { - "x": 2.365093469619751, + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -1.5143047799084299e-18, - "angularVelocity": 5.84054737228804e-20, - "velocityX": -8.219525464434245e-19, - "velocityY": 2.1312698857170775e-19, - "timestamp": 0.9902206836991522 - }, - { - "x": 2.299030158572247, - "y": 4.131185422567857, - "heading": -3.8824131952933046e-8, - "angularVelocity": -5.096982155619529e-7, - "velocityX": -0.867304690515307, - "velocityY": 0.20407165237139407, - "timestamp": 1.0663915055221642 - }, - { - "x": 2.166903539754318, - "y": 4.162274032529242, - "heading": -1.3861601297880188e-7, - "angularVelocity": -0.000001310106398423391, - "velocityX": -1.7346093380078471, - "velocityY": 0.408143291845037, - "timestamp": 1.1425623273451764 - }, - { - "x": 2.018611568604941, - "y": 4.197166257232936, - "heading": -0.0722676107720337, - "angularVelocity": -0.948755316359051, - "velocityX": -1.9468343336762697, - "velocityY": 0.4580786168326712, - "timestamp": 1.2187331491681885 - }, - { - "x": 1.8703195973960691, - "y": 4.2320584816837785, - "heading": -0.19373209136885944, - "angularVelocity": -1.5946326649731632, - "velocityX": -1.946834334457335, - "velocityY": 0.458078613513146, - "timestamp": 1.2949039709912007 - }, - { - "x": 1.7220276260831988, - "y": 4.2669507056926275, - "heading": -0.34047859723492613, - "angularVelocity": -1.9265448678897275, - "velocityX": -1.9468343358226654, - "velocityY": 0.45807860771049036, - "timestamp": 1.3710747928142129 - }, - { - "x": 1.5737356547013517, - "y": 4.301842929408325, - "heading": -0.4972933443760097, - "angularVelocity": -2.058724632188595, - "velocityX": -1.946834336728219, - "velocityY": 0.4580786038618867, - "timestamp": 1.447245614637225 - }, - { - "x": 1.4254436831497614, - "y": 4.336735152402613, - "heading": -0.6538038889287485, - "angularVelocity": -2.0547309429902363, - "velocityX": -1.9468343389566751, - "velocityY": 0.458078594390947, - "timestamp": 1.5234164364602372 - }, - { - "x": 1.2771517114677216, - "y": 4.371627374842491, - "heading": -0.799921474135194, - "angularVelocity": -1.9182881543008596, - "velocityX": -1.946834340669268, - "velocityY": 0.45807858711242555, - "timestamp": 1.5995872582832493 - }, - { - "x": 1.1288597396330466, - "y": 4.406519596633671, - "heading": -0.9209030529092569, - "angularVelocity": -1.5882929431321022, - "velocityX": -1.9468343426731174, - "velocityY": 0.4580785785960651, - "timestamp": 1.6757580801062615 - }, - { - "x": 0.9805677676343831, - "y": 4.4414118177279, - "heading": -0.9928951480324381, - "angularVelocity": -0.945140060198636, - "velocityX": -1.946834344826023, - "velocityY": 0.45807856944623543, - "timestamp": 1.7519289019292736 - }, - { - "x": 0.8484411497365192, - "y": 4.472500431600075, - "heading": -0.9928952477030265, - "angularVelocity": -0.0000013085140236118818, - "velocityX": -1.734609325928881, - "velocityY": 0.40814334318738743, - "timestamp": 1.8280997237522858 - }, - { - "x": 0.7823778390884398, + "heading": 9.94909152424984e-17, + "angularVelocity": -3.9263017083187414e-18, + "velocityX": 3.083103040454795e-18, + "velocityY": -7.248912091154562e-19, + "timestamp": 1.2730154348166043 + }, + { + "x": 2.331947260333817, + "y": 4.12344022398805, + "heading": -4.377032610264403e-8, + "angularVelocity": -5.501367379565397e-7, + "velocityX": -0.416600878567453, + "velocityY": 0.09802372136321691, + "timestamp": 1.3525788994926409 + }, + { + "x": 2.2656548443599256, + "y": 4.1390384371615845, + "heading": -1.4368564209179275e-7, + "angularVelocity": -0.0000012558079284124016, + "velocityX": -0.8332017244816303, + "velocityY": 0.1960474350510945, + "timestamp": 1.4321423641686775 + }, + { + "x": 2.1662162294867975, + "y": 4.162435754785825, + "heading": -3.368854431485356e-7, + "angularVelocity": -0.000002428279485036437, + "velocityX": -1.249802472500972, + "velocityY": 0.2940711257289318, + "timestamp": 1.511705828844714 + }, + { + "x": 2.034553879452772, + "y": 4.193415126576901, + "heading": -0.004253358762050876, + "angularVelocity": -0.05345445796038289, + "velocityX": -1.6548091585770712, + "velocityY": 0.38936680192617446, + "timestamp": 1.5912692935207506 + }, + { + "x": 1.902891529421956, + "y": 4.224394498381643, + "heading": -0.08145530813915991, + "angularVelocity": -0.9703190992427685, + "velocityX": -1.6548091585363642, + "velocityY": 0.38936680209788715, + "timestamp": 1.6708327581967872 + }, + { + "x": 1.7712291793910706, + "y": 4.255373870186102, + "heading": -0.21916938411480638, + "angularVelocity": -1.7308707776251606, + "velocityX": -1.6548091585372338, + "velocityY": 0.38936680209431446, + "timestamp": 1.7503962228728238 + }, + { + "x": 1.6395668293638401, + "y": 4.286353242006105, + "heading": -0.3988962080198311, + "angularVelocity": -2.2589114819767695, + "velocityX": -1.6548091584912985, + "velocityY": 0.38936680228969167, + "timestamp": 1.8299596875488604 + }, + { + "x": 1.50790447934162, + "y": 4.31733261384741, + "heading": -0.5941239976444426, + "angularVelocity": -2.4537366543360197, + "velocityX": -1.6548091584283233, + "velocityY": 0.38936680255742584, + "timestamp": 1.909523152224897 + }, + { + "x": 1.3762421293155438, + "y": 4.34831198567232, + "heading": -0.7738010431009228, + "angularVelocity": -2.2582858374199257, + "velocityX": -1.6548091584767906, + "velocityY": 0.38936680235136056, + "timestamp": 1.9890866169009336 + }, + { + "x": 1.2445797792853979, + "y": 4.379291357479921, + "heading": -0.911479619218358, + "angularVelocity": -1.7304245947137935, + "velocityX": -1.6548091585279405, + "velocityY": 0.38936680213381303, + "timestamp": 2.06865008157697 + }, + { + "x": 1.1129174292546509, + "y": 4.410270729284957, + "heading": -0.9886607024030146, + "angularVelocity": -0.9700568407721322, + "velocityX": -1.6548091585354978, + "velocityY": 0.38936680210156993, + "timestamp": 2.1482135462530065 + }, + { + "x": 0.9812550792241712, + "y": 4.441250101091103, + "heading": -0.9928949494741156, + "angularVelocity": -0.05321848525978111, + "velocityX": -1.6548091585325024, + "velocityY": 0.38936680211562363, + "timestamp": 2.227777010929043 + }, + { + "x": 0.8818164643493253, + "y": 4.46464741870832, + "heading": -0.992895142733805, + "angularVelocity": -0.0000024290322074090013, + "velocityX": -1.2498024725225658, + "velocityY": 0.2940711256406529, + "timestamp": 2.3073404756050797 + }, + { + "x": 0.8155240483746753, + "y": 4.4802456318786925, + "heading": -0.9928952426862623, + "angularVelocity": -0.0000012562747482591942, + "velocityX": -0.8332017244911675, + "velocityY": 0.19604743501134286, + "timestamp": 2.3869039402811163 + }, + { + "x": 0.78237783908844, "y": 4.488044738769531, - "heading": -0.9928952864743934, - "angularVelocity": -5.09005494945374e-7, - "velocityX": -0.8673046852715057, - "velocityY": 0.2040716746574537, - "timestamp": 1.904270545575298 + "heading": -0.9928952864743935, + "angularVelocity": -5.503605231480291e-7, + "velocityX": -0.41660087857124434, + "velocityY": 0.0980237213473347, + "timestamp": 2.466467404957153 }, { - "x": 0.7823778390884398, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 1.8053835791622905e-18, - "velocityX": 3.8085009658841144e-17, - "velocityY": -4.441213712458509e-17, - "timestamp": 1.9804413673983101 - }, - { - "x": 0.8633075709529878, - "y": 4.398931081775789, - "heading": -0.9928954814598945, - "angularVelocity": -0.0000019220769098791852, - "velocityX": 0.7977678757913192, - "velocityY": -0.8784412255669002, - "timestamp": 2.0818865801229673 - }, - { - "x": 0.9997101217758085, - "y": 4.248734981396651, - "heading": -0.9705934228713969, - "angularVelocity": 0.21984338136319737, - "velocityX": 1.3445932751212721, - "velocityY": -1.480563708676927, - "timestamp": 2.1833317928476244 - }, - { - "x": 1.1361126726042652, - "y": 4.098538881022651, - "heading": -0.9135846602313676, - "angularVelocity": 0.5619660219429253, - "velocityX": 1.3445932751768286, - "velocityY": -1.4805637086262706, - "timestamp": 2.2847770055722814 - }, - { - "x": 1.2725152234348027, - "y": 3.948342780650542, - "heading": -0.8396943742506122, - "angularVelocity": 0.7283762732235443, - "velocityX": 1.3445932751973406, - "velocityY": -1.4805637086076424, - "timestamp": 2.3862222182969384 - }, - { - "x": 1.4089177742668983, - "y": 3.7981466802798476, - "heading": -0.7577094886902542, - "angularVelocity": 0.808169093034302, - "velocityX": 1.3445932752127017, - "velocityY": -1.4805637085936922, - "timestamp": 2.4876674310215954 - }, - { - "x": 1.5453203251000565, - "y": 3.6479505799101184, - "heading": -0.6718990052626181, - "angularVelocity": 0.8458800678997359, - "velocityX": 1.3445932752231742, - "velocityY": -1.4805637085841812, - "timestamp": 2.5891126437462524 - }, - { - "x": 1.6817228759337532, - "y": 3.4977544795408777, - "heading": -0.5843529548214927, - "angularVelocity": 0.8629884850135144, - "velocityX": 1.3445932752284844, - "velocityY": -1.4805637085793588, - "timestamp": 2.6905578564709094 - }, - { - "x": 1.8181254267674478, - "y": 3.3475583791716352, - "heading": -0.496152534626129, - "angularVelocity": 0.8694389594781329, - "velocityX": 1.3445932752284602, - "velocityY": -1.4805637085793808, - "timestamp": 2.7920030691955664 - }, - { - "x": 1.9545279776008493, - "y": 3.1973622788021268, - "heading": -0.4079812901328632, - "angularVelocity": 0.8691513588973434, - "velocityX": 1.3445932752255727, - "velocityY": -1.480563708582003, - "timestamp": 2.8934482819202234 - }, - { - "x": 2.0909305284339355, - "y": 3.0471661784323323, - "heading": -0.32051597252506797, - "angularVelocity": 0.8621926580724313, - "velocityX": 1.344593275222464, - "velocityY": -1.4805637085848262, - "timestamp": 2.9948934946448804 - }, - { - "x": 2.227333079266667, - "y": 2.896970078062216, - "heading": -0.234818517184951, - "angularVelocity": 0.8447658892757932, - "velocityX": 1.3445932752189704, - "velocityY": -1.4805637085879988, - "timestamp": 3.0963387073695374 - }, - { - "x": 2.3637356300989167, - "y": 2.7467739776916615, - "heading": -0.15295154788429333, - "angularVelocity": 0.8070067290692294, - "velocityX": 1.3445932752142213, - "velocityY": -1.480563708592312, - "timestamp": 3.1977839200941944 - }, - { - "x": 2.5001381809305556, - "y": 2.596577877320553, - "heading": -0.07915862654379939, - "angularVelocity": 0.7274164976200789, - "velocityX": 1.3445932752082013, - "velocityY": -1.4805637085977792, - "timestamp": 3.2992291328188514 - }, - { - "x": 2.6365407317614795, - "y": 2.4463817769487943, - "heading": -0.02222133180148921, - "angularVelocity": 0.5612615244531013, - "velocityX": 1.3445932752011478, - "velocityY": -1.480563708604185, - "timestamp": 3.4006743455435084 - }, - { - "x": 2.7729432825902887, - "y": 2.296185676575095, - "heading": 1.9540240946006604e-7, - "angularVelocity": 0.21904954021056072, - "velocityX": 1.3445932751803078, - "velocityY": -1.480563708623314, - "timestamp": 3.5021195582681655 - }, - { - "x": 2.8538730144500737, - "y": 2.207072019577026, - "heading": -5.5771741386668314e-18, - "angularVelocity": -0.0000019261866008177493, - "velocityX": 0.7977678757443631, - "velocityY": -0.8784412256095482, - "timestamp": 3.6035647709928225 + "angularVelocity": 2.916682100466704e-17, + "velocityX": 2.7675115242860868e-17, + "velocityY": -1.1179897514445951e-17, + "timestamp": 2.5460308696331895 + }, + { + "x": 0.8196446190430545, + "y": 4.447009398972592, + "heading": -0.9928952805674836, + "angularVelocity": 5.81882889925494e-8, + "velocityX": 0.3671091090870526, + "velocityY": -0.4042325913933224, + "timestamp": 2.64754504902057 + }, + { + "x": 0.8941781769785785, + "y": 4.364938721552028, + "heading": -0.99289526724749, + "angularVelocity": 1.3121372132974804e-7, + "velocityX": 0.734218198731366, + "velocityY": -0.8084651613775955, + "timestamp": 2.7490592284079503 + }, + { + "x": 1.0059785069740759, + "y": 4.241832713027579, + "heading": -0.9928952419960708, + "angularVelocity": 2.4874881077364186e-7, + "velocityX": 1.1013272300492336, + "velocityY": -1.2126976671366712, + "timestamp": 2.8505734077953306 + }, + { + "x": 1.12199949551598, + "y": 4.114079236762414, + "heading": -0.9645969367606738, + "angularVelocity": 0.27876209418441944, + "velocityX": 1.1429042646264433, + "velocityY": -1.258479131054583, + "timestamp": 2.952087587182711 + }, + { + "x": 1.23802048405803, + "y": 3.986325760497411, + "heading": -0.9142950585420234, + "angularVelocity": 0.4955157843191217, + "velocityX": 1.1429042646278826, + "velocityY": -1.2584791310529981, + "timestamp": 3.0536017665700914 + }, + { + "x": 1.3540414726000847, + "y": 3.8585722842324115, + "heading": -0.8476346267532343, + "angularVelocity": 0.6566612880194648, + "velocityX": 1.1429042646279266, + "velocityY": -1.2584791310529604, + "timestamp": 3.155115945957472 + }, + { + "x": 1.4700624611421922, + "y": 3.73081880796746, + "heading": -0.7691366490852417, + "angularVelocity": 0.7732710655955318, + "velocityX": 1.142904264628449, + "velocityY": -1.2584791310524879, + "timestamp": 3.256630125344852 + }, + { + "x": 1.5860834496844116, + "y": 3.60306533170261, + "heading": -0.6824653897720189, + "angularVelocity": 0.853784760278754, + "velocityX": 1.142904264629548, + "velocityY": -1.2584791310514911, + "timestamp": 3.3581443047322326 + }, + { + "x": 1.7021044382267643, + "y": 3.475311855437881, + "heading": -0.5906727010032288, + "angularVelocity": 0.9042351455004396, + "velocityX": 1.142904264630865, + "velocityY": -1.2584791310502963, + "timestamp": 3.459658484119613 + }, + { + "x": 1.8181254267692102, + "y": 3.3475583791732366, + "heading": -0.4964175576320614, + "angularVelocity": 0.9284923932783603, + "velocityX": 1.1429042646317817, + "velocityY": -1.2584791310494643, + "timestamp": 3.5611726635069934 + }, + { + "x": 1.9341464153116614, + "y": 3.219804902908597, + "heading": -0.40216661154016314, + "angularVelocity": 0.9284510465493182, + "velocityX": 1.1429042646318321, + "velocityY": -1.2584791310494186, + "timestamp": 3.6626868428943737 + }, + { + "x": 2.0501674038540294, + "y": 3.0920514266438817, + "heading": -0.3103850714051686, + "angularVelocity": 0.904125322085988, + "velocityX": 1.142904264631013, + "velocityY": -1.2584791310501617, + "timestamp": 3.764201022281754 + }, + { + "x": 2.1661883923962715, + "y": 2.964297950379052, + "heading": -0.22372826223723585, + "angularVelocity": 0.8536424142008245, + "velocityX": 1.1429042646297711, + "velocityY": -1.2584791310512888, + "timestamp": 3.8657152016691345 + }, + { + "x": 2.2822093809384034, + "y": 2.836544474114123, + "heading": -0.1452438497360607, + "angularVelocity": 0.773137437299473, + "velocityX": 1.1429042646286902, + "velocityY": -1.2584791310522687, + "timestamp": 3.967229381056515 + }, + { + "x": 2.3982303694804767, + "y": 2.7087909978491407, + "heading": -0.07859314585229263, + "angularVelocity": 0.6565654599777103, + "velocityX": 1.1429042646281113, + "velocityY": -1.2584791310527925, + "timestamp": 4.068743560443895 + }, + { + "x": 2.5142513580225363, + "y": 2.5810375215841455, + "heading": -0.02829642791844555, + "angularVelocity": 0.4954649511760642, + "velocityX": 1.14290426462797, + "velocityY": -1.2584791310529186, + "timestamp": 4.170257739831276 + }, + { + "x": 2.6302723465645417, + "y": 2.453284045319074, + "heading": -4.448219856121038e-8, + "angularVelocity": 0.27874316284732203, + "velocityX": 1.1429042646274454, + "velocityY": -1.2584791310536732, + "timestamp": 4.271771919218656 + }, + { + "x": 2.742072676559976, + "y": 2.330178036794566, + "heading": -1.922893514266496e-8, + "angularVelocity": 2.4876697841719244e-7, + "velocityX": 1.1013272300486043, + "velocityY": -1.2126976671372425, + "timestamp": 4.3732860986060365 + }, + { + "x": 2.8166062344954708, + "y": 2.2481073593739755, + "heading": -5.9076026803677414e-9, + "angularVelocity": 1.3122691044422866e-7, + "velocityX": 0.7342181987310801, + "velocityY": -0.8084651613778552, + "timestamp": 4.474800277993417 }, { "x": 2.8538730144500732, "y": 2.2070720195770264, - "heading": -2.725395356358997e-18, - "angularVelocity": 1.2458293783490516e-18, - "velocityX": 3.57553609530669e-17, - "velocityY": -3.937144509458571e-17, - "timestamp": 3.7050099837174795 + "heading": 7.504980978671108e-18, + "angularVelocity": 5.819511377556866e-8, + "velocityX": 0.3671091090869381, + "velocityY": -0.40423259139342627, + "timestamp": 4.576314457380797 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": 3.684608436293488e-18, + "angularVelocity": -1.3374320571761547e-18, + "velocityX": 2.8725878976324764e-18, + "velocityY": -3.1629865336005647e-18, + "timestamp": 4.677828636768178 } ], "constraints": [ @@ -1168,7 +1406,13 @@ "last" ], "type": "MaxVelocity", - "velocity": 2 + "velocity": 1.7 + }, + { + "scope": [ + 1 + ], + "type": "WptZeroVelocity" } ], "usesControlIntervalGuessing": true, @@ -1185,7 +1429,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 19 }, { "x": 2.8771493434906006, @@ -1202,154 +1446,181 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": -4.047258922723054e-33, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": 0, + "velocityX": 1.674488653195789e-33, + "velocityY": 7.11171747202546e-38, "timestamp": 0 }, { - "x": 1.3562741157973404, - "y": 5.551277015096179, - "heading": 5.1790019930154625e-8, - "angularVelocity": 8.222689572844604e-7, - "velocityX": 0.6926593812833762, - "velocityY": -0.2510250916993935, - "timestamp": 0.06298428215226594 - }, - { - "x": 1.4435274144279229, - "y": 5.519655747707938, - "heading": 1.8284200071758018e-7, - "angularVelocity": 0.0000020807092906136113, - "velocityX": 1.3853186167883211, - "velocityY": -0.5020501354892848, - "timestamp": 0.12596856430453188 - }, - { - "x": 1.561958503686593, - "y": 5.476735411831991, - "heading": 0.03758882643806014, - "angularVelocity": 0.5967940303771042, - "velocityX": 1.8803276819502406, - "velocityY": -0.6814451861527359, - "timestamp": 0.18895284645679783 - }, - { - "x": 1.6803895924155994, - "y": 5.433815074494518, - "heading": 0.1495361372904831, - "angularVelocity": 1.7773848812277921, - "velocityX": 1.8803276735407826, - "velocityY": -0.6814452093573423, - "timestamp": 0.25193712860906375 - }, - { - "x": 1.7988206811669072, - "y": 5.3908947372185825, - "heading": 0.3084692846412802, - "angularVelocity": 2.5233779273148262, - "velocityX": 1.8803276738948609, - "velocityY": -0.6814452083803143, - "timestamp": 0.3149214107613297 - }, - { - "x": 1.9172517698549563, - "y": 5.347974399768096, - "heading": 0.4903249170402497, - "angularVelocity": 2.887317695537583, - "velocityX": 1.880327672890501, - "velocityY": -0.6814452111516576, - "timestamp": 0.3779056929135956 - }, - { - "x": 2.0356828583945066, - "y": 5.305054061907854, - "heading": 0.6822100511004133, - "angularVelocity": 3.0465558628782476, - "velocityX": 1.880327670532791, - "velocityY": -0.6814452176573361, - "timestamp": 0.44088997506586153 - }, - { - "x": 2.1541139468698915, - "y": 5.26213372387056, - "heading": 0.8783830367201194, - "angularVelocity": 3.1146339835302603, - "velocityX": 1.880327669514046, - "velocityY": -0.6814452204683829, - "timestamp": 0.5038742572181275 - }, - { - "x": 2.272545035456946, - "y": 5.219213386141397, - "heading": 1.0737017740998906, - "angularVelocity": 3.1010711038602228, - "velocityX": 1.8803276712870123, - "velocityY": -0.6814452155761989, - "timestamp": 0.5668585393703934 - }, - { - "x": 2.390976124200322, - "y": 5.176293048843576, - "heading": 1.2594557238711797, - "angularVelocity": 2.949211190852737, - "velocityX": 1.8803276737689296, - "velocityY": -0.6814452087277921, - "timestamp": 0.6298428215226594 - }, - { - "x": 2.5094072129682594, - "y": 5.133372711613527, - "heading": 1.4204186158420298, - "angularVelocity": 2.5556041359925064, - "velocityX": 1.8803276741588917, - "velocityY": -0.6814452076517687, - "timestamp": 0.6928271036749253 - }, - { - "x": 2.627838301697845, - "y": 5.090452374277653, - "heading": 1.5329754550704864, - "angularVelocity": 1.787062349243704, - "velocityX": 1.880327673549982, - "velocityY": -0.6814452093319581, - "timestamp": 0.7558113858271912 - }, - { - "x": 2.746269390946397, - "y": 5.047532038373786, - "heading": 1.5707961445746523, - "angularVelocity": 0.6004782179263964, - "velocityX": 1.8803276817895906, - "velocityY": -0.6814451865960204, - "timestamp": 0.8187956679794571 - }, - { - "x": 2.8335226895825705, - "y": 5.015910771000973, - "heading": 1.570796275196268, - "angularVelocity": 0.000002073876396402678, - "velocityX": 1.3853186168770866, - "velocityY": -0.5020501352443532, - "timestamp": 0.8817799501317231 + "x": 1.34929738878066, + "y": 5.5538054348907835, + "heading": 1.2355468400098194e-8, + "angularVelocity": 1.451370137183604e-7, + "velocityX": 0.4305187599139744, + "velocityY": -0.156023310052692, + "timestamp": 0.08512968609500099 + }, + { + "x": 1.4225972370219229, + "y": 5.527241006054132, + "heading": 4.5628523121809076e-8, + "angularVelocity": 3.908513740983071e-7, + "velocityX": 0.8610374547776837, + "velocityY": -0.31204659684762154, + "timestamp": 0.17025937219000198 + }, + { + "x": 1.5186402607278184, + "y": 5.492434275732478, + "heading": 0.023798969361148174, + "angularVelocity": 0.2795608068619741, + "velocityX": 1.1281966152056035, + "velocityY": -0.4088671287100916, + "timestamp": 0.25538905828500297 + }, + { + "x": 1.614683284388423, + "y": 5.457627545285846, + "heading": 0.08983587293589476, + "angularVelocity": 0.7757212155234814, + "velocityX": 1.1281966146735791, + "velocityY": -0.40886713017817483, + "timestamp": 0.34051874438000396 + }, + { + "x": 1.7107263080490587, + "y": 5.4228208148393, + "heading": 0.19002287752886737, + "angularVelocity": 1.1768750619045902, + "velocityX": 1.1281966146739437, + "velocityY": -0.4088671301771691, + "timestamp": 0.42564843047500495 + }, + { + "x": 1.806769331709714, + "y": 5.388014084392808, + "heading": 0.3166500634968596, + "angularVelocity": 1.4874621507082946, + "velocityX": 1.1281966146741744, + "velocityY": -0.40886713017653226, + "timestamp": 0.5107781165700059 + }, + { + "x": 1.902812355370287, + "y": 5.35320735394609, + "heading": 0.46249608510530194, + "angularVelocity": 1.7132216539091256, + "velocityX": 1.1281966146732083, + "velocityY": -0.4088671301791978, + "timestamp": 0.5959078026650069 + }, + { + "x": 1.9988553790306578, + "y": 5.318400623498813, + "heading": 0.6208292331181784, + "angularVelocity": 1.8599052254954127, + "velocityX": 1.1281966146708353, + "velocityY": -0.408867130185746, + "timestamp": 0.6810374887600079 + }, + { + "x": 2.094898402690861, + "y": 5.283593893051074, + "heading": 0.7853048024978768, + "angularVelocity": 1.9320589200358482, + "velocityX": 1.1281966146688651, + "velocityY": -0.40886713019118176, + "timestamp": 0.766167174855009 + }, + { + "x": 2.1909414263511025, + "y": 5.248787162603439, + "heading": 0.9497879513318085, + "angularVelocity": 1.9321479542444888, + "velocityX": 1.1281966146693132, + "velocityY": -0.40886713018994547, + "timestamp": 0.85129686095001 + }, + { + "x": 2.286984450011549, + "y": 5.213980432156372, + "heading": 1.108141013047214, + "angularVelocity": 1.8601391474495799, + "velocityX": 1.128196614671724, + "velocityY": -0.4088671301832935, + "timestamp": 0.936426547045011 + }, + { + "x": 2.383027473672174, + "y": 5.1791737017097965, + "heading": 1.254013432652853, + "angularVelocity": 1.7135317454695138, + "velocityX": 1.1281966146738198, + "velocityY": -0.40886713017751036, + "timestamp": 1.021556233140012 + }, + { + "x": 2.479070497332845, + "y": 5.144366971263349, + "heading": 1.3806693463444173, + "angularVelocity": 1.4877996090602523, + "velocityX": 1.1281966146743598, + "velocityY": -0.4088671301760209, + "timestamp": 1.1066859192350131 + }, + { + "x": 2.5751135209934812, + "y": 5.109560240816806, + "heading": 1.4808868727958657, + "angularVelocity": 1.1772335955709967, + "velocityX": 1.128196614673956, + "velocityY": -0.40886713017713466, + "timestamp": 1.1918156053300142 + }, + { + "x": 2.6711565446540857, + "y": 5.074753510370173, + "heading": 1.5469577993154264, + "angularVelocity": 0.776120875693452, + "velocityX": 1.1281966146735771, + "velocityY": -0.4088671301781801, + "timestamp": 1.2769452914250152 + }, + { + "x": 2.7671995683594357, + "y": 5.039946780047014, + "heading": 1.5707962811320204, + "angularVelocity": 0.2800254871137584, + "velocityX": 1.1281966151991965, + "velocityY": -0.4088671287277703, + "timestamp": 1.3620749775200163 + }, + { + "x": 2.840499416601011, + "y": 5.0133823512112246, + "heading": 1.5707963144274508, + "angularVelocity": 3.911142169044648e-7, + "velocityX": 0.8610374547813509, + "velocityY": -0.3120465968375025, + "timestamp": 1.4472046636150173 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 8.19230237095319e-7, - "velocityX": 0.6926593813129686, - "velocityY": -0.25102509161774106, - "timestamp": 0.944764232283989 + "angularVelocity": 1.4527771000322412e-7, + "velocityX": 0.4305187599151969, + "velocityY": -0.15602331004931888, + "timestamp": 1.5323343497100184 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 0, - "velocityX": -1.88980553526586e-32, + "angularVelocity": 1.8805605259344148e-32, + "velocityX": 2.677429684849837e-34, "velocityY": 0, - "timestamp": 1.007748514436255 + "timestamp": 1.6174640358050194 } ], "constraints": [ @@ -1371,7 +1642,7 @@ "last" ], "type": "MaxVelocity", - "velocity": 2 + "velocity": 1.2 } ], "usesControlIntervalGuessing": true, @@ -1388,7 +1659,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 27 }, { "x": 2.832932472229004, @@ -1402,220 +1673,966 @@ ], "trajectory": [ { - "x": 0.7669039964675903, + "x": 0.7669039964675901, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 4.703190638042804e-18, - "velocityX": -1.208675917564465e-17, - "velocityY": -6.352810871367948e-18, + "angularVelocity": 1.0512088330044834e-17, + "velocityX": 1.2971420838221857e-16, + "velocityY": 6.851970292534496e-17, "timestamp": 0 }, { - "x": 0.8020378021280192, - "y": 6.657644636391979, - "heading": 1.007479841292087, - "angularVelocity": -3.1089087057766036e-7, - "velocityX": 0.6028069120245683, - "velocityY": 0.31846394498851993, - "timestamp": 0.05828368082645525 - }, - { - "x": 0.8723054048773754, - "y": 6.694767133707721, - "heading": 1.0074798016172224, - "angularVelocity": -6.807244191958373e-7, - "velocityX": 1.2056136769836088, - "velocityY": 0.6369278121994305, - "timestamp": 0.1165673616529105 - }, - { - "x": 0.9753734970192464, - "y": 6.749218186944615, - "heading": 0.9990535149100053, - "angularVelocity": -0.14457368832954978, - "velocityX": 1.7683868053695166, - "velocityY": 0.9342418403365317, - "timestamp": 0.17485104247936575 - }, - { - "x": 1.078441589183816, - "y": 6.803669240138584, - "heading": 0.9195149817064242, - "angularVelocity": -1.3646793077415271, - "velocityX": 1.7683868057588992, - "velocityY": 0.9342418396000037, - "timestamp": 0.233134723305821 - }, - { - "x": 1.1815096813012231, - "y": 6.858120293421828, - "heading": 0.7885846802295383, - "angularVelocity": -2.2464315846153347, - "velocityX": 1.7683868049497107, - "velocityY": 0.9342418411317296, - "timestamp": 0.29141840413227627 - }, - { - "x": 1.2845777733691615, - "y": 6.912571346798711, - "heading": 0.6269133013543681, - "angularVelocity": -2.7738704313454483, - "velocityX": 1.7683868041009485, - "velocityY": 0.9342418427383741, - "timestamp": 0.3497020849587315 - }, - { - "x": 1.3876458655603612, - "y": 6.967022399942281, - "heading": 0.45022486052054533, - "angularVelocity": -3.03152509120432, - "velocityX": 1.7683868062158064, - "velocityY": 0.9342418387352904, - "timestamp": 0.40798576578518675 - }, - { - "x": 1.4907139579910005, - "y": 7.021473452632629, - "heading": 0.2680622315698304, - "angularVelocity": -3.125448262138626, - "velocityX": 1.768386810323986, - "velocityY": 0.9342418309591196, - "timestamp": 0.466269446611642 - }, - { - "x": 1.5937820505189422, - "y": 7.075924505138796, - "heading": 0.0852527280795436, - "angularVelocity": -3.136546987065588, - "velocityX": 1.7683868119934505, - "velocityY": 0.9342418277990836, - "timestamp": 0.5245531274380972 - }, - { - "x": 1.696850142939802, - "y": 7.130375557847656, - "heading": -0.09670059771549627, - "angularVelocity": -3.1218571513776574, - "velocityX": 1.7683868101561941, - "velocityY": 0.934241831276761, - "timestamp": 0.5828368082645525 - }, - { - "x": 1.7999182351302574, - "y": 7.184826610992639, - "heading": -0.27821616946416433, - "angularVelocity": -3.1143464032592627, - "velocityX": 1.7683868062030377, - "velocityY": 0.934241838759528, - "timestamp": 0.6411204890910078 - }, - { - "x": 1.9029863270693197, - "y": 7.239277664613472, - "heading": -0.4604343199403316, - "angularVelocity": -3.126400870599392, - "velocityX": 1.7683868018897635, - "velocityY": 0.9342418469239417, - "timestamp": 0.6994041699174631 - }, - { - "x": 2.0060544188463965, - "y": 7.293728718540922, - "heading": -0.6442150495894297, - "angularVelocity": -3.1532107623299397, - "velocityX": 1.7683867991104978, - "velocityY": 0.9342418521846886, - "timestamp": 0.7576878507439184 - }, - { - "x": 2.1091225106680933, - "y": 7.348179772383911, - "heading": -0.829167725791517, - "angularVelocity": -3.173318389980438, - "velocityX": 1.7683867998760676, - "velocityY": 0.9342418507355571, - "timestamp": 0.8159715315703737 - }, - { - "x": 2.212190602724116, - "y": 7.402630825783353, - "heading": -1.0127639833296274, - "angularVelocity": -3.1500456891898856, - "velocityX": 1.7683868038965105, - "velocityY": 0.9342418431254117, - "timestamp": 0.874255212396829 - }, - { - "x": 2.3152586950079552, - "y": 7.45708187875157, - "heading": -1.189780759847791, - "angularVelocity": -3.0371584980121242, - "velocityX": 1.7683868078052658, - "velocityY": 0.9342418357266746, - "timestamp": 0.9325388932232843 - }, - { - "x": 2.4183267873051926, - "y": 7.511532931694425, - "heading": -1.3512503965030376, - "angularVelocity": -2.770409047008454, - "velocityX": 1.768386808035141, - "velocityY": 0.934241835291512, - "timestamp": 0.9908225740497396 - }, - { - "x": 2.5213948795058583, - "y": 7.565983984820071, - "heading": -1.4820477254792255, - "angularVelocity": -2.244150114076672, - "velocityX": 1.7683868063782155, - "velocityY": 0.9342418384277741, - "timestamp": 1.049106254876195 - }, - { - "x": 2.6244629716588035, - "y": 7.620435038036042, - "heading": -1.5615658496268932, - "angularVelocity": -1.3643291401540225, - "velocityX": 1.7683868055594567, - "velocityY": 0.9342418399775199, - "timestamp": 1.1073899357026502 - }, - { - "x": 2.7275310638313552, - "y": 7.674886091214861, - "heading": -1.569999963079154, - "angularVelocity": -0.14470797540511007, - "velocityX": 1.7683868058959282, - "velocityY": 0.9342418393401073, - "timestamp": 1.1656736165291055 - }, - { - "x": 2.7977986665714587, - "y": 7.7120085885476914, - "heading": -1.5699999870889756, - "angularVelocity": -4.119521318427831e-7, - "velocityX": 1.2056136768248569, - "velocityY": 0.6369278124926107, - "timestamp": 1.2239572973555608 - }, - { - "x": 2.8329324722290035, + "x": 0.7855613477332503, + "y": 6.648940096740809, + "heading": 1.0074798315639235, + "angularVelocity": -4.44626773132229e-7, + "velocityX": 0.29788756864663374, + "velocityY": 0.15737452300923568, + "timestamp": 0.06263219156953911 + }, + { + "x": 0.8228760494865476, + "y": 6.6686535188760185, + "heading": 1.0074797723867348, + "angularVelocity": -9.448368272101287e-7, + "velocityX": 0.5957751248711716, + "velocityY": 0.31474903945078925, + "timestamp": 0.12526438313907823 + }, + { + "x": 0.8788481004307849, + "y": 6.6982236511875834, + "heading": 1.0074796760782534, + "angularVelocity": -0.000001537683717241804, + "velocityX": 0.8936626603923403, + "velocityY": 0.47212354494627234, + "timestamp": 0.18789657470861734 + }, + { + "x": 0.9534774979725686, + "y": 6.737650492304352, + "heading": 1.0074795310346885, + "angularVelocity": -0.000002315799427807793, + "velocityX": 1.191550154506795, + "velocityY": 0.6294980285496351, + "timestamp": 0.25052876627815646 + }, + { + "x": 1.0467642343312271, + "y": 6.786934038112618, + "heading": 1.0074793024441084, + "angularVelocity": -0.000003649730463432068, + "velocityX": 1.489437524393268, + "velocityY": 0.7868724464726433, + "timestamp": 0.3131609578476956 + }, + { + "x": 1.140908484326656, + "y": 6.836670609920262, + "heading": 0.9598451569897353, + "angularVelocity": -0.7605377404288135, + "velocityX": 1.5031287846746073, + "velocityY": 0.7941055639482466, + "timestamp": 0.37579314941723474 + }, + { + "x": 1.2350527343337772, + "y": 6.886407181705814, + "heading": 0.868308368234956, + "angularVelocity": -1.461497457791568, + "velocityX": 1.503128784861293, + "velocityY": 0.7941055635955249, + "timestamp": 0.4384253409867739 + }, + { + "x": 1.3291969843350966, + "y": 6.936143753502349, + "heading": 0.7375936922085377, + "angularVelocity": -2.0870206318948967, + "velocityX": 1.5031287847686552, + "velocityY": 0.7941055637708808, + "timestamp": 0.501057532556313 + }, + { + "x": 1.4233412343345928, + "y": 6.985880325302336, + "heading": 0.5733869923214697, + "angularVelocity": -2.6217620008513047, + "velocityX": 1.50312878473955, + "velocityY": 0.794105563825983, + "timestamp": 0.5636897241258522 + }, + { + "x": 1.517485484369411, + "y": 7.035616897035464, + "heading": 0.38230326840749074, + "angularVelocity": -3.050886758477845, + "velocityX": 1.503128785303508, + "velocityY": 0.7941055627585059, + "timestamp": 0.6263219156953913 + }, + { + "x": 1.611629734494345, + "y": 7.085353468598017, + "heading": 0.1715892655816257, + "angularVelocity": -3.364308314070061, + "velocityX": 1.5031287867423193, + "velocityY": 0.7941055600350643, + "timestamp": 0.6889541072649304 + }, + { + "x": 1.705773984659377, + "y": 7.1350900400846715, + "heading": -0.05152973146485926, + "angularVelocity": -3.562369309697853, + "velocityX": 1.5031287873825276, + "velocityY": 0.7941055588232755, + "timestamp": 0.7515862988344696 + }, + { + "x": 1.7999182346855311, + "y": 7.184826611834204, + "heading": -0.2805858659507677, + "angularVelocity": -3.657163013871995, + "velocityX": 1.5031287851651773, + "velocityY": 0.7941055630204278, + "timestamp": 0.8142184904040087 + }, + { + "x": 1.8940624845219065, + "y": 7.23456318394296, + "heading": -0.5098527052085398, + "angularVelocity": -3.660527174802386, + "velocityX": 1.5031287821351207, + "velocityY": 0.7941055687558903, + "timestamp": 0.8768506819735479 + }, + { + "x": 1.9882067343467884, + "y": 7.28429975607347, + "heading": -0.7334067984715299, + "angularVelocity": -3.56931615612939, + "velocityX": 1.503128781951615, + "velocityY": 0.7941055691032111, + "timestamp": 0.939482873543087 + }, + { + "x": 2.082350984290343, + "y": 7.334036327979348, + "heading": -0.9444794864772089, + "angularVelocity": -3.370035164285983, + "velocityX": 1.5031287838463745, + "velocityY": 0.7941055655166722, + "timestamp": 1.002115065112626 + }, + { + "x": 2.1764952343123167, + "y": 7.383772899736789, + "heading": -1.1357610108525462, + "angularVelocity": -3.054044886213787, + "velocityX": 1.5031287850984254, + "velocityY": 0.7941055631466987, + "timestamp": 1.0647472566821652 + }, + { + "x": 2.2706394843420803, + "y": 7.433509471479484, + "heading": -1.3000590815023898, + "angularVelocity": -2.623220847500082, + "velocityX": 1.5031287852228015, + "velocityY": 0.794105562911257, + "timestamp": 1.1273794482517043 + }, + { + "x": 2.364783734359114, + "y": 7.483246043246272, + "heading": -1.4308123405268247, + "angularVelocity": -2.0876366569300906, + "velocityX": 1.5031287850195627, + "velocityY": 0.7941055632959484, + "timestamp": 1.1900116398212435 + }, + { + "x": 2.458927984369609, + "y": 7.532982615025439, + "heading": -1.5223626326145774, + "angularVelocity": -1.4617130551167605, + "velocityX": 1.503128784915151, + "velocityY": 0.7941055634935796, + "timestamp": 1.2526438313907826 + }, + { + "x": 2.5530722343913332, + "y": 7.582719186783309, + "heading": -1.569999443293059, + "angularVelocity": -0.7605802940109437, + "velocityX": 1.503128785094449, + "velocityY": 0.7941055631535453, + "timestamp": 1.3152760229603218 + }, + { + "x": 2.6463589707381336, + "y": 7.6320027326140165, + "heading": -1.5699996718030853, + "angularVelocity": -0.0000036484443212939075, + "velocityX": 1.4894375242039373, + "velocityY": 0.7868724468309513, + "timestamp": 1.377908214529861 + }, + { + "x": 2.7209883682732525, + "y": 7.671429573743398, + "heading": -1.5699998167765745, + "angularVelocity": -0.000002314680584768984, + "velocityX": 1.1915501544003835, + "velocityY": 0.6294980287510356, + "timestamp": 1.4405404060994 + }, + { + "x": 2.776960419213422, + "y": 7.700999706062662, + "heading": -1.5699999130306501, + "angularVelocity": -0.0000015368150590879431, + "velocityX": 0.8936626603273936, + "velocityY": 0.4721235450691958, + "timestamp": 1.5031725976689392 + }, + { + "x": 2.814275120964383, + "y": 7.720713128202295, + "heading": -1.5699999721708229, + "angularVelocity": -9.44245822746899e-7, + "velocityX": 0.5957751248338641, + "velocityY": 0.31474903952139993, + "timestamp": 1.5658047892384783 + }, + { + "x": 2.8329324722290044, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": -2.215218740040656e-7, - "velocityX": 0.6028069119750795, - "velocityY": 0.3184639450811426, - "timestamp": 1.282240978182016 + "angularVelocity": -4.443271507992151e-7, + "velocityX": 0.2978875686300534, + "velocityY": 0.15737452304061644, + "timestamp": 1.6284369808080175 }, { "x": 2.832932472229004, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": 2.8428255363446037e-18, - "velocityX": -6.6133223633086636e-18, - "velocityY": -3.51368640376029e-18, - "timestamp": 1.3405246590084714 + "angularVelocity": 5.736792697557943e-18, + "velocityX": 3.8921831241382794e-17, + "velocityY": 2.057106543177864e-17, + "timestamp": 1.6910691723775566 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 1.7 + }, + { + "scope": [ + 1 + ], + "type": "WptZeroVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4 piece": { + "waypoints": [ + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.349189281463623, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.3418185710906982, + "y": 6.978495121002197, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3126474618911605, + "y": 5.567087650299072, + "heading": 8.749948102163363e-30, + "angularVelocity": 4.377756859907261e-29, + "velocityX": 2.3339363502495176e-15, + "velocityY": 4.559945650024092e-30, + "timestamp": 0 + }, + { + "x": 1.3684657734338344, + "y": 5.567087650299072, + "heading": -3.350607690445875e-20, + "angularVelocity": -3.2891903642626456e-19, + "velocityX": 0.5479514970030248, + "velocityY": 1.0705616112850545e-15, + "timestamp": 0.10186724892254397 + }, + { + "x": 1.480102395391638, + "y": 5.567087650299072, + "heading": -6.684009989043216e-20, + "angularVelocity": -3.2723003929334973e-19, + "velocityX": 1.0959029829371527, + "velocityY": 2.141123201298504e-15, + "timestamp": 0.20373449784508793 + }, + { + "x": 1.647557324391959, + "y": 5.567087650299072, + "heading": 1.9081720907940924e-20, + "angularVelocity": 8.434685372104281e-19, + "velocityX": 1.6438544357633669, + "velocityY": 3.2116847274817474e-15, + "timestamp": 0.3056017467676319 + }, + { + "x": 1.830918371677413, + "y": 5.567087650299072, + "heading": -1.7359971745167246e-18, + "angularVelocity": -1.7229079155247835e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.516754499906575e-15, + "timestamp": 0.40746899569017586 + }, + { + "x": 2.014279418962867, + "y": 5.567087650299072, + "heading": 1.701473951779379e-18, + "angularVelocity": 3.374461529154692e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.5167545000213866e-15, + "timestamp": 0.5093362446127199 + }, + { + "x": 2.1817343479631877, + "y": 5.567087650299072, + "heading": 6.998133615959998e-19, + "angularVelocity": -9.83299931311397e-18, + "velocityX": 1.643854435763364, + "velocityY": 3.2116847272591396e-15, + "timestamp": 0.6112034935352638 + }, + { + "x": 2.2933709699209914, + "y": 5.567087650299072, + "heading": 1.9411941943855612e-19, + "angularVelocity": -4.964244608659548e-18, + "velocityX": 1.0959029829371498, + "velocityY": 2.1411232011515742e-15, + "timestamp": 0.7130707424578078 + }, + { + "x": 2.349189281463665, + "y": 5.567087650299072, + "heading": -9.06696023328258e-29, + "angularVelocity": -1.905611677680185e-18, + "velocityX": 0.5479514970030218, + "velocityY": 1.0705616112089172e-15, + "timestamp": 0.8149379913803517 + }, + { + "x": 2.3491892814636506, + "y": 5.567087650299072, + "heading": -8.631559872072984e-29, + "angularVelocity": -4.3321556253199767e-29, + "velocityX": -7.059727912830758e-16, + "velocityY": -1.2733006393905255e-26, + "timestamp": 0.9168052403028957 + }, + { + "x": 2.293370969920977, + "y": 5.567087650299072, + "heading": 3.7613099065105144e-19, + "angularVelocity": 3.692364275833631e-18, + "velocityX": -0.5479514970030234, + "velocityY": 1.0308631110321649e-16, + "timestamp": 1.0186724892254397 + }, + { + "x": 2.181734347963174, + "y": 5.567087650299072, + "heading": 1.2011497534698325e-18, + "angularVelocity": 8.098959886028294e-18, + "velocityX": -1.0959029829371514, + "velocityY": 2.3798569412312996e-17, + "timestamp": 1.1205397381479838 + }, + { + "x": 2.0142794189628526, + "y": 5.567087650299072, + "heading": 2.4969607309895686e-18, + "angularVelocity": 1.2720584788941894e-17, + "velocityX": -1.6438544357633655, + "velocityY": -5.006988249789681e-16, + "timestamp": 1.2224069870705279 + }, + { + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 3.324496900460492e-18, + "angularVelocity": 8.123672494715722e-18, + "velocityX": -1.7999999923909678, + "velocityY": -4.2863448770853046e-16, + "timestamp": 1.324274235993072 + }, + { + "x": 1.6475573243919444, + "y": 5.567087650299072, + "heading": 1.771361781319927e-18, + "angularVelocity": -1.5246658146525354e-17, + "velocityX": -1.7999999923909678, + "velocityY": -4.728353666252759e-16, + "timestamp": 1.426141484915616 + }, + { + "x": 1.4801023953916232, + "y": 5.567087650299072, + "heading": 1.0009187336716733e-18, + "angularVelocity": -7.563206565555211e-18, + "velocityX": -1.6438544357633653, + "velocityY": -3.2195944429460503e-16, + "timestamp": 1.52800873383816 + }, + { + "x": 1.36846577343382, + "y": 5.567087650299071, + "heading": 3.66285032042137e-19, + "angularVelocity": -6.230007259713378e-18, + "velocityX": -1.0959029829371512, + "velocityY": 1.541445309390221e-16, + "timestamp": 1.6298759827607041 + }, + { + "x": 1.3126474618911463, + "y": 5.567087650299071, + "heading": -4.902178913705171e-28, + "angularVelocity": -3.595709471600392e-18, + "velocityX": -0.5479514970030231, + "velocityY": 2.0586346500407271e-16, + "timestamp": 1.7317432316832482 + }, + { + "x": 1.31264746189116, + "y": 5.567087650299071, + "heading": -6.149899148755531e-28, + "angularVelocity": -4.636006760170492e-28, + "velocityX": -3.976778090883029e-16, + "velocityY": 8.683385912114236e-17, + "timestamp": 1.8336104806057922 + }, + { + "x": 1.3502677179953657, + "y": 5.618680150361126, + "heading": 4.532916044804573e-19, + "angularVelocity": 4.160489322353646e-18, + "velocityX": 0.34529356424961244, + "velocityY": 0.4735363360002019, + "timestamp": 1.9425619931715181 + }, + { + "x": 1.425508229517596, + "y": 5.721865149544201, + "heading": 1.747154185604359e-18, + "angularVelocity": 1.1875581645180033e-17, + "velocityX": 0.6905871222011823, + "velocityY": 0.9470726633632547, + "timestamp": 2.051513505737244 + }, + { + "x": 1.5383689944041468, + "y": 5.876642645031848, + "heading": 5.645077352326899e-18, + "angularVelocity": 3.5776677866624406e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.42060896487578, + "timestamp": 2.1604650183029697 + }, + { + "x": 1.653914603238861, + "y": 6.035102141279364, + "heading": 8.608437484479335e-18, + "angularVelocity": 2.719888934006118e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259298, + "timestamp": 2.2694165308686953 + }, + { + "x": 1.7694602120735736, + "y": 6.193561637526878, + "heading": 9.873897307535619e-18, + "angularVelocity": 1.1614889913397002e-17, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 2.378368043434421 + }, + { + "x": 1.8850058209082863, + "y": 6.352021133774392, + "heading": 8.890496485421955e-18, + "angularVelocity": -9.026041015666318e-18, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 2.4873195560001466 + }, + { + "x": 2.0005514297429987, + "y": 6.510480630021907, + "heading": 5.9513864400033404e-18, + "angularVelocity": -2.697631244937109e-17, + "velocityX": 1.0605232191247393, + "velocityY": 1.4544038216259076, + "timestamp": 2.5962710685658723 + }, + { + "x": 2.1160970385777134, + "y": 6.668940126269423, + "heading": 2.0004191174937195e-18, + "angularVelocity": -3.626353805716405e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259296, + "timestamp": 2.705222581131598 + }, + { + "x": 2.228957803464264, + "y": 6.823717621757069, + "heading": -2.138334357056216e-18, + "angularVelocity": -3.798711350391465e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.4206089648757803, + "timestamp": 2.8141740936973236 + }, + { + "x": 2.3041983149864937, + "y": 6.926902620940145, + "heading": -2.805869618577932e-18, + "angularVelocity": -6.126902193701436e-18, + "velocityX": 0.6905871222011825, + "velocityY": 0.9470726633632547, + "timestamp": 2.9231256062630493 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002199, + "heading": 1.401424278348935e-27, + "angularVelocity": 2.575337922063006e-17, + "velocityX": 0.34529356424961277, + "velocityY": 0.47353633600020184, + "timestamp": 3.032077118828775 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002198, + "heading": 6.552737055754714e-28, + "angularVelocity": -4.91699230639919e-28, + "velocityX": -1.8586364396279522e-17, + "velocityY": -2.5489381044432514e-17, + "timestamp": 3.1410286313945006 + }, + { + "x": 2.304198314986494, + "y": 6.926902620940145, + "heading": -1.6258900448266429e-18, + "angularVelocity": -1.4923060820323447e-17, + "velocityX": -0.34529356424960833, + "velocityY": -0.47353633600020245, + "timestamp": 3.249980143960226 + }, + { + "x": 2.2289578034642648, + "y": 6.823717621757068, + "heading": -2.7548328890398584e-18, + "angularVelocity": -1.03618831931977e-17, + "velocityX": -0.6905871222011744, + "velocityY": -0.9470726633632552, + "timestamp": 3.358931656525951 + }, + { + "x": 2.1160970385777156, + "y": 6.668940126269423, + "heading": -1.3133552490827444e-18, + "angularVelocity": 1.3230450918999471e-17, + "velocityX": -1.0358806613030351, + "velocityY": -1.4206089648757798, + "timestamp": 3.4678831690916763 + }, + { + "x": 2.0005514297430027, + "y": 6.510480630021907, + "heading": -1.6506585170593292e-18, + "angularVelocity": -3.0959025742305048e-18, + "velocityX": -1.0605232191247447, + "velocityY": -1.4544038216259367, + "timestamp": 3.5768346816574015 + }, + { + "x": 1.8850058209082918, + "y": 6.352021133774392, + "heading": -2.0454310820150905e-18, + "angularVelocity": -3.6233784679199945e-18, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 3.6857861942231267 + }, + { + "x": 1.7694602120735807, + "y": 6.193561637526877, + "heading": -8.094833008781398e-19, + "angularVelocity": 1.1344016734492403e-17, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 3.794737706788852 + }, + { + "x": 1.6539146032388696, + "y": 6.035102141279363, + "heading": 2.1576684653761504e-18, + "angularVelocity": 2.7233690448860427e-17, + "velocityX": -1.0605232191247282, + "velocityY": -1.4544038216259159, + "timestamp": 3.903689219354577 + }, + { + "x": 1.5383689944041568, + "y": 5.876642645031847, + "heading": 4.338142671690236e-18, + "angularVelocity": 2.0013253198238136e-17, + "velocityX": -1.0605232191247451, + "velocityY": -1.4544038216259363, + "timestamp": 4.012640731920302 + }, + { + "x": 1.4255082295176076, + "y": 5.721865149544201, + "heading": 3.587785895663659e-18, + "angularVelocity": -6.887070745442744e-18, + "velocityX": -1.0358806613030345, + "velocityY": -1.4206089648757805, + "timestamp": 4.121592244486028 + }, + { + "x": 1.3502677179953786, + "y": 5.618680150361125, + "heading": 1.1960395158046676e-18, + "angularVelocity": -2.1952392596859126e-17, + "velocityX": -0.690587122201174, + "velocityY": -0.9470726633632556, + "timestamp": 4.230543757051753 + }, + { + "x": 1.3126474618911737, + "y": 5.567087650299071, + "heading": -2.151002138732551e-28, + "angularVelocity": -1.097772290683926e-17, + "velocityX": -0.34529356424960816, + "velocityY": -0.47353633600020256, + "timestamp": 4.339495269617478 + }, + { + "x": 1.3126474618911739, + "y": 5.567087650299072, + "heading": -2.805587391661966e-28, + "angularVelocity": -2.5846482509897715e-28, + "velocityX": -3.90611366689139e-17, + "velocityY": -6.850675515620994e-17, + "timestamp": 4.448446782183203 + }, + { + "x": 1.3536439878702002, + "y": 5.515302553738671, + "heading": 1.0651040188255802e-19, + "angularVelocity": 9.61200821593777e-19, + "velocityX": 0.36997226934362815, + "velocityY": -0.46733349314603306, + "timestamp": 4.559256508684486 + }, + { + "x": 1.4356370390978233, + "y": 5.411732361540517, + "heading": -3.328225499624599e-18, + "angularVelocity": -3.099670048679532e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.9346669779656481, + "timestamp": 4.670066235185769 + }, + { + "x": 1.5586266133876248, + "y": 5.256377076466403, + "heading": -1.0014608416167351e-17, + "angularVelocity": -6.03411192104104e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615278, + "timestamp": 4.780875961687052 + }, + { + "x": 1.6824304588849308, + "y": 5.099993238067341, + "heading": -7.784211196275788e-18, + "angularVelocity": 2.012817187466661e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 4.891685688188335 + }, + { + "x": 1.8062343043822369, + "y": 4.94360939966828, + "heading": -2.180390152426642e-18, + "angularVelocity": 5.057156281947364e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 5.0024954146896174 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": 3.356944437156282e-18, + "angularVelocity": 4.997155723864182e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 5.1133051411909 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": 7.008255309374147e-18, + "angularVelocity": 3.2951176651010183e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805754, + "timestamp": 5.224114867692183 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": 8.815234075857993e-18, + "angularVelocity": 1.6307041114184896e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 5.334924594193466 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": 6.3728040622429035e-18, + "angularVelocity": -2.2041657280874785e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615276, + "timestamp": 5.445734320694749 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 3.61542268471678e-18, + "angularVelocity": -2.4883929063649245e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.934666977965648, + "timestamp": 5.556544047196032 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 9.086068070823674e-28, + "angularVelocity": -3.262730443555877e-17, + "velocityX": 0.3699722693436281, + "velocityY": -0.46733349314603295, + "timestamp": 5.6673537736973145 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 5.931881185987618e-28, + "angularVelocity": 2.148760273829923e-29, + "velocityX": -1.6344283195010131e-18, + "velocityY": 2.0645414970491578e-18, + "timestamp": 5.778163500198597 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 2.5809854735534117e-18, + "angularVelocity": 2.3292048019313352e-17, + "velocityX": -0.36997226934362804, + "velocityY": 0.46733349314603323, + "timestamp": 5.88897322669988 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": -3.635479866817947e-18, + "angularVelocity": -5.610035807757445e-17, + "velocityX": -0.7399445320955099, + "velocityY": 0.9346669779656486, + "timestamp": 5.999782953201163 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": -8.293710197495525e-18, + "angularVelocity": -4.2038099666365453e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.402000437861529, + "timestamp": 6.110592679702446 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": -1.2713469170987557e-17, + "angularVelocity": -3.988602002843467e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 6.221402406203729 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": -1.4536509782910432e-17, + "angularVelocity": -1.6451990888019063e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 6.332212132705012 + }, + { + "x": 1.806234304382237, + "y": 4.943609399668281, + "heading": -1.4741169168642963e-17, + "angularVelocity": -1.8469442384254893e-18, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 6.443021859206294 + }, + { + "x": 1.682430458884931, + "y": 5.099993238067342, + "heading": -1.3603933385071938e-17, + "angularVelocity": 1.0262959923842215e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.4112825952805752, + "timestamp": 6.553831585707577 + }, + { + "x": 1.558626613387625, + "y": 5.2563770764664035, + "heading": -1.2316142581173569e-17, + "angularVelocity": 1.162164046785376e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 6.66464131220886 + }, + { + "x": 1.4356370390978235, + "y": 5.411732361540517, + "heading": -1.1146449974389838e-17, + "angularVelocity": 1.0555865817235836e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.4020004378615287, + "timestamp": 6.775451038710143 + }, + { + "x": 1.3536439878702005, + "y": 5.515302553738671, + "heading": -4.770660310358341e-18, + "angularVelocity": 5.753817702370985e-17, + "velocityX": -0.7399445320955101, + "velocityY": 0.9346669779656486, + "timestamp": 6.886260765211426 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -5.571608128785667e-28, + "angularVelocity": 4.305272160914999e-17, + "velocityX": -0.36997226934362815, + "velocityY": 0.4673334931460333, + "timestamp": 6.997070491712709 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -2.752910621958426e-28, + "angularVelocity": 5.936929258081384e-29, + "velocityX": -2.690449803663919e-18, + "velocityY": 3.3984636410469572e-18, + "timestamp": 7.1078802182139915 } ], "constraints": [ @@ -1631,19 +2648,85 @@ ], "type": "StopPoint" }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, { "scope": [ 1 ], "type": "StopPoint" }, + { + "scope": [ + 1 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 2 + ], + "type": "WptZeroVelocity" + }, { "scope": [ "first", "last" ], "type": "MaxVelocity", - "velocity": 2 + "velocity": 1.8 + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 4 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 6 + ], + "type": "WptZeroVelocity" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/2 piece left.1.traj b/src/main/deploy/choreo/2 piece left.1.traj index 509ec72..46553a0 100644 --- a/src/main/deploy/choreo/2 piece left.1.traj +++ b/src/main/deploy/choreo/2 piece left.1.traj @@ -4,127 +4,154 @@ "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 2.0283839438048332e-19, - "velocityX": 8.094691061889947e-19, - "velocityY": 1.7638426740730563e-19, + "angularVelocity": -7.087535479777707e-19, + "velocityX": 7.50401412133545e-18, + "velocityY": 1.4256736684497237e-18, "timestamp": 0 }, { - "x": 0.8322802456836562, - "y": 6.653172699607564, - "heading": 1.0074798311324673, - "angularVelocity": -3.7400126935671097e-7, - "velocityX": 0.8646152527784429, - "velocityY": 0.18633427357388527, - "timestamp": 0.07561311115664361 - }, - { - "x": 0.9630327340876748, - "y": 6.681351325717821, - "heading": 1.007479762583371, - "angularVelocity": -9.065768535902331e-7, - "velocityX": 1.7292303729328837, - "velocityY": 0.3726685184515223, - "timestamp": 0.15122622231328722 - }, - { - "x": 1.1108648715080907, - "y": 6.713210807608451, - "heading": 0.935326621538057, - "angularVelocity": -0.9542411354538504, - "velocityX": 1.955112481936631, - "velocityY": 0.4213486444781865, - "timestamp": 0.22683933346993085 - }, - { - "x": 1.258697008933641, - "y": 6.745070289475266, - "heading": 0.8127259503436992, - "angularVelocity": -1.6214208001621926, - "velocityX": 1.9551124820045325, - "velocityY": 0.421348644163222, - "timestamp": 0.30245244462657445 - }, - { - "x": 1.406529146351303, - "y": 6.776929771378683, - "heading": 0.6640255812369789, - "angularVelocity": -1.966595036655294, - "velocityX": 1.9551124819002106, - "velocityY": 0.4213486446472832, - "timestamp": 0.37806555578321804 - }, - { - "x": 1.5543612837652037, - "y": 6.808789253299551, - "heading": 0.5044825425717008, - "angularVelocity": -2.1099917226625076, - "velocityX": 1.9551124818504668, - "velocityY": 0.42134864487809753, - "timestamp": 0.45367866693986164 - }, - { - "x": 1.702193421185673, - "y": 6.840648735189942, - "heading": 0.344692011554351, - "angularVelocity": -2.1132648633690048, - "velocityX": 1.9551124819373364, - "velocityY": 0.42134864447501086, - "timestamp": 0.5292917780965053 - }, - { - "x": 1.8500255586150136, - "y": 6.872508217039167, - "heading": 0.19547183240040739, - "angularVelocity": -1.9734696386822674, - "velocityX": 1.9551124820546628, - "velocityY": 0.42134864393060517, - "timestamp": 0.6049048892531489 - }, - { - "x": 1.9978576960460066, - "y": 6.904367698880726, - "heading": 0.07244487018227615, - "angularVelocity": -1.6270585925668755, - "velocityX": 1.9551124820765156, - "velocityY": 0.42134864382921067, - "timestamp": 0.6805180004097925 - }, - { - "x": 2.1456898334992545, - "y": 6.936227180619012, - "heading": 9.65887879992878e-8, - "angularVelocity": -0.9580980399471752, - "velocityX": 1.955112482370841, - "velocityY": 0.4213486424633956, - "timestamp": 0.7561311115664361 - }, - { - "x": 2.2764423218817926, - "y": 6.964405806828939, - "heading": 2.820501204963407e-8, - "angularVelocity": -9.043904534147509e-7, - "velocityX": 1.7292303726488056, - "velocityY": 0.37266851976968174, - "timestamp": 0.8317442227230797 + "x": 0.7981646775700082, + "y": 6.645820413966075, + "heading": 1.0074794822217241, + "angularVelocity": -0.000004891992156933861, + "velocityX": 0.4054374425885426, + "velocityY": 0.08737633054596244, + "timestamp": 0.07710358693768382 + }, + { + "x": 0.8606860382778649, + "y": 6.659294470675445, + "heading": 1.0074786780539715, + "angularVelocity": -0.000010429706123137783, + "velocityX": 0.8108748657619178, + "velocityY": 0.17475265736028606, + "timestamp": 0.15420717387536764 + }, + { + "x": 0.9544680756755046, + "y": 6.679505554656715, + "heading": 1.00747734637885, + "angularVelocity": -0.000017271247347385705, + "velocityX": 1.2163122511205047, + "velocityY": 0.2621289719971706, + "timestamp": 0.23131076081305146 + }, + { + "x": 1.0795107812763696, + "y": 6.706453661799292, + "heading": 1.007475162920361, + "angularVelocity": -0.000028318507292526304, + "velocityX": 1.6217495264121258, + "velocityY": 0.3495052333214708, + "timestamp": 0.3084143477507353 + }, + { + "x": 1.2151823459896474, + "y": 6.735692436676619, + "heading": 0.9400813447765213, + "angularVelocity": -0.8740685202921675, + "velocityX": 1.7596012079558552, + "velocityY": 0.37921419791997063, + "timestamp": 0.3855179346884191 + }, + { + "x": 1.3508539216363087, + "y": 6.764931160821833, + "heading": 0.8025402930124232, + "angularVelocity": -1.7838476422019192, + "velocityX": 1.7596013497570884, + "velocityY": 0.37921353994654156, + "timestamp": 0.4626215216261029 + }, + { + "x": 1.4865254934430294, + "y": 6.794169902784878, + "heading": 0.6117409589886299, + "angularVelocity": -2.4745844078304686, + "velocityX": 1.7596012999547292, + "velocityY": 0.3792137710360449, + "timestamp": 0.5397251085637867 + }, + { + "x": 1.6221970645812445, + "y": 6.8234086478498766, + "heading": 0.3987348321748914, + "angularVelocity": -2.7625968553952354, + "velocityX": 1.7596012912844983, + "velocityY": 0.3792138112670414, + "timestamp": 0.6168286955014706 + }, + { + "x": 1.757868635365604, + "y": 6.852647394556811, + "heading": 0.20722880927427756, + "angularVelocity": -2.4837498553133672, + "velocityX": 1.7596012866951443, + "velocityY": 0.37921383256224117, + "timestamp": 0.6939322824391544 + }, + { + "x": 1.8935401957987985, + "y": 6.881886189294451, + "heading": 0.06863797850596423, + "angularVelocity": -1.7974628194706992, + "velocityX": 1.7596011524450381, + "velocityY": 0.3792144554995873, + "timestamp": 0.7710358693768382 + }, + { + "x": 2.029211813519273, + "y": 6.911124718210696, + "heading": 0.000004762891013022644, + "angularVelocity": -0.8901429666355407, + "velocityX": 1.7596018954361505, + "velocityY": 0.37921100791168616, + "timestamp": 0.848139456314522 + }, + { + "x": 2.1542545037959298, + "y": 6.938072896534997, + "heading": 0.00000255375105981851, + "angularVelocity": -0.00002865158471797654, + "velocityX": 1.6217493276638106, + "velocityY": 0.34950615651748473, + "timestamp": 0.9252430432522059 + }, + { + "x": 2.2480365342099393, + "y": 6.958284012920916, + "heading": 0.0000012010318378255, + "angularVelocity": -0.000017544180183003836, + "velocityX": 1.2163121605458629, + "velocityY": 0.26212939227137494, + "timestamp": 1.0023466301898898 + }, + { + "x": 2.3105578914207334, + "y": 6.971758085856952, + "heading": 3.834757266936798e-7, + "angularVelocity": -0.000010603347309739972, + "velocityX": 0.8108748204065306, + "velocityY": 0.1747528678130866, + "timestamp": 1.0794502171275737 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.9396360429134164e-18, - "angularVelocity": -3.7301747793182745e-7, - "velocityX": 0.8646152526837502, - "velocityY": 0.18633427401327182, - "timestamp": 0.9073573338797233 + "heading": 3.134962737955188e-18, + "angularVelocity": -0.000004973513450108379, + "velocityX": 0.405437424010253, + "velocityY": 0.08737641675075283, + "timestamp": 1.1565538040652577 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.2932527092188034e-18, - "angularVelocity": 4.19600892926216e-20, - "velocityX": -8.058124746715222e-20, - "velocityY": -1.860414915840738e-20, - "timestamp": 0.9829704450363669 + "heading": 2.0846959931491773e-18, + "angularVelocity": -8.557421751891334e-20, + "velocityX": -4.987723153383344e-19, + "velocityY": -9.652716908345432e-20, + "timestamp": 1.2336573910029416 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.2.traj b/src/main/deploy/choreo/2 piece left.2.traj index a50ba99..6381d78 100644 --- a/src/main/deploy/choreo/2 piece left.2.traj +++ b/src/main/deploy/choreo/2 piece left.2.traj @@ -3,128 +3,155 @@ { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.2932527092188034e-18, - "angularVelocity": 4.19600892926216e-20, - "velocityX": -8.058124746715222e-20, - "velocityY": -1.860414915840738e-20, + "heading": 2.0846959931491773e-18, + "angularVelocity": -8.557421751891334e-20, + "velocityX": -4.987723153383344e-19, + "velocityY": -9.652716908345432e-20, "timestamp": 0 }, { - "x": 2.2764423218798027, - "y": 6.964405806838171, - "heading": 2.828141101686341e-8, - "angularVelocity": 3.7402787103507053e-7, - "velocityX": -0.8646152527100619, - "velocityY": -0.18633427389118332, - "timestamp": 0.07561311115664371 - }, - { - "x": 2.145689833491728, - "y": 6.936227180653932, - "heading": 9.68470451708674e-8, - "angularVelocity": 9.067955680869744e-7, - "velocityX": -1.7292303727220222, - "velocityY": -0.3726685194299493, - "timestamp": 0.1512262223132873 - }, - { - "x": 1.9978576960446424, - "y": 6.904367698887054, - "heading": 0.07260812222938247, - "angularVelocity": 0.960257080705476, - "velocityX": -1.9551124822893438, - "velocityY": -0.42134864284155416, - "timestamp": 0.2268393334699309 - }, - { - "x": 1.8500255586147867, - "y": 6.872508217040216, - "heading": 0.1958338387782673, - "angularVelocity": 1.6296871622383675, - "velocityX": -1.955112482061473, - "velocityY": -0.42134864389901044, - "timestamp": 0.3024524446265745 - }, - { - "x": 1.7021934211910146, - "y": 6.840648735165152, - "heading": 0.3451117743398479, - "angularVelocity": 1.9742334798568144, - "velocityX": -1.95511248198102, - "velocityY": -0.42134864427231705, - "timestamp": 0.3780655557832181 - }, - { - "x": 1.5543612837761693, - "y": 6.8087892532486665, - "heading": 0.5047130791830543, - "angularVelocity": 2.110762305661104, - "velocityX": -1.9551124818629584, - "velocityY": -0.42134864482013473, - "timestamp": 0.4536786669398617 - }, - { - "x": 1.4065291463620155, - "y": 6.776929771328972, - "heading": 0.6639739239039864, - "angularVelocity": 2.106259645777567, - "velocityX": -1.9551124818538146, - "velocityY": -0.4213486448625627, - "timestamp": 0.5292917780965053 - }, - { - "x": 1.2586970089400131, - "y": 6.745070289445696, - "heading": 0.8125476250392986, - "angularVelocity": 1.9649198249166078, - "velocityX": -1.9551124819576147, - "velocityY": -0.4213486443809204, - "timestamp": 0.6049048892531489 - }, - { - "x": 1.1108648715115719, - "y": 6.713210807592297, - "heading": 0.9352215452290742, - "angularVelocity": 1.622389534212375, - "velocityX": -1.9551124820427679, - "velocityY": -0.42134864398580435, - "timestamp": 0.6805180004097925 - }, - { - "x": 0.9630327340807132, - "y": 6.681351325750123, - "heading": 1.0074797622855187, - "angularVelocity": 0.9556307887761831, - "velocityX": -1.9551124820747394, - "velocityY": -0.42134864383734444, - "timestamp": 0.7561311115664361 - }, - { - "x": 0.8322802456820239, - "y": 6.653172699615138, - "heading": 1.0074798310413775, - "angularVelocity": 9.093113311976794e-7, - "velocityX": -1.7292303728624039, - "velocityY": -0.3726685187785591, - "timestamp": 0.8317442227230797 + "x": 2.3105578909850855, + "y": 6.971758087878447, + "heading": 3.828776753639658e-7, + "angularVelocity": 0.000004965756984393634, + "velocityX": -0.40543742966046686, + "velocityY": -0.08737639053285204, + "timestamp": 0.07710358693767327 + }, + { + "x": 2.2480365327877774, + "y": 6.958284019520034, + "heading": 0.0000011992649992501823, + "angularVelocity": 0.000010588188647324088, + "velocityX": -0.8108748332013094, + "velocityY": -0.17475280844332217, + "timestamp": 0.15420717387534655 + }, + { + "x": 2.154254500585808, + "y": 6.938072911430576, + "heading": 0.0000025503266632355084, + "angularVelocity": 0.000017522682376308707, + "velocityX": -1.2163121837350852, + "velocityY": -0.2621292846699298, + "timestamp": 0.23131076081301982 + }, + { + "x": 2.0292118068653178, + "y": 6.911124749085196, + "heading": 0.000004757557407174423, + "angularVelocity": 0.000028626823103883053, + "velocityX": -1.6217493723290508, + "velocityY": -0.34950594927788836, + "timestamp": 0.3084143477506931 + }, + { + "x": 1.8935401996566608, + "y": 6.881886171392507, + "heading": 0.06878997325201827, + "angularVelocity": 0.8921143415832743, + "velocityX": -1.7596017591026925, + "velocityY": -0.3792116405210038, + "timestamp": 0.38551793468836637 + }, + { + "x": 1.7578686367778409, + "y": 6.852647388002959, + "heading": 0.20749385875571683, + "angularVelocity": 1.7989290902357107, + "velocityX": -1.7596011841639752, + "velocityY": -0.379214308319837, + "timestamp": 0.46262152162603964 + }, + { + "x": 1.6221970666934797, + "y": 6.823408638048033, + "heading": 0.39902661142653667, + "angularVelocity": 2.484096528811891, + "velocityX": -1.7596012776167158, + "velocityY": -0.3792138746873246, + "timestamp": 0.5397251085637129 + }, + { + "x": 1.4865254965870125, + "y": 6.794169888195682, + "heading": 0.6119135529013362, + "angularVelocity": 2.7610510733681806, + "velocityX": -1.7596012779034225, + "velocityY": -0.37921387335696644, + "timestamp": 0.6168286955013862 + }, + { + "x": 1.3508539260055112, + "y": 6.7649311405475485, + "heading": 0.8026194720400166, + "angularVelocity": 2.473372857385191, + "velocityX": -1.7596012840644037, + "velocityY": -0.37921384476922, + "timestamp": 0.6939322824390595 + }, + { + "x": 1.2151823556967136, + "y": 6.735692391634036, + "heading": 0.9401132494355333, + "angularVelocity": 1.7832345141952992, + "velocityX": -1.7596012805275563, + "velocityY": -0.37921386118065376, + "timestamp": 0.7710358693767327 + }, + { + "x": 1.0795107810200666, + "y": 6.706453662988116, + "heading": 1.0074751648495315, + "angularVelocity": 0.873654755756699, + "velocityX": -1.7596013371766643, + "velocityY": -0.37921359831878204, + "timestamp": 0.848139456314406 + }, + { + "x": 0.9544680762714443, + "y": 6.679505551891246, + "heading": 1.007477347596638, + "angularVelocity": 0.000028309280971946704, + "velocityX": -1.6217495153591333, + "velocityY": -0.34950528460696234, + "timestamp": 0.9252430432520793 + }, + { + "x": 0.8606860387160242, + "y": 6.659294468642213, + "heading": 1.007478678720387, + "angularVelocity": 0.000017264096285479253, + "velocityX": -1.2163122531670139, + "velocityY": -0.26212896250040785, + "timestamp": 1.0023466301897526 + }, + { + "x": 0.7981646777386805, + "y": 6.645820413183375, + "heading": 1.007479482462847, + "angularVelocity": 0.000010424190260479305, + "velocityX": -0.8108748692571571, + "velocityY": -0.17475264114145156, + "timestamp": 1.0794502171274258 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 3.75205950615561e-7, - "velocityX": -0.8646152527568558, - "velocityY": -0.18633427367405322, - "timestamp": 0.9073573338797233 + "angularVelocity": 0.0000048888648998734084, + "velocityX": -0.4054374447762054, + "velocityY": -0.08737632039468787, + "timestamp": 1.1565538040650991 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": -2.242417201812226e-19, - "velocityX": -4.158054673626952e-19, - "velocityY": -9.844851544165139e-20, - "timestamp": 0.9829704450363669 + "angularVelocity": 6.720704922337583e-19, + "velocityX": -1.755081189133287e-18, + "velocityY": -1.4248543300928555e-19, + "timestamp": 1.2336573910027724 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.3.traj b/src/main/deploy/choreo/2 piece left.3.traj index 1ab8595..64c7407 100644 --- a/src/main/deploy/choreo/2 piece left.3.traj +++ b/src/main/deploy/choreo/2 piece left.3.traj @@ -4,154 +4,190 @@ "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": -2.242417201812226e-19, - "velocityX": -4.158054673626952e-19, - "velocityY": -9.844851544165139e-20, + "angularVelocity": 6.720704922337583e-19, + "velocityX": -1.755081189133287e-18, + "velocityY": -1.4248543300928555e-19, "timestamp": 0 }, { - "x": 0.867342073873612, - "y": 6.677517805054488, - "heading": 1.0074798401596599, - "angularVelocity": -2.0078718725992087e-7, - "velocityX": 1.0474996006171284, - "velocityY": 0.4008443829976782, - "timestamp": 0.09588364267332339 - }, - { - "x": 1.0464438315164646, - "y": 6.746054283251552, - "heading": 0.9900885492460468, - "angularVelocity": -0.18137912190993252, - "velocityX": 1.8679073160899227, - "velocityY": 0.7147880106158334, - "timestamp": 0.19176728534664655 - }, - { - "x": 1.2255455891596982, - "y": 6.814590761447637, - "heading": 0.9294452786879966, - "angularVelocity": -0.6324673204653116, - "velocityX": 1.8679073160938997, - "velocityY": 0.7147880106056311, - "timestamp": 0.2876509280199697 - }, - { - "x": 1.4046473468026037, - "y": 6.883127239644582, - "heading": 0.8469789832587827, - "angularVelocity": -0.8600663588697559, - "velocityX": 1.8679073160904744, - "velocityY": 0.7147880106145814, - "timestamp": 0.3835345706932929 - }, - { - "x": 1.5837491044451752, - "y": 6.951663717842399, - "heading": 0.7537506929542891, - "angularVelocity": -0.9723065134490515, - "velocityX": 1.867907316086992, - "velocityY": 0.7147880106236818, - "timestamp": 0.47941821336661605 - }, - { - "x": 1.762850862087631, - "y": 7.020200196040518, - "heading": 0.6553282675051919, - "angularVelocity": -1.0264777464121126, - "velocityX": 1.867907316085786, - "velocityY": 0.7147880106268334, - "timestamp": 0.5753018560399392 - }, - { - "x": 1.941952619730239, - "y": 7.088736674238239, - "heading": 0.5545759098990335, - "angularVelocity": -1.0507773254862967, - "velocityX": 1.8679073160873736, - "velocityY": 0.7147880106226845, - "timestamp": 0.6711854987132624 - }, - { - "x": 2.121054377373199, - "y": 7.157273152435041, - "heading": 0.453150200963735, - "angularVelocity": -1.0577999135979546, - "velocityX": 1.8679073160910444, - "velocityY": 0.7147880106130912, - "timestamp": 0.7670691413865856 - }, - { - "x": 2.3001561350165884, - "y": 7.225809630630721, - "heading": 0.3523691136343556, - "angularVelocity": -1.0510769566060587, - "velocityX": 1.8679073160955193, - "velocityY": 0.7147880106013975, - "timestamp": 0.8629527840599087 - }, - { - "x": 2.479257892660336, - "y": 7.294346108825463, - "heading": 0.2538978395296278, - "angularVelocity": -1.026987204065876, - "velocityX": 1.867907316099261, - "velocityY": 0.7147880105916198, - "timestamp": 0.9588364267332319 - }, - { - "x": 2.658359650304228, - "y": 7.362882587019828, - "heading": 0.16061512982409304, - "angularVelocity": -0.9728740701200732, - "velocityX": 1.8679073161007647, - "velocityY": 0.7147880105876907, - "timestamp": 1.054720069406555 - }, - { - "x": 2.8374614079479796, - "y": 7.431419065214562, - "heading": 0.07810324033722603, - "angularVelocity": -0.8605418733202074, - "velocityX": 1.867907316099298, - "velocityY": 0.7147880105915232, - "timestamp": 1.1506037120798782 - }, - { - "x": 3.0165631655914407, - "y": 7.499955543410054, - "heading": 0.01742736740360304, - "angularVelocity": -0.6328073406675466, - "velocityX": 1.8679073160962676, - "velocityY": 0.714788010599443, - "timestamp": 1.2464873547532014 - }, - { - "x": 3.195664923235554, - "y": 7.568492021603823, - "heading": 1.924276877277615e-8, - "angularVelocity": -0.18175517403118957, - "velocityX": 1.867907316103074, - "velocityY": 0.7147880105814662, - "timestamp": 1.3423709974265245 + "x": 0.8096592904256411, + "y": 6.655444460332552, + "heading": 1.0074798681868895, + "angularVelocity": 9.511953785975191e-8, + "velocityX": 0.4634590560851991, + "velocityY": 0.17735086375321324, + "timestamp": 0.09225258066850861 + }, + { + "x": 0.8951698755926256, + "y": 6.688166608994117, + "heading": 1.007479884316123, + "angularVelocity": 1.7483774726918832e-7, + "velocityX": 0.9269180823705058, + "velocityY": 0.3547017159243002, + "timestamp": 0.18450516133701722 + }, + { + "x": 1.0234357437188477, + "y": 6.737249828252512, + "heading": 1.0074798953442166, + "angularVelocity": 1.1954238566332632e-7, + "velocityX": 1.3903770192307126, + "velocityY": 0.5320525334111323, + "timestamp": 0.27675774200552583 + }, + { + "x": 1.1785230905174175, + "y": 6.796596764677482, + "heading": 0.989392624850058, + "angularVelocity": -0.19606248804195148, + "velocityX": 1.6811166221554836, + "velocityY": 0.6433092277192939, + "timestamp": 0.36901032267403444 + }, + { + "x": 1.3336104374264297, + "y": 6.855943700813832, + "heading": 0.9393084137910155, + "angularVelocity": -0.5429030894974105, + "velocityX": 1.68111662335266, + "velocityY": 0.6433092245907064, + "timestamp": 0.46126290334254305 + }, + { + "x": 1.488697784324255, + "y": 6.9152906369794165, + "heading": 0.8650983768805, + "angularVelocity": -0.8044223410635456, + "velocityX": 1.681116623231394, + "velocityY": 0.6433092249076018, + "timestamp": 0.5535154840110517 + }, + { + "x": 1.6437851312260245, + "y": 6.974637573134693, + "heading": 0.7734662642227176, + "angularVelocity": -0.9932742476554033, + "velocityX": 1.6811166232741508, + "velocityY": 0.6433092247958684, + "timestamp": 0.6457680646795603 + }, + { + "x": 1.7988724781311176, + "y": 7.033984509281286, + "heading": 0.6701256962472372, + "angularVelocity": -1.120191622029676, + "velocityX": 1.6811166233101758, + "velocityY": 0.6433092247017268, + "timestamp": 0.7380206453480689 + }, + { + "x": 1.9539598250398915, + "y": 7.093331445418259, + "heading": 0.5600545044204543, + "angularVelocity": -1.1931502731864139, + "velocityX": 1.6811166233500756, + "velocityY": 0.6433092245974589, + "timestamp": 0.8302732260165775 + }, + { + "x": 2.109047171953823, + "y": 7.152678381541754, + "heading": 0.4477833516576351, + "angularVelocity": -1.2169974210937597, + "velocityX": 1.6811166234059833, + "velocityY": 0.6433092244513593, + "timestamp": 0.9225258066850861 + }, + { + "x": 2.2641345188735014, + "y": 7.21202531765023, + "heading": 0.33769407921933825, + "angularVelocity": -1.1933462634924075, + "velocityX": 1.6811166234682786, + "velocityY": 0.6433092242885672, + "timestamp": 1.0147783873535947 + }, + { + "x": 2.4192218657983178, + "y": 7.271372253745279, + "heading": 0.23431876445689956, + "angularVelocity": -1.120568270430258, + "velocityX": 1.6811166235239754, + "velocityY": 0.6433092241430183, + "timestamp": 1.1070309680221033 + }, + { + "x": 2.5743092127283593, + "y": 7.3307191898266755, + "heading": 0.14263672871411986, + "angularVelocity": -0.9938154041697848, + "velocityX": 1.6811166235806112, + "velocityY": 0.6433092239950159, + "timestamp": 1.199283548690612 + }, + { + "x": 2.7293965596646075, + "y": 7.390066125891852, + "heading": 0.06836140472618683, + "angularVelocity": -0.8051300402622511, + "velocityX": 1.681116623647891, + "velocityY": 0.6433092238191984, + "timestamp": 1.2915361293591205 + }, + { + "x": 2.8844839065819534, + "y": 7.449413062006423, + "heading": 0.0181937383834693, + "angularVelocity": -0.5438077285120623, + "velocityX": 1.6811166234429973, + "velocityY": 0.6433092243546334, + "timestamp": 1.3837887100276292 + }, + { + "x": 3.0395712536852333, + "y": 7.5087599976351145, + "heading": -3.599554284111106e-8, + "angularVelocity": -0.19721696940259958, + "velocityX": 1.6811166254584862, + "velocityY": 0.64330921908778, + "timestamp": 1.4760412906961378 + }, + { + "x": 3.1678371216372656, + "y": 7.557843217348709, + "heading": -2.4922879509035407e-8, + "angularVelocity": 1.2002551312894157e-7, + "velocityX": 1.3903770173425265, + "velocityY": 0.5320525383454039, + "timestamp": 1.5682938713646464 + }, + { + "x": 3.2533477067184124, + "y": 7.590565366234588, + "heading": -8.779782162288685e-9, + "angularVelocity": 1.7498802992739692e-7, + "velocityX": 0.926918081440039, + "velocityY": 0.35470171835582626, + "timestamp": 1.6605464520331554 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -1.4869843514051715e-19, - "angularVelocity": -2.006887540420477e-7, - "velocityX": 1.0474996006097614, - "velocityY": 0.4008443830169303, - "timestamp": 1.4382546400998477 + "heading": 2.7720082526846182e-19, + "angularVelocity": 9.517112799549954e-8, + "velocityX": 0.4634590556993664, + "velocityY": 0.17735086476148368, + "timestamp": 1.7527990327016645 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -7.359523754309027e-20, - "angularVelocity": 1.5727023698046957e-20, - "velocityX": -4.856900753028092e-20, - "velocityY": -1.8631537844939477e-20, - "timestamp": 1.5341382827731709 + "heading": 1.3557196489765115e-19, + "angularVelocity": -6.565556495699838e-20, + "velocityX": 1.1847381270605898e-18, + "velocityY": 4.521287308945025e-19, + "timestamp": 1.8450516133701735 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.traj b/src/main/deploy/choreo/2 piece left.traj index 0f491e4..3dbf666 100644 --- a/src/main/deploy/choreo/2 piece left.traj +++ b/src/main/deploy/choreo/2 piece left.traj @@ -4,388 +4,478 @@ "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 2.0283839438048332e-19, - "velocityX": 8.094691061889947e-19, - "velocityY": 1.7638426740730563e-19, + "angularVelocity": -7.087535479777707e-19, + "velocityX": 7.50401412133545e-18, + "velocityY": 1.4256736684497237e-18, "timestamp": 0 }, { - "x": 0.8322802456836562, - "y": 6.653172699607564, - "heading": 1.0074798311324673, - "angularVelocity": -3.7400126935671097e-7, - "velocityX": 0.8646152527784429, - "velocityY": 0.18633427357388527, - "timestamp": 0.07561311115664361 - }, - { - "x": 0.9630327340876748, - "y": 6.681351325717821, - "heading": 1.007479762583371, - "angularVelocity": -9.065768535902331e-7, - "velocityX": 1.7292303729328837, - "velocityY": 0.3726685184515223, - "timestamp": 0.15122622231328722 - }, - { - "x": 1.1108648715080907, - "y": 6.713210807608451, - "heading": 0.935326621538057, - "angularVelocity": -0.9542411354538504, - "velocityX": 1.955112481936631, - "velocityY": 0.4213486444781865, - "timestamp": 0.22683933346993085 - }, - { - "x": 1.258697008933641, - "y": 6.745070289475266, - "heading": 0.8127259503436992, - "angularVelocity": -1.6214208001621926, - "velocityX": 1.9551124820045325, - "velocityY": 0.421348644163222, - "timestamp": 0.30245244462657445 - }, - { - "x": 1.406529146351303, - "y": 6.776929771378683, - "heading": 0.6640255812369789, - "angularVelocity": -1.966595036655294, - "velocityX": 1.9551124819002106, - "velocityY": 0.4213486446472832, - "timestamp": 0.37806555578321804 - }, - { - "x": 1.5543612837652037, - "y": 6.808789253299551, - "heading": 0.5044825425717008, - "angularVelocity": -2.1099917226625076, - "velocityX": 1.9551124818504668, - "velocityY": 0.42134864487809753, - "timestamp": 0.45367866693986164 - }, - { - "x": 1.702193421185673, - "y": 6.840648735189942, - "heading": 0.344692011554351, - "angularVelocity": -2.1132648633690048, - "velocityX": 1.9551124819373364, - "velocityY": 0.42134864447501086, - "timestamp": 0.5292917780965053 - }, - { - "x": 1.8500255586150136, - "y": 6.872508217039167, - "heading": 0.19547183240040739, - "angularVelocity": -1.9734696386822674, - "velocityX": 1.9551124820546628, - "velocityY": 0.42134864393060517, - "timestamp": 0.6049048892531489 - }, - { - "x": 1.9978576960460066, - "y": 6.904367698880726, - "heading": 0.07244487018227615, - "angularVelocity": -1.6270585925668755, - "velocityX": 1.9551124820765156, - "velocityY": 0.42134864382921067, - "timestamp": 0.6805180004097925 - }, - { - "x": 2.1456898334992545, - "y": 6.936227180619012, - "heading": 9.65887879992878e-8, - "angularVelocity": -0.9580980399471752, - "velocityX": 1.955112482370841, - "velocityY": 0.4213486424633956, - "timestamp": 0.7561311115664361 - }, - { - "x": 2.2764423218817926, - "y": 6.964405806828939, - "heading": 2.820501204963407e-8, - "angularVelocity": -9.043904534147509e-7, - "velocityX": 1.7292303726488056, - "velocityY": 0.37266851976968174, - "timestamp": 0.8317442227230797 + "x": 0.7981646775700082, + "y": 6.645820413966075, + "heading": 1.0074794822217241, + "angularVelocity": -0.000004891992156933861, + "velocityX": 0.4054374425885426, + "velocityY": 0.08737633054596244, + "timestamp": 0.07710358693768382 + }, + { + "x": 0.8606860382778649, + "y": 6.659294470675445, + "heading": 1.0074786780539715, + "angularVelocity": -0.000010429706123137783, + "velocityX": 0.8108748657619178, + "velocityY": 0.17475265736028606, + "timestamp": 0.15420717387536764 + }, + { + "x": 0.9544680756755046, + "y": 6.679505554656715, + "heading": 1.00747734637885, + "angularVelocity": -0.000017271247347385705, + "velocityX": 1.2163122511205047, + "velocityY": 0.2621289719971706, + "timestamp": 0.23131076081305146 + }, + { + "x": 1.0795107812763696, + "y": 6.706453661799292, + "heading": 1.007475162920361, + "angularVelocity": -0.000028318507292526304, + "velocityX": 1.6217495264121258, + "velocityY": 0.3495052333214708, + "timestamp": 0.3084143477507353 + }, + { + "x": 1.2151823459896474, + "y": 6.735692436676619, + "heading": 0.9400813447765213, + "angularVelocity": -0.8740685202921675, + "velocityX": 1.7596012079558552, + "velocityY": 0.37921419791997063, + "timestamp": 0.3855179346884191 + }, + { + "x": 1.3508539216363087, + "y": 6.764931160821833, + "heading": 0.8025402930124232, + "angularVelocity": -1.7838476422019192, + "velocityX": 1.7596013497570884, + "velocityY": 0.37921353994654156, + "timestamp": 0.4626215216261029 + }, + { + "x": 1.4865254934430294, + "y": 6.794169902784878, + "heading": 0.6117409589886299, + "angularVelocity": -2.4745844078304686, + "velocityX": 1.7596012999547292, + "velocityY": 0.3792137710360449, + "timestamp": 0.5397251085637867 + }, + { + "x": 1.6221970645812445, + "y": 6.8234086478498766, + "heading": 0.3987348321748914, + "angularVelocity": -2.7625968553952354, + "velocityX": 1.7596012912844983, + "velocityY": 0.3792138112670414, + "timestamp": 0.6168286955014706 + }, + { + "x": 1.757868635365604, + "y": 6.852647394556811, + "heading": 0.20722880927427756, + "angularVelocity": -2.4837498553133672, + "velocityX": 1.7596012866951443, + "velocityY": 0.37921383256224117, + "timestamp": 0.6939322824391544 + }, + { + "x": 1.8935401957987985, + "y": 6.881886189294451, + "heading": 0.06863797850596423, + "angularVelocity": -1.7974628194706992, + "velocityX": 1.7596011524450381, + "velocityY": 0.3792144554995873, + "timestamp": 0.7710358693768382 + }, + { + "x": 2.029211813519273, + "y": 6.911124718210696, + "heading": 0.000004762891013022644, + "angularVelocity": -0.8901429666355407, + "velocityX": 1.7596018954361505, + "velocityY": 0.37921100791168616, + "timestamp": 0.848139456314522 + }, + { + "x": 2.1542545037959298, + "y": 6.938072896534997, + "heading": 0.00000255375105981851, + "angularVelocity": -0.00002865158471797654, + "velocityX": 1.6217493276638106, + "velocityY": 0.34950615651748473, + "timestamp": 0.9252430432522059 + }, + { + "x": 2.2480365342099393, + "y": 6.958284012920916, + "heading": 0.0000012010318378255, + "angularVelocity": -0.000017544180183003836, + "velocityX": 1.2163121605458629, + "velocityY": 0.26212939227137494, + "timestamp": 1.0023466301898898 + }, + { + "x": 2.3105578914207334, + "y": 6.971758085856952, + "heading": 3.834757266936798e-7, + "angularVelocity": -0.000010603347309739972, + "velocityX": 0.8108748204065306, + "velocityY": 0.1747528678130866, + "timestamp": 1.0794502171275737 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.9396360429134164e-18, - "angularVelocity": -3.7301747793182745e-7, - "velocityX": 0.8646152526837502, - "velocityY": 0.18633427401327182, - "timestamp": 0.9073573338797233 + "heading": 3.134962737955188e-18, + "angularVelocity": -0.000004973513450108379, + "velocityX": 0.405437424010253, + "velocityY": 0.08737641675075283, + "timestamp": 1.1565538040652577 }, { "x": 2.3418185710906982, "y": 6.978495121002197, - "heading": -1.2932527092188034e-18, - "angularVelocity": 4.19600892926216e-20, - "velocityX": -8.058124746715222e-20, - "velocityY": -1.860414915840738e-20, - "timestamp": 0.9829704450363669 - }, - { - "x": 2.2764423218798027, - "y": 6.964405806838171, - "heading": 2.828141101686341e-8, - "angularVelocity": 3.7402787103507053e-7, - "velocityX": -0.8646152527100619, - "velocityY": -0.18633427389118332, - "timestamp": 1.0585835561930106 - }, - { - "x": 2.145689833491728, - "y": 6.936227180653932, - "heading": 9.68470451708674e-8, - "angularVelocity": 9.067955680869744e-7, - "velocityX": -1.7292303727220222, - "velocityY": -0.3726685194299493, - "timestamp": 1.1341966673496542 - }, - { - "x": 1.9978576960446424, - "y": 6.904367698887054, - "heading": 0.07260812222938247, - "angularVelocity": 0.960257080705476, - "velocityX": -1.9551124822893438, - "velocityY": -0.42134864284155416, - "timestamp": 1.2098097785062978 - }, - { - "x": 1.8500255586147867, - "y": 6.872508217040216, - "heading": 0.1958338387782673, - "angularVelocity": 1.6296871622383675, - "velocityX": -1.955112482061473, - "velocityY": -0.42134864389901044, - "timestamp": 1.2854228896629414 - }, - { - "x": 1.7021934211910146, - "y": 6.840648735165152, - "heading": 0.3451117743398479, - "angularVelocity": 1.9742334798568144, - "velocityX": -1.95511248198102, - "velocityY": -0.42134864427231705, - "timestamp": 1.361036000819585 - }, - { - "x": 1.5543612837761693, - "y": 6.8087892532486665, - "heading": 0.5047130791830543, - "angularVelocity": 2.110762305661104, - "velocityX": -1.9551124818629584, - "velocityY": -0.42134864482013473, - "timestamp": 1.4366491119762286 - }, - { - "x": 1.4065291463620155, - "y": 6.776929771328972, - "heading": 0.6639739239039864, - "angularVelocity": 2.106259645777567, - "velocityX": -1.9551124818538146, - "velocityY": -0.4213486448625627, - "timestamp": 1.5122622231328722 - }, - { - "x": 1.2586970089400131, - "y": 6.745070289445696, - "heading": 0.8125476250392986, - "angularVelocity": 1.9649198249166078, - "velocityX": -1.9551124819576147, - "velocityY": -0.4213486443809204, - "timestamp": 1.5878753342895158 - }, - { - "x": 1.1108648715115719, - "y": 6.713210807592297, - "heading": 0.9352215452290742, - "angularVelocity": 1.622389534212375, - "velocityX": -1.9551124820427679, - "velocityY": -0.42134864398580435, - "timestamp": 1.6634884454461594 - }, - { - "x": 0.9630327340807132, - "y": 6.681351325750123, - "heading": 1.0074797622855187, - "angularVelocity": 0.9556307887761831, - "velocityX": -1.9551124820747394, - "velocityY": -0.42134864383734444, - "timestamp": 1.739101556602803 - }, - { - "x": 0.8322802456820239, - "y": 6.653172699615138, - "heading": 1.0074798310413775, - "angularVelocity": 9.093113311976794e-7, - "velocityX": -1.7292303728624039, - "velocityY": -0.3726685187785591, - "timestamp": 1.8147146677594466 + "heading": 2.0846959931491773e-18, + "angularVelocity": -8.557421751891334e-20, + "velocityX": -4.987723153383344e-19, + "velocityY": -9.652716908345432e-20, + "timestamp": 1.2336573910029416 + }, + { + "x": 2.3105578909850855, + "y": 6.971758087878447, + "heading": 3.828776753639658e-7, + "angularVelocity": 0.000004965756984393634, + "velocityX": -0.40543742966046686, + "velocityY": -0.08737639053285204, + "timestamp": 1.3107609779406149 + }, + { + "x": 2.2480365327877774, + "y": 6.958284019520034, + "heading": 0.0000011992649992501823, + "angularVelocity": 0.000010588188647324088, + "velocityX": -0.8108748332013094, + "velocityY": -0.17475280844332217, + "timestamp": 1.3878645648782881 + }, + { + "x": 2.154254500585808, + "y": 6.938072911430576, + "heading": 0.0000025503266632355084, + "angularVelocity": 0.000017522682376308707, + "velocityX": -1.2163121837350852, + "velocityY": -0.2621292846699298, + "timestamp": 1.4649681518159614 + }, + { + "x": 2.0292118068653178, + "y": 6.911124749085196, + "heading": 0.000004757557407174423, + "angularVelocity": 0.000028626823103883053, + "velocityX": -1.6217493723290508, + "velocityY": -0.34950594927788836, + "timestamp": 1.5420717387536347 + }, + { + "x": 1.8935401996566608, + "y": 6.881886171392507, + "heading": 0.06878997325201827, + "angularVelocity": 0.8921143415832743, + "velocityX": -1.7596017591026925, + "velocityY": -0.3792116405210038, + "timestamp": 1.619175325691308 + }, + { + "x": 1.7578686367778409, + "y": 6.852647388002959, + "heading": 0.20749385875571683, + "angularVelocity": 1.7989290902357107, + "velocityX": -1.7596011841639752, + "velocityY": -0.379214308319837, + "timestamp": 1.6962789126289812 + }, + { + "x": 1.6221970666934797, + "y": 6.823408638048033, + "heading": 0.39902661142653667, + "angularVelocity": 2.484096528811891, + "velocityX": -1.7596012776167158, + "velocityY": -0.3792138746873246, + "timestamp": 1.7733824995666545 + }, + { + "x": 1.4865254965870125, + "y": 6.794169888195682, + "heading": 0.6119135529013362, + "angularVelocity": 2.7610510733681806, + "velocityX": -1.7596012779034225, + "velocityY": -0.37921387335696644, + "timestamp": 1.8504860865043278 + }, + { + "x": 1.3508539260055112, + "y": 6.7649311405475485, + "heading": 0.8026194720400166, + "angularVelocity": 2.473372857385191, + "velocityX": -1.7596012840644037, + "velocityY": -0.37921384476922, + "timestamp": 1.927589673442001 + }, + { + "x": 1.2151823556967136, + "y": 6.735692391634036, + "heading": 0.9401132494355333, + "angularVelocity": 1.7832345141952992, + "velocityX": -1.7596012805275563, + "velocityY": -0.37921386118065376, + "timestamp": 2.0046932603796743 + }, + { + "x": 1.0795107810200666, + "y": 6.706453662988116, + "heading": 1.0074751648495315, + "angularVelocity": 0.873654755756699, + "velocityX": -1.7596013371766643, + "velocityY": -0.37921359831878204, + "timestamp": 2.0817968473173476 + }, + { + "x": 0.9544680762714443, + "y": 6.679505551891246, + "heading": 1.007477347596638, + "angularVelocity": 0.000028309280971946704, + "velocityX": -1.6217495153591333, + "velocityY": -0.34950528460696234, + "timestamp": 2.158900434255021 + }, + { + "x": 0.8606860387160242, + "y": 6.659294468642213, + "heading": 1.007478678720387, + "angularVelocity": 0.000017264096285479253, + "velocityX": -1.2163122531670139, + "velocityY": -0.26212896250040785, + "timestamp": 2.236004021192694 + }, + { + "x": 0.7981646777386805, + "y": 6.645820413183375, + "heading": 1.007479482462847, + "angularVelocity": 0.000010424190260479305, + "velocityX": -0.8108748692571571, + "velocityY": -0.17475264114145156, + "timestamp": 2.3131076081303674 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 3.75205950615561e-7, - "velocityX": -0.8646152527568558, - "velocityY": -0.18633427367405322, - "timestamp": 1.8903277789160902 + "angularVelocity": 0.0000048888648998734084, + "velocityX": -0.4054374447762054, + "velocityY": -0.08737632039468787, + "timestamp": 2.3902111950680407 }, { "x": 0.7669039964675903, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": -2.242417201812226e-19, - "velocityX": -4.158054673626952e-19, - "velocityY": -9.844851544165139e-20, - "timestamp": 1.9659408900727338 - }, - { - "x": 0.867342073873612, - "y": 6.677517805054488, - "heading": 1.0074798401596599, - "angularVelocity": -2.0078718725992087e-7, - "velocityX": 1.0474996006171284, - "velocityY": 0.4008443829976782, - "timestamp": 2.061824532746057 - }, - { - "x": 1.0464438315164646, - "y": 6.746054283251552, - "heading": 0.9900885492460468, - "angularVelocity": -0.18137912190993252, - "velocityX": 1.8679073160899227, - "velocityY": 0.7147880106158334, - "timestamp": 2.1577081754193803 - }, - { - "x": 1.2255455891596982, - "y": 6.814590761447637, - "heading": 0.9294452786879966, - "angularVelocity": -0.6324673204653116, - "velocityX": 1.8679073160938997, - "velocityY": 0.7147880106056311, - "timestamp": 2.2535918180927035 - }, - { - "x": 1.4046473468026037, - "y": 6.883127239644582, - "heading": 0.8469789832587827, - "angularVelocity": -0.8600663588697559, - "velocityX": 1.8679073160904744, - "velocityY": 0.7147880106145814, - "timestamp": 2.3494754607660266 - }, - { - "x": 1.5837491044451752, - "y": 6.951663717842399, - "heading": 0.7537506929542891, - "angularVelocity": -0.9723065134490515, - "velocityX": 1.867907316086992, - "velocityY": 0.7147880106236818, - "timestamp": 2.44535910343935 - }, - { - "x": 1.762850862087631, - "y": 7.020200196040518, - "heading": 0.6553282675051919, - "angularVelocity": -1.0264777464121126, - "velocityX": 1.867907316085786, - "velocityY": 0.7147880106268334, - "timestamp": 2.541242746112673 - }, - { - "x": 1.941952619730239, - "y": 7.088736674238239, - "heading": 0.5545759098990335, - "angularVelocity": -1.0507773254862967, - "velocityX": 1.8679073160873736, - "velocityY": 0.7147880106226845, - "timestamp": 2.637126388785996 - }, - { - "x": 2.121054377373199, - "y": 7.157273152435041, - "heading": 0.453150200963735, - "angularVelocity": -1.0577999135979546, - "velocityX": 1.8679073160910444, - "velocityY": 0.7147880106130912, - "timestamp": 2.7330100314593193 - }, - { - "x": 2.3001561350165884, - "y": 7.225809630630721, - "heading": 0.3523691136343556, - "angularVelocity": -1.0510769566060587, - "velocityX": 1.8679073160955193, - "velocityY": 0.7147880106013975, - "timestamp": 2.8288936741326425 - }, - { - "x": 2.479257892660336, - "y": 7.294346108825463, - "heading": 0.2538978395296278, - "angularVelocity": -1.026987204065876, - "velocityX": 1.867907316099261, - "velocityY": 0.7147880105916198, - "timestamp": 2.9247773168059656 - }, - { - "x": 2.658359650304228, - "y": 7.362882587019828, - "heading": 0.16061512982409304, - "angularVelocity": -0.9728740701200732, - "velocityX": 1.8679073161007647, - "velocityY": 0.7147880105876907, - "timestamp": 3.020660959479289 - }, - { - "x": 2.8374614079479796, - "y": 7.431419065214562, - "heading": 0.07810324033722603, - "angularVelocity": -0.8605418733202074, - "velocityX": 1.867907316099298, - "velocityY": 0.7147880105915232, - "timestamp": 3.116544602152612 - }, - { - "x": 3.0165631655914407, - "y": 7.499955543410054, - "heading": 0.01742736740360304, - "angularVelocity": -0.6328073406675466, - "velocityX": 1.8679073160962676, - "velocityY": 0.714788010599443, - "timestamp": 3.212428244825935 - }, - { - "x": 3.195664923235554, - "y": 7.568492021603823, - "heading": 1.924276877277615e-8, - "angularVelocity": -0.18175517403118957, - "velocityX": 1.867907316103074, - "velocityY": 0.7147880105814662, - "timestamp": 3.3083118874992583 + "angularVelocity": 6.720704922337583e-19, + "velocityX": -1.755081189133287e-18, + "velocityY": -1.4248543300928555e-19, + "timestamp": 2.467314782005714 + }, + { + "x": 0.8096592904256411, + "y": 6.655444460332552, + "heading": 1.0074798681868895, + "angularVelocity": 9.511953785975191e-8, + "velocityX": 0.4634590560851991, + "velocityY": 0.17735086375321324, + "timestamp": 2.5595673626742226 + }, + { + "x": 0.8951698755926256, + "y": 6.688166608994117, + "heading": 1.007479884316123, + "angularVelocity": 1.7483774726918832e-7, + "velocityX": 0.9269180823705058, + "velocityY": 0.3547017159243002, + "timestamp": 2.651819943342731 + }, + { + "x": 1.0234357437188477, + "y": 6.737249828252512, + "heading": 1.0074798953442166, + "angularVelocity": 1.1954238566332632e-7, + "velocityX": 1.3903770192307126, + "velocityY": 0.5320525334111323, + "timestamp": 2.74407252401124 + }, + { + "x": 1.1785230905174175, + "y": 6.796596764677482, + "heading": 0.989392624850058, + "angularVelocity": -0.19606248804195148, + "velocityX": 1.6811166221554836, + "velocityY": 0.6433092277192939, + "timestamp": 2.8363251046797484 + }, + { + "x": 1.3336104374264297, + "y": 6.855943700813832, + "heading": 0.9393084137910155, + "angularVelocity": -0.5429030894974105, + "velocityX": 1.68111662335266, + "velocityY": 0.6433092245907064, + "timestamp": 2.928577685348257 + }, + { + "x": 1.488697784324255, + "y": 6.9152906369794165, + "heading": 0.8650983768805, + "angularVelocity": -0.8044223410635456, + "velocityX": 1.681116623231394, + "velocityY": 0.6433092249076018, + "timestamp": 3.0208302660167656 + }, + { + "x": 1.6437851312260245, + "y": 6.974637573134693, + "heading": 0.7734662642227176, + "angularVelocity": -0.9932742476554033, + "velocityX": 1.6811166232741508, + "velocityY": 0.6433092247958684, + "timestamp": 3.1130828466852742 + }, + { + "x": 1.7988724781311176, + "y": 7.033984509281286, + "heading": 0.6701256962472372, + "angularVelocity": -1.120191622029676, + "velocityX": 1.6811166233101758, + "velocityY": 0.6433092247017268, + "timestamp": 3.205335427353783 + }, + { + "x": 1.9539598250398915, + "y": 7.093331445418259, + "heading": 0.5600545044204543, + "angularVelocity": -1.1931502731864139, + "velocityX": 1.6811166233500756, + "velocityY": 0.6433092245974589, + "timestamp": 3.2975880080222915 + }, + { + "x": 2.109047171953823, + "y": 7.152678381541754, + "heading": 0.4477833516576351, + "angularVelocity": -1.2169974210937597, + "velocityX": 1.6811166234059833, + "velocityY": 0.6433092244513593, + "timestamp": 3.3898405886908 + }, + { + "x": 2.2641345188735014, + "y": 7.21202531765023, + "heading": 0.33769407921933825, + "angularVelocity": -1.1933462634924075, + "velocityX": 1.6811166234682786, + "velocityY": 0.6433092242885672, + "timestamp": 3.4820931693593087 + }, + { + "x": 2.4192218657983178, + "y": 7.271372253745279, + "heading": 0.23431876445689956, + "angularVelocity": -1.120568270430258, + "velocityX": 1.6811166235239754, + "velocityY": 0.6433092241430183, + "timestamp": 3.5743457500278173 + }, + { + "x": 2.5743092127283593, + "y": 7.3307191898266755, + "heading": 0.14263672871411986, + "angularVelocity": -0.9938154041697848, + "velocityX": 1.6811166235806112, + "velocityY": 0.6433092239950159, + "timestamp": 3.666598330696326 + }, + { + "x": 2.7293965596646075, + "y": 7.390066125891852, + "heading": 0.06836140472618683, + "angularVelocity": -0.8051300402622511, + "velocityX": 1.681116623647891, + "velocityY": 0.6433092238191984, + "timestamp": 3.7588509113648345 + }, + { + "x": 2.8844839065819534, + "y": 7.449413062006423, + "heading": 0.0181937383834693, + "angularVelocity": -0.5438077285120623, + "velocityX": 1.6811166234429973, + "velocityY": 0.6433092243546334, + "timestamp": 3.851103492033343 + }, + { + "x": 3.0395712536852333, + "y": 7.5087599976351145, + "heading": -3.599554284111106e-8, + "angularVelocity": -0.19721696940259958, + "velocityX": 1.6811166254584862, + "velocityY": 0.64330921908778, + "timestamp": 3.9433560727018517 + }, + { + "x": 3.1678371216372656, + "y": 7.557843217348709, + "heading": -2.4922879509035407e-8, + "angularVelocity": 1.2002551312894157e-7, + "velocityX": 1.3903770173425265, + "velocityY": 0.5320525383454039, + "timestamp": 4.03560865337036 + }, + { + "x": 3.2533477067184124, + "y": 7.590565366234588, + "heading": -8.779782162288685e-9, + "angularVelocity": 1.7498802992739692e-7, + "velocityX": 0.926918081440039, + "velocityY": 0.35470171835582626, + "timestamp": 4.127861234038869 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -1.4869843514051715e-19, - "angularVelocity": -2.006887540420477e-7, - "velocityX": 1.0474996006097614, - "velocityY": 0.4008443830169303, - "timestamp": 3.4041955301725815 + "heading": 2.7720082526846182e-19, + "angularVelocity": 9.517112799549954e-8, + "velocityX": 0.4634590556993664, + "velocityY": 0.17735086476148368, + "timestamp": 4.2201138147073785 }, { "x": 3.296103000640869, "y": 7.606926441192627, - "heading": -7.359523754309027e-20, - "angularVelocity": 1.5727023698046957e-20, - "velocityX": -4.856900753028092e-20, - "velocityY": -1.8631537844939477e-20, - "timestamp": 3.5000791728459046 + "heading": 1.3557196489765115e-19, + "angularVelocity": -6.565556495699838e-20, + "velocityX": 1.1847381270605898e-18, + "velocityY": 4.521287308945025e-19, + "timestamp": 4.3123663953758875 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.1.traj b/src/main/deploy/choreo/2 piece right.1.traj index a06f7df..15f9a40 100644 --- a/src/main/deploy/choreo/2 piece right.1.traj +++ b/src/main/deploy/choreo/2 piece right.1.traj @@ -1,130 +1,157 @@ { "samples": [ { - "x": 0.7823778390884399, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 3.4836656833317357e-19, - "velocityX": 5.792784763551635e-18, - "velocityY": -1.1468055635760044e-18, + "angularVelocity": -2.7495232446215487e-17, + "velocityX": -5.069420583996102e-17, + "velocityY": 1.192104631717783e-17, "timestamp": 0 }, { - "x": 0.848441149203722, - "y": 4.472500429335688, - "heading": -0.9928952478971514, - "angularVelocity": 5.064569480290326e-7, - "velocityX": 0.8673046782767408, - "velocityY": -0.20407170438519942, - "timestamp": 0.0761708218230117 - }, - { - "x": 0.9805677659026901, - "y": 4.4414118103682325, - "heading": -0.9928951486952988, - "angularVelocity": 0.0000013023602772620627, - "velocityX": 1.7346093101893227, - "velocityY": -0.40814341008019434, - "timestamp": 0.1523416436460234 - }, - { - "x": 1.1288597388405326, - "y": 4.406519593265519, - "heading": -0.9209894371787697, - "angularVelocity": 0.9440059828106777, - "velocityX": 1.9468343571559381, - "velocityY": -0.4580785170440767, - "timestamp": 0.2285124654690351 - }, - { - "x": 1.2771517113083277, - "y": 4.371627374165104, - "heading": -0.8000545061710758, - "angularVelocity": 1.587680533219069, - "velocityX": 1.9468343509849748, - "velocityY": -0.4580785432706628, - "timestamp": 0.3046832872920468 - }, - { - "x": 1.4254436833762532, - "y": 4.336735153365243, - "heading": -0.6538028446856889, - "angularVelocity": 1.9200483595308036, - "velocityX": 1.9468343457353292, - "velocityY": -0.45807856558166105, - "timestamp": 0.3808541091150585 - }, - { - "x": 1.57373565515521, - "y": 4.301842931337266, - "heading": -0.4970386957469823, - "angularVelocity": 2.058060359424233, - "velocityX": 1.9468343419416414, - "velocityY": -0.458078581704838, - "timestamp": 0.45702493093807023 - }, - { - "x": 1.7220276265458214, - "y": 4.26695070765882, - "heading": -0.3400744666794648, - "angularVelocity": 2.060687088715348, - "velocityX": 1.946834336843289, - "velocityY": -0.4580786033728385, - "timestamp": 0.5331957527610819 - }, - { - "x": 1.8703195977756357, - "y": 4.232058483296987, - "heading": -0.19340046314195958, - "angularVelocity": 1.9255930292876797, - "velocityX": 1.9468343347322845, - "velocityY": -0.45807861234460967, - "timestamp": 0.6093665745840936 - }, - { - "x": 2.0186115688161, - "y": 4.197166258130416, - "heading": -0.0721222371749164, - "angularVelocity": 1.592187442178866, - "velocityX": 1.946834332246427, - "velocityY": -0.45807862290950535, - "timestamp": 0.6855373964071053 - }, - { - "x": 2.1669035397570235, - "y": 4.162274032540797, - "heading": -1.3790420971273228e-7, - "angularVelocity": 0.946846804912877, - "velocityX": 1.9468343309396154, - "velocityY": -0.4580786284634526, - "timestamp": 0.761708218230117 - }, - { - "x": 2.299030158583636, - "y": 4.131185422616258, - "heading": -3.861728516505335e-8, - "angularVelocity": 0.0000013034771345390653, - "velocityX": 1.7346093381218648, - "velocityY": -0.4081432913612903, - "timestamp": 0.8378790400531287 - }, - { - "x": 2.365093469619751, + "x": 0.8155240483745178, + "y": 4.480245631878025, + "heading": -0.9928952427133843, + "angularVelocity": 5.500196348995766e-7, + "velocityX": 0.4166008785692591, + "velocityY": -0.09802372135573052, + "timestamp": 0.07956346467603777 + }, + { + "x": 0.8818164643488411, + "y": 4.46464741870627, + "heading": -0.9928951428202392, + "angularVelocity": 0.000001255529267463834, + "velocityX": 0.8332017244870482, + "velocityY": -0.19604743502871538, + "timestamp": 0.15912692935207554 + }, + { + "x": 0.9812550792231622, + "y": 4.441250101086857, + "heading": -0.9928949496672224, + "angularVelocity": 0.000002427691463657242, + "velocityX": 1.2498024725159502, + "velocityY": -0.2940711256682478, + "timestamp": 0.23869039402811332 + }, + { + "x": 1.1129174292533976, + "y": 4.410270729279662, + "heading": -0.988662240113345, + "angularVelocity": 0.05319916084717845, + "velocityX": 1.654809158529407, + "velocityY": -0.3893668021287818, + "timestamp": 0.31825385870415107 + }, + { + "x": 1.244579779283711, + "y": 4.379291357472773, + "heading": -0.9114902323862101, + "angularVelocity": 0.969942775132182, + "velocityX": 1.6548091585300193, + "velocityY": -0.3893668021248496, + "timestamp": 0.3978173233801888 + }, + { + "x": 1.3762421293125207, + "y": 4.3483119856594845, + "heading": -0.7738179037346776, + "angularVelocity": 1.7303460729208497, + "velocityX": 1.6548091585111195, + "velocityY": -0.3893668022052987, + "timestamp": 0.4773807880562266 + }, + { + "x": 1.507904479337117, + "y": 4.317332613828277, + "heading": -0.594124699134844, + "angularVelocity": 2.2584889349524437, + "velocityX": 1.6548091584581641, + "velocityY": -0.3893668024305141, + "timestamp": 0.5569442527322643 + }, + { + "x": 1.6395668293620107, + "y": 4.286353241998326, + "heading": -0.39886759130073074, + "angularVelocity": 2.4541051426819718, + "velocityX": 1.654809158461902, + "velocityY": -0.3893668024147043, + "timestamp": 0.6365077174083021 + }, + { + "x": 1.7712291793925206, + "y": 4.25537387019225, + "heading": -0.21914201405263553, + "angularVelocity": 2.258895813265647, + "velocityX": 1.6548091585324882, + "velocityY": -0.38936680211463054, + "timestamp": 0.71607118208434 + }, + { + "x": 1.9028915294236735, + "y": 4.2243944983889214, + "heading": -0.08144285462028061, + "angularVelocity": 1.7306832978144895, + "velocityX": 1.6548091585405724, + "velocityY": -0.3893668020801211, + "timestamp": 0.7956346467603778 + }, + { + "x": 2.034553879453989, + "y": 4.1934151265820425, + "heading": -0.004251710722878646, + "angularVelocity": 0.9701832896755161, + "velocityX": 1.6548091585300484, + "velocityY": -0.38936680212472563, + "timestamp": 0.8751981114364156 + }, + { + "x": 2.166216229487868, + "y": 4.162435754790337, + "heading": -3.367292673298042e-7, + "angularVelocity": 0.053433746406308845, + "velocityX": 1.654809158575206, + "velocityY": -0.3893668019341025, + "timestamp": 0.9547615761124534 + }, + { + "x": 2.2656548443604327, + "y": 4.139038437163738, + "heading": -1.4361728442139784e-7, + "angularVelocity": 0.000002427175717358418, + "velocityX": 1.2498024724938752, + "velocityY": -0.2940711257585697, + "timestamp": 1.0343250407884912 + }, + { + "x": 2.331947260333981, + "y": 4.123440223988747, + "heading": -4.37491568242842e-8, + "angularVelocity": 0.0000012552148287255357, + "velocityX": 0.8332017244772982, + "velocityY": -0.1960474350693968, + "timestamp": 1.1138885054645289 + }, + { + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -2.2763209665573703e-18, - "angularVelocity": 5.069826509442209e-7, - "velocityX": 0.8673046903657866, - "velocityY": -0.2040716530068378, - "timestamp": 0.9140498618761405 + "heading": 1.4952923019590338e-16, + "angularVelocity": 5.498706671572415e-7, + "velocityX": 0.41660087856538297, + "velocityY": -0.09802372137198472, + "timestamp": 1.1934519701405666 }, { - "x": 2.365093469619751, + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -1.5143047799084299e-18, - "angularVelocity": 5.84054737228804e-20, - "velocityX": -8.219525464434245e-19, - "velocityY": 2.1312698857170775e-19, - "timestamp": 0.9902206836991522 + "heading": 9.94909152424984e-17, + "angularVelocity": -3.9263017083187414e-18, + "velocityX": 3.083103040454795e-18, + "velocityY": -7.248912091154562e-19, + "timestamp": 1.2730154348166043 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.2.traj b/src/main/deploy/choreo/2 piece right.2.traj index 2477c36..6a4b278 100644 --- a/src/main/deploy/choreo/2 piece right.2.traj +++ b/src/main/deploy/choreo/2 piece right.2.traj @@ -1,130 +1,157 @@ { "samples": [ { - "x": 2.365093469619751, + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -1.5143047799084299e-18, - "angularVelocity": 5.84054737228804e-20, - "velocityX": -8.219525464434245e-19, - "velocityY": 2.1312698857170775e-19, + "heading": 9.94909152424984e-17, + "angularVelocity": -3.9263017083187414e-18, + "velocityX": 3.083103040454795e-18, + "velocityY": -7.248912091154562e-19, "timestamp": 0 }, { - "x": 2.299030158572247, - "y": 4.131185422567857, - "heading": -3.8824131952933046e-8, - "angularVelocity": -5.096982155619529e-7, - "velocityX": -0.867304690515307, - "velocityY": 0.20407165237139407, - "timestamp": 0.07617082182301205 - }, - { - "x": 2.166903539754318, - "y": 4.162274032529242, - "heading": -1.3861601297880188e-7, - "angularVelocity": -0.000001310106398423391, - "velocityX": -1.7346093380078471, - "velocityY": 0.408143291845037, - "timestamp": 0.1523416436460242 - }, - { - "x": 2.018611568604941, - "y": 4.197166257232936, - "heading": -0.0722676107720337, - "angularVelocity": -0.948755316359051, - "velocityX": -1.9468343336762697, - "velocityY": 0.4580786168326712, - "timestamp": 0.22851246546903636 - }, - { - "x": 1.8703195973960691, - "y": 4.2320584816837785, - "heading": -0.19373209136885944, - "angularVelocity": -1.5946326649731632, - "velocityX": -1.946834334457335, - "velocityY": 0.458078613513146, - "timestamp": 0.3046832872920485 - }, - { - "x": 1.7220276260831988, - "y": 4.2669507056926275, - "heading": -0.34047859723492613, - "angularVelocity": -1.9265448678897275, - "velocityX": -1.9468343358226654, - "velocityY": 0.45807860771049036, - "timestamp": 0.3808541091150607 - }, - { - "x": 1.5737356547013517, - "y": 4.301842929408325, - "heading": -0.4972933443760097, - "angularVelocity": -2.058724632188595, - "velocityX": -1.946834336728219, - "velocityY": 0.4580786038618867, - "timestamp": 0.45702493093807284 - }, - { - "x": 1.4254436831497614, - "y": 4.336735152402613, - "heading": -0.6538038889287485, - "angularVelocity": -2.0547309429902363, - "velocityX": -1.9468343389566751, - "velocityY": 0.458078594390947, - "timestamp": 0.533195752761085 - }, - { - "x": 1.2771517114677216, - "y": 4.371627374842491, - "heading": -0.799921474135194, - "angularVelocity": -1.9182881543008596, - "velocityX": -1.946834340669268, - "velocityY": 0.45807858711242555, - "timestamp": 0.6093665745840972 - }, - { - "x": 1.1288597396330466, - "y": 4.406519596633671, - "heading": -0.9209030529092569, - "angularVelocity": -1.5882929431321022, - "velocityX": -1.9468343426731174, - "velocityY": 0.4580785785960651, - "timestamp": 0.6855373964071093 - }, - { - "x": 0.9805677676343831, - "y": 4.4414118177279, - "heading": -0.9928951480324381, - "angularVelocity": -0.945140060198636, - "velocityX": -1.946834344826023, - "velocityY": 0.45807856944623543, - "timestamp": 0.7617082182301215 - }, - { - "x": 0.8484411497365192, - "y": 4.472500431600075, - "heading": -0.9928952477030265, - "angularVelocity": -0.0000013085140236118818, - "velocityX": -1.734609325928881, - "velocityY": 0.40814334318738743, - "timestamp": 0.8378790400531336 - }, - { - "x": 0.7823778390884398, + "x": 2.331947260333817, + "y": 4.12344022398805, + "heading": -4.377032610264403e-8, + "angularVelocity": -5.501367379565397e-7, + "velocityX": -0.416600878567453, + "velocityY": 0.09802372136321691, + "timestamp": 0.07956346467603659 + }, + { + "x": 2.2656548443599256, + "y": 4.1390384371615845, + "heading": -1.4368564209179275e-7, + "angularVelocity": -0.0000012558079284124016, + "velocityX": -0.8332017244816303, + "velocityY": 0.1960474350510945, + "timestamp": 0.15912692935207318 + }, + { + "x": 2.1662162294867975, + "y": 4.162435754785825, + "heading": -3.368854431485356e-7, + "angularVelocity": -0.000002428279485036437, + "velocityX": -1.249802472500972, + "velocityY": 0.2940711257289318, + "timestamp": 0.23869039402810976 + }, + { + "x": 2.034553879452772, + "y": 4.193415126576901, + "heading": -0.004253358762050876, + "angularVelocity": -0.05345445796038289, + "velocityX": -1.6548091585770712, + "velocityY": 0.38936680192617446, + "timestamp": 0.31825385870414635 + }, + { + "x": 1.902891529421956, + "y": 4.224394498381643, + "heading": -0.08145530813915991, + "angularVelocity": -0.9703190992427685, + "velocityX": -1.6548091585363642, + "velocityY": 0.38936680209788715, + "timestamp": 0.39781732338018294 + }, + { + "x": 1.7712291793910706, + "y": 4.255373870186102, + "heading": -0.21916938411480638, + "angularVelocity": -1.7308707776251606, + "velocityX": -1.6548091585372338, + "velocityY": 0.38936680209431446, + "timestamp": 0.47738078805621953 + }, + { + "x": 1.6395668293638401, + "y": 4.286353242006105, + "heading": -0.3988962080198311, + "angularVelocity": -2.2589114819767695, + "velocityX": -1.6548091584912985, + "velocityY": 0.38936680228969167, + "timestamp": 0.5569442527322561 + }, + { + "x": 1.50790447934162, + "y": 4.31733261384741, + "heading": -0.5941239976444426, + "angularVelocity": -2.4537366543360197, + "velocityX": -1.6548091584283233, + "velocityY": 0.38936680255742584, + "timestamp": 0.6365077174082927 + }, + { + "x": 1.3762421293155438, + "y": 4.34831198567232, + "heading": -0.7738010431009228, + "angularVelocity": -2.2582858374199257, + "velocityX": -1.6548091584767906, + "velocityY": 0.38936680235136056, + "timestamp": 0.7160711820843293 + }, + { + "x": 1.2445797792853979, + "y": 4.379291357479921, + "heading": -0.911479619218358, + "angularVelocity": -1.7304245947137935, + "velocityX": -1.6548091585279405, + "velocityY": 0.38936680213381303, + "timestamp": 0.7956346467603657 + }, + { + "x": 1.1129174292546509, + "y": 4.410270729284957, + "heading": -0.9886607024030146, + "angularVelocity": -0.9700568407721322, + "velocityX": -1.6548091585354978, + "velocityY": 0.38936680210156993, + "timestamp": 0.8751981114364022 + }, + { + "x": 0.9812550792241712, + "y": 4.441250101091103, + "heading": -0.9928949494741156, + "angularVelocity": -0.05321848525978111, + "velocityX": -1.6548091585325024, + "velocityY": 0.38936680211562363, + "timestamp": 0.9547615761124388 + }, + { + "x": 0.8818164643493253, + "y": 4.46464741870832, + "heading": -0.992895142733805, + "angularVelocity": -0.0000024290322074090013, + "velocityX": -1.2498024725225658, + "velocityY": 0.2940711256406529, + "timestamp": 1.0343250407884754 + }, + { + "x": 0.8155240483746753, + "y": 4.4802456318786925, + "heading": -0.9928952426862623, + "angularVelocity": -0.0000012562747482591942, + "velocityX": -0.8332017244911675, + "velocityY": 0.19604743501134286, + "timestamp": 1.113888505464512 + }, + { + "x": 0.78237783908844, "y": 4.488044738769531, - "heading": -0.9928952864743934, - "angularVelocity": -5.09005494945374e-7, - "velocityX": -0.8673046852715057, - "velocityY": 0.2040716746574537, - "timestamp": 0.9140498618761458 + "heading": -0.9928952864743935, + "angularVelocity": -5.503605231480291e-7, + "velocityX": -0.41660087857124434, + "velocityY": 0.0980237213473347, + "timestamp": 1.1934519701405486 }, { - "x": 0.7823778390884398, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 1.8053835791622905e-18, - "velocityX": 3.8085009658841144e-17, - "velocityY": -4.441213712458509e-17, - "timestamp": 0.9902206836991579 + "angularVelocity": 2.916682100466704e-17, + "velocityX": 2.7675115242860868e-17, + "velocityY": -1.1179897514445951e-17, + "timestamp": 1.2730154348165852 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.3.traj b/src/main/deploy/choreo/2 piece right.3.traj index b1995c1..4166fc1 100644 --- a/src/main/deploy/choreo/2 piece right.3.traj +++ b/src/main/deploy/choreo/2 piece right.3.traj @@ -1,166 +1,202 @@ { "samples": [ { - "x": 0.7823778390884398, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 1.8053835791622905e-18, - "velocityX": 3.8085009658841144e-17, - "velocityY": -4.441213712458509e-17, + "angularVelocity": 2.916682100466704e-17, + "velocityX": 2.7675115242860868e-17, + "velocityY": -1.1179897514445951e-17, "timestamp": 0 }, { - "x": 0.8633075709529878, - "y": 4.398931081775789, - "heading": -0.9928954814598945, - "angularVelocity": -0.0000019220769098791852, - "velocityX": 0.7977678757913192, - "velocityY": -0.8784412255669002, - "timestamp": 0.10144521272465723 - }, - { - "x": 0.9997101217758085, - "y": 4.248734981396651, - "heading": -0.9705934228713969, - "angularVelocity": 0.21984338136319737, - "velocityX": 1.3445932751212721, - "velocityY": -1.480563708676927, - "timestamp": 0.20289042544931424 - }, - { - "x": 1.1361126726042652, - "y": 4.098538881022651, - "heading": -0.9135846602313676, - "angularVelocity": 0.5619660219429253, - "velocityX": 1.3445932751768286, - "velocityY": -1.4805637086262706, - "timestamp": 0.30433563817397125 - }, - { - "x": 1.2725152234348027, - "y": 3.948342780650542, - "heading": -0.8396943742506122, - "angularVelocity": 0.7283762732235443, - "velocityX": 1.3445932751973406, - "velocityY": -1.4805637086076424, - "timestamp": 0.40578085089862825 - }, - { - "x": 1.4089177742668983, - "y": 3.7981466802798476, - "heading": -0.7577094886902542, - "angularVelocity": 0.808169093034302, - "velocityX": 1.3445932752127017, - "velocityY": -1.4805637085936922, - "timestamp": 0.5072260636232853 - }, - { - "x": 1.5453203251000565, - "y": 3.6479505799101184, - "heading": -0.6718990052626181, - "angularVelocity": 0.8458800678997359, - "velocityX": 1.3445932752231742, - "velocityY": -1.4805637085841812, - "timestamp": 0.6086712763479423 - }, - { - "x": 1.6817228759337532, - "y": 3.4977544795408777, - "heading": -0.5843529548214927, - "angularVelocity": 0.8629884850135144, - "velocityX": 1.3445932752284844, - "velocityY": -1.4805637085793588, - "timestamp": 0.7101164890725993 - }, - { - "x": 1.8181254267674478, - "y": 3.3475583791716352, - "heading": -0.496152534626129, - "angularVelocity": 0.8694389594781329, - "velocityX": 1.3445932752284602, - "velocityY": -1.4805637085793808, - "timestamp": 0.8115617017972563 - }, - { - "x": 1.9545279776008493, - "y": 3.1973622788021268, - "heading": -0.4079812901328632, - "angularVelocity": 0.8691513588973434, - "velocityX": 1.3445932752255727, - "velocityY": -1.480563708582003, - "timestamp": 0.9130069145219133 - }, - { - "x": 2.0909305284339355, - "y": 3.0471661784323323, - "heading": -0.32051597252506797, - "angularVelocity": 0.8621926580724313, - "velocityX": 1.344593275222464, - "velocityY": -1.4805637085848262, - "timestamp": 1.0144521272465703 - }, - { - "x": 2.227333079266667, - "y": 2.896970078062216, - "heading": -0.234818517184951, - "angularVelocity": 0.8447658892757932, - "velocityX": 1.3445932752189704, - "velocityY": -1.4805637085879988, - "timestamp": 1.1158973399712273 - }, - { - "x": 2.3637356300989167, - "y": 2.7467739776916615, - "heading": -0.15295154788429333, - "angularVelocity": 0.8070067290692294, - "velocityX": 1.3445932752142213, - "velocityY": -1.480563708592312, - "timestamp": 1.2173425526958843 - }, - { - "x": 2.5001381809305556, - "y": 2.596577877320553, - "heading": -0.07915862654379939, - "angularVelocity": 0.7274164976200789, - "velocityX": 1.3445932752082013, - "velocityY": -1.4805637085977792, - "timestamp": 1.3187877654205413 - }, - { - "x": 2.6365407317614795, - "y": 2.4463817769487943, - "heading": -0.02222133180148921, - "angularVelocity": 0.5612615244531013, - "velocityX": 1.3445932752011478, - "velocityY": -1.480563708604185, - "timestamp": 1.4202329781451983 - }, - { - "x": 2.7729432825902887, - "y": 2.296185676575095, - "heading": 1.9540240946006604e-7, - "angularVelocity": 0.21904954021056072, - "velocityX": 1.3445932751803078, - "velocityY": -1.480563708623314, - "timestamp": 1.5216781908698553 - }, - { - "x": 2.8538730144500737, - "y": 2.207072019577026, - "heading": -5.5771741386668314e-18, - "angularVelocity": -0.0000019261866008177493, - "velocityX": 0.7977678757443631, - "velocityY": -0.8784412256095482, - "timestamp": 1.6231234035945123 + "x": 0.8196446190430545, + "y": 4.447009398972592, + "heading": -0.9928952805674836, + "angularVelocity": 5.81882889925494e-8, + "velocityX": 0.3671091090870526, + "velocityY": -0.4042325913933224, + "timestamp": 0.10151417938738039 + }, + { + "x": 0.8941781769785785, + "y": 4.364938721552028, + "heading": -0.99289526724749, + "angularVelocity": 1.3121372132974804e-7, + "velocityX": 0.734218198731366, + "velocityY": -0.8084651613775955, + "timestamp": 0.20302835877476078 + }, + { + "x": 1.0059785069740759, + "y": 4.241832713027579, + "heading": -0.9928952419960708, + "angularVelocity": 2.4874881077364186e-7, + "velocityX": 1.1013272300492336, + "velocityY": -1.2126976671366712, + "timestamp": 0.30454253816214116 + }, + { + "x": 1.12199949551598, + "y": 4.114079236762414, + "heading": -0.9645969367606738, + "angularVelocity": 0.27876209418441944, + "velocityX": 1.1429042646264433, + "velocityY": -1.258479131054583, + "timestamp": 0.40605671754952155 + }, + { + "x": 1.23802048405803, + "y": 3.986325760497411, + "heading": -0.9142950585420234, + "angularVelocity": 0.4955157843191217, + "velocityX": 1.1429042646278826, + "velocityY": -1.2584791310529981, + "timestamp": 0.5075708969369019 + }, + { + "x": 1.3540414726000847, + "y": 3.8585722842324115, + "heading": -0.8476346267532343, + "angularVelocity": 0.6566612880194648, + "velocityX": 1.1429042646279266, + "velocityY": -1.2584791310529604, + "timestamp": 0.6090850763242823 + }, + { + "x": 1.4700624611421922, + "y": 3.73081880796746, + "heading": -0.7691366490852417, + "angularVelocity": 0.7732710655955318, + "velocityX": 1.142904264628449, + "velocityY": -1.2584791310524879, + "timestamp": 0.7105992557116627 + }, + { + "x": 1.5860834496844116, + "y": 3.60306533170261, + "heading": -0.6824653897720189, + "angularVelocity": 0.853784760278754, + "velocityX": 1.142904264629548, + "velocityY": -1.2584791310514911, + "timestamp": 0.8121134350990431 + }, + { + "x": 1.7021044382267643, + "y": 3.475311855437881, + "heading": -0.5906727010032288, + "angularVelocity": 0.9042351455004396, + "velocityX": 1.142904264630865, + "velocityY": -1.2584791310502963, + "timestamp": 0.9136276144864235 + }, + { + "x": 1.8181254267692102, + "y": 3.3475583791732366, + "heading": -0.4964175576320614, + "angularVelocity": 0.9284923932783603, + "velocityX": 1.1429042646317817, + "velocityY": -1.2584791310494643, + "timestamp": 1.0151417938738039 + }, + { + "x": 1.9341464153116614, + "y": 3.219804902908597, + "heading": -0.40216661154016314, + "angularVelocity": 0.9284510465493182, + "velocityX": 1.1429042646318321, + "velocityY": -1.2584791310494186, + "timestamp": 1.1166559732611843 + }, + { + "x": 2.0501674038540294, + "y": 3.0920514266438817, + "heading": -0.3103850714051686, + "angularVelocity": 0.904125322085988, + "velocityX": 1.142904264631013, + "velocityY": -1.2584791310501617, + "timestamp": 1.2181701526485647 + }, + { + "x": 2.1661883923962715, + "y": 2.964297950379052, + "heading": -0.22372826223723585, + "angularVelocity": 0.8536424142008245, + "velocityX": 1.1429042646297711, + "velocityY": -1.2584791310512888, + "timestamp": 1.319684332035945 + }, + { + "x": 2.2822093809384034, + "y": 2.836544474114123, + "heading": -0.1452438497360607, + "angularVelocity": 0.773137437299473, + "velocityX": 1.1429042646286902, + "velocityY": -1.2584791310522687, + "timestamp": 1.4211985114233254 + }, + { + "x": 2.3982303694804767, + "y": 2.7087909978491407, + "heading": -0.07859314585229263, + "angularVelocity": 0.6565654599777103, + "velocityX": 1.1429042646281113, + "velocityY": -1.2584791310527925, + "timestamp": 1.5227126908107058 + }, + { + "x": 2.5142513580225363, + "y": 2.5810375215841455, + "heading": -0.02829642791844555, + "angularVelocity": 0.4954649511760642, + "velocityX": 1.14290426462797, + "velocityY": -1.2584791310529186, + "timestamp": 1.6242268701980862 + }, + { + "x": 2.6302723465645417, + "y": 2.453284045319074, + "heading": -4.448219856121038e-8, + "angularVelocity": 0.27874316284732203, + "velocityX": 1.1429042646274454, + "velocityY": -1.2584791310536732, + "timestamp": 1.7257410495854666 + }, + { + "x": 2.742072676559976, + "y": 2.330178036794566, + "heading": -1.922893514266496e-8, + "angularVelocity": 2.4876697841719244e-7, + "velocityX": 1.1013272300486043, + "velocityY": -1.2126976671372425, + "timestamp": 1.827255228972847 + }, + { + "x": 2.8166062344954708, + "y": 2.2481073593739755, + "heading": -5.9076026803677414e-9, + "angularVelocity": 1.3122691044422866e-7, + "velocityX": 0.7342181987310801, + "velocityY": -0.8084651613778552, + "timestamp": 1.9287694083602274 }, { "x": 2.8538730144500732, "y": 2.2070720195770264, - "heading": -2.725395356358997e-18, - "angularVelocity": 1.2458293783490516e-18, - "velocityX": 3.57553609530669e-17, - "velocityY": -3.937144509458571e-17, - "timestamp": 1.7245686163191694 + "heading": 7.504980978671108e-18, + "angularVelocity": 5.819511377556866e-8, + "velocityX": 0.3671091090869381, + "velocityY": -0.40423259139342627, + "timestamp": 2.0302835877476078 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": 3.684608436293488e-18, + "angularVelocity": -1.3374320571761547e-18, + "velocityX": 2.8725878976324764e-18, + "velocityY": -3.1629865336005647e-18, + "timestamp": 2.131797767134988 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.traj b/src/main/deploy/choreo/2 piece right.traj index 489c053..c7a765a 100644 --- a/src/main/deploy/choreo/2 piece right.traj +++ b/src/main/deploy/choreo/2 piece right.traj @@ -1,400 +1,490 @@ { "samples": [ { - "x": 0.7823778390884399, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 3.4836656833317357e-19, - "velocityX": 5.792784763551635e-18, - "velocityY": -1.1468055635760044e-18, + "angularVelocity": -2.7495232446215487e-17, + "velocityX": -5.069420583996102e-17, + "velocityY": 1.192104631717783e-17, "timestamp": 0 }, { - "x": 0.848441149203722, - "y": 4.472500429335688, - "heading": -0.9928952478971514, - "angularVelocity": 5.064569480290326e-7, - "velocityX": 0.8673046782767408, - "velocityY": -0.20407170438519942, - "timestamp": 0.0761708218230117 - }, - { - "x": 0.9805677659026901, - "y": 4.4414118103682325, - "heading": -0.9928951486952988, - "angularVelocity": 0.0000013023602772620627, - "velocityX": 1.7346093101893227, - "velocityY": -0.40814341008019434, - "timestamp": 0.1523416436460234 - }, - { - "x": 1.1288597388405326, - "y": 4.406519593265519, - "heading": -0.9209894371787697, - "angularVelocity": 0.9440059828106777, - "velocityX": 1.9468343571559381, - "velocityY": -0.4580785170440767, - "timestamp": 0.2285124654690351 - }, - { - "x": 1.2771517113083277, - "y": 4.371627374165104, - "heading": -0.8000545061710758, - "angularVelocity": 1.587680533219069, - "velocityX": 1.9468343509849748, - "velocityY": -0.4580785432706628, - "timestamp": 0.3046832872920468 - }, - { - "x": 1.4254436833762532, - "y": 4.336735153365243, - "heading": -0.6538028446856889, - "angularVelocity": 1.9200483595308036, - "velocityX": 1.9468343457353292, - "velocityY": -0.45807856558166105, - "timestamp": 0.3808541091150585 - }, - { - "x": 1.57373565515521, - "y": 4.301842931337266, - "heading": -0.4970386957469823, - "angularVelocity": 2.058060359424233, - "velocityX": 1.9468343419416414, - "velocityY": -0.458078581704838, - "timestamp": 0.45702493093807023 - }, - { - "x": 1.7220276265458214, - "y": 4.26695070765882, - "heading": -0.3400744666794648, - "angularVelocity": 2.060687088715348, - "velocityX": 1.946834336843289, - "velocityY": -0.4580786033728385, - "timestamp": 0.5331957527610819 - }, - { - "x": 1.8703195977756357, - "y": 4.232058483296987, - "heading": -0.19340046314195958, - "angularVelocity": 1.9255930292876797, - "velocityX": 1.9468343347322845, - "velocityY": -0.45807861234460967, - "timestamp": 0.6093665745840936 - }, - { - "x": 2.0186115688161, - "y": 4.197166258130416, - "heading": -0.0721222371749164, - "angularVelocity": 1.592187442178866, - "velocityX": 1.946834332246427, - "velocityY": -0.45807862290950535, - "timestamp": 0.6855373964071053 - }, - { - "x": 2.1669035397570235, - "y": 4.162274032540797, - "heading": -1.3790420971273228e-7, - "angularVelocity": 0.946846804912877, - "velocityX": 1.9468343309396154, - "velocityY": -0.4580786284634526, - "timestamp": 0.761708218230117 - }, - { - "x": 2.299030158583636, - "y": 4.131185422616258, - "heading": -3.861728516505335e-8, - "angularVelocity": 0.0000013034771345390653, - "velocityX": 1.7346093381218648, - "velocityY": -0.4081432913612903, - "timestamp": 0.8378790400531287 - }, - { - "x": 2.365093469619751, + "x": 0.8155240483745178, + "y": 4.480245631878025, + "heading": -0.9928952427133843, + "angularVelocity": 5.500196348995766e-7, + "velocityX": 0.4166008785692591, + "velocityY": -0.09802372135573052, + "timestamp": 0.07956346467603777 + }, + { + "x": 0.8818164643488411, + "y": 4.46464741870627, + "heading": -0.9928951428202392, + "angularVelocity": 0.000001255529267463834, + "velocityX": 0.8332017244870482, + "velocityY": -0.19604743502871538, + "timestamp": 0.15912692935207554 + }, + { + "x": 0.9812550792231622, + "y": 4.441250101086857, + "heading": -0.9928949496672224, + "angularVelocity": 0.000002427691463657242, + "velocityX": 1.2498024725159502, + "velocityY": -0.2940711256682478, + "timestamp": 0.23869039402811332 + }, + { + "x": 1.1129174292533976, + "y": 4.410270729279662, + "heading": -0.988662240113345, + "angularVelocity": 0.05319916084717845, + "velocityX": 1.654809158529407, + "velocityY": -0.3893668021287818, + "timestamp": 0.31825385870415107 + }, + { + "x": 1.244579779283711, + "y": 4.379291357472773, + "heading": -0.9114902323862101, + "angularVelocity": 0.969942775132182, + "velocityX": 1.6548091585300193, + "velocityY": -0.3893668021248496, + "timestamp": 0.3978173233801888 + }, + { + "x": 1.3762421293125207, + "y": 4.3483119856594845, + "heading": -0.7738179037346776, + "angularVelocity": 1.7303460729208497, + "velocityX": 1.6548091585111195, + "velocityY": -0.3893668022052987, + "timestamp": 0.4773807880562266 + }, + { + "x": 1.507904479337117, + "y": 4.317332613828277, + "heading": -0.594124699134844, + "angularVelocity": 2.2584889349524437, + "velocityX": 1.6548091584581641, + "velocityY": -0.3893668024305141, + "timestamp": 0.5569442527322643 + }, + { + "x": 1.6395668293620107, + "y": 4.286353241998326, + "heading": -0.39886759130073074, + "angularVelocity": 2.4541051426819718, + "velocityX": 1.654809158461902, + "velocityY": -0.3893668024147043, + "timestamp": 0.6365077174083021 + }, + { + "x": 1.7712291793925206, + "y": 4.25537387019225, + "heading": -0.21914201405263553, + "angularVelocity": 2.258895813265647, + "velocityX": 1.6548091585324882, + "velocityY": -0.38936680211463054, + "timestamp": 0.71607118208434 + }, + { + "x": 1.9028915294236735, + "y": 4.2243944983889214, + "heading": -0.08144285462028061, + "angularVelocity": 1.7306832978144895, + "velocityX": 1.6548091585405724, + "velocityY": -0.3893668020801211, + "timestamp": 0.7956346467603778 + }, + { + "x": 2.034553879453989, + "y": 4.1934151265820425, + "heading": -0.004251710722878646, + "angularVelocity": 0.9701832896755161, + "velocityX": 1.6548091585300484, + "velocityY": -0.38936680212472563, + "timestamp": 0.8751981114364156 + }, + { + "x": 2.166216229487868, + "y": 4.162435754790337, + "heading": -3.367292673298042e-7, + "angularVelocity": 0.053433746406308845, + "velocityX": 1.654809158575206, + "velocityY": -0.3893668019341025, + "timestamp": 0.9547615761124534 + }, + { + "x": 2.2656548443604327, + "y": 4.139038437163738, + "heading": -1.4361728442139784e-7, + "angularVelocity": 0.000002427175717358418, + "velocityX": 1.2498024724938752, + "velocityY": -0.2940711257585697, + "timestamp": 1.0343250407884912 + }, + { + "x": 2.331947260333981, + "y": 4.123440223988747, + "heading": -4.37491568242842e-8, + "angularVelocity": 0.0000012552148287255357, + "velocityX": 0.8332017244772982, + "velocityY": -0.1960474350693968, + "timestamp": 1.1138885054645289 + }, + { + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -2.2763209665573703e-18, - "angularVelocity": 5.069826509442209e-7, - "velocityX": 0.8673046903657866, - "velocityY": -0.2040716530068378, - "timestamp": 0.9140498618761405 + "heading": 1.4952923019590338e-16, + "angularVelocity": 5.498706671572415e-7, + "velocityX": 0.41660087856538297, + "velocityY": -0.09802372137198472, + "timestamp": 1.1934519701405666 }, { - "x": 2.365093469619751, + "x": 2.3650934696197505, "y": 4.115641117095947, - "heading": -1.5143047799084299e-18, - "angularVelocity": 5.84054737228804e-20, - "velocityX": -8.219525464434245e-19, - "velocityY": 2.1312698857170775e-19, - "timestamp": 0.9902206836991522 - }, - { - "x": 2.299030158572247, - "y": 4.131185422567857, - "heading": -3.8824131952933046e-8, - "angularVelocity": -5.096982155619529e-7, - "velocityX": -0.867304690515307, - "velocityY": 0.20407165237139407, - "timestamp": 1.0663915055221642 - }, - { - "x": 2.166903539754318, - "y": 4.162274032529242, - "heading": -1.3861601297880188e-7, - "angularVelocity": -0.000001310106398423391, - "velocityX": -1.7346093380078471, - "velocityY": 0.408143291845037, - "timestamp": 1.1425623273451764 - }, - { - "x": 2.018611568604941, - "y": 4.197166257232936, - "heading": -0.0722676107720337, - "angularVelocity": -0.948755316359051, - "velocityX": -1.9468343336762697, - "velocityY": 0.4580786168326712, - "timestamp": 1.2187331491681885 - }, - { - "x": 1.8703195973960691, - "y": 4.2320584816837785, - "heading": -0.19373209136885944, - "angularVelocity": -1.5946326649731632, - "velocityX": -1.946834334457335, - "velocityY": 0.458078613513146, - "timestamp": 1.2949039709912007 - }, - { - "x": 1.7220276260831988, - "y": 4.2669507056926275, - "heading": -0.34047859723492613, - "angularVelocity": -1.9265448678897275, - "velocityX": -1.9468343358226654, - "velocityY": 0.45807860771049036, - "timestamp": 1.3710747928142129 - }, - { - "x": 1.5737356547013517, - "y": 4.301842929408325, - "heading": -0.4972933443760097, - "angularVelocity": -2.058724632188595, - "velocityX": -1.946834336728219, - "velocityY": 0.4580786038618867, - "timestamp": 1.447245614637225 - }, - { - "x": 1.4254436831497614, - "y": 4.336735152402613, - "heading": -0.6538038889287485, - "angularVelocity": -2.0547309429902363, - "velocityX": -1.9468343389566751, - "velocityY": 0.458078594390947, - "timestamp": 1.5234164364602372 - }, - { - "x": 1.2771517114677216, - "y": 4.371627374842491, - "heading": -0.799921474135194, - "angularVelocity": -1.9182881543008596, - "velocityX": -1.946834340669268, - "velocityY": 0.45807858711242555, - "timestamp": 1.5995872582832493 - }, - { - "x": 1.1288597396330466, - "y": 4.406519596633671, - "heading": -0.9209030529092569, - "angularVelocity": -1.5882929431321022, - "velocityX": -1.9468343426731174, - "velocityY": 0.4580785785960651, - "timestamp": 1.6757580801062615 - }, - { - "x": 0.9805677676343831, - "y": 4.4414118177279, - "heading": -0.9928951480324381, - "angularVelocity": -0.945140060198636, - "velocityX": -1.946834344826023, - "velocityY": 0.45807856944623543, - "timestamp": 1.7519289019292736 - }, - { - "x": 0.8484411497365192, - "y": 4.472500431600075, - "heading": -0.9928952477030265, - "angularVelocity": -0.0000013085140236118818, - "velocityX": -1.734609325928881, - "velocityY": 0.40814334318738743, - "timestamp": 1.8280997237522858 - }, - { - "x": 0.7823778390884398, + "heading": 9.94909152424984e-17, + "angularVelocity": -3.9263017083187414e-18, + "velocityX": 3.083103040454795e-18, + "velocityY": -7.248912091154562e-19, + "timestamp": 1.2730154348166043 + }, + { + "x": 2.331947260333817, + "y": 4.12344022398805, + "heading": -4.377032610264403e-8, + "angularVelocity": -5.501367379565397e-7, + "velocityX": -0.416600878567453, + "velocityY": 0.09802372136321691, + "timestamp": 1.3525788994926409 + }, + { + "x": 2.2656548443599256, + "y": 4.1390384371615845, + "heading": -1.4368564209179275e-7, + "angularVelocity": -0.0000012558079284124016, + "velocityX": -0.8332017244816303, + "velocityY": 0.1960474350510945, + "timestamp": 1.4321423641686775 + }, + { + "x": 2.1662162294867975, + "y": 4.162435754785825, + "heading": -3.368854431485356e-7, + "angularVelocity": -0.000002428279485036437, + "velocityX": -1.249802472500972, + "velocityY": 0.2940711257289318, + "timestamp": 1.511705828844714 + }, + { + "x": 2.034553879452772, + "y": 4.193415126576901, + "heading": -0.004253358762050876, + "angularVelocity": -0.05345445796038289, + "velocityX": -1.6548091585770712, + "velocityY": 0.38936680192617446, + "timestamp": 1.5912692935207506 + }, + { + "x": 1.902891529421956, + "y": 4.224394498381643, + "heading": -0.08145530813915991, + "angularVelocity": -0.9703190992427685, + "velocityX": -1.6548091585363642, + "velocityY": 0.38936680209788715, + "timestamp": 1.6708327581967872 + }, + { + "x": 1.7712291793910706, + "y": 4.255373870186102, + "heading": -0.21916938411480638, + "angularVelocity": -1.7308707776251606, + "velocityX": -1.6548091585372338, + "velocityY": 0.38936680209431446, + "timestamp": 1.7503962228728238 + }, + { + "x": 1.6395668293638401, + "y": 4.286353242006105, + "heading": -0.3988962080198311, + "angularVelocity": -2.2589114819767695, + "velocityX": -1.6548091584912985, + "velocityY": 0.38936680228969167, + "timestamp": 1.8299596875488604 + }, + { + "x": 1.50790447934162, + "y": 4.31733261384741, + "heading": -0.5941239976444426, + "angularVelocity": -2.4537366543360197, + "velocityX": -1.6548091584283233, + "velocityY": 0.38936680255742584, + "timestamp": 1.909523152224897 + }, + { + "x": 1.3762421293155438, + "y": 4.34831198567232, + "heading": -0.7738010431009228, + "angularVelocity": -2.2582858374199257, + "velocityX": -1.6548091584767906, + "velocityY": 0.38936680235136056, + "timestamp": 1.9890866169009336 + }, + { + "x": 1.2445797792853979, + "y": 4.379291357479921, + "heading": -0.911479619218358, + "angularVelocity": -1.7304245947137935, + "velocityX": -1.6548091585279405, + "velocityY": 0.38936680213381303, + "timestamp": 2.06865008157697 + }, + { + "x": 1.1129174292546509, + "y": 4.410270729284957, + "heading": -0.9886607024030146, + "angularVelocity": -0.9700568407721322, + "velocityX": -1.6548091585354978, + "velocityY": 0.38936680210156993, + "timestamp": 2.1482135462530065 + }, + { + "x": 0.9812550792241712, + "y": 4.441250101091103, + "heading": -0.9928949494741156, + "angularVelocity": -0.05321848525978111, + "velocityX": -1.6548091585325024, + "velocityY": 0.38936680211562363, + "timestamp": 2.227777010929043 + }, + { + "x": 0.8818164643493253, + "y": 4.46464741870832, + "heading": -0.992895142733805, + "angularVelocity": -0.0000024290322074090013, + "velocityX": -1.2498024725225658, + "velocityY": 0.2940711256406529, + "timestamp": 2.3073404756050797 + }, + { + "x": 0.8155240483746753, + "y": 4.4802456318786925, + "heading": -0.9928952426862623, + "angularVelocity": -0.0000012562747482591942, + "velocityX": -0.8332017244911675, + "velocityY": 0.19604743501134286, + "timestamp": 2.3869039402811163 + }, + { + "x": 0.78237783908844, "y": 4.488044738769531, - "heading": -0.9928952864743934, - "angularVelocity": -5.09005494945374e-7, - "velocityX": -0.8673046852715057, - "velocityY": 0.2040716746574537, - "timestamp": 1.904270545575298 + "heading": -0.9928952864743935, + "angularVelocity": -5.503605231480291e-7, + "velocityX": -0.41660087857124434, + "velocityY": 0.0980237213473347, + "timestamp": 2.466467404957153 }, { - "x": 0.7823778390884398, + "x": 0.78237783908844, "y": 4.488044738769531, "heading": -0.9928952864743934, - "angularVelocity": 1.8053835791622905e-18, - "velocityX": 3.8085009658841144e-17, - "velocityY": -4.441213712458509e-17, - "timestamp": 1.9804413673983101 - }, - { - "x": 0.8633075709529878, - "y": 4.398931081775789, - "heading": -0.9928954814598945, - "angularVelocity": -0.0000019220769098791852, - "velocityX": 0.7977678757913192, - "velocityY": -0.8784412255669002, - "timestamp": 2.0818865801229673 - }, - { - "x": 0.9997101217758085, - "y": 4.248734981396651, - "heading": -0.9705934228713969, - "angularVelocity": 0.21984338136319737, - "velocityX": 1.3445932751212721, - "velocityY": -1.480563708676927, - "timestamp": 2.1833317928476244 - }, - { - "x": 1.1361126726042652, - "y": 4.098538881022651, - "heading": -0.9135846602313676, - "angularVelocity": 0.5619660219429253, - "velocityX": 1.3445932751768286, - "velocityY": -1.4805637086262706, - "timestamp": 2.2847770055722814 - }, - { - "x": 1.2725152234348027, - "y": 3.948342780650542, - "heading": -0.8396943742506122, - "angularVelocity": 0.7283762732235443, - "velocityX": 1.3445932751973406, - "velocityY": -1.4805637086076424, - "timestamp": 2.3862222182969384 - }, - { - "x": 1.4089177742668983, - "y": 3.7981466802798476, - "heading": -0.7577094886902542, - "angularVelocity": 0.808169093034302, - "velocityX": 1.3445932752127017, - "velocityY": -1.4805637085936922, - "timestamp": 2.4876674310215954 - }, - { - "x": 1.5453203251000565, - "y": 3.6479505799101184, - "heading": -0.6718990052626181, - "angularVelocity": 0.8458800678997359, - "velocityX": 1.3445932752231742, - "velocityY": -1.4805637085841812, - "timestamp": 2.5891126437462524 - }, - { - "x": 1.6817228759337532, - "y": 3.4977544795408777, - "heading": -0.5843529548214927, - "angularVelocity": 0.8629884850135144, - "velocityX": 1.3445932752284844, - "velocityY": -1.4805637085793588, - "timestamp": 2.6905578564709094 - }, - { - "x": 1.8181254267674478, - "y": 3.3475583791716352, - "heading": -0.496152534626129, - "angularVelocity": 0.8694389594781329, - "velocityX": 1.3445932752284602, - "velocityY": -1.4805637085793808, - "timestamp": 2.7920030691955664 - }, - { - "x": 1.9545279776008493, - "y": 3.1973622788021268, - "heading": -0.4079812901328632, - "angularVelocity": 0.8691513588973434, - "velocityX": 1.3445932752255727, - "velocityY": -1.480563708582003, - "timestamp": 2.8934482819202234 - }, - { - "x": 2.0909305284339355, - "y": 3.0471661784323323, - "heading": -0.32051597252506797, - "angularVelocity": 0.8621926580724313, - "velocityX": 1.344593275222464, - "velocityY": -1.4805637085848262, - "timestamp": 2.9948934946448804 - }, - { - "x": 2.227333079266667, - "y": 2.896970078062216, - "heading": -0.234818517184951, - "angularVelocity": 0.8447658892757932, - "velocityX": 1.3445932752189704, - "velocityY": -1.4805637085879988, - "timestamp": 3.0963387073695374 - }, - { - "x": 2.3637356300989167, - "y": 2.7467739776916615, - "heading": -0.15295154788429333, - "angularVelocity": 0.8070067290692294, - "velocityX": 1.3445932752142213, - "velocityY": -1.480563708592312, - "timestamp": 3.1977839200941944 - }, - { - "x": 2.5001381809305556, - "y": 2.596577877320553, - "heading": -0.07915862654379939, - "angularVelocity": 0.7274164976200789, - "velocityX": 1.3445932752082013, - "velocityY": -1.4805637085977792, - "timestamp": 3.2992291328188514 - }, - { - "x": 2.6365407317614795, - "y": 2.4463817769487943, - "heading": -0.02222133180148921, - "angularVelocity": 0.5612615244531013, - "velocityX": 1.3445932752011478, - "velocityY": -1.480563708604185, - "timestamp": 3.4006743455435084 - }, - { - "x": 2.7729432825902887, - "y": 2.296185676575095, - "heading": 1.9540240946006604e-7, - "angularVelocity": 0.21904954021056072, - "velocityX": 1.3445932751803078, - "velocityY": -1.480563708623314, - "timestamp": 3.5021195582681655 - }, - { - "x": 2.8538730144500737, - "y": 2.207072019577026, - "heading": -5.5771741386668314e-18, - "angularVelocity": -0.0000019261866008177493, - "velocityX": 0.7977678757443631, - "velocityY": -0.8784412256095482, - "timestamp": 3.6035647709928225 + "angularVelocity": 2.916682100466704e-17, + "velocityX": 2.7675115242860868e-17, + "velocityY": -1.1179897514445951e-17, + "timestamp": 2.5460308696331895 + }, + { + "x": 0.8196446190430545, + "y": 4.447009398972592, + "heading": -0.9928952805674836, + "angularVelocity": 5.81882889925494e-8, + "velocityX": 0.3671091090870526, + "velocityY": -0.4042325913933224, + "timestamp": 2.64754504902057 + }, + { + "x": 0.8941781769785785, + "y": 4.364938721552028, + "heading": -0.99289526724749, + "angularVelocity": 1.3121372132974804e-7, + "velocityX": 0.734218198731366, + "velocityY": -0.8084651613775955, + "timestamp": 2.7490592284079503 + }, + { + "x": 1.0059785069740759, + "y": 4.241832713027579, + "heading": -0.9928952419960708, + "angularVelocity": 2.4874881077364186e-7, + "velocityX": 1.1013272300492336, + "velocityY": -1.2126976671366712, + "timestamp": 2.8505734077953306 + }, + { + "x": 1.12199949551598, + "y": 4.114079236762414, + "heading": -0.9645969367606738, + "angularVelocity": 0.27876209418441944, + "velocityX": 1.1429042646264433, + "velocityY": -1.258479131054583, + "timestamp": 2.952087587182711 + }, + { + "x": 1.23802048405803, + "y": 3.986325760497411, + "heading": -0.9142950585420234, + "angularVelocity": 0.4955157843191217, + "velocityX": 1.1429042646278826, + "velocityY": -1.2584791310529981, + "timestamp": 3.0536017665700914 + }, + { + "x": 1.3540414726000847, + "y": 3.8585722842324115, + "heading": -0.8476346267532343, + "angularVelocity": 0.6566612880194648, + "velocityX": 1.1429042646279266, + "velocityY": -1.2584791310529604, + "timestamp": 3.155115945957472 + }, + { + "x": 1.4700624611421922, + "y": 3.73081880796746, + "heading": -0.7691366490852417, + "angularVelocity": 0.7732710655955318, + "velocityX": 1.142904264628449, + "velocityY": -1.2584791310524879, + "timestamp": 3.256630125344852 + }, + { + "x": 1.5860834496844116, + "y": 3.60306533170261, + "heading": -0.6824653897720189, + "angularVelocity": 0.853784760278754, + "velocityX": 1.142904264629548, + "velocityY": -1.2584791310514911, + "timestamp": 3.3581443047322326 + }, + { + "x": 1.7021044382267643, + "y": 3.475311855437881, + "heading": -0.5906727010032288, + "angularVelocity": 0.9042351455004396, + "velocityX": 1.142904264630865, + "velocityY": -1.2584791310502963, + "timestamp": 3.459658484119613 + }, + { + "x": 1.8181254267692102, + "y": 3.3475583791732366, + "heading": -0.4964175576320614, + "angularVelocity": 0.9284923932783603, + "velocityX": 1.1429042646317817, + "velocityY": -1.2584791310494643, + "timestamp": 3.5611726635069934 + }, + { + "x": 1.9341464153116614, + "y": 3.219804902908597, + "heading": -0.40216661154016314, + "angularVelocity": 0.9284510465493182, + "velocityX": 1.1429042646318321, + "velocityY": -1.2584791310494186, + "timestamp": 3.6626868428943737 + }, + { + "x": 2.0501674038540294, + "y": 3.0920514266438817, + "heading": -0.3103850714051686, + "angularVelocity": 0.904125322085988, + "velocityX": 1.142904264631013, + "velocityY": -1.2584791310501617, + "timestamp": 3.764201022281754 + }, + { + "x": 2.1661883923962715, + "y": 2.964297950379052, + "heading": -0.22372826223723585, + "angularVelocity": 0.8536424142008245, + "velocityX": 1.1429042646297711, + "velocityY": -1.2584791310512888, + "timestamp": 3.8657152016691345 + }, + { + "x": 2.2822093809384034, + "y": 2.836544474114123, + "heading": -0.1452438497360607, + "angularVelocity": 0.773137437299473, + "velocityX": 1.1429042646286902, + "velocityY": -1.2584791310522687, + "timestamp": 3.967229381056515 + }, + { + "x": 2.3982303694804767, + "y": 2.7087909978491407, + "heading": -0.07859314585229263, + "angularVelocity": 0.6565654599777103, + "velocityX": 1.1429042646281113, + "velocityY": -1.2584791310527925, + "timestamp": 4.068743560443895 + }, + { + "x": 2.5142513580225363, + "y": 2.5810375215841455, + "heading": -0.02829642791844555, + "angularVelocity": 0.4954649511760642, + "velocityX": 1.14290426462797, + "velocityY": -1.2584791310529186, + "timestamp": 4.170257739831276 + }, + { + "x": 2.6302723465645417, + "y": 2.453284045319074, + "heading": -4.448219856121038e-8, + "angularVelocity": 0.27874316284732203, + "velocityX": 1.1429042646274454, + "velocityY": -1.2584791310536732, + "timestamp": 4.271771919218656 + }, + { + "x": 2.742072676559976, + "y": 2.330178036794566, + "heading": -1.922893514266496e-8, + "angularVelocity": 2.4876697841719244e-7, + "velocityX": 1.1013272300486043, + "velocityY": -1.2126976671372425, + "timestamp": 4.3732860986060365 + }, + { + "x": 2.8166062344954708, + "y": 2.2481073593739755, + "heading": -5.9076026803677414e-9, + "angularVelocity": 1.3122691044422866e-7, + "velocityX": 0.7342181987310801, + "velocityY": -0.8084651613778552, + "timestamp": 4.474800277993417 + }, + { + "x": 2.8538730144500732, + "y": 2.2070720195770264, + "heading": 7.504980978671108e-18, + "angularVelocity": 5.819511377556866e-8, + "velocityX": 0.3671091090869381, + "velocityY": -0.40423259139342627, + "timestamp": 4.576314457380797 }, { "x": 2.8538730144500732, "y": 2.2070720195770264, - "heading": -2.725395356358997e-18, - "angularVelocity": 1.2458293783490516e-18, - "velocityX": 3.57553609530669e-17, - "velocityY": -3.937144509458571e-17, - "timestamp": 3.7050099837174795 + "heading": 3.684608436293488e-18, + "angularVelocity": -1.3374320571761547e-18, + "velocityX": 2.8725878976324764e-18, + "velocityY": -3.1629865336005647e-18, + "timestamp": 4.677828636768178 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.1.traj b/src/main/deploy/choreo/2piece.1.traj index 78c44a6..961c129 100644 --- a/src/main/deploy/choreo/2piece.1.traj +++ b/src/main/deploy/choreo/2piece.1.traj @@ -1,67 +1,94 @@ { "samples": [ { - "x": 1.3126474618911723, + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": 2.630502167570374e-32, - "angularVelocity": 6.912068173828048e-33, - "velocityX": -4.041414755979078e-16, - "velocityY": 2.1900742120129258e-39, + "heading": -3.275463440583649e-29, + "angularVelocity": -1.5542228939894143e-29, + "velocityX": -1.0432080618288646e-15, + "velocityY": -2.0061080376600202e-17, "timestamp": 0 }, { - "x": 1.4761905162579165, - "y": 5.567087650299072, - "heading": 5.081856100994774e-22, - "angularVelocity": 4.2976983347588495e-21, - "velocityX": 1.383114259492122, - "velocityY": 7.266238948856172e-27, - "timestamp": 0.11824262021257878 + "x": 1.3669642138871982, + "y": 5.568132171401609, + "heading": -5.88414271395013e-19, + "angularVelocity": -5.855040371370758e-18, + "velocityX": 0.5404810945104359, + "velocityY": 0.01039355057127695, + "timestamp": 0.10049704337061591 }, { - "x": 1.7126757532986059, - "y": 5.567087650299072, - "heading": 5.225371158203822e-19, - "angularVelocity": 4.415167063895265e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.427644052438308e-27, - "timestamp": 0.23648524042515756 + "x": 1.4755977148071626, + "y": 5.570221213547605, + "heading": -1.2948347856012382e-18, + "angularVelocity": -7.029266314478605e-18, + "velocityX": 1.080962158452135, + "velocityY": 0.020787100554711394, + "timestamp": 0.20099408674123181 }, { - "x": 1.9491609900561957, - "y": 5.567087650299072, - "heading": 2.6577036541275914e-19, - "angularVelocity": -2.1718062327712947e-18, - "velocityX": 1.9999999681274145, - "velocityY": 3.4263757854502784e-27, - "timestamp": 0.3547278606377363 + "x": 1.6385479553988989, + "y": 5.57335477655914, + "heading": -2.2094750613970746e-18, + "angularVelocity": -9.101165621984674e-18, + "velocityX": 1.6214431303297006, + "velocityY": 0.03118064876773541, + "timestamp": 0.3014911301118477 }, { - "x": 2.1856462270968846, - "y": 5.567087650299072, - "heading": -1.0906583480085762e-21, - "angularVelocity": -2.2568820046674306e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.421212896814989e-27, - "timestamp": 0.4729704808503151 + "x": 1.8194091916084238, + "y": 5.5768327713012695, + "heading": -7.770058287246279e-19, + "angularVelocity": 1.4253844128634057e-17, + "velocityX": 1.7996672354084517, + "velocityY": 0.03460793099457427, + "timestamp": 0.40198817348246363 }, { - "x": 2.349189281463629, - "y": 5.567087650299072, - "heading": -3.2298015655990765e-32, - "angularVelocity": 9.223475207094912e-21, - "velocityX": 1.383114259492122, - "velocityY": -1.754147302356677e-26, - "timestamp": 0.5912131010628939 + "x": 2.000270427817948, + "y": 5.580310766043398, + "heading": 1.3008360366019906e-18, + "angularVelocity": 2.0675650427630753e-17, + "velocityX": 1.7996672354084513, + "velocityY": 0.03460793099457139, + "timestamp": 0.5024852168530796 }, { - "x": 2.349189281463627, - "y": 5.567087650299072, - "heading": -3.648621613916173e-33, - "angularVelocity": 1.9825798352488026e-32, - "velocityX": -3.5425947355709975e-16, - "velocityY": -9.477719520863627e-38, - "timestamp": 0.7094557212754727 + "x": 2.163220668409685, + "y": 5.583444329054934, + "heading": 7.353018561899664e-19, + "angularVelocity": -5.62737100639049e-18, + "velocityX": 1.6214431303297023, + "velocityY": 0.031180648767737112, + "timestamp": 0.6029822602236955 + }, + { + "x": 2.2718541693296492, + "y": 5.58553337120093, + "heading": 2.1533805611247358e-19, + "angularVelocity": -5.173921101276726e-18, + "velocityX": 1.080962158452136, + "velocityY": 0.02078710055471218, + "timestamp": 0.7034793035943114 + }, + { + "x": 2.3261709213256676, + "y": 5.586577892303466, + "heading": 1.773087403019019e-28, + "angularVelocity": -2.142730137028445e-18, + "velocityX": 0.5404810945104372, + "velocityY": 0.01039355057127728, + "timestamp": 0.8039763469649274 + }, + { + "x": 2.326170921325673, + "y": 5.586577892303467, + "heading": 1.4827806105172361e-28, + "angularVelocity": 3.750914457354201e-29, + "velocityX": 2.67021227320413e-16, + "velocityY": 5.134872532614327e-18, + "timestamp": 0.9044733903355433 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.2.traj b/src/main/deploy/choreo/2piece.2.traj index 1aeff89..5c4b662 100644 --- a/src/main/deploy/choreo/2piece.2.traj +++ b/src/main/deploy/choreo/2piece.2.traj @@ -1,67 +1,94 @@ { "samples": [ { - "x": 2.349189281463627, - "y": 5.567087650299072, - "heading": -3.648621613916173e-33, - "angularVelocity": 1.9825798352488026e-32, - "velocityX": -3.5425947355709975e-16, - "velocityY": -9.477719520863627e-38, + "x": 2.326170921325673, + "y": 5.586577892303467, + "heading": 1.4827806105172361e-28, + "angularVelocity": 3.750914457354201e-29, + "velocityX": 2.67021227320413e-16, + "velocityY": 5.134872532614327e-18, "timestamp": 0 }, { - "x": 2.185646227096883, - "y": 5.567087650299072, - "heading": 3.738718946616064e-21, - "angularVelocity": 3.1617850562356095e-20, - "velocityX": -1.3831142594921222, - "velocityY": 5.863050032870948e-30, - "timestamp": 0.1182426202125787 + "x": 2.271854169329654, + "y": 5.585533371200933, + "heading": 1.0512311117475804e-18, + "angularVelocity": 1.046031869151828e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571247395, + "timestamp": 0.10049704337061582 }, { - "x": 1.9491609900561935, - "y": 5.567087650299072, - "heading": -1.3594265469642462e-18, - "angularVelocity": -1.1527879544319905e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.470823667011192e-30, - "timestamp": 0.2364852404251574 + "x": 2.1632206684096897, + "y": 5.5834443290549425, + "heading": 3.191325466769564e-18, + "angularVelocity": 2.1295097409897457e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554652216, + "timestamp": 0.20099408674123165 }, { - "x": 1.7126757532986037, - "y": 5.567087650299072, - "heading": -1.1949667812351735e-18, - "angularVelocity": 1.3904520102372167e-18, - "velocityX": -1.9999999681274145, - "velocityY": 8.47082157931736e-30, - "timestamp": 0.3547278606377361 + "x": 2.000270427817954, + "y": 5.580310766043416, + "heading": 6.075146753884222e-18, + "angularVelocity": 2.8695582894264616e-17, + "velocityX": -1.6214431303297012, + "velocityY": -0.031180648767646445, + "timestamp": 0.30149113011184747 }, { - "x": 1.4761905162579143, - "y": 5.567087650299072, - "heading": -5.013730755356182e-22, - "angularVelocity": 1.0101569711037546e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.47082075819828e-30, - "timestamp": 0.4729704808503149 + "x": 1.8194091916084287, + "y": 5.576832771301296, + "heading": -3.924520679097354e-19, + "angularVelocity": -6.435610856432199e-17, + "velocityX": -1.7996672354084535, + "velocityY": -0.03460793099447712, + "timestamp": 0.4019881734824633 }, { - "x": 1.3126474618911703, - "y": 5.567087650299072, - "heading": -4.6245267198088095e-32, - "angularVelocity": 4.2399724814725485e-21, - "velocityX": -1.3831142594921215, - "velocityY": 5.85686190815934e-30, - "timestamp": 0.5912131010628937 + "x": 1.638547955398904, + "y": 5.573354776559178, + "heading": -7.054275205144732e-18, + "angularVelocity": -6.628874683251403e-17, + "velocityX": -1.7996672354084533, + "velocityY": -0.034607930994474186, + "timestamp": 0.5024852168530791 + }, + { + "x": 1.475597714807168, + "y": 5.570221213547637, + "heading": -3.521961615863548e-18, + "angularVelocity": 3.514843204208217e-17, + "velocityX": -1.6214431303297017, + "velocityY": -0.031180648767786465, + "timestamp": 0.6029822602236949 + }, + { + "x": 1.3669642138872036, + "y": 5.568132171401634, + "heading": -1.0343608078918469e-18, + "angularVelocity": 2.4752974455228876e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554791237, + "timestamp": 0.7034793035943108 + }, + { + "x": 1.312647461891185, + "y": 5.5670876502990865, + "heading": -2.3633742772886086e-28, + "angularVelocity": 1.0292449890816985e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571385917, + "timestamp": 0.8039763469649266 }, { - "x": 1.3126474618911723, + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": -2.2656479665287125e-32, - "angularVelocity": 7.883926885959527e-33, - "velocityX": 3.7971136148997106e-16, - "velocityY": -9.967559307460824e-45, - "timestamp": 0.7094557212754725 + "heading": -1.1552046309606331e-28, + "angularVelocity": 5.267876884045709e-29, + "velocityX": 2.7782898002965743e-16, + "velocityY": -1.3818335759211866e-13, + "timestamp": 0.9044733903355424 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2piece.traj b/src/main/deploy/choreo/2piece.traj index bbcbc6f..fe27e7e 100644 --- a/src/main/deploy/choreo/2piece.traj +++ b/src/main/deploy/choreo/2piece.traj @@ -1,121 +1,175 @@ { "samples": [ { - "x": 1.3126474618911723, + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": 2.630502167570374e-32, - "angularVelocity": 6.912068173828048e-33, - "velocityX": -4.041414755979078e-16, - "velocityY": 2.1900742120129258e-39, + "heading": -3.275463440583649e-29, + "angularVelocity": -1.5542228939894143e-29, + "velocityX": -1.0432080618288646e-15, + "velocityY": -2.0061080376600202e-17, "timestamp": 0 }, { - "x": 1.4761905162579165, - "y": 5.567087650299072, - "heading": 5.081856100994774e-22, - "angularVelocity": 4.2976983347588495e-21, - "velocityX": 1.383114259492122, - "velocityY": 7.266238948856172e-27, - "timestamp": 0.11824262021257878 + "x": 1.3669642138871982, + "y": 5.568132171401609, + "heading": -5.88414271395013e-19, + "angularVelocity": -5.855040371370758e-18, + "velocityX": 0.5404810945104359, + "velocityY": 0.01039355057127695, + "timestamp": 0.10049704337061591 }, { - "x": 1.7126757532986059, - "y": 5.567087650299072, - "heading": 5.225371158203822e-19, - "angularVelocity": 4.415167063895265e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.427644052438308e-27, - "timestamp": 0.23648524042515756 + "x": 1.4755977148071626, + "y": 5.570221213547605, + "heading": -1.2948347856012382e-18, + "angularVelocity": -7.029266314478605e-18, + "velocityX": 1.080962158452135, + "velocityY": 0.020787100554711394, + "timestamp": 0.20099408674123181 }, { - "x": 1.9491609900561957, - "y": 5.567087650299072, - "heading": 2.6577036541275914e-19, - "angularVelocity": -2.1718062327712947e-18, - "velocityX": 1.9999999681274145, - "velocityY": 3.4263757854502784e-27, - "timestamp": 0.3547278606377363 + "x": 1.6385479553988989, + "y": 5.57335477655914, + "heading": -2.2094750613970746e-18, + "angularVelocity": -9.101165621984674e-18, + "velocityX": 1.6214431303297006, + "velocityY": 0.03118064876773541, + "timestamp": 0.3014911301118477 }, { - "x": 2.1856462270968846, - "y": 5.567087650299072, - "heading": -1.0906583480085762e-21, - "angularVelocity": -2.2568820046674306e-18, - "velocityX": 1.9999999705215028, - "velocityY": 3.421212896814989e-27, - "timestamp": 0.4729704808503151 + "x": 1.8194091916084238, + "y": 5.5768327713012695, + "heading": -7.770058287246279e-19, + "angularVelocity": 1.4253844128634057e-17, + "velocityX": 1.7996672354084517, + "velocityY": 0.03460793099457427, + "timestamp": 0.40198817348246363 }, { - "x": 2.349189281463629, - "y": 5.567087650299072, - "heading": -3.2298015655990765e-32, - "angularVelocity": 9.223475207094912e-21, - "velocityX": 1.383114259492122, - "velocityY": -1.754147302356677e-26, - "timestamp": 0.5912131010628939 + "x": 2.000270427817948, + "y": 5.580310766043398, + "heading": 1.3008360366019906e-18, + "angularVelocity": 2.0675650427630753e-17, + "velocityX": 1.7996672354084513, + "velocityY": 0.03460793099457139, + "timestamp": 0.5024852168530796 }, { - "x": 2.349189281463627, - "y": 5.567087650299072, - "heading": -3.648621613916173e-33, - "angularVelocity": 1.9825798352488026e-32, - "velocityX": -3.5425947355709975e-16, - "velocityY": -9.477719520863627e-38, - "timestamp": 0.7094557212754727 + "x": 2.163220668409685, + "y": 5.583444329054934, + "heading": 7.353018561899664e-19, + "angularVelocity": -5.62737100639049e-18, + "velocityX": 1.6214431303297023, + "velocityY": 0.031180648767737112, + "timestamp": 0.6029822602236955 }, { - "x": 2.185646227096883, - "y": 5.567087650299072, - "heading": 3.738718946616064e-21, - "angularVelocity": 3.1617850562356095e-20, - "velocityX": -1.3831142594921222, - "velocityY": 5.863050032870948e-30, - "timestamp": 0.8276983414880514 + "x": 2.2718541693296492, + "y": 5.58553337120093, + "heading": 2.1533805611247358e-19, + "angularVelocity": -5.173921101276726e-18, + "velocityX": 1.080962158452136, + "velocityY": 0.02078710055471218, + "timestamp": 0.7034793035943114 }, { - "x": 1.9491609900561935, - "y": 5.567087650299072, - "heading": -1.3594265469642462e-18, - "angularVelocity": -1.1527879544319905e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.470823667011192e-30, - "timestamp": 0.9459409617006301 + "x": 2.3261709213256676, + "y": 5.586577892303466, + "heading": 1.773087403019019e-28, + "angularVelocity": -2.142730137028445e-18, + "velocityX": 0.5404810945104372, + "velocityY": 0.01039355057127728, + "timestamp": 0.8039763469649274 }, { - "x": 1.7126757532986037, - "y": 5.567087650299072, - "heading": -1.1949667812351735e-18, - "angularVelocity": 1.3904520102372167e-18, - "velocityX": -1.9999999681274145, - "velocityY": 8.47082157931736e-30, - "timestamp": 1.0641835819132088 + "x": 2.326170921325673, + "y": 5.586577892303467, + "heading": 1.4827806105172361e-28, + "angularVelocity": 3.750914457354201e-29, + "velocityX": 2.67021227320413e-16, + "velocityY": 5.134872532614327e-18, + "timestamp": 0.9044733903355433 }, { - "x": 1.4761905162579143, - "y": 5.567087650299072, - "heading": -5.013730755356182e-22, - "angularVelocity": 1.0101569711037546e-17, - "velocityX": -1.9999999705215028, - "velocityY": 8.47082075819828e-30, - "timestamp": 1.1824262021257876 + "x": 2.271854169329654, + "y": 5.585533371200933, + "heading": 1.0512311117475804e-18, + "angularVelocity": 1.046031869151828e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571247395, + "timestamp": 1.0049704337061591 }, { - "x": 1.3126474618911703, - "y": 5.567087650299072, - "heading": -4.6245267198088095e-32, - "angularVelocity": 4.2399724814725485e-21, - "velocityX": -1.3831142594921215, - "velocityY": 5.85686190815934e-30, - "timestamp": 1.3006688223383664 + "x": 2.1632206684096897, + "y": 5.5834443290549425, + "heading": 3.191325466769564e-18, + "angularVelocity": 2.1295097409897457e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554652216, + "timestamp": 1.105467477076775 + }, + { + "x": 2.000270427817954, + "y": 5.580310766043416, + "heading": 6.075146753884222e-18, + "angularVelocity": 2.8695582894264616e-17, + "velocityX": -1.6214431303297012, + "velocityY": -0.031180648767646445, + "timestamp": 1.2059645204473908 + }, + { + "x": 1.8194091916084287, + "y": 5.576832771301296, + "heading": -3.924520679097354e-19, + "angularVelocity": -6.435610856432199e-17, + "velocityX": -1.7996672354084535, + "velocityY": -0.03460793099447712, + "timestamp": 1.3064615638180066 + }, + { + "x": 1.638547955398904, + "y": 5.573354776559178, + "heading": -7.054275205144732e-18, + "angularVelocity": -6.628874683251403e-17, + "velocityX": -1.7996672354084533, + "velocityY": -0.034607930994474186, + "timestamp": 1.4069586071886224 + }, + { + "x": 1.475597714807168, + "y": 5.570221213547637, + "heading": -3.521961615863548e-18, + "angularVelocity": 3.514843204208217e-17, + "velocityX": -1.6214431303297017, + "velocityY": -0.031180648767786465, + "timestamp": 1.5074556505592382 + }, + { + "x": 1.3669642138872036, + "y": 5.568132171401634, + "heading": -1.0343608078918469e-18, + "angularVelocity": 2.4752974455228876e-17, + "velocityX": -1.0809621584521356, + "velocityY": -0.020787100554791237, + "timestamp": 1.607952693929854 + }, + { + "x": 1.312647461891185, + "y": 5.5670876502990865, + "heading": -2.3633742772886086e-28, + "angularVelocity": 1.0292449890816985e-17, + "velocityX": -0.5404810945104367, + "velocityY": -0.010393550571385917, + "timestamp": 1.70844973730047 }, { - "x": 1.3126474618911723, + "x": 1.3126474618911796, "y": 5.567087650299072, - "heading": -2.2656479665287125e-32, - "angularVelocity": 7.883926885959527e-33, - "velocityX": 3.7971136148997106e-16, - "velocityY": -9.967559307460824e-45, - "timestamp": 1.4189114425509453 + "heading": -1.1552046309606331e-28, + "angularVelocity": 5.267876884045709e-29, + "velocityX": 2.7782898002965743e-16, + "velocityY": -1.3818335759211866e-13, + "timestamp": 1.8089467806710857 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.1.traj b/src/main/deploy/choreo/4 piece.1.traj new file mode 100644 index 0000000..e081b05 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.1.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 1.3126474618911605, + "y": 5.567087650299072, + "heading": 8.749948102163363e-30, + "angularVelocity": 4.377756859907261e-29, + "velocityX": 2.3339363502495176e-15, + "velocityY": 4.559945650024092e-30, + "timestamp": 0 + }, + { + "x": 1.3684657734338344, + "y": 5.567087650299072, + "heading": -3.350607690445875e-20, + "angularVelocity": -3.2891903642626456e-19, + "velocityX": 0.5479514970030248, + "velocityY": 1.0705616112850545e-15, + "timestamp": 0.10186724892254397 + }, + { + "x": 1.480102395391638, + "y": 5.567087650299072, + "heading": -6.684009989043216e-20, + "angularVelocity": -3.2723003929334973e-19, + "velocityX": 1.0959029829371527, + "velocityY": 2.141123201298504e-15, + "timestamp": 0.20373449784508793 + }, + { + "x": 1.647557324391959, + "y": 5.567087650299072, + "heading": 1.9081720907940924e-20, + "angularVelocity": 8.434685372104281e-19, + "velocityX": 1.6438544357633669, + "velocityY": 3.2116847274817474e-15, + "timestamp": 0.3056017467676319 + }, + { + "x": 1.830918371677413, + "y": 5.567087650299072, + "heading": -1.7359971745167246e-18, + "angularVelocity": -1.7229079155247835e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.516754499906575e-15, + "timestamp": 0.40746899569017586 + }, + { + "x": 2.014279418962867, + "y": 5.567087650299072, + "heading": 1.701473951779379e-18, + "angularVelocity": 3.374461529154692e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.5167545000213866e-15, + "timestamp": 0.5093362446127199 + }, + { + "x": 2.1817343479631877, + "y": 5.567087650299072, + "heading": 6.998133615959998e-19, + "angularVelocity": -9.83299931311397e-18, + "velocityX": 1.643854435763364, + "velocityY": 3.2116847272591396e-15, + "timestamp": 0.6112034935352638 + }, + { + "x": 2.2933709699209914, + "y": 5.567087650299072, + "heading": 1.9411941943855612e-19, + "angularVelocity": -4.964244608659548e-18, + "velocityX": 1.0959029829371498, + "velocityY": 2.1411232011515742e-15, + "timestamp": 0.7130707424578078 + }, + { + "x": 2.349189281463665, + "y": 5.567087650299072, + "heading": -9.06696023328258e-29, + "angularVelocity": -1.905611677680185e-18, + "velocityX": 0.5479514970030218, + "velocityY": 1.0705616112089172e-15, + "timestamp": 0.8149379913803517 + }, + { + "x": 2.3491892814636506, + "y": 5.567087650299072, + "heading": -8.631559872072984e-29, + "angularVelocity": -4.3321556253199767e-29, + "velocityX": -7.059727912830758e-16, + "velocityY": -1.2733006393905255e-26, + "timestamp": 0.9168052403028957 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.2.traj b/src/main/deploy/choreo/4 piece.2.traj new file mode 100644 index 0000000..13c10f2 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.2.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 2.3491892814636506, + "y": 5.567087650299072, + "heading": -8.631559872072984e-29, + "angularVelocity": -4.3321556253199767e-29, + "velocityX": -7.059727912830758e-16, + "velocityY": -1.2733006393905255e-26, + "timestamp": 0 + }, + { + "x": 2.293370969920977, + "y": 5.567087650299072, + "heading": 3.7613099065105144e-19, + "angularVelocity": 3.692364275833631e-18, + "velocityX": -0.5479514970030234, + "velocityY": 1.0308631110321649e-16, + "timestamp": 0.10186724892254406 + }, + { + "x": 2.181734347963174, + "y": 5.567087650299072, + "heading": 1.2011497534698325e-18, + "angularVelocity": 8.098959886028294e-18, + "velocityX": -1.0959029829371514, + "velocityY": 2.3798569412312996e-17, + "timestamp": 0.20373449784508812 + }, + { + "x": 2.0142794189628526, + "y": 5.567087650299072, + "heading": 2.4969607309895686e-18, + "angularVelocity": 1.2720584788941894e-17, + "velocityX": -1.6438544357633655, + "velocityY": -5.006988249789681e-16, + "timestamp": 0.3056017467676322 + }, + { + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 3.324496900460492e-18, + "angularVelocity": 8.123672494715722e-18, + "velocityX": -1.7999999923909678, + "velocityY": -4.2863448770853046e-16, + "timestamp": 0.40746899569017625 + }, + { + "x": 1.6475573243919444, + "y": 5.567087650299072, + "heading": 1.771361781319927e-18, + "angularVelocity": -1.5246658146525354e-17, + "velocityX": -1.7999999923909678, + "velocityY": -4.728353666252759e-16, + "timestamp": 0.5093362446127203 + }, + { + "x": 1.4801023953916232, + "y": 5.567087650299072, + "heading": 1.0009187336716733e-18, + "angularVelocity": -7.563206565555211e-18, + "velocityX": -1.6438544357633653, + "velocityY": -3.2195944429460503e-16, + "timestamp": 0.6112034935352644 + }, + { + "x": 1.36846577343382, + "y": 5.567087650299071, + "heading": 3.66285032042137e-19, + "angularVelocity": -6.230007259713378e-18, + "velocityX": -1.0959029829371512, + "velocityY": 1.541445309390221e-16, + "timestamp": 0.7130707424578084 + }, + { + "x": 1.3126474618911463, + "y": 5.567087650299071, + "heading": -4.902178913705171e-28, + "angularVelocity": -3.595709471600392e-18, + "velocityX": -0.5479514970030231, + "velocityY": 2.0586346500407271e-16, + "timestamp": 0.8149379913803525 + }, + { + "x": 1.31264746189116, + "y": 5.567087650299071, + "heading": -6.149899148755531e-28, + "angularVelocity": -4.636006760170492e-28, + "velocityX": -3.976778090883029e-16, + "velocityY": 8.683385912114236e-17, + "timestamp": 0.9168052403028966 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.3.traj b/src/main/deploy/choreo/4 piece.3.traj new file mode 100644 index 0000000..1774c7f --- /dev/null +++ b/src/main/deploy/choreo/4 piece.3.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 1.31264746189116, + "y": 5.567087650299071, + "heading": -6.149899148755531e-28, + "angularVelocity": -4.636006760170492e-28, + "velocityX": -3.976778090883029e-16, + "velocityY": 8.683385912114236e-17, + "timestamp": 0 + }, + { + "x": 1.3502677179953657, + "y": 5.618680150361126, + "heading": 4.532916044804573e-19, + "angularVelocity": 4.160489322353646e-18, + "velocityX": 0.34529356424961244, + "velocityY": 0.4735363360002019, + "timestamp": 0.10895151256572588 + }, + { + "x": 1.425508229517596, + "y": 5.721865149544201, + "heading": 1.747154185604359e-18, + "angularVelocity": 1.1875581645180033e-17, + "velocityX": 0.6905871222011823, + "velocityY": 0.9470726633632547, + "timestamp": 0.21790302513145177 + }, + { + "x": 1.5383689944041468, + "y": 5.876642645031848, + "heading": 5.645077352326899e-18, + "angularVelocity": 3.5776677866624406e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.42060896487578, + "timestamp": 0.3268545376971774 + }, + { + "x": 1.653914603238861, + "y": 6.035102141279364, + "heading": 8.608437484479335e-18, + "angularVelocity": 2.719888934006118e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259298, + "timestamp": 0.4358060502629031 + }, + { + "x": 1.7694602120735736, + "y": 6.193561637526878, + "heading": 9.873897307535619e-18, + "angularVelocity": 1.1614889913397002e-17, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 0.5447575628286287 + }, + { + "x": 1.8850058209082863, + "y": 6.352021133774392, + "heading": 8.890496485421955e-18, + "angularVelocity": -9.026041015666318e-18, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 0.6537090753943544 + }, + { + "x": 2.0005514297429987, + "y": 6.510480630021907, + "heading": 5.9513864400033404e-18, + "angularVelocity": -2.697631244937109e-17, + "velocityX": 1.0605232191247393, + "velocityY": 1.4544038216259076, + "timestamp": 0.7626605879600801 + }, + { + "x": 2.1160970385777134, + "y": 6.668940126269423, + "heading": 2.0004191174937195e-18, + "angularVelocity": -3.626353805716405e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259296, + "timestamp": 0.8716121005258057 + }, + { + "x": 2.228957803464264, + "y": 6.823717621757069, + "heading": -2.138334357056216e-18, + "angularVelocity": -3.798711350391465e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.4206089648757803, + "timestamp": 0.9805636130915314 + }, + { + "x": 2.3041983149864937, + "y": 6.926902620940145, + "heading": -2.805869618577932e-18, + "angularVelocity": -6.126902193701436e-18, + "velocityX": 0.6905871222011825, + "velocityY": 0.9470726633632547, + "timestamp": 1.089515125657257 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002199, + "heading": 1.401424278348935e-27, + "angularVelocity": 2.575337922063006e-17, + "velocityX": 0.34529356424961277, + "velocityY": 0.47353633600020184, + "timestamp": 1.1984666382229827 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002198, + "heading": 6.552737055754714e-28, + "angularVelocity": -4.91699230639919e-28, + "velocityX": -1.8586364396279522e-17, + "velocityY": -2.5489381044432514e-17, + "timestamp": 1.3074181507887084 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.4.traj b/src/main/deploy/choreo/4 piece.4.traj new file mode 100644 index 0000000..6c60e78 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.4.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 2.341818571090699, + "y": 6.978495121002198, + "heading": 6.552737055754714e-28, + "angularVelocity": -4.91699230639919e-28, + "velocityX": -1.8586364396279522e-17, + "velocityY": -2.5489381044432514e-17, + "timestamp": 0 + }, + { + "x": 2.304198314986494, + "y": 6.926902620940145, + "heading": -1.6258900448266429e-18, + "angularVelocity": -1.4923060820323447e-17, + "velocityX": -0.34529356424960833, + "velocityY": -0.47353633600020245, + "timestamp": 0.10895151256572522 + }, + { + "x": 2.2289578034642648, + "y": 6.823717621757068, + "heading": -2.7548328890398584e-18, + "angularVelocity": -1.03618831931977e-17, + "velocityX": -0.6905871222011744, + "velocityY": -0.9470726633632552, + "timestamp": 0.21790302513145043 + }, + { + "x": 2.1160970385777156, + "y": 6.668940126269423, + "heading": -1.3133552490827444e-18, + "angularVelocity": 1.3230450918999471e-17, + "velocityX": -1.0358806613030351, + "velocityY": -1.4206089648757798, + "timestamp": 0.32685453769717565 + }, + { + "x": 2.0005514297430027, + "y": 6.510480630021907, + "heading": -1.6506585170593292e-18, + "angularVelocity": -3.0959025742305048e-18, + "velocityX": -1.0605232191247447, + "velocityY": -1.4544038216259367, + "timestamp": 0.43580605026290087 + }, + { + "x": 1.8850058209082918, + "y": 6.352021133774392, + "heading": -2.0454310820150905e-18, + "angularVelocity": -3.6233784679199945e-18, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 0.5447575628286261 + }, + { + "x": 1.7694602120735807, + "y": 6.193561637526877, + "heading": -8.094833008781398e-19, + "angularVelocity": 1.1344016734492403e-17, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 0.6537090753943513 + }, + { + "x": 1.6539146032388696, + "y": 6.035102141279363, + "heading": 2.1576684653761504e-18, + "angularVelocity": 2.7233690448860427e-17, + "velocityX": -1.0605232191247282, + "velocityY": -1.4544038216259159, + "timestamp": 0.7626605879600765 + }, + { + "x": 1.5383689944041568, + "y": 5.876642645031847, + "heading": 4.338142671690236e-18, + "angularVelocity": 2.0013253198238136e-17, + "velocityX": -1.0605232191247451, + "velocityY": -1.4544038216259363, + "timestamp": 0.8716121005258017 + }, + { + "x": 1.4255082295176076, + "y": 5.721865149544201, + "heading": 3.587785895663659e-18, + "angularVelocity": -6.887070745442744e-18, + "velocityX": -1.0358806613030345, + "velocityY": -1.4206089648757805, + "timestamp": 0.980563613091527 + }, + { + "x": 1.3502677179953786, + "y": 5.618680150361125, + "heading": 1.1960395158046676e-18, + "angularVelocity": -2.1952392596859126e-17, + "velocityX": -0.690587122201174, + "velocityY": -0.9470726633632556, + "timestamp": 1.0895151256572522 + }, + { + "x": 1.3126474618911737, + "y": 5.567087650299071, + "heading": -2.151002138732551e-28, + "angularVelocity": -1.097772290683926e-17, + "velocityX": -0.34529356424960816, + "velocityY": -0.47353633600020256, + "timestamp": 1.1984666382229774 + }, + { + "x": 1.3126474618911739, + "y": 5.567087650299072, + "heading": -2.805587391661966e-28, + "angularVelocity": -2.5846482509897715e-28, + "velocityX": -3.90611366689139e-17, + "velocityY": -6.850675515620994e-17, + "timestamp": 1.3074181507887026 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.5.traj b/src/main/deploy/choreo/4 piece.5.traj new file mode 100644 index 0000000..b6cce18 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.5.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 1.3126474618911739, + "y": 5.567087650299072, + "heading": -2.805587391661966e-28, + "angularVelocity": -2.5846482509897715e-28, + "velocityX": -3.90611366689139e-17, + "velocityY": -6.850675515620994e-17, + "timestamp": 0 + }, + { + "x": 1.3536439878702002, + "y": 5.515302553738671, + "heading": 1.0651040188255802e-19, + "angularVelocity": 9.61200821593777e-19, + "velocityX": 0.36997226934362815, + "velocityY": -0.46733349314603306, + "timestamp": 0.11080972650128285 + }, + { + "x": 1.4356370390978233, + "y": 5.411732361540517, + "heading": -3.328225499624599e-18, + "angularVelocity": -3.099670048679532e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.9346669779656481, + "timestamp": 0.2216194530025657 + }, + { + "x": 1.5586266133876248, + "y": 5.256377076466403, + "heading": -1.0014608416167351e-17, + "angularVelocity": -6.03411192104104e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615278, + "timestamp": 0.33242917950384854 + }, + { + "x": 1.6824304588849308, + "y": 5.099993238067341, + "heading": -7.784211196275788e-18, + "angularVelocity": 2.012817187466661e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 0.4432389060051314 + }, + { + "x": 1.8062343043822369, + "y": 4.94360939966828, + "heading": -2.180390152426642e-18, + "angularVelocity": 5.057156281947364e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 0.5540486325064142 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": 3.356944437156282e-18, + "angularVelocity": 4.997155723864182e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 0.6648583590076971 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": 7.008255309374147e-18, + "angularVelocity": 3.2951176651010183e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805754, + "timestamp": 0.7756680855089799 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": 8.815234075857993e-18, + "angularVelocity": 1.6307041114184896e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 0.8864778120102628 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": 6.3728040622429035e-18, + "angularVelocity": -2.2041657280874785e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615276, + "timestamp": 0.9972875385115456 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 3.61542268471678e-18, + "angularVelocity": -2.4883929063649245e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.934666977965648, + "timestamp": 1.1080972650128285 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 9.086068070823674e-28, + "angularVelocity": -3.262730443555877e-17, + "velocityX": 0.3699722693436281, + "velocityY": -0.46733349314603295, + "timestamp": 1.2189069915141113 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 5.931881185987618e-28, + "angularVelocity": 2.148760273829923e-29, + "velocityX": -1.6344283195010131e-18, + "velocityY": 2.0645414970491578e-18, + "timestamp": 1.3297167180153942 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.6.traj b/src/main/deploy/choreo/4 piece.6.traj new file mode 100644 index 0000000..2c79b8d --- /dev/null +++ b/src/main/deploy/choreo/4 piece.6.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 5.931881185987618e-28, + "angularVelocity": 2.148760273829923e-29, + "velocityX": -1.6344283195010131e-18, + "velocityY": 2.0645414970491578e-18, + "timestamp": 0 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 2.5809854735534117e-18, + "angularVelocity": 2.3292048019313352e-17, + "velocityX": -0.36997226934362804, + "velocityY": 0.46733349314603323, + "timestamp": 0.11080972650128285 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": -3.635479866817947e-18, + "angularVelocity": -5.610035807757445e-17, + "velocityX": -0.7399445320955099, + "velocityY": 0.9346669779656486, + "timestamp": 0.2216194530025657 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": -8.293710197495525e-18, + "angularVelocity": -4.2038099666365453e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.402000437861529, + "timestamp": 0.33242917950384854 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": -1.2713469170987557e-17, + "angularVelocity": -3.988602002843467e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 0.4432389060051314 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": -1.4536509782910432e-17, + "angularVelocity": -1.6451990888019063e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 0.5540486325064142 + }, + { + "x": 1.806234304382237, + "y": 4.943609399668281, + "heading": -1.4741169168642963e-17, + "angularVelocity": -1.8469442384254893e-18, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 0.6648583590076971 + }, + { + "x": 1.682430458884931, + "y": 5.099993238067342, + "heading": -1.3603933385071938e-17, + "angularVelocity": 1.0262959923842215e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.4112825952805752, + "timestamp": 0.7756680855089799 + }, + { + "x": 1.558626613387625, + "y": 5.2563770764664035, + "heading": -1.2316142581173569e-17, + "angularVelocity": 1.162164046785376e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 0.8864778120102628 + }, + { + "x": 1.4356370390978235, + "y": 5.411732361540517, + "heading": -1.1146449974389838e-17, + "angularVelocity": 1.0555865817235836e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.4020004378615287, + "timestamp": 0.9972875385115456 + }, + { + "x": 1.3536439878702005, + "y": 5.515302553738671, + "heading": -4.770660310358341e-18, + "angularVelocity": 5.753817702370985e-17, + "velocityX": -0.7399445320955101, + "velocityY": 0.9346669779656486, + "timestamp": 1.1080972650128285 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -5.571608128785667e-28, + "angularVelocity": 4.305272160914999e-17, + "velocityX": -0.36997226934362815, + "velocityY": 0.4673334931460333, + "timestamp": 1.2189069915141113 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -2.752910621958426e-28, + "angularVelocity": 5.936929258081384e-29, + "velocityX": -2.690449803663919e-18, + "velocityY": 3.3984636410469572e-18, + "timestamp": 1.3297167180153942 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4 piece.traj b/src/main/deploy/choreo/4 piece.traj new file mode 100644 index 0000000..61b0665 --- /dev/null +++ b/src/main/deploy/choreo/4 piece.traj @@ -0,0 +1,607 @@ +{ + "samples": [ + { + "x": 1.3126474618911605, + "y": 5.567087650299072, + "heading": 8.749948102163363e-30, + "angularVelocity": 4.377756859907261e-29, + "velocityX": 2.3339363502495176e-15, + "velocityY": 4.559945650024092e-30, + "timestamp": 0 + }, + { + "x": 1.3684657734338344, + "y": 5.567087650299072, + "heading": -3.350607690445875e-20, + "angularVelocity": -3.2891903642626456e-19, + "velocityX": 0.5479514970030248, + "velocityY": 1.0705616112850545e-15, + "timestamp": 0.10186724892254397 + }, + { + "x": 1.480102395391638, + "y": 5.567087650299072, + "heading": -6.684009989043216e-20, + "angularVelocity": -3.2723003929334973e-19, + "velocityX": 1.0959029829371527, + "velocityY": 2.141123201298504e-15, + "timestamp": 0.20373449784508793 + }, + { + "x": 1.647557324391959, + "y": 5.567087650299072, + "heading": 1.9081720907940924e-20, + "angularVelocity": 8.434685372104281e-19, + "velocityX": 1.6438544357633669, + "velocityY": 3.2116847274817474e-15, + "timestamp": 0.3056017467676319 + }, + { + "x": 1.830918371677413, + "y": 5.567087650299072, + "heading": -1.7359971745167246e-18, + "angularVelocity": -1.7229079155247835e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.516754499906575e-15, + "timestamp": 0.40746899569017586 + }, + { + "x": 2.014279418962867, + "y": 5.567087650299072, + "heading": 1.701473951779379e-18, + "angularVelocity": 3.374461529154692e-17, + "velocityX": 1.7999999923909678, + "velocityY": 3.5167545000213866e-15, + "timestamp": 0.5093362446127199 + }, + { + "x": 2.1817343479631877, + "y": 5.567087650299072, + "heading": 6.998133615959998e-19, + "angularVelocity": -9.83299931311397e-18, + "velocityX": 1.643854435763364, + "velocityY": 3.2116847272591396e-15, + "timestamp": 0.6112034935352638 + }, + { + "x": 2.2933709699209914, + "y": 5.567087650299072, + "heading": 1.9411941943855612e-19, + "angularVelocity": -4.964244608659548e-18, + "velocityX": 1.0959029829371498, + "velocityY": 2.1411232011515742e-15, + "timestamp": 0.7130707424578078 + }, + { + "x": 2.349189281463665, + "y": 5.567087650299072, + "heading": -9.06696023328258e-29, + "angularVelocity": -1.905611677680185e-18, + "velocityX": 0.5479514970030218, + "velocityY": 1.0705616112089172e-15, + "timestamp": 0.8149379913803517 + }, + { + "x": 2.3491892814636506, + "y": 5.567087650299072, + "heading": -8.631559872072984e-29, + "angularVelocity": -4.3321556253199767e-29, + "velocityX": -7.059727912830758e-16, + "velocityY": -1.2733006393905255e-26, + "timestamp": 0.9168052403028957 + }, + { + "x": 2.293370969920977, + "y": 5.567087650299072, + "heading": 3.7613099065105144e-19, + "angularVelocity": 3.692364275833631e-18, + "velocityX": -0.5479514970030234, + "velocityY": 1.0308631110321649e-16, + "timestamp": 1.0186724892254397 + }, + { + "x": 2.181734347963174, + "y": 5.567087650299072, + "heading": 1.2011497534698325e-18, + "angularVelocity": 8.098959886028294e-18, + "velocityX": -1.0959029829371514, + "velocityY": 2.3798569412312996e-17, + "timestamp": 1.1205397381479838 + }, + { + "x": 2.0142794189628526, + "y": 5.567087650299072, + "heading": 2.4969607309895686e-18, + "angularVelocity": 1.2720584788941894e-17, + "velocityX": -1.6438544357633655, + "velocityY": -5.006988249789681e-16, + "timestamp": 1.2224069870705279 + }, + { + "x": 1.8309183716773985, + "y": 5.567087650299072, + "heading": 3.324496900460492e-18, + "angularVelocity": 8.123672494715722e-18, + "velocityX": -1.7999999923909678, + "velocityY": -4.2863448770853046e-16, + "timestamp": 1.324274235993072 + }, + { + "x": 1.6475573243919444, + "y": 5.567087650299072, + "heading": 1.771361781319927e-18, + "angularVelocity": -1.5246658146525354e-17, + "velocityX": -1.7999999923909678, + "velocityY": -4.728353666252759e-16, + "timestamp": 1.426141484915616 + }, + { + "x": 1.4801023953916232, + "y": 5.567087650299072, + "heading": 1.0009187336716733e-18, + "angularVelocity": -7.563206565555211e-18, + "velocityX": -1.6438544357633653, + "velocityY": -3.2195944429460503e-16, + "timestamp": 1.52800873383816 + }, + { + "x": 1.36846577343382, + "y": 5.567087650299071, + "heading": 3.66285032042137e-19, + "angularVelocity": -6.230007259713378e-18, + "velocityX": -1.0959029829371512, + "velocityY": 1.541445309390221e-16, + "timestamp": 1.6298759827607041 + }, + { + "x": 1.3126474618911463, + "y": 5.567087650299071, + "heading": -4.902178913705171e-28, + "angularVelocity": -3.595709471600392e-18, + "velocityX": -0.5479514970030231, + "velocityY": 2.0586346500407271e-16, + "timestamp": 1.7317432316832482 + }, + { + "x": 1.31264746189116, + "y": 5.567087650299071, + "heading": -6.149899148755531e-28, + "angularVelocity": -4.636006760170492e-28, + "velocityX": -3.976778090883029e-16, + "velocityY": 8.683385912114236e-17, + "timestamp": 1.8336104806057922 + }, + { + "x": 1.3502677179953657, + "y": 5.618680150361126, + "heading": 4.532916044804573e-19, + "angularVelocity": 4.160489322353646e-18, + "velocityX": 0.34529356424961244, + "velocityY": 0.4735363360002019, + "timestamp": 1.9425619931715181 + }, + { + "x": 1.425508229517596, + "y": 5.721865149544201, + "heading": 1.747154185604359e-18, + "angularVelocity": 1.1875581645180033e-17, + "velocityX": 0.6905871222011823, + "velocityY": 0.9470726633632547, + "timestamp": 2.051513505737244 + }, + { + "x": 1.5383689944041468, + "y": 5.876642645031848, + "heading": 5.645077352326899e-18, + "angularVelocity": 3.5776677866624406e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.42060896487578, + "timestamp": 2.1604650183029697 + }, + { + "x": 1.653914603238861, + "y": 6.035102141279364, + "heading": 8.608437484479335e-18, + "angularVelocity": 2.719888934006118e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259298, + "timestamp": 2.2694165308686953 + }, + { + "x": 1.7694602120735736, + "y": 6.193561637526878, + "heading": 9.873897307535619e-18, + "angularVelocity": 1.1614889913397002e-17, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 2.378368043434421 + }, + { + "x": 1.8850058209082863, + "y": 6.352021133774392, + "heading": 8.890496485421955e-18, + "angularVelocity": -9.026041015666318e-18, + "velocityX": 1.0605232191247391, + "velocityY": 1.4544038216259076, + "timestamp": 2.4873195560001466 + }, + { + "x": 2.0005514297429987, + "y": 6.510480630021907, + "heading": 5.9513864400033404e-18, + "angularVelocity": -2.697631244937109e-17, + "velocityX": 1.0605232191247393, + "velocityY": 1.4544038216259076, + "timestamp": 2.5962710685658723 + }, + { + "x": 2.1160970385777134, + "y": 6.668940126269423, + "heading": 2.0004191174937195e-18, + "angularVelocity": -3.626353805716405e-17, + "velocityX": 1.0605232191247542, + "velocityY": 1.4544038216259296, + "timestamp": 2.705222581131598 + }, + { + "x": 2.228957803464264, + "y": 6.823717621757069, + "heading": -2.138334357056216e-18, + "angularVelocity": -3.798711350391465e-17, + "velocityX": 1.0358806613030456, + "velocityY": 1.4206089648757803, + "timestamp": 2.8141740936973236 + }, + { + "x": 2.3041983149864937, + "y": 6.926902620940145, + "heading": -2.805869618577932e-18, + "angularVelocity": -6.126902193701436e-18, + "velocityX": 0.6905871222011825, + "velocityY": 0.9470726633632547, + "timestamp": 2.9231256062630493 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002199, + "heading": 1.401424278348935e-27, + "angularVelocity": 2.575337922063006e-17, + "velocityX": 0.34529356424961277, + "velocityY": 0.47353633600020184, + "timestamp": 3.032077118828775 + }, + { + "x": 2.341818571090699, + "y": 6.978495121002198, + "heading": 6.552737055754714e-28, + "angularVelocity": -4.91699230639919e-28, + "velocityX": -1.8586364396279522e-17, + "velocityY": -2.5489381044432514e-17, + "timestamp": 3.1410286313945006 + }, + { + "x": 2.304198314986494, + "y": 6.926902620940145, + "heading": -1.6258900448266429e-18, + "angularVelocity": -1.4923060820323447e-17, + "velocityX": -0.34529356424960833, + "velocityY": -0.47353633600020245, + "timestamp": 3.249980143960226 + }, + { + "x": 2.2289578034642648, + "y": 6.823717621757068, + "heading": -2.7548328890398584e-18, + "angularVelocity": -1.03618831931977e-17, + "velocityX": -0.6905871222011744, + "velocityY": -0.9470726633632552, + "timestamp": 3.358931656525951 + }, + { + "x": 2.1160970385777156, + "y": 6.668940126269423, + "heading": -1.3133552490827444e-18, + "angularVelocity": 1.3230450918999471e-17, + "velocityX": -1.0358806613030351, + "velocityY": -1.4206089648757798, + "timestamp": 3.4678831690916763 + }, + { + "x": 2.0005514297430027, + "y": 6.510480630021907, + "heading": -1.6506585170593292e-18, + "angularVelocity": -3.0959025742305048e-18, + "velocityX": -1.0605232191247447, + "velocityY": -1.4544038216259367, + "timestamp": 3.5768346816574015 + }, + { + "x": 1.8850058209082918, + "y": 6.352021133774392, + "heading": -2.0454310820150905e-18, + "angularVelocity": -3.6233784679199945e-18, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 3.6857861942231267 + }, + { + "x": 1.7694602120735807, + "y": 6.193561637526877, + "heading": -8.094833008781398e-19, + "angularVelocity": 1.1344016734492403e-17, + "velocityX": -1.0605232191247278, + "velocityY": -1.454403821625916, + "timestamp": 3.794737706788852 + }, + { + "x": 1.6539146032388696, + "y": 6.035102141279363, + "heading": 2.1576684653761504e-18, + "angularVelocity": 2.7233690448860427e-17, + "velocityX": -1.0605232191247282, + "velocityY": -1.4544038216259159, + "timestamp": 3.903689219354577 + }, + { + "x": 1.5383689944041568, + "y": 5.876642645031847, + "heading": 4.338142671690236e-18, + "angularVelocity": 2.0013253198238136e-17, + "velocityX": -1.0605232191247451, + "velocityY": -1.4544038216259363, + "timestamp": 4.012640731920302 + }, + { + "x": 1.4255082295176076, + "y": 5.721865149544201, + "heading": 3.587785895663659e-18, + "angularVelocity": -6.887070745442744e-18, + "velocityX": -1.0358806613030345, + "velocityY": -1.4206089648757805, + "timestamp": 4.121592244486028 + }, + { + "x": 1.3502677179953786, + "y": 5.618680150361125, + "heading": 1.1960395158046676e-18, + "angularVelocity": -2.1952392596859126e-17, + "velocityX": -0.690587122201174, + "velocityY": -0.9470726633632556, + "timestamp": 4.230543757051753 + }, + { + "x": 1.3126474618911737, + "y": 5.567087650299071, + "heading": -2.151002138732551e-28, + "angularVelocity": -1.097772290683926e-17, + "velocityX": -0.34529356424960816, + "velocityY": -0.47353633600020256, + "timestamp": 4.339495269617478 + }, + { + "x": 1.3126474618911739, + "y": 5.567087650299072, + "heading": -2.805587391661966e-28, + "angularVelocity": -2.5846482509897715e-28, + "velocityX": -3.90611366689139e-17, + "velocityY": -6.850675515620994e-17, + "timestamp": 4.448446782183203 + }, + { + "x": 1.3536439878702002, + "y": 5.515302553738671, + "heading": 1.0651040188255802e-19, + "angularVelocity": 9.61200821593777e-19, + "velocityX": 0.36997226934362815, + "velocityY": -0.46733349314603306, + "timestamp": 4.559256508684486 + }, + { + "x": 1.4356370390978233, + "y": 5.411732361540517, + "heading": -3.328225499624599e-18, + "angularVelocity": -3.099670048679532e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.9346669779656481, + "timestamp": 4.670066235185769 + }, + { + "x": 1.5586266133876248, + "y": 5.256377076466403, + "heading": -1.0014608416167351e-17, + "angularVelocity": -6.03411192104104e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615278, + "timestamp": 4.780875961687052 + }, + { + "x": 1.6824304588849308, + "y": 5.099993238067341, + "heading": -7.784211196275788e-18, + "angularVelocity": 2.012817187466661e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 4.891685688188335 + }, + { + "x": 1.8062343043822369, + "y": 4.94360939966828, + "heading": -2.180390152426642e-18, + "angularVelocity": 5.057156281947364e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 5.0024954146896174 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": 3.356944437156282e-18, + "angularVelocity": 4.997155723864182e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805752, + "timestamp": 5.1133051411909 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": 7.008255309374147e-18, + "angularVelocity": 3.2951176651010183e-17, + "velocityX": 1.1172651481625382, + "velocityY": -1.4112825952805754, + "timestamp": 5.224114867692183 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": 8.815234075857993e-18, + "angularVelocity": 1.6307041114184896e-17, + "velocityX": 1.1172651481625397, + "velocityY": -1.4112825952805765, + "timestamp": 5.334924594193466 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": 6.3728040622429035e-18, + "angularVelocity": -2.2041657280874785e-17, + "velocityX": 1.1099167751161065, + "velocityY": -1.4020004378615276, + "timestamp": 5.445734320694749 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 3.61542268471678e-18, + "angularVelocity": -2.4883929063649245e-17, + "velocityX": 0.7399445320955103, + "velocityY": -0.934666977965648, + "timestamp": 5.556544047196032 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 9.086068070823674e-28, + "angularVelocity": -3.262730443555877e-17, + "velocityX": 0.3699722693436281, + "velocityY": -0.46733349314603295, + "timestamp": 5.6673537736973145 + }, + { + "x": 2.4236249923706055, + "y": 4.163747310638428, + "heading": 5.931881185987618e-28, + "angularVelocity": 2.148760273829923e-29, + "velocityX": -1.6344283195010131e-18, + "velocityY": 2.0645414970491578e-18, + "timestamp": 5.778163500198597 + }, + { + "x": 2.382628466391579, + "y": 4.215532407198829, + "heading": 2.5809854735534117e-18, + "angularVelocity": 2.3292048019313352e-17, + "velocityX": -0.36997226934362804, + "velocityY": 0.46733349314603323, + "timestamp": 5.88897322669988 + }, + { + "x": 2.3006354151639563, + "y": 4.319102599396983, + "heading": -3.635479866817947e-18, + "angularVelocity": -5.610035807757445e-17, + "velocityX": -0.7399445320955099, + "velocityY": 0.9346669779656486, + "timestamp": 5.999782953201163 + }, + { + "x": 2.177645840874155, + "y": 4.4744578844710965, + "heading": -8.293710197495525e-18, + "angularVelocity": -4.2038099666365453e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.402000437861529, + "timestamp": 6.110592679702446 + }, + { + "x": 2.0538419953768488, + "y": 4.630841722870158, + "heading": -1.2713469170987557e-17, + "angularVelocity": -3.988602002843467e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 6.221402406203729 + }, + { + "x": 1.930038149879543, + "y": 4.787225561269219, + "heading": -1.4536509782910432e-17, + "angularVelocity": -1.6451990888019063e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 6.332212132705012 + }, + { + "x": 1.806234304382237, + "y": 4.943609399668281, + "heading": -1.4741169168642963e-17, + "angularVelocity": -1.8469442384254893e-18, + "velocityX": -1.1172651481625384, + "velocityY": 1.411282595280575, + "timestamp": 6.443021859206294 + }, + { + "x": 1.682430458884931, + "y": 5.099993238067342, + "heading": -1.3603933385071938e-17, + "angularVelocity": 1.0262959923842215e-17, + "velocityX": -1.1172651481625384, + "velocityY": 1.4112825952805752, + "timestamp": 6.553831585707577 + }, + { + "x": 1.558626613387625, + "y": 5.2563770764664035, + "heading": -1.2316142581173569e-17, + "angularVelocity": 1.162164046785376e-17, + "velocityX": -1.1172651481625395, + "velocityY": 1.4112825952805768, + "timestamp": 6.66464131220886 + }, + { + "x": 1.4356370390978235, + "y": 5.411732361540517, + "heading": -1.1146449974389838e-17, + "angularVelocity": 1.0555865817235836e-17, + "velocityX": -1.1099167751161059, + "velocityY": 1.4020004378615287, + "timestamp": 6.775451038710143 + }, + { + "x": 1.3536439878702005, + "y": 5.515302553738671, + "heading": -4.770660310358341e-18, + "angularVelocity": 5.753817702370985e-17, + "velocityX": -0.7399445320955101, + "velocityY": 0.9346669779656486, + "timestamp": 6.886260765211426 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -5.571608128785667e-28, + "angularVelocity": 4.305272160914999e-17, + "velocityX": -0.36997226934362815, + "velocityY": 0.4673334931460333, + "timestamp": 6.997070491712709 + }, + { + "x": 1.3126474618911743, + "y": 5.567087650299072, + "heading": -2.752910621958426e-28, + "angularVelocity": 5.936929258081384e-29, + "velocityX": -2.690449803663919e-18, + "velocityY": 3.3984636410469572e-18, + "timestamp": 7.1078802182139915 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.1.traj b/src/main/deploy/choreo/amp.1.traj index 89142ab..c544758 100644 --- a/src/main/deploy/choreo/amp.1.traj +++ b/src/main/deploy/choreo/amp.1.traj @@ -1,220 +1,256 @@ { "samples": [ { - "x": 0.7669039964675903, + "x": 0.7669039964675901, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 4.703190638042804e-18, - "velocityX": -1.208675917564465e-17, - "velocityY": -6.352810871367948e-18, + "angularVelocity": 1.0512088330044834e-17, + "velocityX": 1.2971420838221857e-16, + "velocityY": 6.851970292534496e-17, "timestamp": 0 }, { - "x": 0.8020378021280192, - "y": 6.657644636391979, - "heading": 1.007479841292087, - "angularVelocity": -3.1089087057766036e-7, - "velocityX": 0.6028069120245683, - "velocityY": 0.31846394498851993, - "timestamp": 0.05828368082645525 - }, - { - "x": 0.8723054048773754, - "y": 6.694767133707721, - "heading": 1.0074798016172224, - "angularVelocity": -6.807244191958373e-7, - "velocityX": 1.2056136769836088, - "velocityY": 0.6369278121994305, - "timestamp": 0.1165673616529105 - }, - { - "x": 0.9753734970192464, - "y": 6.749218186944615, - "heading": 0.9990535149100053, - "angularVelocity": -0.14457368832954978, - "velocityX": 1.7683868053695166, - "velocityY": 0.9342418403365317, - "timestamp": 0.17485104247936575 - }, - { - "x": 1.078441589183816, - "y": 6.803669240138584, - "heading": 0.9195149817064242, - "angularVelocity": -1.3646793077415271, - "velocityX": 1.7683868057588992, - "velocityY": 0.9342418396000037, - "timestamp": 0.233134723305821 - }, - { - "x": 1.1815096813012231, - "y": 6.858120293421828, - "heading": 0.7885846802295383, - "angularVelocity": -2.2464315846153347, - "velocityX": 1.7683868049497107, - "velocityY": 0.9342418411317296, - "timestamp": 0.29141840413227627 - }, - { - "x": 1.2845777733691615, - "y": 6.912571346798711, - "heading": 0.6269133013543681, - "angularVelocity": -2.7738704313454483, - "velocityX": 1.7683868041009485, - "velocityY": 0.9342418427383741, - "timestamp": 0.3497020849587315 - }, - { - "x": 1.3876458655603612, - "y": 6.967022399942281, - "heading": 0.45022486052054533, - "angularVelocity": -3.03152509120432, - "velocityX": 1.7683868062158064, - "velocityY": 0.9342418387352904, - "timestamp": 0.40798576578518675 - }, - { - "x": 1.4907139579910005, - "y": 7.021473452632629, - "heading": 0.2680622315698304, - "angularVelocity": -3.125448262138626, - "velocityX": 1.768386810323986, - "velocityY": 0.9342418309591196, - "timestamp": 0.466269446611642 - }, - { - "x": 1.5937820505189422, - "y": 7.075924505138796, - "heading": 0.0852527280795436, - "angularVelocity": -3.136546987065588, - "velocityX": 1.7683868119934505, - "velocityY": 0.9342418277990836, - "timestamp": 0.5245531274380972 - }, - { - "x": 1.696850142939802, - "y": 7.130375557847656, - "heading": -0.09670059771549627, - "angularVelocity": -3.1218571513776574, - "velocityX": 1.7683868101561941, - "velocityY": 0.934241831276761, - "timestamp": 0.5828368082645525 - }, - { - "x": 1.7999182351302574, - "y": 7.184826610992639, - "heading": -0.27821616946416433, - "angularVelocity": -3.1143464032592627, - "velocityX": 1.7683868062030377, - "velocityY": 0.934241838759528, - "timestamp": 0.6411204890910078 - }, - { - "x": 1.9029863270693197, - "y": 7.239277664613472, - "heading": -0.4604343199403316, - "angularVelocity": -3.126400870599392, - "velocityX": 1.7683868018897635, - "velocityY": 0.9342418469239417, - "timestamp": 0.6994041699174631 - }, - { - "x": 2.0060544188463965, - "y": 7.293728718540922, - "heading": -0.6442150495894297, - "angularVelocity": -3.1532107623299397, - "velocityX": 1.7683867991104978, - "velocityY": 0.9342418521846886, - "timestamp": 0.7576878507439184 - }, - { - "x": 2.1091225106680933, - "y": 7.348179772383911, - "heading": -0.829167725791517, - "angularVelocity": -3.173318389980438, - "velocityX": 1.7683867998760676, - "velocityY": 0.9342418507355571, - "timestamp": 0.8159715315703737 - }, - { - "x": 2.212190602724116, - "y": 7.402630825783353, - "heading": -1.0127639833296274, - "angularVelocity": -3.1500456891898856, - "velocityX": 1.7683868038965105, - "velocityY": 0.9342418431254117, - "timestamp": 0.874255212396829 - }, - { - "x": 2.3152586950079552, - "y": 7.45708187875157, - "heading": -1.189780759847791, - "angularVelocity": -3.0371584980121242, - "velocityX": 1.7683868078052658, - "velocityY": 0.9342418357266746, - "timestamp": 0.9325388932232843 - }, - { - "x": 2.4183267873051926, - "y": 7.511532931694425, - "heading": -1.3512503965030376, - "angularVelocity": -2.770409047008454, - "velocityX": 1.768386808035141, - "velocityY": 0.934241835291512, - "timestamp": 0.9908225740497396 - }, - { - "x": 2.5213948795058583, - "y": 7.565983984820071, - "heading": -1.4820477254792255, - "angularVelocity": -2.244150114076672, - "velocityX": 1.7683868063782155, - "velocityY": 0.9342418384277741, - "timestamp": 1.049106254876195 - }, - { - "x": 2.6244629716588035, - "y": 7.620435038036042, - "heading": -1.5615658496268932, - "angularVelocity": -1.3643291401540225, - "velocityX": 1.7683868055594567, - "velocityY": 0.9342418399775199, - "timestamp": 1.1073899357026502 - }, - { - "x": 2.7275310638313552, - "y": 7.674886091214861, - "heading": -1.569999963079154, - "angularVelocity": -0.14470797540511007, - "velocityX": 1.7683868058959282, - "velocityY": 0.9342418393401073, - "timestamp": 1.1656736165291055 - }, - { - "x": 2.7977986665714587, - "y": 7.7120085885476914, - "heading": -1.5699999870889756, - "angularVelocity": -4.119521318427831e-7, - "velocityX": 1.2056136768248569, - "velocityY": 0.6369278124926107, - "timestamp": 1.2239572973555608 - }, - { - "x": 2.8329324722290035, + "x": 0.7855613477332503, + "y": 6.648940096740809, + "heading": 1.0074798315639235, + "angularVelocity": -4.44626773132229e-7, + "velocityX": 0.29788756864663374, + "velocityY": 0.15737452300923568, + "timestamp": 0.06263219156953911 + }, + { + "x": 0.8228760494865476, + "y": 6.6686535188760185, + "heading": 1.0074797723867348, + "angularVelocity": -9.448368272101287e-7, + "velocityX": 0.5957751248711716, + "velocityY": 0.31474903945078925, + "timestamp": 0.12526438313907823 + }, + { + "x": 0.8788481004307849, + "y": 6.6982236511875834, + "heading": 1.0074796760782534, + "angularVelocity": -0.000001537683717241804, + "velocityX": 0.8936626603923403, + "velocityY": 0.47212354494627234, + "timestamp": 0.18789657470861734 + }, + { + "x": 0.9534774979725686, + "y": 6.737650492304352, + "heading": 1.0074795310346885, + "angularVelocity": -0.000002315799427807793, + "velocityX": 1.191550154506795, + "velocityY": 0.6294980285496351, + "timestamp": 0.25052876627815646 + }, + { + "x": 1.0467642343312271, + "y": 6.786934038112618, + "heading": 1.0074793024441084, + "angularVelocity": -0.000003649730463432068, + "velocityX": 1.489437524393268, + "velocityY": 0.7868724464726433, + "timestamp": 0.3131609578476956 + }, + { + "x": 1.140908484326656, + "y": 6.836670609920262, + "heading": 0.9598451569897353, + "angularVelocity": -0.7605377404288135, + "velocityX": 1.5031287846746073, + "velocityY": 0.7941055639482466, + "timestamp": 0.37579314941723474 + }, + { + "x": 1.2350527343337772, + "y": 6.886407181705814, + "heading": 0.868308368234956, + "angularVelocity": -1.461497457791568, + "velocityX": 1.503128784861293, + "velocityY": 0.7941055635955249, + "timestamp": 0.4384253409867739 + }, + { + "x": 1.3291969843350966, + "y": 6.936143753502349, + "heading": 0.7375936922085377, + "angularVelocity": -2.0870206318948967, + "velocityX": 1.5031287847686552, + "velocityY": 0.7941055637708808, + "timestamp": 0.501057532556313 + }, + { + "x": 1.4233412343345928, + "y": 6.985880325302336, + "heading": 0.5733869923214697, + "angularVelocity": -2.6217620008513047, + "velocityX": 1.50312878473955, + "velocityY": 0.794105563825983, + "timestamp": 0.5636897241258522 + }, + { + "x": 1.517485484369411, + "y": 7.035616897035464, + "heading": 0.38230326840749074, + "angularVelocity": -3.050886758477845, + "velocityX": 1.503128785303508, + "velocityY": 0.7941055627585059, + "timestamp": 0.6263219156953913 + }, + { + "x": 1.611629734494345, + "y": 7.085353468598017, + "heading": 0.1715892655816257, + "angularVelocity": -3.364308314070061, + "velocityX": 1.5031287867423193, + "velocityY": 0.7941055600350643, + "timestamp": 0.6889541072649304 + }, + { + "x": 1.705773984659377, + "y": 7.1350900400846715, + "heading": -0.05152973146485926, + "angularVelocity": -3.562369309697853, + "velocityX": 1.5031287873825276, + "velocityY": 0.7941055588232755, + "timestamp": 0.7515862988344696 + }, + { + "x": 1.7999182346855311, + "y": 7.184826611834204, + "heading": -0.2805858659507677, + "angularVelocity": -3.657163013871995, + "velocityX": 1.5031287851651773, + "velocityY": 0.7941055630204278, + "timestamp": 0.8142184904040087 + }, + { + "x": 1.8940624845219065, + "y": 7.23456318394296, + "heading": -0.5098527052085398, + "angularVelocity": -3.660527174802386, + "velocityX": 1.5031287821351207, + "velocityY": 0.7941055687558903, + "timestamp": 0.8768506819735479 + }, + { + "x": 1.9882067343467884, + "y": 7.28429975607347, + "heading": -0.7334067984715299, + "angularVelocity": -3.56931615612939, + "velocityX": 1.503128781951615, + "velocityY": 0.7941055691032111, + "timestamp": 0.939482873543087 + }, + { + "x": 2.082350984290343, + "y": 7.334036327979348, + "heading": -0.9444794864772089, + "angularVelocity": -3.370035164285983, + "velocityX": 1.5031287838463745, + "velocityY": 0.7941055655166722, + "timestamp": 1.002115065112626 + }, + { + "x": 2.1764952343123167, + "y": 7.383772899736789, + "heading": -1.1357610108525462, + "angularVelocity": -3.054044886213787, + "velocityX": 1.5031287850984254, + "velocityY": 0.7941055631466987, + "timestamp": 1.0647472566821652 + }, + { + "x": 2.2706394843420803, + "y": 7.433509471479484, + "heading": -1.3000590815023898, + "angularVelocity": -2.623220847500082, + "velocityX": 1.5031287852228015, + "velocityY": 0.794105562911257, + "timestamp": 1.1273794482517043 + }, + { + "x": 2.364783734359114, + "y": 7.483246043246272, + "heading": -1.4308123405268247, + "angularVelocity": -2.0876366569300906, + "velocityX": 1.5031287850195627, + "velocityY": 0.7941055632959484, + "timestamp": 1.1900116398212435 + }, + { + "x": 2.458927984369609, + "y": 7.532982615025439, + "heading": -1.5223626326145774, + "angularVelocity": -1.4617130551167605, + "velocityX": 1.503128784915151, + "velocityY": 0.7941055634935796, + "timestamp": 1.2526438313907826 + }, + { + "x": 2.5530722343913332, + "y": 7.582719186783309, + "heading": -1.569999443293059, + "angularVelocity": -0.7605802940109437, + "velocityX": 1.503128785094449, + "velocityY": 0.7941055631535453, + "timestamp": 1.3152760229603218 + }, + { + "x": 2.6463589707381336, + "y": 7.6320027326140165, + "heading": -1.5699996718030853, + "angularVelocity": -0.0000036484443212939075, + "velocityX": 1.4894375242039373, + "velocityY": 0.7868724468309513, + "timestamp": 1.377908214529861 + }, + { + "x": 2.7209883682732525, + "y": 7.671429573743398, + "heading": -1.5699998167765745, + "angularVelocity": -0.000002314680584768984, + "velocityX": 1.1915501544003835, + "velocityY": 0.6294980287510356, + "timestamp": 1.4405404060994 + }, + { + "x": 2.776960419213422, + "y": 7.700999706062662, + "heading": -1.5699999130306501, + "angularVelocity": -0.0000015368150590879431, + "velocityX": 0.8936626603273936, + "velocityY": 0.4721235450691958, + "timestamp": 1.5031725976689392 + }, + { + "x": 2.814275120964383, + "y": 7.720713128202295, + "heading": -1.5699999721708229, + "angularVelocity": -9.44245822746899e-7, + "velocityX": 0.5957751248338641, + "velocityY": 0.31474903952139993, + "timestamp": 1.5658047892384783 + }, + { + "x": 2.8329324722290044, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": -2.215218740040656e-7, - "velocityX": 0.6028069119750795, - "velocityY": 0.3184639450811426, - "timestamp": 1.282240978182016 + "angularVelocity": -4.443271507992151e-7, + "velocityX": 0.2978875686300534, + "velocityY": 0.15737452304061644, + "timestamp": 1.6284369808080175 }, { "x": 2.832932472229004, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": 2.8428255363446037e-18, - "velocityX": -6.6133223633086636e-18, - "velocityY": -3.51368640376029e-18, - "timestamp": 1.3405246590084714 + "angularVelocity": 5.736792697557943e-18, + "velocityX": 3.8921831241382794e-17, + "velocityY": 2.057106543177864e-17, + "timestamp": 1.6910691723775566 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/amp.traj b/src/main/deploy/choreo/amp.traj index 89142ab..c544758 100644 --- a/src/main/deploy/choreo/amp.traj +++ b/src/main/deploy/choreo/amp.traj @@ -1,220 +1,256 @@ { "samples": [ { - "x": 0.7669039964675903, + "x": 0.7669039964675901, "y": 6.639083385467529, "heading": 1.0074798594118668, - "angularVelocity": 4.703190638042804e-18, - "velocityX": -1.208675917564465e-17, - "velocityY": -6.352810871367948e-18, + "angularVelocity": 1.0512088330044834e-17, + "velocityX": 1.2971420838221857e-16, + "velocityY": 6.851970292534496e-17, "timestamp": 0 }, { - "x": 0.8020378021280192, - "y": 6.657644636391979, - "heading": 1.007479841292087, - "angularVelocity": -3.1089087057766036e-7, - "velocityX": 0.6028069120245683, - "velocityY": 0.31846394498851993, - "timestamp": 0.05828368082645525 - }, - { - "x": 0.8723054048773754, - "y": 6.694767133707721, - "heading": 1.0074798016172224, - "angularVelocity": -6.807244191958373e-7, - "velocityX": 1.2056136769836088, - "velocityY": 0.6369278121994305, - "timestamp": 0.1165673616529105 - }, - { - "x": 0.9753734970192464, - "y": 6.749218186944615, - "heading": 0.9990535149100053, - "angularVelocity": -0.14457368832954978, - "velocityX": 1.7683868053695166, - "velocityY": 0.9342418403365317, - "timestamp": 0.17485104247936575 - }, - { - "x": 1.078441589183816, - "y": 6.803669240138584, - "heading": 0.9195149817064242, - "angularVelocity": -1.3646793077415271, - "velocityX": 1.7683868057588992, - "velocityY": 0.9342418396000037, - "timestamp": 0.233134723305821 - }, - { - "x": 1.1815096813012231, - "y": 6.858120293421828, - "heading": 0.7885846802295383, - "angularVelocity": -2.2464315846153347, - "velocityX": 1.7683868049497107, - "velocityY": 0.9342418411317296, - "timestamp": 0.29141840413227627 - }, - { - "x": 1.2845777733691615, - "y": 6.912571346798711, - "heading": 0.6269133013543681, - "angularVelocity": -2.7738704313454483, - "velocityX": 1.7683868041009485, - "velocityY": 0.9342418427383741, - "timestamp": 0.3497020849587315 - }, - { - "x": 1.3876458655603612, - "y": 6.967022399942281, - "heading": 0.45022486052054533, - "angularVelocity": -3.03152509120432, - "velocityX": 1.7683868062158064, - "velocityY": 0.9342418387352904, - "timestamp": 0.40798576578518675 - }, - { - "x": 1.4907139579910005, - "y": 7.021473452632629, - "heading": 0.2680622315698304, - "angularVelocity": -3.125448262138626, - "velocityX": 1.768386810323986, - "velocityY": 0.9342418309591196, - "timestamp": 0.466269446611642 - }, - { - "x": 1.5937820505189422, - "y": 7.075924505138796, - "heading": 0.0852527280795436, - "angularVelocity": -3.136546987065588, - "velocityX": 1.7683868119934505, - "velocityY": 0.9342418277990836, - "timestamp": 0.5245531274380972 - }, - { - "x": 1.696850142939802, - "y": 7.130375557847656, - "heading": -0.09670059771549627, - "angularVelocity": -3.1218571513776574, - "velocityX": 1.7683868101561941, - "velocityY": 0.934241831276761, - "timestamp": 0.5828368082645525 - }, - { - "x": 1.7999182351302574, - "y": 7.184826610992639, - "heading": -0.27821616946416433, - "angularVelocity": -3.1143464032592627, - "velocityX": 1.7683868062030377, - "velocityY": 0.934241838759528, - "timestamp": 0.6411204890910078 - }, - { - "x": 1.9029863270693197, - "y": 7.239277664613472, - "heading": -0.4604343199403316, - "angularVelocity": -3.126400870599392, - "velocityX": 1.7683868018897635, - "velocityY": 0.9342418469239417, - "timestamp": 0.6994041699174631 - }, - { - "x": 2.0060544188463965, - "y": 7.293728718540922, - "heading": -0.6442150495894297, - "angularVelocity": -3.1532107623299397, - "velocityX": 1.7683867991104978, - "velocityY": 0.9342418521846886, - "timestamp": 0.7576878507439184 - }, - { - "x": 2.1091225106680933, - "y": 7.348179772383911, - "heading": -0.829167725791517, - "angularVelocity": -3.173318389980438, - "velocityX": 1.7683867998760676, - "velocityY": 0.9342418507355571, - "timestamp": 0.8159715315703737 - }, - { - "x": 2.212190602724116, - "y": 7.402630825783353, - "heading": -1.0127639833296274, - "angularVelocity": -3.1500456891898856, - "velocityX": 1.7683868038965105, - "velocityY": 0.9342418431254117, - "timestamp": 0.874255212396829 - }, - { - "x": 2.3152586950079552, - "y": 7.45708187875157, - "heading": -1.189780759847791, - "angularVelocity": -3.0371584980121242, - "velocityX": 1.7683868078052658, - "velocityY": 0.9342418357266746, - "timestamp": 0.9325388932232843 - }, - { - "x": 2.4183267873051926, - "y": 7.511532931694425, - "heading": -1.3512503965030376, - "angularVelocity": -2.770409047008454, - "velocityX": 1.768386808035141, - "velocityY": 0.934241835291512, - "timestamp": 0.9908225740497396 - }, - { - "x": 2.5213948795058583, - "y": 7.565983984820071, - "heading": -1.4820477254792255, - "angularVelocity": -2.244150114076672, - "velocityX": 1.7683868063782155, - "velocityY": 0.9342418384277741, - "timestamp": 1.049106254876195 - }, - { - "x": 2.6244629716588035, - "y": 7.620435038036042, - "heading": -1.5615658496268932, - "angularVelocity": -1.3643291401540225, - "velocityX": 1.7683868055594567, - "velocityY": 0.9342418399775199, - "timestamp": 1.1073899357026502 - }, - { - "x": 2.7275310638313552, - "y": 7.674886091214861, - "heading": -1.569999963079154, - "angularVelocity": -0.14470797540511007, - "velocityX": 1.7683868058959282, - "velocityY": 0.9342418393401073, - "timestamp": 1.1656736165291055 - }, - { - "x": 2.7977986665714587, - "y": 7.7120085885476914, - "heading": -1.5699999870889756, - "angularVelocity": -4.119521318427831e-7, - "velocityX": 1.2056136768248569, - "velocityY": 0.6369278124926107, - "timestamp": 1.2239572973555608 - }, - { - "x": 2.8329324722290035, + "x": 0.7855613477332503, + "y": 6.648940096740809, + "heading": 1.0074798315639235, + "angularVelocity": -4.44626773132229e-7, + "velocityX": 0.29788756864663374, + "velocityY": 0.15737452300923568, + "timestamp": 0.06263219156953911 + }, + { + "x": 0.8228760494865476, + "y": 6.6686535188760185, + "heading": 1.0074797723867348, + "angularVelocity": -9.448368272101287e-7, + "velocityX": 0.5957751248711716, + "velocityY": 0.31474903945078925, + "timestamp": 0.12526438313907823 + }, + { + "x": 0.8788481004307849, + "y": 6.6982236511875834, + "heading": 1.0074796760782534, + "angularVelocity": -0.000001537683717241804, + "velocityX": 0.8936626603923403, + "velocityY": 0.47212354494627234, + "timestamp": 0.18789657470861734 + }, + { + "x": 0.9534774979725686, + "y": 6.737650492304352, + "heading": 1.0074795310346885, + "angularVelocity": -0.000002315799427807793, + "velocityX": 1.191550154506795, + "velocityY": 0.6294980285496351, + "timestamp": 0.25052876627815646 + }, + { + "x": 1.0467642343312271, + "y": 6.786934038112618, + "heading": 1.0074793024441084, + "angularVelocity": -0.000003649730463432068, + "velocityX": 1.489437524393268, + "velocityY": 0.7868724464726433, + "timestamp": 0.3131609578476956 + }, + { + "x": 1.140908484326656, + "y": 6.836670609920262, + "heading": 0.9598451569897353, + "angularVelocity": -0.7605377404288135, + "velocityX": 1.5031287846746073, + "velocityY": 0.7941055639482466, + "timestamp": 0.37579314941723474 + }, + { + "x": 1.2350527343337772, + "y": 6.886407181705814, + "heading": 0.868308368234956, + "angularVelocity": -1.461497457791568, + "velocityX": 1.503128784861293, + "velocityY": 0.7941055635955249, + "timestamp": 0.4384253409867739 + }, + { + "x": 1.3291969843350966, + "y": 6.936143753502349, + "heading": 0.7375936922085377, + "angularVelocity": -2.0870206318948967, + "velocityX": 1.5031287847686552, + "velocityY": 0.7941055637708808, + "timestamp": 0.501057532556313 + }, + { + "x": 1.4233412343345928, + "y": 6.985880325302336, + "heading": 0.5733869923214697, + "angularVelocity": -2.6217620008513047, + "velocityX": 1.50312878473955, + "velocityY": 0.794105563825983, + "timestamp": 0.5636897241258522 + }, + { + "x": 1.517485484369411, + "y": 7.035616897035464, + "heading": 0.38230326840749074, + "angularVelocity": -3.050886758477845, + "velocityX": 1.503128785303508, + "velocityY": 0.7941055627585059, + "timestamp": 0.6263219156953913 + }, + { + "x": 1.611629734494345, + "y": 7.085353468598017, + "heading": 0.1715892655816257, + "angularVelocity": -3.364308314070061, + "velocityX": 1.5031287867423193, + "velocityY": 0.7941055600350643, + "timestamp": 0.6889541072649304 + }, + { + "x": 1.705773984659377, + "y": 7.1350900400846715, + "heading": -0.05152973146485926, + "angularVelocity": -3.562369309697853, + "velocityX": 1.5031287873825276, + "velocityY": 0.7941055588232755, + "timestamp": 0.7515862988344696 + }, + { + "x": 1.7999182346855311, + "y": 7.184826611834204, + "heading": -0.2805858659507677, + "angularVelocity": -3.657163013871995, + "velocityX": 1.5031287851651773, + "velocityY": 0.7941055630204278, + "timestamp": 0.8142184904040087 + }, + { + "x": 1.8940624845219065, + "y": 7.23456318394296, + "heading": -0.5098527052085398, + "angularVelocity": -3.660527174802386, + "velocityX": 1.5031287821351207, + "velocityY": 0.7941055687558903, + "timestamp": 0.8768506819735479 + }, + { + "x": 1.9882067343467884, + "y": 7.28429975607347, + "heading": -0.7334067984715299, + "angularVelocity": -3.56931615612939, + "velocityX": 1.503128781951615, + "velocityY": 0.7941055691032111, + "timestamp": 0.939482873543087 + }, + { + "x": 2.082350984290343, + "y": 7.334036327979348, + "heading": -0.9444794864772089, + "angularVelocity": -3.370035164285983, + "velocityX": 1.5031287838463745, + "velocityY": 0.7941055655166722, + "timestamp": 1.002115065112626 + }, + { + "x": 2.1764952343123167, + "y": 7.383772899736789, + "heading": -1.1357610108525462, + "angularVelocity": -3.054044886213787, + "velocityX": 1.5031287850984254, + "velocityY": 0.7941055631466987, + "timestamp": 1.0647472566821652 + }, + { + "x": 2.2706394843420803, + "y": 7.433509471479484, + "heading": -1.3000590815023898, + "angularVelocity": -2.623220847500082, + "velocityX": 1.5031287852228015, + "velocityY": 0.794105562911257, + "timestamp": 1.1273794482517043 + }, + { + "x": 2.364783734359114, + "y": 7.483246043246272, + "heading": -1.4308123405268247, + "angularVelocity": -2.0876366569300906, + "velocityX": 1.5031287850195627, + "velocityY": 0.7941055632959484, + "timestamp": 1.1900116398212435 + }, + { + "x": 2.458927984369609, + "y": 7.532982615025439, + "heading": -1.5223626326145774, + "angularVelocity": -1.4617130551167605, + "velocityX": 1.503128784915151, + "velocityY": 0.7941055634935796, + "timestamp": 1.2526438313907826 + }, + { + "x": 2.5530722343913332, + "y": 7.582719186783309, + "heading": -1.569999443293059, + "angularVelocity": -0.7605802940109437, + "velocityX": 1.503128785094449, + "velocityY": 0.7941055631535453, + "timestamp": 1.3152760229603218 + }, + { + "x": 2.6463589707381336, + "y": 7.6320027326140165, + "heading": -1.5699996718030853, + "angularVelocity": -0.0000036484443212939075, + "velocityX": 1.4894375242039373, + "velocityY": 0.7868724468309513, + "timestamp": 1.377908214529861 + }, + { + "x": 2.7209883682732525, + "y": 7.671429573743398, + "heading": -1.5699998167765745, + "angularVelocity": -0.000002314680584768984, + "velocityX": 1.1915501544003835, + "velocityY": 0.6294980287510356, + "timestamp": 1.4405404060994 + }, + { + "x": 2.776960419213422, + "y": 7.700999706062662, + "heading": -1.5699999130306501, + "angularVelocity": -0.0000015368150590879431, + "velocityX": 0.8936626603273936, + "velocityY": 0.4721235450691958, + "timestamp": 1.5031725976689392 + }, + { + "x": 2.814275120964383, + "y": 7.720713128202295, + "heading": -1.5699999721708229, + "angularVelocity": -9.44245822746899e-7, + "velocityX": 0.5957751248338641, + "velocityY": 0.31474903952139993, + "timestamp": 1.5658047892384783 + }, + { + "x": 2.8329324722290044, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": -2.215218740040656e-7, - "velocityX": 0.6028069119750795, - "velocityY": 0.3184639450811426, - "timestamp": 1.282240978182016 + "angularVelocity": -4.443271507992151e-7, + "velocityX": 0.2978875686300534, + "velocityY": 0.15737452304061644, + "timestamp": 1.6284369808080175 }, { "x": 2.832932472229004, "y": 7.730569839477539, "heading": -1.57, - "angularVelocity": 2.8428255363446037e-18, - "velocityX": -6.6133223633086636e-18, - "velocityY": -3.51368640376029e-18, - "timestamp": 1.3405246590084714 + "angularVelocity": 5.736792697557943e-18, + "velocityX": 3.8921831241382794e-17, + "velocityY": 2.057106543177864e-17, + "timestamp": 1.6910691723775566 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/exit center.1.traj b/src/main/deploy/choreo/exit center.1.traj index bffdbd3..62557fd 100644 --- a/src/main/deploy/choreo/exit center.1.traj +++ b/src/main/deploy/choreo/exit center.1.traj @@ -4,154 +4,181 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": -4.047258922723054e-33, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": 0, + "velocityX": 1.674488653195789e-33, + "velocityY": 7.11171747202546e-38, "timestamp": 0 }, { - "x": 1.3562741157973404, - "y": 5.551277015096179, - "heading": 5.1790019930154625e-8, - "angularVelocity": 8.222689572844604e-7, - "velocityX": 0.6926593812833762, - "velocityY": -0.2510250916993935, - "timestamp": 0.06298428215226594 - }, - { - "x": 1.4435274144279229, - "y": 5.519655747707938, - "heading": 1.8284200071758018e-7, - "angularVelocity": 0.0000020807092906136113, - "velocityX": 1.3853186167883211, - "velocityY": -0.5020501354892848, - "timestamp": 0.12596856430453188 - }, - { - "x": 1.561958503686593, - "y": 5.476735411831991, - "heading": 0.03758882643806014, - "angularVelocity": 0.5967940303771042, - "velocityX": 1.8803276819502406, - "velocityY": -0.6814451861527359, - "timestamp": 0.18895284645679783 - }, - { - "x": 1.6803895924155994, - "y": 5.433815074494518, - "heading": 0.1495361372904831, - "angularVelocity": 1.7773848812277921, - "velocityX": 1.8803276735407826, - "velocityY": -0.6814452093573423, - "timestamp": 0.25193712860906375 - }, - { - "x": 1.7988206811669072, - "y": 5.3908947372185825, - "heading": 0.3084692846412802, - "angularVelocity": 2.5233779273148262, - "velocityX": 1.8803276738948609, - "velocityY": -0.6814452083803143, - "timestamp": 0.3149214107613297 - }, - { - "x": 1.9172517698549563, - "y": 5.347974399768096, - "heading": 0.4903249170402497, - "angularVelocity": 2.887317695537583, - "velocityX": 1.880327672890501, - "velocityY": -0.6814452111516576, - "timestamp": 0.3779056929135956 - }, - { - "x": 2.0356828583945066, - "y": 5.305054061907854, - "heading": 0.6822100511004133, - "angularVelocity": 3.0465558628782476, - "velocityX": 1.880327670532791, - "velocityY": -0.6814452176573361, - "timestamp": 0.44088997506586153 - }, - { - "x": 2.1541139468698915, - "y": 5.26213372387056, - "heading": 0.8783830367201194, - "angularVelocity": 3.1146339835302603, - "velocityX": 1.880327669514046, - "velocityY": -0.6814452204683829, - "timestamp": 0.5038742572181275 - }, - { - "x": 2.272545035456946, - "y": 5.219213386141397, - "heading": 1.0737017740998906, - "angularVelocity": 3.1010711038602228, - "velocityX": 1.8803276712870123, - "velocityY": -0.6814452155761989, - "timestamp": 0.5668585393703934 - }, - { - "x": 2.390976124200322, - "y": 5.176293048843576, - "heading": 1.2594557238711797, - "angularVelocity": 2.949211190852737, - "velocityX": 1.8803276737689296, - "velocityY": -0.6814452087277921, - "timestamp": 0.6298428215226594 - }, - { - "x": 2.5094072129682594, - "y": 5.133372711613527, - "heading": 1.4204186158420298, - "angularVelocity": 2.5556041359925064, - "velocityX": 1.8803276741588917, - "velocityY": -0.6814452076517687, - "timestamp": 0.6928271036749253 - }, - { - "x": 2.627838301697845, - "y": 5.090452374277653, - "heading": 1.5329754550704864, - "angularVelocity": 1.787062349243704, - "velocityX": 1.880327673549982, - "velocityY": -0.6814452093319581, - "timestamp": 0.7558113858271912 - }, - { - "x": 2.746269390946397, - "y": 5.047532038373786, - "heading": 1.5707961445746523, - "angularVelocity": 0.6004782179263964, - "velocityX": 1.8803276817895906, - "velocityY": -0.6814451865960204, - "timestamp": 0.8187956679794571 - }, - { - "x": 2.8335226895825705, - "y": 5.015910771000973, - "heading": 1.570796275196268, - "angularVelocity": 0.000002073876396402678, - "velocityX": 1.3853186168770866, - "velocityY": -0.5020501352443532, - "timestamp": 0.8817799501317231 + "x": 1.34929738878066, + "y": 5.5538054348907835, + "heading": 1.2355468400098194e-8, + "angularVelocity": 1.451370137183604e-7, + "velocityX": 0.4305187599139744, + "velocityY": -0.156023310052692, + "timestamp": 0.08512968609500099 + }, + { + "x": 1.4225972370219229, + "y": 5.527241006054132, + "heading": 4.5628523121809076e-8, + "angularVelocity": 3.908513740983071e-7, + "velocityX": 0.8610374547776837, + "velocityY": -0.31204659684762154, + "timestamp": 0.17025937219000198 + }, + { + "x": 1.5186402607278184, + "y": 5.492434275732478, + "heading": 0.023798969361148174, + "angularVelocity": 0.2795608068619741, + "velocityX": 1.1281966152056035, + "velocityY": -0.4088671287100916, + "timestamp": 0.25538905828500297 + }, + { + "x": 1.614683284388423, + "y": 5.457627545285846, + "heading": 0.08983587293589476, + "angularVelocity": 0.7757212155234814, + "velocityX": 1.1281966146735791, + "velocityY": -0.40886713017817483, + "timestamp": 0.34051874438000396 + }, + { + "x": 1.7107263080490587, + "y": 5.4228208148393, + "heading": 0.19002287752886737, + "angularVelocity": 1.1768750619045902, + "velocityX": 1.1281966146739437, + "velocityY": -0.4088671301771691, + "timestamp": 0.42564843047500495 + }, + { + "x": 1.806769331709714, + "y": 5.388014084392808, + "heading": 0.3166500634968596, + "angularVelocity": 1.4874621507082946, + "velocityX": 1.1281966146741744, + "velocityY": -0.40886713017653226, + "timestamp": 0.5107781165700059 + }, + { + "x": 1.902812355370287, + "y": 5.35320735394609, + "heading": 0.46249608510530194, + "angularVelocity": 1.7132216539091256, + "velocityX": 1.1281966146732083, + "velocityY": -0.4088671301791978, + "timestamp": 0.5959078026650069 + }, + { + "x": 1.9988553790306578, + "y": 5.318400623498813, + "heading": 0.6208292331181784, + "angularVelocity": 1.8599052254954127, + "velocityX": 1.1281966146708353, + "velocityY": -0.408867130185746, + "timestamp": 0.6810374887600079 + }, + { + "x": 2.094898402690861, + "y": 5.283593893051074, + "heading": 0.7853048024978768, + "angularVelocity": 1.9320589200358482, + "velocityX": 1.1281966146688651, + "velocityY": -0.40886713019118176, + "timestamp": 0.766167174855009 + }, + { + "x": 2.1909414263511025, + "y": 5.248787162603439, + "heading": 0.9497879513318085, + "angularVelocity": 1.9321479542444888, + "velocityX": 1.1281966146693132, + "velocityY": -0.40886713018994547, + "timestamp": 0.85129686095001 + }, + { + "x": 2.286984450011549, + "y": 5.213980432156372, + "heading": 1.108141013047214, + "angularVelocity": 1.8601391474495799, + "velocityX": 1.128196614671724, + "velocityY": -0.4088671301832935, + "timestamp": 0.936426547045011 + }, + { + "x": 2.383027473672174, + "y": 5.1791737017097965, + "heading": 1.254013432652853, + "angularVelocity": 1.7135317454695138, + "velocityX": 1.1281966146738198, + "velocityY": -0.40886713017751036, + "timestamp": 1.021556233140012 + }, + { + "x": 2.479070497332845, + "y": 5.144366971263349, + "heading": 1.3806693463444173, + "angularVelocity": 1.4877996090602523, + "velocityX": 1.1281966146743598, + "velocityY": -0.4088671301760209, + "timestamp": 1.1066859192350131 + }, + { + "x": 2.5751135209934812, + "y": 5.109560240816806, + "heading": 1.4808868727958657, + "angularVelocity": 1.1772335955709967, + "velocityX": 1.128196614673956, + "velocityY": -0.40886713017713466, + "timestamp": 1.1918156053300142 + }, + { + "x": 2.6711565446540857, + "y": 5.074753510370173, + "heading": 1.5469577993154264, + "angularVelocity": 0.776120875693452, + "velocityX": 1.1281966146735771, + "velocityY": -0.4088671301781801, + "timestamp": 1.2769452914250152 + }, + { + "x": 2.7671995683594357, + "y": 5.039946780047014, + "heading": 1.5707962811320204, + "angularVelocity": 0.2800254871137584, + "velocityX": 1.1281966151991965, + "velocityY": -0.4088671287277703, + "timestamp": 1.3620749775200163 + }, + { + "x": 2.840499416601011, + "y": 5.0133823512112246, + "heading": 1.5707963144274508, + "angularVelocity": 3.911142169044648e-7, + "velocityX": 0.8610374547813509, + "velocityY": -0.3120465968375025, + "timestamp": 1.4472046636150173 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 8.19230237095319e-7, - "velocityX": 0.6926593813129686, - "velocityY": -0.25102509161774106, - "timestamp": 0.944764232283989 + "angularVelocity": 1.4527771000322412e-7, + "velocityX": 0.4305187599151969, + "velocityY": -0.15602331004931888, + "timestamp": 1.5323343497100184 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 0, - "velocityX": -1.88980553526586e-32, + "angularVelocity": 1.8805605259344148e-32, + "velocityX": 2.677429684849837e-34, "velocityY": 0, - "timestamp": 1.007748514436255 + "timestamp": 1.6174640358050194 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/exit center.traj b/src/main/deploy/choreo/exit center.traj index bffdbd3..62557fd 100644 --- a/src/main/deploy/choreo/exit center.traj +++ b/src/main/deploy/choreo/exit center.traj @@ -4,154 +4,181 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": -4.047258922723054e-33, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": 0, + "velocityX": 1.674488653195789e-33, + "velocityY": 7.11171747202546e-38, "timestamp": 0 }, { - "x": 1.3562741157973404, - "y": 5.551277015096179, - "heading": 5.1790019930154625e-8, - "angularVelocity": 8.222689572844604e-7, - "velocityX": 0.6926593812833762, - "velocityY": -0.2510250916993935, - "timestamp": 0.06298428215226594 - }, - { - "x": 1.4435274144279229, - "y": 5.519655747707938, - "heading": 1.8284200071758018e-7, - "angularVelocity": 0.0000020807092906136113, - "velocityX": 1.3853186167883211, - "velocityY": -0.5020501354892848, - "timestamp": 0.12596856430453188 - }, - { - "x": 1.561958503686593, - "y": 5.476735411831991, - "heading": 0.03758882643806014, - "angularVelocity": 0.5967940303771042, - "velocityX": 1.8803276819502406, - "velocityY": -0.6814451861527359, - "timestamp": 0.18895284645679783 - }, - { - "x": 1.6803895924155994, - "y": 5.433815074494518, - "heading": 0.1495361372904831, - "angularVelocity": 1.7773848812277921, - "velocityX": 1.8803276735407826, - "velocityY": -0.6814452093573423, - "timestamp": 0.25193712860906375 - }, - { - "x": 1.7988206811669072, - "y": 5.3908947372185825, - "heading": 0.3084692846412802, - "angularVelocity": 2.5233779273148262, - "velocityX": 1.8803276738948609, - "velocityY": -0.6814452083803143, - "timestamp": 0.3149214107613297 - }, - { - "x": 1.9172517698549563, - "y": 5.347974399768096, - "heading": 0.4903249170402497, - "angularVelocity": 2.887317695537583, - "velocityX": 1.880327672890501, - "velocityY": -0.6814452111516576, - "timestamp": 0.3779056929135956 - }, - { - "x": 2.0356828583945066, - "y": 5.305054061907854, - "heading": 0.6822100511004133, - "angularVelocity": 3.0465558628782476, - "velocityX": 1.880327670532791, - "velocityY": -0.6814452176573361, - "timestamp": 0.44088997506586153 - }, - { - "x": 2.1541139468698915, - "y": 5.26213372387056, - "heading": 0.8783830367201194, - "angularVelocity": 3.1146339835302603, - "velocityX": 1.880327669514046, - "velocityY": -0.6814452204683829, - "timestamp": 0.5038742572181275 - }, - { - "x": 2.272545035456946, - "y": 5.219213386141397, - "heading": 1.0737017740998906, - "angularVelocity": 3.1010711038602228, - "velocityX": 1.8803276712870123, - "velocityY": -0.6814452155761989, - "timestamp": 0.5668585393703934 - }, - { - "x": 2.390976124200322, - "y": 5.176293048843576, - "heading": 1.2594557238711797, - "angularVelocity": 2.949211190852737, - "velocityX": 1.8803276737689296, - "velocityY": -0.6814452087277921, - "timestamp": 0.6298428215226594 - }, - { - "x": 2.5094072129682594, - "y": 5.133372711613527, - "heading": 1.4204186158420298, - "angularVelocity": 2.5556041359925064, - "velocityX": 1.8803276741588917, - "velocityY": -0.6814452076517687, - "timestamp": 0.6928271036749253 - }, - { - "x": 2.627838301697845, - "y": 5.090452374277653, - "heading": 1.5329754550704864, - "angularVelocity": 1.787062349243704, - "velocityX": 1.880327673549982, - "velocityY": -0.6814452093319581, - "timestamp": 0.7558113858271912 - }, - { - "x": 2.746269390946397, - "y": 5.047532038373786, - "heading": 1.5707961445746523, - "angularVelocity": 0.6004782179263964, - "velocityX": 1.8803276817895906, - "velocityY": -0.6814451865960204, - "timestamp": 0.8187956679794571 - }, - { - "x": 2.8335226895825705, - "y": 5.015910771000973, - "heading": 1.570796275196268, - "angularVelocity": 0.000002073876396402678, - "velocityX": 1.3853186168770866, - "velocityY": -0.5020501352443532, - "timestamp": 0.8817799501317231 + "x": 1.34929738878066, + "y": 5.5538054348907835, + "heading": 1.2355468400098194e-8, + "angularVelocity": 1.451370137183604e-7, + "velocityX": 0.4305187599139744, + "velocityY": -0.156023310052692, + "timestamp": 0.08512968609500099 + }, + { + "x": 1.4225972370219229, + "y": 5.527241006054132, + "heading": 4.5628523121809076e-8, + "angularVelocity": 3.908513740983071e-7, + "velocityX": 0.8610374547776837, + "velocityY": -0.31204659684762154, + "timestamp": 0.17025937219000198 + }, + { + "x": 1.5186402607278184, + "y": 5.492434275732478, + "heading": 0.023798969361148174, + "angularVelocity": 0.2795608068619741, + "velocityX": 1.1281966152056035, + "velocityY": -0.4088671287100916, + "timestamp": 0.25538905828500297 + }, + { + "x": 1.614683284388423, + "y": 5.457627545285846, + "heading": 0.08983587293589476, + "angularVelocity": 0.7757212155234814, + "velocityX": 1.1281966146735791, + "velocityY": -0.40886713017817483, + "timestamp": 0.34051874438000396 + }, + { + "x": 1.7107263080490587, + "y": 5.4228208148393, + "heading": 0.19002287752886737, + "angularVelocity": 1.1768750619045902, + "velocityX": 1.1281966146739437, + "velocityY": -0.4088671301771691, + "timestamp": 0.42564843047500495 + }, + { + "x": 1.806769331709714, + "y": 5.388014084392808, + "heading": 0.3166500634968596, + "angularVelocity": 1.4874621507082946, + "velocityX": 1.1281966146741744, + "velocityY": -0.40886713017653226, + "timestamp": 0.5107781165700059 + }, + { + "x": 1.902812355370287, + "y": 5.35320735394609, + "heading": 0.46249608510530194, + "angularVelocity": 1.7132216539091256, + "velocityX": 1.1281966146732083, + "velocityY": -0.4088671301791978, + "timestamp": 0.5959078026650069 + }, + { + "x": 1.9988553790306578, + "y": 5.318400623498813, + "heading": 0.6208292331181784, + "angularVelocity": 1.8599052254954127, + "velocityX": 1.1281966146708353, + "velocityY": -0.408867130185746, + "timestamp": 0.6810374887600079 + }, + { + "x": 2.094898402690861, + "y": 5.283593893051074, + "heading": 0.7853048024978768, + "angularVelocity": 1.9320589200358482, + "velocityX": 1.1281966146688651, + "velocityY": -0.40886713019118176, + "timestamp": 0.766167174855009 + }, + { + "x": 2.1909414263511025, + "y": 5.248787162603439, + "heading": 0.9497879513318085, + "angularVelocity": 1.9321479542444888, + "velocityX": 1.1281966146693132, + "velocityY": -0.40886713018994547, + "timestamp": 0.85129686095001 + }, + { + "x": 2.286984450011549, + "y": 5.213980432156372, + "heading": 1.108141013047214, + "angularVelocity": 1.8601391474495799, + "velocityX": 1.128196614671724, + "velocityY": -0.4088671301832935, + "timestamp": 0.936426547045011 + }, + { + "x": 2.383027473672174, + "y": 5.1791737017097965, + "heading": 1.254013432652853, + "angularVelocity": 1.7135317454695138, + "velocityX": 1.1281966146738198, + "velocityY": -0.40886713017751036, + "timestamp": 1.021556233140012 + }, + { + "x": 2.479070497332845, + "y": 5.144366971263349, + "heading": 1.3806693463444173, + "angularVelocity": 1.4877996090602523, + "velocityX": 1.1281966146743598, + "velocityY": -0.4088671301760209, + "timestamp": 1.1066859192350131 + }, + { + "x": 2.5751135209934812, + "y": 5.109560240816806, + "heading": 1.4808868727958657, + "angularVelocity": 1.1772335955709967, + "velocityX": 1.128196614673956, + "velocityY": -0.40886713017713466, + "timestamp": 1.1918156053300142 + }, + { + "x": 2.6711565446540857, + "y": 5.074753510370173, + "heading": 1.5469577993154264, + "angularVelocity": 0.776120875693452, + "velocityX": 1.1281966146735771, + "velocityY": -0.4088671301781801, + "timestamp": 1.2769452914250152 + }, + { + "x": 2.7671995683594357, + "y": 5.039946780047014, + "heading": 1.5707962811320204, + "angularVelocity": 0.2800254871137584, + "velocityX": 1.1281966151991965, + "velocityY": -0.4088671287277703, + "timestamp": 1.3620749775200163 + }, + { + "x": 2.840499416601011, + "y": 5.0133823512112246, + "heading": 1.5707963144274508, + "angularVelocity": 3.911142169044648e-7, + "velocityX": 0.8610374547813509, + "velocityY": -0.3120465968375025, + "timestamp": 1.4472046636150173 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 8.19230237095319e-7, - "velocityX": 0.6926593813129686, - "velocityY": -0.25102509161774106, - "timestamp": 0.944764232283989 + "angularVelocity": 1.4527771000322412e-7, + "velocityX": 0.4305187599151969, + "velocityY": -0.15602331004931888, + "timestamp": 1.5323343497100184 }, { "x": 2.8771493434906006, "y": 5.000100135803223, "heading": 1.5707963267948966, - "angularVelocity": 0, - "velocityX": -1.88980553526586e-32, + "angularVelocity": 1.8805605259344148e-32, + "velocityX": 2.677429684849837e-34, "velocityY": 0, - "timestamp": 1.007748514436255 + "timestamp": 1.6174640358050194 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 piece amp side.auto b/src/main/deploy/pathplanner/autos/2 piece amp side.auto index dac4146..200f7e3 100644 --- a/src/main/deploy/pathplanner/autos/2 piece amp side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece amp side.auto @@ -32,19 +32,39 @@ { "type": "named", "data": { - "name": "ResetScoring" + "name": "StopShooter" } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "2 piece left.1" - } - }, - { - "type": "named", - "data": { - "name": "WaitIntake" + "commands": [ + { + "type": "path", + "data": { + "pathName": "2 piece left.1" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.2 + } + } + ] + } + } + ] } }, { @@ -59,6 +79,12 @@ "name": "SpeakerSetting" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/2 piece center.auto b/src/main/deploy/pathplanner/autos/2 piece center.auto index 628279c..a75fe77 100644 --- a/src/main/deploy/pathplanner/autos/2 piece center.auto +++ b/src/main/deploy/pathplanner/autos/2 piece center.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3126474618832242, - "y": 5.567087649501998 + "x": 1.3126474618911743, + "y": 5.567087650299072 }, - "rotation": 3.2524999489111804e-11 + "rotation": 4.78577780999322e-30 }, "command": { "type": "sequential", @@ -23,6 +23,12 @@ "name": "SpeakerSetting" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "named", "data": { @@ -32,25 +38,63 @@ { "type": "named", "data": { - "name": "ResetScoring" + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "2piece.1" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + } + ] } }, { "type": "path", "data": { - "pathName": "2piece.1" + "pathName": "2piece.2" } }, { "type": "named", "data": { - "name": "WaitIntake" + "name": "SpeakerSetting" } }, { - "type": "named", + "type": "wait", "data": { - "name": "LimelightSetting" + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/2 piece source side.auto b/src/main/deploy/pathplanner/autos/2 piece source side.auto index 7610586..7da5ca4 100644 --- a/src/main/deploy/pathplanner/autos/2 piece source side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece source side.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7823778390884399, + "x": 0.78237783908844, "y": 4.488044738769531 }, "rotation": -56.888709413415555 @@ -32,19 +32,39 @@ { "type": "named", "data": { - "name": "ResetScoring" + "name": "StopShooter" } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "2 piece right.1" - } - }, - { - "type": "named", - "data": { - "name": "WaitIntake" + "commands": [ + { + "type": "path", + "data": { + "pathName": "2 piece right.1" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.3 + } + } + ] + } + } + ] } }, { @@ -59,6 +79,12 @@ "name": "SpeakerSetting" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto b/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto new file mode 100644 index 0000000..280c43e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto @@ -0,0 +1,179 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3126474618911605, + "y": 5.567087650299072 + }, + "rotation": 5.013350972124652e-28 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.1" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.2" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.3" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.4" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 piece.auto b/src/main/deploy/pathplanner/autos/4 piece.auto new file mode 100644 index 0000000..5f7dcae --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 piece.auto @@ -0,0 +1,247 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3126474618911605, + "y": 5.567087650299072 + }, + "rotation": 5.013350972124652e-28 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.1" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.2" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.3" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.4" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "StopShooter" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.5" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece.6" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ec8d8f..8848ce4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Relay.Value; +import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -128,7 +128,7 @@ public RobotContainer() { NamedCommands.registerCommand("SpeakerAlign", new AprilTagVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("NoteAlign", new GamePieceVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("SpeakerSetting", - new SetScoringPosition(arm, shooter, new ScoringPosition(4, ShooterConstants.subwooferVelocity_rpm))); + new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); NamedCommands.registerCommand("LimelightSetting", new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); NamedCommands.registerCommand("WaitIntake", @@ -139,7 +139,8 @@ public RobotContainer() { NamedCommands.registerCommand("SlowForward", Commands.startEnd(() -> swerve.drive(new Translation2d(.3, 0), 0, false), () -> swerve.stop(), swerve) .withTimeout(1.5)); - NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0))); + NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, new ScoringPosition(4, 0))); + NamedCommands.registerCommand("StopShooter", new StopShooter(shooter)); // create paths final List autoNames = AutoBuilder.getAllAutoNames(); for (final String autoName : autoNames) { @@ -183,6 +184,7 @@ private void configureBindings() { () -> false, // no overdrive functionality // driverController.getHID()::getLeftBumper, // override speed () -> driverController.getLeftTriggerAxis() > OperatorConstants.boolTriggerThreshold, // precise mode + driverController.getHID()::getLeftBumper, // super precise mode swerve); swerve.setDefaultCommand(teleopSwerve); @@ -191,9 +193,9 @@ private void configureBindings() { // right bumper -> rotate to speaker apriltag driverController.rightBumper().whileTrue(new AprilTagVision(vision, swerve)); // left bumper -> rotate to note - driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve)); + // driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve)); driverController.a().onTrue(teleopSwerve.toggleFieldRelative()); - // b -> trap/climb align maybe? + driverController.b().onTrue(new SetShooterVelocity(shooter, 2250)); driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); @@ -206,15 +208,16 @@ private void configureBindings() { .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); - operatorController.x().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0))); + operatorController.x().onTrue( + new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0)).andThen(() -> intake.resetNoteDetection())); operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 40)); operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); - operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); - operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); - // operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); + operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kForward), arm)); + operatorController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kReverse), arm)); + // operatorController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kOff), arm)); operatorController.povRight().onTrue(Commands.runOnce(() -> intake.resetNoteDetection())); operatorController.start().toggleOnTrue(Commands.startEnd(() -> arm.setMotorVoltage(-3.7), () -> arm.stop(), arm)); @@ -260,9 +263,9 @@ private void configureBindings() { testController.leftStick().whileTrue(new SetIntakeSpeed(intake, -IntakeConstants.intakeSpeed, true)); testController.rightStick().whileTrue(new ManualArmControl(arm, testController::getLeftY)); - testController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kForward), arm)); - testController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kReverse), arm)); - testController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Value.kOff), arm)); + testController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kForward), arm)); + testController.povUp().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kReverse), arm)); + testController.povLeft().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kOff), arm)); } } diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index bbca74a..4eed487 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -6,7 +6,9 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Lights; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class IntakeWithArm extends Command { private Intake intake; @@ -35,8 +37,11 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - arm.setPosition(40); - intake.stopMotor(); + if (DriverStation.isAutonomous()) + arm.setPosition(4); + else + arm.setPosition(40); + Commands.waitSeconds(.02).andThen(intake::stopMotor, intake).schedule(); Lights.setDefaultStatus(); } diff --git a/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java b/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java index 017089d..a87268f 100644 --- a/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/swerve/TeleopSwerve.java @@ -71,12 +71,14 @@ public Command zeroYaw() { private DoubleSupplier turnSupplier; private BooleanSupplier overrideSpeedSupplier; private BooleanSupplier preciseModeSupplier; + private BooleanSupplier superPreciseModeSupplier; /** * @param overrideSpeedSupplier forces swerve to run at normal speed when held, instead of slow if scoring mechanism is out */ public TeleopSwerve(DoubleSupplier driveSupplier, DoubleSupplier strafeSupplier, DoubleSupplier turnSupplier, BooleanSupplier overrideSpeedSupplier, BooleanSupplier preciseModeSupplier, + BooleanSupplier superPreciseModeSupplier, Swerve swerve) { addRequirements(swerve); this.driveSupplier = driveSupplier; @@ -84,6 +86,7 @@ public TeleopSwerve(DoubleSupplier driveSupplier, DoubleSupplier strafeSupplier, this.turnSupplier = turnSupplier; this.overrideSpeedSupplier = overrideSpeedSupplier; this.preciseModeSupplier = preciseModeSupplier; + this.superPreciseModeSupplier = superPreciseModeSupplier; this.swerve = swerve; } @@ -112,7 +115,10 @@ public void execute() { turnMaxPercent = driveMax * 0.9; } - if (preciseModeSupplier.getAsBoolean()) { + if (superPreciseModeSupplier.getAsBoolean()) { + driveMax *= 0.1; + turnMaxPercent *= 0.15; + } else if (preciseModeSupplier.getAsBoolean()) { driveMax *= 0.3; turnMaxPercent *= 0.2; } diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 3e2c78e..9facd4c 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -4,7 +4,7 @@ public final class ArmConstants { public static final int leadMotorId = 29; public static final int followerMotorId = 30; - public static final double shaftEncoderOffset_deg = 4.22; // needs to be tuned + public static final double shaftEncoderOffset_deg = 14.97; // needs to be tuned // pid public static final double kP = .000095; diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index 6a21f78..b877bec 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .708), - new SwerveModuleConstants(23, 24, 1.763), - new SwerveModuleConstants(25, 26, 3.349), - new SwerveModuleConstants(27, 28, 2.838) + new SwerveModuleConstants(21, 22, .696), + new SwerveModuleConstants(23, 24, 1.904), + new SwerveModuleConstants(25, 26, 3.172), + new SwerveModuleConstants(27, 28, 2.864) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center @@ -42,9 +42,9 @@ public final class SwerveConstants { public static final double angleRampTime_s = 0.5; public static final boolean angleEncoderInverted = false; - public static final double maxVelocity_mps = 5650 /* NEO max RPM */ + public static final double maxVelocity_mps = 5676 /* NEO max RPM */ / 60.0 / driveGearRatio * wheelCircumference_m; - public static final double maxAngularVelocity_radps = 5650 /* NEO max RPM */ + public static final double maxAngularVelocity_radps = 5676 /* NEO max RPM */ / 60.0 / angleGearRatio / Math.hypot(trackWidth_m / 2.0, wheelBase_m / 2.0); // calculated with choreo, todo: see if empirical can match these diff --git a/src/main/java/frc/robot/io/FieldVisionIO.java b/src/main/java/frc/robot/io/FieldVisionIO.java index 030ed5b..5e187c5 100644 --- a/src/main/java/frc/robot/io/FieldVisionIO.java +++ b/src/main/java/frc/robot/io/FieldVisionIO.java @@ -56,8 +56,8 @@ public static class FieldVisionIOInputs { private NetworkTableEntry botpose_wpiblueEntry = table.getEntry("botpose_wpiblue"); private double[] botpose_wpiblueCache = new double[7]; - private LinearFilter targetSpaceXFilter = LinearFilter.movingAverage(40); - private LinearFilter targetSpaceZFilter = LinearFilter.movingAverage(40); + private LinearFilter targetSpaceXFilter = LinearFilter.singlePoleIIR(.1, .02); + private LinearFilter targetSpaceZFilter = LinearFilter.singlePoleIIR(.1, .02); public void updateInputs(FieldVisionIOInputs inputs) { inputs.pipeline = pipelineEntry.getNumber(inputs.pipeline).intValue(); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 0a837c1..4fe0052 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -53,8 +53,8 @@ public Arm() { leadMotor.setSoftLimit(SoftLimitDirection.kForward, (float) (110 + shaftEncoderOffset_deg)); leadMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) (-1.5 + shaftEncoderOffset_deg)); - leadMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - leadMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + leadMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + leadMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); followerMotor.restoreFactoryDefaults(); followerMotor.setIdleMode(IdleMode.kBrake); @@ -68,7 +68,7 @@ public Arm() { // rev / sec * sec / min = RPM shaftEncoder.setVelocityConversionFactor(60); shaftEncoder.setInverted(true); - shaftEncoder.setZeroOffset(0); + shaftEncoder.setZeroOffset(348.9); pidController.setFeedbackDevice(shaftEncoder); // Smart motion applies a velocity and acceleration limiter as it travels to the target position. More info can be diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 340bcca..5338dd8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -34,7 +34,8 @@ public Intake() { motor.enableVoltageCompensation(12); motor.burnFlash(); - Shuffleboard.getTab("drive").addBoolean("Holding Note?", () -> holdingNote).withWidget(BuiltInWidgets.kBooleanBox); + Shuffleboard.getTab("drive").addBoolean("Holding Note?", this::isHoldingNote) + .withWidget(BuiltInWidgets.kBooleanBox); } @@ -74,8 +75,9 @@ public void resetNoteDetection() { @AutoLog public static class IntakeIOInputs { public double current_A = 0.0; - public double voltage_V = 0.0; - public double tempature_C = 0.0; + public double busVoltage_V = 0.0; + public double appliedVoltage_V = 0.0; + public double temperature_C = 0.0; public double percentOutput = 0.0; public boolean noteSensor = false; } @@ -84,8 +86,9 @@ public static class IntakeIOInputs { public void updateInputs(IntakeIOInputs inputs) { inputs.current_A = motor.getOutputCurrent(); - inputs.voltage_V = motor.getBusVoltage(); - inputs.tempature_C = motor.getMotorTemperature(); + inputs.busVoltage_V = motor.getBusVoltage(); + inputs.appliedVoltage_V = motor.getBusVoltage() * motor.getAppliedOutput(); + inputs.temperature_C = motor.getMotorTemperature(); inputs.percentOutput = motor.getAppliedOutput(); inputs.noteSensor = !noteSensor.get(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 152b2cc..48eb8fe 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -257,26 +257,29 @@ public void periodic() { // todo: estimate without using gyro? // Every 0.02s, updating pose2d - if (vision.fieldInputs.hasTarget == true) { - visionSeenCount++; - if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh - var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, - vision.fieldInputs.fieldspaceTranslationY_m, - new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad) - .plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180))); // continuation of lines 259-261. Create a new - // pose2d - // using X,Y and rotation - Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit - var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position - Math.abs(translationX - vision.fieldInputs.fieldspaceTranslationX_m), - Math.abs(translationY - vision.fieldInputs.fieldspaceTranslationY_m)); - poseEstimator.addVisionMeasurement(visionPose, - timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); // remove lag from the time in the calculation of - // estimated pose - VecBuilder.fill(distance / 2, distance / 2, 100); - } - } else - visionSeenCount = 0; + if (!DriverStation.isAutonomous()) { + if (vision.fieldInputs.hasTarget == true) { + visionSeenCount++; + if (visionSeenCount > 2) { // The if statement is used to eliminate "hallucinations". Schizophrenic robot smh + var visionPose = new Pose2d(vision.fieldInputs.fieldspaceTranslationX_m, + vision.fieldInputs.fieldspaceTranslationY_m, + new Rotation2d(vision.fieldInputs.fieldspaceRotationX_rad) + .plus(Rotation2d.fromDegrees(Robot.isBlue() ? 0 : 180))); // continuation of lines 259-261. Create a new + // pose2d + // using X,Y and rotation + Logger.recordOutput("Odometry/VisionPose", visionPose); // log the odometry to advantage kit + var distance = Math.hypot( // use pythagorean theorem to find the distance from the last position + Math.abs(translationX - vision.fieldInputs.fieldspaceTranslationX_m), + Math.abs(translationY - vision.fieldInputs.fieldspaceTranslationY_m)); + poseEstimator.addVisionMeasurement(visionPose, + timer.get() - vision.fieldInputs.fieldspaceTotalLatency_s); // remove lag from the time in the calculation + // of + // estimated pose + VecBuilder.fill(distance / 2, distance / 2, 100); + } + } else + visionSeenCount = 0; + } Logger.recordOutput("Odometry/Robot", pose); // update field pose for (int i = 0; i < modules.length; i++) { From 3b8eff6b6d4ff802eec50b0671ed00d3148c8c99 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sun, 17 Mar 2024 17:07:22 -0400 Subject: [PATCH 17/22] final comp changes --- choreo.chor | 316 ++++++++---------- src/main/deploy/choreo/exit center.1.traj | 306 ++++++++--------- src/main/deploy/choreo/exit center.traj | 306 ++++++++--------- .../pathplanner/autos/MONTY PLAYOFF AUTO.auto | 61 ++++ src/main/java/frc/robot/RobotContainer.java | 5 +- .../robot/commands/intake/IntakeWithArm.java | 2 +- .../frc/robot/constants/SwerveConstants.java | 8 +- .../java/frc/robot/subsystems/Shooter.java | 12 +- 8 files changed, 488 insertions(+), 528 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MONTY PLAYOFF AUTO.auto diff --git a/choreo.chor b/choreo.chor index ed502c2..a5f266a 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1429,12 +1429,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 15 }, { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1446,181 +1446,145 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.674488653195789e-33, - "velocityY": 7.11171747202546e-38, + "angularVelocity": -2.2724863650268547e-35, + "velocityX": -4.1154428429404407e-35, + "velocityY": 3.7900808159397985e-34, "timestamp": 0 }, { - "x": 1.34929738878066, - "y": 5.5538054348907835, - "heading": 1.2355468400098194e-8, - "angularVelocity": 1.451370137183604e-7, - "velocityX": 0.4305187599139744, - "velocityY": -0.156023310052692, - "timestamp": 0.08512968609500099 - }, - { - "x": 1.4225972370219229, - "y": 5.527241006054132, - "heading": 4.5628523121809076e-8, - "angularVelocity": 3.908513740983071e-7, - "velocityX": 0.8610374547776837, - "velocityY": -0.31204659684762154, - "timestamp": 0.17025937219000198 - }, - { - "x": 1.5186402607278184, - "y": 5.492434275732478, - "heading": 0.023798969361148174, - "angularVelocity": 0.2795608068619741, - "velocityX": 1.1281966152056035, - "velocityY": -0.4088671287100916, - "timestamp": 0.25538905828500297 - }, - { - "x": 1.614683284388423, - "y": 5.457627545285846, - "heading": 0.08983587293589476, - "angularVelocity": 0.7757212155234814, - "velocityX": 1.1281966146735791, - "velocityY": -0.40886713017817483, - "timestamp": 0.34051874438000396 - }, - { - "x": 1.7107263080490587, - "y": 5.4228208148393, - "heading": 0.19002287752886737, - "angularVelocity": 1.1768750619045902, - "velocityX": 1.1281966146739437, - "velocityY": -0.4088671301771691, - "timestamp": 0.42564843047500495 - }, - { - "x": 1.806769331709714, - "y": 5.388014084392808, - "heading": 0.3166500634968596, - "angularVelocity": 1.4874621507082946, - "velocityX": 1.1281966146741744, - "velocityY": -0.40886713017653226, - "timestamp": 0.5107781165700059 - }, - { - "x": 1.902812355370287, - "y": 5.35320735394609, - "heading": 0.46249608510530194, - "angularVelocity": 1.7132216539091256, - "velocityX": 1.1281966146732083, - "velocityY": -0.4088671301791978, - "timestamp": 0.5959078026650069 - }, - { - "x": 1.9988553790306578, - "y": 5.318400623498813, - "heading": 0.6208292331181784, - "angularVelocity": 1.8599052254954127, - "velocityX": 1.1281966146708353, - "velocityY": -0.408867130185746, - "timestamp": 0.6810374887600079 - }, - { - "x": 2.094898402690861, - "y": 5.283593893051074, - "heading": 0.7853048024978768, - "angularVelocity": 1.9320589200358482, - "velocityX": 1.1281966146688651, - "velocityY": -0.40886713019118176, - "timestamp": 0.766167174855009 - }, - { - "x": 2.1909414263511025, - "y": 5.248787162603439, - "heading": 0.9497879513318085, - "angularVelocity": 1.9321479542444888, - "velocityX": 1.1281966146693132, - "velocityY": -0.40886713018994547, - "timestamp": 0.85129686095001 - }, - { - "x": 2.286984450011549, - "y": 5.213980432156372, - "heading": 1.108141013047214, - "angularVelocity": 1.8601391474495799, - "velocityX": 1.128196614671724, - "velocityY": -0.4088671301832935, - "timestamp": 0.936426547045011 - }, - { - "x": 2.383027473672174, - "y": 5.1791737017097965, - "heading": 1.254013432652853, - "angularVelocity": 1.7135317454695138, - "velocityX": 1.1281966146738198, - "velocityY": -0.40886713017751036, - "timestamp": 1.021556233140012 - }, - { - "x": 2.479070497332845, - "y": 5.144366971263349, - "heading": 1.3806693463444173, - "angularVelocity": 1.4877996090602523, - "velocityX": 1.1281966146743598, - "velocityY": -0.4088671301760209, - "timestamp": 1.1066859192350131 - }, - { - "x": 2.5751135209934812, - "y": 5.109560240816806, - "heading": 1.4808868727958657, - "angularVelocity": 1.1772335955709967, - "velocityX": 1.128196614673956, - "velocityY": -0.40886713017713466, - "timestamp": 1.1918156053300142 - }, - { - "x": 2.6711565446540857, - "y": 5.074753510370173, - "heading": 1.5469577993154264, - "angularVelocity": 0.776120875693452, - "velocityX": 1.1281966146735771, - "velocityY": -0.4088671301781801, - "timestamp": 1.2769452914250152 - }, - { - "x": 2.7671995683594357, - "y": 5.039946780047014, - "heading": 1.5707962811320204, - "angularVelocity": 0.2800254871137584, - "velocityX": 1.1281966151991965, - "velocityY": -0.4088671287277703, - "timestamp": 1.3620749775200163 - }, - { - "x": 2.840499416601011, - "y": 5.0133823512112246, - "heading": 1.5707963144274508, - "angularVelocity": 3.911142169044648e-7, - "velocityX": 0.8610374547813509, - "velocityY": -0.3120465968375025, - "timestamp": 1.4472046636150173 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.4527771000322412e-7, - "velocityX": 0.4305187599151969, - "velocityY": -0.15602331004931888, - "timestamp": 1.5323343497100184 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.8805605259344148e-32, - "velocityX": 2.677429684849837e-34, + "x": 1.3974808890848593, + "y": 5.5718669843118915, + "heading": 1.6886831292584428e-23, + "angularVelocity": 1.343613746238813e-22, + "velocityX": 0.6749835686447176, + "velocityY": 0.038027131927045286, + "timestamp": 0.1256822108485099 + }, + { + "x": 1.56714773476128, + "y": 5.581425651846773, + "heading": 1.4046959840458008e-22, + "angularVelocity": 9.832970942125023e-22, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 0.2513644216970198 + }, + { + "x": 1.793017544746431, + "y": 5.594150674294191, + "heading": -3.995232059088626e-18, + "angularVelocity": -3.1789483061159005e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 0.37704663254552967 + }, + { + "x": 2.018887354731584, + "y": 5.606875696741609, + "heading": -6.336394833696792e-18, + "angularVelocity": -1.862763831462549e-17, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 0.5027288433940396 + }, + { + "x": 2.2447571647167366, + "y": 5.619600719189027, + "heading": -7.40929128523623e-18, + "angularVelocity": -8.53658157126495e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 0.6284110542425494 + }, + { + "x": 2.470626974701889, + "y": 5.6323257416364445, + "heading": -7.546116554341213e-18, + "angularVelocity": -1.0886605307842563e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.7540932650910592 + }, + { + "x": 2.6964967846870422, + "y": 5.645050764083862, + "heading": -7.077194412954045e-18, + "angularVelocity": 3.731014474233488e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.8797754759395691 + }, + { + "x": 2.922366594672195, + "y": 5.65777578653128, + "heading": -6.335427536393661e-18, + "angularVelocity": 5.901924190789222e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 1.005457686788079 + }, + { + "x": 3.148236404657348, + "y": 5.670500808978698, + "heading": -5.577961733214196e-18, + "angularVelocity": 6.026833854485249e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 1.1311398976365887 + }, + { + "x": 3.3741062146425005, + "y": 5.683225831426116, + "heading": -4.92423321292289e-18, + "angularVelocity": 5.201440295839972e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023421, + "timestamp": 1.2568221084850986 + }, + { + "x": 3.5999760246276535, + "y": 5.6959508538735335, + "heading": -4.343100343450651e-18, + "angularVelocity": 4.623827478949564e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 1.3825043193336084 + }, + { + "x": 3.8258458346128044, + "y": 5.708675876320951, + "heading": -5.8736919683700795e-18, + "angularVelocity": -1.217826792536569e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 1.5081865301821182 + }, + { + "x": 3.995512680289225, + "y": 5.718234543855833, + "heading": 2.728574226956978e-22, + "angularVelocity": 4.6736644461091284e-17, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 1.633868741030628 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -2.4781811920112445e-36, + "angularVelocity": -2.171010658203156e-21, + "velocityX": 0.6749835686447176, + "velocityY": 0.03802713192704529, + "timestamp": 1.759550951879138 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -1.5575812221940043e-36, + "angularVelocity": 7.324824341726948e-36, + "velocityX": -1.625065332502054e-34, "velocityY": 0, - "timestamp": 1.6174640358050194 + "timestamp": 1.8852331627276477 } ], "constraints": [ @@ -1642,7 +1606,7 @@ "last" ], "type": "MaxVelocity", - "velocity": 1.2 + "velocity": 1.8 } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/exit center.1.traj b/src/main/deploy/choreo/exit center.1.traj index 62557fd..445fea9 100644 --- a/src/main/deploy/choreo/exit center.1.traj +++ b/src/main/deploy/choreo/exit center.1.traj @@ -4,181 +4,145 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.674488653195789e-33, - "velocityY": 7.11171747202546e-38, + "angularVelocity": -2.2724863650268547e-35, + "velocityX": -4.1154428429404407e-35, + "velocityY": 3.7900808159397985e-34, "timestamp": 0 }, { - "x": 1.34929738878066, - "y": 5.5538054348907835, - "heading": 1.2355468400098194e-8, - "angularVelocity": 1.451370137183604e-7, - "velocityX": 0.4305187599139744, - "velocityY": -0.156023310052692, - "timestamp": 0.08512968609500099 - }, - { - "x": 1.4225972370219229, - "y": 5.527241006054132, - "heading": 4.5628523121809076e-8, - "angularVelocity": 3.908513740983071e-7, - "velocityX": 0.8610374547776837, - "velocityY": -0.31204659684762154, - "timestamp": 0.17025937219000198 - }, - { - "x": 1.5186402607278184, - "y": 5.492434275732478, - "heading": 0.023798969361148174, - "angularVelocity": 0.2795608068619741, - "velocityX": 1.1281966152056035, - "velocityY": -0.4088671287100916, - "timestamp": 0.25538905828500297 - }, - { - "x": 1.614683284388423, - "y": 5.457627545285846, - "heading": 0.08983587293589476, - "angularVelocity": 0.7757212155234814, - "velocityX": 1.1281966146735791, - "velocityY": -0.40886713017817483, - "timestamp": 0.34051874438000396 - }, - { - "x": 1.7107263080490587, - "y": 5.4228208148393, - "heading": 0.19002287752886737, - "angularVelocity": 1.1768750619045902, - "velocityX": 1.1281966146739437, - "velocityY": -0.4088671301771691, - "timestamp": 0.42564843047500495 - }, - { - "x": 1.806769331709714, - "y": 5.388014084392808, - "heading": 0.3166500634968596, - "angularVelocity": 1.4874621507082946, - "velocityX": 1.1281966146741744, - "velocityY": -0.40886713017653226, - "timestamp": 0.5107781165700059 - }, - { - "x": 1.902812355370287, - "y": 5.35320735394609, - "heading": 0.46249608510530194, - "angularVelocity": 1.7132216539091256, - "velocityX": 1.1281966146732083, - "velocityY": -0.4088671301791978, - "timestamp": 0.5959078026650069 - }, - { - "x": 1.9988553790306578, - "y": 5.318400623498813, - "heading": 0.6208292331181784, - "angularVelocity": 1.8599052254954127, - "velocityX": 1.1281966146708353, - "velocityY": -0.408867130185746, - "timestamp": 0.6810374887600079 - }, - { - "x": 2.094898402690861, - "y": 5.283593893051074, - "heading": 0.7853048024978768, - "angularVelocity": 1.9320589200358482, - "velocityX": 1.1281966146688651, - "velocityY": -0.40886713019118176, - "timestamp": 0.766167174855009 - }, - { - "x": 2.1909414263511025, - "y": 5.248787162603439, - "heading": 0.9497879513318085, - "angularVelocity": 1.9321479542444888, - "velocityX": 1.1281966146693132, - "velocityY": -0.40886713018994547, - "timestamp": 0.85129686095001 - }, - { - "x": 2.286984450011549, - "y": 5.213980432156372, - "heading": 1.108141013047214, - "angularVelocity": 1.8601391474495799, - "velocityX": 1.128196614671724, - "velocityY": -0.4088671301832935, - "timestamp": 0.936426547045011 - }, - { - "x": 2.383027473672174, - "y": 5.1791737017097965, - "heading": 1.254013432652853, - "angularVelocity": 1.7135317454695138, - "velocityX": 1.1281966146738198, - "velocityY": -0.40886713017751036, - "timestamp": 1.021556233140012 - }, - { - "x": 2.479070497332845, - "y": 5.144366971263349, - "heading": 1.3806693463444173, - "angularVelocity": 1.4877996090602523, - "velocityX": 1.1281966146743598, - "velocityY": -0.4088671301760209, - "timestamp": 1.1066859192350131 - }, - { - "x": 2.5751135209934812, - "y": 5.109560240816806, - "heading": 1.4808868727958657, - "angularVelocity": 1.1772335955709967, - "velocityX": 1.128196614673956, - "velocityY": -0.40886713017713466, - "timestamp": 1.1918156053300142 - }, - { - "x": 2.6711565446540857, - "y": 5.074753510370173, - "heading": 1.5469577993154264, - "angularVelocity": 0.776120875693452, - "velocityX": 1.1281966146735771, - "velocityY": -0.4088671301781801, - "timestamp": 1.2769452914250152 - }, - { - "x": 2.7671995683594357, - "y": 5.039946780047014, - "heading": 1.5707962811320204, - "angularVelocity": 0.2800254871137584, - "velocityX": 1.1281966151991965, - "velocityY": -0.4088671287277703, - "timestamp": 1.3620749775200163 - }, - { - "x": 2.840499416601011, - "y": 5.0133823512112246, - "heading": 1.5707963144274508, - "angularVelocity": 3.911142169044648e-7, - "velocityX": 0.8610374547813509, - "velocityY": -0.3120465968375025, - "timestamp": 1.4472046636150173 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.4527771000322412e-7, - "velocityX": 0.4305187599151969, - "velocityY": -0.15602331004931888, - "timestamp": 1.5323343497100184 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.8805605259344148e-32, - "velocityX": 2.677429684849837e-34, + "x": 1.3974808890848593, + "y": 5.5718669843118915, + "heading": 1.6886831292584428e-23, + "angularVelocity": 1.343613746238813e-22, + "velocityX": 0.6749835686447176, + "velocityY": 0.038027131927045286, + "timestamp": 0.1256822108485099 + }, + { + "x": 1.56714773476128, + "y": 5.581425651846773, + "heading": 1.4046959840458008e-22, + "angularVelocity": 9.832970942125023e-22, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 0.2513644216970198 + }, + { + "x": 1.793017544746431, + "y": 5.594150674294191, + "heading": -3.995232059088626e-18, + "angularVelocity": -3.1789483061159005e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 0.37704663254552967 + }, + { + "x": 2.018887354731584, + "y": 5.606875696741609, + "heading": -6.336394833696792e-18, + "angularVelocity": -1.862763831462549e-17, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 0.5027288433940396 + }, + { + "x": 2.2447571647167366, + "y": 5.619600719189027, + "heading": -7.40929128523623e-18, + "angularVelocity": -8.53658157126495e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 0.6284110542425494 + }, + { + "x": 2.470626974701889, + "y": 5.6323257416364445, + "heading": -7.546116554341213e-18, + "angularVelocity": -1.0886605307842563e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.7540932650910592 + }, + { + "x": 2.6964967846870422, + "y": 5.645050764083862, + "heading": -7.077194412954045e-18, + "angularVelocity": 3.731014474233488e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.8797754759395691 + }, + { + "x": 2.922366594672195, + "y": 5.65777578653128, + "heading": -6.335427536393661e-18, + "angularVelocity": 5.901924190789222e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 1.005457686788079 + }, + { + "x": 3.148236404657348, + "y": 5.670500808978698, + "heading": -5.577961733214196e-18, + "angularVelocity": 6.026833854485249e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 1.1311398976365887 + }, + { + "x": 3.3741062146425005, + "y": 5.683225831426116, + "heading": -4.92423321292289e-18, + "angularVelocity": 5.201440295839972e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023421, + "timestamp": 1.2568221084850986 + }, + { + "x": 3.5999760246276535, + "y": 5.6959508538735335, + "heading": -4.343100343450651e-18, + "angularVelocity": 4.623827478949564e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 1.3825043193336084 + }, + { + "x": 3.8258458346128044, + "y": 5.708675876320951, + "heading": -5.8736919683700795e-18, + "angularVelocity": -1.217826792536569e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 1.5081865301821182 + }, + { + "x": 3.995512680289225, + "y": 5.718234543855833, + "heading": 2.728574226956978e-22, + "angularVelocity": 4.6736644461091284e-17, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 1.633868741030628 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -2.4781811920112445e-36, + "angularVelocity": -2.171010658203156e-21, + "velocityX": 0.6749835686447176, + "velocityY": 0.03802713192704529, + "timestamp": 1.759550951879138 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -1.5575812221940043e-36, + "angularVelocity": 7.324824341726948e-36, + "velocityX": -1.625065332502054e-34, "velocityY": 0, - "timestamp": 1.6174640358050194 + "timestamp": 1.8852331627276477 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/exit center.traj b/src/main/deploy/choreo/exit center.traj index 62557fd..445fea9 100644 --- a/src/main/deploy/choreo/exit center.traj +++ b/src/main/deploy/choreo/exit center.traj @@ -4,181 +4,145 @@ "x": 1.3126474618911743, "y": 5.567087650299072, "heading": 0, - "angularVelocity": 0, - "velocityX": 1.674488653195789e-33, - "velocityY": 7.11171747202546e-38, + "angularVelocity": -2.2724863650268547e-35, + "velocityX": -4.1154428429404407e-35, + "velocityY": 3.7900808159397985e-34, "timestamp": 0 }, { - "x": 1.34929738878066, - "y": 5.5538054348907835, - "heading": 1.2355468400098194e-8, - "angularVelocity": 1.451370137183604e-7, - "velocityX": 0.4305187599139744, - "velocityY": -0.156023310052692, - "timestamp": 0.08512968609500099 - }, - { - "x": 1.4225972370219229, - "y": 5.527241006054132, - "heading": 4.5628523121809076e-8, - "angularVelocity": 3.908513740983071e-7, - "velocityX": 0.8610374547776837, - "velocityY": -0.31204659684762154, - "timestamp": 0.17025937219000198 - }, - { - "x": 1.5186402607278184, - "y": 5.492434275732478, - "heading": 0.023798969361148174, - "angularVelocity": 0.2795608068619741, - "velocityX": 1.1281966152056035, - "velocityY": -0.4088671287100916, - "timestamp": 0.25538905828500297 - }, - { - "x": 1.614683284388423, - "y": 5.457627545285846, - "heading": 0.08983587293589476, - "angularVelocity": 0.7757212155234814, - "velocityX": 1.1281966146735791, - "velocityY": -0.40886713017817483, - "timestamp": 0.34051874438000396 - }, - { - "x": 1.7107263080490587, - "y": 5.4228208148393, - "heading": 0.19002287752886737, - "angularVelocity": 1.1768750619045902, - "velocityX": 1.1281966146739437, - "velocityY": -0.4088671301771691, - "timestamp": 0.42564843047500495 - }, - { - "x": 1.806769331709714, - "y": 5.388014084392808, - "heading": 0.3166500634968596, - "angularVelocity": 1.4874621507082946, - "velocityX": 1.1281966146741744, - "velocityY": -0.40886713017653226, - "timestamp": 0.5107781165700059 - }, - { - "x": 1.902812355370287, - "y": 5.35320735394609, - "heading": 0.46249608510530194, - "angularVelocity": 1.7132216539091256, - "velocityX": 1.1281966146732083, - "velocityY": -0.4088671301791978, - "timestamp": 0.5959078026650069 - }, - { - "x": 1.9988553790306578, - "y": 5.318400623498813, - "heading": 0.6208292331181784, - "angularVelocity": 1.8599052254954127, - "velocityX": 1.1281966146708353, - "velocityY": -0.408867130185746, - "timestamp": 0.6810374887600079 - }, - { - "x": 2.094898402690861, - "y": 5.283593893051074, - "heading": 0.7853048024978768, - "angularVelocity": 1.9320589200358482, - "velocityX": 1.1281966146688651, - "velocityY": -0.40886713019118176, - "timestamp": 0.766167174855009 - }, - { - "x": 2.1909414263511025, - "y": 5.248787162603439, - "heading": 0.9497879513318085, - "angularVelocity": 1.9321479542444888, - "velocityX": 1.1281966146693132, - "velocityY": -0.40886713018994547, - "timestamp": 0.85129686095001 - }, - { - "x": 2.286984450011549, - "y": 5.213980432156372, - "heading": 1.108141013047214, - "angularVelocity": 1.8601391474495799, - "velocityX": 1.128196614671724, - "velocityY": -0.4088671301832935, - "timestamp": 0.936426547045011 - }, - { - "x": 2.383027473672174, - "y": 5.1791737017097965, - "heading": 1.254013432652853, - "angularVelocity": 1.7135317454695138, - "velocityX": 1.1281966146738198, - "velocityY": -0.40886713017751036, - "timestamp": 1.021556233140012 - }, - { - "x": 2.479070497332845, - "y": 5.144366971263349, - "heading": 1.3806693463444173, - "angularVelocity": 1.4877996090602523, - "velocityX": 1.1281966146743598, - "velocityY": -0.4088671301760209, - "timestamp": 1.1066859192350131 - }, - { - "x": 2.5751135209934812, - "y": 5.109560240816806, - "heading": 1.4808868727958657, - "angularVelocity": 1.1772335955709967, - "velocityX": 1.128196614673956, - "velocityY": -0.40886713017713466, - "timestamp": 1.1918156053300142 - }, - { - "x": 2.6711565446540857, - "y": 5.074753510370173, - "heading": 1.5469577993154264, - "angularVelocity": 0.776120875693452, - "velocityX": 1.1281966146735771, - "velocityY": -0.4088671301781801, - "timestamp": 1.2769452914250152 - }, - { - "x": 2.7671995683594357, - "y": 5.039946780047014, - "heading": 1.5707962811320204, - "angularVelocity": 0.2800254871137584, - "velocityX": 1.1281966151991965, - "velocityY": -0.4088671287277703, - "timestamp": 1.3620749775200163 - }, - { - "x": 2.840499416601011, - "y": 5.0133823512112246, - "heading": 1.5707963144274508, - "angularVelocity": 3.911142169044648e-7, - "velocityX": 0.8610374547813509, - "velocityY": -0.3120465968375025, - "timestamp": 1.4472046636150173 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.4527771000322412e-7, - "velocityX": 0.4305187599151969, - "velocityY": -0.15602331004931888, - "timestamp": 1.5323343497100184 - }, - { - "x": 2.8771493434906006, - "y": 5.000100135803223, - "heading": 1.5707963267948966, - "angularVelocity": 1.8805605259344148e-32, - "velocityX": 2.677429684849837e-34, + "x": 1.3974808890848593, + "y": 5.5718669843118915, + "heading": 1.6886831292584428e-23, + "angularVelocity": 1.343613746238813e-22, + "velocityX": 0.6749835686447176, + "velocityY": 0.038027131927045286, + "timestamp": 0.1256822108485099 + }, + { + "x": 1.56714773476128, + "y": 5.581425651846773, + "heading": 1.4046959840458008e-22, + "angularVelocity": 9.832970942125023e-22, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 0.2513644216970198 + }, + { + "x": 1.793017544746431, + "y": 5.594150674294191, + "heading": -3.995232059088626e-18, + "angularVelocity": -3.1789483061159005e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 0.37704663254552967 + }, + { + "x": 2.018887354731584, + "y": 5.606875696741609, + "heading": -6.336394833696792e-18, + "angularVelocity": -1.862763831462549e-17, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 0.5027288433940396 + }, + { + "x": 2.2447571647167366, + "y": 5.619600719189027, + "heading": -7.40929128523623e-18, + "angularVelocity": -8.53658157126495e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 0.6284110542425494 + }, + { + "x": 2.470626974701889, + "y": 5.6323257416364445, + "heading": -7.546116554341213e-18, + "angularVelocity": -1.0886605307842563e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.7540932650910592 + }, + { + "x": 2.6964967846870422, + "y": 5.645050764083862, + "heading": -7.077194412954045e-18, + "angularVelocity": 3.731014474233488e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 0.8797754759395691 + }, + { + "x": 2.922366594672195, + "y": 5.65777578653128, + "heading": -6.335427536393661e-18, + "angularVelocity": 5.901924190789222e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.1012476018802342, + "timestamp": 1.005457686788079 + }, + { + "x": 3.148236404657348, + "y": 5.670500808978698, + "heading": -5.577961733214196e-18, + "angularVelocity": 6.026833854485249e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023423, + "timestamp": 1.1311398976365887 + }, + { + "x": 3.3741062146425005, + "y": 5.683225831426116, + "heading": -4.92423321292289e-18, + "angularVelocity": 5.201440295839972e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023421, + "timestamp": 1.2568221084850986 + }, + { + "x": 3.5999760246276535, + "y": 5.6959508538735335, + "heading": -4.343100343450651e-18, + "angularVelocity": 4.623827478949564e-18, + "velocityX": 1.79715019699489, + "velocityY": 0.10124760188023418, + "timestamp": 1.3825043193336084 + }, + { + "x": 3.8258458346128044, + "y": 5.708675876320951, + "heading": -5.8736919683700795e-18, + "angularVelocity": -1.217826792536569e-17, + "velocityX": 1.797150196994875, + "velocityY": 0.10124760188023336, + "timestamp": 1.5081865301821182 + }, + { + "x": 3.995512680289225, + "y": 5.718234543855833, + "heading": 2.728574226956978e-22, + "angularVelocity": 4.6736644461091284e-17, + "velocityX": 1.34996706798011, + "velocityY": 0.07605425994935128, + "timestamp": 1.633868741030628 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -2.4781811920112445e-36, + "angularVelocity": -2.171010658203156e-21, + "velocityX": 0.6749835686447176, + "velocityY": 0.03802713192704529, + "timestamp": 1.759550951879138 + }, + { + "x": 4.08034610748291, + "y": 5.723013877868652, + "heading": -1.5575812221940043e-36, + "angularVelocity": 7.324824341726948e-36, + "velocityX": -1.625065332502054e-34, "velocityY": 0, - "timestamp": 1.6174640358050194 + "timestamp": 1.8852331627276477 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MONTY PLAYOFF AUTO.auto b/src/main/deploy/pathplanner/autos/MONTY PLAYOFF AUTO.auto new file mode 100644 index 0000000..046bc8d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MONTY PLAYOFF AUTO.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3126474618911743, + "y": 5.567087650299072 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Options" + } + }, + { + "type": "named", + "data": { + "name": "SpeakerSetting" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Index" + } + }, + { + "type": "named", + "data": { + "name": "ResetScoring" + } + }, + { + "type": "named", + "data": { + "name": "WaitUntilEnd" + } + }, + { + "type": "path", + "data": { + "pathName": "exit center.1" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8848ce4..7a7438c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -120,7 +120,7 @@ public RobotContainer() { Command shuffleboardAutoOptions = Commands.parallel( Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0), - Commands.runOnce(() -> intake.resetNoteDetection(true))); + Commands.runOnce(() -> intake.resetNoteDetection(noteStartEntry.getBoolean(false)))); // named commands must be registered before any paths are created NamedCommands.registerCommand("Options", shuffleboardAutoOptions); @@ -131,6 +131,7 @@ public RobotContainer() { new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); NamedCommands.registerCommand("LimelightSetting", new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); + NamedCommands.registerCommand("WaitUntilEnd", Commands.idle().until(() -> DriverStation.getMatchTime() <= 2)); NamedCommands.registerCommand("WaitIntake", new IntakeWithArm(intake, arm).beforeStarting(() -> intake.resetNoteDetection(), intake)); NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(1)); @@ -210,7 +211,7 @@ private void configureBindings() { .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(-1.5, ShooterConstants.subwooferVelocity_rpm))); operatorController.x().onTrue( new SetScoringPosition(arm, shooter, new ScoringPosition(40, 0)).andThen(() -> intake.resetNoteDetection())); - operatorController.y().onTrue(new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); + operatorController.y().onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(1, 2950))); operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 40)); operatorController.leftTrigger(boolTriggerThreshold).whileTrue(new IntakeWithArm(intake, arm)); diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index 4eed487..455a2dd 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -41,7 +41,7 @@ public void end(boolean interrupted) { arm.setPosition(4); else arm.setPosition(40); - Commands.waitSeconds(.02).andThen(intake::stopMotor, intake).schedule(); + Commands.waitSeconds(.015).andThen(intake::stopMotor, intake).schedule(); Lights.setDefaultStatus(); } diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index b877bec..26cbcba 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .696), - new SwerveModuleConstants(23, 24, 1.904), - new SwerveModuleConstants(25, 26, 3.172), - new SwerveModuleConstants(27, 28, 2.864) + new SwerveModuleConstants(21, 22, .723), + new SwerveModuleConstants(23, 24, 1.914), + new SwerveModuleConstants(25, 26, 3.073), + new SwerveModuleConstants(27, 28, 2.859) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index b156671..879791a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,14 +1,16 @@ package frc.robot.subsystems; -import static frc.robot.constants.Constants.tuningMode; import static frc.robot.constants.ShooterConstants.*; +import frc.robot.constants.Constants; import frc.robot.util.AutoSetterTunableNumber; import frc.robot.util.SparkUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -38,13 +40,17 @@ public class Shooter extends SubsystemBase { public Shooter() { leadMotor.restoreFactoryDefaults(); followerMotor.restoreFactoryDefaults(); - if (tuningMode) { + if (Constants.tuningMode) { new AutoSetterTunableNumber("Shooter/kP", kP, (double value) -> pidController.setP(value)); new AutoSetterTunableNumber("Shooter/kD", kD, (double value) -> pidController.setD(value)); new AutoSetterTunableNumber("Shooter/setpoint", 0, (double value) -> setVelocity(value)); } + Shuffleboard.getTab("drive") + .addBoolean("Shooter at Target Speed?", () -> MathUtil.isNear(targetVelocity_rpm, inputs.leadVelocity_rpm, 50)) + .withWidget(BuiltInWidgets.kBooleanBox); + pidController.setP(kP); pidController.setI(0); pidController.setD(kD); @@ -57,7 +63,7 @@ public Shooter() { followerMotor.follow(leadMotor); // only fetch position when tuning - SparkUtil.setPeriodicFrames(leadMotor, true, true, tuningMode, false, false, false, false); + SparkUtil.setPeriodicFrames(leadMotor, true, true, Constants.tuningMode, false, false, false, false); SparkUtil.setPeriodicFrames(followerMotor, false, false, false, false, false, false, false); leadMotor.burnFlash(); followerMotor.burnFlash(); From 1f56dbb95f51b992743173e579a496987be25667 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Mon, 18 Mar 2024 16:17:19 -0400 Subject: [PATCH 18/22] removed some testing changes that shouldn't have stayed in --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/subsystems/Arm.java | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a7438c..4a9ca69 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -196,7 +196,6 @@ private void configureBindings() { // left bumper -> rotate to note // driverController.leftBumper().whileTrue(new GamePieceVision(vision, swerve)); driverController.a().onTrue(teleopSwerve.toggleFieldRelative()); - driverController.b().onTrue(new SetShooterVelocity(shooter, 2250)); driverController.x().whileTrue(new XStance(swerve)); driverController.y().onTrue(teleopSwerve.zeroYaw()); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 4fe0052..4a658da 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -53,8 +53,8 @@ public Arm() { leadMotor.setSoftLimit(SoftLimitDirection.kForward, (float) (110 + shaftEncoderOffset_deg)); leadMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) (-1.5 + shaftEncoderOffset_deg)); - leadMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - leadMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); + leadMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + leadMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); followerMotor.restoreFactoryDefaults(); followerMotor.setIdleMode(IdleMode.kBrake); From 00ecfa8d8abac16b7a21d3f2f1379db4fa39b202 Mon Sep 17 00:00:00 2001 From: RushaliB Date: Mon, 18 Mar 2024 20:47:24 -0400 Subject: [PATCH 19/22] switched lights order + individual bit logging --- src/main/java/frc/robot/subsystems/Lights.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index bfaeb6d..5c9ec0e 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; +import frc.robot.constants.Constants; import frc.robot.constants.LightsConstants.Mode; import edu.wpi.first.wpilibj.DigitalOutput; @@ -14,10 +15,10 @@ public class Lights { private static DigitalOutput[] pins = { - new DigitalOutput(4), - new DigitalOutput(3), + new DigitalOutput(1), new DigitalOutput(2), - new DigitalOutput(1) + new DigitalOutput(3), + new DigitalOutput(4) }; public Lights() {} @@ -26,6 +27,8 @@ public static void setStatus(Mode mode) { int code = mode.getCode(); BitSet bits = BitSet.valueOf(new byte[] {(byte) code}); for (int i = 0; i <= 3; i++) { + if (Constants.tuningMode) + Logger.recordOutput("Lights/bits_" + i, bits.get(i) ? 1 : 0); pins[i].set(bits.get(i)); } From f1865109c196ba4132ce085d1082843f6da1bbe1 Mon Sep 17 00:00:00 2001 From: RushaliB Date: Mon, 18 Mar 2024 20:49:10 -0400 Subject: [PATCH 20/22] comments for DIO -> leo pin connections --- src/main/java/frc/robot/subsystems/Lights.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index 5c9ec0e..b433870 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -15,10 +15,10 @@ public class Lights { private static DigitalOutput[] pins = { - new DigitalOutput(1), - new DigitalOutput(2), - new DigitalOutput(3), - new DigitalOutput(4) + new DigitalOutput(1), // 13 + new DigitalOutput(2), // 12 + new DigitalOutput(3), // 11 + new DigitalOutput(4) // 10 }; public Lights() {} From 6cda2ef9edd3458fb377ae6d695d310ecde56b13 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Mon, 18 Mar 2024 21:10:53 -0400 Subject: [PATCH 21/22] rumble operator once we spin up shooter --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++++++ src/main/java/frc/robot/subsystems/Shooter.java | 8 +++++++- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c01eb39..3de4324 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -199,7 +199,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - + robotContainer.checkRumble(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a9ca69..06662f5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper; @@ -45,6 +46,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -297,4 +299,16 @@ public void updateOIAlert() { driverControllerAlert.set(!driverController.getHID().isConnected() || driverController.getHID().getName().indexOf("Xbox") < 0); } + + private boolean cachedShooterAtTarget = false; + + public void checkRumble() { + if (shooter.isAtTargetVelocity() && (shooter.isAtTargetVelocity() != cachedShooterAtTarget)) { + if (shooter.getTargetVelocity_rpm() > 400) { + Commands.startEnd(() -> operatorController.getHID().setRumble(RumbleType.kBothRumble, .7), + () -> operatorController.getHID().setRumble(RumbleType.kBothRumble, 0), new Subsystem[] {}).withTimeout(.5) + .schedule(); + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 879791a..c1faa1e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -37,6 +37,10 @@ public class Shooter extends SubsystemBase { @AutoLogOutput private double targetVelocity_rpm = 0.0; + @Getter + @AutoLogOutput + private boolean atTargetVelocity = false; + public Shooter() { leadMotor.restoreFactoryDefaults(); followerMotor.restoreFactoryDefaults(); @@ -48,7 +52,7 @@ public Shooter() { } Shuffleboard.getTab("drive") - .addBoolean("Shooter at Target Speed?", () -> MathUtil.isNear(targetVelocity_rpm, inputs.leadVelocity_rpm, 50)) + .addBoolean("Shooter at Target Speed?", this::isAtTargetVelocity) .withWidget(BuiltInWidgets.kBooleanBox); pidController.setP(kP); @@ -86,6 +90,8 @@ public void stop() { public void periodic() { updateInputs(inputs); Logger.processInputs(getName(), inputs); + + atTargetVelocity = MathUtil.isNear(targetVelocity_rpm, inputs.leadVelocity_rpm, 50); } @AutoLog From 320303977dd27f85b9cc28d679ff2be475934939 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Tue, 19 Mar 2024 20:37:14 -0400 Subject: [PATCH 22/22] updated to wpilib 2024.3.2 and library updates --- build.gradle | 4 +- vendordeps/AdvantageKit.json | 80 +++++++++++++++++----------------- vendordeps/PathplannerLib.json | 6 +-- vendordeps/REVLib.json | 10 ++--- 4 files changed, 50 insertions(+), 50 deletions(-) diff --git a/build.gradle b/build.gradle index 2934eac..9115f28 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "com.peterabeles.gversion" version "1.10" - id "io.freefair.lombok" version "8.4" + id "io.freefair.lombok" version "8.6" } java { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 44ae357..d4bf7dd 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,42 +1,42 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "3.0.2", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2024", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "3.0.2" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "3.0.2" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "3.0.2" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "3.0.2", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.2.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.2.0" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.0" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.2.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] } \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3ec4c12..a019706 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.6", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.6" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.6", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf8..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,