From f7f50cb4403dd60a033190ebcc39bbdd7c538f30 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 13:06:27 -0500 Subject: [PATCH 01/24] Port note vision from retep nr/note-detection --- .../com/stuypulse/robot/RobotContainer.java | 2 + .../swerve/SwerveDriveDriveToNote.java | 70 ++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 39 ++++++++- .../notevision/AbstractNoteVision.java | 28 +++++++ .../subsystems/notevision/NoteVision.java | 77 ++++++++++++++++++ .../com/stuypulse/robot/util/Limelight.java | 81 +++++++++++++++++++ 6 files changed, 294 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/util/Limelight.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6f13a61..c783f80 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.intake.*; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -36,6 +37,7 @@ public class RobotContainer { public final AbstractOdometry odometry = AbstractOdometry.getInstance(); public final AbstractVision vision = AbstractVision.getInstance(); public final AbstractIntake intake = AbstractIntake.getInstance(); + public final AbstractNoteVision noteVision = AbstractNoteVision.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java new file mode 100644 index 0000000..bafda64 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -0,0 +1,70 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + // Subsystems + private final SwerveDrive swerve; + private final AbstractNoteVision vision; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.vision = AbstractNoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Vision/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + double noteDistance = vision.getDistanceToNote(); + Rotation2d noteRotation = vision.getRotationToNote(); + + Pose2d targetPose = new Pose2d(TARGET_NOTE_DISTANCE.get(), 0, new Rotation2d()); + Pose2d currentPose = new Pose2d(noteDistance * noteRotation.getCos(), noteDistance * noteRotation.getSin(), noteRotation); + + swerve.setChassisSpeeds(controller.update(targetPose, currentPose)); + SmartDashboard.putBoolean("Vision/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8548fe9..4778271 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -10,6 +10,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -148,12 +149,44 @@ public interface GyroFeedback { } + public interface Intake { + SmartNumber ACQUIRE_SPEED = new SmartNumber("Intake/Acquire Speed Bottom", 1); + SmartNumber DEACQUIRE_SPEED = new SmartNumber("Intake/Deacquire Speed Bottom", -1); + } + + + public interface Limelight { + String [] LIMELIGHTS = { + "limelight" + }; + + int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; + Pose3d [] POSITIONS = new Pose3d[] { + new Pose3d() // TODO: determine these values when Limelight is put on + }; + } + public static Vector2D vpow(Vector2D vec, double power) { return vec.mul(Math.pow(vec.magnitude(), power - 1)); } - public interface Intake { - SmartNumber ACQUIRE_SPEED = new SmartNumber("Intake/Acquire Speed Bottom", 1); - SmartNumber DEACQUIRE_SPEED = new SmartNumber("Intake/Deacquire Speed Bottom", -1); + public interface NoteDetection { + SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); + SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); + + SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); + SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", 0.1); + SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 0.1); + + public interface Translation { + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 1); + SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0); + SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0); + } + public interface Rotation { + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 1); + SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0); + } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java new file mode 100644 index 0000000..e66f681 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -0,0 +1,28 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class AbstractNoteVision extends SubsystemBase { + + /** SINGLETON **/ + private static final AbstractNoteVision instance; + + static { + instance = new NoteVision(); + } + + public static AbstractNoteVision getInstance() { + return instance; + } + + public abstract double getDistanceToNote(); + public abstract Rotation2d getRotationToNote(); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java new file mode 100644 index 0000000..c38140a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -0,0 +1,77 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + +import static com.stuypulse.robot.constants.Settings.Limelight.*; + +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.util.Limelight; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class NoteVision extends AbstractNoteVision { + + // store limelight network tables + private final Limelight[] limelights; + + // store fieldobject2d's to display limelight data + private final FieldObject2d[] limelightPoses; + + protected NoteVision() { + // reference to all limelights on robot + String[] hostNames = LIMELIGHTS; + + // setup limelight objects and field objects for april tag data + limelights = new Limelight[hostNames.length]; + limelightPoses = new FieldObject2d[limelights.length]; + + // setup all objects + Field2d field = Odometry.getInstance().getField(); + for(int i = 0; i < hostNames.length; i++){ + limelights[i] = new Limelight(hostNames[i], POSITIONS[i]); + limelightPoses[i] = field.getObject(hostNames[i] + " pose"); + + for (int port : PORTS) { + PortForwarder.add(port + i * 10, hostNames[i] + ".local", port); + } + } + } + + @Override + public double getDistanceToNote() { + for (Limelight limelight : limelights) { + if(limelight.hasNoteData()) { + return limelight.getDistanceToNote(); + } + } + + return Double.NaN; + } + + @Override + public Rotation2d getRotationToNote() { + for (Limelight limelight : limelights) { + if (limelight.hasNoteData()) { + return Rotation2d.fromDegrees(limelight.getXAngle()); + } + } + return Rotation2d.fromDegrees(Double.NaN); + } + + @Override + public void periodic(){ + for (int i = 0; i < limelights.length; ++i) { + if(limelights[i].hasNoteData()) { + SmartDashboard.putNumber("Vision/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Vision/Note Distance", getDistanceToNote()); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java new file mode 100644 index 0000000..d6df497 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -0,0 +1,81 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.util; + +import static com.stuypulse.robot.constants.Settings.Limelight.LIMELIGHTS; +import static com.stuypulse.robot.constants.Settings.Limelight.POSITIONS; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + + +public class Limelight { + private final String tableName; + + private final DoubleEntry txEntry; + private final DoubleEntry tyEntry; + private final IntegerEntry tvEntry; + + private int limelightId; + private final Pose3d robotRelativePose; + + + public Limelight(String tableName, Pose3d robotRelativePose) { + this.tableName = tableName; + this.robotRelativePose = robotRelativePose; + + for(int i = 0; i < LIMELIGHTS.length; i++) { + if(LIMELIGHTS[i].equals(tableName)) { + limelightId = i; + } + } + + NetworkTable limelight = NetworkTableInstance.getDefault().getTable(tableName); + + + txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); + tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); + tvEntry = limelight.getIntegerTopic("tv").getEntry(0); + } + + public String getTableName() { + return tableName; + } + + public boolean hasNoteData() { + return tvEntry.get() == 1; + } + + public double getXAngle() { + if(hasNoteData()) { + return Double.NaN; + } + + return txEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + } + + public double getYAngle() { + if(!hasNoteData()) { + return Double.NaN; + } + + return tyEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + } + + public double getDistanceToNote() { + if(!hasNoteData()) { + return Double.NaN; + } + + Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); + return POSITIONS[limelightId].getZ() / yRotation.getTan() + POSITIONS[limelightId].getX(); + } +} \ No newline at end of file From ac7eb66c3e99e0deac593a1f7ddd1798c3ac7f4a Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 14:24:20 -0500 Subject: [PATCH 02/24] Make note aligned drive command --- .../com/stuypulse/robot/RobotContainer.java | 4 + .../swerve/SwerveDriveNoteAlignedDrive.java | 83 +++++++++++++++++++ .../notevision/AbstractNoteVision.java | 2 + .../subsystems/notevision/NoteVision.java | 10 +++ 4 files changed, 99 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c783f80..7cfa815 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; @@ -75,6 +76,9 @@ private void configureButtonBindings() { driver.getLeftTriggerButton() .whileTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); + + driver.getRightBumper() + .whileTrue(new SwerveDriveNoteAlignedDrive(driver)); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java new file mode 100644 index 0000000..62b808c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -0,0 +1,83 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.constants.Settings .Driver.Turn.GyroFeedback; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveNoteAlignedDrive extends Command { + + private SwerveDrive swerve; + private AbstractNoteVision noteVision; + + private VStream speed; + private IStream turn; + + private final Gamepad driver; + + private AngleController alignController; + + public SwerveDriveNoteAlignedDrive(Gamepad driver) { + this.driver = driver; + + swerve = SwerveDrive.getInstance(); + noteVision = AbstractNoteVision.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1.0), + x -> Settings.vpow(x, Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND.get()), + x -> SLMath.spow(x, Turn.POWER.get()), + x -> x * Turn.MAX_TELEOP_TURNING.get(), + new LowPassFilter(Turn.RC) + ); + + alignController = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D); + + addRequirements(swerve); + } + + @Override + public void execute() { + double angularVel = turn.get(); + + if (noteVision.hasNoteData()) { + angularVel = -alignController.update( + Angle.kZero, + Angle.fromRotation2d(noteVision.getRotationToNote()) + ); + } + + if(driver.getRawStartButton() || driver.getRawSelectButton()) { + swerve.setXMode(); + } + else { + swerve.drive(speed.get(), angularVel); + } + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java index e66f681..a3c762c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -21,6 +21,8 @@ public abstract class AbstractNoteVision extends SubsystemBase { public static AbstractNoteVision getInstance() { return instance; } + + public abstract boolean hasNoteData(); public abstract double getDistanceToNote(); public abstract Rotation2d getRotationToNote(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index c38140a..2dd2529 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -44,6 +44,16 @@ protected NoteVision() { } } + @Override + public boolean hasNoteData() { + for (Limelight limelight : limelights) { + if (limelight.hasNoteData()) + return true; + } + + return false; + } + @Override public double getDistanceToNote() { for (Limelight limelight : limelights) { From 0f44220715742fd4ad049bcbe03df8047a43fb4c Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Fri, 19 Jan 2024 16:24:35 -0500 Subject: [PATCH 03/24] Revert "Merge branch 'main' into note-detection" This reverts commit 974ad00c0041399d93b720d59dcf87f8df67386d, reversing changes made to ac7eb66c3e99e0deac593a1f7ddd1798c3ac7f4a. --- .pathplanner/settings.json | 7 +- src/main/deploy/Johnathan.chor | 1635 ----------------- src/main/deploy/choreo/Straight Line.traj | 103 -- src/main/deploy/choreo/Three Piece.traj | 1336 -------------- src/main/deploy/pathplanner/New Path.path | 74 - .../deploy/pathplanner/autos/3 Piece HFE.auto | 55 - .../deploy/pathplanner/autos/4 Piece ABC.auto | 37 - .../pathplanner/autos/6 Piece ABCDE.auto | 61 - ...To B (ABC).path => 1 Note + Mobility.path} | 20 +- .../pathplanner/paths/2 Note Center 6.path | 97 + .../pathplanner/paths/2 Note Center 7.path | 97 + .../pathplanner/paths/2 Note Center 8.path | 97 + ...Shoot (ABCD).path => 2 Note Mobility.path} | 34 +- .../pathplanner/paths/3 Note (4,1).path | 181 ++ .../pathplanner/paths/3 Note Center 7+6.path | 155 ++ .../pathplanner/paths/3 Note Center 8+7.path | 113 ++ .../pathplanner/paths/3 Note Mobility.path | 65 + .../pathplanner/paths/4 Note (5, 4, 1).path | 129 ++ .../pathplanner/paths/4 Note End 4.path | 113 ++ .../paths/4 Note Mobility End Speaker.path | 92 + .../pathplanner/paths/A To D (ABCD).path | 49 - ...BC).path => Bottom 1 Note + Mobility.path} | 25 +- .../paths/DShoot To E (ABCDE).path | 49 - .../{B To A (ABC).path => Do Nothing.path} | 20 +- .../pathplanner/paths/E To Shoot (ABCDE).path | 49 - .../pathplanner/paths/E To Shoot (HFE).path | 49 - .../pathplanner/paths/F To Shoot (HFE).path | 49 - .../pathplanner/paths/FShoot To E (HFE).path | 49 - .../pathplanner/paths/H To Shoot (HFE).path | 49 - .../pathplanner/paths/HShoot To F (HFE).path | 49 - .../{Start To H (HFE).path => Mobility.path} | 24 +- .../paths/deploy/pathplanner/navgrid.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 20 +- .../commands/swerve/SwerveDriveToPose.java | 81 - .../swerve/SwerveDriveWithAiming.java | 64 - .../stuypulse/robot/constants/Cameras.java | 2 +- .../com/stuypulse/robot/constants/Field.java | 36 +- .../robot/subsystems/odometry/Odometry.java | 12 +- .../robot/subsystems/swerve/SwerveDrive.java | 31 - 39 files changed, 1235 insertions(+), 3974 deletions(-) delete mode 100644 src/main/deploy/Johnathan.chor delete mode 100644 src/main/deploy/choreo/Straight Line.traj delete mode 100644 src/main/deploy/choreo/Three Piece.traj delete mode 100644 src/main/deploy/pathplanner/New Path.path delete mode 100644 src/main/deploy/pathplanner/autos/3 Piece HFE.auto delete mode 100644 src/main/deploy/pathplanner/autos/4 Piece ABC.auto delete mode 100644 src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto rename src/main/deploy/pathplanner/paths/{C To B (ABC).path => 1 Note + Mobility.path} (69%) create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 6.path create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 7.path create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 8.path rename src/main/deploy/pathplanner/paths/{D To Shoot (ABCD).path => 2 Note Mobility.path} (53%) create mode 100644 src/main/deploy/pathplanner/paths/3 Note (4,1).path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 7+6.path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 8+7.path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path create mode 100644 src/main/deploy/pathplanner/paths/4 Note End 4.path create mode 100644 src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path delete mode 100644 src/main/deploy/pathplanner/paths/A To D (ABCD).path rename src/main/deploy/pathplanner/paths/{Start To C (ABC).path => Bottom 1 Note + Mobility.path} (66%) delete mode 100644 src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path rename src/main/deploy/pathplanner/paths/{B To A (ABC).path => Do Nothing.path} (73%) delete mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path delete mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/F To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/FShoot To E (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/H To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/HShoot To F (HFE).path rename src/main/deploy/pathplanner/paths/{Start To H (HFE).path => Mobility.path} (64%) create mode 100644 src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f64d841..70c55ea 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,10 +1,9 @@ { - "robotWidth": 0.66, - "robotLength": 0.66, + "robotWidth": 0.9, + "robotLength": 0.9, "holonomicMode": true, "pathFolders": [ - "ABCDE", - "HFE" + "New Folder" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/Johnathan.chor b/src/main/deploy/Johnathan.chor deleted file mode 100644 index 897ee40..0000000 --- a/src/main/deploy/Johnathan.chor +++ /dev/null @@ -1,1635 +0,0 @@ -{ - "version": "v0.2", - "robotConfiguration": { - "mass": 65.77, - "rotationalInertia": 4.85963002223, - "motorMaxTorque": 0.7248618784530386, - "motorMaxVelocity": 4704, - "gearing": 4.71, - "wheelbase": 0.52, - "trackWidth": 0.67, - "bumperLength": 0.79, - "bumperWidth": 0.94, - "wheelRadius": 0.038 - }, - "paths": { - "Straight Line": { - "waypoints": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 9.80132051089383, - "y": 4.12, - "heading": 1.0196133040364384e-18, - "angularVelocity": 1.0324520604294053e-17, - "velocityX": 0.8234448167415525, - "velocityY": 3.02420635388715e-34, - "timestamp": 0.09875647917200309 - }, - { - "x": 9.963961529614206, - "y": 4.12, - "heading": 3.0591556010799972e-18, - "angularVelocity": 2.0652237848908187e-17, - "velocityX": 1.6468896024240143, - "velocityY": 2.277444821719999e-33, - "timestamp": 0.19751295834400617 - }, - { - "x": 10.207923050026555, - "y": 4.12, - "heading": 6.11931106017425e-18, - "angularVelocity": 3.0986882933133645e-17, - "velocityX": 2.4703343259883073, - "velocityY": 1.5382285718210452e-33, - "timestamp": 0.29626943751600926 - }, - { - "x": 10.533205053727176, - "y": 4.12, - "heading": 1.020162012561919e-17, - "angularVelocity": 4.133712642975716e-17, - "velocityX": 3.29377886319825, - "velocityY": 1.5324076974034493e-33, - "timestamp": 0.39502591668801235 - }, - { - "x": 10.906794946272825, - "y": 4.12, - "heading": -1.0162422335797283e-17, - "angularVelocity": -2.062046220374934e-16, - "velocityX": 3.7829405794729865, - "velocityY": -3.7320649171210315e-33, - "timestamp": 0.49378239586001543 - }, - { - "x": 11.232076949973447, - "y": 4.12, - "heading": -6.100334399460735e-18, - "angularVelocity": 4.11323689413084e-17, - "velocityX": 3.2937788631982494, - "velocityY": 1.28740322268366e-34, - "timestamp": 0.5925388750320185 - }, - { - "x": 11.476038470385795, - "y": 4.12, - "heading": -3.0508797817029e-18, - "angularVelocity": 3.0878527093782464e-17, - "velocityX": 2.470334325988307, - "velocityY": -6.604493200637239e-34, - "timestamp": 0.6912953542040217 - }, - { - "x": 11.63867948910617, - "y": 4.12, - "heading": -1.0171236371693236e-18, - "angularVelocity": 2.0593647744285298e-17, - "velocityX": 1.646889602424014, - "velocityY": -7.180489499042344e-34, - "timestamp": 0.7900518333760247 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 1.0299310442024172e-17, - "velocityX": 0.8234448167415525, - "velocityY": -6.686788666173353e-34, - "timestamp": 0.8888083125480277 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 9.925887673927275e-44, - "timestamp": 0.9875647917200308 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "abdd982e-66b2-455b-8a91-2ff53944faf6" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "269654ce-bbb8-4e2d-b597-db25cd41cc9a" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "Three Piece": { - "waypoints": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 5 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 5 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 23 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "angularVelocity": -2.8038956593328693e-37, - "velocityX": -1.823589012902056e-36, - "velocityY": -4.380800340261528e-36, - "timestamp": 0 - }, - { - "x": 1.907488249459704, - "y": 4.045792662710147, - "heading": -0.014531093178687978, - "angularVelocity": -0.3399428010717914, - "velocityX": 0.21634741051533174, - "velocityY": -0.0012585953421492776, - "timestamp": 0.042745700549838096 - }, - { - "x": 1.9260125772155603, - "y": 4.04570358761662, - "heading": -0.0432778322806298, - "angularVelocity": -0.672505976792342, - "velocityX": 0.4333611922971804, - "velocityY": -0.002083837494312978, - "timestamp": 0.08549140109967619 - }, - { - "x": 1.9538513835951645, - "y": 4.045605326780032, - "heading": -0.0858077691342503, - "angularVelocity": -0.9949523883468459, - "velocityX": 0.6512656482760499, - "velocityY": -0.002298730289234273, - "timestamp": 0.1282371016495143 - }, - { - "x": 1.9910562559823362, - "y": 4.045539296716268, - "heading": -0.1415186846109791, - "angularVelocity": -1.3033103858427657, - "velocityX": 0.8703769480580534, - "velocityY": -0.0015447182503467369, - "timestamp": 0.17098280219935238 - }, - { - "x": 2.0376949895847924, - "y": 4.045574246714428, - "heading": -0.20959549088699864, - "angularVelocity": -1.5926000837592402, - "velocityX": 1.0910742601604893, - "velocityY": 0.0008176260468236386, - "timestamp": 0.21372850274919047 - }, - { - "x": 2.0938509724389283, - "y": 4.0458223742350645, - "heading": -0.28899731848757143, - "angularVelocity": -1.8575395087512154, - "velocityX": 1.3137223658005655, - "velocityY": 0.005804736323069604, - "timestamp": 0.2564742032990286 - }, - { - "x": 2.159619041381372, - "y": 4.046458363697538, - "heading": -0.37845923210523325, - "angularVelocity": -2.0928868276086927, - "velocityX": 1.538589100107577, - "velocityY": 0.014878442844382068, - "timestamp": 0.2992199038488667 - }, - { - "x": 2.2351062437840015, - "y": 4.047751445000734, - "heading": -0.4762822950411069, - "angularVelocity": -2.288488939883434, - "velocityX": 1.7659601183660054, - "velocityY": 0.030250558221350754, - "timestamp": 0.3419656043987048 - }, - { - "x": 2.3204435066348985, - "y": 4.050239499458989, - "heading": -0.5785043698305009, - "angularVelocity": -2.391400151933671, - "velocityX": 1.99639406427321, - "velocityY": 0.058205958172433236, - "timestamp": 0.38471130494854294 - }, - { - "x": 2.4129687747508384, - "y": 4.057068741649624, - "heading": -0.6558501426840586, - "angularVelocity": -1.8094398233894569, - "velocityX": 2.1645514502227607, - "velocityY": 0.15976442315343106, - "timestamp": 0.42745700549838106 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "angularVelocity": -0.6191616505278632, - "velocityX": 2.0985980375619784, - "velocityY": 0.19360057017687107, - "timestamp": 0.4702027060482192 - }, - { - "x": 2.5850596627844538, - "y": 4.076067191512528, - "heading": -0.6785329802999686, - "angularVelocity": 0.08937347775881672, - "velocityX": 1.946004228329371, - "velocityY": 0.25328355970417904, - "timestamp": 0.5125380944025599 - }, - { - "x": 2.659003068905951, - "y": 4.0907479122594275, - "heading": -0.6634760532676318, - "angularVelocity": 0.3556581767081624, - "velocityX": 1.7466098457064405, - "velocityY": 0.3467718454363507, - "timestamp": 0.5548734827569006 - }, - { - "x": 2.7246369617363335, - "y": 4.110317723995373, - "heading": -0.642205314944677, - "angularVelocity": 0.5024339955245456, - "velocityX": 1.5503316582580267, - "velocityY": 0.4622565776921573, - "timestamp": 0.5972088711112413 - }, - { - "x": 2.7821274656068904, - "y": 4.1352087632427805, - "heading": -0.6169095715353604, - "angularVelocity": 0.5975082405668554, - "velocityX": 1.357977477125514, - "velocityY": 0.5879487637877254, - "timestamp": 0.639544259465582 - }, - { - "x": 2.831597012829208, - "y": 4.165661838985507, - "heading": -0.5887878540219638, - "angularVelocity": 0.6642602939654689, - "velocityX": 1.1685152574547029, - "velocityY": 0.7193290749535347, - "timestamp": 0.6818796478199227 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "angularVelocity": 0.7130885845826266, - "velocityX": 0.9811611571580527, - "velocityY": 0.8543111534241201, - "timestamp": 0.7242150361742634 - }, - { - "x": 2.9188396470921765, - "y": 4.2725572090728265, - "heading": -0.5076501226687711, - "angularVelocity": 0.770144031880298, - "velocityX": 0.6908747919669516, - "velocityY": 1.069122760430878, - "timestamp": 0.7903699982135706 - }, - { - "x": 2.9452297152900053, - "y": 4.357347002147122, - "heading": -0.4530144734209342, - "angularVelocity": 0.8258737903193734, - "velocityX": 0.398912906671289, - "velocityY": 1.2816845548776343, - "timestamp": 0.8565249602528777 - }, - { - "x": 2.9521484208350985, - "y": 4.455984216280158, - "heading": -0.39482288845823243, - "angularVelocity": 0.8796254002553318, - "velocityX": 0.10458331970597358, - "velocityY": 1.4910025052153826, - "timestamp": 0.9226799222921849 - }, - { - "x": 2.9393582563456846, - "y": 4.568133489859713, - "heading": -0.3332972563741976, - "angularVelocity": 0.9300229368656926, - "velocityX": -0.19333643456426472, - "velocityY": 1.695251121343236, - "timestamp": 0.988834884331492 - }, - { - "x": 2.906458551093886, - "y": 4.693199281947042, - "heading": -0.26886974643996187, - "angularVelocity": 0.9738877923617428, - "velocityX": -0.4973127372101132, - "velocityY": 1.8904975262931591, - "timestamp": 1.0549898463707992 - }, - { - "x": 2.8526414862163594, - "y": 4.829843806089774, - "heading": -0.2025647523319625, - "angularVelocity": 1.0022678883649443, - "velocityX": -0.81350004925632, - "velocityY": 2.065521918998951, - "timestamp": 1.1211448084101063 - }, - { - "x": 2.775736178341105, - "y": 4.97269513900139, - "heading": -0.13812989460617203, - "angularVelocity": 0.9739988617559082, - "velocityX": -1.1625024866548819, - "velocityY": 2.1593441898846235, - "timestamp": 1.1872997704494135 - }, - { - "x": 2.702646192253521, - "y": 5.09239426390518, - "heading": -0.0945099100170612, - "angularVelocity": 0.6593607379472648, - "velocityX": -1.1048299905932448, - "velocityY": 1.809374855852369, - "timestamp": 1.2534547324887206 - }, - { - "x": 2.6443183089698836, - "y": 5.193435173263213, - "heading": -0.060805157515131665, - "angularVelocity": 0.5094818508383886, - "velocityX": -0.8816856889583129, - "velocityY": 1.5273368201465674, - "timestamp": 1.3196096945280278 - }, - { - "x": 2.602443138845534, - "y": 5.277234789353118, - "heading": -0.03510328646046884, - "angularVelocity": 0.3885101020750583, - "velocityX": -0.63298608046164, - "velocityY": 1.2667170157261087, - "timestamp": 1.385764656567335 - }, - { - "x": 2.577670236134593, - "y": 5.344417005113783, - "heading": -0.016652156501204083, - "angularVelocity": 0.2789077249912364, - "velocityX": -0.3744677942105343, - "velocityY": 1.015527992000793, - "timestamp": 1.451919618606642 - }, - { - "x": 2.5703412148254166, - "y": 5.395330206172067, - "heading": -0.005041861251989399, - "angularVelocity": 0.17550150270385198, - "velocityX": -0.1107856626812363, - "velocityY": 0.7696051738043858, - "timestamp": 1.5180745806459492 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 4.946919732174461e-30, - "angularVelocity": 0.07621289615423993, - "velocityX": 0.15607789513463036, - "velocityY": 0.5270364913068623, - "timestamp": 1.5842295426852564 - }, - { - "x": 2.615476810555517, - "y": 5.449290678492884, - "heading": -0.00249481878254215, - "angularVelocity": -0.03313174320347507, - "velocityX": 0.4622880366833039, - "velocityY": 0.25357775003504285, - "timestamp": 1.659529497994595 - }, - { - "x": 2.6735006636921637, - "y": 5.447944678639869, - "heading": -0.01213678704864337, - "angularVelocity": -0.12804746332838068, - "velocityX": 0.7705695560944235, - "velocityY": -0.017875174659603758, - "timestamp": 1.7348294533039335 - }, - { - "x": 2.7555584259794457, - "y": 5.427049168858778, - "heading": -0.02296797038520493, - "angularVelocity": -0.14384050152574682, - "velocityX": 1.0897451658527806, - "velocityY": -0.27749697453671124, - "timestamp": 1.810129408613272 - }, - { - "x": 2.8158286951067315, - "y": 5.427810213575971, - "heading": -0.01758027986858031, - "angularVelocity": 0.07154971732043594, - "velocityX": 0.8004024554820866, - "velocityY": 0.010106841552119225, - "timestamp": 1.8854293639226105 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "angularVelocity": 0.23346999073716668, - "velocityX": 0.5021023557678279, - "velocityY": 0.2906235864398052, - "timestamp": 1.960729319231949 - }, - { - "x": 2.8693148468887744, - "y": 5.487156976773219, - "heading": 0.025561040529235773, - "angularVelocity": 0.3719476521458912, - "velocityX": 0.22813413061776552, - "velocityY": 0.5451346150355147, - "timestamp": 2.0294514643557737 - }, - { - "x": 2.8662677650419504, - "y": 5.542220250273633, - "heading": 0.0607369491230168, - "angularVelocity": 0.5118569644530235, - "velocityX": -0.04433915503268782, - "velocityY": 0.8012449757090656, - "timestamp": 2.0981736094795984 - }, - { - "x": 2.844654009258225, - "y": 5.615050469126244, - "heading": 0.10566640088464291, - "angularVelocity": 0.6537841867519056, - "velocityX": -0.3145093294858733, - "velocityY": 1.0597780194634927, - "timestamp": 2.166895754603423 - }, - { - "x": 2.804748272410547, - "y": 5.705930381335797, - "heading": 0.16050762468177382, - "angularVelocity": 0.7980138527154114, - "velocityX": -0.5806823517481201, - "velocityY": 1.3224254284525792, - "timestamp": 2.235617899727248 - }, - { - "x": 2.7471447891895058, - "y": 5.815446220375669, - "heading": 0.22535865974809183, - "angularVelocity": 0.9436701219012945, - "velocityX": -0.8382084569282627, - "velocityY": 1.5936033260100555, - "timestamp": 2.3043400448510725 - }, - { - "x": 2.6740678731800593, - "y": 5.945504225324297, - "heading": 0.2996496097363406, - "angularVelocity": 1.0810336297621417, - "velocityX": -1.0633677961852832, - "velocityY": 1.892519575957448, - "timestamp": 2.373062189974897 - }, - { - "x": 2.6265491520645683, - "y": 6.0733583679407905, - "heading": 0.3634596160425804, - "angularVelocity": 0.9285217478480327, - "velocityX": -0.6914615518748894, - "velocityY": 1.8604504033761498, - "timestamp": 2.441784335098722 - }, - { - "x": 2.600377734078398, - "y": 6.186973463102748, - "heading": 0.4156012346676629, - "angularVelocity": 0.7587309524627455, - "velocityX": -0.38082946827422787, - "velocityY": 1.6532530373905598, - "timestamp": 2.5105064802225465 - }, - { - "x": 2.59468135033937, - "y": 6.285113460277094, - "heading": 0.45604131218202076, - "angularVelocity": 0.5884577299135867, - "velocityX": -0.08289007464427144, - "velocityY": 1.4280694672367455, - "timestamp": 2.579228625346371 - }, - { - "x": 2.6091090827604106, - "y": 6.367321455091837, - "heading": 0.4847594324399464, - "angularVelocity": 0.41788742487855773, - "velocityX": 0.20994298701015016, - "velocityY": 1.1962373215594329, - "timestamp": 2.647950770470196 - }, - { - "x": 2.6434723912297375, - "y": 6.433359811846902, - "heading": 0.5017638856587859, - "angularVelocity": 0.24743775369934545, - "velocityX": 0.5000325354717966, - "velocityY": 0.9609472555909916, - "timestamp": 2.7166729155940206 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "angularVelocity": 0.07762170883267666, - "velocityX": 0.7884122231547788, - "velocityY": 0.7235292572375208, - "timestamp": 2.7853950607178453 - }, - { - "x": 2.738806835827011, - "y": 6.507590262026266, - "heading": 0.5059467865388958, - "angularVelocity": -0.027038737137375757, - "velocityX": 0.966387396526008, - "velocityY": 0.5755146244320987, - "timestamp": 2.8279794999687833 - }, - { - "x": 2.7880294623095803, - "y": 6.526430774501013, - "heading": 0.5005508163010642, - "angularVelocity": -0.12671225294372634, - "velocityX": 1.155882931615343, - "velocityY": 0.44242715898465257, - "timestamp": 2.8705639392197213 - }, - { - "x": 2.84591135896884, - "y": 6.540541693646972, - "heading": 0.4912680248506681, - "angularVelocity": -0.21798552742928792, - "velocityX": 1.35922646105959, - "velocityY": 0.3313632724575067, - "timestamp": 2.9131483784706593 - }, - { - "x": 2.913103056068397, - "y": 6.55135708254599, - "heading": 0.4786790444220719, - "angularVelocity": -0.2956239567794471, - "velocityX": 1.5778462340108705, - "velocityY": 0.25397513949368183, - "timestamp": 2.9557328177215973 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "angularVelocity": -0.35298591263083134, - "velocityX": 1.8086189494475877, - "velocityY": 0.22817483768114147, - "timestamp": 2.9983172569725354 - }, - { - "x": 3.1507680403022618, - "y": 6.583513336972122, - "heading": 0.4338303597620861, - "angularVelocity": -0.4082414173674588, - "velocityX": 2.199496396777096, - "velocityY": 0.3072329016846347, - "timestamp": 3.0713548671674924 - }, - { - "x": 3.339966410927534, - "y": 6.61173687205727, - "heading": 0.4005433233854809, - "angularVelocity": -0.4557519925385439, - "velocityX": 2.5904238942135573, - "velocityY": 0.38642467914559536, - "timestamp": 3.1443924773624494 - }, - { - "x": 3.5577217220068316, - "y": 6.645759083728854, - "heading": 0.36470254911055944, - "angularVelocity": -0.4907166893776054, - "velocityX": 2.981413418347747, - "velocityY": 0.46581770105525094, - "timestamp": 3.2174300875574064 - }, - { - "x": 3.8040369699341148, - "y": 6.685605934834726, - "heading": 0.32812163175231024, - "angularVelocity": -0.5008504147466596, - "velocityX": 3.372443967837953, - "velocityY": 0.5455661952726859, - "timestamp": 3.2904676977523635 - }, - { - "x": 4.078865987183254, - "y": 6.731338843022743, - "heading": 0.29648766626359246, - "angularVelocity": -0.43311884663638234, - "velocityX": 3.7628424111296486, - "velocityY": 0.6261555938911911, - "timestamp": 3.3635053079473205 - }, - { - "x": 4.363416873029506, - "y": 6.788685305113101, - "heading": 0.29648766061986404, - "angularVelocity": -7.727153775240832e-8, - "velocityX": 3.8959501151079365, - "velocityY": 0.7851634512312887, - "timestamp": 3.4365429181422775 - }, - { - "x": 4.647967686637335, - "y": 6.846032125646838, - "heading": 0.2964876549762135, - "angularVelocity": -7.72704712131681e-8, - "velocityX": 3.895949126050067, - "velocityY": 0.7851683588860835, - "timestamp": 3.5095805283372346 - }, - { - "x": 4.932518500243366, - "y": 6.903378946189493, - "heading": 0.2964876493325629, - "angularVelocity": -7.727047143608747e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081883, - "timestamp": 3.5826181385321916 - }, - { - "x": 5.217069313849397, - "y": 6.960725766732147, - "heading": 0.29648764368891234, - "angularVelocity": -7.727047107791159e-8, - "velocityX": 3.8959491260254584, - "velocityY": 0.7851683590081909, - "timestamp": 3.6556557487271486 - }, - { - "x": 5.501620127455428, - "y": 7.018072587274802, - "heading": 0.2964876380452618, - "angularVelocity": -7.72704716252991e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081912, - "timestamp": 3.7286933589221056 - }, - { - "x": 5.786170941061528, - "y": 7.075419407817124, - "heading": 0.29648763240161125, - "angularVelocity": -7.727047111704844e-8, - "velocityX": 3.8959491260263763, - "velocityY": 0.7851683590036359, - "timestamp": 3.8017309691170627 - }, - { - "x": 6.070721757362521, - "y": 7.132766214987568, - "heading": 0.29648762675796064, - "angularVelocity": -7.727047165435608e-8, - "velocityX": 3.895949162923739, - "velocityY": 0.7851681759215458, - "timestamp": 3.8747685793120197 - }, - { - "x": 6.355380338499842, - "y": 7.1895756818041425, - "heading": 0.29648762107984833, - "angularVelocity": -7.774230685415808e-8, - "velocityX": 3.8974246333839155, - "velocityY": 0.7778111395614767, - "timestamp": 3.9478061895069767 - }, - { - "x": 6.636405520312889, - "y": 7.217870773127444, - "heading": 0.27570831993349604, - "angularVelocity": -0.28450138347744536, - "velocityX": 3.847677669941766, - "velocityY": 0.387404396827479, - "timestamp": 4.020843799701934 - }, - { - "x": 6.88919116626837, - "y": 7.2399532386560255, - "heading": 0.24179746779298844, - "angularVelocity": -0.4642930135582247, - "velocityX": 3.4610339150025435, - "velocityY": 0.30234375782058953, - "timestamp": 4.093881409896891 - }, - { - "x": 7.113463572845222, - "y": 7.256040709772393, - "heading": 0.20495027392657358, - "angularVelocity": -0.5044961598286116, - "velocityX": 3.070642727468879, - "velocityY": 0.22026283545456557, - "timestamp": 4.166919020091848 - }, - { - "x": 7.3091937477745095, - "y": 7.2662136587336335, - "heading": 0.1681230341900902, - "angularVelocity": -0.5042229563396395, - "velocityX": 2.679854590078042, - "velocityY": 0.13928370512239335, - "timestamp": 4.239956630286805 - }, - { - "x": 7.476376305660358, - "y": 7.270512578107467, - "heading": 0.13274981372464448, - "angularVelocity": -0.4843151408024598, - "velocityX": 2.288992718129635, - "velocityY": 0.058858981863695155, - "timestamp": 4.312994240481762 - }, - { - "x": 7.615010848660238, - "y": 7.268961369646756, - "heading": 0.09968688668304959, - "angularVelocity": -0.45268358251784385, - "velocityX": 1.8981253990899605, - "velocityY": -0.021238488726158328, - "timestamp": 4.386031850676719 - }, - { - "x": 7.7250983383538445, - "y": 7.261575556193537, - "heading": 0.06950965194609489, - "angularVelocity": -0.4131739066544426, - "velocityX": 1.5072712455918797, - "velocityY": -0.10112342714260779, - "timestamp": 4.459069460871676 - }, - { - "x": 7.8066401035121915, - "y": 7.248365897189822, - "heading": 0.04263526512944047, - "angularVelocity": -0.36795271292310205, - "velocityX": 1.1164352850632593, - "velocityY": -0.18086105183966827, - "timestamp": 4.532107071066633 - }, - { - "x": 7.859637512937734, - "y": 7.2293402273974605, - "heading": 0.019381829491425508, - "angularVelocity": -0.3183761842144796, - "velocityX": 0.7256180655976813, - "velocityY": -0.2604914062984367, - "timestamp": 4.60514468126159 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "angularVelocity": -0.26536779393096516, - "velocityX": 0.33481847355697797, - "velocityY": -0.3400403905944489, - "timestamp": 4.678182291456547 - }, - { - "x": 7.8693875341309285, - "y": 7.163825702906692, - "heading": -0.017989310395224636, - "angularVelocity": -0.1948160433685539, - "velocityX": -0.15924109223639094, - "velocityY": -0.44053274732062453, - "timestamp": 4.770522275803691 - }, - { - "x": 7.809062158016808, - "y": 7.113867015205141, - "heading": -0.02942623999470572, - "angularVelocity": -0.12385674180412554, - "velocityX": -0.6532963649564154, - "velocityY": -0.5410298480638316, - "timestamp": 4.862862260150834 - }, - { - "x": 7.703115992354989, - "y": 7.054628240082747, - "heading": -0.03428596694706339, - "angularVelocity": -0.05262863088743923, - "velocityX": -1.1473487504992854, - "velocityY": -0.641528970805246, - "timestamp": 4.955202244497977 - }, - { - "x": 7.551549165293792, - "y": 6.986109443661236, - "heading": -0.032557590593216855, - "angularVelocity": 0.018717529205428393, - "velocityX": -1.6413997482541873, - "velocityY": -0.7420273774784575, - "timestamp": 5.047542228845121 - }, - { - "x": 7.354361663520876, - "y": 6.908310952017223, - "heading": -0.024244154578006688, - "angularVelocity": 0.09003072801005149, - "velocityX": -2.1354508901756786, - "velocityY": -0.8425222528903508, - "timestamp": 5.139882213192264 - }, - { - "x": 7.111553334877506, - "y": 6.8212333658210484, - "heading": -0.00936157178577619, - "angularVelocity": 0.16117159752031956, - "velocityX": -2.6295036799070197, - "velocityY": -0.9430106233158517, - "timestamp": 5.232222197539407 - }, - { - "x": 6.823123896075064, - "y": 6.724877589969487, - "heading": 0.012063558679812484, - "angularVelocity": 0.23202441084506603, - "velocityX": -3.1235595375251535, - "velocityY": -1.0434891941211963, - "timestamp": 5.3245621818865505 - }, - { - "x": 6.48907294008907, - "y": 6.619244934399674, - "heading": 0.039997176732724696, - "angularVelocity": 0.3025083689412195, - "velocityX": -3.6176198030331097, - "velocityY": -1.1439535789035715, - "timestamp": 5.416902166233694 - }, - { - "x": 6.129437398233647, - "y": 6.5461675567988795, - "heading": 0.03999718077634422, - "angularVelocity": 4.3790558964884806e-8, - "velocityX": -3.8946892226384597, - "velocityY": -0.7913947367163172, - "timestamp": 5.509242150580837 - }, - { - "x": 5.769801333833775, - "y": 6.4730927508413325, - "heading": 0.03999718481982793, - "angularVelocity": 4.378908809561466e-8, - "velocityX": -3.8946948815570095, - "velocityY": -0.7913668869904851, - "timestamp": 5.60158213492798 - }, - { - "x": 5.410165307610781, - "y": 6.400017756997467, - "heading": 0.03999718886333287, - "angularVelocity": 4.378931804611701e-8, - "velocityX": -3.8946944681187916, - "velocityY": -0.7913689217137886, - "timestamp": 5.693922119275124 - }, - { - "x": 5.06488865586912, - "y": 6.3170886108245945, - "heading": 0.07172411836277942, - "angularVelocity": 0.3435882053019635, - "velocityX": -3.739188978456266, - "velocityY": -0.8980849061129225, - "timestamp": 5.786262103622267 - }, - { - "x": 4.765275429088764, - "y": 6.243389106592463, - "heading": 0.10120262922920165, - "angularVelocity": 0.3192388549211855, - "velocityX": -3.244674870790441, - "velocityY": -0.7981320849596799, - "timestamp": 5.87860208796941 - }, - { - "x": 4.511309122993011, - "y": 6.178941794592798, - "heading": 0.12636460911438926, - "angularVelocity": 0.27249278915397723, - "velocityX": -2.7503394969289876, - "velocityY": -0.6979350544113176, - "timestamp": 5.970942072316554 - }, - { - "x": 4.302982802001526, - "y": 6.123753419378413, - "heading": 0.14653342260027327, - "angularVelocity": 0.21841906979387646, - "velocityX": -2.256079232245732, - "velocityY": -0.5976649834259027, - "timestamp": 6.063282056663697 - }, - { - "x": 4.140292768892181, - "y": 6.077827039965534, - "heading": 0.16137256146424936, - "angularVelocity": 0.16070111955174202, - "velocityX": -1.7618590067952369, - "velocityY": -0.497361784687127, - "timestamp": 6.15562204101084 - }, - { - "x": 4.023236723432337, - "y": 6.041164401680778, - "heading": 0.17067956492366876, - "angularVelocity": 0.10079061118779088, - "velocityX": -1.2676636918172275, - "velocityY": -0.3970396848555328, - "timestamp": 6.2479620253579835 - }, - { - "x": 3.9518130737172363, - "y": 6.01376672295597, - "heading": 0.174318405736917, - "angularVelocity": 0.03940698971280611, - "velocityX": -0.7734856164431515, - "velocityY": -0.29670438996183723, - "timestamp": 6.340302009705127 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "angularVelocity": -0.023042788250028232, - "velocityX": -0.2793205093781995, - "velocityY": -0.19635795295345568, - "timestamp": 6.43264199405227 - }, - { - "x": 3.930355880820906, - "y": 5.98747940429267, - "heading": 0.1678304420080604, - "angularVelocity": -0.0673769466267154, - "velocityX": 0.06699164087576895, - "velocityY": -0.12602683733396786, - "timestamp": 6.497355419650052 - }, - { - "x": 3.957112124799613, - "y": 5.9838306427917995, - "heading": 0.16069467758998768, - "angularVelocity": -0.11026714089319978, - "velocityX": 0.41345738896603046, - "velocityY": -0.056383377439255714, - "timestamp": 6.562068845247834 - }, - { - "x": 4.006301141076061, - "y": 5.984635223325004, - "heading": 0.15089514245932092, - "angularVelocity": -0.15142970782560608, - "velocityX": 0.760105276796434, - "velocityY": 0.012432977017907793, - "timestamp": 6.6267822708456166 - }, - { - "x": 4.0779371329258005, - "y": 5.989827517197893, - "heading": 0.13856825674408835, - "angularVelocity": -0.1904842094413026, - "velocityX": 1.106972644208068, - "velocityY": 0.08023518806075768, - "timestamp": 6.691495696443399 - }, - { - "x": 4.17203754289526, - "y": 5.999325152011095, - "heading": 0.12388458270411208, - "angularVelocity": -0.22690305611760084, - "velocityX": 1.4541095468246137, - "velocityY": 0.14676451950228486, - "timestamp": 6.756209122041181 - }, - { - "x": 4.288624284050538, - "y": 6.0130216798691905, - "heading": 0.10706400495167072, - "angularVelocity": -0.25992408216784824, - "velocityX": 1.801585066442124, - "velocityY": 0.21164893886513167, - "timestamp": 6.820922547638963 - }, - { - "x": 4.427725657084912, - "y": 6.0307742700897, - "heading": 0.0884013396467056, - "angularVelocity": -0.2883893895674757, - "velocityX": 2.1494979094282636, - "velocityY": 0.27432623225435043, - "timestamp": 6.885635973236745 - }, - { - "x": 4.589379469563093, - "y": 6.0523813458045, - "heading": 0.06831284216496007, - "angularVelocity": -0.3104224092014938, - "velocityX": 2.497994983033644, - "velocityY": 0.3338886099014062, - "timestamp": 6.950349398834527 - }, - { - "x": 4.773638315970202, - "y": 6.077537273797697, - "heading": 0.04742981182100691, - "angularVelocity": -0.32270012213151367, - "velocityX": 2.847304785754114, - "velocityY": 0.3887281157012392, - "timestamp": 7.015062824432309 - }, - { - "x": 4.980578361520177, - "y": 6.105724638750597, - "heading": 0.026818564360069696, - "angularVelocity": -0.31850033081301815, - "velocityX": 3.197791550028341, - "velocityY": 0.43557213503258413, - "timestamp": 7.079776250030092 - }, - { - "x": 5.210305960828753, - "y": 6.1358813908951575, - "heading": 0.008654103339561452, - "angularVelocity": -0.28069076629334544, - "velocityX": 3.549921784954145, - "velocityY": 0.4660045711687753, - "timestamp": 7.144489675627874 - }, - { - "x": 5.462718064244522, - "y": 6.164655483095359, - "heading": 8.629838781735336e-9, - "angularVelocity": -0.13372951021808044, - "velocityX": 3.900459619996067, - "velocityY": 0.44463868099074777, - "timestamp": 7.209203101225656 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "angularVelocity": -1.3335468957212507e-7, - "velocityX": 3.9730265373303566, - "velocityY": 0.09983815454439177, - "timestamp": 7.273916526823438 - }, - { - "x": 5.995970674250576, - "y": 6.151565167078146, - "heading": -2.5127315305597956e-15, - "angularVelocity": -3.607302360522835e-14, - "velocityX": 3.964357212211523, - "velocityY": -0.28067875524252794, - "timestamp": 7.343573330689117 - }, - { - "x": 6.26897851241587, - "y": 6.105687844731353, - "heading": -2.5097428721994853e-15, - "angularVelocity": 4.2905476485829695e-17, - "velocityX": 3.9193276609666734, - "velocityY": -0.6586193996970835, - "timestamp": 7.413230134554796 - }, - { - "x": 6.536346012712411, - "y": 6.0339060327816165, - "heading": -2.5126951036173778e-15, - "angularVelocity": -4.2382527679423133e-17, - "velocityX": 3.838354409887958, - "velocityY": -1.0305068272755022, - "timestamp": 7.482886938420475 - }, - { - "x": 6.787193967022678, - "y": 5.960199932024534, - "heading": -2.073411284199746e-15, - "angularVelocity": 6.306402166664137e-15, - "velocityX": 3.6011981657092096, - "velocityY": -1.058132108663187, - "timestamp": 7.552543742286154 - }, - { - "x": 7.012681141047439, - "y": 5.8942233269936395, - "heading": -1.671095384485508e-15, - "angularVelocity": 5.7756870204886894e-15, - "velocityX": 3.2371162831352196, - "velocityY": -0.9471666997261698, - "timestamp": 7.622200546151833 - }, - { - "x": 7.212807520844226, - "y": 5.835976237454582, - "heading": -1.3121610031518989e-15, - "angularVelocity": 5.1528976439448214e-15, - "velocityX": 2.8730342004018077, - "velocityY": -0.8362010070315249, - "timestamp": 7.6918573500175125 - }, - { - "x": 7.387573101757002, - "y": 5.785458669967885, - "heading": -9.95057743236105e-16, - "angularVelocity": 4.552365916666039e-15, - "velocityX": 2.508952050825841, - "velocityY": -0.7252352201533442, - "timestamp": 7.7615141538831915 - }, - { - "x": 7.536977881454969, - "y": 5.7426706278046975, - "heading": -7.196116224916786e-16, - "angularVelocity": 3.954331894676066e-15, - "velocityX": 2.144869867788704, - "velocityY": -0.6142693863142378, - "timestamp": 7.831170957748871 - }, - { - "x": 7.66102185853843, - "y": 5.707612112923722, - "heading": -4.889855482801724e-16, - "angularVelocity": 3.3108908490429318e-15, - "velocityX": 1.7807876646574228, - "velocityY": -0.5033035243557882, - "timestamp": 7.90082776161455 - }, - { - "x": 7.759705032073642, - "y": 5.680283126628732, - "heading": -3.021004962604421e-16, - "angularVelocity": 2.6829403813236554e-15, - "velocityX": 1.4167054481211605, - "velocityY": -0.3923376436802606, - "timestamp": 7.970484565480229 - }, - { - "x": 7.833027401393294, - "y": 5.660683669849869, - "heading": -1.6049621497454911e-16, - "angularVelocity": 2.0328851408532914e-15, - "velocityX": 1.0526232220049963, - "velocityY": -0.281371749651518, - "timestamp": 8.040141369345909 - }, - { - "x": 7.880988965996717, - "y": 5.648813743284097, - "heading": -5.830363906716236e-17, - "angularVelocity": 1.467086785481853e-15, - "velocityX": 0.6885409887010987, - "velocityY": -0.17040584561707373, - "timestamp": 8.109798173211589 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 2.0598914784739755e-32, - "angularVelocity": 8.370128572804695e-16, - "velocityX": 0.3244587498051791, - "velocityY": -0.05943993380554415, - "timestamp": 8.179454977077269 - }, - { - "x": 7.9005397389522525, - "y": 5.6483789255306105, - "heading": 2.097139947940835e-17, - "angularVelocity": 2.980227828531976e-16, - "velocityX": -0.04334309998236763, - "velocityY": 0.05265965538925957, - "timestamp": 8.249823421958254 - }, - { - "x": 7.871608103494799, - "y": 5.6599727605367445, - "heading": -9.084634582304173e-18, - "angularVelocity": -4.2712371472453026e-16, - "velocityX": -0.41114501686552, - "velocityY": 0.16475900563136703, - "timestamp": 8.32019186683924 - }, - { - "x": 7.816794813351453, - "y": 5.6794548319403075, - "heading": -7.520085122829124e-17, - "angularVelocity": -9.395719591601255e-16, - "velocityX": -0.7789470157537616, - "velocityY": 0.27685806382157147, - "timestamp": 8.390560311720225 - }, - { - "x": 7.736099861309118, - "y": 5.706825114052552, - "heading": -1.8616176975942832e-16, - "angularVelocity": -1.5768562134134974e-15, - "velocityX": -1.1467491171467201, - "velocityY": 0.388956756951142, - "timestamp": 8.460928756601211 - }, - { - "x": 7.629523238094034, - "y": 5.742083573845685, - "heading": -3.3813406750936066e-16, - "angularVelocity": -2.1596654705354767e-15, - "velocityX": -1.5145513503282884, - "velocityY": 0.5010549807255615, - "timestamp": 8.531297201482197 - }, - { - "x": 7.497064931341595, - "y": 5.785230167283751, - "heading": -5.316654292279505e-16, - "angularVelocity": -2.750257804825092e-15, - "velocityX": -1.8823537592222348, - "velocityY": 0.6131525787087516, - "timestamp": 8.601665646363182 - }, - { - "x": 7.338724923742105, - "y": 5.836264832718443, - "heading": -7.669822112767176e-16, - "angularVelocity": -3.3440668612043406e-15, - "velocityX": -2.2501564141027943, - "velocityY": 0.7252493006130729, - "timestamp": 8.672034091244168 - }, - { - "x": 7.154503189332537, - "y": 5.895187477681304, - "heading": -1.0519707319411395e-15, - "angularVelocity": -4.0499477067678845e-15, - "velocityX": -2.617959437941741, - "velocityY": 0.8373447084559508, - "timestamp": 8.742402536125153 - }, - { - "x": 6.944399684844722, - "y": 5.961997948067273, - "heading": -1.3745422496318406e-15, - "angularVelocity": -4.5840364940621565e-15, - "velocityX": -2.985763076660981, - "velocityY": 0.9494379263248582, - "timestamp": 8.812770981006139 - }, - { - "x": 6.708414323753371, - "y": 6.036695935692792, - "heading": -1.7429472421431418e-15, - "angularVelocity": -5.235372077575223e-15, - "velocityX": -3.3535679449837517, - "velocityY": 1.061526764623348, - "timestamp": 8.883139425887125 - }, - { - "x": 6.4465468465502065, - "y": 6.119280516151445, - "heading": -2.1550026112189195e-15, - "angularVelocity": -5.855683878769628e-15, - "velocityX": -3.7213765011567173, - "velocityY": 1.1736024662605187, - "timestamp": 8.95350787076811 - }, - { - "x": 6.173126442867114, - "y": 6.178044585557323, - "heading": -2.191046659518503e-15, - "angularVelocity": -5.122189123349113e-16, - "velocityX": -3.8855541591979326, - "velocityY": 0.8350912046286955, - "timestamp": 9.023876315649096 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "angularVelocity": 3.113677831051976e-14, - "velocityX": -3.9480676215152357, - "velocityY": 0.45570865426474233, - "timestamp": 9.094244760530081 - }, - { - "x": 5.637913658685597, - "y": 6.216758064974601, - "heading": 9.67763523798797e-9, - "angularVelocity": 1.4937766527408904e-7, - "velocityX": -3.972956719905001, - "velocityY": 0.10258285959556052, - "timestamp": 9.159031120794502 - }, - { - "x": 5.380949926914102, - "y": 6.200473867453901, - "heading": 1.892686165769219e-8, - "angularVelocity": 1.4276502617102237e-7, - "velocityX": -3.966324558486143, - "velocityY": -0.2513522514767974, - "timestamp": 9.223817481058923 - }, - { - "x": 5.132255360579735, - "y": 6.163185174806636, - "heading": 0.01632330814348079, - "angularVelocity": 0.25195564544398485, - "velocityX": -3.8386871143710315, - "velocityY": -0.5755639381137804, - "timestamp": 9.288603841323344 - }, - { - "x": 4.905757766533857, - "y": 6.129190200240601, - "heading": 0.04988895608633859, - "angularVelocity": 0.5180974483811885, - "velocityX": -3.4960691281513077, - "velocityY": -0.5247242541278667, - "timestamp": 9.353390201587764 - }, - { - "x": 4.701928110746201, - "y": 6.098578355375242, - "heading": 0.08643838460715948, - "angularVelocity": 0.5641531392054652, - "velocityX": -3.146181618409506, - "velocityY": -0.4725044707141436, - "timestamp": 9.418176561852185 - }, - { - "x": 4.520779220798307, - "y": 6.071364064096333, - "heading": 0.12231563080100817, - "angularVelocity": 0.5537777712384655, - "velocityX": -2.7960961104853954, - "velocityY": -0.4200620496107946, - "timestamp": 9.482962922116606 - }, - { - "x": 4.362300886297595, - "y": 6.047552197658039, - "heading": 0.15578751714707642, - "angularVelocity": 0.5166502055290209, - "velocityX": -2.4461682035211605, - "velocityY": -0.3675444390069978, - "timestamp": 9.547749282381027 - }, - { - "x": 4.226482344491556, - "y": 6.027144059810235, - "heading": 0.1858309320626014, - "angularVelocity": 0.4637305567548124, - "velocityX": -2.096406423384503, - "velocityY": -0.3150067045713868, - "timestamp": 9.612535642645447 - }, - { - "x": 4.113314607743797, - "y": 6.010139422555146, - "heading": 0.21176520360476983, - "angularVelocity": 0.40030449984110905, - "velocityX": -1.7467833705408997, - "velocityY": -0.26247248936131745, - "timestamp": 9.677322002909868 - }, - { - "x": 4.022790393937899, - "y": 5.996537426116962, - "heading": 0.23310242597379685, - "angularVelocity": 0.3293474472395599, - "velocityX": -1.3972727196965211, - "velocityY": -0.20995154509025238, - "timestamp": 9.742108363174289 - }, - { - "x": 3.954903777130295, - "y": 5.986337041905185, - "heading": 0.2494756207657714, - "angularVelocity": 0.2527259553577121, - "velocityX": -1.0478535378517617, - "velocityY": -0.1574464774715696, - "timestamp": 9.80689472343871 - }, - { - "x": 3.9096498799999146, - "y": 5.97953733708754, - "heading": 0.260600273972009, - "angularVelocity": 0.17171289081270033, - "velocityX": -0.6985096391537948, - "velocityY": -0.10495580844363857, - "timestamp": 9.87168108370313 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 0.08723493748434215, - "velocityX": -0.3492284312714647, - "velocityY": -0.052475536233735576, - "timestamp": 9.936467443967551 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 3.6116165249279265e-32, - "velocityX": -3.053502496463557e-33, - "velocityY": 4.523388089316994e-32, - "timestamp": 10.001253804231972 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "14c0e4e2-7c6c-412a-8ad7-0357eeb42788" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "8c0a6ae0-ea46-46c8-8131-8a97a5c22a3a" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - } -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Straight Line.traj b/src/main/deploy/choreo/Straight Line.traj deleted file mode 100644 index 018d4f9..0000000 --- a/src/main/deploy/choreo/Straight Line.traj +++ /dev/null @@ -1,103 +0,0 @@ -{ - "samples": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 9.80132051089383, - "y": 4.12, - "heading": 1.0196133040364384e-18, - "angularVelocity": 1.0324520604294053e-17, - "velocityX": 0.8234448167415525, - "velocityY": 3.02420635388715e-34, - "timestamp": 0.09875647917200309 - }, - { - "x": 9.963961529614206, - "y": 4.12, - "heading": 3.0591556010799972e-18, - "angularVelocity": 2.0652237848908187e-17, - "velocityX": 1.6468896024240143, - "velocityY": 2.277444821719999e-33, - "timestamp": 0.19751295834400617 - }, - { - "x": 10.207923050026555, - "y": 4.12, - "heading": 6.11931106017425e-18, - "angularVelocity": 3.0986882933133645e-17, - "velocityX": 2.4703343259883073, - "velocityY": 1.5382285718210452e-33, - "timestamp": 0.29626943751600926 - }, - { - "x": 10.533205053727176, - "y": 4.12, - "heading": 1.020162012561919e-17, - "angularVelocity": 4.133712642975716e-17, - "velocityX": 3.29377886319825, - "velocityY": 1.5324076974034493e-33, - "timestamp": 0.39502591668801235 - }, - { - "x": 10.906794946272825, - "y": 4.12, - "heading": -1.0162422335797283e-17, - "angularVelocity": -2.062046220374934e-16, - "velocityX": 3.7829405794729865, - "velocityY": -3.7320649171210315e-33, - "timestamp": 0.49378239586001543 - }, - { - "x": 11.232076949973447, - "y": 4.12, - "heading": -6.100334399460735e-18, - "angularVelocity": 4.11323689413084e-17, - "velocityX": 3.2937788631982494, - "velocityY": 1.28740322268366e-34, - "timestamp": 0.5925388750320185 - }, - { - "x": 11.476038470385795, - "y": 4.12, - "heading": -3.0508797817029e-18, - "angularVelocity": 3.0878527093782464e-17, - "velocityX": 2.470334325988307, - "velocityY": -6.604493200637239e-34, - "timestamp": 0.6912953542040217 - }, - { - "x": 11.63867948910617, - "y": 4.12, - "heading": -1.0171236371693236e-18, - "angularVelocity": 2.0593647744285298e-17, - "velocityX": 1.646889602424014, - "velocityY": -7.180489499042344e-34, - "timestamp": 0.7900518333760247 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 1.0299310442024172e-17, - "velocityX": 0.8234448167415525, - "velocityY": -6.686788666173353e-34, - "timestamp": 0.8888083125480277 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 9.925887673927275e-44, - "timestamp": 0.9875647917200308 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Three Piece.traj b/src/main/deploy/choreo/Three Piece.traj deleted file mode 100644 index 888ff45..0000000 --- a/src/main/deploy/choreo/Three Piece.traj +++ /dev/null @@ -1,1336 +0,0 @@ -{ - "samples": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "angularVelocity": -2.8038956593328693e-37, - "velocityX": -1.823589012902056e-36, - "velocityY": -4.380800340261528e-36, - "timestamp": 0 - }, - { - "x": 1.907488249459704, - "y": 4.045792662710147, - "heading": -0.014531093178687978, - "angularVelocity": -0.3399428010717914, - "velocityX": 0.21634741051533174, - "velocityY": -0.0012585953421492776, - "timestamp": 0.042745700549838096 - }, - { - "x": 1.9260125772155603, - "y": 4.04570358761662, - "heading": -0.0432778322806298, - "angularVelocity": -0.672505976792342, - "velocityX": 0.4333611922971804, - "velocityY": -0.002083837494312978, - "timestamp": 0.08549140109967619 - }, - { - "x": 1.9538513835951645, - "y": 4.045605326780032, - "heading": -0.0858077691342503, - "angularVelocity": -0.9949523883468459, - "velocityX": 0.6512656482760499, - "velocityY": -0.002298730289234273, - "timestamp": 0.1282371016495143 - }, - { - "x": 1.9910562559823362, - "y": 4.045539296716268, - "heading": -0.1415186846109791, - "angularVelocity": -1.3033103858427657, - "velocityX": 0.8703769480580534, - "velocityY": -0.0015447182503467369, - "timestamp": 0.17098280219935238 - }, - { - "x": 2.0376949895847924, - "y": 4.045574246714428, - "heading": -0.20959549088699864, - "angularVelocity": -1.5926000837592402, - "velocityX": 1.0910742601604893, - "velocityY": 0.0008176260468236386, - "timestamp": 0.21372850274919047 - }, - { - "x": 2.0938509724389283, - "y": 4.0458223742350645, - "heading": -0.28899731848757143, - "angularVelocity": -1.8575395087512154, - "velocityX": 1.3137223658005655, - "velocityY": 0.005804736323069604, - "timestamp": 0.2564742032990286 - }, - { - "x": 2.159619041381372, - "y": 4.046458363697538, - "heading": -0.37845923210523325, - "angularVelocity": -2.0928868276086927, - "velocityX": 1.538589100107577, - "velocityY": 0.014878442844382068, - "timestamp": 0.2992199038488667 - }, - { - "x": 2.2351062437840015, - "y": 4.047751445000734, - "heading": -0.4762822950411069, - "angularVelocity": -2.288488939883434, - "velocityX": 1.7659601183660054, - "velocityY": 0.030250558221350754, - "timestamp": 0.3419656043987048 - }, - { - "x": 2.3204435066348985, - "y": 4.050239499458989, - "heading": -0.5785043698305009, - "angularVelocity": -2.391400151933671, - "velocityX": 1.99639406427321, - "velocityY": 0.058205958172433236, - "timestamp": 0.38471130494854294 - }, - { - "x": 2.4129687747508384, - "y": 4.057068741649624, - "heading": -0.6558501426840586, - "angularVelocity": -1.8094398233894569, - "velocityX": 2.1645514502227607, - "velocityY": 0.15976442315343106, - "timestamp": 0.42745700549838106 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "angularVelocity": -0.6191616505278632, - "velocityX": 2.0985980375619784, - "velocityY": 0.19360057017687107, - "timestamp": 0.4702027060482192 - }, - { - "x": 2.5850596627844538, - "y": 4.076067191512528, - "heading": -0.6785329802999686, - "angularVelocity": 0.08937347775881672, - "velocityX": 1.946004228329371, - "velocityY": 0.25328355970417904, - "timestamp": 0.5125380944025599 - }, - { - "x": 2.659003068905951, - "y": 4.0907479122594275, - "heading": -0.6634760532676318, - "angularVelocity": 0.3556581767081624, - "velocityX": 1.7466098457064405, - "velocityY": 0.3467718454363507, - "timestamp": 0.5548734827569006 - }, - { - "x": 2.7246369617363335, - "y": 4.110317723995373, - "heading": -0.642205314944677, - "angularVelocity": 0.5024339955245456, - "velocityX": 1.5503316582580267, - "velocityY": 0.4622565776921573, - "timestamp": 0.5972088711112413 - }, - { - "x": 2.7821274656068904, - "y": 4.1352087632427805, - "heading": -0.6169095715353604, - "angularVelocity": 0.5975082405668554, - "velocityX": 1.357977477125514, - "velocityY": 0.5879487637877254, - "timestamp": 0.639544259465582 - }, - { - "x": 2.831597012829208, - "y": 4.165661838985507, - "heading": -0.5887878540219638, - "angularVelocity": 0.6642602939654689, - "velocityX": 1.1685152574547029, - "velocityY": 0.7193290749535347, - "timestamp": 0.6818796478199227 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "angularVelocity": 0.7130885845826266, - "velocityX": 0.9811611571580527, - "velocityY": 0.8543111534241201, - "timestamp": 0.7242150361742634 - }, - { - "x": 2.9188396470921765, - "y": 4.2725572090728265, - "heading": -0.5076501226687711, - "angularVelocity": 0.770144031880298, - "velocityX": 0.6908747919669516, - "velocityY": 1.069122760430878, - "timestamp": 0.7903699982135706 - }, - { - "x": 2.9452297152900053, - "y": 4.357347002147122, - "heading": -0.4530144734209342, - "angularVelocity": 0.8258737903193734, - "velocityX": 0.398912906671289, - "velocityY": 1.2816845548776343, - "timestamp": 0.8565249602528777 - }, - { - "x": 2.9521484208350985, - "y": 4.455984216280158, - "heading": -0.39482288845823243, - "angularVelocity": 0.8796254002553318, - "velocityX": 0.10458331970597358, - "velocityY": 1.4910025052153826, - "timestamp": 0.9226799222921849 - }, - { - "x": 2.9393582563456846, - "y": 4.568133489859713, - "heading": -0.3332972563741976, - "angularVelocity": 0.9300229368656926, - "velocityX": -0.19333643456426472, - "velocityY": 1.695251121343236, - "timestamp": 0.988834884331492 - }, - { - "x": 2.906458551093886, - "y": 4.693199281947042, - "heading": -0.26886974643996187, - "angularVelocity": 0.9738877923617428, - "velocityX": -0.4973127372101132, - "velocityY": 1.8904975262931591, - "timestamp": 1.0549898463707992 - }, - { - "x": 2.8526414862163594, - "y": 4.829843806089774, - "heading": -0.2025647523319625, - "angularVelocity": 1.0022678883649443, - "velocityX": -0.81350004925632, - "velocityY": 2.065521918998951, - "timestamp": 1.1211448084101063 - }, - { - "x": 2.775736178341105, - "y": 4.97269513900139, - "heading": -0.13812989460617203, - "angularVelocity": 0.9739988617559082, - "velocityX": -1.1625024866548819, - "velocityY": 2.1593441898846235, - "timestamp": 1.1872997704494135 - }, - { - "x": 2.702646192253521, - "y": 5.09239426390518, - "heading": -0.0945099100170612, - "angularVelocity": 0.6593607379472648, - "velocityX": -1.1048299905932448, - "velocityY": 1.809374855852369, - "timestamp": 1.2534547324887206 - }, - { - "x": 2.6443183089698836, - "y": 5.193435173263213, - "heading": -0.060805157515131665, - "angularVelocity": 0.5094818508383886, - "velocityX": -0.8816856889583129, - "velocityY": 1.5273368201465674, - "timestamp": 1.3196096945280278 - }, - { - "x": 2.602443138845534, - "y": 5.277234789353118, - "heading": -0.03510328646046884, - "angularVelocity": 0.3885101020750583, - "velocityX": -0.63298608046164, - "velocityY": 1.2667170157261087, - "timestamp": 1.385764656567335 - }, - { - "x": 2.577670236134593, - "y": 5.344417005113783, - "heading": -0.016652156501204083, - "angularVelocity": 0.2789077249912364, - "velocityX": -0.3744677942105343, - "velocityY": 1.015527992000793, - "timestamp": 1.451919618606642 - }, - { - "x": 2.5703412148254166, - "y": 5.395330206172067, - "heading": -0.005041861251989399, - "angularVelocity": 0.17550150270385198, - "velocityX": -0.1107856626812363, - "velocityY": 0.7696051738043858, - "timestamp": 1.5180745806459492 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 4.946919732174461e-30, - "angularVelocity": 0.07621289615423993, - "velocityX": 0.15607789513463036, - "velocityY": 0.5270364913068623, - "timestamp": 1.5842295426852564 - }, - { - "x": 2.615476810555517, - "y": 5.449290678492884, - "heading": -0.00249481878254215, - "angularVelocity": -0.03313174320347507, - "velocityX": 0.4622880366833039, - "velocityY": 0.25357775003504285, - "timestamp": 1.659529497994595 - }, - { - "x": 2.6735006636921637, - "y": 5.447944678639869, - "heading": -0.01213678704864337, - "angularVelocity": -0.12804746332838068, - "velocityX": 0.7705695560944235, - "velocityY": -0.017875174659603758, - "timestamp": 1.7348294533039335 - }, - { - "x": 2.7555584259794457, - "y": 5.427049168858778, - "heading": -0.02296797038520493, - "angularVelocity": -0.14384050152574682, - "velocityX": 1.0897451658527806, - "velocityY": -0.27749697453671124, - "timestamp": 1.810129408613272 - }, - { - "x": 2.8158286951067315, - "y": 5.427810213575971, - "heading": -0.01758027986858031, - "angularVelocity": 0.07154971732043594, - "velocityX": 0.8004024554820866, - "velocityY": 0.010106841552119225, - "timestamp": 1.8854293639226105 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "angularVelocity": 0.23346999073716668, - "velocityX": 0.5021023557678279, - "velocityY": 0.2906235864398052, - "timestamp": 1.960729319231949 - }, - { - "x": 2.8693148468887744, - "y": 5.487156976773219, - "heading": 0.025561040529235773, - "angularVelocity": 0.3719476521458912, - "velocityX": 0.22813413061776552, - "velocityY": 0.5451346150355147, - "timestamp": 2.0294514643557737 - }, - { - "x": 2.8662677650419504, - "y": 5.542220250273633, - "heading": 0.0607369491230168, - "angularVelocity": 0.5118569644530235, - "velocityX": -0.04433915503268782, - "velocityY": 0.8012449757090656, - "timestamp": 2.0981736094795984 - }, - { - "x": 2.844654009258225, - "y": 5.615050469126244, - "heading": 0.10566640088464291, - "angularVelocity": 0.6537841867519056, - "velocityX": -0.3145093294858733, - "velocityY": 1.0597780194634927, - "timestamp": 2.166895754603423 - }, - { - "x": 2.804748272410547, - "y": 5.705930381335797, - "heading": 0.16050762468177382, - "angularVelocity": 0.7980138527154114, - "velocityX": -0.5806823517481201, - "velocityY": 1.3224254284525792, - "timestamp": 2.235617899727248 - }, - { - "x": 2.7471447891895058, - "y": 5.815446220375669, - "heading": 0.22535865974809183, - "angularVelocity": 0.9436701219012945, - "velocityX": -0.8382084569282627, - "velocityY": 1.5936033260100555, - "timestamp": 2.3043400448510725 - }, - { - "x": 2.6740678731800593, - "y": 5.945504225324297, - "heading": 0.2996496097363406, - "angularVelocity": 1.0810336297621417, - "velocityX": -1.0633677961852832, - "velocityY": 1.892519575957448, - "timestamp": 2.373062189974897 - }, - { - "x": 2.6265491520645683, - "y": 6.0733583679407905, - "heading": 0.3634596160425804, - "angularVelocity": 0.9285217478480327, - "velocityX": -0.6914615518748894, - "velocityY": 1.8604504033761498, - "timestamp": 2.441784335098722 - }, - { - "x": 2.600377734078398, - "y": 6.186973463102748, - "heading": 0.4156012346676629, - "angularVelocity": 0.7587309524627455, - "velocityX": -0.38082946827422787, - "velocityY": 1.6532530373905598, - "timestamp": 2.5105064802225465 - }, - { - "x": 2.59468135033937, - "y": 6.285113460277094, - "heading": 0.45604131218202076, - "angularVelocity": 0.5884577299135867, - "velocityX": -0.08289007464427144, - "velocityY": 1.4280694672367455, - "timestamp": 2.579228625346371 - }, - { - "x": 2.6091090827604106, - "y": 6.367321455091837, - "heading": 0.4847594324399464, - "angularVelocity": 0.41788742487855773, - "velocityX": 0.20994298701015016, - "velocityY": 1.1962373215594329, - "timestamp": 2.647950770470196 - }, - { - "x": 2.6434723912297375, - "y": 6.433359811846902, - "heading": 0.5017638856587859, - "angularVelocity": 0.24743775369934545, - "velocityX": 0.5000325354717966, - "velocityY": 0.9609472555909916, - "timestamp": 2.7166729155940206 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "angularVelocity": 0.07762170883267666, - "velocityX": 0.7884122231547788, - "velocityY": 0.7235292572375208, - "timestamp": 2.7853950607178453 - }, - { - "x": 2.738806835827011, - "y": 6.507590262026266, - "heading": 0.5059467865388958, - "angularVelocity": -0.027038737137375757, - "velocityX": 0.966387396526008, - "velocityY": 0.5755146244320987, - "timestamp": 2.8279794999687833 - }, - { - "x": 2.7880294623095803, - "y": 6.526430774501013, - "heading": 0.5005508163010642, - "angularVelocity": -0.12671225294372634, - "velocityX": 1.155882931615343, - "velocityY": 0.44242715898465257, - "timestamp": 2.8705639392197213 - }, - { - "x": 2.84591135896884, - "y": 6.540541693646972, - "heading": 0.4912680248506681, - "angularVelocity": -0.21798552742928792, - "velocityX": 1.35922646105959, - "velocityY": 0.3313632724575067, - "timestamp": 2.9131483784706593 - }, - { - "x": 2.913103056068397, - "y": 6.55135708254599, - "heading": 0.4786790444220719, - "angularVelocity": -0.2956239567794471, - "velocityX": 1.5778462340108705, - "velocityY": 0.25397513949368183, - "timestamp": 2.9557328177215973 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "angularVelocity": -0.35298591263083134, - "velocityX": 1.8086189494475877, - "velocityY": 0.22817483768114147, - "timestamp": 2.9983172569725354 - }, - { - "x": 3.1507680403022618, - "y": 6.583513336972122, - "heading": 0.4338303597620861, - "angularVelocity": -0.4082414173674588, - "velocityX": 2.199496396777096, - "velocityY": 0.3072329016846347, - "timestamp": 3.0713548671674924 - }, - { - "x": 3.339966410927534, - "y": 6.61173687205727, - "heading": 0.4005433233854809, - "angularVelocity": -0.4557519925385439, - "velocityX": 2.5904238942135573, - "velocityY": 0.38642467914559536, - "timestamp": 3.1443924773624494 - }, - { - "x": 3.5577217220068316, - "y": 6.645759083728854, - "heading": 0.36470254911055944, - "angularVelocity": -0.4907166893776054, - "velocityX": 2.981413418347747, - "velocityY": 0.46581770105525094, - "timestamp": 3.2174300875574064 - }, - { - "x": 3.8040369699341148, - "y": 6.685605934834726, - "heading": 0.32812163175231024, - "angularVelocity": -0.5008504147466596, - "velocityX": 3.372443967837953, - "velocityY": 0.5455661952726859, - "timestamp": 3.2904676977523635 - }, - { - "x": 4.078865987183254, - "y": 6.731338843022743, - "heading": 0.29648766626359246, - "angularVelocity": -0.43311884663638234, - "velocityX": 3.7628424111296486, - "velocityY": 0.6261555938911911, - "timestamp": 3.3635053079473205 - }, - { - "x": 4.363416873029506, - "y": 6.788685305113101, - "heading": 0.29648766061986404, - "angularVelocity": -7.727153775240832e-8, - "velocityX": 3.8959501151079365, - "velocityY": 0.7851634512312887, - "timestamp": 3.4365429181422775 - }, - { - "x": 4.647967686637335, - "y": 6.846032125646838, - "heading": 0.2964876549762135, - "angularVelocity": -7.72704712131681e-8, - "velocityX": 3.895949126050067, - "velocityY": 0.7851683588860835, - "timestamp": 3.5095805283372346 - }, - { - "x": 4.932518500243366, - "y": 6.903378946189493, - "heading": 0.2964876493325629, - "angularVelocity": -7.727047143608747e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081883, - "timestamp": 3.5826181385321916 - }, - { - "x": 5.217069313849397, - "y": 6.960725766732147, - "heading": 0.29648764368891234, - "angularVelocity": -7.727047107791159e-8, - "velocityX": 3.8959491260254584, - "velocityY": 0.7851683590081909, - "timestamp": 3.6556557487271486 - }, - { - "x": 5.501620127455428, - "y": 7.018072587274802, - "heading": 0.2964876380452618, - "angularVelocity": -7.72704716252991e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081912, - "timestamp": 3.7286933589221056 - }, - { - "x": 5.786170941061528, - "y": 7.075419407817124, - "heading": 0.29648763240161125, - "angularVelocity": -7.727047111704844e-8, - "velocityX": 3.8959491260263763, - "velocityY": 0.7851683590036359, - "timestamp": 3.8017309691170627 - }, - { - "x": 6.070721757362521, - "y": 7.132766214987568, - "heading": 0.29648762675796064, - "angularVelocity": -7.727047165435608e-8, - "velocityX": 3.895949162923739, - "velocityY": 0.7851681759215458, - "timestamp": 3.8747685793120197 - }, - { - "x": 6.355380338499842, - "y": 7.1895756818041425, - "heading": 0.29648762107984833, - "angularVelocity": -7.774230685415808e-8, - "velocityX": 3.8974246333839155, - "velocityY": 0.7778111395614767, - "timestamp": 3.9478061895069767 - }, - { - "x": 6.636405520312889, - "y": 7.217870773127444, - "heading": 0.27570831993349604, - "angularVelocity": -0.28450138347744536, - "velocityX": 3.847677669941766, - "velocityY": 0.387404396827479, - "timestamp": 4.020843799701934 - }, - { - "x": 6.88919116626837, - "y": 7.2399532386560255, - "heading": 0.24179746779298844, - "angularVelocity": -0.4642930135582247, - "velocityX": 3.4610339150025435, - "velocityY": 0.30234375782058953, - "timestamp": 4.093881409896891 - }, - { - "x": 7.113463572845222, - "y": 7.256040709772393, - "heading": 0.20495027392657358, - "angularVelocity": -0.5044961598286116, - "velocityX": 3.070642727468879, - "velocityY": 0.22026283545456557, - "timestamp": 4.166919020091848 - }, - { - "x": 7.3091937477745095, - "y": 7.2662136587336335, - "heading": 0.1681230341900902, - "angularVelocity": -0.5042229563396395, - "velocityX": 2.679854590078042, - "velocityY": 0.13928370512239335, - "timestamp": 4.239956630286805 - }, - { - "x": 7.476376305660358, - "y": 7.270512578107467, - "heading": 0.13274981372464448, - "angularVelocity": -0.4843151408024598, - "velocityX": 2.288992718129635, - "velocityY": 0.058858981863695155, - "timestamp": 4.312994240481762 - }, - { - "x": 7.615010848660238, - "y": 7.268961369646756, - "heading": 0.09968688668304959, - "angularVelocity": -0.45268358251784385, - "velocityX": 1.8981253990899605, - "velocityY": -0.021238488726158328, - "timestamp": 4.386031850676719 - }, - { - "x": 7.7250983383538445, - "y": 7.261575556193537, - "heading": 0.06950965194609489, - "angularVelocity": -0.4131739066544426, - "velocityX": 1.5072712455918797, - "velocityY": -0.10112342714260779, - "timestamp": 4.459069460871676 - }, - { - "x": 7.8066401035121915, - "y": 7.248365897189822, - "heading": 0.04263526512944047, - "angularVelocity": -0.36795271292310205, - "velocityX": 1.1164352850632593, - "velocityY": -0.18086105183966827, - "timestamp": 4.532107071066633 - }, - { - "x": 7.859637512937734, - "y": 7.2293402273974605, - "heading": 0.019381829491425508, - "angularVelocity": -0.3183761842144796, - "velocityX": 0.7256180655976813, - "velocityY": -0.2604914062984367, - "timestamp": 4.60514468126159 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "angularVelocity": -0.26536779393096516, - "velocityX": 0.33481847355697797, - "velocityY": -0.3400403905944489, - "timestamp": 4.678182291456547 - }, - { - "x": 7.8693875341309285, - "y": 7.163825702906692, - "heading": -0.017989310395224636, - "angularVelocity": -0.1948160433685539, - "velocityX": -0.15924109223639094, - "velocityY": -0.44053274732062453, - "timestamp": 4.770522275803691 - }, - { - "x": 7.809062158016808, - "y": 7.113867015205141, - "heading": -0.02942623999470572, - "angularVelocity": -0.12385674180412554, - "velocityX": -0.6532963649564154, - "velocityY": -0.5410298480638316, - "timestamp": 4.862862260150834 - }, - { - "x": 7.703115992354989, - "y": 7.054628240082747, - "heading": -0.03428596694706339, - "angularVelocity": -0.05262863088743923, - "velocityX": -1.1473487504992854, - "velocityY": -0.641528970805246, - "timestamp": 4.955202244497977 - }, - { - "x": 7.551549165293792, - "y": 6.986109443661236, - "heading": -0.032557590593216855, - "angularVelocity": 0.018717529205428393, - "velocityX": -1.6413997482541873, - "velocityY": -0.7420273774784575, - "timestamp": 5.047542228845121 - }, - { - "x": 7.354361663520876, - "y": 6.908310952017223, - "heading": -0.024244154578006688, - "angularVelocity": 0.09003072801005149, - "velocityX": -2.1354508901756786, - "velocityY": -0.8425222528903508, - "timestamp": 5.139882213192264 - }, - { - "x": 7.111553334877506, - "y": 6.8212333658210484, - "heading": -0.00936157178577619, - "angularVelocity": 0.16117159752031956, - "velocityX": -2.6295036799070197, - "velocityY": -0.9430106233158517, - "timestamp": 5.232222197539407 - }, - { - "x": 6.823123896075064, - "y": 6.724877589969487, - "heading": 0.012063558679812484, - "angularVelocity": 0.23202441084506603, - "velocityX": -3.1235595375251535, - "velocityY": -1.0434891941211963, - "timestamp": 5.3245621818865505 - }, - { - "x": 6.48907294008907, - "y": 6.619244934399674, - "heading": 0.039997176732724696, - "angularVelocity": 0.3025083689412195, - "velocityX": -3.6176198030331097, - "velocityY": -1.1439535789035715, - "timestamp": 5.416902166233694 - }, - { - "x": 6.129437398233647, - "y": 6.5461675567988795, - "heading": 0.03999718077634422, - "angularVelocity": 4.3790558964884806e-8, - "velocityX": -3.8946892226384597, - "velocityY": -0.7913947367163172, - "timestamp": 5.509242150580837 - }, - { - "x": 5.769801333833775, - "y": 6.4730927508413325, - "heading": 0.03999718481982793, - "angularVelocity": 4.378908809561466e-8, - "velocityX": -3.8946948815570095, - "velocityY": -0.7913668869904851, - "timestamp": 5.60158213492798 - }, - { - "x": 5.410165307610781, - "y": 6.400017756997467, - "heading": 0.03999718886333287, - "angularVelocity": 4.378931804611701e-8, - "velocityX": -3.8946944681187916, - "velocityY": -0.7913689217137886, - "timestamp": 5.693922119275124 - }, - { - "x": 5.06488865586912, - "y": 6.3170886108245945, - "heading": 0.07172411836277942, - "angularVelocity": 0.3435882053019635, - "velocityX": -3.739188978456266, - "velocityY": -0.8980849061129225, - "timestamp": 5.786262103622267 - }, - { - "x": 4.765275429088764, - "y": 6.243389106592463, - "heading": 0.10120262922920165, - "angularVelocity": 0.3192388549211855, - "velocityX": -3.244674870790441, - "velocityY": -0.7981320849596799, - "timestamp": 5.87860208796941 - }, - { - "x": 4.511309122993011, - "y": 6.178941794592798, - "heading": 0.12636460911438926, - "angularVelocity": 0.27249278915397723, - "velocityX": -2.7503394969289876, - "velocityY": -0.6979350544113176, - "timestamp": 5.970942072316554 - }, - { - "x": 4.302982802001526, - "y": 6.123753419378413, - "heading": 0.14653342260027327, - "angularVelocity": 0.21841906979387646, - "velocityX": -2.256079232245732, - "velocityY": -0.5976649834259027, - "timestamp": 6.063282056663697 - }, - { - "x": 4.140292768892181, - "y": 6.077827039965534, - "heading": 0.16137256146424936, - "angularVelocity": 0.16070111955174202, - "velocityX": -1.7618590067952369, - "velocityY": -0.497361784687127, - "timestamp": 6.15562204101084 - }, - { - "x": 4.023236723432337, - "y": 6.041164401680778, - "heading": 0.17067956492366876, - "angularVelocity": 0.10079061118779088, - "velocityX": -1.2676636918172275, - "velocityY": -0.3970396848555328, - "timestamp": 6.2479620253579835 - }, - { - "x": 3.9518130737172363, - "y": 6.01376672295597, - "heading": 0.174318405736917, - "angularVelocity": 0.03940698971280611, - "velocityX": -0.7734856164431515, - "velocityY": -0.29670438996183723, - "timestamp": 6.340302009705127 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "angularVelocity": -0.023042788250028232, - "velocityX": -0.2793205093781995, - "velocityY": -0.19635795295345568, - "timestamp": 6.43264199405227 - }, - { - "x": 3.930355880820906, - "y": 5.98747940429267, - "heading": 0.1678304420080604, - "angularVelocity": -0.0673769466267154, - "velocityX": 0.06699164087576895, - "velocityY": -0.12602683733396786, - "timestamp": 6.497355419650052 - }, - { - "x": 3.957112124799613, - "y": 5.9838306427917995, - "heading": 0.16069467758998768, - "angularVelocity": -0.11026714089319978, - "velocityX": 0.41345738896603046, - "velocityY": -0.056383377439255714, - "timestamp": 6.562068845247834 - }, - { - "x": 4.006301141076061, - "y": 5.984635223325004, - "heading": 0.15089514245932092, - "angularVelocity": -0.15142970782560608, - "velocityX": 0.760105276796434, - "velocityY": 0.012432977017907793, - "timestamp": 6.6267822708456166 - }, - { - "x": 4.0779371329258005, - "y": 5.989827517197893, - "heading": 0.13856825674408835, - "angularVelocity": -0.1904842094413026, - "velocityX": 1.106972644208068, - "velocityY": 0.08023518806075768, - "timestamp": 6.691495696443399 - }, - { - "x": 4.17203754289526, - "y": 5.999325152011095, - "heading": 0.12388458270411208, - "angularVelocity": -0.22690305611760084, - "velocityX": 1.4541095468246137, - "velocityY": 0.14676451950228486, - "timestamp": 6.756209122041181 - }, - { - "x": 4.288624284050538, - "y": 6.0130216798691905, - "heading": 0.10706400495167072, - "angularVelocity": -0.25992408216784824, - "velocityX": 1.801585066442124, - "velocityY": 0.21164893886513167, - "timestamp": 6.820922547638963 - }, - { - "x": 4.427725657084912, - "y": 6.0307742700897, - "heading": 0.0884013396467056, - "angularVelocity": -0.2883893895674757, - "velocityX": 2.1494979094282636, - "velocityY": 0.27432623225435043, - "timestamp": 6.885635973236745 - }, - { - "x": 4.589379469563093, - "y": 6.0523813458045, - "heading": 0.06831284216496007, - "angularVelocity": -0.3104224092014938, - "velocityX": 2.497994983033644, - "velocityY": 0.3338886099014062, - "timestamp": 6.950349398834527 - }, - { - "x": 4.773638315970202, - "y": 6.077537273797697, - "heading": 0.04742981182100691, - "angularVelocity": -0.32270012213151367, - "velocityX": 2.847304785754114, - "velocityY": 0.3887281157012392, - "timestamp": 7.015062824432309 - }, - { - "x": 4.980578361520177, - "y": 6.105724638750597, - "heading": 0.026818564360069696, - "angularVelocity": -0.31850033081301815, - "velocityX": 3.197791550028341, - "velocityY": 0.43557213503258413, - "timestamp": 7.079776250030092 - }, - { - "x": 5.210305960828753, - "y": 6.1358813908951575, - "heading": 0.008654103339561452, - "angularVelocity": -0.28069076629334544, - "velocityX": 3.549921784954145, - "velocityY": 0.4660045711687753, - "timestamp": 7.144489675627874 - }, - { - "x": 5.462718064244522, - "y": 6.164655483095359, - "heading": 8.629838781735336e-9, - "angularVelocity": -0.13372951021808044, - "velocityX": 3.900459619996067, - "velocityY": 0.44463868099074777, - "timestamp": 7.209203101225656 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "angularVelocity": -1.3335468957212507e-7, - "velocityX": 3.9730265373303566, - "velocityY": 0.09983815454439177, - "timestamp": 7.273916526823438 - }, - { - "x": 5.995970674250576, - "y": 6.151565167078146, - "heading": -2.5127315305597956e-15, - "angularVelocity": -3.607302360522835e-14, - "velocityX": 3.964357212211523, - "velocityY": -0.28067875524252794, - "timestamp": 7.343573330689117 - }, - { - "x": 6.26897851241587, - "y": 6.105687844731353, - "heading": -2.5097428721994853e-15, - "angularVelocity": 4.2905476485829695e-17, - "velocityX": 3.9193276609666734, - "velocityY": -0.6586193996970835, - "timestamp": 7.413230134554796 - }, - { - "x": 6.536346012712411, - "y": 6.0339060327816165, - "heading": -2.5126951036173778e-15, - "angularVelocity": -4.2382527679423133e-17, - "velocityX": 3.838354409887958, - "velocityY": -1.0305068272755022, - "timestamp": 7.482886938420475 - }, - { - "x": 6.787193967022678, - "y": 5.960199932024534, - "heading": -2.073411284199746e-15, - "angularVelocity": 6.306402166664137e-15, - "velocityX": 3.6011981657092096, - "velocityY": -1.058132108663187, - "timestamp": 7.552543742286154 - }, - { - "x": 7.012681141047439, - "y": 5.8942233269936395, - "heading": -1.671095384485508e-15, - "angularVelocity": 5.7756870204886894e-15, - "velocityX": 3.2371162831352196, - "velocityY": -0.9471666997261698, - "timestamp": 7.622200546151833 - }, - { - "x": 7.212807520844226, - "y": 5.835976237454582, - "heading": -1.3121610031518989e-15, - "angularVelocity": 5.1528976439448214e-15, - "velocityX": 2.8730342004018077, - "velocityY": -0.8362010070315249, - "timestamp": 7.6918573500175125 - }, - { - "x": 7.387573101757002, - "y": 5.785458669967885, - "heading": -9.95057743236105e-16, - "angularVelocity": 4.552365916666039e-15, - "velocityX": 2.508952050825841, - "velocityY": -0.7252352201533442, - "timestamp": 7.7615141538831915 - }, - { - "x": 7.536977881454969, - "y": 5.7426706278046975, - "heading": -7.196116224916786e-16, - "angularVelocity": 3.954331894676066e-15, - "velocityX": 2.144869867788704, - "velocityY": -0.6142693863142378, - "timestamp": 7.831170957748871 - }, - { - "x": 7.66102185853843, - "y": 5.707612112923722, - "heading": -4.889855482801724e-16, - "angularVelocity": 3.3108908490429318e-15, - "velocityX": 1.7807876646574228, - "velocityY": -0.5033035243557882, - "timestamp": 7.90082776161455 - }, - { - "x": 7.759705032073642, - "y": 5.680283126628732, - "heading": -3.021004962604421e-16, - "angularVelocity": 2.6829403813236554e-15, - "velocityX": 1.4167054481211605, - "velocityY": -0.3923376436802606, - "timestamp": 7.970484565480229 - }, - { - "x": 7.833027401393294, - "y": 5.660683669849869, - "heading": -1.6049621497454911e-16, - "angularVelocity": 2.0328851408532914e-15, - "velocityX": 1.0526232220049963, - "velocityY": -0.281371749651518, - "timestamp": 8.040141369345909 - }, - { - "x": 7.880988965996717, - "y": 5.648813743284097, - "heading": -5.830363906716236e-17, - "angularVelocity": 1.467086785481853e-15, - "velocityX": 0.6885409887010987, - "velocityY": -0.17040584561707373, - "timestamp": 8.109798173211589 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 2.0598914784739755e-32, - "angularVelocity": 8.370128572804695e-16, - "velocityX": 0.3244587498051791, - "velocityY": -0.05943993380554415, - "timestamp": 8.179454977077269 - }, - { - "x": 7.9005397389522525, - "y": 5.6483789255306105, - "heading": 2.097139947940835e-17, - "angularVelocity": 2.980227828531976e-16, - "velocityX": -0.04334309998236763, - "velocityY": 0.05265965538925957, - "timestamp": 8.249823421958254 - }, - { - "x": 7.871608103494799, - "y": 5.6599727605367445, - "heading": -9.084634582304173e-18, - "angularVelocity": -4.2712371472453026e-16, - "velocityX": -0.41114501686552, - "velocityY": 0.16475900563136703, - "timestamp": 8.32019186683924 - }, - { - "x": 7.816794813351453, - "y": 5.6794548319403075, - "heading": -7.520085122829124e-17, - "angularVelocity": -9.395719591601255e-16, - "velocityX": -0.7789470157537616, - "velocityY": 0.27685806382157147, - "timestamp": 8.390560311720225 - }, - { - "x": 7.736099861309118, - "y": 5.706825114052552, - "heading": -1.8616176975942832e-16, - "angularVelocity": -1.5768562134134974e-15, - "velocityX": -1.1467491171467201, - "velocityY": 0.388956756951142, - "timestamp": 8.460928756601211 - }, - { - "x": 7.629523238094034, - "y": 5.742083573845685, - "heading": -3.3813406750936066e-16, - "angularVelocity": -2.1596654705354767e-15, - "velocityX": -1.5145513503282884, - "velocityY": 0.5010549807255615, - "timestamp": 8.531297201482197 - }, - { - "x": 7.497064931341595, - "y": 5.785230167283751, - "heading": -5.316654292279505e-16, - "angularVelocity": -2.750257804825092e-15, - "velocityX": -1.8823537592222348, - "velocityY": 0.6131525787087516, - "timestamp": 8.601665646363182 - }, - { - "x": 7.338724923742105, - "y": 5.836264832718443, - "heading": -7.669822112767176e-16, - "angularVelocity": -3.3440668612043406e-15, - "velocityX": -2.2501564141027943, - "velocityY": 0.7252493006130729, - "timestamp": 8.672034091244168 - }, - { - "x": 7.154503189332537, - "y": 5.895187477681304, - "heading": -1.0519707319411395e-15, - "angularVelocity": -4.0499477067678845e-15, - "velocityX": -2.617959437941741, - "velocityY": 0.8373447084559508, - "timestamp": 8.742402536125153 - }, - { - "x": 6.944399684844722, - "y": 5.961997948067273, - "heading": -1.3745422496318406e-15, - "angularVelocity": -4.5840364940621565e-15, - "velocityX": -2.985763076660981, - "velocityY": 0.9494379263248582, - "timestamp": 8.812770981006139 - }, - { - "x": 6.708414323753371, - "y": 6.036695935692792, - "heading": -1.7429472421431418e-15, - "angularVelocity": -5.235372077575223e-15, - "velocityX": -3.3535679449837517, - "velocityY": 1.061526764623348, - "timestamp": 8.883139425887125 - }, - { - "x": 6.4465468465502065, - "y": 6.119280516151445, - "heading": -2.1550026112189195e-15, - "angularVelocity": -5.855683878769628e-15, - "velocityX": -3.7213765011567173, - "velocityY": 1.1736024662605187, - "timestamp": 8.95350787076811 - }, - { - "x": 6.173126442867114, - "y": 6.178044585557323, - "heading": -2.191046659518503e-15, - "angularVelocity": -5.122189123349113e-16, - "velocityX": -3.8855541591979326, - "velocityY": 0.8350912046286955, - "timestamp": 9.023876315649096 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "angularVelocity": 3.113677831051976e-14, - "velocityX": -3.9480676215152357, - "velocityY": 0.45570865426474233, - "timestamp": 9.094244760530081 - }, - { - "x": 5.637913658685597, - "y": 6.216758064974601, - "heading": 9.67763523798797e-9, - "angularVelocity": 1.4937766527408904e-7, - "velocityX": -3.972956719905001, - "velocityY": 0.10258285959556052, - "timestamp": 9.159031120794502 - }, - { - "x": 5.380949926914102, - "y": 6.200473867453901, - "heading": 1.892686165769219e-8, - "angularVelocity": 1.4276502617102237e-7, - "velocityX": -3.966324558486143, - "velocityY": -0.2513522514767974, - "timestamp": 9.223817481058923 - }, - { - "x": 5.132255360579735, - "y": 6.163185174806636, - "heading": 0.01632330814348079, - "angularVelocity": 0.25195564544398485, - "velocityX": -3.8386871143710315, - "velocityY": -0.5755639381137804, - "timestamp": 9.288603841323344 - }, - { - "x": 4.905757766533857, - "y": 6.129190200240601, - "heading": 0.04988895608633859, - "angularVelocity": 0.5180974483811885, - "velocityX": -3.4960691281513077, - "velocityY": -0.5247242541278667, - "timestamp": 9.353390201587764 - }, - { - "x": 4.701928110746201, - "y": 6.098578355375242, - "heading": 0.08643838460715948, - "angularVelocity": 0.5641531392054652, - "velocityX": -3.146181618409506, - "velocityY": -0.4725044707141436, - "timestamp": 9.418176561852185 - }, - { - "x": 4.520779220798307, - "y": 6.071364064096333, - "heading": 0.12231563080100817, - "angularVelocity": 0.5537777712384655, - "velocityX": -2.7960961104853954, - "velocityY": -0.4200620496107946, - "timestamp": 9.482962922116606 - }, - { - "x": 4.362300886297595, - "y": 6.047552197658039, - "heading": 0.15578751714707642, - "angularVelocity": 0.5166502055290209, - "velocityX": -2.4461682035211605, - "velocityY": -0.3675444390069978, - "timestamp": 9.547749282381027 - }, - { - "x": 4.226482344491556, - "y": 6.027144059810235, - "heading": 0.1858309320626014, - "angularVelocity": 0.4637305567548124, - "velocityX": -2.096406423384503, - "velocityY": -0.3150067045713868, - "timestamp": 9.612535642645447 - }, - { - "x": 4.113314607743797, - "y": 6.010139422555146, - "heading": 0.21176520360476983, - "angularVelocity": 0.40030449984110905, - "velocityX": -1.7467833705408997, - "velocityY": -0.26247248936131745, - "timestamp": 9.677322002909868 - }, - { - "x": 4.022790393937899, - "y": 5.996537426116962, - "heading": 0.23310242597379685, - "angularVelocity": 0.3293474472395599, - "velocityX": -1.3972727196965211, - "velocityY": -0.20995154509025238, - "timestamp": 9.742108363174289 - }, - { - "x": 3.954903777130295, - "y": 5.986337041905185, - "heading": 0.2494756207657714, - "angularVelocity": 0.2527259553577121, - "velocityX": -1.0478535378517617, - "velocityY": -0.1574464774715696, - "timestamp": 9.80689472343871 - }, - { - "x": 3.9096498799999146, - "y": 5.97953733708754, - "heading": 0.260600273972009, - "angularVelocity": 0.17171289081270033, - "velocityX": -0.6985096391537948, - "velocityY": -0.10495580844363857, - "timestamp": 9.87168108370313 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 0.08723493748434215, - "velocityX": -0.3492284312714647, - "velocityY": -0.052475536233735576, - "timestamp": 9.936467443967551 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 3.6116165249279265e-32, - "velocityX": -3.053502496463557e-33, - "velocityY": 4.523388089316994e-32, - "timestamp": 10.001253804231972 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path deleted file mode 100644 index 74c48a8..0000000 --- a/src/main/deploy/pathplanner/New Path.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.0, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.0, - "y": 5.0 - }, - "prevControl": { - "x": 3.0, - "y": 4.0 - }, - "nextControl": { - "x": 3.0, - "y": 4.0 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 3.0 - }, - "prevControl": { - "x": 4.0, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Piece HFE.auto b/src/main/deploy/pathplanner/autos/3 Piece HFE.auto deleted file mode 100644 index c79fee3..0000000 --- a/src/main/deploy/pathplanner/autos/3 Piece HFE.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3992020999924704, - "y": 2.850407598330388 - }, - "rotation": -63.38831811724382 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To H (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "H To Shoot (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "HShoot To F (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "F To Shoot (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "FShoot To E (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "E To Shoot (HFE)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto b/src/main/deploy/pathplanner/autos/4 Piece ABC.auto deleted file mode 100644 index 14ca76d..0000000 --- a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.41, - "y": 4.3 - }, - "rotation": 0.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To C (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "C To B (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "B To A (ABC)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto b/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto deleted file mode 100644 index 4e8d00e..0000000 --- a/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.41, - "y": 4.3 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To C (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "C To B (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "B To A (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "A To D (ABCD)" - } - }, - { - "type": "path", - "data": { - "pathName": "D To Shoot (ABCD)" - } - }, - { - "type": "path", - "data": { - "pathName": "DShoot To E (ABCDE)" - } - }, - { - "type": "path", - "data": { - "pathName": "E To Shoot (ABCDE)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/1 Note + Mobility.path similarity index 69% rename from src/main/deploy/pathplanner/paths/C To B (ABC).path rename to src/main/deploy/pathplanner/paths/1 Note + Mobility.path index 92179ad..737d557 100644 --- a/src/main/deploy/pathplanner/paths/C To B (ABC).path +++ b/src/main/deploy/pathplanner/paths/1 Note + Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 4.3 + "x": 0.9536854768153984, + "y": 3.918240646666479 }, "prevControl": null, "nextControl": { - "x": 2.1669872981077813, - "y": 4.55 + "x": 2.0332340849899873, + "y": 5.105453132209409 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 5.54 + "x": 2.393756643998068, + "y": 4.492368813448178 }, "prevControl": { - "x": 2.166987298107781, - "y": 5.29 + "x": 2.4912657927200694, + "y": 4.385180011107611 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": -39.896392922718064, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 6.path b/src/main/deploy/pathplanner/paths/2 Note Center 6.path new file mode 100644 index 0000000..5c036bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 6.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1969112668610487, + "y": 3.548383473371797 + }, + "prevControl": null, + "nextControl": { + "x": 4.405680480405183, + "y": 2.6115165497092763 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.354258240613484, + "y": 3.8645760601078973 + }, + "prevControl": { + "x": 4.065145382336701, + "y": 3.5048236345422836 + }, + "nextControl": { + "x": 7.368522126487903, + "y": 4.42669621430541 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.328810723241988, + "y": 4.075371117931965 + }, + "prevControl": { + "x": 7.33338961685056, + "y": 4.356431195030721 + }, + "nextControl": { + "x": 9.013237768290756, + "y": 3.882121128741725 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.354258240613484, + "y": 3.8645760601078973 + }, + "prevControl": { + "x": 7.591028020857753, + "y": 4.836575493407763 + }, + "nextControl": { + "x": 3.1682196012318817, + "y": 2.914622096397567 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1969112668610487, + "y": 3.548383473371797 + }, + "prevControl": { + "x": 4.534499682408779, + "y": 2.962841646082722 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 7.path b/src/main/deploy/pathplanner/paths/2 Note Center 7.path new file mode 100644 index 0000000..f2301f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 7.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1266462475863597, + "y": 3.9231302428368044 + }, + "prevControl": null, + "nextControl": { + "x": 3.398548537464012, + "y": 3.0096849922597873 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.089487893665121, + "y": 2.576384040065871 + }, + "prevControl": { + "x": 3.095128808200161, + "y": 3.32362429955275 + }, + "nextControl": { + "x": 6.068619269906157, + "y": 1.08910779875768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.164859011597086, + "y": 2.2953239629671147 + }, + "prevControl": { + "x": 6.9991342346038525, + "y": 1.9236435992811916 + }, + "nextControl": { + "x": 8.972906733259972, + "y": 2.5529623669803687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.089487893665121, + "y": 2.576384040065871 + }, + "prevControl": { + "x": 6.068619269906157, + "y": 0.7963368851131419 + }, + "nextControl": { + "x": 3.381204638750186, + "y": 3.213419866968167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1266462475823988, + "y": 3.8879977331934006 + }, + "prevControl": { + "x": 2.3445732483476367, + "y": 3.325877579001948 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 8.path b/src/main/deploy/pathplanner/paths/2 Note Center 8.path new file mode 100644 index 0000000..3d2797b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 8.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1266462475863597, + "y": 3.9231302428368044 + }, + "prevControl": null, + "nextControl": { + "x": 2.3914165945307624, + "y": 2.9862633191742836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.31199378803893, + "y": 2.0611072320575454 + }, + "prevControl": { + "x": 3.007825058291836, + "y": 2.4958301419732445 + }, + "nextControl": { + "x": 5.647029154258021, + "y": 1.616095443317847 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 0.749493538930016 + }, + "prevControl": { + "x": 7.602738857403534, + "y": 0.8080477216589235 + }, + "nextControl": { + "x": 8.556589177984769, + "y": 0.7258192457467483 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.183174586035333, + "y": 2.0611072320575454 + }, + "prevControl": { + "x": 4.710162230595501, + "y": 1.8034688280503517 + }, + "nextControl": { + "x": 3.978896971568412, + "y": 2.1609762880191514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0798029014032335, + "y": 3.9231302428368044 + }, + "prevControl": { + "x": 2.4733924503512332, + "y": 2.7520465882586542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path b/src/main/deploy/pathplanner/paths/2 Note Mobility.path similarity index 53% rename from src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path rename to src/main/deploy/pathplanner/paths/2 Note Mobility.path index 99943a5..9fb93ba 100644 --- a/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path +++ b/src/main/deploy/pathplanner/paths/2 Note Mobility.path @@ -3,47 +3,53 @@ "waypoints": [ { "anchor": { - "x": 7.954724143027569, - "y": 7.431149973519566 + "x": 1.4186651567747581, + "y": 4.0749560202824116 }, "prevControl": null, "nextControl": { - "x": 6.1477623701808435, - "y": 6.910372175243665 + "x": 1.4275950826207213, + "y": 4.085166421779603 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { "anchor": { - "x": 3.3531437468690912, - "y": 6.2 + "x": 2.44939980620702, + "y": 4.0749560202824116 }, "prevControl": { - "x": 5.055355188404675, - "y": 6.647601405814684 + "x": 2.3694684333813116, + "y": 4.087961199070547 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -0.08105107436216544, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 11.56141512604754, + "rotation": -0.42006383232481037, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note (4,1).path b/src/main/deploy/pathplanner/paths/3 Note (4,1).path new file mode 100644 index 0000000..f41e811 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note (4,1).path @@ -0,0 +1,181 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2234294142773756, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.2234294142773756, + "y": 6.799868019709742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 6.244414694343174 + }, + "prevControl": { + "x": 1.964117812947406, + "y": 6.244414694343174 + }, + "nextControl": { + "x": 3.4142135623730976, + "y": 6.244414694343174 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.276640929226852, + "y": 6.244414694343174 + }, + "prevControl": { + "x": 3.894920138865978, + "y": 6.244414694343174 + }, + "nextControl": { + "x": 6.6583617195877265, + "y": 6.244414694343174 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.290831218253917, + "y": 5.771197106303988 + }, + "prevControl": { + "x": 6.4380269751951875, + "y": 6.285864951635245 + }, + "nextControl": { + "x": 8.476003317926567, + "y": 5.719760411946759 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.097853218268906, + "y": 6.151828644514233 + }, + "prevControl": { + "x": 4.8239980189294345, + "y": 6.923379059795515 + }, + "nextControl": { + "x": 1.8891000945282543, + "y": 6.092747571757445 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9435431352126493, + "y": 6.162115983384651 + }, + "prevControl": { + "x": 1.9398341418478882, + "y": 6.261045103573999 + }, + "nextControl": { + "x": 1.9472521285774105, + "y": 6.063186863195303 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.900265650161439, + "y": 6.974815754147602 + }, + "prevControl": { + "x": 3.0545757332176953, + "y": 7.1188384983334405 + }, + "nextControl": { + "x": 2.0955600294777064, + "y": 6.2237571748427865 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0875658793984893, + "y": 6.56 + }, + "prevControl": { + "x": 2.1492899126209917, + "y": 6.724597421926673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -15.592810939801673, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 15.018360631150516, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 175.81508387488162, + "rotateFast": false + }, + { + "waypointRelativePos": 6.0, + "rotationDegrees": 54.293308599397186, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 50.979566839281325, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.44430514468718, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path b/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path new file mode 100644 index 0000000..32fccca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path @@ -0,0 +1,155 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1613758933368123, + "y": 3.534963938258926 + }, + "prevControl": null, + "nextControl": { + "x": 2.622104793438532, + "y": 3.2622945435689217 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 2.424809974168226 + }, + "prevControl": { + "x": 5.816231988338233, + "y": 0.4382186700247651 + }, + "nextControl": { + "x": 8.092728599078075, + "y": 2.7837000265429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 6.410261741050422, + "y": 0.16554927534895622 + }, + "nextControl": { + "x": 2.6293247577370926, + "y": 3.3179668640207294 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.183249464961813, + "y": 3.710251406271605 + }, + "prevControl": { + "x": 3.6154004455152573, + "y": 2.5319300935198457 + }, + "nextControl": { + "x": 5.753399221163684, + "y": 4.1387490491313965 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6275358244666664, + "y": 4.021873571623774 + }, + "prevControl": { + "x": 8.046220471824721, + "y": 4.0338359901197185 + }, + "nextControl": { + "x": 6.605025594392825, + "y": 3.992658993621665 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.183249464961813, + "y": 3.710251406271605 + }, + "prevControl": { + "x": 5.787017410338748, + "y": 3.8660624889495243 + }, + "nextControl": { + "x": 4.603138511506378, + "y": 3.5605453537669765 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 4.667125253586213, + "y": 2.5514064788509176 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -29.744881296441672, + "rotateFast": false + }, + { + "waypointRelativePos": 5.1499999999999995, + "rotationDegrees": 26.88010672415831, + "rotateFast": false + }, + { + "waypointRelativePos": 2.9499999999999997, + "rotationDegrees": 26.290465162772012, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.529705899934235, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path b/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path new file mode 100644 index 0000000..4ce8c21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path @@ -0,0 +1,113 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1613758933368123, + "y": 3.534963938258926 + }, + "prevControl": null, + "nextControl": { + "x": 7.734655943810105, + "y": 0.8180081840521911 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 0.7887936060606108 + }, + "prevControl": { + "x": 6.93406416515882, + "y": 1.2579320105256153 + }, + "nextControl": { + "x": 8.299471118517559, + "y": 0.46743324802687497 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 3.1827465189114155, + "y": 2.927907728824032 + }, + "nextControl": { + "x": 2.0183368480639654, + "y": 3.739465984263165 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 2.424809974168226 + }, + "prevControl": { + "x": 5.816231988338233, + "y": 0.4382186700247651 + }, + "nextControl": { + "x": 8.092728599078075, + "y": 2.7837000265429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 6.05968680502248, + "y": 0.4187422846900244 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -28.393019422172674, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.529705899934235, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Mobility.path b/src/main/deploy/pathplanner/paths/3 Note Mobility.path new file mode 100644 index 0000000..f733e82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Mobility.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3931892586007395, + "y": 4.300319734898729 + }, + "prevControl": null, + "nextControl": { + "x": 1.833108731032277, + "y": 4.274843836724711 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.260023024675679, + "y": 4.11628754738991 + }, + "prevControl": { + "x": 1.843378826811743, + "y": 4.100841889320345 + }, + "nextControl": { + "x": 2.5147226220378367, + "y": 4.125729663496364 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.260023024675679, + "y": 5.517046256314735 + }, + "prevControl": { + "x": 1.674611826075504, + "y": 4.857107663289401 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path b/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path new file mode 100644 index 0000000..47e1bd7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3140196323188642, + "y": 7.026501927468906 + }, + "prevControl": null, + "nextControl": { + "x": 2.8062548621530805, + "y": 7.275509749307659 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 7.436381206571257 + }, + "prevControl": { + "x": 7.406875539339101, + "y": 7.983313605146107 + }, + "nextControl": { + "x": 8.65671414652387, + "y": 7.202164475655628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.618501382509183, + "y": 7.436381206571257 + }, + "prevControl": { + "x": 2.262597392527166, + "y": 7.576911245120637 + }, + "nextControl": { + "x": 0.8241493550218395, + "y": 7.263068036937653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 5.855418272890754 + }, + "prevControl": { + "x": 6.841534481927736, + "y": 6.218454205809981 + }, + "nextControl": { + "x": 9.515985591469327, + "y": 5.544405552185677 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.618501382509183, + "y": 7.22558614874719 + }, + "prevControl": { + "x": 2.649054998537956, + "y": 6.956236908194216 + }, + "nextControl": { + "x": -0.10503619526560826, + "y": 7.676056197483782 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9535367487282747, + "y": 7.026501927468906 + }, + "prevControl": { + "x": 2.8012958736331153, + "y": 6.113056676897948 + }, + "nextControl": { + "x": 3.085046944580154, + "y": 7.815563102580183 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5013930170513679, + "y": 6.5463576290918635 + }, + "prevControl": { + "x": 2.2274648828898207, + "y": 7.4480920431170405 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note End 4.path b/src/main/deploy/pathplanner/paths/4 Note End 4.path new file mode 100644 index 0000000..02d4dc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note End 4.path @@ -0,0 +1,113 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3990197549172392, + "y": 4.122752562280961 + }, + "prevControl": null, + "nextControl": { + "x": 0.86189001475465, + "y": 4.122752562280961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 4.417663265882087 + }, + "prevControl": { + "x": 2.2988487262132926, + "y": 4.417894718409181 + }, + "nextControl": { + "x": 2.639014036486218, + "y": 4.4121352736264425 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 5.51870901890497 + }, + "prevControl": { + "x": 1.7370438094738079, + "y": 5.455725753327694 + }, + "nextControl": { + "x": 2.630607491776283, + "y": 5.553522457244724 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 6.69 + }, + "prevControl": { + "x": 2.0244122309425774, + "y": 6.456084900181747 + }, + "nextControl": { + "x": 2.7512539157284497, + "y": 6.813577788583229 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.560791376212382, + "y": 7.402737796216334 + }, + "prevControl": { + "x": 7.219449971307349, + "y": 7.327200867364698 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": -0.04782618214180992, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": -34.50386141489783, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 38.59798223150914, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.3687240473329847, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path b/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path new file mode 100644 index 0000000..dc29915 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3990197549172392, + "y": 4.122752562280961 + }, + "prevControl": null, + "nextControl": { + "x": 0.86189001475465, + "y": 4.122752562280961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 4.417663265882087 + }, + "prevControl": { + "x": 2.2988487262132926, + "y": 4.417894718409181 + }, + "nextControl": { + "x": 2.639014036486218, + "y": 4.4121352736264425 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 5.51870901890497 + }, + "prevControl": { + "x": 1.7370438094738079, + "y": 5.455725753327694 + }, + "nextControl": { + "x": 2.630607491776283, + "y": 5.553522457244724 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 2.500232833799001, + "y": 6.68953141609987 + }, + "prevControl": { + "x": 2.0248015033058433, + "y": 6.455735119803573 + }, + "nextControl": null, + "isLocked": true, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": -0.04782618214180992, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": -34.50386141489783, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.611383776770865, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A To D (ABCD).path b/src/main/deploy/pathplanner/paths/A To D (ABCD).path deleted file mode 100644 index c1343d5..0000000 --- a/src/main/deploy/pathplanner/paths/A To D (ABCD).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.6, - "y": 6.78 - }, - "prevControl": null, - "nextControl": { - "x": 3.5902680687415707, - "y": 6.919173100960065 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.954724143027569, - "y": 7.431149973519566 - }, - "prevControl": { - "x": 6.964456074285999, - "y": 7.2919768725595 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path similarity index 66% rename from src/main/deploy/pathplanner/paths/Start To C (ABC).path rename to src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path index 08d4184..005509a 100644 --- a/src/main/deploy/pathplanner/paths/Start To C (ABC).path +++ b/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.41, - "y": 4.3 + "x": 0.8539197217278677, + "y": 3.7498265505812527 }, "prevControl": null, "nextControl": { - "x": 1.91, - "y": 4.3 + "x": 1.8328118091416719, + "y": 3.7026753544270083 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 4.3 + "x": 2.667281089191343, + "y": 3.3374614295547143 }, "prevControl": { - "x": 2.1000000000000005, - "y": 4.3 + "x": 2.4942943959957344, + "y": 3.3668566966785813 }, "nextControl": null, "isLocked": false, @@ -39,14 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": -31.56492690745628, + "rotation": -52.08361958069638, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, + "folder": null, + "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path b/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path deleted file mode 100644 index b75ad29..0000000 --- a/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.3531437468690912, - "y": 6.2 - }, - "prevControl": null, - "nextControl": { - "x": 5.119271878480345, - "y": 6.2 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.954724143027569, - "y": 5.729888218095302 - }, - "prevControl": { - "x": 7.29352084832642, - "y": 6.172397406137405 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/Do Nothing.path similarity index 73% rename from src/main/deploy/pathplanner/paths/B To A (ABC).path rename to src/main/deploy/pathplanner/paths/Do Nothing.path index 6c0fb53..6f67a21 100644 --- a/src/main/deploy/pathplanner/paths/B To A (ABC).path +++ b/src/main/deploy/pathplanner/paths/Do Nothing.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 5.54 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 2.166987298107781, - "y": 5.79 + "x": 2.0166698250657547, + "y": 6.979658581285779 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 6.78 + "x": 2.0, + "y": 7.0 }, "prevControl": { - "x": 2.166987298107781, - "y": 6.53 + "x": 1.9932481519741914, + "y": 6.9679477447399965 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 30.0, + "rotation": -1.5074357587748137, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path b/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path deleted file mode 100644 index b135cef..0000000 --- a/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.954724143027569, - "y": 5.729888218095302 - }, - "prevControl": null, - "nextControl": { - "x": 6.6499751272795375, - "y": 6.343712985820069 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.3531437468690912, - "y": 6.2 - }, - "prevControl": { - "x": 4.157316983981167, - "y": 6.2 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 10.353493353078926, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path deleted file mode 100644 index da00da5..0000000 --- a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.929545854615797, - "y": 4.097291348201403 - }, - "prevControl": null, - "nextControl": { - "x": 5.929545854615797, - "y": 4.097291348201403 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.976354741378645, - "y": 5.106639879824356 - }, - "prevControl": { - "x": 5.288791546143204, - "y": 4.49370079566982 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -6.366672483399156, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path deleted file mode 100644 index 5f9ec94..0000000 --- a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.929545854615797, - "y": 2.432016158384531 - }, - "prevControl": null, - "nextControl": { - "x": 6.995066434251791, - "y": 3.4562610112362155 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 - }, - "prevControl": { - "x": 4.259643243410917, - "y": 4.815422774902082 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -13.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path deleted file mode 100644 index 6632f81..0000000 --- a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 - }, - "prevControl": null, - "nextControl": { - "x": 5.639808720888058, - "y": 4.489969784247294 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.929545854615797, - "y": 4.097291348201403 - }, - "prevControl": { - "x": 6.460969663475785, - "y": 4.359129446883026 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path deleted file mode 100644 index 0b9e815..0000000 --- a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 - }, - "prevControl": null, - "nextControl": { - "x": 5.970951958009818, - "y": 1.1314614999083628 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 - }, - "prevControl": { - "x": 4.464336906902614, - "y": 1.7553300211016838 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -40.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path deleted file mode 100644 index 09bd3d4..0000000 --- a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 - }, - "prevControl": null, - "nextControl": { - "x": 5.429728048854969, - "y": 1.1066294807828339 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.929545854615797, - "y": 2.432016158384531 - }, - "prevControl": { - "x": 7.178392770949869, - "y": 2.432016158384531 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To H (HFE).path b/src/main/deploy/pathplanner/paths/Mobility.path similarity index 64% rename from src/main/deploy/pathplanner/paths/Start To H (HFE).path rename to src/main/deploy/pathplanner/paths/Mobility.path index a53726c..1e817ae 100644 --- a/src/main/deploy/pathplanner/paths/Start To H (HFE).path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3992020999924704, - "y": 2.850407598330388 + "x": 1.4882042634455643, + "y": 2.0023818427266487 }, "prevControl": null, "nextControl": { - "x": 3.361176508628245, - "y": 2.06581107755857 + "x": 1.4889168759819007, + "y": 1.9765496382844625 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 2.5869934100979313, + "y": 2.0023818427266487 }, "prevControl": { - "x": 5.745230507252463, - "y": 1.1502945514300327 + "x": 2.5638038104779914, + "y": 1.930912743769932 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 10.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "HFE", + "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8455d54..7cfa815 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,13 +5,10 @@ package com.stuypulse.robot; -import com.pathplanner.lib.auto.AutoBuilder; +import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -25,7 +22,6 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -45,25 +41,16 @@ public class RobotContainer { public final AbstractNoteVision noteVision = AbstractNoteVision.getInstance(); // Autons - private static SendableChooser autonChooser; + private static SendableChooser autonChooser = new SendableChooser<>(); // Robot container public RobotContainer() { configureDefaultCommands(); configureButtonBindings(); - configureNamedCommands(); - - swerve.configureAutoBuilder(); configureAutons(); } - /**********************/ - /*** NAMED COMMANDS ***/ - /**********************/ - - private void configureNamedCommands() {} - /****************/ /*** DEFAULTS ***/ /****************/ @@ -99,7 +86,8 @@ private void configureButtonBindings() { /**************/ public void configureAutons() { - autonChooser = AutoBuilder.buildAutoChooser(); + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java deleted file mode 100644 index 2581431..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ /dev/null @@ -1,81 +0,0 @@ -/************************ PROJECT JON *************************/ -/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ -/* This work is licensed under the terms of the MIT license. */ -/**************************************************************/ - -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; - -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Alignment.Rotation; -import com.stuypulse.robot.constants.Settings.Alignment.Translation; -import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.HolonomicController; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - - -public class SwerveDriveToPose extends Command { - private final SwerveDrive swerve; - private final Pose2d targetPose; - - // Holonomic control - private final HolonomicController controller; - private final BStream aligned; - - private final FieldObject2d targetPose2d; - - public SwerveDriveToPose(Pose2d targetPose){ - this.swerve = SwerveDrive.getInstance(); - this.targetPose = targetPose; - - controller = new HolonomicController( - new PIDController(Translation.P,Translation.I,Translation.D), - new PIDController(Translation.P, Translation.I, Translation.D), - new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); - - SmartDashboard.putData("Alignment/Controller", controller); - - aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); - - targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose"); - addRequirements(swerve); - } - - private boolean isAligned() { - return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get()); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - Pose2d currentPose = AbstractOdometry.getInstance().getPose(); - - targetPose2d.setPose(targetPose); - - controller.update(targetPose, currentPose); - swerve.setChassisSpeeds(controller.getOutput()); - } - - @Override - public boolean isFinished(){ - return aligned.get(); - } - - public void end(boolean interupted) { - swerve.stop(); - targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java deleted file mode 100644 index 4bf65bc..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java +++ /dev/null @@ -1,64 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.vectors.VStream; -import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; -import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; -import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveDriveWithAiming extends Command { - - private final SwerveDrive swerve; - - private VStream speed; - private final AngleController gyroFeedback; - private final Pose2d target; - - public SwerveDriveWithAiming(Pose2d target, Gamepad driver) { - this.swerve = SwerveDrive.getInstance(); - - this.speed = VStream.create(driver::getLeftStick) - .filtered( - new VDeadZone(Drive.DEADBAND), - x -> x.clamp(1.0), - x -> Settings.vpow(x, Drive.POWER.get()), - x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), - new VRateLimit(Drive.MAX_TELEOP_ACCEL), - new VLowPassFilter(Drive.RC) - ); - - this.gyroFeedback = new AnglePIDController(Alignment.Gyro.P.get(), Alignment.Gyro.I.get(), Alignment.Gyro.D.get()); - this.target = target; - - addRequirements(swerve); - } - - @Override - public void execute() { - - Pose2d robotPose = AbstractOdometry.getInstance().getPose(); - - Rotation2d target = new Rotation2d( - robotPose.getX() - this.target.getX(), - robotPose.getY() - this.target.getY()) - .minus(Rotation2d.fromDegrees(180)); - - double angularVel = -gyroFeedback.update( - Angle.fromRotation2d(target.plus(Rotation2d.fromDegrees(180))), - Angle.fromRotation2d(swerve.getGyroAngle())); - - swerve.drive(speed.get(), angularVel); - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 0a88c9a..1fdae7a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -18,7 +18,7 @@ public interface Cameras { new Pose3d(-Units.inchesToMeters(12.5), -Units.inchesToMeters(11.5), +Units.inchesToMeters(8.5), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-34), Units.degreesToRadians(180)))); + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))); public static final CameraConfig[] ROBOT_CAMERAS = new CameraConfig[]{DEFAULT_CAMERA}; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 63627ac..4f117d6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -26,26 +26,26 @@ public interface Field { Fiducial FIDUCIALS[] = { // Simplified Lab Testing Layout - // new Fiducial(1,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), - // new Fiducial(3,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42) - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + new Fiducial(1,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2, Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + new Fiducial(3,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2 - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), // 2024 Field Fiducial Layout - new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), - new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), + // new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + // new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + // new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), + // new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), }; public static Fiducial[] getFiducialLayout(int[] tids) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index d4312c2..def9fcd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -28,18 +28,18 @@ protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); this.estimator = - new SwerveDrivePoseEstimator( + new SwerveDrivePoseEstimator( swerve.getKinematics(), swerve.getGyroAngle(), swerve.getModulePositions(), new Pose2d()); this.odometry = - new SwerveDriveOdometry( - swerve.getKinematics(), - swerve.getGyroAngle(), - swerve.getModulePositions(), - new Pose2d()); + new SwerveDriveOdometry( + swerve.getKinematics(), + swerve.getGyroAngle(), + swerve.getModulePositions(), + new Pose2d()); this.field = new Field2d(); this.odometryPose2D = field.getObject("Odometry Pose"); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 16a7bed..0362e30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -1,10 +1,6 @@ package com.stuypulse.robot.subsystems.swerve; import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -15,7 +11,6 @@ import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.module.SimModule; import com.stuypulse.robot.subsystems.swerve.module.AbstractModule; import com.stuypulse.robot.subsystems.swerve.module.JimModule; @@ -30,7 +25,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -79,31 +73,6 @@ public SwerveDrive(AbstractModule... modules) { module2ds = new FieldObject2d[modules.length]; } - public void configureAutoBuilder() { - AbstractOdometry odometry = AbstractOdometry.getInstance(); - - AutoBuilder.configureHolonomic( - odometry::getPose, - odometry::reset, - this::getChassisSpeeds, - this::setChassisSpeeds, - new HolonomicPathFollowerConfig( - Swerve.Motion.XY, - Swerve.Motion.THETA, - Swerve.MAX_MODULE_SPEED.get(), - Swerve.WIDTH, - new ReplanningConfig(true, true)), - () -> { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - instance - ); - } - public void initModule2ds(Field2d field) { for (int i = 0; i < modules.length; i++) { module2ds[i] = field.getObject(modules[i].getID() + "-2d"); From f051cbf2612ac77fe4ca077b2aca5ab79b9a1675 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Fri, 19 Jan 2024 18:12:37 -0500 Subject: [PATCH 04/24] deadbanding alignment + filters --- .../swerve/SwerveDriveNoteAlignedDrive.java | 24 ++++++------- .../stuypulse/robot/constants/Settings.java | 11 +++++- .../subsystems/notevision/NoteVision.java | 12 ++++--- .../com/stuypulse/robot/util/Limelight.java | 34 ++++++++++++++++--- 4 files changed, 58 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 62b808c..fc6a04c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -3,7 +3,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; -import com.stuypulse.robot.constants.Settings .Driver.Turn.GyroFeedback; +import com.stuypulse.robot.constants.Settings.Driver.Turn.NoteAlignment; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; @@ -18,6 +18,8 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveNoteAlignedDrive extends Command { @@ -28,13 +30,9 @@ public class SwerveDriveNoteAlignedDrive extends Command { private VStream speed; private IStream turn; - private final Gamepad driver; - private AngleController alignController; public SwerveDriveNoteAlignedDrive(Gamepad driver) { - this.driver = driver; - swerve = SwerveDrive.getInstance(); noteVision = AbstractNoteVision.getInstance(); @@ -56,7 +54,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { new LowPassFilter(Turn.RC) ); - alignController = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D); + alignController = new AnglePIDController(NoteAlignment.P, NoteAlignment.I, NoteAlignment.D); addRequirements(swerve); } @@ -65,19 +63,19 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { public void execute() { double angularVel = turn.get(); - if (noteVision.hasNoteData()) { + if (noteVision.hasNoteData() && Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteAlignment.ANGLE_DEADBAND.get()) { angularVel = -alignController.update( Angle.kZero, Angle.fromRotation2d(noteVision.getRotationToNote()) ); } + + // robot relative + swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().x, speed.get().y, angularVel)); + // field relative + // swerve.drive(speed.get(), angularVel); - if(driver.getRawStartButton() || driver.getRawSelectButton()) { - swerve.setXMode(); - } - else { - swerve.drive(speed.get(), angularVel); - } + SmartDashboard.putNumber("Note Vision/Output", alignController.getOutput()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 4778271..ea2b8d7 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -145,6 +145,14 @@ public interface GyroFeedback { SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0); SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); } + + public interface NoteAlignment { + SmartNumber ANGLE_DEADBAND = new SmartNumber("Driver Settings/Note Alignment/Angle Deadband", 1); + + SmartNumber P = new SmartNumber("Driver Settings/Note Alignment/kP", 3.0); + SmartNumber I = new SmartNumber("Driver Settings/Note Alignment/kI", 0.0); + SmartNumber D = new SmartNumber("Driver Settings/Note Alignment/kD", 0.0); + } } } @@ -172,6 +180,7 @@ public static Vector2D vpow(Vector2D vec, double power) { public interface NoteDetection { SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); + SmartNumber X_ANGLE_RC = new SmartNumber("Note Detection/X Angle RC", 0.05); SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); @@ -189,4 +198,4 @@ public interface Rotation { SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0); } } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index 2dd2529..7fc9d75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -57,7 +57,7 @@ public boolean hasNoteData() { @Override public double getDistanceToNote() { for (Limelight limelight : limelights) { - if(limelight.hasNoteData()) { + if (limelight.hasNoteData()) { return limelight.getDistanceToNote(); } } @@ -78,10 +78,12 @@ public Rotation2d getRotationToNote() { @Override public void periodic(){ for (int i = 0; i < limelights.length; ++i) { - if(limelights[i].hasNoteData()) { - SmartDashboard.putNumber("Vision/X Angle", getRotationToNote().getDegrees()); - SmartDashboard.putNumber("Vision/Note Distance", getDistanceToNote()); - } + limelights[i].updateData(); + } + + if (hasNoteData()) { + SmartDashboard.putNumber("Note Detection/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Note Detection/Note Distance", getDistanceToNote()); } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java index d6df497..b7c9392 100644 --- a/src/main/java/com/stuypulse/robot/util/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -8,6 +8,13 @@ import static com.stuypulse.robot.constants.Settings.Limelight.LIMELIGHTS; import static com.stuypulse.robot.constants.Settings.Limelight.POSITIONS; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -18,6 +25,7 @@ public class Limelight { + private final String tableName; private final DoubleEntry txEntry; @@ -27,6 +35,12 @@ public class Limelight { private int limelightId; private final Pose3d robotRelativePose; + private double txData; + private double tyData; + + private IStream xAngle; + private BStream noteData; + public Limelight(String tableName, Pose3d robotRelativePose) { this.tableName = tableName; @@ -44,6 +58,11 @@ public Limelight(String tableName, Pose3d robotRelativePose) { txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); tvEntry = limelight.getIntegerTopic("tv").getEntry(0); + + xAngle = IStream.create(() -> txData) + .filtered(new LowPassFilter(NoteDetection.X_ANGLE_RC)); + noteData = BStream.create(() -> tvEntry.get() == 1) + .filtered(new BDebounceRC.Both(NoteDetection.DEBOUNCE_TIME)); } public String getTableName() { @@ -51,15 +70,22 @@ public String getTableName() { } public boolean hasNoteData() { - return tvEntry.get() == 1; + return noteData.get(); + } + + public void updateData() { + if (tvEntry.get() == 1) { + txData = txEntry.get(); + tyData = tyEntry.get(); + } } public double getXAngle() { - if(hasNoteData()) { + if(!hasNoteData()) { return Double.NaN; } - return txEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + return xAngle.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); } public double getYAngle() { @@ -67,7 +93,7 @@ public double getYAngle() { return Double.NaN; } - return tyEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + return tyData + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); } public double getDistanceToNote() { From b90d1f20636e282192472c462ed018e406ea14f1 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 13:06:27 -0500 Subject: [PATCH 05/24] Port note vision from retep nr/note-detection --- .../com/stuypulse/robot/RobotContainer.java | 2 + .../swerve/SwerveDriveDriveToNote.java | 70 ++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 41 +++++++++- .../notevision/AbstractNoteVision.java | 28 +++++++ .../subsystems/notevision/NoteVision.java | 77 ++++++++++++++++++ .../com/stuypulse/robot/util/Limelight.java | 81 +++++++++++++++++++ 6 files changed, 295 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/util/Limelight.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index bb964b4..1d456ab 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,6 +20,7 @@ import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.intake.*; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -40,6 +41,7 @@ public class RobotContainer { public final AbstractOdometry odometry = AbstractOdometry.getInstance(); public final AbstractVision vision = AbstractVision.getInstance(); public final AbstractIntake intake = AbstractIntake.getInstance(); + public final AbstractNoteVision noteVision = AbstractNoteVision.getInstance(); // Autons private static SendableChooser autonChooser; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java new file mode 100644 index 0000000..bafda64 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -0,0 +1,70 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + // Subsystems + private final SwerveDrive swerve; + private final AbstractNoteVision vision; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + + public SwerveDriveDriveToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.vision = AbstractNoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Vision/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); + } + + @Override + public void execute() { + double noteDistance = vision.getDistanceToNote(); + Rotation2d noteRotation = vision.getRotationToNote(); + + Pose2d targetPose = new Pose2d(TARGET_NOTE_DISTANCE.get(), 0, new Rotation2d()); + Pose2d currentPose = new Pose2d(noteDistance * noteRotation.getCos(), noteDistance * noteRotation.getSin(), noteRotation); + + swerve.setChassisSpeeds(controller.update(targetPose, currentPose)); + SmartDashboard.putBoolean("Vision/Is Aligned", aligned.get()); + } + + @Override + public boolean isFinished() { + return aligned.get(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index c29433a..89e8aed 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -10,6 +10,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -148,10 +149,6 @@ public interface GyroFeedback { } - public static Vector2D vpow(Vector2D vec, double power) { - return vec.mul(Math.pow(vec.magnitude(), power - 1)); - } - public interface Intake { SmartNumber ACQUIRE_SPEED_TOP = new SmartNumber("Intake/Acquire Speed Top", 1); SmartNumber ACQUIRE_SPEED_BOTTOM = new SmartNumber("Intake/Acquire Speed Bottom", 1); @@ -186,4 +183,40 @@ public interface Gyro { SmartNumber D = new SmartNumber("Alignment/Gyro/kD", 0.1); } } + + + public interface Limelight { + String [] LIMELIGHTS = { + "limelight" + }; + + int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; + Pose3d [] POSITIONS = new Pose3d[] { + new Pose3d() // TODO: determine these values when Limelight is put on + }; + } + + public static Vector2D vpow(Vector2D vec, double power) { + return vec.mul(Math.pow(vec.magnitude(), power - 1)); + } + + public interface NoteDetection { + SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); + SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); + + SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); + SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", 0.1); + SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 0.1); + + public interface Translation { + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 1); + SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0); + SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0); + } + public interface Rotation { + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 1); + SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0); + } + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java new file mode 100644 index 0000000..e66f681 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -0,0 +1,28 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class AbstractNoteVision extends SubsystemBase { + + /** SINGLETON **/ + private static final AbstractNoteVision instance; + + static { + instance = new NoteVision(); + } + + public static AbstractNoteVision getInstance() { + return instance; + } + + public abstract double getDistanceToNote(); + public abstract Rotation2d getRotationToNote(); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java new file mode 100644 index 0000000..c38140a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -0,0 +1,77 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + +import static com.stuypulse.robot.constants.Settings.Limelight.*; + +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.util.Limelight; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class NoteVision extends AbstractNoteVision { + + // store limelight network tables + private final Limelight[] limelights; + + // store fieldobject2d's to display limelight data + private final FieldObject2d[] limelightPoses; + + protected NoteVision() { + // reference to all limelights on robot + String[] hostNames = LIMELIGHTS; + + // setup limelight objects and field objects for april tag data + limelights = new Limelight[hostNames.length]; + limelightPoses = new FieldObject2d[limelights.length]; + + // setup all objects + Field2d field = Odometry.getInstance().getField(); + for(int i = 0; i < hostNames.length; i++){ + limelights[i] = new Limelight(hostNames[i], POSITIONS[i]); + limelightPoses[i] = field.getObject(hostNames[i] + " pose"); + + for (int port : PORTS) { + PortForwarder.add(port + i * 10, hostNames[i] + ".local", port); + } + } + } + + @Override + public double getDistanceToNote() { + for (Limelight limelight : limelights) { + if(limelight.hasNoteData()) { + return limelight.getDistanceToNote(); + } + } + + return Double.NaN; + } + + @Override + public Rotation2d getRotationToNote() { + for (Limelight limelight : limelights) { + if (limelight.hasNoteData()) { + return Rotation2d.fromDegrees(limelight.getXAngle()); + } + } + return Rotation2d.fromDegrees(Double.NaN); + } + + @Override + public void periodic(){ + for (int i = 0; i < limelights.length; ++i) { + if(limelights[i].hasNoteData()) { + SmartDashboard.putNumber("Vision/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Vision/Note Distance", getDistanceToNote()); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java new file mode 100644 index 0000000..d6df497 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -0,0 +1,81 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.util; + +import static com.stuypulse.robot.constants.Settings.Limelight.LIMELIGHTS; +import static com.stuypulse.robot.constants.Settings.Limelight.POSITIONS; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + + +public class Limelight { + private final String tableName; + + private final DoubleEntry txEntry; + private final DoubleEntry tyEntry; + private final IntegerEntry tvEntry; + + private int limelightId; + private final Pose3d robotRelativePose; + + + public Limelight(String tableName, Pose3d robotRelativePose) { + this.tableName = tableName; + this.robotRelativePose = robotRelativePose; + + for(int i = 0; i < LIMELIGHTS.length; i++) { + if(LIMELIGHTS[i].equals(tableName)) { + limelightId = i; + } + } + + NetworkTable limelight = NetworkTableInstance.getDefault().getTable(tableName); + + + txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); + tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); + tvEntry = limelight.getIntegerTopic("tv").getEntry(0); + } + + public String getTableName() { + return tableName; + } + + public boolean hasNoteData() { + return tvEntry.get() == 1; + } + + public double getXAngle() { + if(hasNoteData()) { + return Double.NaN; + } + + return txEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + } + + public double getYAngle() { + if(!hasNoteData()) { + return Double.NaN; + } + + return tyEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + } + + public double getDistanceToNote() { + if(!hasNoteData()) { + return Double.NaN; + } + + Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); + return POSITIONS[limelightId].getZ() / yRotation.getTan() + POSITIONS[limelightId].getX(); + } +} \ No newline at end of file From e2c5058b59d96b67f5b9823fe177bf042b66c462 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 14:24:20 -0500 Subject: [PATCH 06/24] Make note aligned drive command --- .../com/stuypulse/robot/RobotContainer.java | 4 + .../swerve/SwerveDriveNoteAlignedDrive.java | 83 +++++++++++++++++++ .../notevision/AbstractNoteVision.java | 2 + .../subsystems/notevision/NoteVision.java | 10 +++ 4 files changed, 99 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1d456ab..3b52d75 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; @@ -94,6 +95,9 @@ private void configureButtonBindings() { driver.getTopButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(7).getPose().toPose2d(), driver)); driver.getBottomButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(8).getPose().toPose2d(), driver)); + + driver.getRightBumper() + .whileTrue(new SwerveDriveNoteAlignedDrive(driver)); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java new file mode 100644 index 0000000..62b808c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -0,0 +1,83 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.constants.Settings .Driver.Turn.GyroFeedback; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveNoteAlignedDrive extends Command { + + private SwerveDrive swerve; + private AbstractNoteVision noteVision; + + private VStream speed; + private IStream turn; + + private final Gamepad driver; + + private AngleController alignController; + + public SwerveDriveNoteAlignedDrive(Gamepad driver) { + this.driver = driver; + + swerve = SwerveDrive.getInstance(); + noteVision = AbstractNoteVision.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1.0), + x -> Settings.vpow(x, Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND.get()), + x -> SLMath.spow(x, Turn.POWER.get()), + x -> x * Turn.MAX_TELEOP_TURNING.get(), + new LowPassFilter(Turn.RC) + ); + + alignController = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D); + + addRequirements(swerve); + } + + @Override + public void execute() { + double angularVel = turn.get(); + + if (noteVision.hasNoteData()) { + angularVel = -alignController.update( + Angle.kZero, + Angle.fromRotation2d(noteVision.getRotationToNote()) + ); + } + + if(driver.getRawStartButton() || driver.getRawSelectButton()) { + swerve.setXMode(); + } + else { + swerve.drive(speed.get(), angularVel); + } + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java index e66f681..a3c762c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -21,6 +21,8 @@ public abstract class AbstractNoteVision extends SubsystemBase { public static AbstractNoteVision getInstance() { return instance; } + + public abstract boolean hasNoteData(); public abstract double getDistanceToNote(); public abstract Rotation2d getRotationToNote(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index c38140a..2dd2529 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -44,6 +44,16 @@ protected NoteVision() { } } + @Override + public boolean hasNoteData() { + for (Limelight limelight : limelights) { + if (limelight.hasNoteData()) + return true; + } + + return false; + } + @Override public double getDistanceToNote() { for (Limelight limelight : limelights) { From 7bc79b6a5ebb858c1c2086c13801caf68b03830a Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Fri, 19 Jan 2024 16:24:35 -0500 Subject: [PATCH 07/24] Revert "Merge branch 'main' into note-detection" This reverts commit 974ad00c0041399d93b720d59dcf87f8df67386d, reversing changes made to ac7eb66c3e99e0deac593a1f7ddd1798c3ac7f4a. --- .pathplanner/settings.json | 7 +- src/main/deploy/Johnathan.chor | 1635 ----------------- src/main/deploy/choreo/Straight Line.traj | 103 -- src/main/deploy/choreo/Three Piece.traj | 1336 -------------- src/main/deploy/pathplanner/New Path.path | 74 - .../deploy/pathplanner/autos/3 Piece HFE.auto | 55 - .../deploy/pathplanner/autos/4 Piece ABC.auto | 37 - .../pathplanner/autos/6 Piece ABCDE.auto | 61 - ...To B (ABC).path => 1 Note + Mobility.path} | 20 +- .../pathplanner/paths/2 Note Center 6.path | 97 + .../pathplanner/paths/2 Note Center 7.path | 97 + .../pathplanner/paths/2 Note Center 8.path | 97 + ...Shoot (ABCD).path => 2 Note Mobility.path} | 34 +- .../pathplanner/paths/3 Note (4,1).path | 181 ++ .../pathplanner/paths/3 Note Center 7+6.path | 155 ++ .../pathplanner/paths/3 Note Center 8+7.path | 113 ++ .../pathplanner/paths/3 Note Mobility.path | 65 + .../pathplanner/paths/4 Note (5, 4, 1).path | 129 ++ .../pathplanner/paths/4 Note End 4.path | 113 ++ .../paths/4 Note Mobility End Speaker.path | 92 + .../pathplanner/paths/A To D (ABCD).path | 49 - ...BC).path => Bottom 1 Note + Mobility.path} | 25 +- .../paths/DShoot To E (ABCDE).path | 49 - .../{B To A (ABC).path => Do Nothing.path} | 20 +- .../pathplanner/paths/E To Shoot (ABCDE).path | 49 - .../pathplanner/paths/E To Shoot (HFE).path | 49 - .../pathplanner/paths/F To Shoot (HFE).path | 49 - .../pathplanner/paths/FShoot To E (HFE).path | 49 - .../pathplanner/paths/H To Shoot (HFE).path | 49 - .../pathplanner/paths/HShoot To F (HFE).path | 49 - .../{Start To H (HFE).path => Mobility.path} | 24 +- .../paths/deploy/pathplanner/navgrid.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 20 +- .../commands/swerve/SwerveDriveToPose.java | 81 - .../swerve/SwerveDriveWithAiming.java | 64 - .../stuypulse/robot/constants/Cameras.java | 2 +- .../com/stuypulse/robot/constants/Field.java | 36 +- .../robot/subsystems/odometry/Odometry.java | 12 +- .../robot/subsystems/swerve/SwerveDrive.java | 31 - 39 files changed, 1235 insertions(+), 3974 deletions(-) delete mode 100644 src/main/deploy/Johnathan.chor delete mode 100644 src/main/deploy/choreo/Straight Line.traj delete mode 100644 src/main/deploy/choreo/Three Piece.traj delete mode 100644 src/main/deploy/pathplanner/New Path.path delete mode 100644 src/main/deploy/pathplanner/autos/3 Piece HFE.auto delete mode 100644 src/main/deploy/pathplanner/autos/4 Piece ABC.auto delete mode 100644 src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto rename src/main/deploy/pathplanner/paths/{C To B (ABC).path => 1 Note + Mobility.path} (69%) create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 6.path create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 7.path create mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 8.path rename src/main/deploy/pathplanner/paths/{D To Shoot (ABCD).path => 2 Note Mobility.path} (53%) create mode 100644 src/main/deploy/pathplanner/paths/3 Note (4,1).path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 7+6.path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 8+7.path create mode 100644 src/main/deploy/pathplanner/paths/3 Note Mobility.path create mode 100644 src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path create mode 100644 src/main/deploy/pathplanner/paths/4 Note End 4.path create mode 100644 src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path delete mode 100644 src/main/deploy/pathplanner/paths/A To D (ABCD).path rename src/main/deploy/pathplanner/paths/{Start To C (ABC).path => Bottom 1 Note + Mobility.path} (66%) delete mode 100644 src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path rename src/main/deploy/pathplanner/paths/{B To A (ABC).path => Do Nothing.path} (73%) delete mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path delete mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/F To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/FShoot To E (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/H To Shoot (HFE).path delete mode 100644 src/main/deploy/pathplanner/paths/HShoot To F (HFE).path rename src/main/deploy/pathplanner/paths/{Start To H (HFE).path => Mobility.path} (64%) create mode 100644 src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f64d841..70c55ea 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,10 +1,9 @@ { - "robotWidth": 0.66, - "robotLength": 0.66, + "robotWidth": 0.9, + "robotLength": 0.9, "holonomicMode": true, "pathFolders": [ - "ABCDE", - "HFE" + "New Folder" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/Johnathan.chor b/src/main/deploy/Johnathan.chor deleted file mode 100644 index 897ee40..0000000 --- a/src/main/deploy/Johnathan.chor +++ /dev/null @@ -1,1635 +0,0 @@ -{ - "version": "v0.2", - "robotConfiguration": { - "mass": 65.77, - "rotationalInertia": 4.85963002223, - "motorMaxTorque": 0.7248618784530386, - "motorMaxVelocity": 4704, - "gearing": 4.71, - "wheelbase": 0.52, - "trackWidth": 0.67, - "bumperLength": 0.79, - "bumperWidth": 0.94, - "wheelRadius": 0.038 - }, - "paths": { - "Straight Line": { - "waypoints": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 9.80132051089383, - "y": 4.12, - "heading": 1.0196133040364384e-18, - "angularVelocity": 1.0324520604294053e-17, - "velocityX": 0.8234448167415525, - "velocityY": 3.02420635388715e-34, - "timestamp": 0.09875647917200309 - }, - { - "x": 9.963961529614206, - "y": 4.12, - "heading": 3.0591556010799972e-18, - "angularVelocity": 2.0652237848908187e-17, - "velocityX": 1.6468896024240143, - "velocityY": 2.277444821719999e-33, - "timestamp": 0.19751295834400617 - }, - { - "x": 10.207923050026555, - "y": 4.12, - "heading": 6.11931106017425e-18, - "angularVelocity": 3.0986882933133645e-17, - "velocityX": 2.4703343259883073, - "velocityY": 1.5382285718210452e-33, - "timestamp": 0.29626943751600926 - }, - { - "x": 10.533205053727176, - "y": 4.12, - "heading": 1.020162012561919e-17, - "angularVelocity": 4.133712642975716e-17, - "velocityX": 3.29377886319825, - "velocityY": 1.5324076974034493e-33, - "timestamp": 0.39502591668801235 - }, - { - "x": 10.906794946272825, - "y": 4.12, - "heading": -1.0162422335797283e-17, - "angularVelocity": -2.062046220374934e-16, - "velocityX": 3.7829405794729865, - "velocityY": -3.7320649171210315e-33, - "timestamp": 0.49378239586001543 - }, - { - "x": 11.232076949973447, - "y": 4.12, - "heading": -6.100334399460735e-18, - "angularVelocity": 4.11323689413084e-17, - "velocityX": 3.2937788631982494, - "velocityY": 1.28740322268366e-34, - "timestamp": 0.5925388750320185 - }, - { - "x": 11.476038470385795, - "y": 4.12, - "heading": -3.0508797817029e-18, - "angularVelocity": 3.0878527093782464e-17, - "velocityX": 2.470334325988307, - "velocityY": -6.604493200637239e-34, - "timestamp": 0.6912953542040217 - }, - { - "x": 11.63867948910617, - "y": 4.12, - "heading": -1.0171236371693236e-18, - "angularVelocity": 2.0593647744285298e-17, - "velocityX": 1.646889602424014, - "velocityY": -7.180489499042344e-34, - "timestamp": 0.7900518333760247 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 1.0299310442024172e-17, - "velocityX": 0.8234448167415525, - "velocityY": -6.686788666173353e-34, - "timestamp": 0.8888083125480277 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 9.925887673927275e-44, - "timestamp": 0.9875647917200308 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "abdd982e-66b2-455b-8a91-2ff53944faf6" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "269654ce-bbb8-4e2d-b597-db25cd41cc9a" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "Three Piece": { - "waypoints": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 6 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 5 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 5 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 23 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "angularVelocity": -2.8038956593328693e-37, - "velocityX": -1.823589012902056e-36, - "velocityY": -4.380800340261528e-36, - "timestamp": 0 - }, - { - "x": 1.907488249459704, - "y": 4.045792662710147, - "heading": -0.014531093178687978, - "angularVelocity": -0.3399428010717914, - "velocityX": 0.21634741051533174, - "velocityY": -0.0012585953421492776, - "timestamp": 0.042745700549838096 - }, - { - "x": 1.9260125772155603, - "y": 4.04570358761662, - "heading": -0.0432778322806298, - "angularVelocity": -0.672505976792342, - "velocityX": 0.4333611922971804, - "velocityY": -0.002083837494312978, - "timestamp": 0.08549140109967619 - }, - { - "x": 1.9538513835951645, - "y": 4.045605326780032, - "heading": -0.0858077691342503, - "angularVelocity": -0.9949523883468459, - "velocityX": 0.6512656482760499, - "velocityY": -0.002298730289234273, - "timestamp": 0.1282371016495143 - }, - { - "x": 1.9910562559823362, - "y": 4.045539296716268, - "heading": -0.1415186846109791, - "angularVelocity": -1.3033103858427657, - "velocityX": 0.8703769480580534, - "velocityY": -0.0015447182503467369, - "timestamp": 0.17098280219935238 - }, - { - "x": 2.0376949895847924, - "y": 4.045574246714428, - "heading": -0.20959549088699864, - "angularVelocity": -1.5926000837592402, - "velocityX": 1.0910742601604893, - "velocityY": 0.0008176260468236386, - "timestamp": 0.21372850274919047 - }, - { - "x": 2.0938509724389283, - "y": 4.0458223742350645, - "heading": -0.28899731848757143, - "angularVelocity": -1.8575395087512154, - "velocityX": 1.3137223658005655, - "velocityY": 0.005804736323069604, - "timestamp": 0.2564742032990286 - }, - { - "x": 2.159619041381372, - "y": 4.046458363697538, - "heading": -0.37845923210523325, - "angularVelocity": -2.0928868276086927, - "velocityX": 1.538589100107577, - "velocityY": 0.014878442844382068, - "timestamp": 0.2992199038488667 - }, - { - "x": 2.2351062437840015, - "y": 4.047751445000734, - "heading": -0.4762822950411069, - "angularVelocity": -2.288488939883434, - "velocityX": 1.7659601183660054, - "velocityY": 0.030250558221350754, - "timestamp": 0.3419656043987048 - }, - { - "x": 2.3204435066348985, - "y": 4.050239499458989, - "heading": -0.5785043698305009, - "angularVelocity": -2.391400151933671, - "velocityX": 1.99639406427321, - "velocityY": 0.058205958172433236, - "timestamp": 0.38471130494854294 - }, - { - "x": 2.4129687747508384, - "y": 4.057068741649624, - "heading": -0.6558501426840586, - "angularVelocity": -1.8094398233894569, - "velocityX": 2.1645514502227607, - "velocityY": 0.15976442315343106, - "timestamp": 0.42745700549838106 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "angularVelocity": -0.6191616505278632, - "velocityX": 2.0985980375619784, - "velocityY": 0.19360057017687107, - "timestamp": 0.4702027060482192 - }, - { - "x": 2.5850596627844538, - "y": 4.076067191512528, - "heading": -0.6785329802999686, - "angularVelocity": 0.08937347775881672, - "velocityX": 1.946004228329371, - "velocityY": 0.25328355970417904, - "timestamp": 0.5125380944025599 - }, - { - "x": 2.659003068905951, - "y": 4.0907479122594275, - "heading": -0.6634760532676318, - "angularVelocity": 0.3556581767081624, - "velocityX": 1.7466098457064405, - "velocityY": 0.3467718454363507, - "timestamp": 0.5548734827569006 - }, - { - "x": 2.7246369617363335, - "y": 4.110317723995373, - "heading": -0.642205314944677, - "angularVelocity": 0.5024339955245456, - "velocityX": 1.5503316582580267, - "velocityY": 0.4622565776921573, - "timestamp": 0.5972088711112413 - }, - { - "x": 2.7821274656068904, - "y": 4.1352087632427805, - "heading": -0.6169095715353604, - "angularVelocity": 0.5975082405668554, - "velocityX": 1.357977477125514, - "velocityY": 0.5879487637877254, - "timestamp": 0.639544259465582 - }, - { - "x": 2.831597012829208, - "y": 4.165661838985507, - "heading": -0.5887878540219638, - "angularVelocity": 0.6642602939654689, - "velocityX": 1.1685152574547029, - "velocityY": 0.7193290749535347, - "timestamp": 0.6818796478199227 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "angularVelocity": 0.7130885845826266, - "velocityX": 0.9811611571580527, - "velocityY": 0.8543111534241201, - "timestamp": 0.7242150361742634 - }, - { - "x": 2.9188396470921765, - "y": 4.2725572090728265, - "heading": -0.5076501226687711, - "angularVelocity": 0.770144031880298, - "velocityX": 0.6908747919669516, - "velocityY": 1.069122760430878, - "timestamp": 0.7903699982135706 - }, - { - "x": 2.9452297152900053, - "y": 4.357347002147122, - "heading": -0.4530144734209342, - "angularVelocity": 0.8258737903193734, - "velocityX": 0.398912906671289, - "velocityY": 1.2816845548776343, - "timestamp": 0.8565249602528777 - }, - { - "x": 2.9521484208350985, - "y": 4.455984216280158, - "heading": -0.39482288845823243, - "angularVelocity": 0.8796254002553318, - "velocityX": 0.10458331970597358, - "velocityY": 1.4910025052153826, - "timestamp": 0.9226799222921849 - }, - { - "x": 2.9393582563456846, - "y": 4.568133489859713, - "heading": -0.3332972563741976, - "angularVelocity": 0.9300229368656926, - "velocityX": -0.19333643456426472, - "velocityY": 1.695251121343236, - "timestamp": 0.988834884331492 - }, - { - "x": 2.906458551093886, - "y": 4.693199281947042, - "heading": -0.26886974643996187, - "angularVelocity": 0.9738877923617428, - "velocityX": -0.4973127372101132, - "velocityY": 1.8904975262931591, - "timestamp": 1.0549898463707992 - }, - { - "x": 2.8526414862163594, - "y": 4.829843806089774, - "heading": -0.2025647523319625, - "angularVelocity": 1.0022678883649443, - "velocityX": -0.81350004925632, - "velocityY": 2.065521918998951, - "timestamp": 1.1211448084101063 - }, - { - "x": 2.775736178341105, - "y": 4.97269513900139, - "heading": -0.13812989460617203, - "angularVelocity": 0.9739988617559082, - "velocityX": -1.1625024866548819, - "velocityY": 2.1593441898846235, - "timestamp": 1.1872997704494135 - }, - { - "x": 2.702646192253521, - "y": 5.09239426390518, - "heading": -0.0945099100170612, - "angularVelocity": 0.6593607379472648, - "velocityX": -1.1048299905932448, - "velocityY": 1.809374855852369, - "timestamp": 1.2534547324887206 - }, - { - "x": 2.6443183089698836, - "y": 5.193435173263213, - "heading": -0.060805157515131665, - "angularVelocity": 0.5094818508383886, - "velocityX": -0.8816856889583129, - "velocityY": 1.5273368201465674, - "timestamp": 1.3196096945280278 - }, - { - "x": 2.602443138845534, - "y": 5.277234789353118, - "heading": -0.03510328646046884, - "angularVelocity": 0.3885101020750583, - "velocityX": -0.63298608046164, - "velocityY": 1.2667170157261087, - "timestamp": 1.385764656567335 - }, - { - "x": 2.577670236134593, - "y": 5.344417005113783, - "heading": -0.016652156501204083, - "angularVelocity": 0.2789077249912364, - "velocityX": -0.3744677942105343, - "velocityY": 1.015527992000793, - "timestamp": 1.451919618606642 - }, - { - "x": 2.5703412148254166, - "y": 5.395330206172067, - "heading": -0.005041861251989399, - "angularVelocity": 0.17550150270385198, - "velocityX": -0.1107856626812363, - "velocityY": 0.7696051738043858, - "timestamp": 1.5180745806459492 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 4.946919732174461e-30, - "angularVelocity": 0.07621289615423993, - "velocityX": 0.15607789513463036, - "velocityY": 0.5270364913068623, - "timestamp": 1.5842295426852564 - }, - { - "x": 2.615476810555517, - "y": 5.449290678492884, - "heading": -0.00249481878254215, - "angularVelocity": -0.03313174320347507, - "velocityX": 0.4622880366833039, - "velocityY": 0.25357775003504285, - "timestamp": 1.659529497994595 - }, - { - "x": 2.6735006636921637, - "y": 5.447944678639869, - "heading": -0.01213678704864337, - "angularVelocity": -0.12804746332838068, - "velocityX": 0.7705695560944235, - "velocityY": -0.017875174659603758, - "timestamp": 1.7348294533039335 - }, - { - "x": 2.7555584259794457, - "y": 5.427049168858778, - "heading": -0.02296797038520493, - "angularVelocity": -0.14384050152574682, - "velocityX": 1.0897451658527806, - "velocityY": -0.27749697453671124, - "timestamp": 1.810129408613272 - }, - { - "x": 2.8158286951067315, - "y": 5.427810213575971, - "heading": -0.01758027986858031, - "angularVelocity": 0.07154971732043594, - "velocityX": 0.8004024554820866, - "velocityY": 0.010106841552119225, - "timestamp": 1.8854293639226105 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "angularVelocity": 0.23346999073716668, - "velocityX": 0.5021023557678279, - "velocityY": 0.2906235864398052, - "timestamp": 1.960729319231949 - }, - { - "x": 2.8693148468887744, - "y": 5.487156976773219, - "heading": 0.025561040529235773, - "angularVelocity": 0.3719476521458912, - "velocityX": 0.22813413061776552, - "velocityY": 0.5451346150355147, - "timestamp": 2.0294514643557737 - }, - { - "x": 2.8662677650419504, - "y": 5.542220250273633, - "heading": 0.0607369491230168, - "angularVelocity": 0.5118569644530235, - "velocityX": -0.04433915503268782, - "velocityY": 0.8012449757090656, - "timestamp": 2.0981736094795984 - }, - { - "x": 2.844654009258225, - "y": 5.615050469126244, - "heading": 0.10566640088464291, - "angularVelocity": 0.6537841867519056, - "velocityX": -0.3145093294858733, - "velocityY": 1.0597780194634927, - "timestamp": 2.166895754603423 - }, - { - "x": 2.804748272410547, - "y": 5.705930381335797, - "heading": 0.16050762468177382, - "angularVelocity": 0.7980138527154114, - "velocityX": -0.5806823517481201, - "velocityY": 1.3224254284525792, - "timestamp": 2.235617899727248 - }, - { - "x": 2.7471447891895058, - "y": 5.815446220375669, - "heading": 0.22535865974809183, - "angularVelocity": 0.9436701219012945, - "velocityX": -0.8382084569282627, - "velocityY": 1.5936033260100555, - "timestamp": 2.3043400448510725 - }, - { - "x": 2.6740678731800593, - "y": 5.945504225324297, - "heading": 0.2996496097363406, - "angularVelocity": 1.0810336297621417, - "velocityX": -1.0633677961852832, - "velocityY": 1.892519575957448, - "timestamp": 2.373062189974897 - }, - { - "x": 2.6265491520645683, - "y": 6.0733583679407905, - "heading": 0.3634596160425804, - "angularVelocity": 0.9285217478480327, - "velocityX": -0.6914615518748894, - "velocityY": 1.8604504033761498, - "timestamp": 2.441784335098722 - }, - { - "x": 2.600377734078398, - "y": 6.186973463102748, - "heading": 0.4156012346676629, - "angularVelocity": 0.7587309524627455, - "velocityX": -0.38082946827422787, - "velocityY": 1.6532530373905598, - "timestamp": 2.5105064802225465 - }, - { - "x": 2.59468135033937, - "y": 6.285113460277094, - "heading": 0.45604131218202076, - "angularVelocity": 0.5884577299135867, - "velocityX": -0.08289007464427144, - "velocityY": 1.4280694672367455, - "timestamp": 2.579228625346371 - }, - { - "x": 2.6091090827604106, - "y": 6.367321455091837, - "heading": 0.4847594324399464, - "angularVelocity": 0.41788742487855773, - "velocityX": 0.20994298701015016, - "velocityY": 1.1962373215594329, - "timestamp": 2.647950770470196 - }, - { - "x": 2.6434723912297375, - "y": 6.433359811846902, - "heading": 0.5017638856587859, - "angularVelocity": 0.24743775369934545, - "velocityX": 0.5000325354717966, - "velocityY": 0.9609472555909916, - "timestamp": 2.7166729155940206 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "angularVelocity": 0.07762170883267666, - "velocityX": 0.7884122231547788, - "velocityY": 0.7235292572375208, - "timestamp": 2.7853950607178453 - }, - { - "x": 2.738806835827011, - "y": 6.507590262026266, - "heading": 0.5059467865388958, - "angularVelocity": -0.027038737137375757, - "velocityX": 0.966387396526008, - "velocityY": 0.5755146244320987, - "timestamp": 2.8279794999687833 - }, - { - "x": 2.7880294623095803, - "y": 6.526430774501013, - "heading": 0.5005508163010642, - "angularVelocity": -0.12671225294372634, - "velocityX": 1.155882931615343, - "velocityY": 0.44242715898465257, - "timestamp": 2.8705639392197213 - }, - { - "x": 2.84591135896884, - "y": 6.540541693646972, - "heading": 0.4912680248506681, - "angularVelocity": -0.21798552742928792, - "velocityX": 1.35922646105959, - "velocityY": 0.3313632724575067, - "timestamp": 2.9131483784706593 - }, - { - "x": 2.913103056068397, - "y": 6.55135708254599, - "heading": 0.4786790444220719, - "angularVelocity": -0.2956239567794471, - "velocityX": 1.5778462340108705, - "velocityY": 0.25397513949368183, - "timestamp": 2.9557328177215973 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "angularVelocity": -0.35298591263083134, - "velocityX": 1.8086189494475877, - "velocityY": 0.22817483768114147, - "timestamp": 2.9983172569725354 - }, - { - "x": 3.1507680403022618, - "y": 6.583513336972122, - "heading": 0.4338303597620861, - "angularVelocity": -0.4082414173674588, - "velocityX": 2.199496396777096, - "velocityY": 0.3072329016846347, - "timestamp": 3.0713548671674924 - }, - { - "x": 3.339966410927534, - "y": 6.61173687205727, - "heading": 0.4005433233854809, - "angularVelocity": -0.4557519925385439, - "velocityX": 2.5904238942135573, - "velocityY": 0.38642467914559536, - "timestamp": 3.1443924773624494 - }, - { - "x": 3.5577217220068316, - "y": 6.645759083728854, - "heading": 0.36470254911055944, - "angularVelocity": -0.4907166893776054, - "velocityX": 2.981413418347747, - "velocityY": 0.46581770105525094, - "timestamp": 3.2174300875574064 - }, - { - "x": 3.8040369699341148, - "y": 6.685605934834726, - "heading": 0.32812163175231024, - "angularVelocity": -0.5008504147466596, - "velocityX": 3.372443967837953, - "velocityY": 0.5455661952726859, - "timestamp": 3.2904676977523635 - }, - { - "x": 4.078865987183254, - "y": 6.731338843022743, - "heading": 0.29648766626359246, - "angularVelocity": -0.43311884663638234, - "velocityX": 3.7628424111296486, - "velocityY": 0.6261555938911911, - "timestamp": 3.3635053079473205 - }, - { - "x": 4.363416873029506, - "y": 6.788685305113101, - "heading": 0.29648766061986404, - "angularVelocity": -7.727153775240832e-8, - "velocityX": 3.8959501151079365, - "velocityY": 0.7851634512312887, - "timestamp": 3.4365429181422775 - }, - { - "x": 4.647967686637335, - "y": 6.846032125646838, - "heading": 0.2964876549762135, - "angularVelocity": -7.72704712131681e-8, - "velocityX": 3.895949126050067, - "velocityY": 0.7851683588860835, - "timestamp": 3.5095805283372346 - }, - { - "x": 4.932518500243366, - "y": 6.903378946189493, - "heading": 0.2964876493325629, - "angularVelocity": -7.727047143608747e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081883, - "timestamp": 3.5826181385321916 - }, - { - "x": 5.217069313849397, - "y": 6.960725766732147, - "heading": 0.29648764368891234, - "angularVelocity": -7.727047107791159e-8, - "velocityX": 3.8959491260254584, - "velocityY": 0.7851683590081909, - "timestamp": 3.6556557487271486 - }, - { - "x": 5.501620127455428, - "y": 7.018072587274802, - "heading": 0.2964876380452618, - "angularVelocity": -7.72704716252991e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081912, - "timestamp": 3.7286933589221056 - }, - { - "x": 5.786170941061528, - "y": 7.075419407817124, - "heading": 0.29648763240161125, - "angularVelocity": -7.727047111704844e-8, - "velocityX": 3.8959491260263763, - "velocityY": 0.7851683590036359, - "timestamp": 3.8017309691170627 - }, - { - "x": 6.070721757362521, - "y": 7.132766214987568, - "heading": 0.29648762675796064, - "angularVelocity": -7.727047165435608e-8, - "velocityX": 3.895949162923739, - "velocityY": 0.7851681759215458, - "timestamp": 3.8747685793120197 - }, - { - "x": 6.355380338499842, - "y": 7.1895756818041425, - "heading": 0.29648762107984833, - "angularVelocity": -7.774230685415808e-8, - "velocityX": 3.8974246333839155, - "velocityY": 0.7778111395614767, - "timestamp": 3.9478061895069767 - }, - { - "x": 6.636405520312889, - "y": 7.217870773127444, - "heading": 0.27570831993349604, - "angularVelocity": -0.28450138347744536, - "velocityX": 3.847677669941766, - "velocityY": 0.387404396827479, - "timestamp": 4.020843799701934 - }, - { - "x": 6.88919116626837, - "y": 7.2399532386560255, - "heading": 0.24179746779298844, - "angularVelocity": -0.4642930135582247, - "velocityX": 3.4610339150025435, - "velocityY": 0.30234375782058953, - "timestamp": 4.093881409896891 - }, - { - "x": 7.113463572845222, - "y": 7.256040709772393, - "heading": 0.20495027392657358, - "angularVelocity": -0.5044961598286116, - "velocityX": 3.070642727468879, - "velocityY": 0.22026283545456557, - "timestamp": 4.166919020091848 - }, - { - "x": 7.3091937477745095, - "y": 7.2662136587336335, - "heading": 0.1681230341900902, - "angularVelocity": -0.5042229563396395, - "velocityX": 2.679854590078042, - "velocityY": 0.13928370512239335, - "timestamp": 4.239956630286805 - }, - { - "x": 7.476376305660358, - "y": 7.270512578107467, - "heading": 0.13274981372464448, - "angularVelocity": -0.4843151408024598, - "velocityX": 2.288992718129635, - "velocityY": 0.058858981863695155, - "timestamp": 4.312994240481762 - }, - { - "x": 7.615010848660238, - "y": 7.268961369646756, - "heading": 0.09968688668304959, - "angularVelocity": -0.45268358251784385, - "velocityX": 1.8981253990899605, - "velocityY": -0.021238488726158328, - "timestamp": 4.386031850676719 - }, - { - "x": 7.7250983383538445, - "y": 7.261575556193537, - "heading": 0.06950965194609489, - "angularVelocity": -0.4131739066544426, - "velocityX": 1.5072712455918797, - "velocityY": -0.10112342714260779, - "timestamp": 4.459069460871676 - }, - { - "x": 7.8066401035121915, - "y": 7.248365897189822, - "heading": 0.04263526512944047, - "angularVelocity": -0.36795271292310205, - "velocityX": 1.1164352850632593, - "velocityY": -0.18086105183966827, - "timestamp": 4.532107071066633 - }, - { - "x": 7.859637512937734, - "y": 7.2293402273974605, - "heading": 0.019381829491425508, - "angularVelocity": -0.3183761842144796, - "velocityX": 0.7256180655976813, - "velocityY": -0.2604914062984367, - "timestamp": 4.60514468126159 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "angularVelocity": -0.26536779393096516, - "velocityX": 0.33481847355697797, - "velocityY": -0.3400403905944489, - "timestamp": 4.678182291456547 - }, - { - "x": 7.8693875341309285, - "y": 7.163825702906692, - "heading": -0.017989310395224636, - "angularVelocity": -0.1948160433685539, - "velocityX": -0.15924109223639094, - "velocityY": -0.44053274732062453, - "timestamp": 4.770522275803691 - }, - { - "x": 7.809062158016808, - "y": 7.113867015205141, - "heading": -0.02942623999470572, - "angularVelocity": -0.12385674180412554, - "velocityX": -0.6532963649564154, - "velocityY": -0.5410298480638316, - "timestamp": 4.862862260150834 - }, - { - "x": 7.703115992354989, - "y": 7.054628240082747, - "heading": -0.03428596694706339, - "angularVelocity": -0.05262863088743923, - "velocityX": -1.1473487504992854, - "velocityY": -0.641528970805246, - "timestamp": 4.955202244497977 - }, - { - "x": 7.551549165293792, - "y": 6.986109443661236, - "heading": -0.032557590593216855, - "angularVelocity": 0.018717529205428393, - "velocityX": -1.6413997482541873, - "velocityY": -0.7420273774784575, - "timestamp": 5.047542228845121 - }, - { - "x": 7.354361663520876, - "y": 6.908310952017223, - "heading": -0.024244154578006688, - "angularVelocity": 0.09003072801005149, - "velocityX": -2.1354508901756786, - "velocityY": -0.8425222528903508, - "timestamp": 5.139882213192264 - }, - { - "x": 7.111553334877506, - "y": 6.8212333658210484, - "heading": -0.00936157178577619, - "angularVelocity": 0.16117159752031956, - "velocityX": -2.6295036799070197, - "velocityY": -0.9430106233158517, - "timestamp": 5.232222197539407 - }, - { - "x": 6.823123896075064, - "y": 6.724877589969487, - "heading": 0.012063558679812484, - "angularVelocity": 0.23202441084506603, - "velocityX": -3.1235595375251535, - "velocityY": -1.0434891941211963, - "timestamp": 5.3245621818865505 - }, - { - "x": 6.48907294008907, - "y": 6.619244934399674, - "heading": 0.039997176732724696, - "angularVelocity": 0.3025083689412195, - "velocityX": -3.6176198030331097, - "velocityY": -1.1439535789035715, - "timestamp": 5.416902166233694 - }, - { - "x": 6.129437398233647, - "y": 6.5461675567988795, - "heading": 0.03999718077634422, - "angularVelocity": 4.3790558964884806e-8, - "velocityX": -3.8946892226384597, - "velocityY": -0.7913947367163172, - "timestamp": 5.509242150580837 - }, - { - "x": 5.769801333833775, - "y": 6.4730927508413325, - "heading": 0.03999718481982793, - "angularVelocity": 4.378908809561466e-8, - "velocityX": -3.8946948815570095, - "velocityY": -0.7913668869904851, - "timestamp": 5.60158213492798 - }, - { - "x": 5.410165307610781, - "y": 6.400017756997467, - "heading": 0.03999718886333287, - "angularVelocity": 4.378931804611701e-8, - "velocityX": -3.8946944681187916, - "velocityY": -0.7913689217137886, - "timestamp": 5.693922119275124 - }, - { - "x": 5.06488865586912, - "y": 6.3170886108245945, - "heading": 0.07172411836277942, - "angularVelocity": 0.3435882053019635, - "velocityX": -3.739188978456266, - "velocityY": -0.8980849061129225, - "timestamp": 5.786262103622267 - }, - { - "x": 4.765275429088764, - "y": 6.243389106592463, - "heading": 0.10120262922920165, - "angularVelocity": 0.3192388549211855, - "velocityX": -3.244674870790441, - "velocityY": -0.7981320849596799, - "timestamp": 5.87860208796941 - }, - { - "x": 4.511309122993011, - "y": 6.178941794592798, - "heading": 0.12636460911438926, - "angularVelocity": 0.27249278915397723, - "velocityX": -2.7503394969289876, - "velocityY": -0.6979350544113176, - "timestamp": 5.970942072316554 - }, - { - "x": 4.302982802001526, - "y": 6.123753419378413, - "heading": 0.14653342260027327, - "angularVelocity": 0.21841906979387646, - "velocityX": -2.256079232245732, - "velocityY": -0.5976649834259027, - "timestamp": 6.063282056663697 - }, - { - "x": 4.140292768892181, - "y": 6.077827039965534, - "heading": 0.16137256146424936, - "angularVelocity": 0.16070111955174202, - "velocityX": -1.7618590067952369, - "velocityY": -0.497361784687127, - "timestamp": 6.15562204101084 - }, - { - "x": 4.023236723432337, - "y": 6.041164401680778, - "heading": 0.17067956492366876, - "angularVelocity": 0.10079061118779088, - "velocityX": -1.2676636918172275, - "velocityY": -0.3970396848555328, - "timestamp": 6.2479620253579835 - }, - { - "x": 3.9518130737172363, - "y": 6.01376672295597, - "heading": 0.174318405736917, - "angularVelocity": 0.03940698971280611, - "velocityX": -0.7734856164431515, - "velocityY": -0.29670438996183723, - "timestamp": 6.340302009705127 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "angularVelocity": -0.023042788250028232, - "velocityX": -0.2793205093781995, - "velocityY": -0.19635795295345568, - "timestamp": 6.43264199405227 - }, - { - "x": 3.930355880820906, - "y": 5.98747940429267, - "heading": 0.1678304420080604, - "angularVelocity": -0.0673769466267154, - "velocityX": 0.06699164087576895, - "velocityY": -0.12602683733396786, - "timestamp": 6.497355419650052 - }, - { - "x": 3.957112124799613, - "y": 5.9838306427917995, - "heading": 0.16069467758998768, - "angularVelocity": -0.11026714089319978, - "velocityX": 0.41345738896603046, - "velocityY": -0.056383377439255714, - "timestamp": 6.562068845247834 - }, - { - "x": 4.006301141076061, - "y": 5.984635223325004, - "heading": 0.15089514245932092, - "angularVelocity": -0.15142970782560608, - "velocityX": 0.760105276796434, - "velocityY": 0.012432977017907793, - "timestamp": 6.6267822708456166 - }, - { - "x": 4.0779371329258005, - "y": 5.989827517197893, - "heading": 0.13856825674408835, - "angularVelocity": -0.1904842094413026, - "velocityX": 1.106972644208068, - "velocityY": 0.08023518806075768, - "timestamp": 6.691495696443399 - }, - { - "x": 4.17203754289526, - "y": 5.999325152011095, - "heading": 0.12388458270411208, - "angularVelocity": -0.22690305611760084, - "velocityX": 1.4541095468246137, - "velocityY": 0.14676451950228486, - "timestamp": 6.756209122041181 - }, - { - "x": 4.288624284050538, - "y": 6.0130216798691905, - "heading": 0.10706400495167072, - "angularVelocity": -0.25992408216784824, - "velocityX": 1.801585066442124, - "velocityY": 0.21164893886513167, - "timestamp": 6.820922547638963 - }, - { - "x": 4.427725657084912, - "y": 6.0307742700897, - "heading": 0.0884013396467056, - "angularVelocity": -0.2883893895674757, - "velocityX": 2.1494979094282636, - "velocityY": 0.27432623225435043, - "timestamp": 6.885635973236745 - }, - { - "x": 4.589379469563093, - "y": 6.0523813458045, - "heading": 0.06831284216496007, - "angularVelocity": -0.3104224092014938, - "velocityX": 2.497994983033644, - "velocityY": 0.3338886099014062, - "timestamp": 6.950349398834527 - }, - { - "x": 4.773638315970202, - "y": 6.077537273797697, - "heading": 0.04742981182100691, - "angularVelocity": -0.32270012213151367, - "velocityX": 2.847304785754114, - "velocityY": 0.3887281157012392, - "timestamp": 7.015062824432309 - }, - { - "x": 4.980578361520177, - "y": 6.105724638750597, - "heading": 0.026818564360069696, - "angularVelocity": -0.31850033081301815, - "velocityX": 3.197791550028341, - "velocityY": 0.43557213503258413, - "timestamp": 7.079776250030092 - }, - { - "x": 5.210305960828753, - "y": 6.1358813908951575, - "heading": 0.008654103339561452, - "angularVelocity": -0.28069076629334544, - "velocityX": 3.549921784954145, - "velocityY": 0.4660045711687753, - "timestamp": 7.144489675627874 - }, - { - "x": 5.462718064244522, - "y": 6.164655483095359, - "heading": 8.629838781735336e-9, - "angularVelocity": -0.13372951021808044, - "velocityX": 3.900459619996067, - "velocityY": 0.44463868099074777, - "timestamp": 7.209203101225656 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "angularVelocity": -1.3335468957212507e-7, - "velocityX": 3.9730265373303566, - "velocityY": 0.09983815454439177, - "timestamp": 7.273916526823438 - }, - { - "x": 5.995970674250576, - "y": 6.151565167078146, - "heading": -2.5127315305597956e-15, - "angularVelocity": -3.607302360522835e-14, - "velocityX": 3.964357212211523, - "velocityY": -0.28067875524252794, - "timestamp": 7.343573330689117 - }, - { - "x": 6.26897851241587, - "y": 6.105687844731353, - "heading": -2.5097428721994853e-15, - "angularVelocity": 4.2905476485829695e-17, - "velocityX": 3.9193276609666734, - "velocityY": -0.6586193996970835, - "timestamp": 7.413230134554796 - }, - { - "x": 6.536346012712411, - "y": 6.0339060327816165, - "heading": -2.5126951036173778e-15, - "angularVelocity": -4.2382527679423133e-17, - "velocityX": 3.838354409887958, - "velocityY": -1.0305068272755022, - "timestamp": 7.482886938420475 - }, - { - "x": 6.787193967022678, - "y": 5.960199932024534, - "heading": -2.073411284199746e-15, - "angularVelocity": 6.306402166664137e-15, - "velocityX": 3.6011981657092096, - "velocityY": -1.058132108663187, - "timestamp": 7.552543742286154 - }, - { - "x": 7.012681141047439, - "y": 5.8942233269936395, - "heading": -1.671095384485508e-15, - "angularVelocity": 5.7756870204886894e-15, - "velocityX": 3.2371162831352196, - "velocityY": -0.9471666997261698, - "timestamp": 7.622200546151833 - }, - { - "x": 7.212807520844226, - "y": 5.835976237454582, - "heading": -1.3121610031518989e-15, - "angularVelocity": 5.1528976439448214e-15, - "velocityX": 2.8730342004018077, - "velocityY": -0.8362010070315249, - "timestamp": 7.6918573500175125 - }, - { - "x": 7.387573101757002, - "y": 5.785458669967885, - "heading": -9.95057743236105e-16, - "angularVelocity": 4.552365916666039e-15, - "velocityX": 2.508952050825841, - "velocityY": -0.7252352201533442, - "timestamp": 7.7615141538831915 - }, - { - "x": 7.536977881454969, - "y": 5.7426706278046975, - "heading": -7.196116224916786e-16, - "angularVelocity": 3.954331894676066e-15, - "velocityX": 2.144869867788704, - "velocityY": -0.6142693863142378, - "timestamp": 7.831170957748871 - }, - { - "x": 7.66102185853843, - "y": 5.707612112923722, - "heading": -4.889855482801724e-16, - "angularVelocity": 3.3108908490429318e-15, - "velocityX": 1.7807876646574228, - "velocityY": -0.5033035243557882, - "timestamp": 7.90082776161455 - }, - { - "x": 7.759705032073642, - "y": 5.680283126628732, - "heading": -3.021004962604421e-16, - "angularVelocity": 2.6829403813236554e-15, - "velocityX": 1.4167054481211605, - "velocityY": -0.3923376436802606, - "timestamp": 7.970484565480229 - }, - { - "x": 7.833027401393294, - "y": 5.660683669849869, - "heading": -1.6049621497454911e-16, - "angularVelocity": 2.0328851408532914e-15, - "velocityX": 1.0526232220049963, - "velocityY": -0.281371749651518, - "timestamp": 8.040141369345909 - }, - { - "x": 7.880988965996717, - "y": 5.648813743284097, - "heading": -5.830363906716236e-17, - "angularVelocity": 1.467086785481853e-15, - "velocityX": 0.6885409887010987, - "velocityY": -0.17040584561707373, - "timestamp": 8.109798173211589 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 2.0598914784739755e-32, - "angularVelocity": 8.370128572804695e-16, - "velocityX": 0.3244587498051791, - "velocityY": -0.05943993380554415, - "timestamp": 8.179454977077269 - }, - { - "x": 7.9005397389522525, - "y": 5.6483789255306105, - "heading": 2.097139947940835e-17, - "angularVelocity": 2.980227828531976e-16, - "velocityX": -0.04334309998236763, - "velocityY": 0.05265965538925957, - "timestamp": 8.249823421958254 - }, - { - "x": 7.871608103494799, - "y": 5.6599727605367445, - "heading": -9.084634582304173e-18, - "angularVelocity": -4.2712371472453026e-16, - "velocityX": -0.41114501686552, - "velocityY": 0.16475900563136703, - "timestamp": 8.32019186683924 - }, - { - "x": 7.816794813351453, - "y": 5.6794548319403075, - "heading": -7.520085122829124e-17, - "angularVelocity": -9.395719591601255e-16, - "velocityX": -0.7789470157537616, - "velocityY": 0.27685806382157147, - "timestamp": 8.390560311720225 - }, - { - "x": 7.736099861309118, - "y": 5.706825114052552, - "heading": -1.8616176975942832e-16, - "angularVelocity": -1.5768562134134974e-15, - "velocityX": -1.1467491171467201, - "velocityY": 0.388956756951142, - "timestamp": 8.460928756601211 - }, - { - "x": 7.629523238094034, - "y": 5.742083573845685, - "heading": -3.3813406750936066e-16, - "angularVelocity": -2.1596654705354767e-15, - "velocityX": -1.5145513503282884, - "velocityY": 0.5010549807255615, - "timestamp": 8.531297201482197 - }, - { - "x": 7.497064931341595, - "y": 5.785230167283751, - "heading": -5.316654292279505e-16, - "angularVelocity": -2.750257804825092e-15, - "velocityX": -1.8823537592222348, - "velocityY": 0.6131525787087516, - "timestamp": 8.601665646363182 - }, - { - "x": 7.338724923742105, - "y": 5.836264832718443, - "heading": -7.669822112767176e-16, - "angularVelocity": -3.3440668612043406e-15, - "velocityX": -2.2501564141027943, - "velocityY": 0.7252493006130729, - "timestamp": 8.672034091244168 - }, - { - "x": 7.154503189332537, - "y": 5.895187477681304, - "heading": -1.0519707319411395e-15, - "angularVelocity": -4.0499477067678845e-15, - "velocityX": -2.617959437941741, - "velocityY": 0.8373447084559508, - "timestamp": 8.742402536125153 - }, - { - "x": 6.944399684844722, - "y": 5.961997948067273, - "heading": -1.3745422496318406e-15, - "angularVelocity": -4.5840364940621565e-15, - "velocityX": -2.985763076660981, - "velocityY": 0.9494379263248582, - "timestamp": 8.812770981006139 - }, - { - "x": 6.708414323753371, - "y": 6.036695935692792, - "heading": -1.7429472421431418e-15, - "angularVelocity": -5.235372077575223e-15, - "velocityX": -3.3535679449837517, - "velocityY": 1.061526764623348, - "timestamp": 8.883139425887125 - }, - { - "x": 6.4465468465502065, - "y": 6.119280516151445, - "heading": -2.1550026112189195e-15, - "angularVelocity": -5.855683878769628e-15, - "velocityX": -3.7213765011567173, - "velocityY": 1.1736024662605187, - "timestamp": 8.95350787076811 - }, - { - "x": 6.173126442867114, - "y": 6.178044585557323, - "heading": -2.191046659518503e-15, - "angularVelocity": -5.122189123349113e-16, - "velocityX": -3.8855541591979326, - "velocityY": 0.8350912046286955, - "timestamp": 9.023876315649096 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "angularVelocity": 3.113677831051976e-14, - "velocityX": -3.9480676215152357, - "velocityY": 0.45570865426474233, - "timestamp": 9.094244760530081 - }, - { - "x": 5.637913658685597, - "y": 6.216758064974601, - "heading": 9.67763523798797e-9, - "angularVelocity": 1.4937766527408904e-7, - "velocityX": -3.972956719905001, - "velocityY": 0.10258285959556052, - "timestamp": 9.159031120794502 - }, - { - "x": 5.380949926914102, - "y": 6.200473867453901, - "heading": 1.892686165769219e-8, - "angularVelocity": 1.4276502617102237e-7, - "velocityX": -3.966324558486143, - "velocityY": -0.2513522514767974, - "timestamp": 9.223817481058923 - }, - { - "x": 5.132255360579735, - "y": 6.163185174806636, - "heading": 0.01632330814348079, - "angularVelocity": 0.25195564544398485, - "velocityX": -3.8386871143710315, - "velocityY": -0.5755639381137804, - "timestamp": 9.288603841323344 - }, - { - "x": 4.905757766533857, - "y": 6.129190200240601, - "heading": 0.04988895608633859, - "angularVelocity": 0.5180974483811885, - "velocityX": -3.4960691281513077, - "velocityY": -0.5247242541278667, - "timestamp": 9.353390201587764 - }, - { - "x": 4.701928110746201, - "y": 6.098578355375242, - "heading": 0.08643838460715948, - "angularVelocity": 0.5641531392054652, - "velocityX": -3.146181618409506, - "velocityY": -0.4725044707141436, - "timestamp": 9.418176561852185 - }, - { - "x": 4.520779220798307, - "y": 6.071364064096333, - "heading": 0.12231563080100817, - "angularVelocity": 0.5537777712384655, - "velocityX": -2.7960961104853954, - "velocityY": -0.4200620496107946, - "timestamp": 9.482962922116606 - }, - { - "x": 4.362300886297595, - "y": 6.047552197658039, - "heading": 0.15578751714707642, - "angularVelocity": 0.5166502055290209, - "velocityX": -2.4461682035211605, - "velocityY": -0.3675444390069978, - "timestamp": 9.547749282381027 - }, - { - "x": 4.226482344491556, - "y": 6.027144059810235, - "heading": 0.1858309320626014, - "angularVelocity": 0.4637305567548124, - "velocityX": -2.096406423384503, - "velocityY": -0.3150067045713868, - "timestamp": 9.612535642645447 - }, - { - "x": 4.113314607743797, - "y": 6.010139422555146, - "heading": 0.21176520360476983, - "angularVelocity": 0.40030449984110905, - "velocityX": -1.7467833705408997, - "velocityY": -0.26247248936131745, - "timestamp": 9.677322002909868 - }, - { - "x": 4.022790393937899, - "y": 5.996537426116962, - "heading": 0.23310242597379685, - "angularVelocity": 0.3293474472395599, - "velocityX": -1.3972727196965211, - "velocityY": -0.20995154509025238, - "timestamp": 9.742108363174289 - }, - { - "x": 3.954903777130295, - "y": 5.986337041905185, - "heading": 0.2494756207657714, - "angularVelocity": 0.2527259553577121, - "velocityX": -1.0478535378517617, - "velocityY": -0.1574464774715696, - "timestamp": 9.80689472343871 - }, - { - "x": 3.9096498799999146, - "y": 5.97953733708754, - "heading": 0.260600273972009, - "angularVelocity": 0.17171289081270033, - "velocityX": -0.6985096391537948, - "velocityY": -0.10495580844363857, - "timestamp": 9.87168108370313 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 0.08723493748434215, - "velocityX": -0.3492284312714647, - "velocityY": -0.052475536233735576, - "timestamp": 9.936467443967551 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 3.6116165249279265e-32, - "velocityX": -3.053502496463557e-33, - "velocityY": 4.523388089316994e-32, - "timestamp": 10.001253804231972 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint", - "uuid": "14c0e4e2-7c6c-412a-8ad7-0357eeb42788" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint", - "uuid": "8c0a6ae0-ea46-46c8-8131-8a97a5c22a3a" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - } -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Straight Line.traj b/src/main/deploy/choreo/Straight Line.traj deleted file mode 100644 index 018d4f9..0000000 --- a/src/main/deploy/choreo/Straight Line.traj +++ /dev/null @@ -1,103 +0,0 @@ -{ - "samples": [ - { - "x": 9.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 9.80132051089383, - "y": 4.12, - "heading": 1.0196133040364384e-18, - "angularVelocity": 1.0324520604294053e-17, - "velocityX": 0.8234448167415525, - "velocityY": 3.02420635388715e-34, - "timestamp": 0.09875647917200309 - }, - { - "x": 9.963961529614206, - "y": 4.12, - "heading": 3.0591556010799972e-18, - "angularVelocity": 2.0652237848908187e-17, - "velocityX": 1.6468896024240143, - "velocityY": 2.277444821719999e-33, - "timestamp": 0.19751295834400617 - }, - { - "x": 10.207923050026555, - "y": 4.12, - "heading": 6.11931106017425e-18, - "angularVelocity": 3.0986882933133645e-17, - "velocityX": 2.4703343259883073, - "velocityY": 1.5382285718210452e-33, - "timestamp": 0.29626943751600926 - }, - { - "x": 10.533205053727176, - "y": 4.12, - "heading": 1.020162012561919e-17, - "angularVelocity": 4.133712642975716e-17, - "velocityX": 3.29377886319825, - "velocityY": 1.5324076974034493e-33, - "timestamp": 0.39502591668801235 - }, - { - "x": 10.906794946272825, - "y": 4.12, - "heading": -1.0162422335797283e-17, - "angularVelocity": -2.062046220374934e-16, - "velocityX": 3.7829405794729865, - "velocityY": -3.7320649171210315e-33, - "timestamp": 0.49378239586001543 - }, - { - "x": 11.232076949973447, - "y": 4.12, - "heading": -6.100334399460735e-18, - "angularVelocity": 4.11323689413084e-17, - "velocityX": 3.2937788631982494, - "velocityY": 1.28740322268366e-34, - "timestamp": 0.5925388750320185 - }, - { - "x": 11.476038470385795, - "y": 4.12, - "heading": -3.0508797817029e-18, - "angularVelocity": 3.0878527093782464e-17, - "velocityX": 2.470334325988307, - "velocityY": -6.604493200637239e-34, - "timestamp": 0.6912953542040217 - }, - { - "x": 11.63867948910617, - "y": 4.12, - "heading": -1.0171236371693236e-18, - "angularVelocity": 2.0593647744285298e-17, - "velocityX": 1.646889602424014, - "velocityY": -7.180489499042344e-34, - "timestamp": 0.7900518333760247 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 1.0299310442024172e-17, - "velocityX": 0.8234448167415525, - "velocityY": -6.686788666173353e-34, - "timestamp": 0.8888083125480277 - }, - { - "x": 11.72, - "y": 4.12, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 9.925887673927275e-44, - "timestamp": 0.9875647917200308 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Three Piece.traj b/src/main/deploy/choreo/Three Piece.traj deleted file mode 100644 index 888ff45..0000000 --- a/src/main/deploy/choreo/Three Piece.traj +++ /dev/null @@ -1,1336 +0,0 @@ -{ - "samples": [ - { - "x": 1.898240327835083, - "y": 4.045846462249756, - "heading": 0, - "angularVelocity": -2.8038956593328693e-37, - "velocityX": -1.823589012902056e-36, - "velocityY": -4.380800340261528e-36, - "timestamp": 0 - }, - { - "x": 1.907488249459704, - "y": 4.045792662710147, - "heading": -0.014531093178687978, - "angularVelocity": -0.3399428010717914, - "velocityX": 0.21634741051533174, - "velocityY": -0.0012585953421492776, - "timestamp": 0.042745700549838096 - }, - { - "x": 1.9260125772155603, - "y": 4.04570358761662, - "heading": -0.0432778322806298, - "angularVelocity": -0.672505976792342, - "velocityX": 0.4333611922971804, - "velocityY": -0.002083837494312978, - "timestamp": 0.08549140109967619 - }, - { - "x": 1.9538513835951645, - "y": 4.045605326780032, - "heading": -0.0858077691342503, - "angularVelocity": -0.9949523883468459, - "velocityX": 0.6512656482760499, - "velocityY": -0.002298730289234273, - "timestamp": 0.1282371016495143 - }, - { - "x": 1.9910562559823362, - "y": 4.045539296716268, - "heading": -0.1415186846109791, - "angularVelocity": -1.3033103858427657, - "velocityX": 0.8703769480580534, - "velocityY": -0.0015447182503467369, - "timestamp": 0.17098280219935238 - }, - { - "x": 2.0376949895847924, - "y": 4.045574246714428, - "heading": -0.20959549088699864, - "angularVelocity": -1.5926000837592402, - "velocityX": 1.0910742601604893, - "velocityY": 0.0008176260468236386, - "timestamp": 0.21372850274919047 - }, - { - "x": 2.0938509724389283, - "y": 4.0458223742350645, - "heading": -0.28899731848757143, - "angularVelocity": -1.8575395087512154, - "velocityX": 1.3137223658005655, - "velocityY": 0.005804736323069604, - "timestamp": 0.2564742032990286 - }, - { - "x": 2.159619041381372, - "y": 4.046458363697538, - "heading": -0.37845923210523325, - "angularVelocity": -2.0928868276086927, - "velocityX": 1.538589100107577, - "velocityY": 0.014878442844382068, - "timestamp": 0.2992199038488667 - }, - { - "x": 2.2351062437840015, - "y": 4.047751445000734, - "heading": -0.4762822950411069, - "angularVelocity": -2.288488939883434, - "velocityX": 1.7659601183660054, - "velocityY": 0.030250558221350754, - "timestamp": 0.3419656043987048 - }, - { - "x": 2.3204435066348985, - "y": 4.050239499458989, - "heading": -0.5785043698305009, - "angularVelocity": -2.391400151933671, - "velocityX": 1.99639406427321, - "velocityY": 0.058205958172433236, - "timestamp": 0.38471130494854294 - }, - { - "x": 2.4129687747508384, - "y": 4.057068741649624, - "heading": -0.6558501426840586, - "angularVelocity": -1.8094398233894569, - "velocityX": 2.1645514502227607, - "velocityY": 0.15976442315343106, - "timestamp": 0.42745700549838106 - }, - { - "x": 2.5026748180389404, - "y": 4.065344333648682, - "heading": -0.6823166411894661, - "angularVelocity": -0.6191616505278632, - "velocityX": 2.0985980375619784, - "velocityY": 0.19360057017687107, - "timestamp": 0.4702027060482192 - }, - { - "x": 2.5850596627844538, - "y": 4.076067191512528, - "heading": -0.6785329802999686, - "angularVelocity": 0.08937347775881672, - "velocityX": 1.946004228329371, - "velocityY": 0.25328355970417904, - "timestamp": 0.5125380944025599 - }, - { - "x": 2.659003068905951, - "y": 4.0907479122594275, - "heading": -0.6634760532676318, - "angularVelocity": 0.3556581767081624, - "velocityX": 1.7466098457064405, - "velocityY": 0.3467718454363507, - "timestamp": 0.5548734827569006 - }, - { - "x": 2.7246369617363335, - "y": 4.110317723995373, - "heading": -0.642205314944677, - "angularVelocity": 0.5024339955245456, - "velocityX": 1.5503316582580267, - "velocityY": 0.4622565776921573, - "timestamp": 0.5972088711112413 - }, - { - "x": 2.7821274656068904, - "y": 4.1352087632427805, - "heading": -0.6169095715353604, - "angularVelocity": 0.5975082405668554, - "velocityX": 1.357977477125514, - "velocityY": 0.5879487637877254, - "timestamp": 0.639544259465582 - }, - { - "x": 2.831597012829208, - "y": 4.165661838985507, - "heading": -0.5887878540219638, - "angularVelocity": 0.6642602939654689, - "velocityX": 1.1685152574547029, - "velocityY": 0.7193290749535347, - "timestamp": 0.6818796478199227 - }, - { - "x": 2.8731348514556885, - "y": 4.201829433441162, - "heading": -0.5585989718626111, - "angularVelocity": 0.7130885845826266, - "velocityX": 0.9811611571580527, - "velocityY": 0.8543111534241201, - "timestamp": 0.7242150361742634 - }, - { - "x": 2.9188396470921765, - "y": 4.2725572090728265, - "heading": -0.5076501226687711, - "angularVelocity": 0.770144031880298, - "velocityX": 0.6908747919669516, - "velocityY": 1.069122760430878, - "timestamp": 0.7903699982135706 - }, - { - "x": 2.9452297152900053, - "y": 4.357347002147122, - "heading": -0.4530144734209342, - "angularVelocity": 0.8258737903193734, - "velocityX": 0.398912906671289, - "velocityY": 1.2816845548776343, - "timestamp": 0.8565249602528777 - }, - { - "x": 2.9521484208350985, - "y": 4.455984216280158, - "heading": -0.39482288845823243, - "angularVelocity": 0.8796254002553318, - "velocityX": 0.10458331970597358, - "velocityY": 1.4910025052153826, - "timestamp": 0.9226799222921849 - }, - { - "x": 2.9393582563456846, - "y": 4.568133489859713, - "heading": -0.3332972563741976, - "angularVelocity": 0.9300229368656926, - "velocityX": -0.19333643456426472, - "velocityY": 1.695251121343236, - "timestamp": 0.988834884331492 - }, - { - "x": 2.906458551093886, - "y": 4.693199281947042, - "heading": -0.26886974643996187, - "angularVelocity": 0.9738877923617428, - "velocityX": -0.4973127372101132, - "velocityY": 1.8904975262931591, - "timestamp": 1.0549898463707992 - }, - { - "x": 2.8526414862163594, - "y": 4.829843806089774, - "heading": -0.2025647523319625, - "angularVelocity": 1.0022678883649443, - "velocityX": -0.81350004925632, - "velocityY": 2.065521918998951, - "timestamp": 1.1211448084101063 - }, - { - "x": 2.775736178341105, - "y": 4.97269513900139, - "heading": -0.13812989460617203, - "angularVelocity": 0.9739988617559082, - "velocityX": -1.1625024866548819, - "velocityY": 2.1593441898846235, - "timestamp": 1.1872997704494135 - }, - { - "x": 2.702646192253521, - "y": 5.09239426390518, - "heading": -0.0945099100170612, - "angularVelocity": 0.6593607379472648, - "velocityX": -1.1048299905932448, - "velocityY": 1.809374855852369, - "timestamp": 1.2534547324887206 - }, - { - "x": 2.6443183089698836, - "y": 5.193435173263213, - "heading": -0.060805157515131665, - "angularVelocity": 0.5094818508383886, - "velocityX": -0.8816856889583129, - "velocityY": 1.5273368201465674, - "timestamp": 1.3196096945280278 - }, - { - "x": 2.602443138845534, - "y": 5.277234789353118, - "heading": -0.03510328646046884, - "angularVelocity": 0.3885101020750583, - "velocityX": -0.63298608046164, - "velocityY": 1.2667170157261087, - "timestamp": 1.385764656567335 - }, - { - "x": 2.577670236134593, - "y": 5.344417005113783, - "heading": -0.016652156501204083, - "angularVelocity": 0.2789077249912364, - "velocityX": -0.3744677942105343, - "velocityY": 1.015527992000793, - "timestamp": 1.451919618606642 - }, - { - "x": 2.5703412148254166, - "y": 5.395330206172067, - "heading": -0.005041861251989399, - "angularVelocity": 0.17550150270385198, - "velocityX": -0.1107856626812363, - "velocityY": 0.7696051738043858, - "timestamp": 1.5180745806459492 - }, - { - "x": 2.5806665420532227, - "y": 5.430196285247803, - "heading": 4.946919732174461e-30, - "angularVelocity": 0.07621289615423993, - "velocityX": 0.15607789513463036, - "velocityY": 0.5270364913068623, - "timestamp": 1.5842295426852564 - }, - { - "x": 2.615476810555517, - "y": 5.449290678492884, - "heading": -0.00249481878254215, - "angularVelocity": -0.03313174320347507, - "velocityX": 0.4622880366833039, - "velocityY": 0.25357775003504285, - "timestamp": 1.659529497994595 - }, - { - "x": 2.6735006636921637, - "y": 5.447944678639869, - "heading": -0.01213678704864337, - "angularVelocity": -0.12804746332838068, - "velocityX": 0.7705695560944235, - "velocityY": -0.017875174659603758, - "timestamp": 1.7348294533039335 - }, - { - "x": 2.7555584259794457, - "y": 5.427049168858778, - "heading": -0.02296797038520493, - "angularVelocity": -0.14384050152574682, - "velocityX": 1.0897451658527806, - "velocityY": -0.27749697453671124, - "timestamp": 1.810129408613272 - }, - { - "x": 2.8158286951067315, - "y": 5.427810213575971, - "heading": -0.01758027986858031, - "angularVelocity": 0.07154971732043594, - "velocityX": 0.8004024554820866, - "velocityY": 0.010106841552119225, - "timestamp": 1.8854293639226105 - }, - { - "x": 2.8536369800567627, - "y": 5.4496941566467285, - "heading": 0, - "angularVelocity": 0.23346999073716668, - "velocityX": 0.5021023557678279, - "velocityY": 0.2906235864398052, - "timestamp": 1.960729319231949 - }, - { - "x": 2.8693148468887744, - "y": 5.487156976773219, - "heading": 0.025561040529235773, - "angularVelocity": 0.3719476521458912, - "velocityX": 0.22813413061776552, - "velocityY": 0.5451346150355147, - "timestamp": 2.0294514643557737 - }, - { - "x": 2.8662677650419504, - "y": 5.542220250273633, - "heading": 0.0607369491230168, - "angularVelocity": 0.5118569644530235, - "velocityX": -0.04433915503268782, - "velocityY": 0.8012449757090656, - "timestamp": 2.0981736094795984 - }, - { - "x": 2.844654009258225, - "y": 5.615050469126244, - "heading": 0.10566640088464291, - "angularVelocity": 0.6537841867519056, - "velocityX": -0.3145093294858733, - "velocityY": 1.0597780194634927, - "timestamp": 2.166895754603423 - }, - { - "x": 2.804748272410547, - "y": 5.705930381335797, - "heading": 0.16050762468177382, - "angularVelocity": 0.7980138527154114, - "velocityX": -0.5806823517481201, - "velocityY": 1.3224254284525792, - "timestamp": 2.235617899727248 - }, - { - "x": 2.7471447891895058, - "y": 5.815446220375669, - "heading": 0.22535865974809183, - "angularVelocity": 0.9436701219012945, - "velocityX": -0.8382084569282627, - "velocityY": 1.5936033260100555, - "timestamp": 2.3043400448510725 - }, - { - "x": 2.6740678731800593, - "y": 5.945504225324297, - "heading": 0.2996496097363406, - "angularVelocity": 1.0810336297621417, - "velocityX": -1.0633677961852832, - "velocityY": 1.892519575957448, - "timestamp": 2.373062189974897 - }, - { - "x": 2.6265491520645683, - "y": 6.0733583679407905, - "heading": 0.3634596160425804, - "angularVelocity": 0.9285217478480327, - "velocityX": -0.6914615518748894, - "velocityY": 1.8604504033761498, - "timestamp": 2.441784335098722 - }, - { - "x": 2.600377734078398, - "y": 6.186973463102748, - "heading": 0.4156012346676629, - "angularVelocity": 0.7587309524627455, - "velocityX": -0.38082946827422787, - "velocityY": 1.6532530373905598, - "timestamp": 2.5105064802225465 - }, - { - "x": 2.59468135033937, - "y": 6.285113460277094, - "heading": 0.45604131218202076, - "angularVelocity": 0.5884577299135867, - "velocityX": -0.08289007464427144, - "velocityY": 1.4280694672367455, - "timestamp": 2.579228625346371 - }, - { - "x": 2.6091090827604106, - "y": 6.367321455091837, - "heading": 0.4847594324399464, - "angularVelocity": 0.41788742487855773, - "velocityX": 0.20994298701015016, - "velocityY": 1.1962373215594329, - "timestamp": 2.647950770470196 - }, - { - "x": 2.6434723912297375, - "y": 6.433359811846902, - "heading": 0.5017638856587859, - "angularVelocity": 0.24743775369934545, - "velocityX": 0.5000325354717966, - "velocityY": 0.9609472555909916, - "timestamp": 2.7166729155940206 - }, - { - "x": 2.6976537704467773, - "y": 6.483082294464111, - "heading": 0.5070982159979445, - "angularVelocity": 0.07762170883267666, - "velocityX": 0.7884122231547788, - "velocityY": 0.7235292572375208, - "timestamp": 2.7853950607178453 - }, - { - "x": 2.738806835827011, - "y": 6.507590262026266, - "heading": 0.5059467865388958, - "angularVelocity": -0.027038737137375757, - "velocityX": 0.966387396526008, - "velocityY": 0.5755146244320987, - "timestamp": 2.8279794999687833 - }, - { - "x": 2.7880294623095803, - "y": 6.526430774501013, - "heading": 0.5005508163010642, - "angularVelocity": -0.12671225294372634, - "velocityX": 1.155882931615343, - "velocityY": 0.44242715898465257, - "timestamp": 2.8705639392197213 - }, - { - "x": 2.84591135896884, - "y": 6.540541693646972, - "heading": 0.4912680248506681, - "angularVelocity": -0.21798552742928792, - "velocityX": 1.35922646105959, - "velocityY": 0.3313632724575067, - "timestamp": 2.9131483784706593 - }, - { - "x": 2.913103056068397, - "y": 6.55135708254599, - "heading": 0.4786790444220719, - "angularVelocity": -0.2956239567794471, - "velocityX": 1.5778462340108705, - "velocityY": 0.25397513949368183, - "timestamp": 2.9557328177215973 - }, - { - "x": 2.990122079849243, - "y": 6.5610737800598145, - "heading": 0.46364733726920737, - "angularVelocity": -0.35298591263083134, - "velocityX": 1.8086189494475877, - "velocityY": 0.22817483768114147, - "timestamp": 2.9983172569725354 - }, - { - "x": 3.1507680403022618, - "y": 6.583513336972122, - "heading": 0.4338303597620861, - "angularVelocity": -0.4082414173674588, - "velocityX": 2.199496396777096, - "velocityY": 0.3072329016846347, - "timestamp": 3.0713548671674924 - }, - { - "x": 3.339966410927534, - "y": 6.61173687205727, - "heading": 0.4005433233854809, - "angularVelocity": -0.4557519925385439, - "velocityX": 2.5904238942135573, - "velocityY": 0.38642467914559536, - "timestamp": 3.1443924773624494 - }, - { - "x": 3.5577217220068316, - "y": 6.645759083728854, - "heading": 0.36470254911055944, - "angularVelocity": -0.4907166893776054, - "velocityX": 2.981413418347747, - "velocityY": 0.46581770105525094, - "timestamp": 3.2174300875574064 - }, - { - "x": 3.8040369699341148, - "y": 6.685605934834726, - "heading": 0.32812163175231024, - "angularVelocity": -0.5008504147466596, - "velocityX": 3.372443967837953, - "velocityY": 0.5455661952726859, - "timestamp": 3.2904676977523635 - }, - { - "x": 4.078865987183254, - "y": 6.731338843022743, - "heading": 0.29648766626359246, - "angularVelocity": -0.43311884663638234, - "velocityX": 3.7628424111296486, - "velocityY": 0.6261555938911911, - "timestamp": 3.3635053079473205 - }, - { - "x": 4.363416873029506, - "y": 6.788685305113101, - "heading": 0.29648766061986404, - "angularVelocity": -7.727153775240832e-8, - "velocityX": 3.8959501151079365, - "velocityY": 0.7851634512312887, - "timestamp": 3.4365429181422775 - }, - { - "x": 4.647967686637335, - "y": 6.846032125646838, - "heading": 0.2964876549762135, - "angularVelocity": -7.72704712131681e-8, - "velocityX": 3.895949126050067, - "velocityY": 0.7851683588860835, - "timestamp": 3.5095805283372346 - }, - { - "x": 4.932518500243366, - "y": 6.903378946189493, - "heading": 0.2964876493325629, - "angularVelocity": -7.727047143608747e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081883, - "timestamp": 3.5826181385321916 - }, - { - "x": 5.217069313849397, - "y": 6.960725766732147, - "heading": 0.29648764368891234, - "angularVelocity": -7.727047107791159e-8, - "velocityX": 3.8959491260254584, - "velocityY": 0.7851683590081909, - "timestamp": 3.6556557487271486 - }, - { - "x": 5.501620127455428, - "y": 7.018072587274802, - "heading": 0.2964876380452618, - "angularVelocity": -7.72704716252991e-8, - "velocityX": 3.895949126025459, - "velocityY": 0.7851683590081912, - "timestamp": 3.7286933589221056 - }, - { - "x": 5.786170941061528, - "y": 7.075419407817124, - "heading": 0.29648763240161125, - "angularVelocity": -7.727047111704844e-8, - "velocityX": 3.8959491260263763, - "velocityY": 0.7851683590036359, - "timestamp": 3.8017309691170627 - }, - { - "x": 6.070721757362521, - "y": 7.132766214987568, - "heading": 0.29648762675796064, - "angularVelocity": -7.727047165435608e-8, - "velocityX": 3.895949162923739, - "velocityY": 0.7851681759215458, - "timestamp": 3.8747685793120197 - }, - { - "x": 6.355380338499842, - "y": 7.1895756818041425, - "heading": 0.29648762107984833, - "angularVelocity": -7.774230685415808e-8, - "velocityX": 3.8974246333839155, - "velocityY": 0.7778111395614767, - "timestamp": 3.9478061895069767 - }, - { - "x": 6.636405520312889, - "y": 7.217870773127444, - "heading": 0.27570831993349604, - "angularVelocity": -0.28450138347744536, - "velocityX": 3.847677669941766, - "velocityY": 0.387404396827479, - "timestamp": 4.020843799701934 - }, - { - "x": 6.88919116626837, - "y": 7.2399532386560255, - "heading": 0.24179746779298844, - "angularVelocity": -0.4642930135582247, - "velocityX": 3.4610339150025435, - "velocityY": 0.30234375782058953, - "timestamp": 4.093881409896891 - }, - { - "x": 7.113463572845222, - "y": 7.256040709772393, - "heading": 0.20495027392657358, - "angularVelocity": -0.5044961598286116, - "velocityX": 3.070642727468879, - "velocityY": 0.22026283545456557, - "timestamp": 4.166919020091848 - }, - { - "x": 7.3091937477745095, - "y": 7.2662136587336335, - "heading": 0.1681230341900902, - "angularVelocity": -0.5042229563396395, - "velocityX": 2.679854590078042, - "velocityY": 0.13928370512239335, - "timestamp": 4.239956630286805 - }, - { - "x": 7.476376305660358, - "y": 7.270512578107467, - "heading": 0.13274981372464448, - "angularVelocity": -0.4843151408024598, - "velocityX": 2.288992718129635, - "velocityY": 0.058858981863695155, - "timestamp": 4.312994240481762 - }, - { - "x": 7.615010848660238, - "y": 7.268961369646756, - "heading": 0.09968688668304959, - "angularVelocity": -0.45268358251784385, - "velocityX": 1.8981253990899605, - "velocityY": -0.021238488726158328, - "timestamp": 4.386031850676719 - }, - { - "x": 7.7250983383538445, - "y": 7.261575556193537, - "heading": 0.06950965194609489, - "angularVelocity": -0.4131739066544426, - "velocityX": 1.5072712455918797, - "velocityY": -0.10112342714260779, - "timestamp": 4.459069460871676 - }, - { - "x": 7.8066401035121915, - "y": 7.248365897189822, - "heading": 0.04263526512944047, - "angularVelocity": -0.36795271292310205, - "velocityX": 1.1164352850632593, - "velocityY": -0.18086105183966827, - "timestamp": 4.532107071066633 - }, - { - "x": 7.859637512937734, - "y": 7.2293402273974605, - "heading": 0.019381829491425508, - "angularVelocity": -0.3183761842144796, - "velocityX": 0.7256180655976813, - "velocityY": -0.2604914062984367, - "timestamp": 4.60514468126159 - }, - { - "x": 7.884091854095459, - "y": 7.204504489898682, - "heading": 0, - "angularVelocity": -0.26536779393096516, - "velocityX": 0.33481847355697797, - "velocityY": -0.3400403905944489, - "timestamp": 4.678182291456547 - }, - { - "x": 7.8693875341309285, - "y": 7.163825702906692, - "heading": -0.017989310395224636, - "angularVelocity": -0.1948160433685539, - "velocityX": -0.15924109223639094, - "velocityY": -0.44053274732062453, - "timestamp": 4.770522275803691 - }, - { - "x": 7.809062158016808, - "y": 7.113867015205141, - "heading": -0.02942623999470572, - "angularVelocity": -0.12385674180412554, - "velocityX": -0.6532963649564154, - "velocityY": -0.5410298480638316, - "timestamp": 4.862862260150834 - }, - { - "x": 7.703115992354989, - "y": 7.054628240082747, - "heading": -0.03428596694706339, - "angularVelocity": -0.05262863088743923, - "velocityX": -1.1473487504992854, - "velocityY": -0.641528970805246, - "timestamp": 4.955202244497977 - }, - { - "x": 7.551549165293792, - "y": 6.986109443661236, - "heading": -0.032557590593216855, - "angularVelocity": 0.018717529205428393, - "velocityX": -1.6413997482541873, - "velocityY": -0.7420273774784575, - "timestamp": 5.047542228845121 - }, - { - "x": 7.354361663520876, - "y": 6.908310952017223, - "heading": -0.024244154578006688, - "angularVelocity": 0.09003072801005149, - "velocityX": -2.1354508901756786, - "velocityY": -0.8425222528903508, - "timestamp": 5.139882213192264 - }, - { - "x": 7.111553334877506, - "y": 6.8212333658210484, - "heading": -0.00936157178577619, - "angularVelocity": 0.16117159752031956, - "velocityX": -2.6295036799070197, - "velocityY": -0.9430106233158517, - "timestamp": 5.232222197539407 - }, - { - "x": 6.823123896075064, - "y": 6.724877589969487, - "heading": 0.012063558679812484, - "angularVelocity": 0.23202441084506603, - "velocityX": -3.1235595375251535, - "velocityY": -1.0434891941211963, - "timestamp": 5.3245621818865505 - }, - { - "x": 6.48907294008907, - "y": 6.619244934399674, - "heading": 0.039997176732724696, - "angularVelocity": 0.3025083689412195, - "velocityX": -3.6176198030331097, - "velocityY": -1.1439535789035715, - "timestamp": 5.416902166233694 - }, - { - "x": 6.129437398233647, - "y": 6.5461675567988795, - "heading": 0.03999718077634422, - "angularVelocity": 4.3790558964884806e-8, - "velocityX": -3.8946892226384597, - "velocityY": -0.7913947367163172, - "timestamp": 5.509242150580837 - }, - { - "x": 5.769801333833775, - "y": 6.4730927508413325, - "heading": 0.03999718481982793, - "angularVelocity": 4.378908809561466e-8, - "velocityX": -3.8946948815570095, - "velocityY": -0.7913668869904851, - "timestamp": 5.60158213492798 - }, - { - "x": 5.410165307610781, - "y": 6.400017756997467, - "heading": 0.03999718886333287, - "angularVelocity": 4.378931804611701e-8, - "velocityX": -3.8946944681187916, - "velocityY": -0.7913689217137886, - "timestamp": 5.693922119275124 - }, - { - "x": 5.06488865586912, - "y": 6.3170886108245945, - "heading": 0.07172411836277942, - "angularVelocity": 0.3435882053019635, - "velocityX": -3.739188978456266, - "velocityY": -0.8980849061129225, - "timestamp": 5.786262103622267 - }, - { - "x": 4.765275429088764, - "y": 6.243389106592463, - "heading": 0.10120262922920165, - "angularVelocity": 0.3192388549211855, - "velocityX": -3.244674870790441, - "velocityY": -0.7981320849596799, - "timestamp": 5.87860208796941 - }, - { - "x": 4.511309122993011, - "y": 6.178941794592798, - "heading": 0.12636460911438926, - "angularVelocity": 0.27249278915397723, - "velocityX": -2.7503394969289876, - "velocityY": -0.6979350544113176, - "timestamp": 5.970942072316554 - }, - { - "x": 4.302982802001526, - "y": 6.123753419378413, - "heading": 0.14653342260027327, - "angularVelocity": 0.21841906979387646, - "velocityX": -2.256079232245732, - "velocityY": -0.5976649834259027, - "timestamp": 6.063282056663697 - }, - { - "x": 4.140292768892181, - "y": 6.077827039965534, - "heading": 0.16137256146424936, - "angularVelocity": 0.16070111955174202, - "velocityX": -1.7618590067952369, - "velocityY": -0.497361784687127, - "timestamp": 6.15562204101084 - }, - { - "x": 4.023236723432337, - "y": 6.041164401680778, - "heading": 0.17067956492366876, - "angularVelocity": 0.10079061118779088, - "velocityX": -1.2676636918172275, - "velocityY": -0.3970396848555328, - "timestamp": 6.2479620253579835 - }, - { - "x": 3.9518130737172363, - "y": 6.01376672295597, - "heading": 0.174318405736917, - "angularVelocity": 0.03940698971280611, - "velocityX": -0.7734856164431515, - "velocityY": -0.29670438996183723, - "timestamp": 6.340302009705127 - }, - { - "x": 3.926020622253418, - "y": 5.995635032653809, - "heading": 0.17219063503059454, - "angularVelocity": -0.023042788250028232, - "velocityX": -0.2793205093781995, - "velocityY": -0.19635795295345568, - "timestamp": 6.43264199405227 - }, - { - "x": 3.930355880820906, - "y": 5.98747940429267, - "heading": 0.1678304420080604, - "angularVelocity": -0.0673769466267154, - "velocityX": 0.06699164087576895, - "velocityY": -0.12602683733396786, - "timestamp": 6.497355419650052 - }, - { - "x": 3.957112124799613, - "y": 5.9838306427917995, - "heading": 0.16069467758998768, - "angularVelocity": -0.11026714089319978, - "velocityX": 0.41345738896603046, - "velocityY": -0.056383377439255714, - "timestamp": 6.562068845247834 - }, - { - "x": 4.006301141076061, - "y": 5.984635223325004, - "heading": 0.15089514245932092, - "angularVelocity": -0.15142970782560608, - "velocityX": 0.760105276796434, - "velocityY": 0.012432977017907793, - "timestamp": 6.6267822708456166 - }, - { - "x": 4.0779371329258005, - "y": 5.989827517197893, - "heading": 0.13856825674408835, - "angularVelocity": -0.1904842094413026, - "velocityX": 1.106972644208068, - "velocityY": 0.08023518806075768, - "timestamp": 6.691495696443399 - }, - { - "x": 4.17203754289526, - "y": 5.999325152011095, - "heading": 0.12388458270411208, - "angularVelocity": -0.22690305611760084, - "velocityX": 1.4541095468246137, - "velocityY": 0.14676451950228486, - "timestamp": 6.756209122041181 - }, - { - "x": 4.288624284050538, - "y": 6.0130216798691905, - "heading": 0.10706400495167072, - "angularVelocity": -0.25992408216784824, - "velocityX": 1.801585066442124, - "velocityY": 0.21164893886513167, - "timestamp": 6.820922547638963 - }, - { - "x": 4.427725657084912, - "y": 6.0307742700897, - "heading": 0.0884013396467056, - "angularVelocity": -0.2883893895674757, - "velocityX": 2.1494979094282636, - "velocityY": 0.27432623225435043, - "timestamp": 6.885635973236745 - }, - { - "x": 4.589379469563093, - "y": 6.0523813458045, - "heading": 0.06831284216496007, - "angularVelocity": -0.3104224092014938, - "velocityX": 2.497994983033644, - "velocityY": 0.3338886099014062, - "timestamp": 6.950349398834527 - }, - { - "x": 4.773638315970202, - "y": 6.077537273797697, - "heading": 0.04742981182100691, - "angularVelocity": -0.32270012213151367, - "velocityX": 2.847304785754114, - "velocityY": 0.3887281157012392, - "timestamp": 7.015062824432309 - }, - { - "x": 4.980578361520177, - "y": 6.105724638750597, - "heading": 0.026818564360069696, - "angularVelocity": -0.31850033081301815, - "velocityX": 3.197791550028341, - "velocityY": 0.43557213503258413, - "timestamp": 7.079776250030092 - }, - { - "x": 5.210305960828753, - "y": 6.1358813908951575, - "heading": 0.008654103339561452, - "angularVelocity": -0.28069076629334544, - "velocityX": 3.549921784954145, - "velocityY": 0.4660045711687753, - "timestamp": 7.144489675627874 - }, - { - "x": 5.462718064244522, - "y": 6.164655483095359, - "heading": 8.629838781735336e-9, - "angularVelocity": -0.13372951021808044, - "velocityX": 3.900459619996067, - "velocityY": 0.44463868099074777, - "timestamp": 7.209203101225656 - }, - { - "x": 5.7198262214660645, - "y": 6.171116352081299, - "heading": 0, - "angularVelocity": -1.3335468957212507e-7, - "velocityX": 3.9730265373303566, - "velocityY": 0.09983815454439177, - "timestamp": 7.273916526823438 - }, - { - "x": 5.995970674250576, - "y": 6.151565167078146, - "heading": -2.5127315305597956e-15, - "angularVelocity": -3.607302360522835e-14, - "velocityX": 3.964357212211523, - "velocityY": -0.28067875524252794, - "timestamp": 7.343573330689117 - }, - { - "x": 6.26897851241587, - "y": 6.105687844731353, - "heading": -2.5097428721994853e-15, - "angularVelocity": 4.2905476485829695e-17, - "velocityX": 3.9193276609666734, - "velocityY": -0.6586193996970835, - "timestamp": 7.413230134554796 - }, - { - "x": 6.536346012712411, - "y": 6.0339060327816165, - "heading": -2.5126951036173778e-15, - "angularVelocity": -4.2382527679423133e-17, - "velocityX": 3.838354409887958, - "velocityY": -1.0305068272755022, - "timestamp": 7.482886938420475 - }, - { - "x": 6.787193967022678, - "y": 5.960199932024534, - "heading": -2.073411284199746e-15, - "angularVelocity": 6.306402166664137e-15, - "velocityX": 3.6011981657092096, - "velocityY": -1.058132108663187, - "timestamp": 7.552543742286154 - }, - { - "x": 7.012681141047439, - "y": 5.8942233269936395, - "heading": -1.671095384485508e-15, - "angularVelocity": 5.7756870204886894e-15, - "velocityX": 3.2371162831352196, - "velocityY": -0.9471666997261698, - "timestamp": 7.622200546151833 - }, - { - "x": 7.212807520844226, - "y": 5.835976237454582, - "heading": -1.3121610031518989e-15, - "angularVelocity": 5.1528976439448214e-15, - "velocityX": 2.8730342004018077, - "velocityY": -0.8362010070315249, - "timestamp": 7.6918573500175125 - }, - { - "x": 7.387573101757002, - "y": 5.785458669967885, - "heading": -9.95057743236105e-16, - "angularVelocity": 4.552365916666039e-15, - "velocityX": 2.508952050825841, - "velocityY": -0.7252352201533442, - "timestamp": 7.7615141538831915 - }, - { - "x": 7.536977881454969, - "y": 5.7426706278046975, - "heading": -7.196116224916786e-16, - "angularVelocity": 3.954331894676066e-15, - "velocityX": 2.144869867788704, - "velocityY": -0.6142693863142378, - "timestamp": 7.831170957748871 - }, - { - "x": 7.66102185853843, - "y": 5.707612112923722, - "heading": -4.889855482801724e-16, - "angularVelocity": 3.3108908490429318e-15, - "velocityX": 1.7807876646574228, - "velocityY": -0.5033035243557882, - "timestamp": 7.90082776161455 - }, - { - "x": 7.759705032073642, - "y": 5.680283126628732, - "heading": -3.021004962604421e-16, - "angularVelocity": 2.6829403813236554e-15, - "velocityX": 1.4167054481211605, - "velocityY": -0.3923376436802606, - "timestamp": 7.970484565480229 - }, - { - "x": 7.833027401393294, - "y": 5.660683669849869, - "heading": -1.6049621497454911e-16, - "angularVelocity": 2.0328851408532914e-15, - "velocityX": 1.0526232220049963, - "velocityY": -0.281371749651518, - "timestamp": 8.040141369345909 - }, - { - "x": 7.880988965996717, - "y": 5.648813743284097, - "heading": -5.830363906716236e-17, - "angularVelocity": 1.467086785481853e-15, - "velocityX": 0.6885409887010987, - "velocityY": -0.17040584561707373, - "timestamp": 8.109798173211589 - }, - { - "x": 7.903589725494385, - "y": 5.6446733474731445, - "heading": 2.0598914784739755e-32, - "angularVelocity": 8.370128572804695e-16, - "velocityX": 0.3244587498051791, - "velocityY": -0.05943993380554415, - "timestamp": 8.179454977077269 - }, - { - "x": 7.9005397389522525, - "y": 5.6483789255306105, - "heading": 2.097139947940835e-17, - "angularVelocity": 2.980227828531976e-16, - "velocityX": -0.04334309998236763, - "velocityY": 0.05265965538925957, - "timestamp": 8.249823421958254 - }, - { - "x": 7.871608103494799, - "y": 5.6599727605367445, - "heading": -9.084634582304173e-18, - "angularVelocity": -4.2712371472453026e-16, - "velocityX": -0.41114501686552, - "velocityY": 0.16475900563136703, - "timestamp": 8.32019186683924 - }, - { - "x": 7.816794813351453, - "y": 5.6794548319403075, - "heading": -7.520085122829124e-17, - "angularVelocity": -9.395719591601255e-16, - "velocityX": -0.7789470157537616, - "velocityY": 0.27685806382157147, - "timestamp": 8.390560311720225 - }, - { - "x": 7.736099861309118, - "y": 5.706825114052552, - "heading": -1.8616176975942832e-16, - "angularVelocity": -1.5768562134134974e-15, - "velocityX": -1.1467491171467201, - "velocityY": 0.388956756951142, - "timestamp": 8.460928756601211 - }, - { - "x": 7.629523238094034, - "y": 5.742083573845685, - "heading": -3.3813406750936066e-16, - "angularVelocity": -2.1596654705354767e-15, - "velocityX": -1.5145513503282884, - "velocityY": 0.5010549807255615, - "timestamp": 8.531297201482197 - }, - { - "x": 7.497064931341595, - "y": 5.785230167283751, - "heading": -5.316654292279505e-16, - "angularVelocity": -2.750257804825092e-15, - "velocityX": -1.8823537592222348, - "velocityY": 0.6131525787087516, - "timestamp": 8.601665646363182 - }, - { - "x": 7.338724923742105, - "y": 5.836264832718443, - "heading": -7.669822112767176e-16, - "angularVelocity": -3.3440668612043406e-15, - "velocityX": -2.2501564141027943, - "velocityY": 0.7252493006130729, - "timestamp": 8.672034091244168 - }, - { - "x": 7.154503189332537, - "y": 5.895187477681304, - "heading": -1.0519707319411395e-15, - "angularVelocity": -4.0499477067678845e-15, - "velocityX": -2.617959437941741, - "velocityY": 0.8373447084559508, - "timestamp": 8.742402536125153 - }, - { - "x": 6.944399684844722, - "y": 5.961997948067273, - "heading": -1.3745422496318406e-15, - "angularVelocity": -4.5840364940621565e-15, - "velocityX": -2.985763076660981, - "velocityY": 0.9494379263248582, - "timestamp": 8.812770981006139 - }, - { - "x": 6.708414323753371, - "y": 6.036695935692792, - "heading": -1.7429472421431418e-15, - "angularVelocity": -5.235372077575223e-15, - "velocityX": -3.3535679449837517, - "velocityY": 1.061526764623348, - "timestamp": 8.883139425887125 - }, - { - "x": 6.4465468465502065, - "y": 6.119280516151445, - "heading": -2.1550026112189195e-15, - "angularVelocity": -5.855683878769628e-15, - "velocityX": -3.7213765011567173, - "velocityY": 1.1736024662605187, - "timestamp": 8.95350787076811 - }, - { - "x": 6.173126442867114, - "y": 6.178044585557323, - "heading": -2.191046659518503e-15, - "angularVelocity": -5.122189123349113e-16, - "velocityX": -3.8855541591979326, - "velocityY": 0.8350912046286955, - "timestamp": 9.023876315649096 - }, - { - "x": 5.8953070640563965, - "y": 6.21011209487915, - "heading": 0, - "angularVelocity": 3.113677831051976e-14, - "velocityX": -3.9480676215152357, - "velocityY": 0.45570865426474233, - "timestamp": 9.094244760530081 - }, - { - "x": 5.637913658685597, - "y": 6.216758064974601, - "heading": 9.67763523798797e-9, - "angularVelocity": 1.4937766527408904e-7, - "velocityX": -3.972956719905001, - "velocityY": 0.10258285959556052, - "timestamp": 9.159031120794502 - }, - { - "x": 5.380949926914102, - "y": 6.200473867453901, - "heading": 1.892686165769219e-8, - "angularVelocity": 1.4276502617102237e-7, - "velocityX": -3.966324558486143, - "velocityY": -0.2513522514767974, - "timestamp": 9.223817481058923 - }, - { - "x": 5.132255360579735, - "y": 6.163185174806636, - "heading": 0.01632330814348079, - "angularVelocity": 0.25195564544398485, - "velocityX": -3.8386871143710315, - "velocityY": -0.5755639381137804, - "timestamp": 9.288603841323344 - }, - { - "x": 4.905757766533857, - "y": 6.129190200240601, - "heading": 0.04988895608633859, - "angularVelocity": 0.5180974483811885, - "velocityX": -3.4960691281513077, - "velocityY": -0.5247242541278667, - "timestamp": 9.353390201587764 - }, - { - "x": 4.701928110746201, - "y": 6.098578355375242, - "heading": 0.08643838460715948, - "angularVelocity": 0.5641531392054652, - "velocityX": -3.146181618409506, - "velocityY": -0.4725044707141436, - "timestamp": 9.418176561852185 - }, - { - "x": 4.520779220798307, - "y": 6.071364064096333, - "heading": 0.12231563080100817, - "angularVelocity": 0.5537777712384655, - "velocityX": -2.7960961104853954, - "velocityY": -0.4200620496107946, - "timestamp": 9.482962922116606 - }, - { - "x": 4.362300886297595, - "y": 6.047552197658039, - "heading": 0.15578751714707642, - "angularVelocity": 0.5166502055290209, - "velocityX": -2.4461682035211605, - "velocityY": -0.3675444390069978, - "timestamp": 9.547749282381027 - }, - { - "x": 4.226482344491556, - "y": 6.027144059810235, - "heading": 0.1858309320626014, - "angularVelocity": 0.4637305567548124, - "velocityX": -2.096406423384503, - "velocityY": -0.3150067045713868, - "timestamp": 9.612535642645447 - }, - { - "x": 4.113314607743797, - "y": 6.010139422555146, - "heading": 0.21176520360476983, - "angularVelocity": 0.40030449984110905, - "velocityX": -1.7467833705408997, - "velocityY": -0.26247248936131745, - "timestamp": 9.677322002909868 - }, - { - "x": 4.022790393937899, - "y": 5.996537426116962, - "heading": 0.23310242597379685, - "angularVelocity": 0.3293474472395599, - "velocityX": -1.3972727196965211, - "velocityY": -0.20995154509025238, - "timestamp": 9.742108363174289 - }, - { - "x": 3.954903777130295, - "y": 5.986337041905185, - "heading": 0.2494756207657714, - "angularVelocity": 0.2527259553577121, - "velocityX": -1.0478535378517617, - "velocityY": -0.1574464774715696, - "timestamp": 9.80689472343871 - }, - { - "x": 3.9096498799999146, - "y": 5.97953733708754, - "heading": 0.260600273972009, - "angularVelocity": 0.17171289081270033, - "velocityX": -0.6985096391537948, - "velocityY": -0.10495580844363857, - "timestamp": 9.87168108370313 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 0.08723493748434215, - "velocityX": -0.3492284312714647, - "velocityY": -0.052475536233735576, - "timestamp": 9.936467443967551 - }, - { - "x": 3.8870246410369873, - "y": 5.976137638092041, - "heading": 0.26625190805951465, - "angularVelocity": 3.6116165249279265e-32, - "velocityX": -3.053502496463557e-33, - "velocityY": 4.523388089316994e-32, - "timestamp": 10.001253804231972 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path deleted file mode 100644 index 74c48a8..0000000 --- a/src/main/deploy/pathplanner/New Path.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.0, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.0, - "y": 3.0 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 3.0, - "y": 5.0 - }, - "prevControl": { - "x": 3.0, - "y": 4.0 - }, - "nextControl": { - "x": 3.0, - "y": 4.0 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.0, - "y": 3.0 - }, - "prevControl": { - "x": 4.0, - "y": 3.0 - }, - "nextControl": null, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Piece HFE.auto b/src/main/deploy/pathplanner/autos/3 Piece HFE.auto deleted file mode 100644 index c79fee3..0000000 --- a/src/main/deploy/pathplanner/autos/3 Piece HFE.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3992020999924704, - "y": 2.850407598330388 - }, - "rotation": -63.38831811724382 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To H (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "H To Shoot (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "HShoot To F (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "F To Shoot (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "FShoot To E (HFE)" - } - }, - { - "type": "path", - "data": { - "pathName": "E To Shoot (HFE)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto b/src/main/deploy/pathplanner/autos/4 Piece ABC.auto deleted file mode 100644 index 14ca76d..0000000 --- a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.41, - "y": 4.3 - }, - "rotation": 0.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To C (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "C To B (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "B To A (ABC)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto b/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto deleted file mode 100644 index 4e8d00e..0000000 --- a/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.41, - "y": 4.3 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start To C (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "C To B (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "B To A (ABC)" - } - }, - { - "type": "path", - "data": { - "pathName": "A To D (ABCD)" - } - }, - { - "type": "path", - "data": { - "pathName": "D To Shoot (ABCD)" - } - }, - { - "type": "path", - "data": { - "pathName": "DShoot To E (ABCDE)" - } - }, - { - "type": "path", - "data": { - "pathName": "E To Shoot (ABCDE)" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/1 Note + Mobility.path similarity index 69% rename from src/main/deploy/pathplanner/paths/C To B (ABC).path rename to src/main/deploy/pathplanner/paths/1 Note + Mobility.path index 92179ad..737d557 100644 --- a/src/main/deploy/pathplanner/paths/C To B (ABC).path +++ b/src/main/deploy/pathplanner/paths/1 Note + Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 4.3 + "x": 0.9536854768153984, + "y": 3.918240646666479 }, "prevControl": null, "nextControl": { - "x": 2.1669872981077813, - "y": 4.55 + "x": 2.0332340849899873, + "y": 5.105453132209409 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 5.54 + "x": 2.393756643998068, + "y": 4.492368813448178 }, "prevControl": { - "x": 2.166987298107781, - "y": 5.29 + "x": 2.4912657927200694, + "y": 4.385180011107611 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": -39.896392922718064, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 6.path b/src/main/deploy/pathplanner/paths/2 Note Center 6.path new file mode 100644 index 0000000..5c036bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 6.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1969112668610487, + "y": 3.548383473371797 + }, + "prevControl": null, + "nextControl": { + "x": 4.405680480405183, + "y": 2.6115165497092763 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.354258240613484, + "y": 3.8645760601078973 + }, + "prevControl": { + "x": 4.065145382336701, + "y": 3.5048236345422836 + }, + "nextControl": { + "x": 7.368522126487903, + "y": 4.42669621430541 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.328810723241988, + "y": 4.075371117931965 + }, + "prevControl": { + "x": 7.33338961685056, + "y": 4.356431195030721 + }, + "nextControl": { + "x": 9.013237768290756, + "y": 3.882121128741725 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.354258240613484, + "y": 3.8645760601078973 + }, + "prevControl": { + "x": 7.591028020857753, + "y": 4.836575493407763 + }, + "nextControl": { + "x": 3.1682196012318817, + "y": 2.914622096397567 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1969112668610487, + "y": 3.548383473371797 + }, + "prevControl": { + "x": 4.534499682408779, + "y": 2.962841646082722 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 7.path b/src/main/deploy/pathplanner/paths/2 Note Center 7.path new file mode 100644 index 0000000..f2301f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 7.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1266462475863597, + "y": 3.9231302428368044 + }, + "prevControl": null, + "nextControl": { + "x": 3.398548537464012, + "y": 3.0096849922597873 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.089487893665121, + "y": 2.576384040065871 + }, + "prevControl": { + "x": 3.095128808200161, + "y": 3.32362429955275 + }, + "nextControl": { + "x": 6.068619269906157, + "y": 1.08910779875768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.164859011597086, + "y": 2.2953239629671147 + }, + "prevControl": { + "x": 6.9991342346038525, + "y": 1.9236435992811916 + }, + "nextControl": { + "x": 8.972906733259972, + "y": 2.5529623669803687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.089487893665121, + "y": 2.576384040065871 + }, + "prevControl": { + "x": 6.068619269906157, + "y": 0.7963368851131419 + }, + "nextControl": { + "x": 3.381204638750186, + "y": 3.213419866968167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1266462475823988, + "y": 3.8879977331934006 + }, + "prevControl": { + "x": 2.3445732483476367, + "y": 3.325877579001948 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 8.path b/src/main/deploy/pathplanner/paths/2 Note Center 8.path new file mode 100644 index 0000000..3d2797b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2 Note Center 8.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1266462475863597, + "y": 3.9231302428368044 + }, + "prevControl": null, + "nextControl": { + "x": 2.3914165945307624, + "y": 2.9862633191742836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.31199378803893, + "y": 2.0611072320575454 + }, + "prevControl": { + "x": 3.007825058291836, + "y": 2.4958301419732445 + }, + "nextControl": { + "x": 5.647029154258021, + "y": 1.616095443317847 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 0.749493538930016 + }, + "prevControl": { + "x": 7.602738857403534, + "y": 0.8080477216589235 + }, + "nextControl": { + "x": 8.556589177984769, + "y": 0.7258192457467483 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.183174586035333, + "y": 2.0611072320575454 + }, + "prevControl": { + "x": 4.710162230595501, + "y": 1.8034688280503517 + }, + "nextControl": { + "x": 3.978896971568412, + "y": 2.1609762880191514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0798029014032335, + "y": 3.9231302428368044 + }, + "prevControl": { + "x": 2.4733924503512332, + "y": 2.7520465882586542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path b/src/main/deploy/pathplanner/paths/2 Note Mobility.path similarity index 53% rename from src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path rename to src/main/deploy/pathplanner/paths/2 Note Mobility.path index 99943a5..9fb93ba 100644 --- a/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path +++ b/src/main/deploy/pathplanner/paths/2 Note Mobility.path @@ -3,47 +3,53 @@ "waypoints": [ { "anchor": { - "x": 7.954724143027569, - "y": 7.431149973519566 + "x": 1.4186651567747581, + "y": 4.0749560202824116 }, "prevControl": null, "nextControl": { - "x": 6.1477623701808435, - "y": 6.910372175243665 + "x": 1.4275950826207213, + "y": 4.085166421779603 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { "anchor": { - "x": 3.3531437468690912, - "y": 6.2 + "x": 2.44939980620702, + "y": 4.0749560202824116 }, "prevControl": { - "x": 5.055355188404675, - "y": 6.647601405814684 + "x": 2.3694684333813116, + "y": 4.087961199070547 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -0.08105107436216544, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 11.56141512604754, + "rotation": -0.42006383232481037, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note (4,1).path b/src/main/deploy/pathplanner/paths/3 Note (4,1).path new file mode 100644 index 0000000..f41e811 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note (4,1).path @@ -0,0 +1,181 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2234294142773756, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.2234294142773756, + "y": 6.799868019709742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 6.244414694343174 + }, + "prevControl": { + "x": 1.964117812947406, + "y": 6.244414694343174 + }, + "nextControl": { + "x": 3.4142135623730976, + "y": 6.244414694343174 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.276640929226852, + "y": 6.244414694343174 + }, + "prevControl": { + "x": 3.894920138865978, + "y": 6.244414694343174 + }, + "nextControl": { + "x": 6.6583617195877265, + "y": 6.244414694343174 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.290831218253917, + "y": 5.771197106303988 + }, + "prevControl": { + "x": 6.4380269751951875, + "y": 6.285864951635245 + }, + "nextControl": { + "x": 8.476003317926567, + "y": 5.719760411946759 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.097853218268906, + "y": 6.151828644514233 + }, + "prevControl": { + "x": 4.8239980189294345, + "y": 6.923379059795515 + }, + "nextControl": { + "x": 1.8891000945282543, + "y": 6.092747571757445 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9435431352126493, + "y": 6.162115983384651 + }, + "prevControl": { + "x": 1.9398341418478882, + "y": 6.261045103573999 + }, + "nextControl": { + "x": 1.9472521285774105, + "y": 6.063186863195303 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.900265650161439, + "y": 6.974815754147602 + }, + "prevControl": { + "x": 3.0545757332176953, + "y": 7.1188384983334405 + }, + "nextControl": { + "x": 2.0955600294777064, + "y": 6.2237571748427865 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0875658793984893, + "y": 6.56 + }, + "prevControl": { + "x": 2.1492899126209917, + "y": 6.724597421926673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -15.592810939801673, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 15.018360631150516, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 175.81508387488162, + "rotateFast": false + }, + { + "waypointRelativePos": 6.0, + "rotationDegrees": 54.293308599397186, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 50.979566839281325, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.44430514468718, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path b/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path new file mode 100644 index 0000000..32fccca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path @@ -0,0 +1,155 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1613758933368123, + "y": 3.534963938258926 + }, + "prevControl": null, + "nextControl": { + "x": 2.622104793438532, + "y": 3.2622945435689217 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 2.424809974168226 + }, + "prevControl": { + "x": 5.816231988338233, + "y": 0.4382186700247651 + }, + "nextControl": { + "x": 8.092728599078075, + "y": 2.7837000265429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 6.410261741050422, + "y": 0.16554927534895622 + }, + "nextControl": { + "x": 2.6293247577370926, + "y": 3.3179668640207294 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.183249464961813, + "y": 3.710251406271605 + }, + "prevControl": { + "x": 3.6154004455152573, + "y": 2.5319300935198457 + }, + "nextControl": { + "x": 5.753399221163684, + "y": 4.1387490491313965 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6275358244666664, + "y": 4.021873571623774 + }, + "prevControl": { + "x": 8.046220471824721, + "y": 4.0338359901197185 + }, + "nextControl": { + "x": 6.605025594392825, + "y": 3.992658993621665 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.183249464961813, + "y": 3.710251406271605 + }, + "prevControl": { + "x": 5.787017410338748, + "y": 3.8660624889495243 + }, + "nextControl": { + "x": 4.603138511506378, + "y": 3.5605453537669765 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 4.667125253586213, + "y": 2.5514064788509176 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -29.744881296441672, + "rotateFast": false + }, + { + "waypointRelativePos": 5.1499999999999995, + "rotationDegrees": 26.88010672415831, + "rotateFast": false + }, + { + "waypointRelativePos": 2.9499999999999997, + "rotationDegrees": 26.290465162772012, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.529705899934235, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path b/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path new file mode 100644 index 0000000..4ce8c21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path @@ -0,0 +1,113 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1613758933368123, + "y": 3.534963938258926 + }, + "prevControl": null, + "nextControl": { + "x": 7.734655943810105, + "y": 0.8180081840521911 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 0.7887936060606108 + }, + "prevControl": { + "x": 6.93406416515882, + "y": 1.2579320105256153 + }, + "nextControl": { + "x": 8.299471118517559, + "y": 0.46743324802687497 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 3.1827465189114155, + "y": 2.927907728824032 + }, + "nextControl": { + "x": 2.0183368480639654, + "y": 3.739465984263165 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.744394136478873, + "y": 2.424809974168226 + }, + "prevControl": { + "x": 5.816231988338233, + "y": 0.4382186700247651 + }, + "nextControl": { + "x": 8.092728599078075, + "y": 2.7837000265429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6610575641103793, + "y": 3.2915091215641485 + }, + "prevControl": { + "x": 6.05968680502248, + "y": 0.4187422846900244 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -28.393019422172674, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.529705899934235, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Mobility.path b/src/main/deploy/pathplanner/paths/3 Note Mobility.path new file mode 100644 index 0000000..f733e82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Note Mobility.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3931892586007395, + "y": 4.300319734898729 + }, + "prevControl": null, + "nextControl": { + "x": 1.833108731032277, + "y": 4.274843836724711 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.260023024675679, + "y": 4.11628754738991 + }, + "prevControl": { + "x": 1.843378826811743, + "y": 4.100841889320345 + }, + "nextControl": { + "x": 2.5147226220378367, + "y": 4.125729663496364 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.260023024675679, + "y": 5.517046256314735 + }, + "prevControl": { + "x": 1.674611826075504, + "y": 4.857107663289401 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path b/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path new file mode 100644 index 0000000..47e1bd7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3140196323188642, + "y": 7.026501927468906 + }, + "prevControl": null, + "nextControl": { + "x": 2.8062548621530805, + "y": 7.275509749307659 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 7.436381206571257 + }, + "prevControl": { + "x": 7.406875539339101, + "y": 7.983313605146107 + }, + "nextControl": { + "x": 8.65671414652387, + "y": 7.202164475655628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.618501382509183, + "y": 7.436381206571257 + }, + "prevControl": { + "x": 2.262597392527166, + "y": 7.576911245120637 + }, + "nextControl": { + "x": 0.8241493550218395, + "y": 7.263068036937653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 5.855418272890754 + }, + "prevControl": { + "x": 6.841534481927736, + "y": 6.218454205809981 + }, + "nextControl": { + "x": 9.515985591469327, + "y": 5.544405552185677 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.618501382509183, + "y": 7.22558614874719 + }, + "prevControl": { + "x": 2.649054998537956, + "y": 6.956236908194216 + }, + "nextControl": { + "x": -0.10503619526560826, + "y": 7.676056197483782 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9535367487282747, + "y": 7.026501927468906 + }, + "prevControl": { + "x": 2.8012958736331153, + "y": 6.113056676897948 + }, + "nextControl": { + "x": 3.085046944580154, + "y": 7.815563102580183 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5013930170513679, + "y": 6.5463576290918635 + }, + "prevControl": { + "x": 2.2274648828898207, + "y": 7.4480920431170405 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note End 4.path b/src/main/deploy/pathplanner/paths/4 Note End 4.path new file mode 100644 index 0000000..02d4dc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note End 4.path @@ -0,0 +1,113 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3990197549172392, + "y": 4.122752562280961 + }, + "prevControl": null, + "nextControl": { + "x": 0.86189001475465, + "y": 4.122752562280961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 4.417663265882087 + }, + "prevControl": { + "x": 2.2988487262132926, + "y": 4.417894718409181 + }, + "nextControl": { + "x": 2.639014036486218, + "y": 4.4121352736264425 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 5.51870901890497 + }, + "prevControl": { + "x": 1.7370438094738079, + "y": 5.455725753327694 + }, + "nextControl": { + "x": 2.630607491776283, + "y": 5.553522457244724 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 6.69 + }, + "prevControl": { + "x": 2.0244122309425774, + "y": 6.456084900181747 + }, + "nextControl": { + "x": 2.7512539157284497, + "y": 6.813577788583229 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.560791376212382, + "y": 7.402737796216334 + }, + "prevControl": { + "x": 7.219449971307349, + "y": 7.327200867364698 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": -0.04782618214180992, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": -34.50386141489783, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 38.59798223150914, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.3687240473329847, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path b/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path new file mode 100644 index 0000000..dc29915 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3990197549172392, + "y": 4.122752562280961 + }, + "prevControl": null, + "nextControl": { + "x": 0.86189001475465, + "y": 4.122752562280961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 4.417663265882087 + }, + "prevControl": { + "x": 2.2988487262132926, + "y": 4.417894718409181 + }, + "nextControl": { + "x": 2.639014036486218, + "y": 4.4121352736264425 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 2.312518814852445, + "y": 5.51870901890497 + }, + "prevControl": { + "x": 1.7370438094738079, + "y": 5.455725753327694 + }, + "nextControl": { + "x": 2.630607491776283, + "y": 5.553522457244724 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 2.500232833799001, + "y": 6.68953141609987 + }, + "prevControl": { + "x": 2.0248015033058433, + "y": 6.455735119803573 + }, + "nextControl": null, + "isLocked": true, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": -0.04782618214180992, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": -34.50386141489783, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.611383776770865, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A To D (ABCD).path b/src/main/deploy/pathplanner/paths/A To D (ABCD).path deleted file mode 100644 index c1343d5..0000000 --- a/src/main/deploy/pathplanner/paths/A To D (ABCD).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.6, - "y": 6.78 - }, - "prevControl": null, - "nextControl": { - "x": 3.5902680687415707, - "y": 6.919173100960065 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.954724143027569, - "y": 7.431149973519566 - }, - "prevControl": { - "x": 6.964456074285999, - "y": 7.2919768725595 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path similarity index 66% rename from src/main/deploy/pathplanner/paths/Start To C (ABC).path rename to src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path index 08d4184..005509a 100644 --- a/src/main/deploy/pathplanner/paths/Start To C (ABC).path +++ b/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.41, - "y": 4.3 + "x": 0.8539197217278677, + "y": 3.7498265505812527 }, "prevControl": null, "nextControl": { - "x": 1.91, - "y": 4.3 + "x": 1.8328118091416719, + "y": 3.7026753544270083 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 4.3 + "x": 2.667281089191343, + "y": 3.3374614295547143 }, "prevControl": { - "x": 2.1000000000000005, - "y": 4.3 + "x": 2.4942943959957344, + "y": 3.3668566966785813 }, "nextControl": null, "isLocked": false, @@ -39,14 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": -31.56492690745628, + "rotation": -52.08361958069638, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, + "folder": null, + "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path b/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path deleted file mode 100644 index b75ad29..0000000 --- a/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.3531437468690912, - "y": 6.2 - }, - "prevControl": null, - "nextControl": { - "x": 5.119271878480345, - "y": 6.2 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.954724143027569, - "y": 5.729888218095302 - }, - "prevControl": { - "x": 7.29352084832642, - "y": 6.172397406137405 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/Do Nothing.path similarity index 73% rename from src/main/deploy/pathplanner/paths/B To A (ABC).path rename to src/main/deploy/pathplanner/paths/Do Nothing.path index 6c0fb53..6f67a21 100644 --- a/src/main/deploy/pathplanner/paths/B To A (ABC).path +++ b/src/main/deploy/pathplanner/paths/Do Nothing.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 5.54 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 2.166987298107781, - "y": 5.79 + "x": 2.0166698250657547, + "y": 6.979658581285779 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6, - "y": 6.78 + "x": 2.0, + "y": 7.0 }, "prevControl": { - "x": 2.166987298107781, - "y": 6.53 + "x": 1.9932481519741914, + "y": 6.9679477447399965 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 30.0, + "rotation": -1.5074357587748137, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path b/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path deleted file mode 100644 index b135cef..0000000 --- a/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.954724143027569, - "y": 5.729888218095302 - }, - "prevControl": null, - "nextControl": { - "x": 6.6499751272795375, - "y": 6.343712985820069 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.3531437468690912, - "y": 6.2 - }, - "prevControl": { - "x": 4.157316983981167, - "y": 6.2 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 10.353493353078926, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path deleted file mode 100644 index da00da5..0000000 --- a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.929545854615797, - "y": 4.097291348201403 - }, - "prevControl": null, - "nextControl": { - "x": 5.929545854615797, - "y": 4.097291348201403 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.976354741378645, - "y": 5.106639879824356 - }, - "prevControl": { - "x": 5.288791546143204, - "y": 4.49370079566982 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -6.366672483399156, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path deleted file mode 100644 index 5f9ec94..0000000 --- a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.929545854615797, - "y": 2.432016158384531 - }, - "prevControl": null, - "nextControl": { - "x": 6.995066434251791, - "y": 3.4562610112362155 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 - }, - "prevControl": { - "x": 4.259643243410917, - "y": 4.815422774902082 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -13.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path deleted file mode 100644 index 6632f81..0000000 --- a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 - }, - "prevControl": null, - "nextControl": { - "x": 5.639808720888058, - "y": 4.489969784247294 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.929545854615797, - "y": 4.097291348201403 - }, - "prevControl": { - "x": 6.460969663475785, - "y": 4.359129446883026 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path deleted file mode 100644 index 0b9e815..0000000 --- a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 - }, - "prevControl": null, - "nextControl": { - "x": 5.970951958009818, - "y": 1.1314614999083628 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 - }, - "prevControl": { - "x": 4.464336906902614, - "y": 1.7553300211016838 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -40.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path deleted file mode 100644 index 09bd3d4..0000000 --- a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 - }, - "prevControl": null, - "nextControl": { - "x": 5.429728048854969, - "y": 1.1066294807828339 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.929545854615797, - "y": 2.432016158384531 - }, - "prevControl": { - "x": 7.178392770949869, - "y": 2.432016158384531 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "HFE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To H (HFE).path b/src/main/deploy/pathplanner/paths/Mobility.path similarity index 64% rename from src/main/deploy/pathplanner/paths/Start To H (HFE).path rename to src/main/deploy/pathplanner/paths/Mobility.path index a53726c..1e817ae 100644 --- a/src/main/deploy/pathplanner/paths/Start To H (HFE).path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3992020999924704, - "y": 2.850407598330388 + "x": 1.4882042634455643, + "y": 2.0023818427266487 }, "prevControl": null, "nextControl": { - "x": 3.361176508628245, - "y": 2.06581107755857 + "x": 1.4889168759819007, + "y": 1.9765496382844625 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 2.5869934100979313, + "y": 2.0023818427266487 }, "prevControl": { - "x": 5.745230507252463, - "y": 1.1502945514300327 + "x": 2.5638038104779914, + "y": 1.930912743769932 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 10.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "HFE", + "folder": null, "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3b52d75..c0dab87 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,13 +5,10 @@ package com.stuypulse.robot; -import com.pathplanner.lib.auto.AutoBuilder; +import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -25,7 +22,6 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -45,25 +41,16 @@ public class RobotContainer { public final AbstractNoteVision noteVision = AbstractNoteVision.getInstance(); // Autons - private static SendableChooser autonChooser; + private static SendableChooser autonChooser = new SendableChooser<>(); // Robot container public RobotContainer() { configureDefaultCommands(); configureButtonBindings(); - configureNamedCommands(); - - swerve.configureAutoBuilder(); configureAutons(); } - /**********************/ - /*** NAMED COMMANDS ***/ - /**********************/ - - private void configureNamedCommands() {} - /****************/ /*** DEFAULTS ***/ /****************/ @@ -105,7 +92,8 @@ private void configureButtonBindings() { /**************/ public void configureAutons() { - autonChooser = AutoBuilder.buildAutoChooser(); + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java deleted file mode 100644 index 2581431..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ /dev/null @@ -1,81 +0,0 @@ -/************************ PROJECT JON *************************/ -/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ -/* This work is licensed under the terms of the MIT license. */ -/**************************************************************/ - -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; - -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Alignment.Rotation; -import com.stuypulse.robot.constants.Settings.Alignment.Translation; -import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.HolonomicController; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - - -public class SwerveDriveToPose extends Command { - private final SwerveDrive swerve; - private final Pose2d targetPose; - - // Holonomic control - private final HolonomicController controller; - private final BStream aligned; - - private final FieldObject2d targetPose2d; - - public SwerveDriveToPose(Pose2d targetPose){ - this.swerve = SwerveDrive.getInstance(); - this.targetPose = targetPose; - - controller = new HolonomicController( - new PIDController(Translation.P,Translation.I,Translation.D), - new PIDController(Translation.P, Translation.I, Translation.D), - new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); - - SmartDashboard.putData("Alignment/Controller", controller); - - aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); - - targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose"); - addRequirements(swerve); - } - - private boolean isAligned() { - return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get()); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - Pose2d currentPose = AbstractOdometry.getInstance().getPose(); - - targetPose2d.setPose(targetPose); - - controller.update(targetPose, currentPose); - swerve.setChassisSpeeds(controller.getOutput()); - } - - @Override - public boolean isFinished(){ - return aligned.get(); - } - - public void end(boolean interupted) { - swerve.stop(); - targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java deleted file mode 100644 index 4bf65bc..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java +++ /dev/null @@ -1,64 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.vectors.VStream; -import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; -import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; -import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveDriveWithAiming extends Command { - - private final SwerveDrive swerve; - - private VStream speed; - private final AngleController gyroFeedback; - private final Pose2d target; - - public SwerveDriveWithAiming(Pose2d target, Gamepad driver) { - this.swerve = SwerveDrive.getInstance(); - - this.speed = VStream.create(driver::getLeftStick) - .filtered( - new VDeadZone(Drive.DEADBAND), - x -> x.clamp(1.0), - x -> Settings.vpow(x, Drive.POWER.get()), - x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), - new VRateLimit(Drive.MAX_TELEOP_ACCEL), - new VLowPassFilter(Drive.RC) - ); - - this.gyroFeedback = new AnglePIDController(Alignment.Gyro.P.get(), Alignment.Gyro.I.get(), Alignment.Gyro.D.get()); - this.target = target; - - addRequirements(swerve); - } - - @Override - public void execute() { - - Pose2d robotPose = AbstractOdometry.getInstance().getPose(); - - Rotation2d target = new Rotation2d( - robotPose.getX() - this.target.getX(), - robotPose.getY() - this.target.getY()) - .minus(Rotation2d.fromDegrees(180)); - - double angularVel = -gyroFeedback.update( - Angle.fromRotation2d(target.plus(Rotation2d.fromDegrees(180))), - Angle.fromRotation2d(swerve.getGyroAngle())); - - swerve.drive(speed.get(), angularVel); - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 0a88c9a..1fdae7a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -18,7 +18,7 @@ public interface Cameras { new Pose3d(-Units.inchesToMeters(12.5), -Units.inchesToMeters(11.5), +Units.inchesToMeters(8.5), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-34), Units.degreesToRadians(180)))); + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))); public static final CameraConfig[] ROBOT_CAMERAS = new CameraConfig[]{DEFAULT_CAMERA}; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 63627ac..4f117d6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -26,26 +26,26 @@ public interface Field { Fiducial FIDUCIALS[] = { // Simplified Lab Testing Layout - // new Fiducial(1,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), - // new Fiducial(3,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42) - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + new Fiducial(1,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2, Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + new Fiducial(3,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2 - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), // 2024 Field Fiducial Layout - new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), - new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), + // new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + // new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + // new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), + // new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + // new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + // new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + // new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + // new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), }; public static Fiducial[] getFiducialLayout(int[] tids) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index d4312c2..def9fcd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -28,18 +28,18 @@ protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); this.estimator = - new SwerveDrivePoseEstimator( + new SwerveDrivePoseEstimator( swerve.getKinematics(), swerve.getGyroAngle(), swerve.getModulePositions(), new Pose2d()); this.odometry = - new SwerveDriveOdometry( - swerve.getKinematics(), - swerve.getGyroAngle(), - swerve.getModulePositions(), - new Pose2d()); + new SwerveDriveOdometry( + swerve.getKinematics(), + swerve.getGyroAngle(), + swerve.getModulePositions(), + new Pose2d()); this.field = new Field2d(); this.odometryPose2D = field.getObject("Odometry Pose"); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 16a7bed..0362e30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -1,10 +1,6 @@ package com.stuypulse.robot.subsystems.swerve; import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -15,7 +11,6 @@ import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; -import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.module.SimModule; import com.stuypulse.robot.subsystems.swerve.module.AbstractModule; import com.stuypulse.robot.subsystems.swerve.module.JimModule; @@ -30,7 +25,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -79,31 +73,6 @@ public SwerveDrive(AbstractModule... modules) { module2ds = new FieldObject2d[modules.length]; } - public void configureAutoBuilder() { - AbstractOdometry odometry = AbstractOdometry.getInstance(); - - AutoBuilder.configureHolonomic( - odometry::getPose, - odometry::reset, - this::getChassisSpeeds, - this::setChassisSpeeds, - new HolonomicPathFollowerConfig( - Swerve.Motion.XY, - Swerve.Motion.THETA, - Swerve.MAX_MODULE_SPEED.get(), - Swerve.WIDTH, - new ReplanningConfig(true, true)), - () -> { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - instance - ); - } - public void initModule2ds(Field2d field) { for (int i = 0; i < modules.length; i++) { module2ds[i] = field.getObject(modules[i].getID() + "-2d"); From 1c749b62ce490440918b5ad3187eac2c026fa88e Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Fri, 19 Jan 2024 18:12:37 -0500 Subject: [PATCH 08/24] deadbanding alignment + filters --- .../swerve/SwerveDriveNoteAlignedDrive.java | 24 ++++++------- .../stuypulse/robot/constants/Settings.java | 11 +++++- .../subsystems/notevision/NoteVision.java | 12 ++++--- .../com/stuypulse/robot/util/Limelight.java | 34 ++++++++++++++++--- 4 files changed, 58 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 62b808c..fc6a04c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -3,7 +3,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; -import com.stuypulse.robot.constants.Settings .Driver.Turn.GyroFeedback; +import com.stuypulse.robot.constants.Settings.Driver.Turn.NoteAlignment; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; @@ -18,6 +18,8 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveNoteAlignedDrive extends Command { @@ -28,13 +30,9 @@ public class SwerveDriveNoteAlignedDrive extends Command { private VStream speed; private IStream turn; - private final Gamepad driver; - private AngleController alignController; public SwerveDriveNoteAlignedDrive(Gamepad driver) { - this.driver = driver; - swerve = SwerveDrive.getInstance(); noteVision = AbstractNoteVision.getInstance(); @@ -56,7 +54,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { new LowPassFilter(Turn.RC) ); - alignController = new AnglePIDController(GyroFeedback.P, GyroFeedback.I, GyroFeedback.D); + alignController = new AnglePIDController(NoteAlignment.P, NoteAlignment.I, NoteAlignment.D); addRequirements(swerve); } @@ -65,19 +63,19 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { public void execute() { double angularVel = turn.get(); - if (noteVision.hasNoteData()) { + if (noteVision.hasNoteData() && Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteAlignment.ANGLE_DEADBAND.get()) { angularVel = -alignController.update( Angle.kZero, Angle.fromRotation2d(noteVision.getRotationToNote()) ); } + + // robot relative + swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().x, speed.get().y, angularVel)); + // field relative + // swerve.drive(speed.get(), angularVel); - if(driver.getRawStartButton() || driver.getRawSelectButton()) { - swerve.setXMode(); - } - else { - swerve.drive(speed.get(), angularVel); - } + SmartDashboard.putNumber("Note Vision/Output", alignController.getOutput()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 89e8aed..b1f3b0d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -145,6 +145,14 @@ public interface GyroFeedback { SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0); SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); } + + public interface NoteAlignment { + SmartNumber ANGLE_DEADBAND = new SmartNumber("Driver Settings/Note Alignment/Angle Deadband", 1); + + SmartNumber P = new SmartNumber("Driver Settings/Note Alignment/kP", 3.0); + SmartNumber I = new SmartNumber("Driver Settings/Note Alignment/kI", 0.0); + SmartNumber D = new SmartNumber("Driver Settings/Note Alignment/kD", 0.0); + } } } @@ -202,6 +210,7 @@ public static Vector2D vpow(Vector2D vec, double power) { public interface NoteDetection { SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); + SmartNumber X_ANGLE_RC = new SmartNumber("Note Detection/X Angle RC", 0.05); SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); @@ -219,4 +228,4 @@ public interface Rotation { SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0); } } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index 2dd2529..7fc9d75 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -57,7 +57,7 @@ public boolean hasNoteData() { @Override public double getDistanceToNote() { for (Limelight limelight : limelights) { - if(limelight.hasNoteData()) { + if (limelight.hasNoteData()) { return limelight.getDistanceToNote(); } } @@ -78,10 +78,12 @@ public Rotation2d getRotationToNote() { @Override public void periodic(){ for (int i = 0; i < limelights.length; ++i) { - if(limelights[i].hasNoteData()) { - SmartDashboard.putNumber("Vision/X Angle", getRotationToNote().getDegrees()); - SmartDashboard.putNumber("Vision/Note Distance", getDistanceToNote()); - } + limelights[i].updateData(); + } + + if (hasNoteData()) { + SmartDashboard.putNumber("Note Detection/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Note Detection/Note Distance", getDistanceToNote()); } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java index d6df497..b7c9392 100644 --- a/src/main/java/com/stuypulse/robot/util/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -8,6 +8,13 @@ import static com.stuypulse.robot.constants.Settings.Limelight.LIMELIGHTS; import static com.stuypulse.robot.constants.Settings.Limelight.POSITIONS; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -18,6 +25,7 @@ public class Limelight { + private final String tableName; private final DoubleEntry txEntry; @@ -27,6 +35,12 @@ public class Limelight { private int limelightId; private final Pose3d robotRelativePose; + private double txData; + private double tyData; + + private IStream xAngle; + private BStream noteData; + public Limelight(String tableName, Pose3d robotRelativePose) { this.tableName = tableName; @@ -44,6 +58,11 @@ public Limelight(String tableName, Pose3d robotRelativePose) { txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); tvEntry = limelight.getIntegerTopic("tv").getEntry(0); + + xAngle = IStream.create(() -> txData) + .filtered(new LowPassFilter(NoteDetection.X_ANGLE_RC)); + noteData = BStream.create(() -> tvEntry.get() == 1) + .filtered(new BDebounceRC.Both(NoteDetection.DEBOUNCE_TIME)); } public String getTableName() { @@ -51,15 +70,22 @@ public String getTableName() { } public boolean hasNoteData() { - return tvEntry.get() == 1; + return noteData.get(); + } + + public void updateData() { + if (tvEntry.get() == 1) { + txData = txEntry.get(); + tyData = tyEntry.get(); + } } public double getXAngle() { - if(hasNoteData()) { + if(!hasNoteData()) { return Double.NaN; } - return txEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + return xAngle.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); } public double getYAngle() { @@ -67,7 +93,7 @@ public double getYAngle() { return Double.NaN; } - return tyEntry.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + return tyData + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); } public double getDistanceToNote() { From 41fbff17024b4ca961d231a0c04f130c651edd12 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 22:14:03 -0500 Subject: [PATCH 09/24] Merge main into note-detection --- .pathplanner/settings.json | 7 +- src/main/deploy/pathplanner/New Path.path | 74 +++++++++++++++++ .../deploy/pathplanner/autos/3 Piece HFE.auto | 55 +++++++++++++ .../deploy/pathplanner/autos/4 Piece ABC.auto | 37 +++++++++ .../pathplanner/autos/6 Piece ABCDE.auto | 61 ++++++++++++++ .../pathplanner/paths/A To D (ABCD).path | 49 +++++++++++ .../pathplanner/paths/B To A (ABC).path | 49 +++++++++++ .../pathplanner/paths/C To B (ABC).path | 49 +++++++++++ .../pathplanner/paths/D To Shoot (ABCD).path | 49 +++++++++++ .../paths/DShoot To E (ABCDE).path | 49 +++++++++++ .../pathplanner/paths/E To Shoot (ABCDE).path | 49 +++++++++++ .../pathplanner/paths/E To Shoot (HFE).path | 49 +++++++++++ .../pathplanner/paths/F To Shoot (HFE).path | 49 +++++++++++ .../pathplanner/paths/FShoot To E (HFE).path | 49 +++++++++++ .../pathplanner/paths/H To Shoot (HFE).path | 49 +++++++++++ .../pathplanner/paths/HShoot To F (HFE).path | 49 +++++++++++ .../pathplanner/paths/Start To C (ABC).path | 52 ++++++++++++ .../pathplanner/paths/Start To H (HFE).path | 49 +++++++++++ .../com/stuypulse/robot/RobotContainer.java | 24 ++++-- .../swerve/SwerveDriveNoteAlignedDrive.java | 8 +- .../commands/swerve/SwerveDriveToPose.java | 81 +++++++++++++++++++ .../swerve/SwerveDriveWithAiming.java | 64 +++++++++++++++ .../stuypulse/robot/constants/Cameras.java | 2 +- .../com/stuypulse/robot/constants/Field.java | 36 ++++----- .../stuypulse/robot/constants/Settings.java | 62 +++++++++----- .../robot/subsystems/odometry/Odometry.java | 12 +-- .../robot/subsystems/swerve/SwerveDrive.java | 31 +++++++ 27 files changed, 1137 insertions(+), 57 deletions(-) create mode 100644 src/main/deploy/pathplanner/New Path.path create mode 100644 src/main/deploy/pathplanner/autos/3 Piece HFE.auto create mode 100644 src/main/deploy/pathplanner/autos/4 Piece ABC.auto create mode 100644 src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto create mode 100644 src/main/deploy/pathplanner/paths/A To D (ABCD).path create mode 100644 src/main/deploy/pathplanner/paths/B To A (ABC).path create mode 100644 src/main/deploy/pathplanner/paths/C To B (ABC).path create mode 100644 src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path create mode 100644 src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path create mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path create mode 100644 src/main/deploy/pathplanner/paths/E To Shoot (HFE).path create mode 100644 src/main/deploy/pathplanner/paths/F To Shoot (HFE).path create mode 100644 src/main/deploy/pathplanner/paths/FShoot To E (HFE).path create mode 100644 src/main/deploy/pathplanner/paths/H To Shoot (HFE).path create mode 100644 src/main/deploy/pathplanner/paths/HShoot To F (HFE).path create mode 100644 src/main/deploy/pathplanner/paths/Start To C (ABC).path create mode 100644 src/main/deploy/pathplanner/paths/Start To H (HFE).path create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 70c55ea..f64d841 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,9 +1,10 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.66, + "robotLength": 0.66, "holonomicMode": true, "pathFolders": [ - "New Folder" + "ABCDE", + "HFE" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Piece HFE.auto b/src/main/deploy/pathplanner/autos/3 Piece HFE.auto new file mode 100644 index 0000000..c79fee3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Piece HFE.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3992020999924704, + "y": 2.850407598330388 + }, + "rotation": -63.38831811724382 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start To H (HFE)" + } + }, + { + "type": "path", + "data": { + "pathName": "H To Shoot (HFE)" + } + }, + { + "type": "path", + "data": { + "pathName": "HShoot To F (HFE)" + } + }, + { + "type": "path", + "data": { + "pathName": "F To Shoot (HFE)" + } + }, + { + "type": "path", + "data": { + "pathName": "FShoot To E (HFE)" + } + }, + { + "type": "path", + "data": { + "pathName": "E To Shoot (HFE)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto b/src/main/deploy/pathplanner/autos/4 Piece ABC.auto new file mode 100644 index 0000000..14ca76d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 Piece ABC.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.41, + "y": 4.3 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start To C (ABC)" + } + }, + { + "type": "path", + "data": { + "pathName": "C To B (ABC)" + } + }, + { + "type": "path", + "data": { + "pathName": "B To A (ABC)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto b/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto new file mode 100644 index 0000000..4e8d00e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/6 Piece ABCDE.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.41, + "y": 4.3 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start To C (ABC)" + } + }, + { + "type": "path", + "data": { + "pathName": "C To B (ABC)" + } + }, + { + "type": "path", + "data": { + "pathName": "B To A (ABC)" + } + }, + { + "type": "path", + "data": { + "pathName": "A To D (ABCD)" + } + }, + { + "type": "path", + "data": { + "pathName": "D To Shoot (ABCD)" + } + }, + { + "type": "path", + "data": { + "pathName": "DShoot To E (ABCDE)" + } + }, + { + "type": "path", + "data": { + "pathName": "E To Shoot (ABCDE)" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A To D (ABCD).path b/src/main/deploy/pathplanner/paths/A To D (ABCD).path new file mode 100644 index 0000000..c1343d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A To D (ABCD).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6, + "y": 6.78 + }, + "prevControl": null, + "nextControl": { + "x": 3.5902680687415707, + "y": 6.919173100960065 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.954724143027569, + "y": 7.431149973519566 + }, + "prevControl": { + "x": 6.964456074285999, + "y": 7.2919768725595 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/B To A (ABC).path new file mode 100644 index 0000000..6c0fb53 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B To A (ABC).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6, + "y": 5.54 + }, + "prevControl": null, + "nextControl": { + "x": 2.166987298107781, + "y": 5.79 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6, + "y": 6.78 + }, + "prevControl": { + "x": 2.166987298107781, + "y": 6.53 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/C To B (ABC).path new file mode 100644 index 0000000..92179ad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C To B (ABC).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6, + "y": 4.3 + }, + "prevControl": null, + "nextControl": { + "x": 2.1669872981077813, + "y": 4.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6, + "y": 5.54 + }, + "prevControl": { + "x": 2.166987298107781, + "y": 5.29 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path b/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path new file mode 100644 index 0000000..99943a5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D To Shoot (ABCD).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.954724143027569, + "y": 7.431149973519566 + }, + "prevControl": null, + "nextControl": { + "x": 6.1477623701808435, + "y": 6.910372175243665 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3531437468690912, + "y": 6.2 + }, + "prevControl": { + "x": 5.055355188404675, + "y": 6.647601405814684 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 11.56141512604754, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path b/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path new file mode 100644 index 0000000..b75ad29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DShoot To E (ABCDE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.3531437468690912, + "y": 6.2 + }, + "prevControl": null, + "nextControl": { + "x": 5.119271878480345, + "y": 6.2 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.954724143027569, + "y": 5.729888218095302 + }, + "prevControl": { + "x": 7.29352084832642, + "y": 6.172397406137405 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path b/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path new file mode 100644 index 0000000..b135cef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E To Shoot (ABCDE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.954724143027569, + "y": 5.729888218095302 + }, + "prevControl": null, + "nextControl": { + "x": 6.6499751272795375, + "y": 6.343712985820069 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3531437468690912, + "y": 6.2 + }, + "prevControl": { + "x": 4.157316983981167, + "y": 6.2 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 10.353493353078926, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path new file mode 100644 index 0000000..da00da5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.929545854615797, + "y": 4.097291348201403 + }, + "prevControl": null, + "nextControl": { + "x": 5.929545854615797, + "y": 4.097291348201403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.976354741378645, + "y": 5.106639879824356 + }, + "prevControl": { + "x": 5.288791546143204, + "y": 4.49370079566982 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -6.366672483399156, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path new file mode 100644 index 0000000..5f9ec94 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.929545854615797, + "y": 2.432016158384531 + }, + "prevControl": null, + "nextControl": { + "x": 6.995066434251791, + "y": 3.4562610112362155 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8814838652626524, + "y": 4.827042510937235 + }, + "prevControl": { + "x": 4.259643243410917, + "y": 4.815422774902082 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -13.0, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path new file mode 100644 index 0000000..6632f81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.8814838652626524, + "y": 4.827042510937235 + }, + "prevControl": null, + "nextControl": { + "x": 5.639808720888058, + "y": 4.489969784247294 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929545854615797, + "y": 4.097291348201403 + }, + "prevControl": { + "x": 6.460969663475785, + "y": 4.359129446883026 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path new file mode 100644 index 0000000..0b9e815 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.940567464034234, + "y": 0.7841651445745021 + }, + "prevControl": null, + "nextControl": { + "x": 5.970951958009818, + "y": 1.1314614999083628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.932248020664661, + "y": 3.0409052404747614 + }, + "prevControl": { + "x": 4.464336906902614, + "y": 1.7553300211016838 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -40.0, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path new file mode 100644 index 0000000..09bd3d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.932248020664661, + "y": 3.0409052404747614 + }, + "prevControl": null, + "nextControl": { + "x": 5.429728048854969, + "y": 1.1066294807828339 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929545854615797, + "y": 2.432016158384531 + }, + "prevControl": { + "x": 7.178392770949869, + "y": 2.432016158384531 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Start To C (ABC).path new file mode 100644 index 0000000..08d4184 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start To C (ABC).path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.41, + "y": 4.3 + }, + "prevControl": null, + "nextControl": { + "x": 1.91, + "y": 4.3 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6, + "y": 4.3 + }, + "prevControl": { + "x": 2.1000000000000005, + "y": 4.3 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.56492690745628, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To H (HFE).path b/src/main/deploy/pathplanner/paths/Start To H (HFE).path new file mode 100644 index 0000000..a53726c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start To H (HFE).path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3992020999924704, + "y": 2.850407598330388 + }, + "prevControl": null, + "nextControl": { + "x": 3.361176508628245, + "y": 2.06581107755857 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.940567464034234, + "y": 0.7841651445745021 + }, + "prevControl": { + "x": 5.745230507252463, + "y": 1.1502945514300327 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "HFE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 40ecf2e..3338596 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,10 +5,13 @@ package com.stuypulse.robot; -import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.pathplanner.lib.auto.AutoBuilder; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -22,6 +25,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -38,19 +42,27 @@ public class RobotContainer { public final AbstractOdometry odometry = AbstractOdometry.getInstance(); public final AbstractVision vision = AbstractVision.getInstance(); public final AbstractIntake intake = AbstractIntake.getInstance(); - public final AbstractNoteVision noteVision = AbstractNoteVision.getInstance(); // Autons - private static SendableChooser autonChooser = new SendableChooser<>(); + private static SendableChooser autonChooser; // Robot container public RobotContainer() { configureDefaultCommands(); configureButtonBindings(); + configureNamedCommands(); + + swerve.configureAutoBuilder(); configureAutons(); } + /**********************/ + /*** NAMED COMMANDS ***/ + /**********************/ + + private void configureNamedCommands() {} + /****************/ /*** DEFAULTS ***/ /****************/ @@ -76,6 +88,9 @@ private void configureButtonBindings() { driver.getLeftTriggerButton() .whileTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); + + driver.getLeftButton().whileTrue(new SwerveDriveToPose(new Pose2d(4, 5.5, new Rotation2d()))); + driver.getRightButton().whileTrue(new SwerveDriveToPose(new Pose2d(1.5, 5.5, new Rotation2d()))); driver.getTopButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(7).getPose().toPose2d(), driver)); driver.getBottomButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(8).getPose().toPose2d(), driver)); @@ -89,8 +104,7 @@ private void configureButtonBindings() { /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); - + autonChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index fc6a04c..e6012f7 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.commands.swerve; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.NoteDetection; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; -import com.stuypulse.robot.constants.Settings.Driver.Turn.NoteAlignment; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; @@ -54,7 +54,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { new LowPassFilter(Turn.RC) ); - alignController = new AnglePIDController(NoteAlignment.P, NoteAlignment.I, NoteAlignment.D); + alignController = new AnglePIDController(NoteDetection.Rotation.P, NoteDetection.Rotation.I, NoteDetection.Rotation.D); addRequirements(swerve); } @@ -63,7 +63,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { public void execute() { double angularVel = turn.get(); - if (noteVision.hasNoteData() && Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteAlignment.ANGLE_DEADBAND.get()) { + if (noteVision.hasNoteData() && Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteDetection.THRESHOLD_ANGLE.get()) { angularVel = -alignController.update( Angle.kZero, Angle.fromRotation2d(noteVision.getRotationToNote()) @@ -72,8 +72,6 @@ public void execute() { // robot relative swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().x, speed.get().y, angularVel)); - // field relative - // swerve.drive(speed.get(), angularVel); SmartDashboard.putNumber("Note Vision/Output", alignController.getOutput()); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java new file mode 100644 index 0000000..2581431 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -0,0 +1,81 @@ +/************************ PROJECT JON *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + + +public class SwerveDriveToPose extends Command { + private final SwerveDrive swerve; + private final Pose2d targetPose; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + + private final FieldObject2d targetPose2d; + + public SwerveDriveToPose(Pose2d targetPose){ + this.swerve = SwerveDrive.getInstance(); + this.targetPose = targetPose; + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + SmartDashboard.putData("Alignment/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(Alignment.DEBOUNCE_TIME)); + + targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + Pose2d currentPose = AbstractOdometry.getInstance().getPose(); + + targetPose2d.setPose(targetPose); + + controller.update(targetPose, currentPose); + swerve.setChassisSpeeds(controller.getOutput()); + } + + @Override + public boolean isFinished(){ + return aligned.get(); + } + + public void end(boolean interupted) { + swerve.stop(); + targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java new file mode 100644 index 0000000..4bf65bc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveWithAiming extends Command { + + private final SwerveDrive swerve; + + private VStream speed; + private final AngleController gyroFeedback; + private final Pose2d target; + + public SwerveDriveWithAiming(Pose2d target, Gamepad driver) { + this.swerve = SwerveDrive.getInstance(); + + this.speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1.0), + x -> Settings.vpow(x, Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC) + ); + + this.gyroFeedback = new AnglePIDController(Alignment.Gyro.P.get(), Alignment.Gyro.I.get(), Alignment.Gyro.D.get()); + this.target = target; + + addRequirements(swerve); + } + + @Override + public void execute() { + + Pose2d robotPose = AbstractOdometry.getInstance().getPose(); + + Rotation2d target = new Rotation2d( + robotPose.getX() - this.target.getX(), + robotPose.getY() - this.target.getY()) + .minus(Rotation2d.fromDegrees(180)); + + double angularVel = -gyroFeedback.update( + Angle.fromRotation2d(target.plus(Rotation2d.fromDegrees(180))), + Angle.fromRotation2d(swerve.getGyroAngle())); + + swerve.drive(speed.get(), angularVel); + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 1fdae7a..0a88c9a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -18,7 +18,7 @@ public interface Cameras { new Pose3d(-Units.inchesToMeters(12.5), -Units.inchesToMeters(11.5), +Units.inchesToMeters(8.5), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))); + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-34), Units.degreesToRadians(180)))); public static final CameraConfig[] ROBOT_CAMERAS = new CameraConfig[]{DEFAULT_CAMERA}; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 4f117d6..63627ac 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -26,26 +26,26 @@ public interface Field { Fiducial FIDUCIALS[] = { // Simplified Lab Testing Layout - new Fiducial(1,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2, Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), - new Fiducial(3,new Pose3d(new Translation3d(WIDTH / 2, HEIGHT / 2 - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + // new Fiducial(1,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + // new Fiducial(3,new Pose3d(new Translation3d(0, Units.inchesToMeters(218.42) - Units.inchesToMeters(44.25), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(0)))), // 2024 Field Fiducial Layout - // new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - // new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - // new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - // new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - // new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - // new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), - // new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - // new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - // new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - // new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - // new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), - // new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), - // new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), - // new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), - // new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), - // new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), + new Fiducial(1, new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + new Fiducial(2, new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + new Fiducial(3, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + new Fiducial(4, new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + new Fiducial(5, new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + new Fiducial(6, new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))), + new Fiducial(7, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + new Fiducial(8, new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + new Fiducial(9, new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))), + new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))), + new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))), + new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))), + new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))), + new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))), }; public static Fiducial[] getFiducialLayout(int[] tids) { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b368c16..39077d1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -24,7 +24,7 @@ public interface Settings { public enum RobotType { JIM("03262B9F"), // DeviceCode=0x7AAE@EDITOR=vi@PWD=/@TERM=dumb@DeviceDesc=roboRIO 2.0@SHLVL=3@TargetClass=cRIO@serialnum=03262B9F@PATH=/usr/local/bin:/usr/bin:/bin:/usr/local/frc/bin:/usr/local/natinst/bin@FPGADeviceCode=0x7AAF@_=/usr/local/frc/JRE/bin/java - OFFSEASON_BOT("0305a69d"), // DeviceCode=0x76F2@PWD=/usr/local/natinst/labview@DeviceDesc=roboRIO@SHLVL=2@TargetClass=cRIO@serialnum=0305a69d@FPGADeviceCode=0x77A9@_=/sbin/start-stop-daemon@ + OFFSEASON_BOT("0305A69D"), // DeviceCode=0x76F2@PWD=/usr/local/natinst/labview@DeviceDesc=roboRIO@SHLVL=2@TargetClass=cRIO@serialnum=0305a69d@FPGADeviceCode=0x77A9@_=/sbin/start-stop-daemon@ SIM(""); public final String serialNum; @@ -35,7 +35,7 @@ public enum RobotType { public static RobotType fromString(String serialNum) { for (RobotType robot : RobotType.values()) { - if (robot.serialNum.equals(serialNum)) { + if (robot.serialNum.equals(serialNum.toUpperCase())) { return robot; } } @@ -118,7 +118,7 @@ public interface Drive { } } } - + public interface Driver { public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); @@ -145,16 +145,7 @@ public interface GyroFeedback { SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0); SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); } - - public interface NoteAlignment { - SmartNumber ANGLE_DEADBAND = new SmartNumber("Driver Settings/Note Alignment/Angle Deadband", 1); - - SmartNumber P = new SmartNumber("Driver Settings/Note Alignment/kP", 3.0); - SmartNumber I = new SmartNumber("Driver Settings/Note Alignment/kI", 0.0); - SmartNumber D = new SmartNumber("Driver Settings/Note Alignment/kD", 0.0); - } } - } public interface Intake { @@ -188,17 +179,50 @@ public interface NoteDetection { SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", 0.1); - SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 0.1); + SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 1); public interface Translation { - SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 1); - SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0); - SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0); + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 1.0); + SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.0); } public interface Rotation { - SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 1); - SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0); - SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0); + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.0); + SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.0); + } + } + + public interface NoteAlignment { + + SmartNumber P = new SmartNumber("Driver Settings/Note Alignment/kP", 3.0); + SmartNumber I = new SmartNumber("Driver Settings/Note Alignment/kI", 0.0); + SmartNumber D = new SmartNumber("Driver Settings/Note Alignment/kD", 0.0); + } + + public interface Alignment { + SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + + + public interface Translation { + SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2.5); + SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Translation/kD", 0.0); + } + + public interface Rotation { + SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 1); + SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0); + } + + public interface Gyro { + SmartNumber P = new SmartNumber("Alignment/Gyro/kP", 12); + SmartNumber I = new SmartNumber("Alignment/Gyro/kI", 0); + SmartNumber D = new SmartNumber("Alignment/Gyro/kD", 0.1); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index def9fcd..d4312c2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -28,18 +28,18 @@ protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); this.estimator = - new SwerveDrivePoseEstimator( + new SwerveDrivePoseEstimator( swerve.getKinematics(), swerve.getGyroAngle(), swerve.getModulePositions(), new Pose2d()); this.odometry = - new SwerveDriveOdometry( - swerve.getKinematics(), - swerve.getGyroAngle(), - swerve.getModulePositions(), - new Pose2d()); + new SwerveDriveOdometry( + swerve.getKinematics(), + swerve.getGyroAngle(), + swerve.getModulePositions(), + new Pose2d()); this.field = new Field2d(); this.odometryPose2D = field.getObject("Odometry Pose"); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 0362e30..16a7bed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -1,6 +1,10 @@ package com.stuypulse.robot.subsystems.swerve; import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -11,6 +15,7 @@ import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft; import com.stuypulse.robot.constants.Settings.Swerve.FrontRight; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.module.SimModule; import com.stuypulse.robot.subsystems.swerve.module.AbstractModule; import com.stuypulse.robot.subsystems.swerve.module.JimModule; @@ -25,6 +30,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -73,6 +79,31 @@ public SwerveDrive(AbstractModule... modules) { module2ds = new FieldObject2d[modules.length]; } + public void configureAutoBuilder() { + AbstractOdometry odometry = AbstractOdometry.getInstance(); + + AutoBuilder.configureHolonomic( + odometry::getPose, + odometry::reset, + this::getChassisSpeeds, + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Swerve.Motion.XY, + Swerve.Motion.THETA, + Swerve.MAX_MODULE_SPEED.get(), + Swerve.WIDTH, + new ReplanningConfig(true, true)), + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + instance + ); + } + public void initModule2ds(Field2d field) { for (int i = 0; i < modules.length; i++) { module2ds[i] = field.getObject(modules[i].getID() + "-2d"); From c203dd3d2abfc483a436aa0c2b1e1edf189923bd Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 22:19:40 -0500 Subject: [PATCH 10/24] Merge main into note-detection --- src/main/deploy/Johnathan.chor | 1635 +++++++++++++++++ src/main/deploy/choreo/Straight Line.traj | 103 ++ src/main/deploy/choreo/Three Piece.traj | 1336 ++++++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 - .../pathplanner/paths/1 Note + Mobility.path | 49 - .../pathplanner/paths/2 Note Center 6.path | 97 - .../pathplanner/paths/2 Note Center 7.path | 97 - .../pathplanner/paths/2 Note Center 8.path | 97 - .../pathplanner/paths/2 Note Mobility.path | 55 - .../pathplanner/paths/3 Note (4,1).path | 181 -- .../pathplanner/paths/3 Note Center 7+6.path | 155 -- .../pathplanner/paths/3 Note Center 8+7.path | 113 -- .../pathplanner/paths/3 Note Mobility.path | 65 - .../pathplanner/paths/4 Note (5, 4, 1).path | 129 -- .../pathplanner/paths/4 Note End 4.path | 113 -- .../paths/4 Note Mobility End Speaker.path | 92 - .../paths/Bottom 1 Note + Mobility.path | 49 - .../deploy/pathplanner/paths/Do Nothing.path | 49 - .../deploy/pathplanner/paths/Mobility.path | 49 - 19 files changed, 3074 insertions(+), 1391 deletions(-) create mode 100644 src/main/deploy/Johnathan.chor create mode 100644 src/main/deploy/choreo/Straight Line.traj create mode 100644 src/main/deploy/choreo/Three Piece.traj delete mode 100644 src/main/deploy/pathplanner/navgrid.json delete mode 100644 src/main/deploy/pathplanner/paths/1 Note + Mobility.path delete mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 6.path delete mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 7.path delete mode 100644 src/main/deploy/pathplanner/paths/2 Note Center 8.path delete mode 100644 src/main/deploy/pathplanner/paths/2 Note Mobility.path delete mode 100644 src/main/deploy/pathplanner/paths/3 Note (4,1).path delete mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 7+6.path delete mode 100644 src/main/deploy/pathplanner/paths/3 Note Center 8+7.path delete mode 100644 src/main/deploy/pathplanner/paths/3 Note Mobility.path delete mode 100644 src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path delete mode 100644 src/main/deploy/pathplanner/paths/4 Note End 4.path delete mode 100644 src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path delete mode 100644 src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path delete mode 100644 src/main/deploy/pathplanner/paths/Do Nothing.path delete mode 100644 src/main/deploy/pathplanner/paths/Mobility.path diff --git a/src/main/deploy/Johnathan.chor b/src/main/deploy/Johnathan.chor new file mode 100644 index 0000000..897ee40 --- /dev/null +++ b/src/main/deploy/Johnathan.chor @@ -0,0 +1,1635 @@ +{ + "version": "v0.2", + "robotConfiguration": { + "mass": 65.77, + "rotationalInertia": 4.85963002223, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 4704, + "gearing": 4.71, + "wheelbase": 0.52, + "trackWidth": 0.67, + "bumperLength": 0.79, + "bumperWidth": 0.94, + "wheelRadius": 0.038 + }, + "paths": { + "Straight Line": { + "waypoints": [ + { + "x": 9.72, + "y": 4.12, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 11.72, + "y": 4.12, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 9.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 9.80132051089383, + "y": 4.12, + "heading": 1.0196133040364384e-18, + "angularVelocity": 1.0324520604294053e-17, + "velocityX": 0.8234448167415525, + "velocityY": 3.02420635388715e-34, + "timestamp": 0.09875647917200309 + }, + { + "x": 9.963961529614206, + "y": 4.12, + "heading": 3.0591556010799972e-18, + "angularVelocity": 2.0652237848908187e-17, + "velocityX": 1.6468896024240143, + "velocityY": 2.277444821719999e-33, + "timestamp": 0.19751295834400617 + }, + { + "x": 10.207923050026555, + "y": 4.12, + "heading": 6.11931106017425e-18, + "angularVelocity": 3.0986882933133645e-17, + "velocityX": 2.4703343259883073, + "velocityY": 1.5382285718210452e-33, + "timestamp": 0.29626943751600926 + }, + { + "x": 10.533205053727176, + "y": 4.12, + "heading": 1.020162012561919e-17, + "angularVelocity": 4.133712642975716e-17, + "velocityX": 3.29377886319825, + "velocityY": 1.5324076974034493e-33, + "timestamp": 0.39502591668801235 + }, + { + "x": 10.906794946272825, + "y": 4.12, + "heading": -1.0162422335797283e-17, + "angularVelocity": -2.062046220374934e-16, + "velocityX": 3.7829405794729865, + "velocityY": -3.7320649171210315e-33, + "timestamp": 0.49378239586001543 + }, + { + "x": 11.232076949973447, + "y": 4.12, + "heading": -6.100334399460735e-18, + "angularVelocity": 4.11323689413084e-17, + "velocityX": 3.2937788631982494, + "velocityY": 1.28740322268366e-34, + "timestamp": 0.5925388750320185 + }, + { + "x": 11.476038470385795, + "y": 4.12, + "heading": -3.0508797817029e-18, + "angularVelocity": 3.0878527093782464e-17, + "velocityX": 2.470334325988307, + "velocityY": -6.604493200637239e-34, + "timestamp": 0.6912953542040217 + }, + { + "x": 11.63867948910617, + "y": 4.12, + "heading": -1.0171236371693236e-18, + "angularVelocity": 2.0593647744285298e-17, + "velocityX": 1.646889602424014, + "velocityY": -7.180489499042344e-34, + "timestamp": 0.7900518333760247 + }, + { + "x": 11.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 1.0299310442024172e-17, + "velocityX": 0.8234448167415525, + "velocityY": -6.686788666173353e-34, + "timestamp": 0.8888083125480277 + }, + { + "x": 11.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 9.925887673927275e-44, + "timestamp": 0.9875647917200308 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "abdd982e-66b2-455b-8a91-2ff53944faf6" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "269654ce-bbb8-4e2d-b597-db25cd41cc9a" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "Three Piece": { + "waypoints": [ + { + "x": 1.898240327835083, + "y": 4.045846462249756, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.5026748180389404, + "y": 4.065344333648682, + "heading": -0.6823166411894661, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.8731348514556885, + "y": 4.201829433441162, + "heading": -0.5585989718626111, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.5806665420532227, + "y": 5.430196285247803, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 2.8536369800567627, + "y": 5.4496941566467285, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.6976537704467773, + "y": 6.483082294464111, + "heading": 0.5070982159979445, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 2.990122079849243, + "y": 6.5610737800598145, + "heading": 0.46364733726920737, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.884091854095459, + "y": 7.204504489898682, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 3.926020622253418, + "y": 5.995635032653809, + "heading": 0.17219063503059454, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.7198262214660645, + "y": 6.171116352081299, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 7.903589725494385, + "y": 5.6446733474731445, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.8953070640563965, + "y": 6.21011209487915, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 3.8870246410369873, + "y": 5.976137638092041, + "heading": 0.26625190805951465, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.898240327835083, + "y": 4.045846462249756, + "heading": 0, + "angularVelocity": -2.8038956593328693e-37, + "velocityX": -1.823589012902056e-36, + "velocityY": -4.380800340261528e-36, + "timestamp": 0 + }, + { + "x": 1.907488249459704, + "y": 4.045792662710147, + "heading": -0.014531093178687978, + "angularVelocity": -0.3399428010717914, + "velocityX": 0.21634741051533174, + "velocityY": -0.0012585953421492776, + "timestamp": 0.042745700549838096 + }, + { + "x": 1.9260125772155603, + "y": 4.04570358761662, + "heading": -0.0432778322806298, + "angularVelocity": -0.672505976792342, + "velocityX": 0.4333611922971804, + "velocityY": -0.002083837494312978, + "timestamp": 0.08549140109967619 + }, + { + "x": 1.9538513835951645, + "y": 4.045605326780032, + "heading": -0.0858077691342503, + "angularVelocity": -0.9949523883468459, + "velocityX": 0.6512656482760499, + "velocityY": -0.002298730289234273, + "timestamp": 0.1282371016495143 + }, + { + "x": 1.9910562559823362, + "y": 4.045539296716268, + "heading": -0.1415186846109791, + "angularVelocity": -1.3033103858427657, + "velocityX": 0.8703769480580534, + "velocityY": -0.0015447182503467369, + "timestamp": 0.17098280219935238 + }, + { + "x": 2.0376949895847924, + "y": 4.045574246714428, + "heading": -0.20959549088699864, + "angularVelocity": -1.5926000837592402, + "velocityX": 1.0910742601604893, + "velocityY": 0.0008176260468236386, + "timestamp": 0.21372850274919047 + }, + { + "x": 2.0938509724389283, + "y": 4.0458223742350645, + "heading": -0.28899731848757143, + "angularVelocity": -1.8575395087512154, + "velocityX": 1.3137223658005655, + "velocityY": 0.005804736323069604, + "timestamp": 0.2564742032990286 + }, + { + "x": 2.159619041381372, + "y": 4.046458363697538, + "heading": -0.37845923210523325, + "angularVelocity": -2.0928868276086927, + "velocityX": 1.538589100107577, + "velocityY": 0.014878442844382068, + "timestamp": 0.2992199038488667 + }, + { + "x": 2.2351062437840015, + "y": 4.047751445000734, + "heading": -0.4762822950411069, + "angularVelocity": -2.288488939883434, + "velocityX": 1.7659601183660054, + "velocityY": 0.030250558221350754, + "timestamp": 0.3419656043987048 + }, + { + "x": 2.3204435066348985, + "y": 4.050239499458989, + "heading": -0.5785043698305009, + "angularVelocity": -2.391400151933671, + "velocityX": 1.99639406427321, + "velocityY": 0.058205958172433236, + "timestamp": 0.38471130494854294 + }, + { + "x": 2.4129687747508384, + "y": 4.057068741649624, + "heading": -0.6558501426840586, + "angularVelocity": -1.8094398233894569, + "velocityX": 2.1645514502227607, + "velocityY": 0.15976442315343106, + "timestamp": 0.42745700549838106 + }, + { + "x": 2.5026748180389404, + "y": 4.065344333648682, + "heading": -0.6823166411894661, + "angularVelocity": -0.6191616505278632, + "velocityX": 2.0985980375619784, + "velocityY": 0.19360057017687107, + "timestamp": 0.4702027060482192 + }, + { + "x": 2.5850596627844538, + "y": 4.076067191512528, + "heading": -0.6785329802999686, + "angularVelocity": 0.08937347775881672, + "velocityX": 1.946004228329371, + "velocityY": 0.25328355970417904, + "timestamp": 0.5125380944025599 + }, + { + "x": 2.659003068905951, + "y": 4.0907479122594275, + "heading": -0.6634760532676318, + "angularVelocity": 0.3556581767081624, + "velocityX": 1.7466098457064405, + "velocityY": 0.3467718454363507, + "timestamp": 0.5548734827569006 + }, + { + "x": 2.7246369617363335, + "y": 4.110317723995373, + "heading": -0.642205314944677, + "angularVelocity": 0.5024339955245456, + "velocityX": 1.5503316582580267, + "velocityY": 0.4622565776921573, + "timestamp": 0.5972088711112413 + }, + { + "x": 2.7821274656068904, + "y": 4.1352087632427805, + "heading": -0.6169095715353604, + "angularVelocity": 0.5975082405668554, + "velocityX": 1.357977477125514, + "velocityY": 0.5879487637877254, + "timestamp": 0.639544259465582 + }, + { + "x": 2.831597012829208, + "y": 4.165661838985507, + "heading": -0.5887878540219638, + "angularVelocity": 0.6642602939654689, + "velocityX": 1.1685152574547029, + "velocityY": 0.7193290749535347, + "timestamp": 0.6818796478199227 + }, + { + "x": 2.8731348514556885, + "y": 4.201829433441162, + "heading": -0.5585989718626111, + "angularVelocity": 0.7130885845826266, + "velocityX": 0.9811611571580527, + "velocityY": 0.8543111534241201, + "timestamp": 0.7242150361742634 + }, + { + "x": 2.9188396470921765, + "y": 4.2725572090728265, + "heading": -0.5076501226687711, + "angularVelocity": 0.770144031880298, + "velocityX": 0.6908747919669516, + "velocityY": 1.069122760430878, + "timestamp": 0.7903699982135706 + }, + { + "x": 2.9452297152900053, + "y": 4.357347002147122, + "heading": -0.4530144734209342, + "angularVelocity": 0.8258737903193734, + "velocityX": 0.398912906671289, + "velocityY": 1.2816845548776343, + "timestamp": 0.8565249602528777 + }, + { + "x": 2.9521484208350985, + "y": 4.455984216280158, + "heading": -0.39482288845823243, + "angularVelocity": 0.8796254002553318, + "velocityX": 0.10458331970597358, + "velocityY": 1.4910025052153826, + "timestamp": 0.9226799222921849 + }, + { + "x": 2.9393582563456846, + "y": 4.568133489859713, + "heading": -0.3332972563741976, + "angularVelocity": 0.9300229368656926, + "velocityX": -0.19333643456426472, + "velocityY": 1.695251121343236, + "timestamp": 0.988834884331492 + }, + { + "x": 2.906458551093886, + "y": 4.693199281947042, + "heading": -0.26886974643996187, + "angularVelocity": 0.9738877923617428, + "velocityX": -0.4973127372101132, + "velocityY": 1.8904975262931591, + "timestamp": 1.0549898463707992 + }, + { + "x": 2.8526414862163594, + "y": 4.829843806089774, + "heading": -0.2025647523319625, + "angularVelocity": 1.0022678883649443, + "velocityX": -0.81350004925632, + "velocityY": 2.065521918998951, + "timestamp": 1.1211448084101063 + }, + { + "x": 2.775736178341105, + "y": 4.97269513900139, + "heading": -0.13812989460617203, + "angularVelocity": 0.9739988617559082, + "velocityX": -1.1625024866548819, + "velocityY": 2.1593441898846235, + "timestamp": 1.1872997704494135 + }, + { + "x": 2.702646192253521, + "y": 5.09239426390518, + "heading": -0.0945099100170612, + "angularVelocity": 0.6593607379472648, + "velocityX": -1.1048299905932448, + "velocityY": 1.809374855852369, + "timestamp": 1.2534547324887206 + }, + { + "x": 2.6443183089698836, + "y": 5.193435173263213, + "heading": -0.060805157515131665, + "angularVelocity": 0.5094818508383886, + "velocityX": -0.8816856889583129, + "velocityY": 1.5273368201465674, + "timestamp": 1.3196096945280278 + }, + { + "x": 2.602443138845534, + "y": 5.277234789353118, + "heading": -0.03510328646046884, + "angularVelocity": 0.3885101020750583, + "velocityX": -0.63298608046164, + "velocityY": 1.2667170157261087, + "timestamp": 1.385764656567335 + }, + { + "x": 2.577670236134593, + "y": 5.344417005113783, + "heading": -0.016652156501204083, + "angularVelocity": 0.2789077249912364, + "velocityX": -0.3744677942105343, + "velocityY": 1.015527992000793, + "timestamp": 1.451919618606642 + }, + { + "x": 2.5703412148254166, + "y": 5.395330206172067, + "heading": -0.005041861251989399, + "angularVelocity": 0.17550150270385198, + "velocityX": -0.1107856626812363, + "velocityY": 0.7696051738043858, + "timestamp": 1.5180745806459492 + }, + { + "x": 2.5806665420532227, + "y": 5.430196285247803, + "heading": 4.946919732174461e-30, + "angularVelocity": 0.07621289615423993, + "velocityX": 0.15607789513463036, + "velocityY": 0.5270364913068623, + "timestamp": 1.5842295426852564 + }, + { + "x": 2.615476810555517, + "y": 5.449290678492884, + "heading": -0.00249481878254215, + "angularVelocity": -0.03313174320347507, + "velocityX": 0.4622880366833039, + "velocityY": 0.25357775003504285, + "timestamp": 1.659529497994595 + }, + { + "x": 2.6735006636921637, + "y": 5.447944678639869, + "heading": -0.01213678704864337, + "angularVelocity": -0.12804746332838068, + "velocityX": 0.7705695560944235, + "velocityY": -0.017875174659603758, + "timestamp": 1.7348294533039335 + }, + { + "x": 2.7555584259794457, + "y": 5.427049168858778, + "heading": -0.02296797038520493, + "angularVelocity": -0.14384050152574682, + "velocityX": 1.0897451658527806, + "velocityY": -0.27749697453671124, + "timestamp": 1.810129408613272 + }, + { + "x": 2.8158286951067315, + "y": 5.427810213575971, + "heading": -0.01758027986858031, + "angularVelocity": 0.07154971732043594, + "velocityX": 0.8004024554820866, + "velocityY": 0.010106841552119225, + "timestamp": 1.8854293639226105 + }, + { + "x": 2.8536369800567627, + "y": 5.4496941566467285, + "heading": 0, + "angularVelocity": 0.23346999073716668, + "velocityX": 0.5021023557678279, + "velocityY": 0.2906235864398052, + "timestamp": 1.960729319231949 + }, + { + "x": 2.8693148468887744, + "y": 5.487156976773219, + "heading": 0.025561040529235773, + "angularVelocity": 0.3719476521458912, + "velocityX": 0.22813413061776552, + "velocityY": 0.5451346150355147, + "timestamp": 2.0294514643557737 + }, + { + "x": 2.8662677650419504, + "y": 5.542220250273633, + "heading": 0.0607369491230168, + "angularVelocity": 0.5118569644530235, + "velocityX": -0.04433915503268782, + "velocityY": 0.8012449757090656, + "timestamp": 2.0981736094795984 + }, + { + "x": 2.844654009258225, + "y": 5.615050469126244, + "heading": 0.10566640088464291, + "angularVelocity": 0.6537841867519056, + "velocityX": -0.3145093294858733, + "velocityY": 1.0597780194634927, + "timestamp": 2.166895754603423 + }, + { + "x": 2.804748272410547, + "y": 5.705930381335797, + "heading": 0.16050762468177382, + "angularVelocity": 0.7980138527154114, + "velocityX": -0.5806823517481201, + "velocityY": 1.3224254284525792, + "timestamp": 2.235617899727248 + }, + { + "x": 2.7471447891895058, + "y": 5.815446220375669, + "heading": 0.22535865974809183, + "angularVelocity": 0.9436701219012945, + "velocityX": -0.8382084569282627, + "velocityY": 1.5936033260100555, + "timestamp": 2.3043400448510725 + }, + { + "x": 2.6740678731800593, + "y": 5.945504225324297, + "heading": 0.2996496097363406, + "angularVelocity": 1.0810336297621417, + "velocityX": -1.0633677961852832, + "velocityY": 1.892519575957448, + "timestamp": 2.373062189974897 + }, + { + "x": 2.6265491520645683, + "y": 6.0733583679407905, + "heading": 0.3634596160425804, + "angularVelocity": 0.9285217478480327, + "velocityX": -0.6914615518748894, + "velocityY": 1.8604504033761498, + "timestamp": 2.441784335098722 + }, + { + "x": 2.600377734078398, + "y": 6.186973463102748, + "heading": 0.4156012346676629, + "angularVelocity": 0.7587309524627455, + "velocityX": -0.38082946827422787, + "velocityY": 1.6532530373905598, + "timestamp": 2.5105064802225465 + }, + { + "x": 2.59468135033937, + "y": 6.285113460277094, + "heading": 0.45604131218202076, + "angularVelocity": 0.5884577299135867, + "velocityX": -0.08289007464427144, + "velocityY": 1.4280694672367455, + "timestamp": 2.579228625346371 + }, + { + "x": 2.6091090827604106, + "y": 6.367321455091837, + "heading": 0.4847594324399464, + "angularVelocity": 0.41788742487855773, + "velocityX": 0.20994298701015016, + "velocityY": 1.1962373215594329, + "timestamp": 2.647950770470196 + }, + { + "x": 2.6434723912297375, + "y": 6.433359811846902, + "heading": 0.5017638856587859, + "angularVelocity": 0.24743775369934545, + "velocityX": 0.5000325354717966, + "velocityY": 0.9609472555909916, + "timestamp": 2.7166729155940206 + }, + { + "x": 2.6976537704467773, + "y": 6.483082294464111, + "heading": 0.5070982159979445, + "angularVelocity": 0.07762170883267666, + "velocityX": 0.7884122231547788, + "velocityY": 0.7235292572375208, + "timestamp": 2.7853950607178453 + }, + { + "x": 2.738806835827011, + "y": 6.507590262026266, + "heading": 0.5059467865388958, + "angularVelocity": -0.027038737137375757, + "velocityX": 0.966387396526008, + "velocityY": 0.5755146244320987, + "timestamp": 2.8279794999687833 + }, + { + "x": 2.7880294623095803, + "y": 6.526430774501013, + "heading": 0.5005508163010642, + "angularVelocity": -0.12671225294372634, + "velocityX": 1.155882931615343, + "velocityY": 0.44242715898465257, + "timestamp": 2.8705639392197213 + }, + { + "x": 2.84591135896884, + "y": 6.540541693646972, + "heading": 0.4912680248506681, + "angularVelocity": -0.21798552742928792, + "velocityX": 1.35922646105959, + "velocityY": 0.3313632724575067, + "timestamp": 2.9131483784706593 + }, + { + "x": 2.913103056068397, + "y": 6.55135708254599, + "heading": 0.4786790444220719, + "angularVelocity": -0.2956239567794471, + "velocityX": 1.5778462340108705, + "velocityY": 0.25397513949368183, + "timestamp": 2.9557328177215973 + }, + { + "x": 2.990122079849243, + "y": 6.5610737800598145, + "heading": 0.46364733726920737, + "angularVelocity": -0.35298591263083134, + "velocityX": 1.8086189494475877, + "velocityY": 0.22817483768114147, + "timestamp": 2.9983172569725354 + }, + { + "x": 3.1507680403022618, + "y": 6.583513336972122, + "heading": 0.4338303597620861, + "angularVelocity": -0.4082414173674588, + "velocityX": 2.199496396777096, + "velocityY": 0.3072329016846347, + "timestamp": 3.0713548671674924 + }, + { + "x": 3.339966410927534, + "y": 6.61173687205727, + "heading": 0.4005433233854809, + "angularVelocity": -0.4557519925385439, + "velocityX": 2.5904238942135573, + "velocityY": 0.38642467914559536, + "timestamp": 3.1443924773624494 + }, + { + "x": 3.5577217220068316, + "y": 6.645759083728854, + "heading": 0.36470254911055944, + "angularVelocity": -0.4907166893776054, + "velocityX": 2.981413418347747, + "velocityY": 0.46581770105525094, + "timestamp": 3.2174300875574064 + }, + { + "x": 3.8040369699341148, + "y": 6.685605934834726, + "heading": 0.32812163175231024, + "angularVelocity": -0.5008504147466596, + "velocityX": 3.372443967837953, + "velocityY": 0.5455661952726859, + "timestamp": 3.2904676977523635 + }, + { + "x": 4.078865987183254, + "y": 6.731338843022743, + "heading": 0.29648766626359246, + "angularVelocity": -0.43311884663638234, + "velocityX": 3.7628424111296486, + "velocityY": 0.6261555938911911, + "timestamp": 3.3635053079473205 + }, + { + "x": 4.363416873029506, + "y": 6.788685305113101, + "heading": 0.29648766061986404, + "angularVelocity": -7.727153775240832e-8, + "velocityX": 3.8959501151079365, + "velocityY": 0.7851634512312887, + "timestamp": 3.4365429181422775 + }, + { + "x": 4.647967686637335, + "y": 6.846032125646838, + "heading": 0.2964876549762135, + "angularVelocity": -7.72704712131681e-8, + "velocityX": 3.895949126050067, + "velocityY": 0.7851683588860835, + "timestamp": 3.5095805283372346 + }, + { + "x": 4.932518500243366, + "y": 6.903378946189493, + "heading": 0.2964876493325629, + "angularVelocity": -7.727047143608747e-8, + "velocityX": 3.895949126025459, + "velocityY": 0.7851683590081883, + "timestamp": 3.5826181385321916 + }, + { + "x": 5.217069313849397, + "y": 6.960725766732147, + "heading": 0.29648764368891234, + "angularVelocity": -7.727047107791159e-8, + "velocityX": 3.8959491260254584, + "velocityY": 0.7851683590081909, + "timestamp": 3.6556557487271486 + }, + { + "x": 5.501620127455428, + "y": 7.018072587274802, + "heading": 0.2964876380452618, + "angularVelocity": -7.72704716252991e-8, + "velocityX": 3.895949126025459, + "velocityY": 0.7851683590081912, + "timestamp": 3.7286933589221056 + }, + { + "x": 5.786170941061528, + "y": 7.075419407817124, + "heading": 0.29648763240161125, + "angularVelocity": -7.727047111704844e-8, + "velocityX": 3.8959491260263763, + "velocityY": 0.7851683590036359, + "timestamp": 3.8017309691170627 + }, + { + "x": 6.070721757362521, + "y": 7.132766214987568, + "heading": 0.29648762675796064, + "angularVelocity": -7.727047165435608e-8, + "velocityX": 3.895949162923739, + "velocityY": 0.7851681759215458, + "timestamp": 3.8747685793120197 + }, + { + "x": 6.355380338499842, + "y": 7.1895756818041425, + "heading": 0.29648762107984833, + "angularVelocity": -7.774230685415808e-8, + "velocityX": 3.8974246333839155, + "velocityY": 0.7778111395614767, + "timestamp": 3.9478061895069767 + }, + { + "x": 6.636405520312889, + "y": 7.217870773127444, + "heading": 0.27570831993349604, + "angularVelocity": -0.28450138347744536, + "velocityX": 3.847677669941766, + "velocityY": 0.387404396827479, + "timestamp": 4.020843799701934 + }, + { + "x": 6.88919116626837, + "y": 7.2399532386560255, + "heading": 0.24179746779298844, + "angularVelocity": -0.4642930135582247, + "velocityX": 3.4610339150025435, + "velocityY": 0.30234375782058953, + "timestamp": 4.093881409896891 + }, + { + "x": 7.113463572845222, + "y": 7.256040709772393, + "heading": 0.20495027392657358, + "angularVelocity": -0.5044961598286116, + "velocityX": 3.070642727468879, + "velocityY": 0.22026283545456557, + "timestamp": 4.166919020091848 + }, + { + "x": 7.3091937477745095, + "y": 7.2662136587336335, + "heading": 0.1681230341900902, + "angularVelocity": -0.5042229563396395, + "velocityX": 2.679854590078042, + "velocityY": 0.13928370512239335, + "timestamp": 4.239956630286805 + }, + { + "x": 7.476376305660358, + "y": 7.270512578107467, + "heading": 0.13274981372464448, + "angularVelocity": -0.4843151408024598, + "velocityX": 2.288992718129635, + "velocityY": 0.058858981863695155, + "timestamp": 4.312994240481762 + }, + { + "x": 7.615010848660238, + "y": 7.268961369646756, + "heading": 0.09968688668304959, + "angularVelocity": -0.45268358251784385, + "velocityX": 1.8981253990899605, + "velocityY": -0.021238488726158328, + "timestamp": 4.386031850676719 + }, + { + "x": 7.7250983383538445, + "y": 7.261575556193537, + "heading": 0.06950965194609489, + "angularVelocity": -0.4131739066544426, + "velocityX": 1.5072712455918797, + "velocityY": -0.10112342714260779, + "timestamp": 4.459069460871676 + }, + { + "x": 7.8066401035121915, + "y": 7.248365897189822, + "heading": 0.04263526512944047, + "angularVelocity": -0.36795271292310205, + "velocityX": 1.1164352850632593, + "velocityY": -0.18086105183966827, + "timestamp": 4.532107071066633 + }, + { + "x": 7.859637512937734, + "y": 7.2293402273974605, + "heading": 0.019381829491425508, + "angularVelocity": -0.3183761842144796, + "velocityX": 0.7256180655976813, + "velocityY": -0.2604914062984367, + "timestamp": 4.60514468126159 + }, + { + "x": 7.884091854095459, + "y": 7.204504489898682, + "heading": 0, + "angularVelocity": -0.26536779393096516, + "velocityX": 0.33481847355697797, + "velocityY": -0.3400403905944489, + "timestamp": 4.678182291456547 + }, + { + "x": 7.8693875341309285, + "y": 7.163825702906692, + "heading": -0.017989310395224636, + "angularVelocity": -0.1948160433685539, + "velocityX": -0.15924109223639094, + "velocityY": -0.44053274732062453, + "timestamp": 4.770522275803691 + }, + { + "x": 7.809062158016808, + "y": 7.113867015205141, + "heading": -0.02942623999470572, + "angularVelocity": -0.12385674180412554, + "velocityX": -0.6532963649564154, + "velocityY": -0.5410298480638316, + "timestamp": 4.862862260150834 + }, + { + "x": 7.703115992354989, + "y": 7.054628240082747, + "heading": -0.03428596694706339, + "angularVelocity": -0.05262863088743923, + "velocityX": -1.1473487504992854, + "velocityY": -0.641528970805246, + "timestamp": 4.955202244497977 + }, + { + "x": 7.551549165293792, + "y": 6.986109443661236, + "heading": -0.032557590593216855, + "angularVelocity": 0.018717529205428393, + "velocityX": -1.6413997482541873, + "velocityY": -0.7420273774784575, + "timestamp": 5.047542228845121 + }, + { + "x": 7.354361663520876, + "y": 6.908310952017223, + "heading": -0.024244154578006688, + "angularVelocity": 0.09003072801005149, + "velocityX": -2.1354508901756786, + "velocityY": -0.8425222528903508, + "timestamp": 5.139882213192264 + }, + { + "x": 7.111553334877506, + "y": 6.8212333658210484, + "heading": -0.00936157178577619, + "angularVelocity": 0.16117159752031956, + "velocityX": -2.6295036799070197, + "velocityY": -0.9430106233158517, + "timestamp": 5.232222197539407 + }, + { + "x": 6.823123896075064, + "y": 6.724877589969487, + "heading": 0.012063558679812484, + "angularVelocity": 0.23202441084506603, + "velocityX": -3.1235595375251535, + "velocityY": -1.0434891941211963, + "timestamp": 5.3245621818865505 + }, + { + "x": 6.48907294008907, + "y": 6.619244934399674, + "heading": 0.039997176732724696, + "angularVelocity": 0.3025083689412195, + "velocityX": -3.6176198030331097, + "velocityY": -1.1439535789035715, + "timestamp": 5.416902166233694 + }, + { + "x": 6.129437398233647, + "y": 6.5461675567988795, + "heading": 0.03999718077634422, + "angularVelocity": 4.3790558964884806e-8, + "velocityX": -3.8946892226384597, + "velocityY": -0.7913947367163172, + "timestamp": 5.509242150580837 + }, + { + "x": 5.769801333833775, + "y": 6.4730927508413325, + "heading": 0.03999718481982793, + "angularVelocity": 4.378908809561466e-8, + "velocityX": -3.8946948815570095, + "velocityY": -0.7913668869904851, + "timestamp": 5.60158213492798 + }, + { + "x": 5.410165307610781, + "y": 6.400017756997467, + "heading": 0.03999718886333287, + "angularVelocity": 4.378931804611701e-8, + "velocityX": -3.8946944681187916, + "velocityY": -0.7913689217137886, + "timestamp": 5.693922119275124 + }, + { + "x": 5.06488865586912, + "y": 6.3170886108245945, + "heading": 0.07172411836277942, + "angularVelocity": 0.3435882053019635, + "velocityX": -3.739188978456266, + "velocityY": -0.8980849061129225, + "timestamp": 5.786262103622267 + }, + { + "x": 4.765275429088764, + "y": 6.243389106592463, + "heading": 0.10120262922920165, + "angularVelocity": 0.3192388549211855, + "velocityX": -3.244674870790441, + "velocityY": -0.7981320849596799, + "timestamp": 5.87860208796941 + }, + { + "x": 4.511309122993011, + "y": 6.178941794592798, + "heading": 0.12636460911438926, + "angularVelocity": 0.27249278915397723, + "velocityX": -2.7503394969289876, + "velocityY": -0.6979350544113176, + "timestamp": 5.970942072316554 + }, + { + "x": 4.302982802001526, + "y": 6.123753419378413, + "heading": 0.14653342260027327, + "angularVelocity": 0.21841906979387646, + "velocityX": -2.256079232245732, + "velocityY": -0.5976649834259027, + "timestamp": 6.063282056663697 + }, + { + "x": 4.140292768892181, + "y": 6.077827039965534, + "heading": 0.16137256146424936, + "angularVelocity": 0.16070111955174202, + "velocityX": -1.7618590067952369, + "velocityY": -0.497361784687127, + "timestamp": 6.15562204101084 + }, + { + "x": 4.023236723432337, + "y": 6.041164401680778, + "heading": 0.17067956492366876, + "angularVelocity": 0.10079061118779088, + "velocityX": -1.2676636918172275, + "velocityY": -0.3970396848555328, + "timestamp": 6.2479620253579835 + }, + { + "x": 3.9518130737172363, + "y": 6.01376672295597, + "heading": 0.174318405736917, + "angularVelocity": 0.03940698971280611, + "velocityX": -0.7734856164431515, + "velocityY": -0.29670438996183723, + "timestamp": 6.340302009705127 + }, + { + "x": 3.926020622253418, + "y": 5.995635032653809, + "heading": 0.17219063503059454, + "angularVelocity": -0.023042788250028232, + "velocityX": -0.2793205093781995, + "velocityY": -0.19635795295345568, + "timestamp": 6.43264199405227 + }, + { + "x": 3.930355880820906, + "y": 5.98747940429267, + "heading": 0.1678304420080604, + "angularVelocity": -0.0673769466267154, + "velocityX": 0.06699164087576895, + "velocityY": -0.12602683733396786, + "timestamp": 6.497355419650052 + }, + { + "x": 3.957112124799613, + "y": 5.9838306427917995, + "heading": 0.16069467758998768, + "angularVelocity": -0.11026714089319978, + "velocityX": 0.41345738896603046, + "velocityY": -0.056383377439255714, + "timestamp": 6.562068845247834 + }, + { + "x": 4.006301141076061, + "y": 5.984635223325004, + "heading": 0.15089514245932092, + "angularVelocity": -0.15142970782560608, + "velocityX": 0.760105276796434, + "velocityY": 0.012432977017907793, + "timestamp": 6.6267822708456166 + }, + { + "x": 4.0779371329258005, + "y": 5.989827517197893, + "heading": 0.13856825674408835, + "angularVelocity": -0.1904842094413026, + "velocityX": 1.106972644208068, + "velocityY": 0.08023518806075768, + "timestamp": 6.691495696443399 + }, + { + "x": 4.17203754289526, + "y": 5.999325152011095, + "heading": 0.12388458270411208, + "angularVelocity": -0.22690305611760084, + "velocityX": 1.4541095468246137, + "velocityY": 0.14676451950228486, + "timestamp": 6.756209122041181 + }, + { + "x": 4.288624284050538, + "y": 6.0130216798691905, + "heading": 0.10706400495167072, + "angularVelocity": -0.25992408216784824, + "velocityX": 1.801585066442124, + "velocityY": 0.21164893886513167, + "timestamp": 6.820922547638963 + }, + { + "x": 4.427725657084912, + "y": 6.0307742700897, + "heading": 0.0884013396467056, + "angularVelocity": -0.2883893895674757, + "velocityX": 2.1494979094282636, + "velocityY": 0.27432623225435043, + "timestamp": 6.885635973236745 + }, + { + "x": 4.589379469563093, + "y": 6.0523813458045, + "heading": 0.06831284216496007, + "angularVelocity": -0.3104224092014938, + "velocityX": 2.497994983033644, + "velocityY": 0.3338886099014062, + "timestamp": 6.950349398834527 + }, + { + "x": 4.773638315970202, + "y": 6.077537273797697, + "heading": 0.04742981182100691, + "angularVelocity": -0.32270012213151367, + "velocityX": 2.847304785754114, + "velocityY": 0.3887281157012392, + "timestamp": 7.015062824432309 + }, + { + "x": 4.980578361520177, + "y": 6.105724638750597, + "heading": 0.026818564360069696, + "angularVelocity": -0.31850033081301815, + "velocityX": 3.197791550028341, + "velocityY": 0.43557213503258413, + "timestamp": 7.079776250030092 + }, + { + "x": 5.210305960828753, + "y": 6.1358813908951575, + "heading": 0.008654103339561452, + "angularVelocity": -0.28069076629334544, + "velocityX": 3.549921784954145, + "velocityY": 0.4660045711687753, + "timestamp": 7.144489675627874 + }, + { + "x": 5.462718064244522, + "y": 6.164655483095359, + "heading": 8.629838781735336e-9, + "angularVelocity": -0.13372951021808044, + "velocityX": 3.900459619996067, + "velocityY": 0.44463868099074777, + "timestamp": 7.209203101225656 + }, + { + "x": 5.7198262214660645, + "y": 6.171116352081299, + "heading": 0, + "angularVelocity": -1.3335468957212507e-7, + "velocityX": 3.9730265373303566, + "velocityY": 0.09983815454439177, + "timestamp": 7.273916526823438 + }, + { + "x": 5.995970674250576, + "y": 6.151565167078146, + "heading": -2.5127315305597956e-15, + "angularVelocity": -3.607302360522835e-14, + "velocityX": 3.964357212211523, + "velocityY": -0.28067875524252794, + "timestamp": 7.343573330689117 + }, + { + "x": 6.26897851241587, + "y": 6.105687844731353, + "heading": -2.5097428721994853e-15, + "angularVelocity": 4.2905476485829695e-17, + "velocityX": 3.9193276609666734, + "velocityY": -0.6586193996970835, + "timestamp": 7.413230134554796 + }, + { + "x": 6.536346012712411, + "y": 6.0339060327816165, + "heading": -2.5126951036173778e-15, + "angularVelocity": -4.2382527679423133e-17, + "velocityX": 3.838354409887958, + "velocityY": -1.0305068272755022, + "timestamp": 7.482886938420475 + }, + { + "x": 6.787193967022678, + "y": 5.960199932024534, + "heading": -2.073411284199746e-15, + "angularVelocity": 6.306402166664137e-15, + "velocityX": 3.6011981657092096, + "velocityY": -1.058132108663187, + "timestamp": 7.552543742286154 + }, + { + "x": 7.012681141047439, + "y": 5.8942233269936395, + "heading": -1.671095384485508e-15, + "angularVelocity": 5.7756870204886894e-15, + "velocityX": 3.2371162831352196, + "velocityY": -0.9471666997261698, + "timestamp": 7.622200546151833 + }, + { + "x": 7.212807520844226, + "y": 5.835976237454582, + "heading": -1.3121610031518989e-15, + "angularVelocity": 5.1528976439448214e-15, + "velocityX": 2.8730342004018077, + "velocityY": -0.8362010070315249, + "timestamp": 7.6918573500175125 + }, + { + "x": 7.387573101757002, + "y": 5.785458669967885, + "heading": -9.95057743236105e-16, + "angularVelocity": 4.552365916666039e-15, + "velocityX": 2.508952050825841, + "velocityY": -0.7252352201533442, + "timestamp": 7.7615141538831915 + }, + { + "x": 7.536977881454969, + "y": 5.7426706278046975, + "heading": -7.196116224916786e-16, + "angularVelocity": 3.954331894676066e-15, + "velocityX": 2.144869867788704, + "velocityY": -0.6142693863142378, + "timestamp": 7.831170957748871 + }, + { + "x": 7.66102185853843, + "y": 5.707612112923722, + "heading": -4.889855482801724e-16, + "angularVelocity": 3.3108908490429318e-15, + "velocityX": 1.7807876646574228, + "velocityY": -0.5033035243557882, + "timestamp": 7.90082776161455 + }, + { + "x": 7.759705032073642, + "y": 5.680283126628732, + "heading": -3.021004962604421e-16, + "angularVelocity": 2.6829403813236554e-15, + "velocityX": 1.4167054481211605, + "velocityY": -0.3923376436802606, + "timestamp": 7.970484565480229 + }, + { + "x": 7.833027401393294, + "y": 5.660683669849869, + "heading": -1.6049621497454911e-16, + "angularVelocity": 2.0328851408532914e-15, + "velocityX": 1.0526232220049963, + "velocityY": -0.281371749651518, + "timestamp": 8.040141369345909 + }, + { + "x": 7.880988965996717, + "y": 5.648813743284097, + "heading": -5.830363906716236e-17, + "angularVelocity": 1.467086785481853e-15, + "velocityX": 0.6885409887010987, + "velocityY": -0.17040584561707373, + "timestamp": 8.109798173211589 + }, + { + "x": 7.903589725494385, + "y": 5.6446733474731445, + "heading": 2.0598914784739755e-32, + "angularVelocity": 8.370128572804695e-16, + "velocityX": 0.3244587498051791, + "velocityY": -0.05943993380554415, + "timestamp": 8.179454977077269 + }, + { + "x": 7.9005397389522525, + "y": 5.6483789255306105, + "heading": 2.097139947940835e-17, + "angularVelocity": 2.980227828531976e-16, + "velocityX": -0.04334309998236763, + "velocityY": 0.05265965538925957, + "timestamp": 8.249823421958254 + }, + { + "x": 7.871608103494799, + "y": 5.6599727605367445, + "heading": -9.084634582304173e-18, + "angularVelocity": -4.2712371472453026e-16, + "velocityX": -0.41114501686552, + "velocityY": 0.16475900563136703, + "timestamp": 8.32019186683924 + }, + { + "x": 7.816794813351453, + "y": 5.6794548319403075, + "heading": -7.520085122829124e-17, + "angularVelocity": -9.395719591601255e-16, + "velocityX": -0.7789470157537616, + "velocityY": 0.27685806382157147, + "timestamp": 8.390560311720225 + }, + { + "x": 7.736099861309118, + "y": 5.706825114052552, + "heading": -1.8616176975942832e-16, + "angularVelocity": -1.5768562134134974e-15, + "velocityX": -1.1467491171467201, + "velocityY": 0.388956756951142, + "timestamp": 8.460928756601211 + }, + { + "x": 7.629523238094034, + "y": 5.742083573845685, + "heading": -3.3813406750936066e-16, + "angularVelocity": -2.1596654705354767e-15, + "velocityX": -1.5145513503282884, + "velocityY": 0.5010549807255615, + "timestamp": 8.531297201482197 + }, + { + "x": 7.497064931341595, + "y": 5.785230167283751, + "heading": -5.316654292279505e-16, + "angularVelocity": -2.750257804825092e-15, + "velocityX": -1.8823537592222348, + "velocityY": 0.6131525787087516, + "timestamp": 8.601665646363182 + }, + { + "x": 7.338724923742105, + "y": 5.836264832718443, + "heading": -7.669822112767176e-16, + "angularVelocity": -3.3440668612043406e-15, + "velocityX": -2.2501564141027943, + "velocityY": 0.7252493006130729, + "timestamp": 8.672034091244168 + }, + { + "x": 7.154503189332537, + "y": 5.895187477681304, + "heading": -1.0519707319411395e-15, + "angularVelocity": -4.0499477067678845e-15, + "velocityX": -2.617959437941741, + "velocityY": 0.8373447084559508, + "timestamp": 8.742402536125153 + }, + { + "x": 6.944399684844722, + "y": 5.961997948067273, + "heading": -1.3745422496318406e-15, + "angularVelocity": -4.5840364940621565e-15, + "velocityX": -2.985763076660981, + "velocityY": 0.9494379263248582, + "timestamp": 8.812770981006139 + }, + { + "x": 6.708414323753371, + "y": 6.036695935692792, + "heading": -1.7429472421431418e-15, + "angularVelocity": -5.235372077575223e-15, + "velocityX": -3.3535679449837517, + "velocityY": 1.061526764623348, + "timestamp": 8.883139425887125 + }, + { + "x": 6.4465468465502065, + "y": 6.119280516151445, + "heading": -2.1550026112189195e-15, + "angularVelocity": -5.855683878769628e-15, + "velocityX": -3.7213765011567173, + "velocityY": 1.1736024662605187, + "timestamp": 8.95350787076811 + }, + { + "x": 6.173126442867114, + "y": 6.178044585557323, + "heading": -2.191046659518503e-15, + "angularVelocity": -5.122189123349113e-16, + "velocityX": -3.8855541591979326, + "velocityY": 0.8350912046286955, + "timestamp": 9.023876315649096 + }, + { + "x": 5.8953070640563965, + "y": 6.21011209487915, + "heading": 0, + "angularVelocity": 3.113677831051976e-14, + "velocityX": -3.9480676215152357, + "velocityY": 0.45570865426474233, + "timestamp": 9.094244760530081 + }, + { + "x": 5.637913658685597, + "y": 6.216758064974601, + "heading": 9.67763523798797e-9, + "angularVelocity": 1.4937766527408904e-7, + "velocityX": -3.972956719905001, + "velocityY": 0.10258285959556052, + "timestamp": 9.159031120794502 + }, + { + "x": 5.380949926914102, + "y": 6.200473867453901, + "heading": 1.892686165769219e-8, + "angularVelocity": 1.4276502617102237e-7, + "velocityX": -3.966324558486143, + "velocityY": -0.2513522514767974, + "timestamp": 9.223817481058923 + }, + { + "x": 5.132255360579735, + "y": 6.163185174806636, + "heading": 0.01632330814348079, + "angularVelocity": 0.25195564544398485, + "velocityX": -3.8386871143710315, + "velocityY": -0.5755639381137804, + "timestamp": 9.288603841323344 + }, + { + "x": 4.905757766533857, + "y": 6.129190200240601, + "heading": 0.04988895608633859, + "angularVelocity": 0.5180974483811885, + "velocityX": -3.4960691281513077, + "velocityY": -0.5247242541278667, + "timestamp": 9.353390201587764 + }, + { + "x": 4.701928110746201, + "y": 6.098578355375242, + "heading": 0.08643838460715948, + "angularVelocity": 0.5641531392054652, + "velocityX": -3.146181618409506, + "velocityY": -0.4725044707141436, + "timestamp": 9.418176561852185 + }, + { + "x": 4.520779220798307, + "y": 6.071364064096333, + "heading": 0.12231563080100817, + "angularVelocity": 0.5537777712384655, + "velocityX": -2.7960961104853954, + "velocityY": -0.4200620496107946, + "timestamp": 9.482962922116606 + }, + { + "x": 4.362300886297595, + "y": 6.047552197658039, + "heading": 0.15578751714707642, + "angularVelocity": 0.5166502055290209, + "velocityX": -2.4461682035211605, + "velocityY": -0.3675444390069978, + "timestamp": 9.547749282381027 + }, + { + "x": 4.226482344491556, + "y": 6.027144059810235, + "heading": 0.1858309320626014, + "angularVelocity": 0.4637305567548124, + "velocityX": -2.096406423384503, + "velocityY": -0.3150067045713868, + "timestamp": 9.612535642645447 + }, + { + "x": 4.113314607743797, + "y": 6.010139422555146, + "heading": 0.21176520360476983, + "angularVelocity": 0.40030449984110905, + "velocityX": -1.7467833705408997, + "velocityY": -0.26247248936131745, + "timestamp": 9.677322002909868 + }, + { + "x": 4.022790393937899, + "y": 5.996537426116962, + "heading": 0.23310242597379685, + "angularVelocity": 0.3293474472395599, + "velocityX": -1.3972727196965211, + "velocityY": -0.20995154509025238, + "timestamp": 9.742108363174289 + }, + { + "x": 3.954903777130295, + "y": 5.986337041905185, + "heading": 0.2494756207657714, + "angularVelocity": 0.2527259553577121, + "velocityX": -1.0478535378517617, + "velocityY": -0.1574464774715696, + "timestamp": 9.80689472343871 + }, + { + "x": 3.9096498799999146, + "y": 5.97953733708754, + "heading": 0.260600273972009, + "angularVelocity": 0.17171289081270033, + "velocityX": -0.6985096391537948, + "velocityY": -0.10495580844363857, + "timestamp": 9.87168108370313 + }, + { + "x": 3.8870246410369873, + "y": 5.976137638092041, + "heading": 0.26625190805951465, + "angularVelocity": 0.08723493748434215, + "velocityX": -0.3492284312714647, + "velocityY": -0.052475536233735576, + "timestamp": 9.936467443967551 + }, + { + "x": 3.8870246410369873, + "y": 5.976137638092041, + "heading": 0.26625190805951465, + "angularVelocity": 3.6116165249279265e-32, + "velocityX": -3.053502496463557e-33, + "velocityY": 4.523388089316994e-32, + "timestamp": 10.001253804231972 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "14c0e4e2-7c6c-412a-8ad7-0357eeb42788" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "8c0a6ae0-ea46-46c8-8131-8a97a5c22a3a" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + } +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Straight Line.traj b/src/main/deploy/choreo/Straight Line.traj new file mode 100644 index 0000000..018d4f9 --- /dev/null +++ b/src/main/deploy/choreo/Straight Line.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 9.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 9.80132051089383, + "y": 4.12, + "heading": 1.0196133040364384e-18, + "angularVelocity": 1.0324520604294053e-17, + "velocityX": 0.8234448167415525, + "velocityY": 3.02420635388715e-34, + "timestamp": 0.09875647917200309 + }, + { + "x": 9.963961529614206, + "y": 4.12, + "heading": 3.0591556010799972e-18, + "angularVelocity": 2.0652237848908187e-17, + "velocityX": 1.6468896024240143, + "velocityY": 2.277444821719999e-33, + "timestamp": 0.19751295834400617 + }, + { + "x": 10.207923050026555, + "y": 4.12, + "heading": 6.11931106017425e-18, + "angularVelocity": 3.0986882933133645e-17, + "velocityX": 2.4703343259883073, + "velocityY": 1.5382285718210452e-33, + "timestamp": 0.29626943751600926 + }, + { + "x": 10.533205053727176, + "y": 4.12, + "heading": 1.020162012561919e-17, + "angularVelocity": 4.133712642975716e-17, + "velocityX": 3.29377886319825, + "velocityY": 1.5324076974034493e-33, + "timestamp": 0.39502591668801235 + }, + { + "x": 10.906794946272825, + "y": 4.12, + "heading": -1.0162422335797283e-17, + "angularVelocity": -2.062046220374934e-16, + "velocityX": 3.7829405794729865, + "velocityY": -3.7320649171210315e-33, + "timestamp": 0.49378239586001543 + }, + { + "x": 11.232076949973447, + "y": 4.12, + "heading": -6.100334399460735e-18, + "angularVelocity": 4.11323689413084e-17, + "velocityX": 3.2937788631982494, + "velocityY": 1.28740322268366e-34, + "timestamp": 0.5925388750320185 + }, + { + "x": 11.476038470385795, + "y": 4.12, + "heading": -3.0508797817029e-18, + "angularVelocity": 3.0878527093782464e-17, + "velocityX": 2.470334325988307, + "velocityY": -6.604493200637239e-34, + "timestamp": 0.6912953542040217 + }, + { + "x": 11.63867948910617, + "y": 4.12, + "heading": -1.0171236371693236e-18, + "angularVelocity": 2.0593647744285298e-17, + "velocityX": 1.646889602424014, + "velocityY": -7.180489499042344e-34, + "timestamp": 0.7900518333760247 + }, + { + "x": 11.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 1.0299310442024172e-17, + "velocityX": 0.8234448167415525, + "velocityY": -6.686788666173353e-34, + "timestamp": 0.8888083125480277 + }, + { + "x": 11.72, + "y": 4.12, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 9.925887673927275e-44, + "timestamp": 0.9875647917200308 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Three Piece.traj b/src/main/deploy/choreo/Three Piece.traj new file mode 100644 index 0000000..888ff45 --- /dev/null +++ b/src/main/deploy/choreo/Three Piece.traj @@ -0,0 +1,1336 @@ +{ + "samples": [ + { + "x": 1.898240327835083, + "y": 4.045846462249756, + "heading": 0, + "angularVelocity": -2.8038956593328693e-37, + "velocityX": -1.823589012902056e-36, + "velocityY": -4.380800340261528e-36, + "timestamp": 0 + }, + { + "x": 1.907488249459704, + "y": 4.045792662710147, + "heading": -0.014531093178687978, + "angularVelocity": -0.3399428010717914, + "velocityX": 0.21634741051533174, + "velocityY": -0.0012585953421492776, + "timestamp": 0.042745700549838096 + }, + { + "x": 1.9260125772155603, + "y": 4.04570358761662, + "heading": -0.0432778322806298, + "angularVelocity": -0.672505976792342, + "velocityX": 0.4333611922971804, + "velocityY": -0.002083837494312978, + "timestamp": 0.08549140109967619 + }, + { + "x": 1.9538513835951645, + "y": 4.045605326780032, + "heading": -0.0858077691342503, + "angularVelocity": -0.9949523883468459, + "velocityX": 0.6512656482760499, + "velocityY": -0.002298730289234273, + "timestamp": 0.1282371016495143 + }, + { + "x": 1.9910562559823362, + "y": 4.045539296716268, + "heading": -0.1415186846109791, + "angularVelocity": -1.3033103858427657, + "velocityX": 0.8703769480580534, + "velocityY": -0.0015447182503467369, + "timestamp": 0.17098280219935238 + }, + { + "x": 2.0376949895847924, + "y": 4.045574246714428, + "heading": -0.20959549088699864, + "angularVelocity": -1.5926000837592402, + "velocityX": 1.0910742601604893, + "velocityY": 0.0008176260468236386, + "timestamp": 0.21372850274919047 + }, + { + "x": 2.0938509724389283, + "y": 4.0458223742350645, + "heading": -0.28899731848757143, + "angularVelocity": -1.8575395087512154, + "velocityX": 1.3137223658005655, + "velocityY": 0.005804736323069604, + "timestamp": 0.2564742032990286 + }, + { + "x": 2.159619041381372, + "y": 4.046458363697538, + "heading": -0.37845923210523325, + "angularVelocity": -2.0928868276086927, + "velocityX": 1.538589100107577, + "velocityY": 0.014878442844382068, + "timestamp": 0.2992199038488667 + }, + { + "x": 2.2351062437840015, + "y": 4.047751445000734, + "heading": -0.4762822950411069, + "angularVelocity": -2.288488939883434, + "velocityX": 1.7659601183660054, + "velocityY": 0.030250558221350754, + "timestamp": 0.3419656043987048 + }, + { + "x": 2.3204435066348985, + "y": 4.050239499458989, + "heading": -0.5785043698305009, + "angularVelocity": -2.391400151933671, + "velocityX": 1.99639406427321, + "velocityY": 0.058205958172433236, + "timestamp": 0.38471130494854294 + }, + { + "x": 2.4129687747508384, + "y": 4.057068741649624, + "heading": -0.6558501426840586, + "angularVelocity": -1.8094398233894569, + "velocityX": 2.1645514502227607, + "velocityY": 0.15976442315343106, + "timestamp": 0.42745700549838106 + }, + { + "x": 2.5026748180389404, + "y": 4.065344333648682, + "heading": -0.6823166411894661, + "angularVelocity": -0.6191616505278632, + "velocityX": 2.0985980375619784, + "velocityY": 0.19360057017687107, + "timestamp": 0.4702027060482192 + }, + { + "x": 2.5850596627844538, + "y": 4.076067191512528, + "heading": -0.6785329802999686, + "angularVelocity": 0.08937347775881672, + "velocityX": 1.946004228329371, + "velocityY": 0.25328355970417904, + "timestamp": 0.5125380944025599 + }, + { + "x": 2.659003068905951, + "y": 4.0907479122594275, + "heading": -0.6634760532676318, + "angularVelocity": 0.3556581767081624, + "velocityX": 1.7466098457064405, + "velocityY": 0.3467718454363507, + "timestamp": 0.5548734827569006 + }, + { + "x": 2.7246369617363335, + "y": 4.110317723995373, + "heading": -0.642205314944677, + "angularVelocity": 0.5024339955245456, + "velocityX": 1.5503316582580267, + "velocityY": 0.4622565776921573, + "timestamp": 0.5972088711112413 + }, + { + "x": 2.7821274656068904, + "y": 4.1352087632427805, + "heading": -0.6169095715353604, + "angularVelocity": 0.5975082405668554, + "velocityX": 1.357977477125514, + "velocityY": 0.5879487637877254, + "timestamp": 0.639544259465582 + }, + { + "x": 2.831597012829208, + "y": 4.165661838985507, + "heading": -0.5887878540219638, + "angularVelocity": 0.6642602939654689, + "velocityX": 1.1685152574547029, + "velocityY": 0.7193290749535347, + "timestamp": 0.6818796478199227 + }, + { + "x": 2.8731348514556885, + "y": 4.201829433441162, + "heading": -0.5585989718626111, + "angularVelocity": 0.7130885845826266, + "velocityX": 0.9811611571580527, + "velocityY": 0.8543111534241201, + "timestamp": 0.7242150361742634 + }, + { + "x": 2.9188396470921765, + "y": 4.2725572090728265, + "heading": -0.5076501226687711, + "angularVelocity": 0.770144031880298, + "velocityX": 0.6908747919669516, + "velocityY": 1.069122760430878, + "timestamp": 0.7903699982135706 + }, + { + "x": 2.9452297152900053, + "y": 4.357347002147122, + "heading": -0.4530144734209342, + "angularVelocity": 0.8258737903193734, + "velocityX": 0.398912906671289, + "velocityY": 1.2816845548776343, + "timestamp": 0.8565249602528777 + }, + { + "x": 2.9521484208350985, + "y": 4.455984216280158, + "heading": -0.39482288845823243, + "angularVelocity": 0.8796254002553318, + "velocityX": 0.10458331970597358, + "velocityY": 1.4910025052153826, + "timestamp": 0.9226799222921849 + }, + { + "x": 2.9393582563456846, + "y": 4.568133489859713, + "heading": -0.3332972563741976, + "angularVelocity": 0.9300229368656926, + "velocityX": -0.19333643456426472, + "velocityY": 1.695251121343236, + "timestamp": 0.988834884331492 + }, + { + "x": 2.906458551093886, + "y": 4.693199281947042, + "heading": -0.26886974643996187, + "angularVelocity": 0.9738877923617428, + "velocityX": -0.4973127372101132, + "velocityY": 1.8904975262931591, + "timestamp": 1.0549898463707992 + }, + { + "x": 2.8526414862163594, + "y": 4.829843806089774, + "heading": -0.2025647523319625, + "angularVelocity": 1.0022678883649443, + "velocityX": -0.81350004925632, + "velocityY": 2.065521918998951, + "timestamp": 1.1211448084101063 + }, + { + "x": 2.775736178341105, + "y": 4.97269513900139, + "heading": -0.13812989460617203, + "angularVelocity": 0.9739988617559082, + "velocityX": -1.1625024866548819, + "velocityY": 2.1593441898846235, + "timestamp": 1.1872997704494135 + }, + { + "x": 2.702646192253521, + "y": 5.09239426390518, + "heading": -0.0945099100170612, + "angularVelocity": 0.6593607379472648, + "velocityX": -1.1048299905932448, + "velocityY": 1.809374855852369, + "timestamp": 1.2534547324887206 + }, + { + "x": 2.6443183089698836, + "y": 5.193435173263213, + "heading": -0.060805157515131665, + "angularVelocity": 0.5094818508383886, + "velocityX": -0.8816856889583129, + "velocityY": 1.5273368201465674, + "timestamp": 1.3196096945280278 + }, + { + "x": 2.602443138845534, + "y": 5.277234789353118, + "heading": -0.03510328646046884, + "angularVelocity": 0.3885101020750583, + "velocityX": -0.63298608046164, + "velocityY": 1.2667170157261087, + "timestamp": 1.385764656567335 + }, + { + "x": 2.577670236134593, + "y": 5.344417005113783, + "heading": -0.016652156501204083, + "angularVelocity": 0.2789077249912364, + "velocityX": -0.3744677942105343, + "velocityY": 1.015527992000793, + "timestamp": 1.451919618606642 + }, + { + "x": 2.5703412148254166, + "y": 5.395330206172067, + "heading": -0.005041861251989399, + "angularVelocity": 0.17550150270385198, + "velocityX": -0.1107856626812363, + "velocityY": 0.7696051738043858, + "timestamp": 1.5180745806459492 + }, + { + "x": 2.5806665420532227, + "y": 5.430196285247803, + "heading": 4.946919732174461e-30, + "angularVelocity": 0.07621289615423993, + "velocityX": 0.15607789513463036, + "velocityY": 0.5270364913068623, + "timestamp": 1.5842295426852564 + }, + { + "x": 2.615476810555517, + "y": 5.449290678492884, + "heading": -0.00249481878254215, + "angularVelocity": -0.03313174320347507, + "velocityX": 0.4622880366833039, + "velocityY": 0.25357775003504285, + "timestamp": 1.659529497994595 + }, + { + "x": 2.6735006636921637, + "y": 5.447944678639869, + "heading": -0.01213678704864337, + "angularVelocity": -0.12804746332838068, + "velocityX": 0.7705695560944235, + "velocityY": -0.017875174659603758, + "timestamp": 1.7348294533039335 + }, + { + "x": 2.7555584259794457, + "y": 5.427049168858778, + "heading": -0.02296797038520493, + "angularVelocity": -0.14384050152574682, + "velocityX": 1.0897451658527806, + "velocityY": -0.27749697453671124, + "timestamp": 1.810129408613272 + }, + { + "x": 2.8158286951067315, + "y": 5.427810213575971, + "heading": -0.01758027986858031, + "angularVelocity": 0.07154971732043594, + "velocityX": 0.8004024554820866, + "velocityY": 0.010106841552119225, + "timestamp": 1.8854293639226105 + }, + { + "x": 2.8536369800567627, + "y": 5.4496941566467285, + "heading": 0, + "angularVelocity": 0.23346999073716668, + "velocityX": 0.5021023557678279, + "velocityY": 0.2906235864398052, + "timestamp": 1.960729319231949 + }, + { + "x": 2.8693148468887744, + "y": 5.487156976773219, + "heading": 0.025561040529235773, + "angularVelocity": 0.3719476521458912, + "velocityX": 0.22813413061776552, + "velocityY": 0.5451346150355147, + "timestamp": 2.0294514643557737 + }, + { + "x": 2.8662677650419504, + "y": 5.542220250273633, + "heading": 0.0607369491230168, + "angularVelocity": 0.5118569644530235, + "velocityX": -0.04433915503268782, + "velocityY": 0.8012449757090656, + "timestamp": 2.0981736094795984 + }, + { + "x": 2.844654009258225, + "y": 5.615050469126244, + "heading": 0.10566640088464291, + "angularVelocity": 0.6537841867519056, + "velocityX": -0.3145093294858733, + "velocityY": 1.0597780194634927, + "timestamp": 2.166895754603423 + }, + { + "x": 2.804748272410547, + "y": 5.705930381335797, + "heading": 0.16050762468177382, + "angularVelocity": 0.7980138527154114, + "velocityX": -0.5806823517481201, + "velocityY": 1.3224254284525792, + "timestamp": 2.235617899727248 + }, + { + "x": 2.7471447891895058, + "y": 5.815446220375669, + "heading": 0.22535865974809183, + "angularVelocity": 0.9436701219012945, + "velocityX": -0.8382084569282627, + "velocityY": 1.5936033260100555, + "timestamp": 2.3043400448510725 + }, + { + "x": 2.6740678731800593, + "y": 5.945504225324297, + "heading": 0.2996496097363406, + "angularVelocity": 1.0810336297621417, + "velocityX": -1.0633677961852832, + "velocityY": 1.892519575957448, + "timestamp": 2.373062189974897 + }, + { + "x": 2.6265491520645683, + "y": 6.0733583679407905, + "heading": 0.3634596160425804, + "angularVelocity": 0.9285217478480327, + "velocityX": -0.6914615518748894, + "velocityY": 1.8604504033761498, + "timestamp": 2.441784335098722 + }, + { + "x": 2.600377734078398, + "y": 6.186973463102748, + "heading": 0.4156012346676629, + "angularVelocity": 0.7587309524627455, + "velocityX": -0.38082946827422787, + "velocityY": 1.6532530373905598, + "timestamp": 2.5105064802225465 + }, + { + "x": 2.59468135033937, + "y": 6.285113460277094, + "heading": 0.45604131218202076, + "angularVelocity": 0.5884577299135867, + "velocityX": -0.08289007464427144, + "velocityY": 1.4280694672367455, + "timestamp": 2.579228625346371 + }, + { + "x": 2.6091090827604106, + "y": 6.367321455091837, + "heading": 0.4847594324399464, + "angularVelocity": 0.41788742487855773, + "velocityX": 0.20994298701015016, + "velocityY": 1.1962373215594329, + "timestamp": 2.647950770470196 + }, + { + "x": 2.6434723912297375, + "y": 6.433359811846902, + "heading": 0.5017638856587859, + "angularVelocity": 0.24743775369934545, + "velocityX": 0.5000325354717966, + "velocityY": 0.9609472555909916, + "timestamp": 2.7166729155940206 + }, + { + "x": 2.6976537704467773, + "y": 6.483082294464111, + "heading": 0.5070982159979445, + "angularVelocity": 0.07762170883267666, + "velocityX": 0.7884122231547788, + "velocityY": 0.7235292572375208, + "timestamp": 2.7853950607178453 + }, + { + "x": 2.738806835827011, + "y": 6.507590262026266, + "heading": 0.5059467865388958, + "angularVelocity": -0.027038737137375757, + "velocityX": 0.966387396526008, + "velocityY": 0.5755146244320987, + "timestamp": 2.8279794999687833 + }, + { + "x": 2.7880294623095803, + "y": 6.526430774501013, + "heading": 0.5005508163010642, + "angularVelocity": -0.12671225294372634, + "velocityX": 1.155882931615343, + "velocityY": 0.44242715898465257, + "timestamp": 2.8705639392197213 + }, + { + "x": 2.84591135896884, + "y": 6.540541693646972, + "heading": 0.4912680248506681, + "angularVelocity": -0.21798552742928792, + "velocityX": 1.35922646105959, + "velocityY": 0.3313632724575067, + "timestamp": 2.9131483784706593 + }, + { + "x": 2.913103056068397, + "y": 6.55135708254599, + "heading": 0.4786790444220719, + "angularVelocity": -0.2956239567794471, + "velocityX": 1.5778462340108705, + "velocityY": 0.25397513949368183, + "timestamp": 2.9557328177215973 + }, + { + "x": 2.990122079849243, + "y": 6.5610737800598145, + "heading": 0.46364733726920737, + "angularVelocity": -0.35298591263083134, + "velocityX": 1.8086189494475877, + "velocityY": 0.22817483768114147, + "timestamp": 2.9983172569725354 + }, + { + "x": 3.1507680403022618, + "y": 6.583513336972122, + "heading": 0.4338303597620861, + "angularVelocity": -0.4082414173674588, + "velocityX": 2.199496396777096, + "velocityY": 0.3072329016846347, + "timestamp": 3.0713548671674924 + }, + { + "x": 3.339966410927534, + "y": 6.61173687205727, + "heading": 0.4005433233854809, + "angularVelocity": -0.4557519925385439, + "velocityX": 2.5904238942135573, + "velocityY": 0.38642467914559536, + "timestamp": 3.1443924773624494 + }, + { + "x": 3.5577217220068316, + "y": 6.645759083728854, + "heading": 0.36470254911055944, + "angularVelocity": -0.4907166893776054, + "velocityX": 2.981413418347747, + "velocityY": 0.46581770105525094, + "timestamp": 3.2174300875574064 + }, + { + "x": 3.8040369699341148, + "y": 6.685605934834726, + "heading": 0.32812163175231024, + "angularVelocity": -0.5008504147466596, + "velocityX": 3.372443967837953, + "velocityY": 0.5455661952726859, + "timestamp": 3.2904676977523635 + }, + { + "x": 4.078865987183254, + "y": 6.731338843022743, + "heading": 0.29648766626359246, + "angularVelocity": -0.43311884663638234, + "velocityX": 3.7628424111296486, + "velocityY": 0.6261555938911911, + "timestamp": 3.3635053079473205 + }, + { + "x": 4.363416873029506, + "y": 6.788685305113101, + "heading": 0.29648766061986404, + "angularVelocity": -7.727153775240832e-8, + "velocityX": 3.8959501151079365, + "velocityY": 0.7851634512312887, + "timestamp": 3.4365429181422775 + }, + { + "x": 4.647967686637335, + "y": 6.846032125646838, + "heading": 0.2964876549762135, + "angularVelocity": -7.72704712131681e-8, + "velocityX": 3.895949126050067, + "velocityY": 0.7851683588860835, + "timestamp": 3.5095805283372346 + }, + { + "x": 4.932518500243366, + "y": 6.903378946189493, + "heading": 0.2964876493325629, + "angularVelocity": -7.727047143608747e-8, + "velocityX": 3.895949126025459, + "velocityY": 0.7851683590081883, + "timestamp": 3.5826181385321916 + }, + { + "x": 5.217069313849397, + "y": 6.960725766732147, + "heading": 0.29648764368891234, + "angularVelocity": -7.727047107791159e-8, + "velocityX": 3.8959491260254584, + "velocityY": 0.7851683590081909, + "timestamp": 3.6556557487271486 + }, + { + "x": 5.501620127455428, + "y": 7.018072587274802, + "heading": 0.2964876380452618, + "angularVelocity": -7.72704716252991e-8, + "velocityX": 3.895949126025459, + "velocityY": 0.7851683590081912, + "timestamp": 3.7286933589221056 + }, + { + "x": 5.786170941061528, + "y": 7.075419407817124, + "heading": 0.29648763240161125, + "angularVelocity": -7.727047111704844e-8, + "velocityX": 3.8959491260263763, + "velocityY": 0.7851683590036359, + "timestamp": 3.8017309691170627 + }, + { + "x": 6.070721757362521, + "y": 7.132766214987568, + "heading": 0.29648762675796064, + "angularVelocity": -7.727047165435608e-8, + "velocityX": 3.895949162923739, + "velocityY": 0.7851681759215458, + "timestamp": 3.8747685793120197 + }, + { + "x": 6.355380338499842, + "y": 7.1895756818041425, + "heading": 0.29648762107984833, + "angularVelocity": -7.774230685415808e-8, + "velocityX": 3.8974246333839155, + "velocityY": 0.7778111395614767, + "timestamp": 3.9478061895069767 + }, + { + "x": 6.636405520312889, + "y": 7.217870773127444, + "heading": 0.27570831993349604, + "angularVelocity": -0.28450138347744536, + "velocityX": 3.847677669941766, + "velocityY": 0.387404396827479, + "timestamp": 4.020843799701934 + }, + { + "x": 6.88919116626837, + "y": 7.2399532386560255, + "heading": 0.24179746779298844, + "angularVelocity": -0.4642930135582247, + "velocityX": 3.4610339150025435, + "velocityY": 0.30234375782058953, + "timestamp": 4.093881409896891 + }, + { + "x": 7.113463572845222, + "y": 7.256040709772393, + "heading": 0.20495027392657358, + "angularVelocity": -0.5044961598286116, + "velocityX": 3.070642727468879, + "velocityY": 0.22026283545456557, + "timestamp": 4.166919020091848 + }, + { + "x": 7.3091937477745095, + "y": 7.2662136587336335, + "heading": 0.1681230341900902, + "angularVelocity": -0.5042229563396395, + "velocityX": 2.679854590078042, + "velocityY": 0.13928370512239335, + "timestamp": 4.239956630286805 + }, + { + "x": 7.476376305660358, + "y": 7.270512578107467, + "heading": 0.13274981372464448, + "angularVelocity": -0.4843151408024598, + "velocityX": 2.288992718129635, + "velocityY": 0.058858981863695155, + "timestamp": 4.312994240481762 + }, + { + "x": 7.615010848660238, + "y": 7.268961369646756, + "heading": 0.09968688668304959, + "angularVelocity": -0.45268358251784385, + "velocityX": 1.8981253990899605, + "velocityY": -0.021238488726158328, + "timestamp": 4.386031850676719 + }, + { + "x": 7.7250983383538445, + "y": 7.261575556193537, + "heading": 0.06950965194609489, + "angularVelocity": -0.4131739066544426, + "velocityX": 1.5072712455918797, + "velocityY": -0.10112342714260779, + "timestamp": 4.459069460871676 + }, + { + "x": 7.8066401035121915, + "y": 7.248365897189822, + "heading": 0.04263526512944047, + "angularVelocity": -0.36795271292310205, + "velocityX": 1.1164352850632593, + "velocityY": -0.18086105183966827, + "timestamp": 4.532107071066633 + }, + { + "x": 7.859637512937734, + "y": 7.2293402273974605, + "heading": 0.019381829491425508, + "angularVelocity": -0.3183761842144796, + "velocityX": 0.7256180655976813, + "velocityY": -0.2604914062984367, + "timestamp": 4.60514468126159 + }, + { + "x": 7.884091854095459, + "y": 7.204504489898682, + "heading": 0, + "angularVelocity": -0.26536779393096516, + "velocityX": 0.33481847355697797, + "velocityY": -0.3400403905944489, + "timestamp": 4.678182291456547 + }, + { + "x": 7.8693875341309285, + "y": 7.163825702906692, + "heading": -0.017989310395224636, + "angularVelocity": -0.1948160433685539, + "velocityX": -0.15924109223639094, + "velocityY": -0.44053274732062453, + "timestamp": 4.770522275803691 + }, + { + "x": 7.809062158016808, + "y": 7.113867015205141, + "heading": -0.02942623999470572, + "angularVelocity": -0.12385674180412554, + "velocityX": -0.6532963649564154, + "velocityY": -0.5410298480638316, + "timestamp": 4.862862260150834 + }, + { + "x": 7.703115992354989, + "y": 7.054628240082747, + "heading": -0.03428596694706339, + "angularVelocity": -0.05262863088743923, + "velocityX": -1.1473487504992854, + "velocityY": -0.641528970805246, + "timestamp": 4.955202244497977 + }, + { + "x": 7.551549165293792, + "y": 6.986109443661236, + "heading": -0.032557590593216855, + "angularVelocity": 0.018717529205428393, + "velocityX": -1.6413997482541873, + "velocityY": -0.7420273774784575, + "timestamp": 5.047542228845121 + }, + { + "x": 7.354361663520876, + "y": 6.908310952017223, + "heading": -0.024244154578006688, + "angularVelocity": 0.09003072801005149, + "velocityX": -2.1354508901756786, + "velocityY": -0.8425222528903508, + "timestamp": 5.139882213192264 + }, + { + "x": 7.111553334877506, + "y": 6.8212333658210484, + "heading": -0.00936157178577619, + "angularVelocity": 0.16117159752031956, + "velocityX": -2.6295036799070197, + "velocityY": -0.9430106233158517, + "timestamp": 5.232222197539407 + }, + { + "x": 6.823123896075064, + "y": 6.724877589969487, + "heading": 0.012063558679812484, + "angularVelocity": 0.23202441084506603, + "velocityX": -3.1235595375251535, + "velocityY": -1.0434891941211963, + "timestamp": 5.3245621818865505 + }, + { + "x": 6.48907294008907, + "y": 6.619244934399674, + "heading": 0.039997176732724696, + "angularVelocity": 0.3025083689412195, + "velocityX": -3.6176198030331097, + "velocityY": -1.1439535789035715, + "timestamp": 5.416902166233694 + }, + { + "x": 6.129437398233647, + "y": 6.5461675567988795, + "heading": 0.03999718077634422, + "angularVelocity": 4.3790558964884806e-8, + "velocityX": -3.8946892226384597, + "velocityY": -0.7913947367163172, + "timestamp": 5.509242150580837 + }, + { + "x": 5.769801333833775, + "y": 6.4730927508413325, + "heading": 0.03999718481982793, + "angularVelocity": 4.378908809561466e-8, + "velocityX": -3.8946948815570095, + "velocityY": -0.7913668869904851, + "timestamp": 5.60158213492798 + }, + { + "x": 5.410165307610781, + "y": 6.400017756997467, + "heading": 0.03999718886333287, + "angularVelocity": 4.378931804611701e-8, + "velocityX": -3.8946944681187916, + "velocityY": -0.7913689217137886, + "timestamp": 5.693922119275124 + }, + { + "x": 5.06488865586912, + "y": 6.3170886108245945, + "heading": 0.07172411836277942, + "angularVelocity": 0.3435882053019635, + "velocityX": -3.739188978456266, + "velocityY": -0.8980849061129225, + "timestamp": 5.786262103622267 + }, + { + "x": 4.765275429088764, + "y": 6.243389106592463, + "heading": 0.10120262922920165, + "angularVelocity": 0.3192388549211855, + "velocityX": -3.244674870790441, + "velocityY": -0.7981320849596799, + "timestamp": 5.87860208796941 + }, + { + "x": 4.511309122993011, + "y": 6.178941794592798, + "heading": 0.12636460911438926, + "angularVelocity": 0.27249278915397723, + "velocityX": -2.7503394969289876, + "velocityY": -0.6979350544113176, + "timestamp": 5.970942072316554 + }, + { + "x": 4.302982802001526, + "y": 6.123753419378413, + "heading": 0.14653342260027327, + "angularVelocity": 0.21841906979387646, + "velocityX": -2.256079232245732, + "velocityY": -0.5976649834259027, + "timestamp": 6.063282056663697 + }, + { + "x": 4.140292768892181, + "y": 6.077827039965534, + "heading": 0.16137256146424936, + "angularVelocity": 0.16070111955174202, + "velocityX": -1.7618590067952369, + "velocityY": -0.497361784687127, + "timestamp": 6.15562204101084 + }, + { + "x": 4.023236723432337, + "y": 6.041164401680778, + "heading": 0.17067956492366876, + "angularVelocity": 0.10079061118779088, + "velocityX": -1.2676636918172275, + "velocityY": -0.3970396848555328, + "timestamp": 6.2479620253579835 + }, + { + "x": 3.9518130737172363, + "y": 6.01376672295597, + "heading": 0.174318405736917, + "angularVelocity": 0.03940698971280611, + "velocityX": -0.7734856164431515, + "velocityY": -0.29670438996183723, + "timestamp": 6.340302009705127 + }, + { + "x": 3.926020622253418, + "y": 5.995635032653809, + "heading": 0.17219063503059454, + "angularVelocity": -0.023042788250028232, + "velocityX": -0.2793205093781995, + "velocityY": -0.19635795295345568, + "timestamp": 6.43264199405227 + }, + { + "x": 3.930355880820906, + "y": 5.98747940429267, + "heading": 0.1678304420080604, + "angularVelocity": -0.0673769466267154, + "velocityX": 0.06699164087576895, + "velocityY": -0.12602683733396786, + "timestamp": 6.497355419650052 + }, + { + "x": 3.957112124799613, + "y": 5.9838306427917995, + "heading": 0.16069467758998768, + "angularVelocity": -0.11026714089319978, + "velocityX": 0.41345738896603046, + "velocityY": -0.056383377439255714, + "timestamp": 6.562068845247834 + }, + { + "x": 4.006301141076061, + "y": 5.984635223325004, + "heading": 0.15089514245932092, + "angularVelocity": -0.15142970782560608, + "velocityX": 0.760105276796434, + "velocityY": 0.012432977017907793, + "timestamp": 6.6267822708456166 + }, + { + "x": 4.0779371329258005, + "y": 5.989827517197893, + "heading": 0.13856825674408835, + "angularVelocity": -0.1904842094413026, + "velocityX": 1.106972644208068, + "velocityY": 0.08023518806075768, + "timestamp": 6.691495696443399 + }, + { + "x": 4.17203754289526, + "y": 5.999325152011095, + "heading": 0.12388458270411208, + "angularVelocity": -0.22690305611760084, + "velocityX": 1.4541095468246137, + "velocityY": 0.14676451950228486, + "timestamp": 6.756209122041181 + }, + { + "x": 4.288624284050538, + "y": 6.0130216798691905, + "heading": 0.10706400495167072, + "angularVelocity": -0.25992408216784824, + "velocityX": 1.801585066442124, + "velocityY": 0.21164893886513167, + "timestamp": 6.820922547638963 + }, + { + "x": 4.427725657084912, + "y": 6.0307742700897, + "heading": 0.0884013396467056, + "angularVelocity": -0.2883893895674757, + "velocityX": 2.1494979094282636, + "velocityY": 0.27432623225435043, + "timestamp": 6.885635973236745 + }, + { + "x": 4.589379469563093, + "y": 6.0523813458045, + "heading": 0.06831284216496007, + "angularVelocity": -0.3104224092014938, + "velocityX": 2.497994983033644, + "velocityY": 0.3338886099014062, + "timestamp": 6.950349398834527 + }, + { + "x": 4.773638315970202, + "y": 6.077537273797697, + "heading": 0.04742981182100691, + "angularVelocity": -0.32270012213151367, + "velocityX": 2.847304785754114, + "velocityY": 0.3887281157012392, + "timestamp": 7.015062824432309 + }, + { + "x": 4.980578361520177, + "y": 6.105724638750597, + "heading": 0.026818564360069696, + "angularVelocity": -0.31850033081301815, + "velocityX": 3.197791550028341, + "velocityY": 0.43557213503258413, + "timestamp": 7.079776250030092 + }, + { + "x": 5.210305960828753, + "y": 6.1358813908951575, + "heading": 0.008654103339561452, + "angularVelocity": -0.28069076629334544, + "velocityX": 3.549921784954145, + "velocityY": 0.4660045711687753, + "timestamp": 7.144489675627874 + }, + { + "x": 5.462718064244522, + "y": 6.164655483095359, + "heading": 8.629838781735336e-9, + "angularVelocity": -0.13372951021808044, + "velocityX": 3.900459619996067, + "velocityY": 0.44463868099074777, + "timestamp": 7.209203101225656 + }, + { + "x": 5.7198262214660645, + "y": 6.171116352081299, + "heading": 0, + "angularVelocity": -1.3335468957212507e-7, + "velocityX": 3.9730265373303566, + "velocityY": 0.09983815454439177, + "timestamp": 7.273916526823438 + }, + { + "x": 5.995970674250576, + "y": 6.151565167078146, + "heading": -2.5127315305597956e-15, + "angularVelocity": -3.607302360522835e-14, + "velocityX": 3.964357212211523, + "velocityY": -0.28067875524252794, + "timestamp": 7.343573330689117 + }, + { + "x": 6.26897851241587, + "y": 6.105687844731353, + "heading": -2.5097428721994853e-15, + "angularVelocity": 4.2905476485829695e-17, + "velocityX": 3.9193276609666734, + "velocityY": -0.6586193996970835, + "timestamp": 7.413230134554796 + }, + { + "x": 6.536346012712411, + "y": 6.0339060327816165, + "heading": -2.5126951036173778e-15, + "angularVelocity": -4.2382527679423133e-17, + "velocityX": 3.838354409887958, + "velocityY": -1.0305068272755022, + "timestamp": 7.482886938420475 + }, + { + "x": 6.787193967022678, + "y": 5.960199932024534, + "heading": -2.073411284199746e-15, + "angularVelocity": 6.306402166664137e-15, + "velocityX": 3.6011981657092096, + "velocityY": -1.058132108663187, + "timestamp": 7.552543742286154 + }, + { + "x": 7.012681141047439, + "y": 5.8942233269936395, + "heading": -1.671095384485508e-15, + "angularVelocity": 5.7756870204886894e-15, + "velocityX": 3.2371162831352196, + "velocityY": -0.9471666997261698, + "timestamp": 7.622200546151833 + }, + { + "x": 7.212807520844226, + "y": 5.835976237454582, + "heading": -1.3121610031518989e-15, + "angularVelocity": 5.1528976439448214e-15, + "velocityX": 2.8730342004018077, + "velocityY": -0.8362010070315249, + "timestamp": 7.6918573500175125 + }, + { + "x": 7.387573101757002, + "y": 5.785458669967885, + "heading": -9.95057743236105e-16, + "angularVelocity": 4.552365916666039e-15, + "velocityX": 2.508952050825841, + "velocityY": -0.7252352201533442, + "timestamp": 7.7615141538831915 + }, + { + "x": 7.536977881454969, + "y": 5.7426706278046975, + "heading": -7.196116224916786e-16, + "angularVelocity": 3.954331894676066e-15, + "velocityX": 2.144869867788704, + "velocityY": -0.6142693863142378, + "timestamp": 7.831170957748871 + }, + { + "x": 7.66102185853843, + "y": 5.707612112923722, + "heading": -4.889855482801724e-16, + "angularVelocity": 3.3108908490429318e-15, + "velocityX": 1.7807876646574228, + "velocityY": -0.5033035243557882, + "timestamp": 7.90082776161455 + }, + { + "x": 7.759705032073642, + "y": 5.680283126628732, + "heading": -3.021004962604421e-16, + "angularVelocity": 2.6829403813236554e-15, + "velocityX": 1.4167054481211605, + "velocityY": -0.3923376436802606, + "timestamp": 7.970484565480229 + }, + { + "x": 7.833027401393294, + "y": 5.660683669849869, + "heading": -1.6049621497454911e-16, + "angularVelocity": 2.0328851408532914e-15, + "velocityX": 1.0526232220049963, + "velocityY": -0.281371749651518, + "timestamp": 8.040141369345909 + }, + { + "x": 7.880988965996717, + "y": 5.648813743284097, + "heading": -5.830363906716236e-17, + "angularVelocity": 1.467086785481853e-15, + "velocityX": 0.6885409887010987, + "velocityY": -0.17040584561707373, + "timestamp": 8.109798173211589 + }, + { + "x": 7.903589725494385, + "y": 5.6446733474731445, + "heading": 2.0598914784739755e-32, + "angularVelocity": 8.370128572804695e-16, + "velocityX": 0.3244587498051791, + "velocityY": -0.05943993380554415, + "timestamp": 8.179454977077269 + }, + { + "x": 7.9005397389522525, + "y": 5.6483789255306105, + "heading": 2.097139947940835e-17, + "angularVelocity": 2.980227828531976e-16, + "velocityX": -0.04334309998236763, + "velocityY": 0.05265965538925957, + "timestamp": 8.249823421958254 + }, + { + "x": 7.871608103494799, + "y": 5.6599727605367445, + "heading": -9.084634582304173e-18, + "angularVelocity": -4.2712371472453026e-16, + "velocityX": -0.41114501686552, + "velocityY": 0.16475900563136703, + "timestamp": 8.32019186683924 + }, + { + "x": 7.816794813351453, + "y": 5.6794548319403075, + "heading": -7.520085122829124e-17, + "angularVelocity": -9.395719591601255e-16, + "velocityX": -0.7789470157537616, + "velocityY": 0.27685806382157147, + "timestamp": 8.390560311720225 + }, + { + "x": 7.736099861309118, + "y": 5.706825114052552, + "heading": -1.8616176975942832e-16, + "angularVelocity": -1.5768562134134974e-15, + "velocityX": -1.1467491171467201, + "velocityY": 0.388956756951142, + "timestamp": 8.460928756601211 + }, + { + "x": 7.629523238094034, + "y": 5.742083573845685, + "heading": -3.3813406750936066e-16, + "angularVelocity": -2.1596654705354767e-15, + "velocityX": -1.5145513503282884, + "velocityY": 0.5010549807255615, + "timestamp": 8.531297201482197 + }, + { + "x": 7.497064931341595, + "y": 5.785230167283751, + "heading": -5.316654292279505e-16, + "angularVelocity": -2.750257804825092e-15, + "velocityX": -1.8823537592222348, + "velocityY": 0.6131525787087516, + "timestamp": 8.601665646363182 + }, + { + "x": 7.338724923742105, + "y": 5.836264832718443, + "heading": -7.669822112767176e-16, + "angularVelocity": -3.3440668612043406e-15, + "velocityX": -2.2501564141027943, + "velocityY": 0.7252493006130729, + "timestamp": 8.672034091244168 + }, + { + "x": 7.154503189332537, + "y": 5.895187477681304, + "heading": -1.0519707319411395e-15, + "angularVelocity": -4.0499477067678845e-15, + "velocityX": -2.617959437941741, + "velocityY": 0.8373447084559508, + "timestamp": 8.742402536125153 + }, + { + "x": 6.944399684844722, + "y": 5.961997948067273, + "heading": -1.3745422496318406e-15, + "angularVelocity": -4.5840364940621565e-15, + "velocityX": -2.985763076660981, + "velocityY": 0.9494379263248582, + "timestamp": 8.812770981006139 + }, + { + "x": 6.708414323753371, + "y": 6.036695935692792, + "heading": -1.7429472421431418e-15, + "angularVelocity": -5.235372077575223e-15, + "velocityX": -3.3535679449837517, + "velocityY": 1.061526764623348, + "timestamp": 8.883139425887125 + }, + { + "x": 6.4465468465502065, + "y": 6.119280516151445, + "heading": -2.1550026112189195e-15, + "angularVelocity": -5.855683878769628e-15, + "velocityX": -3.7213765011567173, + "velocityY": 1.1736024662605187, + "timestamp": 8.95350787076811 + }, + { + "x": 6.173126442867114, + "y": 6.178044585557323, + "heading": -2.191046659518503e-15, + "angularVelocity": -5.122189123349113e-16, + "velocityX": -3.8855541591979326, + "velocityY": 0.8350912046286955, + "timestamp": 9.023876315649096 + }, + { + "x": 5.8953070640563965, + "y": 6.21011209487915, + "heading": 0, + "angularVelocity": 3.113677831051976e-14, + "velocityX": -3.9480676215152357, + "velocityY": 0.45570865426474233, + "timestamp": 9.094244760530081 + }, + { + "x": 5.637913658685597, + "y": 6.216758064974601, + "heading": 9.67763523798797e-9, + "angularVelocity": 1.4937766527408904e-7, + "velocityX": -3.972956719905001, + "velocityY": 0.10258285959556052, + "timestamp": 9.159031120794502 + }, + { + "x": 5.380949926914102, + "y": 6.200473867453901, + "heading": 1.892686165769219e-8, + "angularVelocity": 1.4276502617102237e-7, + "velocityX": -3.966324558486143, + "velocityY": -0.2513522514767974, + "timestamp": 9.223817481058923 + }, + { + "x": 5.132255360579735, + "y": 6.163185174806636, + "heading": 0.01632330814348079, + "angularVelocity": 0.25195564544398485, + "velocityX": -3.8386871143710315, + "velocityY": -0.5755639381137804, + "timestamp": 9.288603841323344 + }, + { + "x": 4.905757766533857, + "y": 6.129190200240601, + "heading": 0.04988895608633859, + "angularVelocity": 0.5180974483811885, + "velocityX": -3.4960691281513077, + "velocityY": -0.5247242541278667, + "timestamp": 9.353390201587764 + }, + { + "x": 4.701928110746201, + "y": 6.098578355375242, + "heading": 0.08643838460715948, + "angularVelocity": 0.5641531392054652, + "velocityX": -3.146181618409506, + "velocityY": -0.4725044707141436, + "timestamp": 9.418176561852185 + }, + { + "x": 4.520779220798307, + "y": 6.071364064096333, + "heading": 0.12231563080100817, + "angularVelocity": 0.5537777712384655, + "velocityX": -2.7960961104853954, + "velocityY": -0.4200620496107946, + "timestamp": 9.482962922116606 + }, + { + "x": 4.362300886297595, + "y": 6.047552197658039, + "heading": 0.15578751714707642, + "angularVelocity": 0.5166502055290209, + "velocityX": -2.4461682035211605, + "velocityY": -0.3675444390069978, + "timestamp": 9.547749282381027 + }, + { + "x": 4.226482344491556, + "y": 6.027144059810235, + "heading": 0.1858309320626014, + "angularVelocity": 0.4637305567548124, + "velocityX": -2.096406423384503, + "velocityY": -0.3150067045713868, + "timestamp": 9.612535642645447 + }, + { + "x": 4.113314607743797, + "y": 6.010139422555146, + "heading": 0.21176520360476983, + "angularVelocity": 0.40030449984110905, + "velocityX": -1.7467833705408997, + "velocityY": -0.26247248936131745, + "timestamp": 9.677322002909868 + }, + { + "x": 4.022790393937899, + "y": 5.996537426116962, + "heading": 0.23310242597379685, + "angularVelocity": 0.3293474472395599, + "velocityX": -1.3972727196965211, + "velocityY": -0.20995154509025238, + "timestamp": 9.742108363174289 + }, + { + "x": 3.954903777130295, + "y": 5.986337041905185, + "heading": 0.2494756207657714, + "angularVelocity": 0.2527259553577121, + "velocityX": -1.0478535378517617, + "velocityY": -0.1574464774715696, + "timestamp": 9.80689472343871 + }, + { + "x": 3.9096498799999146, + "y": 5.97953733708754, + "heading": 0.260600273972009, + "angularVelocity": 0.17171289081270033, + "velocityX": -0.6985096391537948, + "velocityY": -0.10495580844363857, + "timestamp": 9.87168108370313 + }, + { + "x": 3.8870246410369873, + "y": 5.976137638092041, + "heading": 0.26625190805951465, + "angularVelocity": 0.08723493748434215, + "velocityX": -0.3492284312714647, + "velocityY": -0.052475536233735576, + "timestamp": 9.936467443967551 + }, + { + "x": 3.8870246410369873, + "y": 5.976137638092041, + "heading": 0.26625190805951465, + "angularVelocity": 3.6116165249279265e-32, + "velocityX": -3.053502496463557e-33, + "velocityY": 4.523388089316994e-32, + "timestamp": 10.001253804231972 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index bab0da9..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1 Note + Mobility.path b/src/main/deploy/pathplanner/paths/1 Note + Mobility.path deleted file mode 100644 index 737d557..0000000 --- a/src/main/deploy/pathplanner/paths/1 Note + Mobility.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.9536854768153984, - "y": 3.918240646666479 - }, - "prevControl": null, - "nextControl": { - "x": 2.0332340849899873, - "y": 5.105453132209409 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.393756643998068, - "y": 4.492368813448178 - }, - "prevControl": { - "x": 2.4912657927200694, - "y": 4.385180011107611 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -39.896392922718064, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 6.path b/src/main/deploy/pathplanner/paths/2 Note Center 6.path deleted file mode 100644 index 5c036bd..0000000 --- a/src/main/deploy/pathplanner/paths/2 Note Center 6.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1969112668610487, - "y": 3.548383473371797 - }, - "prevControl": null, - "nextControl": { - "x": 4.405680480405183, - "y": 2.6115165497092763 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.354258240613484, - "y": 3.8645760601078973 - }, - "prevControl": { - "x": 4.065145382336701, - "y": 3.5048236345422836 - }, - "nextControl": { - "x": 7.368522126487903, - "y": 4.42669621430541 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.328810723241988, - "y": 4.075371117931965 - }, - "prevControl": { - "x": 7.33338961685056, - "y": 4.356431195030721 - }, - "nextControl": { - "x": 9.013237768290756, - "y": 3.882121128741725 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.354258240613484, - "y": 3.8645760601078973 - }, - "prevControl": { - "x": 7.591028020857753, - "y": 4.836575493407763 - }, - "nextControl": { - "x": 3.1682196012318817, - "y": 2.914622096397567 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1969112668610487, - "y": 3.548383473371797 - }, - "prevControl": { - "x": 4.534499682408779, - "y": 2.962841646082722 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 7.path b/src/main/deploy/pathplanner/paths/2 Note Center 7.path deleted file mode 100644 index f2301f4..0000000 --- a/src/main/deploy/pathplanner/paths/2 Note Center 7.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1266462475863597, - "y": 3.9231302428368044 - }, - "prevControl": null, - "nextControl": { - "x": 3.398548537464012, - "y": 3.0096849922597873 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.089487893665121, - "y": 2.576384040065871 - }, - "prevControl": { - "x": 3.095128808200161, - "y": 3.32362429955275 - }, - "nextControl": { - "x": 6.068619269906157, - "y": 1.08910779875768 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.164859011597086, - "y": 2.2953239629671147 - }, - "prevControl": { - "x": 6.9991342346038525, - "y": 1.9236435992811916 - }, - "nextControl": { - "x": 8.972906733259972, - "y": 2.5529623669803687 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.089487893665121, - "y": 2.576384040065871 - }, - "prevControl": { - "x": 6.068619269906157, - "y": 0.7963368851131419 - }, - "nextControl": { - "x": 3.381204638750186, - "y": 3.213419866968167 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1266462475823988, - "y": 3.8879977331934006 - }, - "prevControl": { - "x": 2.3445732483476367, - "y": 3.325877579001948 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Center 8.path b/src/main/deploy/pathplanner/paths/2 Note Center 8.path deleted file mode 100644 index 3d2797b..0000000 --- a/src/main/deploy/pathplanner/paths/2 Note Center 8.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1266462475863597, - "y": 3.9231302428368044 - }, - "prevControl": null, - "nextControl": { - "x": 2.3914165945307624, - "y": 2.9862633191742836 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.31199378803893, - "y": 2.0611072320575454 - }, - "prevControl": { - "x": 3.007825058291836, - "y": 2.4958301419732445 - }, - "nextControl": { - "x": 5.647029154258021, - "y": 1.616095443317847 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.281967377058862, - "y": 0.749493538930016 - }, - "prevControl": { - "x": 7.602738857403534, - "y": 0.8080477216589235 - }, - "nextControl": { - "x": 8.556589177984769, - "y": 0.7258192457467483 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.183174586035333, - "y": 2.0611072320575454 - }, - "prevControl": { - "x": 4.710162230595501, - "y": 1.8034688280503517 - }, - "nextControl": { - "x": 3.978896971568412, - "y": 2.1609762880191514 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0798029014032335, - "y": 3.9231302428368044 - }, - "prevControl": { - "x": 2.4733924503512332, - "y": 2.7520465882586542 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2 Note Mobility.path b/src/main/deploy/pathplanner/paths/2 Note Mobility.path deleted file mode 100644 index 9fb93ba..0000000 --- a/src/main/deploy/pathplanner/paths/2 Note Mobility.path +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.4186651567747581, - "y": 4.0749560202824116 - }, - "prevControl": null, - "nextControl": { - "x": 1.4275950826207213, - "y": 4.085166421779603 - }, - "isLocked": true, - "linkedName": null - }, - { - "anchor": { - "x": 2.44939980620702, - "y": 4.0749560202824116 - }, - "prevControl": { - "x": 2.3694684333813116, - "y": 4.087961199070547 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.0, - "rotationDegrees": -0.08105107436216544, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -0.42006383232481037, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note (4,1).path b/src/main/deploy/pathplanner/paths/3 Note (4,1).path deleted file mode 100644 index f41e811..0000000 --- a/src/main/deploy/pathplanner/paths/3 Note (4,1).path +++ /dev/null @@ -1,181 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.2234294142773756, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.2234294142773756, - "y": 6.799868019709742 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 6.244414694343174 - }, - "prevControl": { - "x": 1.964117812947406, - "y": 6.244414694343174 - }, - "nextControl": { - "x": 3.4142135623730976, - "y": 6.244414694343174 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.276640929226852, - "y": 6.244414694343174 - }, - "prevControl": { - "x": 3.894920138865978, - "y": 6.244414694343174 - }, - "nextControl": { - "x": 6.6583617195877265, - "y": 6.244414694343174 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.290831218253917, - "y": 5.771197106303988 - }, - "prevControl": { - "x": 6.4380269751951875, - "y": 6.285864951635245 - }, - "nextControl": { - "x": 8.476003317926567, - "y": 5.719760411946759 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.097853218268906, - "y": 6.151828644514233 - }, - "prevControl": { - "x": 4.8239980189294345, - "y": 6.923379059795515 - }, - "nextControl": { - "x": 1.8891000945282543, - "y": 6.092747571757445 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.9435431352126493, - "y": 6.162115983384651 - }, - "prevControl": { - "x": 1.9398341418478882, - "y": 6.261045103573999 - }, - "nextControl": { - "x": 1.9472521285774105, - "y": 6.063186863195303 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.900265650161439, - "y": 6.974815754147602 - }, - "prevControl": { - "x": 3.0545757332176953, - "y": 7.1188384983334405 - }, - "nextControl": { - "x": 2.0955600294777064, - "y": 6.2237571748427865 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0875658793984893, - "y": 6.56 - }, - "prevControl": { - "x": 2.1492899126209917, - "y": 6.724597421926673 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": -15.592810939801673, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 4, - "rotationDegrees": 15.018360631150516, - "rotateFast": false - }, - { - "waypointRelativePos": 4, - "rotationDegrees": 175.81508387488162, - "rotateFast": false - }, - { - "waypointRelativePos": 6.0, - "rotationDegrees": 54.293308599397186, - "rotateFast": false - }, - { - "waypointRelativePos": 5.0, - "rotationDegrees": 50.979566839281325, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 36.44430514468718, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path b/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path deleted file mode 100644 index 32fccca..0000000 --- a/src/main/deploy/pathplanner/paths/3 Note Center 7+6.path +++ /dev/null @@ -1,155 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1613758933368123, - "y": 3.534963938258926 - }, - "prevControl": null, - "nextControl": { - "x": 2.622104793438532, - "y": 3.2622945435689217 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.744394136478873, - "y": 2.424809974168226 - }, - "prevControl": { - "x": 5.816231988338233, - "y": 0.4382186700247651 - }, - "nextControl": { - "x": 8.092728599078075, - "y": 2.7837000265429 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6610575641103793, - "y": 3.2915091215641485 - }, - "prevControl": { - "x": 6.410261741050422, - "y": 0.16554927534895622 - }, - "nextControl": { - "x": 2.6293247577370926, - "y": 3.3179668640207294 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.183249464961813, - "y": 3.710251406271605 - }, - "prevControl": { - "x": 3.6154004455152573, - "y": 2.5319300935198457 - }, - "nextControl": { - "x": 5.753399221163684, - "y": 4.1387490491313965 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.6275358244666664, - "y": 4.021873571623774 - }, - "prevControl": { - "x": 8.046220471824721, - "y": 4.0338359901197185 - }, - "nextControl": { - "x": 6.605025594392825, - "y": 3.992658993621665 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.183249464961813, - "y": 3.710251406271605 - }, - "prevControl": { - "x": 5.787017410338748, - "y": 3.8660624889495243 - }, - "nextControl": { - "x": 4.603138511506378, - "y": 3.5605453537669765 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6610575641103793, - "y": 3.2915091215641485 - }, - "prevControl": { - "x": 4.667125253586213, - "y": 2.5514064788509176 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 4.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": -29.744881296441672, - "rotateFast": false - }, - { - "waypointRelativePos": 5.1499999999999995, - "rotationDegrees": 26.88010672415831, - "rotateFast": false - }, - { - "waypointRelativePos": 2.9499999999999997, - "rotationDegrees": 26.290465162772012, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -30.529705899934235, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path b/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path deleted file mode 100644 index 4ce8c21..0000000 --- a/src/main/deploy/pathplanner/paths/3 Note Center 8+7.path +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1613758933368123, - "y": 3.534963938258926 - }, - "prevControl": null, - "nextControl": { - "x": 7.734655943810105, - "y": 0.8180081840521911 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.744394136478873, - "y": 0.7887936060606108 - }, - "prevControl": { - "x": 6.93406416515882, - "y": 1.2579320105256153 - }, - "nextControl": { - "x": 8.299471118517559, - "y": 0.46743324802687497 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6610575641103793, - "y": 3.2915091215641485 - }, - "prevControl": { - "x": 3.1827465189114155, - "y": 2.927907728824032 - }, - "nextControl": { - "x": 2.0183368480639654, - "y": 3.739465984263165 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.744394136478873, - "y": 2.424809974168226 - }, - "prevControl": { - "x": 5.816231988338233, - "y": 0.4382186700247651 - }, - "nextControl": { - "x": 8.092728599078075, - "y": 2.7837000265429 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6610575641103793, - "y": 3.2915091215641485 - }, - "prevControl": { - "x": 6.05968680502248, - "y": 0.4187422846900244 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": -28.393019422172674, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -30.529705899934235, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Note Mobility.path b/src/main/deploy/pathplanner/paths/3 Note Mobility.path deleted file mode 100644 index f733e82..0000000 --- a/src/main/deploy/pathplanner/paths/3 Note Mobility.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3931892586007395, - "y": 4.300319734898729 - }, - "prevControl": null, - "nextControl": { - "x": 1.833108731032277, - "y": 4.274843836724711 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.260023024675679, - "y": 4.11628754738991 - }, - "prevControl": { - "x": 1.843378826811743, - "y": 4.100841889320345 - }, - "nextControl": { - "x": 2.5147226220378367, - "y": 4.125729663496364 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.260023024675679, - "y": 5.517046256314735 - }, - "prevControl": { - "x": 1.674611826075504, - "y": 4.857107663289401 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path b/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path deleted file mode 100644 index 47e1bd7..0000000 --- a/src/main/deploy/pathplanner/paths/4 Note (5, 4, 1).path +++ /dev/null @@ -1,129 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3140196323188642, - "y": 7.026501927468906 - }, - "prevControl": null, - "nextControl": { - "x": 2.8062548621530805, - "y": 7.275509749307659 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.281967377058862, - "y": 7.436381206571257 - }, - "prevControl": { - "x": 7.406875539339101, - "y": 7.983313605146107 - }, - "nextControl": { - "x": 8.65671414652387, - "y": 7.202164475655628 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.618501382509183, - "y": 7.436381206571257 - }, - "prevControl": { - "x": 2.262597392527166, - "y": 7.576911245120637 - }, - "nextControl": { - "x": 0.8241493550218395, - "y": 7.263068036937653 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.281967377058862, - "y": 5.855418272890754 - }, - "prevControl": { - "x": 6.841534481927736, - "y": 6.218454205809981 - }, - "nextControl": { - "x": 9.515985591469327, - "y": 5.544405552185677 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.618501382509183, - "y": 7.22558614874719 - }, - "prevControl": { - "x": 2.649054998537956, - "y": 6.956236908194216 - }, - "nextControl": { - "x": -0.10503619526560826, - "y": 7.676056197483782 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9535367487282747, - "y": 7.026501927468906 - }, - "prevControl": { - "x": 2.8012958736331153, - "y": 6.113056676897948 - }, - "nextControl": { - "x": 3.085046944580154, - "y": 7.815563102580183 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5013930170513679, - "y": 6.5463576290918635 - }, - "prevControl": { - "x": 2.2274648828898207, - "y": 7.4480920431170405 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note End 4.path b/src/main/deploy/pathplanner/paths/4 Note End 4.path deleted file mode 100644 index 02d4dc3..0000000 --- a/src/main/deploy/pathplanner/paths/4 Note End 4.path +++ /dev/null @@ -1,113 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3990197549172392, - "y": 4.122752562280961 - }, - "prevControl": null, - "nextControl": { - "x": 0.86189001475465, - "y": 4.122752562280961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.312518814852445, - "y": 4.417663265882087 - }, - "prevControl": { - "x": 2.2988487262132926, - "y": 4.417894718409181 - }, - "nextControl": { - "x": 2.639014036486218, - "y": 4.4121352736264425 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.312518814852445, - "y": 5.51870901890497 - }, - "prevControl": { - "x": 1.7370438094738079, - "y": 5.455725753327694 - }, - "nextControl": { - "x": 2.630607491776283, - "y": 5.553522457244724 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5, - "y": 6.69 - }, - "prevControl": { - "x": 2.0244122309425774, - "y": 6.456084900181747 - }, - "nextControl": { - "x": 2.7512539157284497, - "y": 6.813577788583229 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.560791376212382, - "y": 7.402737796216334 - }, - "prevControl": { - "x": 7.219449971307349, - "y": 7.327200867364698 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2, - "rotationDegrees": -0.04782618214180992, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": -34.50386141489783, - "rotateFast": false - }, - { - "waypointRelativePos": 3, - "rotationDegrees": 38.59798223150914, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -1.3687240473329847, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path b/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path deleted file mode 100644 index dc29915..0000000 --- a/src/main/deploy/pathplanner/paths/4 Note Mobility End Speaker.path +++ /dev/null @@ -1,92 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3990197549172392, - "y": 4.122752562280961 - }, - "prevControl": null, - "nextControl": { - "x": 0.86189001475465, - "y": 4.122752562280961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.312518814852445, - "y": 4.417663265882087 - }, - "prevControl": { - "x": 2.2988487262132926, - "y": 4.417894718409181 - }, - "nextControl": { - "x": 2.639014036486218, - "y": 4.4121352736264425 - }, - "isLocked": true, - "linkedName": null - }, - { - "anchor": { - "x": 2.312518814852445, - "y": 5.51870901890497 - }, - "prevControl": { - "x": 1.7370438094738079, - "y": 5.455725753327694 - }, - "nextControl": { - "x": 2.630607491776283, - "y": 5.553522457244724 - }, - "isLocked": true, - "linkedName": null - }, - { - "anchor": { - "x": 2.500232833799001, - "y": 6.68953141609987 - }, - "prevControl": { - "x": 2.0248015033058433, - "y": 6.455735119803573 - }, - "nextControl": null, - "isLocked": true, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2, - "rotationDegrees": -0.04782618214180992, - "rotateFast": false - }, - { - "waypointRelativePos": 1, - "rotationDegrees": -34.50386141489783, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 36.611383776770865, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path b/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path deleted file mode 100644 index 005509a..0000000 --- a/src/main/deploy/pathplanner/paths/Bottom 1 Note + Mobility.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8539197217278677, - "y": 3.7498265505812527 - }, - "prevControl": null, - "nextControl": { - "x": 1.8328118091416719, - "y": 3.7026753544270083 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.667281089191343, - "y": 3.3374614295547143 - }, - "prevControl": { - "x": 2.4942943959957344, - "y": 3.3668566966785813 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -52.08361958069638, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Do Nothing.path b/src/main/deploy/pathplanner/paths/Do Nothing.path deleted file mode 100644 index 6f67a21..0000000 --- a/src/main/deploy/pathplanner/paths/Do Nothing.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.0166698250657547, - "y": 6.979658581285779 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": { - "x": 1.9932481519741914, - "y": 6.9679477447399965 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -1.5074357587748137, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path deleted file mode 100644 index 1e817ae..0000000 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.4882042634455643, - "y": 2.0023818427266487 - }, - "prevControl": null, - "nextControl": { - "x": 1.4889168759819007, - "y": 1.9765496382844625 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5869934100979313, - "y": 2.0023818427266487 - }, - "prevControl": { - "x": 2.5638038104779914, - "y": 1.930912743769932 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file From b67a08b663971c3c364d318782c5ba54e3321e52 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 19 Jan 2024 22:20:43 -0500 Subject: [PATCH 11/24] Remove random Settings.NoteAlignment --- src/main/java/com/stuypulse/robot/constants/Settings.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 39077d1..1b98914 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -193,13 +193,6 @@ public interface Rotation { } } - public interface NoteAlignment { - - SmartNumber P = new SmartNumber("Driver Settings/Note Alignment/kP", 3.0); - SmartNumber I = new SmartNumber("Driver Settings/Note Alignment/kI", 0.0); - SmartNumber D = new SmartNumber("Driver Settings/Note Alignment/kD", 0.0); - } - public interface Alignment { SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); From dfe6a04ec2918586c90eeedccb5e27a3dc4f12d3 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Sat, 20 Jan 2024 15:02:52 -0500 Subject: [PATCH 12/24] Note detection code --- .../com/stuypulse/robot/RobotContainer.java | 6 ++++++ .../swerve/SwerveDriveDriveToNote.java | 15 +++++++++++---- .../swerve/SwerveDriveNoteAlignedDrive.java | 7 ++++--- .../stuypulse/robot/constants/Settings.java | 18 +++++++++++------- .../subsystems/notevision/NoteVision.java | 18 +++++++++++------- .../com/stuypulse/robot/util/Limelight.java | 18 +++--------------- 6 files changed, 46 insertions(+), 36 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 3338596..edcb90c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; @@ -97,6 +98,11 @@ private void configureButtonBindings() { driver.getRightBumper() .whileTrue(new SwerveDriveNoteAlignedDrive(driver)); + + driver.getLeftBumper() + .whileTrue(new SwerveDriveDriveToNote()) + .whileTrue(new IntakeAcquire()) + .onFalse(new IntakeStop()); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java index bafda64..c0f71bb 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -7,6 +7,7 @@ import static com.stuypulse.robot.constants.Settings.NoteDetection.*; +import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.HolonomicController; @@ -40,7 +41,7 @@ public SwerveDriveDriveToNote(){ new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) ); - SmartDashboard.putData("Vision/Controller", controller); + SmartDashboard.putData("Note Detection/Controller", controller); aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); @@ -53,14 +54,20 @@ private boolean isAligned() { @Override public void execute() { - double noteDistance = vision.getDistanceToNote(); + double noteDistance = vision.getDistanceToNote() - Swerve.LENGTH / 2.0; Rotation2d noteRotation = vision.getRotationToNote(); - Pose2d targetPose = new Pose2d(TARGET_NOTE_DISTANCE.get(), 0, new Rotation2d()); + // origin is center of intake facing forwards + Pose2d targetPose = new Pose2d(0, 0, new Rotation2d()); Pose2d currentPose = new Pose2d(noteDistance * noteRotation.getCos(), noteDistance * noteRotation.getSin(), noteRotation); + if (!vision.hasNoteData()) { + currentPose = new Pose2d(targetPose.getTranslation(), currentPose.getRotation()); + } + swerve.setChassisSpeeds(controller.update(targetPose, currentPose)); - SmartDashboard.putBoolean("Vision/Is Aligned", aligned.get()); + + SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index e6012f7..9f884b2 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -63,7 +63,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { public void execute() { double angularVel = turn.get(); - if (noteVision.hasNoteData() && Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteDetection.THRESHOLD_ANGLE.get()) { + if (Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteDetection.THRESHOLD_ANGLE.get()) { angularVel = -alignController.update( Angle.kZero, Angle.fromRotation2d(noteVision.getRotationToNote()) @@ -71,9 +71,10 @@ public void execute() { } // robot relative - swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().x, speed.get().y, angularVel)); + // swerve.setChassisSpeeds(new ChassisSpeeds(-speed.get().y, speed.get().x, -angularVel)); + swerve.drive(speed.get(), angularVel); - SmartDashboard.putNumber("Note Vision/Output", alignController.getOutput()); + SmartDashboard.putNumber("Note Detection/Angle Output", alignController.getOutput()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 1b98914..2ab2301 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -12,7 +12,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; /*- @@ -164,7 +166,9 @@ public interface Limelight { int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; Pose3d [] POSITIONS = new Pose3d[] { - new Pose3d() // TODO: determine these values when Limelight is put on + new Pose3d( + new Translation3d(Units.inchesToMeters(-3), 0, Units.inchesToMeters(13.75)), + new Rotation3d(0, Math.toRadians(8), Math.toRadians(2))) }; } @@ -177,19 +181,19 @@ public interface NoteDetection { SmartNumber X_ANGLE_RC = new SmartNumber("Note Detection/X Angle RC", 0.05); SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); - SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.08); - SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", 0.1); + SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.2); + SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", Units.inchesToMeters(2)); SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 1); public interface Translation { - SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 1.0); + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 4.0); SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0.0); - SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.15); } public interface Rotation { - SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.0); + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.5); SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0.0); - SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.1); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index 7fc9d75..4565ea0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -56,33 +56,37 @@ public boolean hasNoteData() { @Override public double getDistanceToNote() { + double distance = 0; + for (Limelight limelight : limelights) { if (limelight.hasNoteData()) { - return limelight.getDistanceToNote(); + distance += limelight.getDistanceToNote(); } } - return Double.NaN; + return distance / limelights.length; } @Override public Rotation2d getRotationToNote() { + double degrees = 0; + for (Limelight limelight : limelights) { - if (limelight.hasNoteData()) { - return Rotation2d.fromDegrees(limelight.getXAngle()); - } + degrees += limelight.getXAngle(); } - return Rotation2d.fromDegrees(Double.NaN); + + return Rotation2d.fromDegrees(degrees / limelights.length); } @Override - public void periodic(){ + public void periodic() { for (int i = 0; i < limelights.length; ++i) { limelights[i].updateData(); } if (hasNoteData()) { SmartDashboard.putNumber("Note Detection/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); SmartDashboard.putNumber("Note Detection/Note Distance", getDistanceToNote()); } } diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java index b7c9392..f7e7692 100644 --- a/src/main/java/com/stuypulse/robot/util/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -81,27 +81,15 @@ public void updateData() { } public double getXAngle() { - if(!hasNoteData()) { - return Double.NaN; - } - - return xAngle.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + return xAngle.get() - Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); } public double getYAngle() { - if(!hasNoteData()) { - return Double.NaN; - } - - return tyData + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + return tyData - Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); } public double getDistanceToNote() { - if(!hasNoteData()) { - return Double.NaN; - } - Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); - return POSITIONS[limelightId].getZ() / yRotation.getTan() + POSITIONS[limelightId].getX(); + return POSITIONS[limelightId].getZ() / -yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); } } \ No newline at end of file From 09da6ea35d917730f1855df7a29249687f5f5df8 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Tue, 23 Jan 2024 09:59:10 -0500 Subject: [PATCH 13/24] Make note translation field relative (unfinished) --- .../swerve/SwerveDriveDriveToNote.java | 19 ++++--- .../swerve/SwerveDriveNoteAlignedDrive.java | 25 +++++---- .../stuypulse/robot/constants/Settings.java | 4 +- .../notevision/AbstractNoteVision.java | 6 +-- .../subsystems/notevision/NoteVision.java | 52 +++++++++++++------ .../com/stuypulse/robot/util/Limelight.java | 4 +- 6 files changed, 70 insertions(+), 40 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java index c0f71bb..2ea33cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.HolonomicController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; @@ -18,6 +19,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -25,6 +28,7 @@ public class SwerveDriveDriveToNote extends Command { // Subsystems private final SwerveDrive swerve; + private final AbstractOdometry odometry; private final AbstractNoteVision vision; // Holonomic control @@ -33,6 +37,7 @@ public class SwerveDriveDriveToNote extends Command { public SwerveDriveDriveToNote(){ this.swerve = SwerveDrive.getInstance(); + this.odometry = AbstractOdometry.getInstance(); this.vision = AbstractNoteVision.getInstance(); controller = new HolonomicController( @@ -54,18 +59,18 @@ private boolean isAligned() { @Override public void execute() { - double noteDistance = vision.getDistanceToNote() - Swerve.LENGTH / 2.0; - Rotation2d noteRotation = vision.getRotationToNote(); + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Units.inchesToMeters(18), 0).rotateBy(odometry.getRotation())); - // origin is center of intake facing forwards - Pose2d targetPose = new Pose2d(0, 0, new Rotation2d()); - Pose2d currentPose = new Pose2d(noteDistance * noteRotation.getCos(), noteDistance * noteRotation.getSin(), noteRotation); + Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); if (!vision.hasNoteData()) { - currentPose = new Pose2d(targetPose.getTranslation(), currentPose.getRotation()); + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetTranslation, odometry.getRotation()))); } - swerve.setChassisSpeeds(controller.update(targetPose, currentPose)); + swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose())); SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 9f884b2..91c45fc 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; @@ -18,14 +19,18 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveNoteAlignedDrive extends Command { - private SwerveDrive swerve; - private AbstractNoteVision noteVision; + private final SwerveDrive swerve; + private final AbstractOdometry odometry; + private final AbstractNoteVision noteVision; private VStream speed; private IStream turn; @@ -34,6 +39,7 @@ public class SwerveDriveNoteAlignedDrive extends Command { public SwerveDriveNoteAlignedDrive(Gamepad driver) { swerve = SwerveDrive.getInstance(); + odometry = AbstractOdometry.getInstance(); noteVision = AbstractNoteVision.getInstance(); speed = VStream.create(driver::getLeftStick) @@ -61,14 +67,15 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { @Override public void execute() { - double angularVel = turn.get(); + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Units.inchesToMeters(18), 0).rotateBy(odometry.getRotation())); - if (Math.abs(noteVision.getRotationToNote().getDegrees()) > NoteDetection.THRESHOLD_ANGLE.get()) { - angularVel = -alignController.update( - Angle.kZero, - Angle.fromRotation2d(noteVision.getRotationToNote()) - ); - } + Rotation2d targetRotation = noteVision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + + double angularVel = -alignController.update( + Angle.fromRotation2d(targetRotation), + Angle.fromRotation2d(odometry.getRotation()) + ); // robot relative // swerve.setChassisSpeeds(new ChassisSpeeds(-speed.get().y, speed.get().x, -angularVel)); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2ab2301..eb66bf2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -167,8 +167,8 @@ public interface Limelight { int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; Pose3d [] POSITIONS = new Pose3d[] { new Pose3d( - new Translation3d(Units.inchesToMeters(-3), 0, Units.inchesToMeters(13.75)), - new Rotation3d(0, Math.toRadians(8), Math.toRadians(2))) + new Translation3d(Units.inchesToMeters(3), 0, Units.inchesToMeters(13.75)), + new Rotation3d(0, Math.toRadians(8), Math.toRadians(182))) }; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java index a3c762c..b3864f0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -5,8 +5,7 @@ package com.stuypulse.robot.subsystems.notevision; - -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class AbstractNoteVision extends SubsystemBase { @@ -24,7 +23,6 @@ public static AbstractNoteVision getInstance() { public abstract boolean hasNoteData(); - public abstract double getDistanceToNote(); - public abstract Rotation2d getRotationToNote(); + public abstract Translation2d getEstimatedNotePose(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index 4565ea0..3458b0f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -7,10 +7,13 @@ import static com.stuypulse.robot.constants.Settings.Limelight.*; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.util.Limelight; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -24,6 +27,10 @@ public class NoteVision extends AbstractNoteVision { // store fieldobject2d's to display limelight data private final FieldObject2d[] limelightPoses; + private Translation2d notePose; + + private FieldObject2d note; + protected NoteVision() { // reference to all limelights on robot String[] hostNames = LIMELIGHTS; @@ -42,6 +49,8 @@ protected NoteVision() { PortForwarder.add(port + i * 10, hostNames[i] + ".local", port); } } + + note = AbstractOdometry.getInstance().getField().getObject("Note"); } @Override @@ -55,39 +64,50 @@ public boolean hasNoteData() { } @Override - public double getDistanceToNote() { - double distance = 0; + public Translation2d getEstimatedNotePose() { + Translation2d sum = new Translation2d(); for (Limelight limelight : limelights) { - if (limelight.hasNoteData()) { - distance += limelight.getDistanceToNote(); - } - } + AbstractOdometry odometry = AbstractOdometry.getInstance(); - return distance / limelights.length; - } + Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); - @Override - public Rotation2d getRotationToNote() { - double degrees = 0; + SmartDashboard.putNumber("limelight to note/x", limelightToNote.getX()); + SmartDashboard.putNumber("limelight to note/y", limelightToNote.getY()); - for (Limelight limelight : limelights) { - degrees += limelight.getXAngle(); + Translation2d robotToNote = limelightToNote + .minus(limelight.robotRelativePose.getTranslation().toTranslation2d()) + .rotateBy(limelight.robotRelativePose.getRotation().toRotation2d()); + + SmartDashboard.putNumber("robot to note/x", robotToNote.getX()); + SmartDashboard.putNumber("robot to note/y", robotToNote.getY()); + + Translation2d fieldToNote = robotToNote.plus(odometry.getTranslation()); + + SmartDashboard.putNumber("field to note/x", fieldToNote.getX()); + SmartDashboard.putNumber("field to note/y", fieldToNote.getY()); + + sum = sum.plus(fieldToNote); } - return Rotation2d.fromDegrees(degrees / limelights.length); + return sum.div(limelights.length); } @Override public void periodic() { for (int i = 0; i < limelights.length; ++i) { limelights[i].updateData(); + + notePose = getEstimatedNotePose(); } + note.setPose(new Pose2d(notePose, new Rotation2d())); + if (hasNoteData()) { - SmartDashboard.putNumber("Note Detection/X Angle", getRotationToNote().getDegrees()); + SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); - SmartDashboard.putNumber("Note Detection/Note Distance", getDistanceToNote()); + SmartDashboard.putNumber("Note Detection/Estimated X", notePose.getX()); + SmartDashboard.putNumber("Note Detection/Estimated Y", notePose.getY()); } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java index f7e7692..2c23dd0 100644 --- a/src/main/java/com/stuypulse/robot/util/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -33,7 +33,6 @@ public class Limelight { private final IntegerEntry tvEntry; private int limelightId; - private final Pose3d robotRelativePose; private double txData; private double tyData; @@ -41,6 +40,7 @@ public class Limelight { private IStream xAngle; private BStream noteData; + public final Pose3d robotRelativePose; public Limelight(String tableName, Pose3d robotRelativePose) { this.tableName = tableName; @@ -90,6 +90,6 @@ public double getYAngle() { public double getDistanceToNote() { Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); - return POSITIONS[limelightId].getZ() / -yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); + return POSITIONS[limelightId].getZ() / yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); } } \ No newline at end of file From 3319c793bc791520c5a87238c6d2b65df54cc4e5 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Tue, 23 Jan 2024 17:22:13 -0500 Subject: [PATCH 14/24] Fix note field relative positioning --- .../swerve/SwerveDriveNoteAlignedDrive.java | 1 - .../stuypulse/robot/constants/Settings.java | 6 ++--- .../subsystems/notevision/NoteVision.java | 22 +++++++++---------- .../com/stuypulse/robot/util/Limelight.java | 6 ++--- 4 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 91c45fc..15c31bd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -78,7 +78,6 @@ public void execute() { ); // robot relative - // swerve.setChassisSpeeds(new ChassisSpeeds(-speed.get().y, speed.get().x, -angularVel)); swerve.drive(speed.get(), angularVel); SmartDashboard.putNumber("Note Detection/Angle Output", alignController.getOutput()); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 33e2cc7..677db87 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -164,7 +164,7 @@ public interface Limelight { Pose3d [] POSITIONS = new Pose3d[] { new Pose3d( new Translation3d(Units.inchesToMeters(3), 0, Units.inchesToMeters(13.75)), - new Rotation3d(0, Math.toRadians(8), Math.toRadians(182))) + new Rotation3d(0, Math.toRadians(8), Math.toRadians(2))) }; } @@ -187,9 +187,9 @@ public interface Translation { SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.15); } public interface Rotation { - SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.5); + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 1); SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0.0); - SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.1); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index 3458b0f..b485a7c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -51,6 +51,8 @@ protected NoteVision() { } note = AbstractOdometry.getInstance().getField().getObject("Note"); + + notePose = new Translation2d(); } @Override @@ -65,6 +67,10 @@ public boolean hasNoteData() { @Override public Translation2d getEstimatedNotePose() { + return notePose; + } + + public void updateNotePose() { Translation2d sum = new Translation2d(); for (Limelight limelight : limelights) { @@ -72,25 +78,16 @@ public Translation2d getEstimatedNotePose() { Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); - SmartDashboard.putNumber("limelight to note/x", limelightToNote.getX()); - SmartDashboard.putNumber("limelight to note/y", limelightToNote.getY()); - Translation2d robotToNote = limelightToNote .minus(limelight.robotRelativePose.getTranslation().toTranslation2d()) .rotateBy(limelight.robotRelativePose.getRotation().toRotation2d()); - - SmartDashboard.putNumber("robot to note/x", robotToNote.getX()); - SmartDashboard.putNumber("robot to note/y", robotToNote.getY()); - Translation2d fieldToNote = robotToNote.plus(odometry.getTranslation()); - - SmartDashboard.putNumber("field to note/x", fieldToNote.getX()); - SmartDashboard.putNumber("field to note/y", fieldToNote.getY()); + Translation2d fieldToNote = robotToNote.rotateBy(odometry.getRotation()).plus(odometry.getTranslation()); sum = sum.plus(fieldToNote); } - return sum.div(limelights.length); + notePose = sum.div(limelights.length); } @Override @@ -104,8 +101,11 @@ public void periodic() { note.setPose(new Pose2d(notePose, new Rotation2d())); if (hasNoteData()) { + updateNotePose(); + SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); + SmartDashboard.putNumber("Note Detection/Distance", limelights[0].getDistanceToNote()); SmartDashboard.putNumber("Note Detection/Estimated X", notePose.getX()); SmartDashboard.putNumber("Note Detection/Estimated Y", notePose.getY()); } diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java index 2c23dd0..3baa752 100644 --- a/src/main/java/com/stuypulse/robot/util/Limelight.java +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -62,7 +62,7 @@ public Limelight(String tableName, Pose3d robotRelativePose) { xAngle = IStream.create(() -> txData) .filtered(new LowPassFilter(NoteDetection.X_ANGLE_RC)); noteData = BStream.create(() -> tvEntry.get() == 1) - .filtered(new BDebounceRC.Both(NoteDetection.DEBOUNCE_TIME)); + .filtered(new BDebounceRC.Rising(NoteDetection.DEBOUNCE_TIME)); } public String getTableName() { @@ -81,7 +81,7 @@ public void updateData() { } public double getXAngle() { - return xAngle.get() - Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + return -xAngle.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); } public double getYAngle() { @@ -90,6 +90,6 @@ public double getYAngle() { public double getDistanceToNote() { Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); - return POSITIONS[limelightId].getZ() / yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); + return POSITIONS[limelightId].getZ() / -yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); } } \ No newline at end of file From 09a7000f4a5e0de64055a6a93bd8c5afc7f0e2c7 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Sat, 27 Jan 2024 14:07:06 -0500 Subject: [PATCH 15/24] remove turn stream from note aligned drive, add center to intake front constant --- .../swerve/SwerveDriveNoteAlignedDrive.java | 26 +++++-------------- .../stuypulse/robot/constants/Settings.java | 1 + .../subsystems/notevision/NoteVision.java | 8 +++--- 3 files changed, 10 insertions(+), 25 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 15c31bd..1981b4f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -1,9 +1,9 @@ package com.stuypulse.robot.commands.swerve; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.NoteDetection; import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -11,9 +11,6 @@ import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; @@ -21,8 +18,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -32,10 +27,9 @@ public class SwerveDriveNoteAlignedDrive extends Command { private final AbstractOdometry odometry; private final AbstractNoteVision noteVision; - private VStream speed; - private IStream turn; + private final VStream speed; - private AngleController alignController; + private final AngleController alignController; public SwerveDriveNoteAlignedDrive(Gamepad driver) { swerve = SwerveDrive.getInstance(); @@ -50,15 +44,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), new VRateLimit(Drive.MAX_TELEOP_ACCEL), new VLowPassFilter(Drive.RC) - ); - - turn = IStream.create(driver::getRightX) - .filtered( - x -> SLMath.deadband(x, Turn.DEADBAND.get()), - x -> SLMath.spow(x, Turn.POWER.get()), - x -> x * Turn.MAX_TELEOP_TURNING.get(), - new LowPassFilter(Turn.RC) - ); + ); alignController = new AnglePIDController(NoteDetection.Rotation.P, NoteDetection.Rotation.I, NoteDetection.Rotation.D); @@ -68,7 +54,7 @@ public SwerveDriveNoteAlignedDrive(Gamepad driver) { @Override public void execute() { Translation2d targetTranslation = odometry.getTranslation().plus( - new Translation2d(Units.inchesToMeters(18), 0).rotateBy(odometry.getRotation())); + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); Rotation2d targetRotation = noteVision.getEstimatedNotePose().minus(targetTranslation).getAngle(); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 677db87..aa67974 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -51,6 +51,7 @@ public static RobotType fromString(String serialNum) { public interface Swerve { double WIDTH = Units.inchesToMeters(26); double LENGTH = Units.inchesToMeters(26); + double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(18); SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02); SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed (m per s)", 5.06); diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index b485a7c..ab984bf 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -29,7 +29,7 @@ public class NoteVision extends AbstractNoteVision { private Translation2d notePose; - private FieldObject2d note; + private FieldObject2d note2d; protected NoteVision() { // reference to all limelights on robot @@ -50,7 +50,7 @@ protected NoteVision() { } } - note = AbstractOdometry.getInstance().getField().getObject("Note"); + note2d = AbstractOdometry.getInstance().getField().getObject("Note"); notePose = new Translation2d(); } @@ -94,11 +94,9 @@ public void updateNotePose() { public void periodic() { for (int i = 0; i < limelights.length; ++i) { limelights[i].updateData(); - - notePose = getEstimatedNotePose(); } - note.setPose(new Pose2d(notePose, new Rotation2d())); + note2d.setPose(new Pose2d(notePose, new Rotation2d())); if (hasNoteData()) { updateNotePose(); From 50e306cc09f54b1fa54f07b97fa8600ad00dc139 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:28:49 -0500 Subject: [PATCH 16/24] Update method name --- .../robot/subsystems/notevision/AbstractNoteVision.java | 2 +- .../com/stuypulse/robot/subsystems/notevision/NoteVision.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java index b3864f0..022b133 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -23,6 +23,6 @@ public static AbstractNoteVision getInstance() { public abstract boolean hasNoteData(); - public abstract Translation2d getEstimatedNotePose(); + public abstract Translation2d getEstimatedNoteTranslation(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java index ab984bf..b92492d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -66,7 +66,7 @@ public boolean hasNoteData() { } @Override - public Translation2d getEstimatedNotePose() { + public Translation2d getEstimatedNoteTranslation() { return notePose; } From 3e4588079027b9f6ff2568a829c93f0f9e863cbd Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:29:11 -0500 Subject: [PATCH 17/24] Add note detectionn test auto --- .../autos/Note Detection Test.auto | 43 ++++++++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../paths/Note Detection Path.path | 49 +++++++++++++++++++ 3 files changed, 93 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Note Detection Test.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Note Detection Path.path diff --git a/src/main/deploy/pathplanner/autos/Note Detection Test.auto b/src/main/deploy/pathplanner/autos/Note Detection Test.auto new file mode 100644 index 0000000..5475dbb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Note Detection Test.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3033866247492334, + "y": 6.814737809886499 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake acquire" + } + }, + { + "type": "path", + "data": { + "pathName": "Note Detection Path" + } + }, + { + "type": "named", + "data": { + "name": "drive to note" + } + }, + { + "type": "named", + "data": { + "name": "intake stop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note Detection Path.path b/src/main/deploy/pathplanner/paths/Note Detection Path.path new file mode 100644 index 0000000..1c78c21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note Detection Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3033866247492334, + "y": 6.814737809886499 + }, + "prevControl": null, + "nextControl": { + "x": 2.421420613499129, + "y": 6.814737809886499 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9788388950331144, + "y": 5.384346760266148 + }, + "prevControl": { + "x": 3.3859563993722896, + "y": 6.314936615260962 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -40.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file From 39b22f2d052bf27af9c4f34f4f733fc632c2a905 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:37:09 -0500 Subject: [PATCH 18/24] Add DriveToAutoStart --- .../com/stuypulse/robot/RobotContainer.java | 19 ++-- .../swerve/SwerveDriveToAutoStart.java | 92 +++++++++++++++++++ 2 files changed, 101 insertions(+), 10 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2ecbe69..88527e1 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,12 +7,11 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveDriveToAutoStart; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToScore; import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; @@ -24,15 +23,11 @@ import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; -import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.intake.*; -import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -68,6 +63,11 @@ public RobotContainer() { private void configureNamedCommands() { NamedCommands.registerCommand("intake acquire", new IntakeAcquire()); + NamedCommands.registerCommand("intake stop", new IntakeStop()); + NamedCommands.registerCommand("drive to note", new SwerveDriveDriveToNote() + .alongWith(new IntakeAcquire()) + .andThen(new IntakeStop())); + NamedCommands.registerCommand("drive to score", new SwerveDriveToScore()); } /****************/ @@ -93,14 +93,13 @@ private void configureButtonBindings() { .onFalse(new IntakeStop()); driver.getLeftTriggerButton() - .onFalse(new IntakeDeacquire()) + .onTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); - driver.getBottomButton().whileTrue(new SwerveDriveToPose(new Pose2d(2, 5.5, new Rotation2d(Units.degreesToRadians(180))))); - // driver.getRightButton().whileTrue(new SwerveDriveToPose(new Pose2d(1.5, 5.5, new Rotation2d()))); + driver.getStartButton() + .whileTrue(new SwerveDriveToAutoStart(() -> autonChooser.getSelected().getName())); driver.getTopButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(7).getPose().toPose2d(), driver)); - driver.getBottomButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(8).getPose().toPose2d(), driver)); driver.getRightBumper() .whileTrue(new SwerveDriveNoteAlignedDrive(driver)); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java new file mode 100644 index 0000000..14fac18 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java @@ -0,0 +1,92 @@ +/************************ PROJECT JON *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import static com.stuypulse.robot.constants.Settings.Alignment.*; + +import java.util.function.Supplier; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + + +public class SwerveDriveToAutoStart extends Command { + private final SwerveDrive swerve; + private Pose2d targetPose; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + + private final FieldObject2d targetPose2d; + + private final Supplier name; + + public SwerveDriveToAutoStart(Supplier name) { + this.swerve = SwerveDrive.getInstance(); + this.name = name; + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + SmartDashboard.putData("Alignment/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(X_TOLERANCE.get(), Y_TOLERANCE.get(), ANGLE_TOLERANCE.get()); + } + + @Override + public void initialize() { + try { + targetPose = PathPlannerAuto.getStaringPoseFromAutoFile(name.get()); + } catch (Exception e) { + targetPose = Odometry.getInstance().getPose(); + } + } + + @Override + public void execute() { + Pose2d currentPose = AbstractOdometry.getInstance().getPose(); + + targetPose2d.setPose(targetPose); + + controller.update(targetPose, currentPose); + swerve.setChassisSpeeds(controller.getOutput()); + } + + @Override + public boolean isFinished(){ + return aligned.get(); + } + + public void end(boolean interupted) { + swerve.stop(); + targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + } + +} \ No newline at end of file From a657d18407f8899222f1a247dae8812dd4fddf90 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:37:30 -0500 Subject: [PATCH 19/24] Add NaN and large value check to odometry --- .../robot/subsystems/odometry/Odometry.java | 15 +++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 1 + 2 files changed, 16 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index e3ef34e..23fa8fe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.odometry; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AbstractVision; import com.stuypulse.robot.util.Fiducial; @@ -8,6 +9,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -23,6 +25,7 @@ public class Odometry extends AbstractOdometry { private final FieldObject2d estimatorPose2D; private final SmartBoolean VISION_ACTIVE; + private Pose2d lastGoodPose; protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); @@ -48,6 +51,8 @@ protected Odometry() { swerve.initModule2ds(field); SmartDashboard.putData("Field", field); VISION_ACTIVE = new SmartBoolean("Odometry/Vision Active", true); + + lastGoodPose = new Pose2d(); } @Override @@ -97,6 +102,16 @@ public void periodic() { } } + if (estimator.getEstimatedPosition().getTranslation().getNorm() > new Translation2d(Field.WIDTH, Field.HEIGHT).getNorm() || + odometry.getPoseMeters().getTranslation().getNorm() > new Translation2d(Field.WIDTH, Field.HEIGHT).getNorm() || + estimator.getEstimatedPosition().getX() == Double.NaN || estimator.getEstimatedPosition().getY() == Double.NaN || + odometry.getPoseMeters().getX() == Double.NaN || odometry.getPoseMeters().getY() == Double.NaN + ) { + reset(lastGoodPose); + } else { + lastGoodPose = getPose(); + } + odometryPose2D.setPose(odometry.getPoseMeters()); estimatorPose2D.setPose(estimator.getEstimatedPosition()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 88d56c4..403df28 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -141,6 +141,7 @@ public SwerveModulePosition[] getModulePositions() { for (int i = 0; i < modules.length; i++) { positions[i] = modules[i].getModulePosition(); + SmartDashboard.putNumber("Swerve/Modules/" + modules[i].getID() + "/Position", modules[i].getModulePosition().distanceMeters); } return positions; From e084504d14b69b6113717785d67cb4fed5bdf172 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:37:46 -0500 Subject: [PATCH 20/24] Use distance to determine isFinished --- .../swerve/SwerveDriveDriveToNote.java | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java index 2ea33cb..993a44c 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -7,6 +7,8 @@ import static com.stuypulse.robot.constants.Settings.NoteDetection.*; +import com.stuypulse.robot.constants.Settings.NoteDetection.Rotation; +import com.stuypulse.robot.constants.Settings.NoteDetection.Translation; import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; @@ -33,7 +35,6 @@ public class SwerveDriveDriveToNote extends Command { // Holonomic control private final HolonomicController controller; - private final BStream aligned; public SwerveDriveDriveToNote(){ this.swerve = SwerveDrive.getInstance(); @@ -48,35 +49,32 @@ public SwerveDriveDriveToNote(){ SmartDashboard.putData("Note Detection/Controller", controller); - aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); - addRequirements(swerve); } - private boolean isAligned() { - return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); - } - @Override public void execute() { Translation2d targetTranslation = odometry.getTranslation().plus( new Translation2d(Units.inchesToMeters(18), 0).rotateBy(odometry.getRotation())); - Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + Rotation2d targetRotation = vision.getEstimatedNoteTranslation().minus(targetTranslation).getAngle(); Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); if (!vision.hasNoteData()) { swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetTranslation, odometry.getRotation()))); + } else { + swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose())); } - - swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose())); - - SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); } @Override public boolean isFinished() { - return aligned.get(); + return vision.getEstimatedNoteTranslation().minus(odometry.getTranslation()).getNorm() < CUTOFF_DISTANCE; + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); } } \ No newline at end of file From 5149ad586faee7046bf39745f73e8887eac6964d Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Sat, 3 Feb 2024 15:37:58 -0500 Subject: [PATCH 21/24] Tune values --- .../swerve/SwerveDriveNoteAlignedDrive.java | 2 +- .../com/stuypulse/robot/constants/Settings.java | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java index 1981b4f..a6df51d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -56,7 +56,7 @@ public void execute() { Translation2d targetTranslation = odometry.getTranslation().plus( new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); - Rotation2d targetRotation = noteVision.getEstimatedNotePose().minus(targetTranslation).getAngle(); + Rotation2d targetRotation = noteVision.getEstimatedNoteTranslation().minus(targetTranslation).getAngle(); double angularVel = -alignController.update( Angle.fromRotation2d(targetRotation), diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d5c38e3..00c14d2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -173,6 +173,9 @@ public static Vector2D vpow(Vector2D vec, double power) { } public interface NoteDetection { + + double CUTOFF_DISTANCE = Units.inchesToMeters(22); + SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); SmartNumber X_ANGLE_RC = new SmartNumber("Note Detection/X Angle RC", 0.05); SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); @@ -182,22 +185,22 @@ public interface NoteDetection { SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 1); public interface Translation { - SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 4.0); + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 6.0); SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0.0); SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.15); } public interface Rotation { - SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 1); + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.0); SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0.0); - SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.03); } } public interface Alignment { SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); - SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); - SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.05); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.05); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 3); SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110); SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50); From a1db47f5e04ca990b1ff3f3603506e8329a8b2b1 Mon Sep 17 00:00:00 2001 From: Ivan Chen Date: Wed, 7 Feb 2024 16:20:59 -0500 Subject: [PATCH 22/24] Run other side autons --- .pathplanner/settings.json | 4 ++-- .../pathplanner/paths/B To A (ABC).path | 4 ++-- .../pathplanner/paths/C To B (ABC).path | 4 ++-- .../pathplanner/paths/E To Shoot (HFE).path | 14 ++++++------ .../pathplanner/paths/F To Shoot (HFE).path | 20 ++++++++--------- .../pathplanner/paths/FShoot To E (HFE).path | 18 +++++++-------- .../pathplanner/paths/H To Shoot (HFE).path | 22 +++++++++---------- .../pathplanner/paths/HShoot To F (HFE).path | 20 ++++++++--------- .../pathplanner/paths/Start To C (ABC).path | 4 ++-- .../pathplanner/paths/Start To H (HFE).path | 20 ++++++++--------- 10 files changed, 65 insertions(+), 65 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 13def40..fe0b595 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -7,8 +7,8 @@ "HFE" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/B To A (ABC).path index e0deb94..7eeb80a 100644 --- a/src/main/deploy/pathplanner/paths/B To A (ABC).path +++ b/src/main/deploy/pathplanner/paths/B To A (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/C To B (ABC).path index 644d100..ea2bb56 100644 --- a/src/main/deploy/pathplanner/paths/C To B (ABC).path +++ b/src/main/deploy/pathplanner/paths/C To B (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path index da00da5..2dfbf96 100644 --- a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.976354741378645, - "y": 5.106639879824356 + "x": 3.4613550257373955, + "y": 5.349605268437967 }, "prevControl": { - "x": 5.288791546143204, - "y": 4.49370079566982 + "x": 4.746249330823314, + "y": 4.380929158993735 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path index 5f9ec94..30a9cc2 100644 --- a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.929545854615797, + "x": 8.12, "y": 2.432016158384531 }, "prevControl": null, "nextControl": { - "x": 6.995066434251791, + "x": 7.185520579635994, "y": 3.4562610112362155 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 + "x": 3.4, + "y": 5.21 }, "prevControl": { - "x": 4.259643243410917, - "y": 4.815422774902082 + "x": 4.524927668353058, + "y": 4.997546496479711 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,18 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0.0, - "rotation": -13.0, + "rotation": -7.742166844819546, "rotateFast": false }, "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path index 6632f81..529ca26 100644 --- a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path +++ b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 + "x": 3.4, + "y": 5.21 }, "prevControl": null, "nextControl": { - "x": 5.639808720888058, - "y": 4.489969784247294 + "x": 4.787004771012038, + "y": 4.42620908196786 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.097291348201403 }, "prevControl": { - "x": 6.460969663475785, - "y": 4.359129446883026 + "x": 6.165251635651925, + "y": 4.174203465458797 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path index 0b9e815..98e6258 100644 --- a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 8.12, + "y": 0.91 }, "prevControl": null, "nextControl": { - "x": 5.970951958009818, - "y": 1.1314614999083628 + "x": 6.09651200885518, + "y": 1.3734386164887789 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 + "x": 2.5372885306588757, + "y": 3.23 }, "prevControl": { - "x": 4.464336906902614, - "y": 1.7553300211016838 + "x": 4.555375655754567, + "y": 1.9908154535276095 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path index 09bd3d4..142f822 100644 --- a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path +++ b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 + "x": 2.54, + "y": 3.23 }, "prevControl": null, "nextControl": { - "x": 5.429728048854969, - "y": 1.1066294807828339 + "x": 5.3948334681608605, + "y": 1.3399464381665318 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.929545854615797, + "x": 8.12, "y": 2.432016158384531 }, "prevControl": { - "x": 7.178392770949869, - "y": 2.432016158384531 + "x": 6.386539843490928, + "y": 1.755946563650941 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Start To C (ABC).path index c008fd4..c6e9919 100644 --- a/src/main/deploy/pathplanner/paths/Start To C (ABC).path +++ b/src/main/deploy/pathplanner/paths/Start To C (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Start To H (HFE).path b/src/main/deploy/pathplanner/paths/Start To H (HFE).path index a53726c..fc9333d 100644 --- a/src/main/deploy/pathplanner/paths/Start To H (HFE).path +++ b/src/main/deploy/pathplanner/paths/Start To H (HFE).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.361176508628245, - "y": 2.06581107755857 + "x": 3.401375224923844, + "y": 2.135191701481494 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 8.12, + "y": 0.91 }, "prevControl": { - "x": 5.745230507252463, - "y": 1.1502945514300327 + "x": 6.0473689598811475, + "y": 1.4872642457647487 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,18 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 10.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -37.52421356547046, "rotateFast": false }, "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file From 18270a2544fd439f176fa0a1f165fc6b0633e56d Mon Sep 17 00:00:00 2001 From: Keobkeig Date: Sat, 10 Feb 2024 10:38:12 -0500 Subject: [PATCH 23/24] add SwerveDriveDriveToChain for test --- .../com/stuypulse/robot/RobotContainer.java | 3 + .../swerve/SwerveDriveDriveToChain.java | 55 +++++++++++++++++++ .../com/stuypulse/robot/constants/Field.java | 26 +++++++++ .../stuypulse/robot/constants/Settings.java | 4 ++ 4 files changed, 88 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 88527e1..fcf6ec5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -8,6 +8,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; @@ -108,6 +109,8 @@ private void configureButtonBindings() { .whileTrue(new SwerveDriveDriveToNote()) .whileTrue(new IntakeAcquire()) .onFalse(new IntakeStop()); + + driver.getRightButton().whileTrue(new SwerveDriveDriveToChain()); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java new file mode 100644 index 0000000..ed92d0a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java @@ -0,0 +1,55 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToChain extends Command { + + private final AbstractOdometry odometry; + private final SwerveDrive swerve; + + private Pose2d trapPose; + + public SwerveDriveDriveToChain() { + odometry = AbstractOdometry.getInstance(); + swerve = SwerveDrive.getInstance(); + + addRequirements(swerve); + } + + @Override + public void initialize() { + trapPose = Field.getClosestAllianceTrapPose(odometry.getPose()); + } + + @Override + public void execute() { + Rotation2d translationAngle = trapPose.minus(odometry.getPose()).getTranslation().getAngle(); + Translation2d translation = new Translation2d(Alignment.INTO_CHAIN_SPEED.get(), translationAngle); + + swerve.drive(new Vector2D(translation), 0); + } + + private double getDistanceToTrap() { + return odometry.getPose().minus(trapPose).getTranslation().getNorm(); + } + + @Override + public boolean isFinished() { + return getDistanceToTrap() <= Alignment.TRAP_CLIMB_DISTANCE.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 1c54258..ae42b72 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -8,6 +8,7 @@ import com.stuypulse.stuylib.network.SmartNumber; import java.util.ArrayList; +import java.util.Arrays; import com.stuypulse.robot.util.Fiducial; @@ -108,4 +109,29 @@ public static Pose2d getSpeakerPose() { boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; return SPEAKER_POSES[isBlue ? 0 : 1]; } + + /*** TRAP ***/ + + Pose2d TRAP_POSES[][] = { + // BLUE + { + getFiducial(14).getPose().toPose2d(), + getFiducial(15).getPose().toPose2d(), + getFiducial(16).getPose().toPose2d() + }, + // RED + { + getFiducial(11).getPose().toPose2d(), + getFiducial(12).getPose().toPose2d(), + getFiducial(13).getPose().toPose2d() + } + }; + public static Pose2d[] getAllianceTrapPoses() { + boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; + return TRAP_POSES[isBlue ? 0 : 1]; + } + + public static Pose2d getClosestAllianceTrapPose(Pose2d robotPose) { + return robotPose.nearest(Arrays.asList(getAllianceTrapPoses())); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 00c14d2..4688552 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -205,6 +205,10 @@ public interface Alignment { SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110); SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50); + SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(22.0)); + SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0)); + + SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); public interface Translation { SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2.5); SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); From 34f6f8aa2729f3b99c780641149dbb7621587512 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Sat, 10 Feb 2024 11:24:21 -0500 Subject: [PATCH 24/24] Fix swervedrive to chain --- .../commands/swerve/SwerveDriveDriveToChain.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java index ed92d0a..baa50a1 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDriveToChain extends Command { @@ -28,14 +30,18 @@ public SwerveDriveDriveToChain() { @Override public void initialize() { trapPose = Field.getClosestAllianceTrapPose(odometry.getPose()); + + odometry.getField().getObject("Trap").setPose(trapPose); } @Override public void execute() { - Rotation2d translationAngle = trapPose.minus(odometry.getPose()).getTranslation().getAngle(); + Rotation2d translationAngle = trapPose.getTranslation().minus(odometry.getPose().getTranslation()).getAngle(); + Translation2d translation = new Translation2d(Alignment.INTO_CHAIN_SPEED.get(), translationAngle); - swerve.drive(new Vector2D(translation), 0); + swerve.setChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), 0, odometry.getRotation())); } private double getDistanceToTrap() { @@ -50,6 +56,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { swerve.stop(); + odometry.getField().getObject("Trap").setPose(new Pose2d(Double.NaN, Double.NaN, new Rotation2d(Double.NaN))); } }