From 0d50221dc698591adeb4281f8af060623cbaa1c2 Mon Sep 17 00:00:00 2001 From: Kyra <101209@psdschools.org> Date: Fri, 21 Feb 2020 20:15:55 -0700 Subject: [PATCH 01/39] added new command group --- src/main/java/frc4388/robot/Constants.java | 2 ++ src/main/java/frc4388/robot/RobotContainer.java | 16 +++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 54636c2..1243bca 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -19,6 +19,8 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public static final int SELECTED_AUTO = 1; + public static final class DriveConstants { /* Drive Train IDs */ public static final int DRIVE_LEFT_FRONT_CAN_ID = 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 09ee4a4..7b9fd9b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.WaitCommand; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -24,8 +23,10 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; @@ -235,6 +236,19 @@ public class RobotContainer { // Run path following command, then stop at the end. return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/ + if (Constants.SELECTED_AUTO == 0) { + return new InstantCommand(); + } + else if (Constants.SELECTED_AUTO == 1) { + return new SequentialCommandGroup( new Wait(5, m_robotDrive), + new TurnDegrees(45, m_robotDrive), + new ParallelCommandGroup( + new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive), + new TurnDegrees(315, m_robotDrive) + ) + ); + } + return new InstantCommand(); } From 08d543ba748fb1e97420771d8355e2244365b249 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 22 Feb 2020 10:49:34 -0700 Subject: [PATCH 02/39] Remove Parallel Command Group Command Groups gay --- src/main/java/frc4388/robot/RobotContainer.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7b9fd9b..c11ddcc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -242,10 +242,8 @@ public class RobotContainer { else if (Constants.SELECTED_AUTO == 1) { return new SequentialCommandGroup( new Wait(5, m_robotDrive), new TurnDegrees(45, m_robotDrive), - new ParallelCommandGroup( - new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive), - new TurnDegrees(315, m_robotDrive) - ) + new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive), + new TurnDegrees(315, m_robotDrive) ); } From 4bf9873f85b040c0d65e30c0e90062fa1f8505a5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 26 Feb 2020 00:08:42 -0700 Subject: [PATCH 03/39] Add Missing Imports --- src/main/java/frc4388/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e3e4b93..0a152ba 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -41,6 +41,8 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TurnDegrees; +import frc4388.robot.commands.Wait; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; From 1517a165e11ee140670616a0481209057d9c8066 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 17:05:08 -0700 Subject: [PATCH 04/39] Remove unused imports --- .../java/frc4388/robot/RobotContainer.java | 22 ------------------- 1 file changed, 22 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ea39121..e95431d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,8 +29,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.AutoPath1FromCenter; -import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; @@ -38,21 +36,12 @@ import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.HoldTarget; -import frc4388.robot.commands.HoodPositionPID; import frc4388.robot.commands.DriveWithJoystickDriveStraight; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.commands.StorageIntake; -import frc4388.robot.commands.GotoCoordinates; -import frc4388.robot.commands.RunClimberWithTriggers; -import frc4388.robot.commands.RunExtenderOutIn; -import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -60,29 +49,18 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.commands.ShootFireGroup; import frc4388.robot.commands.ShootFullGroup; import frc4388.robot.commands.ShootPrepGroup; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; import frc4388.robot.commands.TurnDegrees; import frc4388.robot.commands.Wait; -import frc4388.robot.commands.storageOutake; import frc4388.robot.commands.TrimShooter; import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; import frc4388.robot.commands.StoragePrepIntake; -import frc4388.robot.commands.StorageRun; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.commands.TurnDegrees; -import frc4388.robot.commands.Wait; -import frc4388.robot.commands.StorageOutake; -import frc4388.robot.subsystems.Camera; -import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; From 5d2b594459b8630e9d25b77a54554952d02157a0 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Mon, 2 Mar 2020 17:05:29 -0700 Subject: [PATCH 05/39] Fix code to fit API --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e95431d..dfe2cae 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -232,10 +232,10 @@ public class RobotContainer { //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); if (Constants.SELECTED_AUTO == 1) { - return new SequentialCommandGroup(new Wait(5, m_robotDrive), - new TurnDegrees(45, m_robotDrive), - new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive), - new TurnDegrees(315, m_robotDrive) + return new SequentialCommandGroup(new Wait(m_robotDrive, 5, 0), + new TurnDegrees(m_robotDrive, 45), + new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive), + new TurnDegrees(m_robotDrive, 315) ); } From b3558887cca1e9964ba0cf02091e2c76cdc2820f Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 6 Mar 2020 17:28:22 -0700 Subject: [PATCH 06/39] Finished 10 ball auto basically, still needs testing tho --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- .../frc4388/robot/commands/AutoPath2FromRight.java | 13 ++++++++++--- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index dfe4926..1ccad70 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,7 +21,7 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int SELECTED_AUTO = 1; + public static final int SELECTED_AUTO = 0; public static final class DriveConstants { /* Drive Train IDs */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dfe2cae..60b786e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; @@ -228,6 +229,7 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); @@ -240,7 +242,6 @@ public class RobotContainer { } return new InstantCommand(); - // return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java index 43657c3..31ce8a3 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -36,10 +36,17 @@ public class AutoPath2FromRight extends SequentialCommandGroup { new GotoCoordinates(m_drive, m_pneumatics, 0, 8), //Shoot 5 Balls new GotoCoordinates(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 1 (second round) + //Start Intake Ball 6 (Ball 1 second round) new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - //Start Moving to 4th Ball - new GotoCoordinates(m_drive, m_pneumatics, 60, -50), + //Move to 7th Ball + new GotoCoordinates(m_drive, m_pneumatics, 86.7, -64.11, -180), + //Move to 8th Ball + new GotoCoordinates(m_drive, m_pneumatics, -6.34, 15.31, 90), + //Move to 9th Ball + new GotoCoordinates(m_drive, m_pneumatics, 7.11, 24.41, 0), + //Move to 10th Ball + new GotoCoordinates(m_drive, m_pneumatics, -6.34, 13.30), + //Shoot 5 more Balls (Total 10 Ball Autonomous Path) new Wait(m_drive, 0, 2) ); } From 3c0b92a4acf3774fb4ee4bead5062d6cdd1eb1ea Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 6 Mar 2020 18:46:45 -0700 Subject: [PATCH 07/39] Odometry Working haha --- .../java/frc4388/robot/RobotContainer.java | 16 +++++------ .../robot/commands/AutoPath1FromCenter.java | 12 ++++---- .../robot/commands/AutoPath2FromRight.java | 20 ++++++------- .../GotoCoordinatesFieldRelative.java | 28 +++++++++++++++++++ ...java => GotoCoordinatesRobotRelative.java} | 6 ++-- .../java/frc4388/robot/subsystems/Drive.java | 6 ++-- 6 files changed, 57 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java rename src/main/java/frc4388/robot/commands/{GotoCoordinates.java => GotoCoordinatesRobotRelative.java} (89%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 60b786e..4655a12 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -228,20 +228,20 @@ public class RobotContainer { Trajectory trajectory = getTrajectory(config); RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. - //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - + return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - if (Constants.SELECTED_AUTO == 1) { + /*if (Constants.SELECTED_AUTO == 1) { return new SequentialCommandGroup(new Wait(m_robotDrive, 5, 0), new TurnDegrees(m_robotDrive, 45), new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive), new TurnDegrees(m_robotDrive, 315) ); - } + }*/ - return new InstantCommand(); + //return new InstantCommand(); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( @@ -257,14 +257,12 @@ public class RobotContainer { new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(10, 0) + new Translation2d(0, 50) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(20, 20, new Rotation2d(0)), + new Pose2d(50, 50, new Rotation2d(0)), // Pass config config); - // 10 = 20, 20 = 35, 30 = 53.5 - // (0,10) = (8,22) return exampleTrajectory; } diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java index 4ec080a..7af0b3f 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java @@ -29,15 +29,15 @@ public class AutoPath1FromCenter extends SequentialCommandGroup { addCommands( new Wait(m_drive, 0, 1), //shoot pre-loaded 3 balls - new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 75, 44, -90), //Start Intake Ball 1 - new GotoCoordinates(m_drive, m_pneumatics, 0, 12), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 12), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 3 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), new Wait(m_drive, 0, 2) //Shoot 3 Balls ); diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java index 31ce8a3..5c9da9f 100644 --- a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -28,24 +28,24 @@ public class AutoPath2FromRight extends SequentialCommandGroup { m_pneumatics = subsystem2; addCommands( new Wait(m_drive, 0, 1), - new GotoCoordinates(m_drive, m_pneumatics, 0, 77), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 77), //Start Intake Ball 1 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 2 - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), //Shoot 5 Balls - new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), //Start Intake Ball 6 (Ball 1 second round) - new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), //Move to 7th Ball - new GotoCoordinates(m_drive, m_pneumatics, 86.7, -64.11, -180), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 86.7, -64.11, -180), //Move to 8th Ball - new GotoCoordinates(m_drive, m_pneumatics, -6.34, 15.31, 90), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 15.31, 90), //Move to 9th Ball - new GotoCoordinates(m_drive, m_pneumatics, 7.11, 24.41, 0), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 7.11, 24.41, 0), //Move to 10th Ball - new GotoCoordinates(m_drive, m_pneumatics, -6.34, 13.30), + new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 13.30), //Shoot 5 more Balls (Total 10 Ball Autonomous Path) new Wait(m_drive, 0, 2) ); diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java b/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java new file mode 100644 index 0000000..927d4b4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class GotoCoordinatesFieldRelative extends SequentialCommandGroup { + /** + * Creates a new GotoCoordinatesFieldRelative. + */ + public GotoCoordinatesFieldRelative(Drive susbsytem, Pneumatics subsystem2, double xTarget, double yTarget) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + addCommands( + + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java similarity index 89% rename from src/main/java/frc4388/robot/commands/GotoCoordinates.java rename to src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java index 6ca3deb..af8e29a 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java @@ -15,7 +15,7 @@ import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GotoCoordinates extends SequentialCommandGroup { +public class GotoCoordinatesRobotRelative extends SequentialCommandGroup { Drive m_drive; Pneumatics m_pneumatics; @@ -28,7 +28,7 @@ public class GotoCoordinates extends SequentialCommandGroup { /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; @@ -49,7 +49,7 @@ public class GotoCoordinates extends SequentialCommandGroup { new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } - public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { + public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { m_drive = subsystem; m_pneumatics = subsystem2; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 059d3c6..0613050 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -442,8 +442,8 @@ public class Drive extends SubsystemBase { // m_currentAngleYaw-(360), // m_currentAngleYaw+(360)); //double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; - double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelLeft = inchesToTicks(leftSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelRight = inchesToTicks(rightSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; //SmartDashboard.putNumber("Move Vel Left", moveVelLeft); //SmartDashboard.putNumber("Move Vel Right", moveVelRight); @@ -783,7 +783,7 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); + SmartDashboard.putString("Odometry Values Meters", getPose().toString()); //SmartDashboard.putNumber("Odometry Heading", getHeading()); //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); From fe9eb1c40ca82fa70d2b5e3185a4d86730c45df0 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 6 Mar 2020 20:48:57 -0700 Subject: [PATCH 08/39] Merge Repairs --- .../robot/commands/drive/GotoCoordinates.java | 87 ------------------- .../GotoCoordinatesFieldRelative.java | 2 +- .../GotoCoordinatesRobotRelative.java | 0 3 files changed, 1 insertion(+), 88 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java rename src/main/java/frc4388/robot/commands/{ => drive}/GotoCoordinatesFieldRelative.java (96%) rename src/main/java/frc4388/robot/commands/{ => drive}/GotoCoordinatesRobotRelative.java (100%) diff --git a/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java deleted file mode 100644 index 5ffd58c..0000000 --- a/src/main/java/frc4388/robot/commands/drive/GotoCoordinates.java +++ /dev/null @@ -1,87 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.drive; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.auto.Wait; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GotoCoordinatesRobotRelative extends SequentialCommandGroup { - Drive m_drive; - Pneumatics m_pneumatics; - - double m_xTarget; - double m_yTarget; - double m_currentAngle; - double m_hypotDist; - double m_endAngle; - - /** - * Creates a new GotoPosition. - */ - public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - m_drive = subsystem; - m_pneumatics = subsystem2; - - m_xTarget = xTarget; - m_yTarget = yTarget; - - m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); - - m_currentAngle = calcAngle(); - - m_endAngle = endAngle; - - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), - new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); - } - - public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { - m_drive = subsystem; - m_pneumatics = subsystem2; - - m_xTarget = xTarget; - m_yTarget = yTarget; - - m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); - - m_currentAngle = calcAngle(); - - m_endAngle = m_currentAngle; - - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), - new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); - } - - public boolean isQuadrantThree() { - if ((m_xTarget < 0) && (m_yTarget < 0)) { - return true; - } else { - return false; - } - } - - public double calcAngle() { - if (isQuadrantThree()) { - return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; - } else { - return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; - } - } - -} diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java similarity index 96% rename from src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java rename to src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java index 927d4b4..6165ffd 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinatesFieldRelative.java +++ b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.commands; +package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java similarity index 100% rename from src/main/java/frc4388/robot/commands/GotoCoordinatesRobotRelative.java rename to src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java From 3f3d9ef92a378c1e2f6a8e3f3fbf8e45ca4bce76 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 6 Mar 2020 20:56:07 -0700 Subject: [PATCH 09/39] Fix Imports --- .../java/frc4388/robot/RobotContainer.java | 38 ------------------- 1 file changed, 38 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d19ba10..72f8b0e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -25,41 +25,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.*; -import frc4388.robot.commands.AutoPath2FromRight; -import frc4388.robot.commands.CalibrateShooter; -import frc4388.robot.commands.DrivePositionMPAux; -import frc4388.robot.commands.DriveStraightAtVelocityPID; -import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.DriveWithJoystickDriveStraight; -import frc4388.robot.commands.RunClimberWithTriggers; -import frc4388.robot.commands.RunExtenderOutIn; -import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.commands.ShooterVelocityControlPID; -import frc4388.robot.commands.StorageIntake; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.Climber; -import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.commands.ShootFullGroup; -import frc4388.robot.commands.ShootPrepGroup; -import frc4388.robot.commands.TrackTarget; -import frc4388.robot.commands.TurnDegrees; -import frc4388.robot.commands.Wait; -import frc4388.robot.commands.TrimShooter; -import frc4388.robot.commands.StorageOutake; -import frc4388.robot.commands.StoragePrepAim; -import frc4388.robot.commands.StoragePrepIntake; -import frc4388.robot.subsystems.Camera; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; -import frc4388.robot.commands.auto.AutoPath1FromCenter; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; import frc4388.robot.commands.climber.RunClimberWithTriggers; @@ -70,12 +37,8 @@ import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.ShootFireGroup; -import frc4388.robot.commands.shooter.ShootFullGroup; -import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; -import frc4388.robot.commands.storage.StorageIntake; -import frc4388.robot.commands.storage.StoragePrepIntake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -90,7 +53,6 @@ import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; -import frc4388.utility.controller.XboxTriggerButton; /** * This class is where the bulk of the robot should be declared. Since From 8f08a14e5c84c4999dd681965c83adda407c5da6 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 6 Mar 2020 20:57:54 -0700 Subject: [PATCH 10/39] I can't believe it's not broken! --- .../java/frc4388/robot/commands/auto/AutoPath1FromCenter.java | 2 +- .../java/frc4388/robot/commands/auto/AutoPath2FromRight.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java index d96516d..b7871b1 100644 --- a/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java @@ -8,7 +8,7 @@ package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.drive.GotoCoordinates; +import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; diff --git a/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java index 3608f6a..5da03fa 100644 --- a/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java +++ b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java @@ -8,7 +8,7 @@ package frc4388.robot.commands.auto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.drive.GotoCoordinates; +import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Pneumatics; From 3bbd00e0f659f0f40ee5ab7c811acf56c50de902 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 7 Mar 2020 09:10:29 -0700 Subject: [PATCH 11/39] Fix Ramsete --- src/main/java/frc4388/robot/Constants.java | 6 ++- .../java/frc4388/robot/RobotContainer.java | 12 ++++-- .../java/frc4388/robot/subsystems/Drive.java | 40 ++++++++++--------- 3 files changed, 35 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d177c44..0d2305f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,9 +70,11 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0); + /* Trajectory Constants */ - public static final double MAX_SPEED_METERS_PER_SECOND = 3; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3; + public static final double MAX_SPEED_METERS_PER_SECOND = 1; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; public static final double TRACK_WIDTH_METERS = 0.648; public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 72f8b0e..6b92ba8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -134,7 +134,7 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); + .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -265,10 +265,16 @@ public class RobotContainer { new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(0, 50) + new Translation2d(1, -1), + new Translation2d(2, 0.0), + new Translation2d(1, 1), + new Translation2d(0, 0), + new Translation2d(1, -1), + new Translation2d(2, 0.0), + new Translation2d(1, 1) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(50, 50, new Rotation2d(0)), + new Pose2d(0, 0, new Rotation2d(0)), // Pass config config); return exampleTrajectory; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe897e7..609b44e 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -64,6 +64,9 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH; public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; + /* Back Motor Gains */ + public static Gains m_gainsVelocityBack = DriveConstants.DRIVE_VELOCITY_GAINS_BACK; + /* Timey Whimey */ public long m_currentTimeMs = System.currentTimeMillis(); public long m_lastTimeMs = m_currentTimeMs; @@ -141,18 +144,18 @@ public class Drive extends SubsystemBase { /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); /* Reset Sensors for WPI_TalonFXs */ resetEncoders(); @@ -325,8 +328,8 @@ public class Drive extends SubsystemBase { m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); m_odometry.update(Rotation2d.fromDegrees( getHeading()), - getDistanceInches(m_leftFrontMotor), - -getDistanceInches(m_rightFrontMotor)); + inchesToMeters(getDistanceInches(m_leftFrontMotor)), + -inchesToMeters(getDistanceInches(m_rightFrontMotor))); } /** @@ -439,8 +442,8 @@ public class Drive extends SubsystemBase { // m_currentAngleYaw-(360), // m_currentAngleYaw+(360)); //double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - double moveVelLeft = inchesToTicks(leftSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; - double moveVelRight = inchesToTicks(rightSpeed)/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; + double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME; //SmartDashboard.putNumber("Move Vel Left", moveVelLeft); //SmartDashboard.putNumber("Move Vel Right", moveVelRight); @@ -450,8 +453,6 @@ public class Drive extends SubsystemBase { m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - System.err.println(moveVelLeft); - m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight); m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft); m_leftFrontMotor.follow(m_leftBackMotor); @@ -751,8 +752,11 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - //SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + + SmartDashboard.putNumber("Left Motor Pos Meters", inchesToMeters(getDistanceInches(m_rightFrontMotor))); + SmartDashboard.putNumber("Right Motor Pos Meters", inchesToMeters(getDistanceInches(m_leftFrontMotor))); /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); From 05c0ef4047cbb46238a3653729e7d539916cffd6 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 12:41:37 -0700 Subject: [PATCH 12/39] Created 6 Ball Auto with PathWeaver Co-Authored-By: Keenan D. Buckley Co-Authored-By: kyrarivera --- PathWeaver/Groups/FirstPathGroup | 2 + PathWeaver/Paths/FirstPath0 | 3 + PathWeaver/Paths/FirstPath1 | 3 + PathWeaver/output/FirstPath.wpilib.json | 1 + PathWeaver/pathweaver.json | 9 +++ src/main/deploy/paths/FirstPath0.wpilib.json | 1 + src/main/deploy/paths/FirstPath1.wpilib.json | 1 + src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 71 ++++++++++++++----- .../commands/auto/SixBallAutoMiddle.java | 35 +++++++++ .../java/frc4388/robot/subsystems/Drive.java | 17 ++--- 12 files changed, 122 insertions(+), 26 deletions(-) create mode 100644 PathWeaver/Groups/FirstPathGroup create mode 100644 PathWeaver/Paths/FirstPath0 create mode 100644 PathWeaver/Paths/FirstPath1 create mode 100644 PathWeaver/output/FirstPath.wpilib.json create mode 100644 PathWeaver/pathweaver.json create mode 100644 src/main/deploy/paths/FirstPath0.wpilib.json create mode 100644 src/main/deploy/paths/FirstPath1.wpilib.json create mode 100644 src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java diff --git a/PathWeaver/Groups/FirstPathGroup b/PathWeaver/Groups/FirstPathGroup new file mode 100644 index 0000000..58e567a --- /dev/null +++ b/PathWeaver/Groups/FirstPathGroup @@ -0,0 +1,2 @@ +FirstPath0 +FirstPath1 diff --git a/PathWeaver/Paths/FirstPath0 b/PathWeaver/Paths/FirstPath0 new file mode 100644 index 0000000..43523ef --- /dev/null +++ b/PathWeaver/Paths/FirstPath0 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +2.923987849862624,-2.381155324331042,3.134,0.0,true, +5.006107200045366,-0.7154598441848491,2.0,0.0,true, diff --git a/PathWeaver/Paths/FirstPath1 b/PathWeaver/Paths/FirstPath1 new file mode 100644 index 0000000..83f52cc --- /dev/null +++ b/PathWeaver/Paths/FirstPath1 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +5.006,-0.715,3.048,0.0,true, +7.635526493704714,-0.7154598441848491,1.0,0.0,true, diff --git a/PathWeaver/output/FirstPath.wpilib.json b/PathWeaver/output/FirstPath.wpilib.json new file mode 100644 index 0000000..758ca33 --- /dev/null +++ b/PathWeaver/output/FirstPath.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.3,"y":-2.416848798905604},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.211173470810985,"velocity":0.6335204124329551,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.366889570140447,"y":-2.4163605362457954},"rotation":{"radians":0.021427560503521688}},"curvature":0.6111933780057722},{"time":0.2999081850801547,"velocity":0.8997245552404642,"acceleration":2.9999999999999982,"pose":{"translation":{"x":3.4348387255942074,"y":-2.413127483495524},"rotation":{"radians":0.07833904937585699}},"curvature":1.0234441896058954},{"time":0.3699333515595466,"velocity":1.1098000546786397,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.5047142961833893,"y":-2.4048968129411943},"rotation":{"radians":0.15838369767185723}},"curvature":1.213822601378998},{"time":0.40145286132680885,"velocity":1.2043585839804265,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.5405910165378556,"y":-2.398342148588244},"rotation":{"radians":0.20314475328020926}},"curvature":1.2329656733386238},{"time":0.4315074877648594,"velocity":1.2945224632945782,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.577185748691339,"y":-2.389919658328446},"rotation":{"radians":0.24914964974226336}},"curvature":1.2110821004593237},{"time":0.46050401407792596,"velocity":1.3815120422337779,"acceleration":3.0,"pose":{"translation":{"x":3.6145536688933833,"y":-2.3794835720596605},"rotation":{"radians":0.29518340315729685}},"curvature":1.1575611604229465},{"time":0.4887377357194707,"velocity":1.4662132071584122,"acceleration":3.0000000000000044,"pose":{"translation":{"x":3.6527393941094592,"y":-2.366915117615063},"rotation":{"radians":0.3402532850169521}},"curvature":1.0820536553272881},{"time":0.5437179276413854,"velocity":1.6311537829241565,"acceleration":3.0,"pose":{"translation":{"x":3.7316925948840147,"y":-2.335034255723885},"rotation":{"radians":0.42468100827986427}},"curvature":0.8990051032472685},{"time":0.5975937989543301,"velocity":1.7927813968629904,"acceleration":2.999999999999998,"pose":{"translation":{"x":3.814207972162949,"y":-2.2938241072957206},"rotation":{"radians":0.4988354340363108}},"curvature":0.7135359234210717},{"time":0.6509282653574082,"velocity":1.9527847960722249,"acceleration":3.0000000000000053,"pose":{"translation":{"x":3.9003076130426964,"y":-2.2431916794422597},"rotation":{"radians":0.5616623820417155}},"curvature":0.5512407842873545},{"time":0.7039320341670622,"velocity":2.111796102501187,"acceleration":2.9999999999999942,"pose":{"translation":{"x":3.989887277814993,"y":-2.183367954498983},"rotation":{"radians":0.6135829743337091}},"curvature":0.4192974357240935},{"time":0.7565960373881516,"velocity":2.269788112164455,"acceleration":2.9999999999999973,"pose":{"translation":{"x":4.08273060721369,"y":-2.1148718927780776},"rotation":{"radians":0.6556696605154494}},"curvature":0.3156321132771837},{"time":0.8087781515227375,"velocity":2.4263344545682126,"acceleration":2.9999999999999964,"pose":{"translation":{"x":4.178523329661565,"y":-2.038474435321345},"rotation":{"radians":0.689150621724}},"curvature":0.23494375911219692},{"time":0.8346228721652991,"velocity":2.503868616495897,"acceleration":3.0,"pose":{"translation":{"x":4.227404226828149,"y":-1.997613644550836},"rotation":{"radians":0.703026083367203}},"curvature":0.20143603346697642},{"time":0.8602616455497234,"velocity":2.58078493664917,"acceleration":2.9999999999999933,"pose":{"translation":{"x":4.276867468517136,"y":-1.9551625066531164},"rotation":{"radians":0.7151623578596862}},"curvature":0.1716130584911985},{"time":0.8856616408535245,"velocity":2.6569849225605733,"acceleration":2.999999999999997,"pose":{"translation":{"x":4.326852559608481,"y":-1.9112722288145225},"rotation":{"radians":0.7256708125595506}},"curvature":0.14489219858770688},{"time":0.9107892607468286,"velocity":2.7323677822404857,"acceleration":2.9999999999999933,"pose":{"translation":{"x":4.377295549321474,"y":-1.8661030175331623},"rotation":{"radians":0.7346488446533966}},"curvature":0.12074979352801006},{"time":0.9356106857771136,"velocity":2.8068320573313406,"acceleration":3.0000000000000098,"pose":{"translation":{"x":4.4281294751912,"y":-1.8198229537049446},"rotation":{"radians":0.7421799396015097}},"curvature":0.0987167163871896},{"time":0.9600923145879835,"velocity":2.8802769437639504,"acceleration":2.614799069105123,"pose":{"translation":{"x":4.479284807045011,"y":-1.7726068677096063},"rotation":{"radians":0.7483339599437678}},"curvature":0.07837160771189397},{"time":0.9842391546760743,"velocity":2.9434160787481205,"acceleration":0.7091827420070252,"pose":{"translation":{"x":4.5306898909789775,"y":-1.7246352144967432},"rotation":{"radians":0.753167533852469}},"curvature":0.05933300885315723},{"time":1.008233944163923,"velocity":2.9604327693509944,"acceleration":0.6929390292542418,"pose":{"translation":{"x":4.582271393334359,"y":-1.6760929486718354},"rotation":{"radians":0.7567244542336702}},"curvature":0.04125109123041828},{"time":1.0322061707397836,"velocity":2.977044060763534,"acceleration":0.6902603500969828,"pose":{"translation":{"x":4.633954744674066,"y":-1.627168399582279},"rotation":{"radians":0.7590360284526021}},"curvature":0.023799333454241605},{"time":1.0560961165417506,"velocity":2.9935343431165977,"acceleration":-0.15360934586446268,"pose":{"translation":{"x":4.685664583759118,"y":-1.5780521464034132},"rotation":{"radians":0.7601213391661734}},"curvature":0.006666277661887551},{"time":1.079922908603498,"velocity":2.989874325173944,"acceleration":-0.7013269525645326,"pose":{"translation":{"x":4.737325201525113,"y":-1.5289358932245467},"rotation":{"radians":0.7599873909288635}},"curvature":-0.010452640946589037},{"time":1.1037564741134904,"velocity":2.973159203306074,"acceleration":-2.462603005612013,"pose":{"translation":{"x":4.7888609850586885,"y":-1.480011344134991},"rotation":{"radians":0.7586291273016804}},"curvature":-0.02786327923933318},{"time":1.127758375594418,"velocity":2.9140520485787382,"acceleration":-3.0,"pose":{"translation":{"x":4.840196861573979,"y":-1.4314690783100834},"rotation":{"radians":0.7560293108474947}},"curvature":-0.045881117204540606},{"time":1.1521061171128049,"velocity":2.841008824023578,"acceleration":-3.000000000000006,"pose":{"translation":{"x":4.891258742389087,"y":-1.3834974250972196},"rotation":{"radians":0.7521582651136997}},"curvature":-0.06483987946511054},{"time":1.176818513738355,"velocity":2.766871634146927,"acceleration":-3.0000000000000067,"pose":{"translation":{"x":4.941973966902539,"y":-1.3362813391018817},"rotation":{"radians":0.7469734848845259}},"curvature":-0.08510057564153622},{"time":1.201861468016625,"velocity":2.691742771312117,"acceleration":-3.0,"pose":{"translation":{"x":4.992271746569754,"y":-1.290001275273664},"rotation":{"radians":0.7404191301220512}},"curvature":-0.10706103507590262},{"time":1.2272000754281833,"velocity":2.6157269490774424,"acceleration":-3.0000000000000067,"pose":{"translation":{"x":5.042083608879501,"y":-1.2448320639923036},"rotation":{"radians":0.7324254318684337}},"curvature":-0.13116590516731727},{"time":1.2527989511809319,"velocity":2.5389303218191968,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":5.091343841330369,"y":-1.2009417861537095},"rotation":{"radians":0.7229080571771316}},"curvature":-0.15791689397528239},{"time":1.2786226365851732,"velocity":2.461459265606473,"acceleration":-2.999999999999993,"pose":{"translation":{"x":5.139989935407221,"y":-1.1584906482559907},"rotation":{"radians":0.7117675077743372}},"curvature":-0.1878827213264359},{"time":1.3046361067471872,"velocity":2.383418855120431,"acceleration":-3.0,"pose":{"translation":{"x":5.187963030557664,"y":-1.1176298574854808},"rotation":{"radians":0.698888667392153}},"curvature":-0.2217077309734497},{"time":1.3570967645000946,"velocity":2.226036881861709,"acceleration":-3.000000000000004,"pose":{"translation":{"x":5.2816756855422335,"y":-1.0412324000287478},"rotation":{"radians":0.6673773445352108}},"curvature":-0.3039170857471932},{"time":1.4099383576443232,"velocity":2.0675121024290233,"acceleration":-2.999999999999998,"pose":{"translation":{"x":5.3721007522253466,"y":-0.9727363383078425},"rotation":{"radians":0.6271532154617565}},"curvature":-0.41122091053998006},{"time":1.4629853653237144,"velocity":1.90837107939085,"acceleration":-3.000000000000002,"pose":{"translation":{"x":5.458943958445762,"y":-0.9129126133645666},"rotation":{"radians":0.5768350522658812}},"curvature":-0.5506698994005365},{"time":1.5161869847789742,"velocity":1.7487662210250707,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":5.542012021186539,"y":-0.8622801855111053},"rotation":{"radians":0.5151039437421481}},"curvature":-0.7268715379569509},{"time":1.569704317704183,"velocity":1.5882142222494449,"acceleration":-3.0000000000000027,"pose":{"translation":{"x":5.62122685382185,"y":-0.82107003708294},"rotation":{"radians":0.44119370579756156}},"curvature":-0.9353726275622822},{"time":1.6240447272199892,"velocity":1.425192993702026,"acceleration":-3.0,"pose":{"translation":{"x":5.696639773363788,"y":-0.7891891751917628},"rotation":{"radians":0.355773742439762}},"curvature":-1.1512466772492118},{"time":1.65183493577584,"velocity":1.3418223680344739,"acceleration":-3.0,"pose":{"translation":{"x":5.732975432412914,"y":-0.7766207207471636},"rotation":{"radians":0.30965392012596843}},"curvature":-1.2454525068745395},{"time":1.6802952251020455,"velocity":1.2564415000558569,"acceleration":-3.0,"pose":{"translation":{"x":5.7684457077091915,"y":-0.7661846344783774},"rotation":{"radians":0.2621942336526418}},"curvature":-1.317284482724688},{"time":1.709712366919219,"velocity":1.1681900746043365,"acceleration":-2.999999999999997,"pose":{"translation":{"x":5.803099735316894,"y":-0.7577621442185807},"rotation":{"radians":0.21442500493297934}},"curvature":-1.3548895419721148},{"time":1.7404828856629597,"velocity":1.0758785183731143,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":5.8369974028864435,"y":-0.7512074798656316},"rotation":{"radians":0.16764067155308857}},"curvature":-1.3463080501042284},{"time":1.8086103280430197,"velocity":0.8714961912329341,"acceleration":-3.0000000000000004,"pose":{"translation":{"x":5.902819630302291,"y":-0.7429768093113003},"rotation":{"radians":0.08329065457988592}},"curvature":-1.152025179001855},{"time":1.8946607845548429,"velocity":0.6133448216974647,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":5.966623393988655,"y":-0.7397437565610252},"rotation":{"radians":0.022845873177659935}},"curvature":-0.6940883889283442},{"time":2.099109058453998,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":6.029320137849446,"y":-0.7392554939012195},"rotation":{"radians":7.105427357601138E-15}},"curvature":2.019483917365906E-28}] \ No newline at end of file diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..cae9a01 --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Always Meters", + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "wheelBase": 0.648, + "gameName": "Infinite Recharge", + "outputDir": ".." +} \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath0.wpilib.json b/src/main/deploy/paths/FirstPath0.wpilib.json new file mode 100644 index 0000000..d4ab98b --- /dev/null +++ b/src/main/deploy/paths/FirstPath0.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":1.0,"pose":{"translation":{"x":2.923987849862624,"y":-2.381155324331042},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.44218437689184154,"velocity":0.44218437689184154,"acceleration":1.0000000000000002,"pose":{"translation":{"x":3.0217501594011638,"y":-2.3806705245269772},"rotation":{"radians":0.0146897503609294}},"curvature":0.29333528903824513},{"time":0.6238359459597442,"velocity":0.6238359459597442,"acceleration":0.9999999999999997,"pose":{"translation":{"x":3.1185190613423117,"y":-2.3774604012288347},"rotation":{"radians":0.055801104897449175}},"curvature":0.5518028905787875},{"time":0.7614104213728297,"velocity":0.7614104213728297,"acceleration":-0.020312238777485957,"pose":{"translation":{"x":3.2134552378254417,"y":-2.369288104224536},"rotation":{"radians":0.11962328720251568}},"curvature":0.7835455155676119},{"time":0.8845575709677203,"velocity":0.7589090270654915,"acceleration":-0.23670869805843414,"pose":{"translation":{"x":3.305878010330035,"y":-2.3544171705664882},"rotation":{"radians":0.2025244128102492}},"curvature":0.9804968904285762},{"time":0.9461205222432847,"velocity":0.7443365410204178,"acceleration":-0.1799104932363984,"pose":{"translation":{"x":3.3509749755370053,"y":-2.3440550990939353},"rotation":{"radians":0.24979596445375538}},"curvature":1.0601182481988838},{"time":1.0083957614112635,"velocity":0.733132572025292,"acceleration":-0.1225833359284373,"pose":{"translation":{"x":3.3952549352656796,"y":-2.3315757826241224},"rotation":{"radians":0.3000956802504631}},"curvature":1.1234869825536697},{"time":1.0712457417484191,"velocity":0.7254282117725268,"acceleration":-0.06551043105781508,"pose":{"translation":{"x":3.4386727888015693,"y":-2.316886980870757},"rotation":{"radians":0.35269632003371687}},"curvature":1.1681980064641713},{"time":1.134532145637512,"velocity":0.7212822921736534,"acceleration":-0.009888307104318211,"pose":{"translation":{"x":3.4811913995620687,"y":-2.2999210261364276},"rotation":{"radians":0.40682014847091763}},"curvature":1.1926534843629537},{"time":1.1981195784423337,"velocity":0.7206535201101041,"acceleration":0.04287858621452664,"pose":{"translation":{"x":3.5227812699586405,"y":-2.2806337063767472},"rotation":{"radians":0.4616677692247436}},"curvature":1.1963869868499444},{"time":1.261880918792892,"velocity":0.7233875162394793,"acceleration":0.09137033478935387,"pose":{"translation":{"x":3.5634202162590025,"y":-2.2590031482644917},"rotation":{"radians":0.5164531175977884}},"curvature":1.1802003969697121},{"time":1.325702955244089,"velocity":0.7292189570769635,"acceleration":0.1343245138273121,"pose":{"translation":{"x":3.603093043449319,"y":-2.2350287002537437},"rotation":{"radians":0.5704391899518312}},"curvature":1.146080956245348},{"time":1.3894909282971848,"velocity":0.7377872455453502,"acceleration":0.17077914994055526,"pose":{"translation":{"x":3.6417912200963847,"y":-2.208729815644034},"rotation":{"radians":0.6229690813832267}},"curvature":1.0969268305279805},{"time":1.4531709546778435,"velocity":0.7486624663188313,"acceleration":0.20017252997962934,"pose":{"translation":{"x":3.679512553209812,"y":-2.180144935644483},"rotation":{"radians":0.6734882066924126}},"curvature":1.0361587010764104},{"time":1.5166898957207804,"velocity":0.7613772134490229,"acceleration":0.2223882977051323,"pose":{"translation":{"x":3.716260863104222,"y":-2.149330372437944},"rotation":{"radians":0.7215557075396957}},"curvature":0.9673130071901889},{"time":1.580012832524956,"velocity":0.7754594935705932,"acceleration":0.2377416301716882,"pose":{"translation":{"x":3.7520456582614283,"y":-2.1163591922451417},"rotation":{"radians":0.7668452706153216}},"curvature":0.8936975562975177},{"time":1.6431187496388067,"velocity":0.7904623971787195,"acceleration":0.24894039216309485,"pose":{"translation":{"x":3.7868818101926265,"y":-2.081320098388818},"rotation":{"radians":0.8091372708920906}},"curvature":0.8181552957739961},{"time":1.7686011530089851,"velocity":0.8217000358832593,"acceleration":0.24906305234095572,"pose":{"translation":{"x":3.853792534741811,"y":-2.0054644668714947},"rotation":{"radians":0.8842979035064634}},"curvature":0.6697194927501393},{"time":1.893100387385028,"velocity":0.8527081952110687,"acceleration":0.23936369079091954,"pose":{"translation":{"x":3.917206914192091,"y":-1.9227434029455939},"rotation":{"radians":0.946833282093064}},"curvature":0.5331300183596569},{"time":2.0164165423541625,"velocity":0.8822256051986257,"acceleration":0.22786218090886912,"pose":{"translation":{"x":3.977403825633883,"y":-1.834315541408051},"rotation":{"radians":0.9972277324197186}},"curvature":0.4120275090416596},{"time":2.138166470714266,"velocity":0.9099678094002575,"acceleration":0.22097608388849158,"pose":{"translation":{"x":4.034716741397703,"y":-1.7414824848456552},"rotation":{"radians":1.0362305174205781}},"curvature":0.30537028739932087},{"time":2.2577476094083764,"velocity":0.9363923811358086,"acceleration":0.22412680138865984,"pose":{"translation":{"x":4.089523324644169,"y":-1.645653061687585},"rotation":{"radians":1.0645726139607177}},"curvature":0.20965549834045422},{"time":2.374341246522086,"velocity":0.9625241400843745,"acceleration":0.24325526789596533,"pose":{"translation":{"x":4.142235024953995,"y":-1.548307584257946},"rotation":{"radians":1.082797067361366}},"curvature":0.12016969703985506},{"time":2.4869389468596177,"velocity":0.9899141238444503,"acceleration":-0.08989432371689705,"pose":{"translation":{"x":4.193286673917996,"y":-1.4509621068283067},"rotation":{"radians":1.0911600272745865}},"curvature":0.03144641201074882},{"time":2.5966005204711453,"velocity":0.9800561708469113,"acceleration":-0.29645758100115066,"pose":{"translation":{"x":4.243126080727084,"y":-1.3551326836702366},"rotation":{"radians":1.0895668164055365}},"curvature":-0.06280765335835946},{"time":2.705539868235784,"velocity":0.9477602753327634,"acceleration":-0.3342144599476055,"pose":{"translation":{"x":4.292203627762266,"y":-1.2622996271078404},"rotation":{"radians":1.0775204885631444}},"curvature":-0.1701207808611152},{"time":2.814165712334998,"velocity":0.9114559475107917,"acceleration":-0.3895828982839913,"pose":{"translation":{"x":4.340961866184649,"y":-1.173871765570298},"rotation":{"radians":1.0540743696689703}},"curvature":-0.2998324969707626},{"time":2.922061882096109,"velocity":0.8694214449815167,"acceleration":-0.4558401339381142,"pose":{"translation":{"x":4.389825111525432,"y":-1.0911507016443966},"rotation":{"radians":1.0177978738880868}},"curvature":-0.4635499088098712},{"time":3.0291652313372652,"velocity":0.8205994399182074,"acceleration":-0.5175273987277358,"pose":{"translation":{"x":4.4391890392759095,"y":-1.0152950701270744},"rotation":{"radians":0.9667945512215511}},"curvature":-0.6747572633079081},{"time":3.135775528406369,"velocity":0.7654256901984429,"acceleration":-0.5484700400579469,"pose":{"translation":{"x":4.489410280477473,"y":-0.9472847960779487},"rotation":{"radians":0.8988641812251724}},"curvature":-0.945872071723171},{"time":3.2424654074278623,"velocity":0.706909487977747,"acceleration":-0.5348283872397173,"pose":{"translation":{"x":4.540796017311605,"y":-0.8878853528718582},"rotation":{"radians":0.8119795371767505}},"curvature":-1.2796551201137232},{"time":3.2960757092532202,"velocity":0.6782371767130563,"acceleration":-0.49969792815057806,"pose":{"translation":{"x":4.567005484389693,"y":-0.8615864682621477},"rotation":{"radians":0.7610796746282562}},"curvature":-1.464229870757777},{"time":3.3499292350563303,"velocity":0.6513266814456385,"acceleration":-0.4429951720848838,"pose":{"translation":{"x":4.59359357868988,"y":-0.8376120202514001},"rotation":{"radians":0.7053027710759296}},"curvature":-1.6522464815533484},{"time":3.4040244578100665,"velocity":0.6273627589328771,"acceleration":-0.3650514227347253,"pose":{"translation":{"x":4.620580157279175,"y":-0.8159814621391459},"rotation":{"radians":0.6449987874949148}},"curvature":-1.833253448963883},{"time":3.458291373425353,"velocity":0.6075525441800915,"acceleration":-0.2675424030750641,"pose":{"translation":{"x":4.6479800358439665,"y":-0.7966941423794647},"rotation":{"radians":0.5808072835890076}},"curvature":-1.9936672001360811},{"time":3.512577503974265,"velocity":0.593028702359389,"acceleration":-0.1528367269212617,"pose":{"translation":{"x":4.675802663552211,"y":-0.7797281876451341},"rotation":{"radians":0.5136887003097897}},"curvature":-2.1180834030120494},{"time":3.566644387560178,"velocity":0.5847652968372852,"acceleration":-0.02318286706797017,"pose":{"translation":{"x":4.704051797915623,"y":-0.7650393858917703},"rotation":{"radians":0.44491642185653585}},"curvature":-2.1916290124257998},{"time":3.620177917379769,"velocity":0.5835242361317984,"acceleration":0.12007141974380939,"pose":{"translation":{"x":4.732725179651859,"y":-0.7525600694219574},"rotation":{"radians":0.37601968000317076}},"curvature":-2.202854559744868},{"time":3.6728118750569414,"velocity":0.589844070156832,"acceleration":0.2771969926585082,"pose":{"translation":{"x":4.761814207546703,"y":-0.7421979979494044},"rotation":{"radians":0.3086811644403933}},"curvature":-2.1461830808554048},{"time":3.7241590649993936,"velocity":0.604077356790345,"acceleration":0.10247113166467756,"pose":{"translation":{"x":4.79130361331626,"y":-0.7338352416630802},"rotation":{"radians":0.24460958247996245}},"curvature":-2.022892354696512},{"time":3.7745471252102867,"velocity":0.6092406783425431,"acceleration":-1.0,"pose":{"translation":{"x":4.821171136469141,"y":-0.7273270642913574},"rotation":{"radians":0.18541805256103536}},"curvature":-1.8401738034507031},{"time":3.8270328664625684,"velocity":0.5567549370902614,"acceleration":-1.0000000000000013,"pose":{"translation":{"x":4.85138719916865,"y":-0.7225008061661495},"rotation":{"radians":0.13253790889217945}},"curvature":-1.6086657324414335},{"time":3.8852342566258984,"velocity":0.49855354692693143,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.881914581094971,"y":-0.7191547672870588},"rotation":{"radians":0.08718511374843187}},"curvature":-1.3394321525761819},{"time":4.030532016951451,"velocity":0.35325578660137885,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.943714258106309,"y":-0.7159446439889163},"rotation":{"radians":0.023010588392320376}},"curvature":-0.7193516920501322},{"time":4.3837878035528295,"velocity":0.0,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":5.00610720004536,"y":-0.7154598441848514},"rotation":{"radians":-3.552713678800552E-15}},"curvature":-3.5527136788007045E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath1.wpilib.json b/src/main/deploy/paths/FirstPath1.wpilib.json new file mode 100644 index 0000000..357c1aa --- /dev/null +++ b/src/main/deploy/paths/FirstPath1.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":0.9999999999999999,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43672580581865883,"velocity":0.4367258058186588,"acceleration":0.9999999999999996,"pose":{"translation":{"x":5.101364714733885,"y":-0.715000133837411},"rotation":{"radians":-4.133406269459032E-6}},"curvature":-8.340395092369047E-5},{"time":0.6186417897382253,"velocity":0.6186417897382251,"acceleration":1.000000000000001,"pose":{"translation":{"x":5.197358832001073,"y":-0.7150010200477351},"rotation":{"radians":-1.5340737547402928E-5}},"curvature":-1.4656459700474635E-4},{"time":0.7595467927096973,"velocity":0.7595467927096974,"acceleration":1.0,"pose":{"translation":{"x":5.294455665127399,"y":-0.7150032761523467},"rotation":{"radians":-3.1829811880428774E-5}},"curvature":-1.8983396025576592E-4},{"time":0.8797386662062437,"velocity":0.8797386662062439,"acceleration":0.9999999999999994,"pose":{"translation":{"x":5.392970060293234,"y":-0.7150073815320199},"rotation":{"radians":-5.1940694850243696E-5}},"curvature":-2.1577333197717033E-4},{"time":0.9869867172733406,"velocity":0.9869867172733408,"acceleration":0.12636200956149105,"pose":{"translation":{"x":5.493071389722454,"y":-0.7150136872941142},"rotation":{"radians":-7.424773047402172E-5}},"curvature":-2.2788163887374496E-4},{"time":1.0893819317414866,"velocity":0.9999255823430155,"acceleration":1.6865081478813383E-5,"pose":{"translation":{"x":5.594796544871399,"y":-0.7150224261397599},"rotation":{"radians":-9.759499776855064E-5}},"curvature":-2.297012203219806E-4},{"time":1.19265591257446,"velocity":0.9999273240671169,"acceleration":3.1333529513589754E-5,"pose":{"translation":{"x":5.698062929617832,"y":-0.7150337222310442},"rotation":{"radians":-1.2108746283101436E-4}},"curvature":-2.243247378336802E-4},{"time":1.2972818696190205,"velocity":0.9999306023676299,"acceleration":4.001965252959674E-5,"pose":{"translation":{"x":5.802681453449902,"y":-0.7150476010581972},"rotation":{"radians":-1.440580917623151E-4}},"curvature":-2.1420508869061022E-4},{"time":1.4029770535527222,"velocity":0.999934832252165,"acceleration":4.492623868769467E-5,"pose":{"translation":{"x":5.908369524655109,"y":-0.715063999306777},"rotation":{"radians":-1.6602563430818663E-4}},"curvature":-2.0114813255280375E-4},{"time":1.5093782536804359,"velocity":0.9999396124578785,"acceleration":4.760827159068493E-5,"pose":{"translation":{"x":6.01476404350926,"y":-0.7150827747248557},"rotation":{"radians":-1.8665207023750203E-4}},"curvature":-1.863925586327476E-4},{"time":1.6160547787367876,"velocity":0.9999446911428558,"acceleration":4.9242040270331036E-5,"pose":{"translation":{"x":6.12143439546543,"y":-0.7151037159902053},"rotation":{"radians":-2.0570427731088185E-4}},"curvature":-1.7071579130564232E-4},{"time":1.7225214395294832,"velocity":0.999949933778454,"acceleration":5.0734682359676405E-5,"pose":{"translation":{"x":6.22789544434293,"y":-0.7151265525774831},"rotation":{"radians":-2.2302157264049555E-4}},"curvature":-1.5453311203111758E-4},{"time":1.8282515334495566,"velocity":0.9999552979611849,"acceleration":5.284205054641302E-5,"pose":{"translation":{"x":6.333620525516261,"y":-0.7151509646254184},"rotation":{"radians":-2.384891161487098E-4}},"curvature":-1.3797542338492307E-4},{"time":1.932689830603459,"velocity":0.9999608166949621,"acceleration":5.6287027733918696E-5,"pose":{"translation":{"x":6.438054439104082,"y":-0.7151765928039965},"rotation":{"radians":-2.52016346465048E-4}},"curvature":-1.2094086552319517E-4},{"time":2.0352655612061343,"velocity":0.9999665903779553,"acceleration":6.188490438000034E-5,"pose":{"translation":{"x":6.540626443158164,"y":-0.7152030481816463},"rotation":{"radians":-2.6351930547014786E-4}},"curvature":-1.0311956260767918E-4},{"time":2.135405403849388,"velocity":0.9999727875225419,"acceleration":7.069238955792907E-5,"pose":{"translation":{"x":6.640763246852358,"y":-0.7152299220924245},"rotation":{"radians":-2.729056617250116E-4}},"curvature":-8.399141357141933E-5},{"time":2.2325464742950283,"velocity":0.999979654656936,"acceleration":8.420632541569917E-5,"pose":{"translation":{"x":6.737902003671551,"y":-0.7152567960032029},"rotation":{"radians":-2.8006132532287797E-4}},"curvature":-6.279554631299584E-5},{"time":2.3261493144579335,"velocity":0.9999875366081545,"acceleration":1.0464975058692262E-4,"pose":{"translation":{"x":6.831503304600633,"y":-0.7152832513808527},"rotation":{"radians":-2.848376935983519E-4}},"curvature":-3.8467738220686515E-5},{"time":2.4157108813665107,"velocity":0.9999969092037937,"acceleration":-6.272661685415307E-5,"pose":{"translation":{"x":6.9210641713134535,"y":-0.7153088795594308},"rotation":{"radians":-2.870387793125379E-4}},"curvature":-9.539523948400935E-6},{"time":2.5007782528032245,"velocity":0.9999915732153788,"acceleration":-1.8157570256514238E-4,"pose":{"translation":{"x":7.006131049361786,"y":-0.715333291607366},"rotation":{"radians":-2.8640782227810663E-4}},"curvature":2.6008813680473154E-5},{"time":2.580961267446303,"velocity":0.9999770139281612,"acceleration":-0.34334358581599045,"pose":{"translation":{"x":7.086312801364286,"y":-0.715356128194644},"rotation":{"radians":-2.826136513720897E-4}},"curvature":7.094629694583373E-5},{"time":2.656934801320935,"velocity":0.9738919883805325,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.161293700195457,"y":-0.7153770694599936},"rotation":{"radians":-2.7523841740668075E-4}},"curvature":1.290294163432336E-4},{"time":2.731182328814193,"velocity":0.8996444608872743,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.230846422174607,"y":-0.7153958448780722},"rotation":{"radians":-2.6377107859051235E-4}},"curvature":2.0521978632845688E-4},{"time":2.805379689250417,"velocity":0.8254471004510505,"acceleration":-1.0,"pose":{"translation":{"x":7.294845040254814,"y":-0.715412243126652},"rotation":{"radians":-2.4761650980063257E-4}},"curvature":3.0569017364415825E-4},{"time":2.9536779192175344,"velocity":0.6771488704839332,"acceleration":-1.0000000000000002,"pose":{"translation":{"x":7.4062611988333185,"y":-0.7154374180450894},"rotation":{"radians":-1.9878917382344983E-4}},"curvature":6.041665401664144E-4},{"time":3.1045757838606516,"velocity":0.526251005840816,"acceleration":-0.9999999999999997,"pose":{"translation":{"x":7.497056433411484,"y":-0.7154524626528294},"rotation":{"radians":-1.264307214018874E-4}},"curvature":0.0010074652170135845},{"time":3.2721414624042318,"velocity":0.358685327297236,"acceleration":-1.0,"pose":{"translation":{"x":7.571198911703647,"y":-0.7154588241371141},"rotation":{"radians":-4.363729332648743E-5}},"curvature":0.001105121415441531},{"time":3.6308267897014677,"velocity":0.0,"acceleration":-1.0,"pose":{"translation":{"x":7.635526493704722,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0d2305f..8d00465 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -73,7 +73,7 @@ public final class Constants { public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0); /* Trajectory Constants */ - public static final double MAX_SPEED_METERS_PER_SECOND = 1; + public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0; public static final double TRACK_WIDTH_METERS = 0.648; public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 9e827dd..82c0c98 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,6 +10,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -78,7 +79,7 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(false); - m_robotContainer.resetOdometry(); + m_robotContainer.resetOdometry(new Pose2d()); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); /* diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6b92ba8..3ebd83c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,26 +7,33 @@ package frc4388.robot; +import java.nio.file.Path; import java.util.List; import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; import frc4388.robot.commands.climber.RunClimberWithTriggers; @@ -233,10 +240,19 @@ public class RobotContainer { public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = getTrajectoryConfig(); - Trajectory trajectory = getTrajectory(config); - RamseteCommand ramseteCommand = getRamseteCommand(trajectory); + //Trajectory trajectory = getTrajectory(config); + + String trajectoryJSON0 = "paths/FirstPath0.wpilib.json"; + String trajectoryJSON1 = "paths/FirstPath1.wpilib.json"; + + String[] sixBallAutoMiddlePaths = new String[]{ + "FirstPath0", + "FirstPath1" + }; + // Run path following command, then stop at the end. - return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1); + return new SixBallAutoMiddle(buildPaths(sixBallAutoMiddlePaths)).andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); @@ -262,25 +278,19 @@ public class RobotContainer { Trajectory getTrajectory(TrajectoryConfig config) { Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), + new Pose2d(2.9, -2.4, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path List.of( - new Translation2d(1, -1), - new Translation2d(2, 0.0), - new Translation2d(1, 1), - new Translation2d(0, 0), - new Translation2d(1, -1), - new Translation2d(2, 0.0), - new Translation2d(1, 1) + new Translation2d(4.1, -1.7) ), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(0, 0, new Rotation2d(0)), + new Pose2d(5.1, -0.7, new Rotation2d(0)), // Pass config config); return exampleTrajectory; } - RamseteCommand getRamseteCommand(Trajectory trajectory) { + public RamseteCommand getRamseteCommand(Trajectory trajectory) { RamseteCommand ramseteCommand = new RamseteCommand( trajectory, m_robotDrive::getPose, @@ -292,6 +302,36 @@ public class RobotContainer { return ramseteCommand; } + public RamseteCommand[] buildPaths(String[] paths) { + RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; + Trajectory initialTrajectory; + + try { + if (true) { + String path = paths[0]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + initialTrajectory = trajectory; + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[0] = ramseteCommand; + } + + for(int i = 1; i < paths.length; i++) { + String path = paths[i]; + String trajectoryJSON = "paths/" + path + ".wpilib.json"; + Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); + ramseteCommands[i] = ramseteCommand; + } + } catch (Exception e) { + DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); + } + + return ramseteCommands; + } + /** * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to @@ -311,9 +351,8 @@ public class RobotContainer { /** * */ - public void resetOdometry() { - m_robotDrive.resetGyroAngles(); - m_robotDrive.setOdometry(new Pose2d()); + public void resetOdometry(Pose2d pose) { + m_robotDrive.setOdometry(pose); } /** diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java new file mode 100644 index 0000000..f89f6ac --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import java.nio.file.Path; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.RobotContainer; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class SixBallAutoMiddle extends SequentialCommandGroup { + /** + * Creates a new SixBallAutoMiddle. + */ + public SixBallAutoMiddle(RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands( + paths[0], + paths[1] + ); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 609b44e..4cb2ab5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -105,7 +105,7 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configFactoryDefault(); m_rightBackMotor.configFactoryDefault(); m_pigeon.configFactoryDefault(); - resetGyroYaw(); + resetGyroYaw(0); /* Config Open Loop Ramp so we don't make sudden output changes */ @@ -497,24 +497,25 @@ public class Drive extends SubsystemBase { */ public void setOdometry(Pose2d pose) { resetEncoders(); + resetGyroYaw(pose.getRotation().getDegrees()); m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); } /** * Resets the yaw of the pigeon */ - public void resetGyroYaw() { - m_pigeon.setYaw(0); - m_pigeon.setAccumZAngle(0); - resetGyroAngles(); + public void resetGyroYaw(double angle) { + m_pigeon.setYaw(angle); + m_pigeon.setAccumZAngle(angle); + resetGyroAngles(angle); } /** * Add docs here */ - public void resetGyroAngles() { - m_lastAngleYaw = 0; - m_currentAngleYaw = 0; + public void resetGyroAngles(double angle) { + m_lastAngleYaw = angle; + m_currentAngleYaw = angle; } /** From 51a8de1045a328e5f6aa9f738f575de94e02e7eb Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 13:41:33 -0700 Subject: [PATCH 13/39] Fixed Waiting Time Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Robot.java | 8 ++++--- .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++++------- .../commands/auto/SixBallAutoMiddle.java | 4 +++- 3 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 82c0c98..8ede29f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -36,7 +36,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - SmartDashboard.putString("Auto?", "NAH"); + SmartDashboard.putString("Is Auto Start?", "NAH"); } /** @@ -64,6 +64,7 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + SmartDashboard.putString("Is Auto Start?", "NAH"); } @Override @@ -75,6 +76,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); @@ -91,8 +93,8 @@ public class Robot extends TimedRobot { // schedule the autonomous command (example) if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - System.err.println("Auto Start"); + //m_autonomousCommand.schedule(); + SmartDashboard.putString("Is Auto Start?", "YEA"); } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3ebd83c..e14d267 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -90,6 +90,9 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + /* Autos */ + SixBallAutoMiddle m_sixBallAutoMiddle; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -107,6 +110,9 @@ public class RobotContainer { configureButtonBindings(); + /* Builds Autos */ + buildAutos(); + /* Default Commands */ // drives the robot with a two-axis input from the driver controller @@ -232,6 +238,14 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); } + public void buildAutos() { + String[] sixBallAutoMiddlePaths = new String[]{ + "FirstPath0", + "FirstPath1" + }; + m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -242,17 +256,9 @@ public class RobotContainer { TrajectoryConfig config = getTrajectoryConfig(); //Trajectory trajectory = getTrajectory(config); - String trajectoryJSON0 = "paths/FirstPath0.wpilib.json"; - String trajectoryJSON1 = "paths/FirstPath1.wpilib.json"; - - String[] sixBallAutoMiddlePaths = new String[]{ - "FirstPath0", - "FirstPath1" - }; - // Run path following command, then stop at the end. //SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1); - return new SixBallAutoMiddle(buildPaths(sixBallAutoMiddlePaths)).andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index f89f6ac..f47951f 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.RobotContainer; +import frc4388.robot.subsystems.Drive; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -23,11 +24,12 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { /** * Creates a new SixBallAutoMiddle. */ - public SixBallAutoMiddle(RamseteCommand[] paths) { + public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( + new Wait(drive, 0, 1), paths[0], paths[1] ); From 7c08840eb8ea67f570b3c4adfabe79551865d2c5 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 13:44:08 -0700 Subject: [PATCH 14/39] Cleaned up code a bit --- src/main/java/frc4388/robot/RobotContainer.java | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e14d267..113f34a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -252,25 +252,14 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // Create config for trajectory - TrajectoryConfig config = getTrajectoryConfig(); + // Create custom trajectories + //TrajectoryConfig config = getTrajectoryConfig(); //Trajectory trajectory = getTrajectory(config); + //RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. - //SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1); return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); - //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - - /*if (Constants.SELECTED_AUTO == 1) { - return new SequentialCommandGroup(new Wait(m_robotDrive, 5, 0), - new TurnDegrees(m_robotDrive, 45), - new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive), - new TurnDegrees(m_robotDrive, 315) - ); - }*/ - //return new InstantCommand(); } TrajectoryConfig getTrajectoryConfig() { From 3bc9704c82c314bef624235b353687d7833815de Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 13:58:41 -0700 Subject: [PATCH 15/39] changed gear ratio needs testing --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d00465..3773f0d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -49,7 +49,7 @@ public final class Constants { public static final double COS_MULTIPLIER_HIGH = 0.8; /* Drive Train Characteristics */ - public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 7.29; public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; public static final double WHEEL_DIAMETER_INCHES = 6; public static final double TICKS_PER_GYRO_REV = 8192; From 9904e9fba30312709fa63461d987abaa6d2a7ef2 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 16:27:06 -0700 Subject: [PATCH 16/39] Tune High Gear PIDs Co-Authored-By: Keenan D. Buckley --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3773f0d..3089bce 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; - public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.16, 0.0, 0.0, 0.058, 0, 1.0); /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; From afff5226c14d3e8343659fb16c74307fcc826c8a Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 7 Mar 2020 18:24:58 -0700 Subject: [PATCH 17/39] Almost finished 6 ball auto, just a little too wide --- PathWeaver/Paths/FirstPath0 | 2 +- PathWeaver/Paths/FirstPath1 | 2 +- PathWeaver/pathweaver.json | 4 ++-- src/main/deploy/paths/FirstPath0.wpilib.json | 2 +- src/main/deploy/paths/FirstPath1.wpilib.json | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++-- .../frc4388/robot/commands/auto/SixBallAutoMiddle.java | 1 - src/main/java/frc4388/robot/subsystems/Drive.java | 4 ++-- 10 files changed, 18 insertions(+), 15 deletions(-) diff --git a/PathWeaver/Paths/FirstPath0 b/PathWeaver/Paths/FirstPath0 index 43523ef..18dd1ec 100644 --- a/PathWeaver/Paths/FirstPath0 +++ b/PathWeaver/Paths/FirstPath0 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -2.923987849862624,-2.381155324331042,3.134,0.0,true, +3.2,-2.4,0.2,2.5,true, 5.006107200045366,-0.7154598441848491,2.0,0.0,true, diff --git a/PathWeaver/Paths/FirstPath1 b/PathWeaver/Paths/FirstPath1 index 83f52cc..4ae0aad 100644 --- a/PathWeaver/Paths/FirstPath1 +++ b/PathWeaver/Paths/FirstPath1 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 5.006,-0.715,3.048,0.0,true, -7.635526493704714,-0.7154598441848491,1.0,0.0,true, +7.2,-0.7154598441848491,1.0,0.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index cae9a01..ddd52e0 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,8 +1,8 @@ { "lengthUnit": "Meter", "exportUnit": "Always Meters", - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 1.7, + "maxAcceleration": 2.7, "wheelBase": 0.648, "gameName": "Infinite Recharge", "outputDir": ".." diff --git a/src/main/deploy/paths/FirstPath0.wpilib.json b/src/main/deploy/paths/FirstPath0.wpilib.json index d4ab98b..9697957 100644 --- a/src/main/deploy/paths/FirstPath0.wpilib.json +++ b/src/main/deploy/paths/FirstPath0.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":1.0,"pose":{"translation":{"x":2.923987849862624,"y":-2.381155324331042},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.44218437689184154,"velocity":0.44218437689184154,"acceleration":1.0000000000000002,"pose":{"translation":{"x":3.0217501594011638,"y":-2.3806705245269772},"rotation":{"radians":0.0146897503609294}},"curvature":0.29333528903824513},{"time":0.6238359459597442,"velocity":0.6238359459597442,"acceleration":0.9999999999999997,"pose":{"translation":{"x":3.1185190613423117,"y":-2.3774604012288347},"rotation":{"radians":0.055801104897449175}},"curvature":0.5518028905787875},{"time":0.7614104213728297,"velocity":0.7614104213728297,"acceleration":-0.020312238777485957,"pose":{"translation":{"x":3.2134552378254417,"y":-2.369288104224536},"rotation":{"radians":0.11962328720251568}},"curvature":0.7835455155676119},{"time":0.8845575709677203,"velocity":0.7589090270654915,"acceleration":-0.23670869805843414,"pose":{"translation":{"x":3.305878010330035,"y":-2.3544171705664882},"rotation":{"radians":0.2025244128102492}},"curvature":0.9804968904285762},{"time":0.9461205222432847,"velocity":0.7443365410204178,"acceleration":-0.1799104932363984,"pose":{"translation":{"x":3.3509749755370053,"y":-2.3440550990939353},"rotation":{"radians":0.24979596445375538}},"curvature":1.0601182481988838},{"time":1.0083957614112635,"velocity":0.733132572025292,"acceleration":-0.1225833359284373,"pose":{"translation":{"x":3.3952549352656796,"y":-2.3315757826241224},"rotation":{"radians":0.3000956802504631}},"curvature":1.1234869825536697},{"time":1.0712457417484191,"velocity":0.7254282117725268,"acceleration":-0.06551043105781508,"pose":{"translation":{"x":3.4386727888015693,"y":-2.316886980870757},"rotation":{"radians":0.35269632003371687}},"curvature":1.1681980064641713},{"time":1.134532145637512,"velocity":0.7212822921736534,"acceleration":-0.009888307104318211,"pose":{"translation":{"x":3.4811913995620687,"y":-2.2999210261364276},"rotation":{"radians":0.40682014847091763}},"curvature":1.1926534843629537},{"time":1.1981195784423337,"velocity":0.7206535201101041,"acceleration":0.04287858621452664,"pose":{"translation":{"x":3.5227812699586405,"y":-2.2806337063767472},"rotation":{"radians":0.4616677692247436}},"curvature":1.1963869868499444},{"time":1.261880918792892,"velocity":0.7233875162394793,"acceleration":0.09137033478935387,"pose":{"translation":{"x":3.5634202162590025,"y":-2.2590031482644917},"rotation":{"radians":0.5164531175977884}},"curvature":1.1802003969697121},{"time":1.325702955244089,"velocity":0.7292189570769635,"acceleration":0.1343245138273121,"pose":{"translation":{"x":3.603093043449319,"y":-2.2350287002537437},"rotation":{"radians":0.5704391899518312}},"curvature":1.146080956245348},{"time":1.3894909282971848,"velocity":0.7377872455453502,"acceleration":0.17077914994055526,"pose":{"translation":{"x":3.6417912200963847,"y":-2.208729815644034},"rotation":{"radians":0.6229690813832267}},"curvature":1.0969268305279805},{"time":1.4531709546778435,"velocity":0.7486624663188313,"acceleration":0.20017252997962934,"pose":{"translation":{"x":3.679512553209812,"y":-2.180144935644483},"rotation":{"radians":0.6734882066924126}},"curvature":1.0361587010764104},{"time":1.5166898957207804,"velocity":0.7613772134490229,"acceleration":0.2223882977051323,"pose":{"translation":{"x":3.716260863104222,"y":-2.149330372437944},"rotation":{"radians":0.7215557075396957}},"curvature":0.9673130071901889},{"time":1.580012832524956,"velocity":0.7754594935705932,"acceleration":0.2377416301716882,"pose":{"translation":{"x":3.7520456582614283,"y":-2.1163591922451417},"rotation":{"radians":0.7668452706153216}},"curvature":0.8936975562975177},{"time":1.6431187496388067,"velocity":0.7904623971787195,"acceleration":0.24894039216309485,"pose":{"translation":{"x":3.7868818101926265,"y":-2.081320098388818},"rotation":{"radians":0.8091372708920906}},"curvature":0.8181552957739961},{"time":1.7686011530089851,"velocity":0.8217000358832593,"acceleration":0.24906305234095572,"pose":{"translation":{"x":3.853792534741811,"y":-2.0054644668714947},"rotation":{"radians":0.8842979035064634}},"curvature":0.6697194927501393},{"time":1.893100387385028,"velocity":0.8527081952110687,"acceleration":0.23936369079091954,"pose":{"translation":{"x":3.917206914192091,"y":-1.9227434029455939},"rotation":{"radians":0.946833282093064}},"curvature":0.5331300183596569},{"time":2.0164165423541625,"velocity":0.8822256051986257,"acceleration":0.22786218090886912,"pose":{"translation":{"x":3.977403825633883,"y":-1.834315541408051},"rotation":{"radians":0.9972277324197186}},"curvature":0.4120275090416596},{"time":2.138166470714266,"velocity":0.9099678094002575,"acceleration":0.22097608388849158,"pose":{"translation":{"x":4.034716741397703,"y":-1.7414824848456552},"rotation":{"radians":1.0362305174205781}},"curvature":0.30537028739932087},{"time":2.2577476094083764,"velocity":0.9363923811358086,"acceleration":0.22412680138865984,"pose":{"translation":{"x":4.089523324644169,"y":-1.645653061687585},"rotation":{"radians":1.0645726139607177}},"curvature":0.20965549834045422},{"time":2.374341246522086,"velocity":0.9625241400843745,"acceleration":0.24325526789596533,"pose":{"translation":{"x":4.142235024953995,"y":-1.548307584257946},"rotation":{"radians":1.082797067361366}},"curvature":0.12016969703985506},{"time":2.4869389468596177,"velocity":0.9899141238444503,"acceleration":-0.08989432371689705,"pose":{"translation":{"x":4.193286673917996,"y":-1.4509621068283067},"rotation":{"radians":1.0911600272745865}},"curvature":0.03144641201074882},{"time":2.5966005204711453,"velocity":0.9800561708469113,"acceleration":-0.29645758100115066,"pose":{"translation":{"x":4.243126080727084,"y":-1.3551326836702366},"rotation":{"radians":1.0895668164055365}},"curvature":-0.06280765335835946},{"time":2.705539868235784,"velocity":0.9477602753327634,"acceleration":-0.3342144599476055,"pose":{"translation":{"x":4.292203627762266,"y":-1.2622996271078404},"rotation":{"radians":1.0775204885631444}},"curvature":-0.1701207808611152},{"time":2.814165712334998,"velocity":0.9114559475107917,"acceleration":-0.3895828982839913,"pose":{"translation":{"x":4.340961866184649,"y":-1.173871765570298},"rotation":{"radians":1.0540743696689703}},"curvature":-0.2998324969707626},{"time":2.922061882096109,"velocity":0.8694214449815167,"acceleration":-0.4558401339381142,"pose":{"translation":{"x":4.389825111525432,"y":-1.0911507016443966},"rotation":{"radians":1.0177978738880868}},"curvature":-0.4635499088098712},{"time":3.0291652313372652,"velocity":0.8205994399182074,"acceleration":-0.5175273987277358,"pose":{"translation":{"x":4.4391890392759095,"y":-1.0152950701270744},"rotation":{"radians":0.9667945512215511}},"curvature":-0.6747572633079081},{"time":3.135775528406369,"velocity":0.7654256901984429,"acceleration":-0.5484700400579469,"pose":{"translation":{"x":4.489410280477473,"y":-0.9472847960779487},"rotation":{"radians":0.8988641812251724}},"curvature":-0.945872071723171},{"time":3.2424654074278623,"velocity":0.706909487977747,"acceleration":-0.5348283872397173,"pose":{"translation":{"x":4.540796017311605,"y":-0.8878853528718582},"rotation":{"radians":0.8119795371767505}},"curvature":-1.2796551201137232},{"time":3.2960757092532202,"velocity":0.6782371767130563,"acceleration":-0.49969792815057806,"pose":{"translation":{"x":4.567005484389693,"y":-0.8615864682621477},"rotation":{"radians":0.7610796746282562}},"curvature":-1.464229870757777},{"time":3.3499292350563303,"velocity":0.6513266814456385,"acceleration":-0.4429951720848838,"pose":{"translation":{"x":4.59359357868988,"y":-0.8376120202514001},"rotation":{"radians":0.7053027710759296}},"curvature":-1.6522464815533484},{"time":3.4040244578100665,"velocity":0.6273627589328771,"acceleration":-0.3650514227347253,"pose":{"translation":{"x":4.620580157279175,"y":-0.8159814621391459},"rotation":{"radians":0.6449987874949148}},"curvature":-1.833253448963883},{"time":3.458291373425353,"velocity":0.6075525441800915,"acceleration":-0.2675424030750641,"pose":{"translation":{"x":4.6479800358439665,"y":-0.7966941423794647},"rotation":{"radians":0.5808072835890076}},"curvature":-1.9936672001360811},{"time":3.512577503974265,"velocity":0.593028702359389,"acceleration":-0.1528367269212617,"pose":{"translation":{"x":4.675802663552211,"y":-0.7797281876451341},"rotation":{"radians":0.5136887003097897}},"curvature":-2.1180834030120494},{"time":3.566644387560178,"velocity":0.5847652968372852,"acceleration":-0.02318286706797017,"pose":{"translation":{"x":4.704051797915623,"y":-0.7650393858917703},"rotation":{"radians":0.44491642185653585}},"curvature":-2.1916290124257998},{"time":3.620177917379769,"velocity":0.5835242361317984,"acceleration":0.12007141974380939,"pose":{"translation":{"x":4.732725179651859,"y":-0.7525600694219574},"rotation":{"radians":0.37601968000317076}},"curvature":-2.202854559744868},{"time":3.6728118750569414,"velocity":0.589844070156832,"acceleration":0.2771969926585082,"pose":{"translation":{"x":4.761814207546703,"y":-0.7421979979494044},"rotation":{"radians":0.3086811644403933}},"curvature":-2.1461830808554048},{"time":3.7241590649993936,"velocity":0.604077356790345,"acceleration":0.10247113166467756,"pose":{"translation":{"x":4.79130361331626,"y":-0.7338352416630802},"rotation":{"radians":0.24460958247996245}},"curvature":-2.022892354696512},{"time":3.7745471252102867,"velocity":0.6092406783425431,"acceleration":-1.0,"pose":{"translation":{"x":4.821171136469141,"y":-0.7273270642913574},"rotation":{"radians":0.18541805256103536}},"curvature":-1.8401738034507031},{"time":3.8270328664625684,"velocity":0.5567549370902614,"acceleration":-1.0000000000000013,"pose":{"translation":{"x":4.85138719916865,"y":-0.7225008061661495},"rotation":{"radians":0.13253790889217945}},"curvature":-1.6086657324414335},{"time":3.8852342566258984,"velocity":0.49855354692693143,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.881914581094971,"y":-0.7191547672870588},"rotation":{"radians":0.08718511374843187}},"curvature":-1.3394321525761819},{"time":4.030532016951451,"velocity":0.35325578660137885,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":4.943714258106309,"y":-0.7159446439889163},"rotation":{"radians":0.023010588392320376}},"curvature":-0.7193516920501322},{"time":4.3837878035528295,"velocity":0.0,"acceleration":-0.9999999999999999,"pose":{"translation":{"x":5.00610720004536,"y":-0.7154598441848514},"rotation":{"radians":-3.552713678800552E-15}},"curvature":-3.5527136788007045E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath1.wpilib.json b/src/main/deploy/paths/FirstPath1.wpilib.json index 357c1aa..1fcfcfc 100644 --- a/src/main/deploy/paths/FirstPath1.wpilib.json +++ b/src/main/deploy/paths/FirstPath1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":0.9999999999999999,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43672580581865883,"velocity":0.4367258058186588,"acceleration":0.9999999999999996,"pose":{"translation":{"x":5.101364714733885,"y":-0.715000133837411},"rotation":{"radians":-4.133406269459032E-6}},"curvature":-8.340395092369047E-5},{"time":0.6186417897382253,"velocity":0.6186417897382251,"acceleration":1.000000000000001,"pose":{"translation":{"x":5.197358832001073,"y":-0.7150010200477351},"rotation":{"radians":-1.5340737547402928E-5}},"curvature":-1.4656459700474635E-4},{"time":0.7595467927096973,"velocity":0.7595467927096974,"acceleration":1.0,"pose":{"translation":{"x":5.294455665127399,"y":-0.7150032761523467},"rotation":{"radians":-3.1829811880428774E-5}},"curvature":-1.8983396025576592E-4},{"time":0.8797386662062437,"velocity":0.8797386662062439,"acceleration":0.9999999999999994,"pose":{"translation":{"x":5.392970060293234,"y":-0.7150073815320199},"rotation":{"radians":-5.1940694850243696E-5}},"curvature":-2.1577333197717033E-4},{"time":0.9869867172733406,"velocity":0.9869867172733408,"acceleration":0.12636200956149105,"pose":{"translation":{"x":5.493071389722454,"y":-0.7150136872941142},"rotation":{"radians":-7.424773047402172E-5}},"curvature":-2.2788163887374496E-4},{"time":1.0893819317414866,"velocity":0.9999255823430155,"acceleration":1.6865081478813383E-5,"pose":{"translation":{"x":5.594796544871399,"y":-0.7150224261397599},"rotation":{"radians":-9.759499776855064E-5}},"curvature":-2.297012203219806E-4},{"time":1.19265591257446,"velocity":0.9999273240671169,"acceleration":3.1333529513589754E-5,"pose":{"translation":{"x":5.698062929617832,"y":-0.7150337222310442},"rotation":{"radians":-1.2108746283101436E-4}},"curvature":-2.243247378336802E-4},{"time":1.2972818696190205,"velocity":0.9999306023676299,"acceleration":4.001965252959674E-5,"pose":{"translation":{"x":5.802681453449902,"y":-0.7150476010581972},"rotation":{"radians":-1.440580917623151E-4}},"curvature":-2.1420508869061022E-4},{"time":1.4029770535527222,"velocity":0.999934832252165,"acceleration":4.492623868769467E-5,"pose":{"translation":{"x":5.908369524655109,"y":-0.715063999306777},"rotation":{"radians":-1.6602563430818663E-4}},"curvature":-2.0114813255280375E-4},{"time":1.5093782536804359,"velocity":0.9999396124578785,"acceleration":4.760827159068493E-5,"pose":{"translation":{"x":6.01476404350926,"y":-0.7150827747248557},"rotation":{"radians":-1.8665207023750203E-4}},"curvature":-1.863925586327476E-4},{"time":1.6160547787367876,"velocity":0.9999446911428558,"acceleration":4.9242040270331036E-5,"pose":{"translation":{"x":6.12143439546543,"y":-0.7151037159902053},"rotation":{"radians":-2.0570427731088185E-4}},"curvature":-1.7071579130564232E-4},{"time":1.7225214395294832,"velocity":0.999949933778454,"acceleration":5.0734682359676405E-5,"pose":{"translation":{"x":6.22789544434293,"y":-0.7151265525774831},"rotation":{"radians":-2.2302157264049555E-4}},"curvature":-1.5453311203111758E-4},{"time":1.8282515334495566,"velocity":0.9999552979611849,"acceleration":5.284205054641302E-5,"pose":{"translation":{"x":6.333620525516261,"y":-0.7151509646254184},"rotation":{"radians":-2.384891161487098E-4}},"curvature":-1.3797542338492307E-4},{"time":1.932689830603459,"velocity":0.9999608166949621,"acceleration":5.6287027733918696E-5,"pose":{"translation":{"x":6.438054439104082,"y":-0.7151765928039965},"rotation":{"radians":-2.52016346465048E-4}},"curvature":-1.2094086552319517E-4},{"time":2.0352655612061343,"velocity":0.9999665903779553,"acceleration":6.188490438000034E-5,"pose":{"translation":{"x":6.540626443158164,"y":-0.7152030481816463},"rotation":{"radians":-2.6351930547014786E-4}},"curvature":-1.0311956260767918E-4},{"time":2.135405403849388,"velocity":0.9999727875225419,"acceleration":7.069238955792907E-5,"pose":{"translation":{"x":6.640763246852358,"y":-0.7152299220924245},"rotation":{"radians":-2.729056617250116E-4}},"curvature":-8.399141357141933E-5},{"time":2.2325464742950283,"velocity":0.999979654656936,"acceleration":8.420632541569917E-5,"pose":{"translation":{"x":6.737902003671551,"y":-0.7152567960032029},"rotation":{"radians":-2.8006132532287797E-4}},"curvature":-6.279554631299584E-5},{"time":2.3261493144579335,"velocity":0.9999875366081545,"acceleration":1.0464975058692262E-4,"pose":{"translation":{"x":6.831503304600633,"y":-0.7152832513808527},"rotation":{"radians":-2.848376935983519E-4}},"curvature":-3.8467738220686515E-5},{"time":2.4157108813665107,"velocity":0.9999969092037937,"acceleration":-6.272661685415307E-5,"pose":{"translation":{"x":6.9210641713134535,"y":-0.7153088795594308},"rotation":{"radians":-2.870387793125379E-4}},"curvature":-9.539523948400935E-6},{"time":2.5007782528032245,"velocity":0.9999915732153788,"acceleration":-1.8157570256514238E-4,"pose":{"translation":{"x":7.006131049361786,"y":-0.715333291607366},"rotation":{"radians":-2.8640782227810663E-4}},"curvature":2.6008813680473154E-5},{"time":2.580961267446303,"velocity":0.9999770139281612,"acceleration":-0.34334358581599045,"pose":{"translation":{"x":7.086312801364286,"y":-0.715356128194644},"rotation":{"radians":-2.826136513720897E-4}},"curvature":7.094629694583373E-5},{"time":2.656934801320935,"velocity":0.9738919883805325,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.161293700195457,"y":-0.7153770694599936},"rotation":{"radians":-2.7523841740668075E-4}},"curvature":1.290294163432336E-4},{"time":2.731182328814193,"velocity":0.8996444608872743,"acceleration":-1.0000000000000009,"pose":{"translation":{"x":7.230846422174607,"y":-0.7153958448780722},"rotation":{"radians":-2.6377107859051235E-4}},"curvature":2.0521978632845688E-4},{"time":2.805379689250417,"velocity":0.8254471004510505,"acceleration":-1.0,"pose":{"translation":{"x":7.294845040254814,"y":-0.715412243126652},"rotation":{"radians":-2.4761650980063257E-4}},"curvature":3.0569017364415825E-4},{"time":2.9536779192175344,"velocity":0.6771488704839332,"acceleration":-1.0000000000000002,"pose":{"translation":{"x":7.4062611988333185,"y":-0.7154374180450894},"rotation":{"radians":-1.9878917382344983E-4}},"curvature":6.041665401664144E-4},{"time":3.1045757838606516,"velocity":0.526251005840816,"acceleration":-0.9999999999999997,"pose":{"translation":{"x":7.497056433411484,"y":-0.7154524626528294},"rotation":{"radians":-1.264307214018874E-4}},"curvature":0.0010074652170135845},{"time":3.2721414624042318,"velocity":0.358685327297236,"acceleration":-1.0,"pose":{"translation":{"x":7.571198911703647,"y":-0.7154588241371141},"rotation":{"radians":-4.363729332648743E-5}},"curvature":0.001105121415441531},{"time":3.6308267897014677,"velocity":0.0,"acceleration":-1.0,"pose":{"translation":{"x":7.635526493704722,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 146225f..9a194ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -70,7 +70,7 @@ public final class Constants { public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; - public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.16, 0.0, 0.0, 0.058, 0, 1.0); + public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.05, 0, 1.0); /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 1.0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 7ad3d4f..084e7b8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -80,7 +80,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(false); + m_robotContainer.setDriveGearState(true); m_robotContainer.resetOdometry(new Pose2d()); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); @@ -93,7 +93,7 @@ public class Robot extends TimedRobot { // schedule the autonomous command (example) if (m_autonomousCommand != null) { - //m_autonomousCommand.schedule(); + m_autonomousCommand.schedule(); SmartDashboard.putString("Is Auto Start?", "YEA"); } } @@ -108,7 +108,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(false); + m_robotContainer.setDriveGearState(true); m_robotContainer.shiftClimberRachet(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3f4e391..184e156 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -259,9 +259,13 @@ public class RobotContainer { //RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + try { + return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + } catch (Exception e) { + System.err.println("ERROR"); + } - //return new InstantCommand(); + return new InstantCommand(); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index f47951f..c1e9f38 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -29,7 +29,6 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { // super(new FooCommand(), new BarCommand()); addCommands( - new Wait(drive, 0, 1), paths[0], paths[1] ); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d545e64..fe96ca2 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -342,7 +342,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); + m_driveTrain.arcadeDrive(-move, steer); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -573,7 +573,7 @@ public class Drive extends SubsystemBase { @Override public void reset() { // TODO Auto-generated method stub - resetGyroYaw(); + resetGyroYaw(0); } @Override From 7a0d9e4cdae9fcfed7df4a25d9fa641b66551f7d Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 9 Mar 2020 19:17:44 -0600 Subject: [PATCH 18/39] Added functionality to get time for trajectory --- PathWeaver/Groups/FirstPathGroup | 1 + PathWeaver/Paths/FirstPath2 | 3 +++ src/main/deploy/paths/FirstPath2.wpilib.json | 1 + src/main/java/frc4388/robot/RobotContainer.java | 10 ++++++++++ 4 files changed, 15 insertions(+) create mode 100644 PathWeaver/Paths/FirstPath2 create mode 100644 src/main/deploy/paths/FirstPath2.wpilib.json diff --git a/PathWeaver/Groups/FirstPathGroup b/PathWeaver/Groups/FirstPathGroup index 58e567a..1d5efec 100644 --- a/PathWeaver/Groups/FirstPathGroup +++ b/PathWeaver/Groups/FirstPathGroup @@ -1,2 +1,3 @@ +FirstPath2 FirstPath0 FirstPath1 diff --git a/PathWeaver/Paths/FirstPath2 b/PathWeaver/Paths/FirstPath2 new file mode 100644 index 0000000..db2f102 --- /dev/null +++ b/PathWeaver/Paths/FirstPath2 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +7.2,-0.715,1.5,0.0,true, +6.8264744033479925,-2.6310096463529717,-0.5948912429093554,-0.29744562145467723,true, diff --git a/src/main/deploy/paths/FirstPath2.wpilib.json b/src/main/deploy/paths/FirstPath2.wpilib.json new file mode 100644 index 0000000..25ef969 --- /dev/null +++ b/src/main/deploy/paths/FirstPath2.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.18574091419977973,"velocity":0.5015004683394053,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.246571637767237,"y":-0.71552330340216},"rotation":{"radians":-0.03359339040978542}},"curvature":-1.4405435131169915},{"time":0.2266410226892701,"velocity":0.6119307612610292,"acceleration":2.699999999999999,"pose":{"translation":{"x":7.269309656446672,"y":-0.716724580735192},"rotation":{"radians":-0.07482060995567351}},"curvature":-2.1920137727383837},{"time":0.26044483766611803,"velocity":0.7032010616985185,"acceleration":1.4167228940696506,"pose":{"translation":{"x":7.2914220857138154,"y":-0.7189906269967898},"rotation":{"radians":-0.1324433145295062}},"curvature":-3.007297576134003},{"time":0.29029013543616583,"velocity":0.745483578329671,"acceleration":-2.699999999999993,"pose":{"translation":{"x":7.312735725027567,"y":-0.7226066855070661},"rotation":{"radians":-0.206853162026066}},"curvature":-3.8886464823836606},{"time":0.30487021129889263,"velocity":0.7061173735003088,"acceleration":-2.669760925580698,"pose":{"translation":{"x":7.323043539168388,"y":-0.725000944213698},"rotation":{"radians":-0.2504088093160685}},"curvature":-4.344233814092265},{"time":0.32009331314664224,"velocity":0.6654753310210515,"acceleration":-2.20371219622657,"pose":{"translation":{"x":7.333094382263903,"y":-0.7278246052076687},"rotation":{"radians":-0.2981374104428669}},"curvature":-4.798040174521225},{"time":0.33600884596997993,"velocity":0.6304020772288179,"acceleration":-1.7722927303336353,"pose":{"translation":{"x":7.342871296984669,"y":-0.7311041261586553},"rotation":{"radians":-0.34989917578712737}},"curvature":-5.236702536281345},{"time":0.3525829471208297,"velocity":0.6010279182473525,"acceleration":-1.3699347820320806,"pose":{"translation":{"x":7.352358319786087,"y":-0.7348640257199522},"rotation":{"radians":-0.4054537862993204}},"curvature":-5.643480174935811},{"time":0.36976286076987,"velocity":0.5774925569872245,"acceleration":-0.9919241283235749,"pose":{"translation":{"x":7.361540463598101,"y":-0.7391269205615391},"rotation":{"radians":-0.4644440730100708}},"curvature":-5.999261987332941},{"time":0.3874752540937721,"velocity":0.5599232066788886,"acceleration":-0.633760498617797,"pose":{"translation":{"x":7.370403700514882,"y":-0.7439135624031483},"rotation":{"radians":-0.5263880359703766}},"curvature":-6.284353805966965},{"time":0.40562669827129005,"velocity":0.5484195383663117,"acceleration":-0.2909483020003185,"pose":{"translation":{"x":7.378934944484527,"y":-0.7492428750473332},"rotation":{"radians":-0.5906828726845003}},"curvature":-6.480915495174348},{"time":0.42410675665832054,"velocity":0.5430427967577385,"acceleration":0.040867278011913304,"pose":{"translation":{"x":7.3871220339987556,"y":-0.755131991412536},"rotation":{"radians":-0.6566233239679846}},"curvature":-6.57564300066678},{"time":0.4427936520640061,"velocity":0.5438064793074622,"acceleration":0.36551173046498503,"pose":{"translation":{"x":7.394953714782599,"y":-0.7615962905661549},"rotation":{"radians":-0.7234341130651554}},"curvature":-6.562074297460497},{"time":0.46156189622249166,"velocity":0.5506664927076196,"acceleration":0.6857461024631776,"pose":{"translation":{"x":7.402419622484099,"y":-0.7686494347576127},"rotation":{"radians":-0.7903131138986692}},"curvature":-6.44187668356058},{"time":0.48029075301778956,"velocity":0.5635097332585861,"acceleration":1.002705190054949,"pose":{"translation":{"x":7.409510265363997,"y":-0.7763034064514244},"rotation":{"radians":-0.8564791640564974}},"curvature":-6.224712372184579},{"time":0.4988721753143321,"velocity":0.582141421833932,"acceleration":1.3154389982154877,"pose":{"translation":{"x":7.4162170069854305,"y":-0.7845685453602647},"rotation":{"radians":-0.921217194032659}},"curvature":-5.926705551959653},{"time":0.5172170027973768,"velocity":0.6062729233206643,"acceleration":1.620684901595592,"pose":{"translation":{"x":7.422532048903632,"y":-0.7934535854780363},"rotation":{"radians":-0.9839140979634297}},"curvature":-5.567955823359643},{"time":0.5352586722305678,"velocity":0.6355127845706157,"acceleration":1.9129717995948965,"pose":{"translation":{"x":7.428448413355612,"y":-0.8029656921129377},"rotation":{"radians":-1.044081205566804}},"curvature":-5.169769119324686},{"time":0.5529542844520852,"velocity":0.6693639917269453,"acceleration":2.1851010903529495,"pose":{"translation":{"x":7.433959925949866,"y":-0.8131104989205302},"rotation":{"radians":-1.1013623608918062}},"curvature":-4.752235515342323},{"time":0.5702833868798093,"velocity":0.7072298323366033,"acceleration":2.4289676387655197,"pose":{"translation":{"x":7.4390611983560575,"y":-0.8238921449368072},"rotation":{"radians":-1.1555293832873916}},"curvature":-4.332545539867548},{"time":0.587245135472715,"velocity":0.7484293707656479,"acceleration":2.636597030345389,"pose":{"translation":{"x":7.443747610994719,"y":-0.8353133116112605},"rotation":{"radians":-1.2064683750474912}},"curvature":-3.9241463540123545},{"time":0.6038545747404133,"velocity":0.7922217690145634,"acceleration":2.699999999999996,"pose":{"translation":{"x":7.448015295726941,"y":-0.8473752598399495},"rotation":{"radians":-1.2541608106925795}},"curvature":-3.536616605499757},{"time":0.6362024768433319,"velocity":0.8795611046924436,"acceleration":2.7000000000000006,"pose":{"translation":{"x":7.4552826622574155,"y":-0.8734196639755125},"rotation":{"radians":-1.3400856851183165}},"curvature":-2.845585051317401},{"time":0.6677848651954601,"velocity":0.9648335532431899,"acceleration":2.7,"pose":{"translation":{"x":7.460846723855813,"y":-0.9020084406998853},"rotation":{"radians":-1.4143103108236423}},"curvature":-2.277572509988386},{"time":0.6989057195952202,"velocity":1.048859860122542,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.464701821619865,"y":-0.9331043146308698},"rotation":{"radians":-1.478233001222403}},"curvature":-1.8251885024621224},{"time":0.7297320912150541,"velocity":1.1320910634960937,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.466852657906909,"y":-0.9666508367058511},"rotation":{"radians":-1.5333229489020965}},"curvature":-1.4707911285203954},{"time":0.7603482420772625,"velocity":1.2147546708240562,"acceleration":2.4689382496452787,"pose":{"translation":{"x":7.467313742404098,"y":-1.0025735692399675},"rotation":{"radians":-1.5809522845751747}},"curvature":-1.1950333509651316},{"time":0.7908701889907861,"velocity":1.2901114730124972,"acceleration":1.6478061425997925,"pose":{"translation":{"x":7.466108838198616,"y":-1.040781270984282},"rotation":{"radians":-1.6223272462013492}},"curvature":-0.9806036708623775},{"time":0.8528650627556515,"velocity":1.3922670068119412,"acceleration":1.1265054842673399,"pose":{"translation":{"x":7.458839059449773,"y":-1.1236097096364015},"rotation":{"radians":-1.6902497191987305}},"curvature":-0.6821918383506778},{"time":0.9169237989671473,"velocity":1.4644295244694259,"acceleration":0.7553834508087204,"pose":{"translation":{"x":7.445397445026421,"y":-1.2141151835996804},"rotation":{"radians":-1.743407706342333}},"curvature":-0.4964864179346202},{"time":0.9832824629764142,"velocity":1.5145557610798024,"acceleration":0.5047361844321614,"pose":{"translation":{"x":7.426250721188545,"y":-1.3110837105092963},"rotation":{"radians":-1.7861325178852274}},"curvature":-0.37790537450487255},{"time":1.051770787169082,"velocity":1.5491242965109624,"acceleration":0.3362979412462314,"pose":{"translation":{"x":7.40196049133683,"y":-1.4131462165595934},"rotation":{"radians":-1.821408517045324}},"curvature":-0.3005993467136097},{"time":1.1219365275677993,"velocity":1.5727208905530685,"acceleration":0.2198034851609914,"pose":{"translation":{"x":7.373165510259402,"y":-1.5188164583655404},"rotation":{"radians":-1.8513381322664064}},"curvature":-0.24978161090879322},{"time":1.1931386100223174,"velocity":1.5883713564272919,"acceleration":0.13404480115479672,"pose":{"translation":{"x":7.340563958378583,"y":-1.6265289448241926},"rotation":{"radians":-1.8774525545327976}},"curvature":-0.2169095086856791},{"time":1.2646178303499827,"velocity":1.5979527743028137,"acceleration":0.06404129568110227,"pose":{"translation":{"x":7.304895715997633,"y":-1.7346768589761474},"rotation":{"radians":-1.9009171561391245}},"curvature":-0.19710255409574393},{"time":1.3355531046387532,"velocity":1.602495561177761,"acceleration":-0.002096322275086082,"pose":{"translation":{"x":7.2669246375475,"y":-1.8416499798670074},"rotation":{"radians":-1.9226712973054454}},"curvature":-0.1877943585524745},{"time":1.405109094208867,"velocity":1.6023497494074594,"acceleration":-0.07612871060578774,"pose":{"translation":{"x":7.227420825833567,"y":-1.9458726044088372},"rotation":{"radians":-1.943529946970129}},"curvature":-0.18809230783361033},{"time":1.4724793462205135,"velocity":1.5972209389886258,"acceleration":-0.17301087241769844,"pose":{"translation":{"x":7.187142906282408,"y":-2.045841469241623},"rotation":{"radians":-1.9642661669856805}},"curvature":-0.19860704074543745},{"time":1.5369281583145993,"velocity":1.5860705937819437,"acceleration":-0.31554579820498185,"pose":{"translation":{"x":7.146820301188521,"y":-2.1401636725947344},"rotation":{"radians":-1.98568919023132}},"curvature":-0.22170133611162526},{"time":1.5978341143112027,"velocity":1.566851975281558,"acceleration":-1.3309955874047237,"pose":{"translation":{"x":7.107135503961091,"y":-2.2275945961483803},"rotation":{"radians":-2.0087318859865846}},"curvature":-0.26227793056302745},{"time":1.6555960479201848,"velocity":1.4899710965280382,"acceleration":-2.7,"pose":{"translation":{"x":7.068706353370726,"y":-2.3070758268950673},"rotation":{"radians":-2.034563405456838}},"curvature":-0.3294600185346016},{"time":1.7119114730269265,"velocity":1.3379194487398354,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.032068307796214,"y":-2.377773079001069},"rotation":{"radians":-2.064747104509842}},"curvature":-0.4399216872784071},{"time":1.7676117337250405,"velocity":1.1875287448549274,"acceleration":-2.7,"pose":{"translation":{"x":6.997656719471268,"y":-2.439114115667869},"rotation":{"radians":-2.101467131011381}},"curvature":-0.6243017215544429},{"time":1.8221431594281243,"velocity":1.040293895456601,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.965789108731271,"y":-2.490826670993634},"rotation":{"radians":-2.1478338626356113}},"curvature":-0.9388068028845457},{"time":1.8750311267951567,"velocity":0.8974963835656139,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.936647438260028,"y":-2.5329763718346676},"rotation":{"radians":-2.2081880495457282}},"curvature":-1.4818665318907704},{"time":1.9260491103410267,"velocity":0.7597478279917649,"acceleration":-2.700000000000004,"pose":{"translation":{"x":6.910260387336512,"y":-2.5660046596668717},"rotation":{"radians":-2.2879536229403787}},"curvature":-2.39507742000404},{"time":1.9509462729406097,"velocity":0.6925254889728903,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":6.898062530851734,"y":-2.5793483183090196},"rotation":{"radians":-2.336664233442283}},"curvature":-3.0225092551252013},{"time":1.9756123368228327,"velocity":0.6259271164908882,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.8864856260816145,"y":-2.5907667124471985},"rotation":{"radians":-2.391450278038916}},"curvature":-3.7370486015184476},{"time":2.000334632533119,"velocity":0.5591769180731152,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.875482253478922,"y":-2.6004376072065902},"rotation":{"radians":-2.4513985995970358}},"curvature":-4.444436732694084},{"time":2.0256138831254242,"velocity":0.49092294147389115,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.864992089704889,"y":-2.6085693664751215},"rotation":{"radians":-2.5140796747043557}},"curvature":-4.951976919791685},{"time":2.052333259353671,"velocity":0.4187806256576248,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.854941353699419,"y":-2.61540213796164},"rotation":{"radians":-2.5750408517708028}},"curvature":-4.970238408366828},{"time":2.082203426917947,"velocity":0.3381311732340795,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.845242252751303,"y":-2.6212090382540865},"rotation":{"radians":-2.627762206688829}},"curvature":-4.195902581911151},{"time":2.207437194782421,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.8264744033479845,"y":-2.6310096463529753},"rotation":{"radians":-2.677945044589006}},"curvature":-8.61988124756193E-14}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 184e156..a21de35 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,6 +92,7 @@ public class RobotContainer { /* Autos */ SixBallAutoMiddle m_sixBallAutoMiddle; + double m_totalTimeAuto; /** @@ -260,6 +261,7 @@ public class RobotContainer { // Run path following command, then stop at the end. try { + SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); @@ -304,7 +306,9 @@ public class RobotContainer { public RamseteCommand[] buildPaths(String[] paths) { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; + double[] times = new double[paths.length]; Trajectory initialTrajectory; + m_totalTimeAuto = 0; try { if (true) { @@ -315,6 +319,7 @@ public class RobotContainer { initialTrajectory = trajectory; RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); ramseteCommands[0] = ramseteCommand; + times[0] = initialTrajectory.getTotalTimeSeconds(); } for(int i = 1; i < paths.length; i++) { @@ -324,11 +329,16 @@ public class RobotContainer { Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); ramseteCommands[i] = ramseteCommand; + times[i] = trajectory.getTotalTimeSeconds(); } } catch (Exception e) { DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); } + for (int i = 0; i < times.length; i++) { + m_totalTimeAuto += times[i]; + } + return ramseteCommands; } From 0c925451dc1f62411b8dd918de1d72c5fcd42217 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 9 Mar 2020 19:53:07 -0600 Subject: [PATCH 19/39] Added EightballAutoMid Co-Authored-By: kyrarivera --- PathWeaver/Groups/EightBallMid | 3 ++ PathWeaver/Groups/FirstPathGroup | 3 -- PathWeaver/Groups/SixBallMid | 2 ++ .../Paths/{FirstPath0 => EightBallMid0} | 0 .../Paths/{FirstPath1 => EightBallMid1} | 0 PathWeaver/Paths/EightBallMid2 | 3 ++ PathWeaver/Paths/EightBallMidComplete | 5 ++++ PathWeaver/Paths/FirstPath2 | 3 -- PathWeaver/Paths/SixBallMid0 | 3 ++ PathWeaver/Paths/SixBallMid1 | 3 ++ PathWeaver/Paths/SixBallMidComplete | 4 +++ ....wpilib.json => EightBallMid0.wpilib.json} | 0 ....wpilib.json => EightBallMid1.wpilib.json} | 0 .../deploy/paths/EightBallMid2.wpilib.json | 1 + .../paths/EightBallMidComplete.wpilib.json | 1 + src/main/deploy/paths/FirstPath2.wpilib.json | 1 - src/main/deploy/paths/SixBallMid0.wpilib.json | 1 + src/main/deploy/paths/SixBallMid1.wpilib.json | 1 + .../paths/SixBallMidComplete.wpilib.json | 1 + .../java/frc4388/robot/RobotContainer.java | 14 +++++++-- .../commands/auto/EightBallAutoMiddle.java | 29 +++++++++++++++++++ .../commands/auto/SixBallAutoMiddle.java | 3 +- 22 files changed, 70 insertions(+), 11 deletions(-) create mode 100644 PathWeaver/Groups/EightBallMid delete mode 100644 PathWeaver/Groups/FirstPathGroup create mode 100644 PathWeaver/Groups/SixBallMid rename PathWeaver/Paths/{FirstPath0 => EightBallMid0} (100%) rename PathWeaver/Paths/{FirstPath1 => EightBallMid1} (100%) create mode 100644 PathWeaver/Paths/EightBallMid2 create mode 100644 PathWeaver/Paths/EightBallMidComplete delete mode 100644 PathWeaver/Paths/FirstPath2 create mode 100644 PathWeaver/Paths/SixBallMid0 create mode 100644 PathWeaver/Paths/SixBallMid1 create mode 100644 PathWeaver/Paths/SixBallMidComplete rename src/main/deploy/paths/{FirstPath0.wpilib.json => EightBallMid0.wpilib.json} (100%) rename src/main/deploy/paths/{FirstPath1.wpilib.json => EightBallMid1.wpilib.json} (100%) create mode 100644 src/main/deploy/paths/EightBallMid2.wpilib.json create mode 100644 src/main/deploy/paths/EightBallMidComplete.wpilib.json delete mode 100644 src/main/deploy/paths/FirstPath2.wpilib.json create mode 100644 src/main/deploy/paths/SixBallMid0.wpilib.json create mode 100644 src/main/deploy/paths/SixBallMid1.wpilib.json create mode 100644 src/main/deploy/paths/SixBallMidComplete.wpilib.json create mode 100644 src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java diff --git a/PathWeaver/Groups/EightBallMid b/PathWeaver/Groups/EightBallMid new file mode 100644 index 0000000..e87dc39 --- /dev/null +++ b/PathWeaver/Groups/EightBallMid @@ -0,0 +1,3 @@ +EightBallMid0 +EightBallMid1 +EightBallMid2 diff --git a/PathWeaver/Groups/FirstPathGroup b/PathWeaver/Groups/FirstPathGroup deleted file mode 100644 index 1d5efec..0000000 --- a/PathWeaver/Groups/FirstPathGroup +++ /dev/null @@ -1,3 +0,0 @@ -FirstPath2 -FirstPath0 -FirstPath1 diff --git a/PathWeaver/Groups/SixBallMid b/PathWeaver/Groups/SixBallMid new file mode 100644 index 0000000..ed5971c --- /dev/null +++ b/PathWeaver/Groups/SixBallMid @@ -0,0 +1,2 @@ +SixBallMid0 +SixBallMid1 diff --git a/PathWeaver/Paths/FirstPath0 b/PathWeaver/Paths/EightBallMid0 similarity index 100% rename from PathWeaver/Paths/FirstPath0 rename to PathWeaver/Paths/EightBallMid0 diff --git a/PathWeaver/Paths/FirstPath1 b/PathWeaver/Paths/EightBallMid1 similarity index 100% rename from PathWeaver/Paths/FirstPath1 rename to PathWeaver/Paths/EightBallMid1 diff --git a/PathWeaver/Paths/EightBallMid2 b/PathWeaver/Paths/EightBallMid2 new file mode 100644 index 0000000..671f3d6 --- /dev/null +++ b/PathWeaver/Paths/EightBallMid2 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +7.2,-0.715,1.5,0.0,true, +6.8,-2.6,-0.6,-0.3,true, diff --git a/PathWeaver/Paths/EightBallMidComplete b/PathWeaver/Paths/EightBallMidComplete new file mode 100644 index 0000000..22711d6 --- /dev/null +++ b/PathWeaver/Paths/EightBallMidComplete @@ -0,0 +1,5 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.4,0.2,2.5,true, +5.0,-0.715,3.0,0.0,true, +7.2,-0.715,1.5,0.0,true, +6.8,-2.6,-0.6,-0.3,true, diff --git a/PathWeaver/Paths/FirstPath2 b/PathWeaver/Paths/FirstPath2 deleted file mode 100644 index db2f102..0000000 --- a/PathWeaver/Paths/FirstPath2 +++ /dev/null @@ -1,3 +0,0 @@ -X,Y,Tangent X,Tangent Y,Fixed Theta,Name -7.2,-0.715,1.5,0.0,true, -6.8264744033479925,-2.6310096463529717,-0.5948912429093554,-0.29744562145467723,true, diff --git a/PathWeaver/Paths/SixBallMid0 b/PathWeaver/Paths/SixBallMid0 new file mode 100644 index 0000000..18dd1ec --- /dev/null +++ b/PathWeaver/Paths/SixBallMid0 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.4,0.2,2.5,true, +5.006107200045366,-0.7154598441848491,2.0,0.0,true, diff --git a/PathWeaver/Paths/SixBallMid1 b/PathWeaver/Paths/SixBallMid1 new file mode 100644 index 0000000..4ae0aad --- /dev/null +++ b/PathWeaver/Paths/SixBallMid1 @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +5.006,-0.715,3.048,0.0,true, +7.2,-0.7154598441848491,1.0,0.0,true, diff --git a/PathWeaver/Paths/SixBallMidComplete b/PathWeaver/Paths/SixBallMidComplete new file mode 100644 index 0000000..5fa9b7c --- /dev/null +++ b/PathWeaver/Paths/SixBallMidComplete @@ -0,0 +1,4 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.4,0.2,2.5,true, +5.0,-0.715,3.0,0.0,true, +7.2,-0.715,1.5,0.0,true, diff --git a/src/main/deploy/paths/FirstPath0.wpilib.json b/src/main/deploy/paths/EightBallMid0.wpilib.json similarity index 100% rename from src/main/deploy/paths/FirstPath0.wpilib.json rename to src/main/deploy/paths/EightBallMid0.wpilib.json diff --git a/src/main/deploy/paths/FirstPath1.wpilib.json b/src/main/deploy/paths/EightBallMid1.wpilib.json similarity index 100% rename from src/main/deploy/paths/FirstPath1.wpilib.json rename to src/main/deploy/paths/EightBallMid1.wpilib.json diff --git a/src/main/deploy/paths/EightBallMid2.wpilib.json b/src/main/deploy/paths/EightBallMid2.wpilib.json new file mode 100644 index 0000000..ee76706 --- /dev/null +++ b/src/main/deploy/paths/EightBallMid2.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999993,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1857265193636004,"velocity":0.501461602281721,"acceleration":2.699999999999999,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":0.22660073611189058,"velocity":0.6118219875021044,"acceleration":2.7000000000000015,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":0.26036037214234553,"velocity":0.7029730047843328,"acceleration":1.4538431600368555,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":0.2901133482323059,"velocity":0.7462291655634617,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":0.30461963577730317,"velocity":0.7070621891919691,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":0.31975062184802994,"velocity":0.6662085268010068,"acceleration":-2.302451123186552,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":0.33557102199759564,"velocity":0.6297828287073786,"acceleration":-1.8675460534492743,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":0.3520637919772221,"velocity":0.5989818212214805,"acceleration":-1.4613709602031215,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":0.369181463367336,"velocity":0.5739665533456683,"acceleration":-1.079676100951105,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":0.38685538744616993,"velocity":0.554884439907727,"acceleration":-0.718302000748962,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":0.40499556745388565,"velocity":0.5418543123142384,"acceleration":-0.3729250173328537,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":0.4234931432073012,"velocity":0.5349561035557802,"acceleration":-0.03916357131337384,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":0.4422256519746503,"velocity":0.5342224716127917,"acceleration":0.28704156286608506,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":0.46106457684825486,"velocity":0.5396300260512279,"acceleration":0.6088741709257178,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":0.4798841040036331,"velocity":0.5510887500451729,"acceleration":0.9280335229053567,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":0.49856967238109146,"velocity":0.5684295838939945,"acceleration":1.244199218395254,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":0.5170249585196953,"velocity":0.5913916364829062,"acceleration":1.55470303079482,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":0.5351763702192629,"velocity":0.6196116912654284,"acceleration":1.8545226594628177,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":0.5529747450744766,"velocity":0.6526191807360355,"acceleration":2.1366633368747885,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":0.5703945330851697,"velocity":0.6898394031146146,"acceleration":2.3929144893008223,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":0.5874311208341066,"velocity":0.7306065007872906,"acceleration":2.614876812845669,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":0.6040970809732634,"velocity":0.7741859335189818,"acceleration":2.700000000000003,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":0.6204335317180859,"velocity":0.8182943505300027,"acceleration":2.699999999999989,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":0.6365062512871126,"velocity":0.8616906933663744,"acceleration":2.700000000000001,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":0.6680852946379339,"velocity":0.9469541104135919,"acceleration":2.7000000000000024,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":0.6991702640578552,"velocity":1.0308835278473796,"acceleration":2.7,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":0.7299416895512951,"velocity":1.1139663766796675,"acceleration":2.6999999999999993,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":0.7604917708724955,"velocity":1.1964515962469084,"acceleration":2.6999999999999957,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":0.7908583645653527,"velocity":1.2784413992176227,"acceleration":1.7317501674300086,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":0.8522944364989423,"velocity":1.3848333270748585,"acceleration":1.1714925683775692,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":0.9156777853297742,"velocity":1.4590864491890612,"acceleration":0.7858676493625434,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":0.9813100267777496,"velocity":1.5106647044981765,"acceleration":0.5250113170323235,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":1.0490457248506553,"velocity":1.5462267125535365,"acceleration":0.34981100784937563,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":1.1184519520705818,"velocity":1.5705057748483617,"acceleration":0.2288975554736658,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":1.1889035211319336,"velocity":1.5866319667857893,"acceleration":0.14022072554540496,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":1.2596548250824198,"velocity":1.59655276595901,"acceleration":0.06824791072810449,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":1.3298966361891713,"velocity":1.601346622812804,"acceleration":7.56678468946936E-4,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":1.398803911610538,"velocity":1.601398763464469,"acceleration":-0.07418428163614217,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":1.4655788655112532,"velocity":1.596445111478058,"acceleration":-0.1715480180400782,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":1.5294925579236458,"velocity":1.5854808442190889,"acceleration":-0.31394105304156766,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":1.5899279221203853,"velocity":1.5665077023422138,"acceleration":-1.3509793267586743,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":1.647297847739518,"velocity":1.4890021188530829,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":1.7032812155507002,"velocity":1.3378470257628907,"acceleration":-2.699999999999999,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":1.7586823776797251,"velocity":1.1882638880145233,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":1.8129609537084448,"velocity":1.0417117327369803,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":1.8656590466798884,"velocity":0.8994268817140824,"acceleration":-2.7,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":1.9165692937923622,"velocity":0.7619692145104032,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":1.9414487107808738,"velocity":0.6947947886414217,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":1.9661233789888195,"velocity":0.628173184479968,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":1.9908811602880785,"velocity":0.5613271749719686,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":2.0162223622736977,"velocity":0.4929059296107963,"acceleration":-2.7,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":2.0430294857607096,"velocity":0.42052669619586397,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":2.073014782085448,"velocity":0.33956639611906964,"acceleration":-2.7,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":2.1987801139813996,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMidComplete.wpilib.json b/src/main/deploy/paths/EightBallMidComplete.wpilib.json new file mode 100644 index 0000000..9aca47f --- /dev/null +++ b/src/main/deploy/paths/EightBallMidComplete.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24104312286019983,"velocity":0.6508164317225397,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.2063923239707948,"y":-2.3218234953284265},"rotation":{"radians":1.4857527840978701}},"curvature":-0.13003316961730046},{"time":0.34125642343054197,"velocity":0.9213923432624636,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.2136058807373047,"y":-2.2433763408660887},"rotation":{"radians":1.4709813552860977}},"curvature":-0.2416411635394689},{"time":0.41862951975667484,"velocity":1.1302997033430222,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.222373878955841,"y":-2.1644892266392706},"rotation":{"radians":1.4480070351506305}},"curvature":-0.33393289305160856},{"time":0.4843752197211856,"velocity":1.3078130932472012,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.23333740234375,"y":-2.0850949096679687},"rotation":{"radians":1.418153476722264}},"curvature":-0.40799291240160246},{"time":0.5428188459431846,"velocity":1.4656108840465984,"acceleration":-0.12363267963904842,"pose":{"translation":{"x":3.247049701213837,"y":-2.005218879878521},"rotation":{"radians":1.3826311795834407}},"curvature":-0.4659998167211871},{"time":0.5989114443624675,"velocity":1.4586760057961055,"acceleration":-0.23878049786970917,"pose":{"translation":{"x":3.263980484008789,"y":-1.9249700260162352},"rotation":{"radians":1.3424938853817683}},"curvature":-0.5106186292535235},{"time":0.656093483931331,"velocity":1.4450220499186468,"acceleration":-0.17582458312750424,"pose":{"translation":{"x":3.284520208835602,"y":-1.8445313015580176},"rotation":{"radians":1.2986210341256466}},"curvature":-0.5446069018648376},{"time":0.7144460101119858,"velocity":1.4347622413284964,"acceleration":-0.13307656507771634,"pose":{"translation":{"x":3.308984375,"y":-1.7641503906249998},"rotation":{"radians":1.2517175479138372}},"curvature":-0.5705719275620846},{"time":0.7738450156145333,"velocity":1.426857625707185,"acceleration":-0.10571177853926464,"pose":{"translation":{"x":3.337617814540863,"y":-1.6841303738951683},"rotation":{"radians":1.2023248883292088}},"curvature":-0.5908312113511963},{"time":0.8341780829208127,"velocity":1.420479709857509,"acceleration":-0.08976520784153569,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6048203945159911},"rotation":{"radians":1.1508391887199425}},"curvature":-0.6073419696862633},{"time":0.8953429670423647,"velocity":1.4149892313217345,"acceleration":-0.08191722568263685,"pose":{"translation":{"x":3.4080442547798158,"y":-1.5266063240170478},"rotation":{"radians":1.0975335619661353}},"curvature":-0.621674601346085},{"time":0.9572461651384423,"velocity":1.4099182930728211,"acceleration":-0.07932481308319937,"pose":{"translation":{"x":3.45001220703125,"y":-1.449901428222656},"rotation":{"radians":1.0425826601190191}},"curvature":-0.6350112021866137},{"time":1.0198016941975718,"velocity":1.404956087422885,"acceleration":-0.07948863640008091,"pose":{"translation":{"x":3.4965079188346864,"y":-1.3751370331645012},"rotation":{"radians":0.9860883281959857}},"curvature":-0.6481550334261905},{"time":1.0829301018210549,"velocity":1.3999380963827859,"acceleration":-0.08015206912118296,"pose":{"translation":{"x":3.547487258911133,"y":-1.3027531909942627},"rotation":{"radians":0.9281057752826144}},"curvature":-0.6615413844839425},{"time":1.146557600543031,"velocity":1.394838220707214,"acceleration":-0.07923440840802286,"pose":{"translation":{"x":3.602861177921295,"y":-1.233189345896244},"rotation":{"radians":0.8686700639277289}},"curvature":-0.6752448631775445},{"time":1.2106151395445883,"velocity":1.3897626595003518,"acceleration":-0.07480385075761357,"pose":{"translation":{"x":3.6624999999999996,"y":-1.1668749999999999},"rotation":{"radians":0.8078228647022838}},"curvature":-0.6889828628777236},{"time":1.2750372072434193,"velocity":1.3849436407627116,"acceleration":-0.06509475812905682,"pose":{"translation":{"x":3.726237714290619,"y":-1.1042203792929648},"rotation":{"radians":0.745639315828271}},"curvature":-0.7021196688913226},{"time":1.3397601915745783,"velocity":1.380730513752284,"acceleration":-0.04856760891892339,"pose":{"translation":{"x":3.793876266479492,"y":-1.045607099533081},"rotation":{"radians":0.68225447169036}},"curvature":-0.71367992457469},{"time":1.4047202049596175,"velocity":1.3775755612268314,"acceleration":-0.023998991327275822,"pose":{"translation":{"x":3.8651898503303523,"y":-0.9913788321614265},"rotation":{"radians":0.6178882696500003}},"curvature":-0.7223829927855778},{"time":1.4698503918277483,"velocity":1.3760125024370393,"acceleration":0.009425020866650225,"pose":{"translation":{"x":3.9399291992187493,"y":-0.9418319702148437},"rotation":{"radians":0.5528673168364797}},"curvature":-0.7267095396737598},{"time":1.535077848801426,"velocity":1.3766272725800948,"acceleration":0.05205176529824429,"pose":{"translation":{"x":4.017825877666472,"y":-0.8972062942385672},"rotation":{"radians":0.48764131303933056}},"curvature":-0.7250066836520244},{"time":1.6003203604517349,"velocity":1.3800232604839846,"acceleration":0.10378762257480224,"pose":{"translation":{"x":4.098596572875976,"y":-0.8576756381988526},"rotation":{"radians":0.4227918675196265}},"curvature":-0.715627451832997},{"time":1.665483145837002,"velocity":1.3867863510594736,"acceleration":0.16424958779401677,"pose":{"translation":{"x":4.1819473862648,"y":-0.8233385553956034},"rotation":{"radians":0.3590321072445168}},"curvature":-0.6970855981440633},{"time":1.7304557025837932,"velocity":1.3974580667230574,"acceleration":0.2330731468720872,"pose":{"translation":{"x":4.267578124999998,"y":-0.794208984375},"rotation":{"radians":0.29719692634159106}},"curvature":-0.668192786058002},{"time":1.7951086483327998,"velocity":1.4125269322433287,"acceleration":0.31039116241082515,"pose":{"translation":{"x":4.35518659353256,"y":-0.7702069148421291},"rotation":{"radians":0.23822580998113968}},"curvature":-0.6281385045136234},{"time":1.859290265714348,"velocity":1.4324483390677942,"acceleration":0.3974925676686374,"pose":{"translation":{"x":4.444472885131834,"y":-0.751149053573609},"rotation":{"radians":0.18314236815259385}},"curvature":-0.5764792409963189},{"time":1.922822358232916,"velocity":1.4577018736523613,"acceleration":0.4976892074994749,"pose":{"translation":{"x":4.53514367341995,"y":-0.7367394903302198},"rotation":{"radians":0.1330363391804617}},"curvature":-0.5130224065785399},{"time":1.9854951125589597,"velocity":1.4888934270846992,"acceleration":0.6174792927963386,"pose":{"translation":{"x":4.626916503906247,"y":-0.7265603637695319},"rotation":{"radians":0.08905427201814982}},"curvature":-0.43761593999910725},{"time":2.047060989445588,"velocity":1.5269090812050408,"acceleration":0.7682064361829994,"pose":{"translation":{"x":4.7195240855216944,"y":-0.7200625273585326},"rotation":{"radians":0.052404113332160406}},"curvature":-0.3498775646988902},{"time":2.107228253203399,"velocity":1.5731299604713114,"acceleration":0.9685643952029747,"pose":{"translation":{"x":4.812718582153316,"y":-0.7165562152862552},"rotation":{"radians":0.024376580770117368}},"curvature":-0.24891407952009473},{"time":2.1656556590359175,"velocity":1.6297206654647636,"acceleration":1.248398336461174,"pose":{"translation":{"x":4.906275904178614,"y":-0.7152017083764088},"rotation":{"radians":0.0063825391669578325}},"curvature":-0.13309736505149056},{"time":2.221951259953214,"velocity":1.6999999999999984,"acceleration":2.843979367649469E-14,"pose":{"translation":{"x":4.999999999999995,"y":-0.7150000000000012},"rotation":{"radians":-2.0724163126336422E-15}},"curvature":-2.9605947323338176E-15},{"time":2.2770632519617626,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3319714486775105,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.386489936201094,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.440451733873254,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.49370816316683,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5461282165787624,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.597597926522085,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6480197342179226,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6973118585874882,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.7454076651440773,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.792255034885068,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8378157331839153,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.882064778682146,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.924989812181359,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9665904655352184,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.006877730541452,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0458733278338475,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.083609075774248,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.1554749990386948,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.222908019686529,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.286461829806158,"velocity":1.7,"acceleration":-0.8218715729622541,"pose":{"translation":{"x":6.80966796875,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.347697529046253,"velocity":1.6496721195440995,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.912227725982667,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.4105333918156933,"velocity":1.4800152900666113,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.010556030273439,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.479345141764698,"velocity":1.2942235652042982,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.106006145477297,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.5585075755357845,"velocity":1.0804849940223653,"acceleration":-2.7,"pose":{"translation":{"x":7.200000000000003,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.6042166199637675,"velocity":0.9570705740668113,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":3.628845057991022,"velocity":0.8905737913932243,"acceleration":-2.700000000000001,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":3.654785556334576,"velocity":0.8205344458656272,"acceleration":-2.700000000000001,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":3.682306030520562,"velocity":0.7462291655634643,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":3.6968123180655597,"velocity":0.7070621891919708,"acceleration":-2.700000000000004,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":3.711943304136287,"velocity":0.6662085268010068,"acceleration":-2.3024511231865272,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":3.727763704285853,"velocity":0.6297828287073786,"acceleration":-1.8675460534493051,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":3.7442564742654794,"velocity":0.5989818212214805,"acceleration":-1.461370960203089,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":3.761374145655594,"velocity":0.5739665533456683,"acceleration":-1.079676100951081,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":3.779048069734428,"velocity":0.554884439907727,"acceleration":-0.718302000748944,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":3.797188249742144,"velocity":0.5418543123142384,"acceleration":-0.37292501733286726,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":3.815685825495559,"velocity":0.5349561035557802,"acceleration":-0.03916357131337287,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":3.8344183342629083,"velocity":0.5342224716127917,"acceleration":0.2870415628660772,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":3.8532572591365133,"velocity":0.5396300260512279,"acceleration":0.6088741709257113,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":3.872076786291892,"velocity":0.5510887500451729,"acceleration":0.9280335229053246,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":3.890762354669351,"velocity":0.5684295838939945,"acceleration":1.2441992183952022,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":3.9092176408079555,"velocity":0.5913916364829062,"acceleration":1.5547030307947924,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":3.9273690525075233,"velocity":0.6196116912654284,"acceleration":1.8545226594627813,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":3.9451674273627373,"velocity":0.6526191807360355,"acceleration":2.136663336874829,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":3.9625872153734303,"velocity":0.6898394031146146,"acceleration":2.3929144893007783,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":3.9796238031223674,"velocity":0.7306065007872906,"acceleration":2.614876812845623,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":3.9962897632615246,"velocity":0.7741859335189818,"acceleration":2.7000000000000375,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":4.012626214006347,"velocity":0.8182943505300027,"acceleration":2.7000000000000224,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":4.028698933575373,"velocity":0.8616906933663744,"acceleration":2.6999999999999695,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":4.060277976926194,"velocity":0.9469541104135919,"acceleration":2.6999999999999877,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":4.091362946346115,"velocity":1.0308835278473796,"acceleration":2.7,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":4.1221343718395556,"velocity":1.1139663766796675,"acceleration":2.699999999999995,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":4.152684453160756,"velocity":1.1964515962469084,"acceleration":2.7000000000000197,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":4.183051046853612,"velocity":1.2784413992176227,"acceleration":1.731750167430012,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":4.244487118787202,"velocity":1.3848333270748585,"acceleration":1.171492568377572,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":4.3078704676180335,"velocity":1.4590864491890612,"acceleration":0.7858676493625462,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":4.373502709066009,"velocity":1.5106647044981765,"acceleration":0.5250113170323213,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":4.441238407138915,"velocity":1.5462267125535365,"acceleration":0.34981100784937635,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":4.510644634358841,"velocity":1.5705057748483617,"acceleration":0.22889755547366647,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":4.581096203420193,"velocity":1.5866319667857893,"acceleration":0.14022072554540496,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":4.651847507370679,"velocity":1.59655276595901,"acceleration":0.06824791072810449,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":4.722089318477431,"velocity":1.601346622812804,"acceleration":7.56678468946939E-4,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":4.790996593898797,"velocity":1.601398763464469,"acceleration":-0.07418428163614232,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":4.857771547799512,"velocity":1.596445111478058,"acceleration":-0.17154801804007855,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":4.921685240211905,"velocity":1.5854808442190889,"acceleration":-0.3139410530415669,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":4.982120604408644,"velocity":1.5665077023422138,"acceleration":-1.3509793267586387,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":5.039490530027777,"velocity":1.4890021188530849,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":5.09547389783896,"velocity":1.3378470257628925,"acceleration":-2.7,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":5.1508750599679844,"velocity":1.1882638880145262,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":5.205153635996704,"velocity":1.0417117327369836,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":5.257851728968148,"velocity":0.8994268817140864,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":5.308761976080621,"velocity":0.7619692145104063,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":5.333641393069134,"velocity":0.6947947886414234,"acceleration":-2.7,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":5.35831606127708,"velocity":0.6281731844799698,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":5.383073842576339,"velocity":0.5613271749719707,"acceleration":-2.7,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":5.408415044561957,"velocity":0.4929059296108012,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":5.43522216804897,"velocity":0.42052669619586686,"acceleration":-2.7,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":5.46520746437371,"velocity":0.33956639611906964,"acceleration":-2.7,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":5.590972796269662,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/FirstPath2.wpilib.json b/src/main/deploy/paths/FirstPath2.wpilib.json deleted file mode 100644 index 25ef969..0000000 --- a/src/main/deploy/paths/FirstPath2.wpilib.json +++ /dev/null @@ -1 +0,0 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.18574091419977973,"velocity":0.5015004683394053,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.246571637767237,"y":-0.71552330340216},"rotation":{"radians":-0.03359339040978542}},"curvature":-1.4405435131169915},{"time":0.2266410226892701,"velocity":0.6119307612610292,"acceleration":2.699999999999999,"pose":{"translation":{"x":7.269309656446672,"y":-0.716724580735192},"rotation":{"radians":-0.07482060995567351}},"curvature":-2.1920137727383837},{"time":0.26044483766611803,"velocity":0.7032010616985185,"acceleration":1.4167228940696506,"pose":{"translation":{"x":7.2914220857138154,"y":-0.7189906269967898},"rotation":{"radians":-0.1324433145295062}},"curvature":-3.007297576134003},{"time":0.29029013543616583,"velocity":0.745483578329671,"acceleration":-2.699999999999993,"pose":{"translation":{"x":7.312735725027567,"y":-0.7226066855070661},"rotation":{"radians":-0.206853162026066}},"curvature":-3.8886464823836606},{"time":0.30487021129889263,"velocity":0.7061173735003088,"acceleration":-2.669760925580698,"pose":{"translation":{"x":7.323043539168388,"y":-0.725000944213698},"rotation":{"radians":-0.2504088093160685}},"curvature":-4.344233814092265},{"time":0.32009331314664224,"velocity":0.6654753310210515,"acceleration":-2.20371219622657,"pose":{"translation":{"x":7.333094382263903,"y":-0.7278246052076687},"rotation":{"radians":-0.2981374104428669}},"curvature":-4.798040174521225},{"time":0.33600884596997993,"velocity":0.6304020772288179,"acceleration":-1.7722927303336353,"pose":{"translation":{"x":7.342871296984669,"y":-0.7311041261586553},"rotation":{"radians":-0.34989917578712737}},"curvature":-5.236702536281345},{"time":0.3525829471208297,"velocity":0.6010279182473525,"acceleration":-1.3699347820320806,"pose":{"translation":{"x":7.352358319786087,"y":-0.7348640257199522},"rotation":{"radians":-0.4054537862993204}},"curvature":-5.643480174935811},{"time":0.36976286076987,"velocity":0.5774925569872245,"acceleration":-0.9919241283235749,"pose":{"translation":{"x":7.361540463598101,"y":-0.7391269205615391},"rotation":{"radians":-0.4644440730100708}},"curvature":-5.999261987332941},{"time":0.3874752540937721,"velocity":0.5599232066788886,"acceleration":-0.633760498617797,"pose":{"translation":{"x":7.370403700514882,"y":-0.7439135624031483},"rotation":{"radians":-0.5263880359703766}},"curvature":-6.284353805966965},{"time":0.40562669827129005,"velocity":0.5484195383663117,"acceleration":-0.2909483020003185,"pose":{"translation":{"x":7.378934944484527,"y":-0.7492428750473332},"rotation":{"radians":-0.5906828726845003}},"curvature":-6.480915495174348},{"time":0.42410675665832054,"velocity":0.5430427967577385,"acceleration":0.040867278011913304,"pose":{"translation":{"x":7.3871220339987556,"y":-0.755131991412536},"rotation":{"radians":-0.6566233239679846}},"curvature":-6.57564300066678},{"time":0.4427936520640061,"velocity":0.5438064793074622,"acceleration":0.36551173046498503,"pose":{"translation":{"x":7.394953714782599,"y":-0.7615962905661549},"rotation":{"radians":-0.7234341130651554}},"curvature":-6.562074297460497},{"time":0.46156189622249166,"velocity":0.5506664927076196,"acceleration":0.6857461024631776,"pose":{"translation":{"x":7.402419622484099,"y":-0.7686494347576127},"rotation":{"radians":-0.7903131138986692}},"curvature":-6.44187668356058},{"time":0.48029075301778956,"velocity":0.5635097332585861,"acceleration":1.002705190054949,"pose":{"translation":{"x":7.409510265363997,"y":-0.7763034064514244},"rotation":{"radians":-0.8564791640564974}},"curvature":-6.224712372184579},{"time":0.4988721753143321,"velocity":0.582141421833932,"acceleration":1.3154389982154877,"pose":{"translation":{"x":7.4162170069854305,"y":-0.7845685453602647},"rotation":{"radians":-0.921217194032659}},"curvature":-5.926705551959653},{"time":0.5172170027973768,"velocity":0.6062729233206643,"acceleration":1.620684901595592,"pose":{"translation":{"x":7.422532048903632,"y":-0.7934535854780363},"rotation":{"radians":-0.9839140979634297}},"curvature":-5.567955823359643},{"time":0.5352586722305678,"velocity":0.6355127845706157,"acceleration":1.9129717995948965,"pose":{"translation":{"x":7.428448413355612,"y":-0.8029656921129377},"rotation":{"radians":-1.044081205566804}},"curvature":-5.169769119324686},{"time":0.5529542844520852,"velocity":0.6693639917269453,"acceleration":2.1851010903529495,"pose":{"translation":{"x":7.433959925949866,"y":-0.8131104989205302},"rotation":{"radians":-1.1013623608918062}},"curvature":-4.752235515342323},{"time":0.5702833868798093,"velocity":0.7072298323366033,"acceleration":2.4289676387655197,"pose":{"translation":{"x":7.4390611983560575,"y":-0.8238921449368072},"rotation":{"radians":-1.1555293832873916}},"curvature":-4.332545539867548},{"time":0.587245135472715,"velocity":0.7484293707656479,"acceleration":2.636597030345389,"pose":{"translation":{"x":7.443747610994719,"y":-0.8353133116112605},"rotation":{"radians":-1.2064683750474912}},"curvature":-3.9241463540123545},{"time":0.6038545747404133,"velocity":0.7922217690145634,"acceleration":2.699999999999996,"pose":{"translation":{"x":7.448015295726941,"y":-0.8473752598399495},"rotation":{"radians":-1.2541608106925795}},"curvature":-3.536616605499757},{"time":0.6362024768433319,"velocity":0.8795611046924436,"acceleration":2.7000000000000006,"pose":{"translation":{"x":7.4552826622574155,"y":-0.8734196639755125},"rotation":{"radians":-1.3400856851183165}},"curvature":-2.845585051317401},{"time":0.6677848651954601,"velocity":0.9648335532431899,"acceleration":2.7,"pose":{"translation":{"x":7.460846723855813,"y":-0.9020084406998853},"rotation":{"radians":-1.4143103108236423}},"curvature":-2.277572509988386},{"time":0.6989057195952202,"velocity":1.048859860122542,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.464701821619865,"y":-0.9331043146308698},"rotation":{"radians":-1.478233001222403}},"curvature":-1.8251885024621224},{"time":0.7297320912150541,"velocity":1.1320910634960937,"acceleration":2.6999999999999966,"pose":{"translation":{"x":7.466852657906909,"y":-0.9666508367058511},"rotation":{"radians":-1.5333229489020965}},"curvature":-1.4707911285203954},{"time":0.7603482420772625,"velocity":1.2147546708240562,"acceleration":2.4689382496452787,"pose":{"translation":{"x":7.467313742404098,"y":-1.0025735692399675},"rotation":{"radians":-1.5809522845751747}},"curvature":-1.1950333509651316},{"time":0.7908701889907861,"velocity":1.2901114730124972,"acceleration":1.6478061425997925,"pose":{"translation":{"x":7.466108838198616,"y":-1.040781270984282},"rotation":{"radians":-1.6223272462013492}},"curvature":-0.9806036708623775},{"time":0.8528650627556515,"velocity":1.3922670068119412,"acceleration":1.1265054842673399,"pose":{"translation":{"x":7.458839059449773,"y":-1.1236097096364015},"rotation":{"radians":-1.6902497191987305}},"curvature":-0.6821918383506778},{"time":0.9169237989671473,"velocity":1.4644295244694259,"acceleration":0.7553834508087204,"pose":{"translation":{"x":7.445397445026421,"y":-1.2141151835996804},"rotation":{"radians":-1.743407706342333}},"curvature":-0.4964864179346202},{"time":0.9832824629764142,"velocity":1.5145557610798024,"acceleration":0.5047361844321614,"pose":{"translation":{"x":7.426250721188545,"y":-1.3110837105092963},"rotation":{"radians":-1.7861325178852274}},"curvature":-0.37790537450487255},{"time":1.051770787169082,"velocity":1.5491242965109624,"acceleration":0.3362979412462314,"pose":{"translation":{"x":7.40196049133683,"y":-1.4131462165595934},"rotation":{"radians":-1.821408517045324}},"curvature":-0.3005993467136097},{"time":1.1219365275677993,"velocity":1.5727208905530685,"acceleration":0.2198034851609914,"pose":{"translation":{"x":7.373165510259402,"y":-1.5188164583655404},"rotation":{"radians":-1.8513381322664064}},"curvature":-0.24978161090879322},{"time":1.1931386100223174,"velocity":1.5883713564272919,"acceleration":0.13404480115479672,"pose":{"translation":{"x":7.340563958378583,"y":-1.6265289448241926},"rotation":{"radians":-1.8774525545327976}},"curvature":-0.2169095086856791},{"time":1.2646178303499827,"velocity":1.5979527743028137,"acceleration":0.06404129568110227,"pose":{"translation":{"x":7.304895715997633,"y":-1.7346768589761474},"rotation":{"radians":-1.9009171561391245}},"curvature":-0.19710255409574393},{"time":1.3355531046387532,"velocity":1.602495561177761,"acceleration":-0.002096322275086082,"pose":{"translation":{"x":7.2669246375475,"y":-1.8416499798670074},"rotation":{"radians":-1.9226712973054454}},"curvature":-0.1877943585524745},{"time":1.405109094208867,"velocity":1.6023497494074594,"acceleration":-0.07612871060578774,"pose":{"translation":{"x":7.227420825833567,"y":-1.9458726044088372},"rotation":{"radians":-1.943529946970129}},"curvature":-0.18809230783361033},{"time":1.4724793462205135,"velocity":1.5972209389886258,"acceleration":-0.17301087241769844,"pose":{"translation":{"x":7.187142906282408,"y":-2.045841469241623},"rotation":{"radians":-1.9642661669856805}},"curvature":-0.19860704074543745},{"time":1.5369281583145993,"velocity":1.5860705937819437,"acceleration":-0.31554579820498185,"pose":{"translation":{"x":7.146820301188521,"y":-2.1401636725947344},"rotation":{"radians":-1.98568919023132}},"curvature":-0.22170133611162526},{"time":1.5978341143112027,"velocity":1.566851975281558,"acceleration":-1.3309955874047237,"pose":{"translation":{"x":7.107135503961091,"y":-2.2275945961483803},"rotation":{"radians":-2.0087318859865846}},"curvature":-0.26227793056302745},{"time":1.6555960479201848,"velocity":1.4899710965280382,"acceleration":-2.7,"pose":{"translation":{"x":7.068706353370726,"y":-2.3070758268950673},"rotation":{"radians":-2.034563405456838}},"curvature":-0.3294600185346016},{"time":1.7119114730269265,"velocity":1.3379194487398354,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.032068307796214,"y":-2.377773079001069},"rotation":{"radians":-2.064747104509842}},"curvature":-0.4399216872784071},{"time":1.7676117337250405,"velocity":1.1875287448549274,"acceleration":-2.7,"pose":{"translation":{"x":6.997656719471268,"y":-2.439114115667869},"rotation":{"radians":-2.101467131011381}},"curvature":-0.6243017215544429},{"time":1.8221431594281243,"velocity":1.040293895456601,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.965789108731271,"y":-2.490826670993634},"rotation":{"radians":-2.1478338626356113}},"curvature":-0.9388068028845457},{"time":1.8750311267951567,"velocity":0.8974963835656139,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.936647438260028,"y":-2.5329763718346676},"rotation":{"radians":-2.2081880495457282}},"curvature":-1.4818665318907704},{"time":1.9260491103410267,"velocity":0.7597478279917649,"acceleration":-2.700000000000004,"pose":{"translation":{"x":6.910260387336512,"y":-2.5660046596668717},"rotation":{"radians":-2.2879536229403787}},"curvature":-2.39507742000404},{"time":1.9509462729406097,"velocity":0.6925254889728903,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":6.898062530851734,"y":-2.5793483183090196},"rotation":{"radians":-2.336664233442283}},"curvature":-3.0225092551252013},{"time":1.9756123368228327,"velocity":0.6259271164908882,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.8864856260816145,"y":-2.5907667124471985},"rotation":{"radians":-2.391450278038916}},"curvature":-3.7370486015184476},{"time":2.000334632533119,"velocity":0.5591769180731152,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.875482253478922,"y":-2.6004376072065902},"rotation":{"radians":-2.4513985995970358}},"curvature":-4.444436732694084},{"time":2.0256138831254242,"velocity":0.49092294147389115,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.864992089704889,"y":-2.6085693664751215},"rotation":{"radians":-2.5140796747043557}},"curvature":-4.951976919791685},{"time":2.052333259353671,"velocity":0.4187806256576248,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.854941353699419,"y":-2.61540213796164},"rotation":{"radians":-2.5750408517708028}},"curvature":-4.970238408366828},{"time":2.082203426917947,"velocity":0.3381311732340795,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.845242252751303,"y":-2.6212090382540865},"rotation":{"radians":-2.627762206688829}},"curvature":-4.195902581911151},{"time":2.207437194782421,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.8264744033479845,"y":-2.6310096463529753},"rotation":{"radians":-2.677945044589006}},"curvature":-8.61988124756193E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid0.wpilib.json b/src/main/deploy/paths/SixBallMid0.wpilib.json new file mode 100644 index 0000000..9697957 --- /dev/null +++ b/src/main/deploy/paths/SixBallMid0.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid1.wpilib.json b/src/main/deploy/paths/SixBallMid1.wpilib.json new file mode 100644 index 0000000..1fcfcfc --- /dev/null +++ b/src/main/deploy/paths/SixBallMid1.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMidComplete.wpilib.json b/src/main/deploy/paths/SixBallMidComplete.wpilib.json new file mode 100644 index 0000000..40512d6 --- /dev/null +++ b/src/main/deploy/paths/SixBallMidComplete.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24104312286019983,"velocity":0.6508164317225397,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.2063923239707948,"y":-2.3218234953284265},"rotation":{"radians":1.4857527840978701}},"curvature":-0.13003316961730046},{"time":0.34125642343054197,"velocity":0.9213923432624636,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.2136058807373047,"y":-2.2433763408660887},"rotation":{"radians":1.4709813552860977}},"curvature":-0.2416411635394689},{"time":0.41862951975667484,"velocity":1.1302997033430222,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.222373878955841,"y":-2.1644892266392706},"rotation":{"radians":1.4480070351506305}},"curvature":-0.33393289305160856},{"time":0.4843752197211856,"velocity":1.3078130932472012,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.23333740234375,"y":-2.0850949096679687},"rotation":{"radians":1.418153476722264}},"curvature":-0.40799291240160246},{"time":0.5428188459431846,"velocity":1.4656108840465984,"acceleration":-0.12363267963904842,"pose":{"translation":{"x":3.247049701213837,"y":-2.005218879878521},"rotation":{"radians":1.3826311795834407}},"curvature":-0.4659998167211871},{"time":0.5989114443624675,"velocity":1.4586760057961055,"acceleration":-0.23878049786970917,"pose":{"translation":{"x":3.263980484008789,"y":-1.9249700260162352},"rotation":{"radians":1.3424938853817683}},"curvature":-0.5106186292535235},{"time":0.656093483931331,"velocity":1.4450220499186468,"acceleration":-0.17582458312750424,"pose":{"translation":{"x":3.284520208835602,"y":-1.8445313015580176},"rotation":{"radians":1.2986210341256466}},"curvature":-0.5446069018648376},{"time":0.7144460101119858,"velocity":1.4347622413284964,"acceleration":-0.13307656507771634,"pose":{"translation":{"x":3.308984375,"y":-1.7641503906249998},"rotation":{"radians":1.2517175479138372}},"curvature":-0.5705719275620846},{"time":0.7738450156145333,"velocity":1.426857625707185,"acceleration":-0.10571177853926464,"pose":{"translation":{"x":3.337617814540863,"y":-1.6841303738951683},"rotation":{"radians":1.2023248883292088}},"curvature":-0.5908312113511963},{"time":0.8341780829208127,"velocity":1.420479709857509,"acceleration":-0.08976520784153569,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6048203945159911},"rotation":{"radians":1.1508391887199425}},"curvature":-0.6073419696862633},{"time":0.8953429670423647,"velocity":1.4149892313217345,"acceleration":-0.08191722568263685,"pose":{"translation":{"x":3.4080442547798158,"y":-1.5266063240170478},"rotation":{"radians":1.0975335619661353}},"curvature":-0.621674601346085},{"time":0.9572461651384423,"velocity":1.4099182930728211,"acceleration":-0.07932481308319937,"pose":{"translation":{"x":3.45001220703125,"y":-1.449901428222656},"rotation":{"radians":1.0425826601190191}},"curvature":-0.6350112021866137},{"time":1.0198016941975718,"velocity":1.404956087422885,"acceleration":-0.07948863640008091,"pose":{"translation":{"x":3.4965079188346864,"y":-1.3751370331645012},"rotation":{"radians":0.9860883281959857}},"curvature":-0.6481550334261905},{"time":1.0829301018210549,"velocity":1.3999380963827859,"acceleration":-0.08015206912118296,"pose":{"translation":{"x":3.547487258911133,"y":-1.3027531909942627},"rotation":{"radians":0.9281057752826144}},"curvature":-0.6615413844839425},{"time":1.146557600543031,"velocity":1.394838220707214,"acceleration":-0.07923440840802286,"pose":{"translation":{"x":3.602861177921295,"y":-1.233189345896244},"rotation":{"radians":0.8686700639277289}},"curvature":-0.6752448631775445},{"time":1.2106151395445883,"velocity":1.3897626595003518,"acceleration":-0.07480385075761357,"pose":{"translation":{"x":3.6624999999999996,"y":-1.1668749999999999},"rotation":{"radians":0.8078228647022838}},"curvature":-0.6889828628777236},{"time":1.2750372072434193,"velocity":1.3849436407627116,"acceleration":-0.06509475812905682,"pose":{"translation":{"x":3.726237714290619,"y":-1.1042203792929648},"rotation":{"radians":0.745639315828271}},"curvature":-0.7021196688913226},{"time":1.3397601915745783,"velocity":1.380730513752284,"acceleration":-0.04856760891892339,"pose":{"translation":{"x":3.793876266479492,"y":-1.045607099533081},"rotation":{"radians":0.68225447169036}},"curvature":-0.71367992457469},{"time":1.4047202049596175,"velocity":1.3775755612268314,"acceleration":-0.023998991327275822,"pose":{"translation":{"x":3.8651898503303523,"y":-0.9913788321614265},"rotation":{"radians":0.6178882696500003}},"curvature":-0.7223829927855778},{"time":1.4698503918277483,"velocity":1.3760125024370393,"acceleration":0.009425020866650225,"pose":{"translation":{"x":3.9399291992187493,"y":-0.9418319702148437},"rotation":{"radians":0.5528673168364797}},"curvature":-0.7267095396737598},{"time":1.535077848801426,"velocity":1.3766272725800948,"acceleration":0.05205176529824429,"pose":{"translation":{"x":4.017825877666472,"y":-0.8972062942385672},"rotation":{"radians":0.48764131303933056}},"curvature":-0.7250066836520244},{"time":1.6003203604517349,"velocity":1.3800232604839846,"acceleration":0.10378762257480224,"pose":{"translation":{"x":4.098596572875976,"y":-0.8576756381988526},"rotation":{"radians":0.4227918675196265}},"curvature":-0.715627451832997},{"time":1.665483145837002,"velocity":1.3867863510594736,"acceleration":0.16424958779401677,"pose":{"translation":{"x":4.1819473862648,"y":-0.8233385553956034},"rotation":{"radians":0.3590321072445168}},"curvature":-0.6970855981440633},{"time":1.7304557025837932,"velocity":1.3974580667230574,"acceleration":0.2330731468720872,"pose":{"translation":{"x":4.267578124999998,"y":-0.794208984375},"rotation":{"radians":0.29719692634159106}},"curvature":-0.668192786058002},{"time":1.7951086483327998,"velocity":1.4125269322433287,"acceleration":0.31039116241082515,"pose":{"translation":{"x":4.35518659353256,"y":-0.7702069148421291},"rotation":{"radians":0.23822580998113968}},"curvature":-0.6281385045136234},{"time":1.859290265714348,"velocity":1.4324483390677942,"acceleration":0.3974925676686374,"pose":{"translation":{"x":4.444472885131834,"y":-0.751149053573609},"rotation":{"radians":0.18314236815259385}},"curvature":-0.5764792409963189},{"time":1.922822358232916,"velocity":1.4577018736523613,"acceleration":0.4976892074994749,"pose":{"translation":{"x":4.53514367341995,"y":-0.7367394903302198},"rotation":{"radians":0.1330363391804617}},"curvature":-0.5130224065785399},{"time":1.9854951125589597,"velocity":1.4888934270846992,"acceleration":0.6174792927963386,"pose":{"translation":{"x":4.626916503906247,"y":-0.7265603637695319},"rotation":{"radians":0.08905427201814982}},"curvature":-0.43761593999910725},{"time":2.047060989445588,"velocity":1.5269090812050408,"acceleration":0.7682064361829994,"pose":{"translation":{"x":4.7195240855216944,"y":-0.7200625273585326},"rotation":{"radians":0.052404113332160406}},"curvature":-0.3498775646988902},{"time":2.107228253203399,"velocity":1.5731299604713114,"acceleration":0.9685643952029747,"pose":{"translation":{"x":4.812718582153316,"y":-0.7165562152862552},"rotation":{"radians":0.024376580770117368}},"curvature":-0.24891407952009473},{"time":2.1656556590359175,"velocity":1.6297206654647636,"acceleration":1.248398336461174,"pose":{"translation":{"x":4.906275904178614,"y":-0.7152017083764088},"rotation":{"radians":0.0063825391669578325}},"curvature":-0.13309736505149056},{"time":2.221951259953214,"velocity":1.6999999999999984,"acceleration":2.843979367649469E-14,"pose":{"translation":{"x":4.999999999999995,"y":-0.7150000000000012},"rotation":{"radians":-2.0724163126336422E-15}},"curvature":-2.9605947323338176E-15},{"time":2.2770632519617626,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3319714486775105,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.386489936201094,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.440451733873254,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.49370816316683,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5461282165787624,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.597597926522085,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6480197342179226,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6973118585874882,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.7454076651440773,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.792255034885068,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8378157331839153,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.882064778682146,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.924989812181359,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9665904655352184,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.006877730541452,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0458733278338475,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.083609075774248,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.1554749990386948,"velocity":1.7,"acceleration":-0.8670174294348908,"pose":{"translation":{"x":6.5869903564453125,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2241092626820116,"velocity":1.6404928971648147,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.701626491546631,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.293986626673893,"velocity":1.4518240143867347,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.80966796875,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.370001653212677,"velocity":1.2465834427320184,"acceleration":-2.7,"pose":{"translation":{"x":6.912227725982667,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.457094277398368,"velocity":1.0114333574306535,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.010556030273439,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.5678335818442313,"velocity":0.7124372354268221,"acceleration":-2.7,"pose":{"translation":{"x":7.106006145477297,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.831699224594906,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":7.200000000000003,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a21de35..c982f8e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.auto.EightBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.Wait; import frc4388.robot.commands.climber.DisengageRachet; @@ -94,6 +95,8 @@ public class RobotContainer { SixBallAutoMiddle m_sixBallAutoMiddle; double m_totalTimeAuto; + EightBallAutoMiddle m_eightBallAutoMiddle; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -242,10 +245,14 @@ public class RobotContainer { public void buildAutos() { String[] sixBallAutoMiddlePaths = new String[]{ - "FirstPath0", - "FirstPath1" + "SixBallMidComplete" }; m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + + String[] eightBallAutoMiddlePaths = new String[]{ + "EightBallMidComplete" + }; + m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); } /** @@ -262,7 +269,10 @@ public class RobotContainer { // Run path following command, then stop at the end. try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); + return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + } catch (Exception e) { System.err.println("ERROR"); } diff --git a/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java new file mode 100644 index 0000000..1c0f863 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class EightBallAutoMiddle extends SequentialCommandGroup { + /** + * Creates a new EightBallAutoMiddle. + */ + public EightBallAutoMiddle(Drive drive, RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands( + paths[0] + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java index c1e9f38..9900c65 100644 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java @@ -29,8 +29,7 @@ public class SixBallAutoMiddle extends SequentialCommandGroup { // super(new FooCommand(), new BarCommand()); addCommands( - paths[0], - paths[1] + paths[0] ); } } From 28d9e0a5f4b0f281277ccf51826ecc546f49d1ea Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 9 Mar 2020 20:24:44 -0600 Subject: [PATCH 20/39] Tested autos, kinda working Co-Authored-By: kyrarivera --- PathWeaver/Paths/EightBallMidComplete | 7 ++++--- PathWeaver/Paths/SixBallMid0 | 2 +- PathWeaver/Paths/SixBallMid1 | 4 ++-- PathWeaver/Paths/SixBallMidComplete | 4 ++-- PathWeaver/pathweaver.json | 4 ++-- src/main/deploy/paths/EightBallMid0.wpilib.json | 2 +- src/main/deploy/paths/EightBallMid1.wpilib.json | 2 +- src/main/deploy/paths/EightBallMid2.wpilib.json | 2 +- src/main/deploy/paths/EightBallMidComplete.wpilib.json | 2 +- src/main/deploy/paths/SixBallMid0.wpilib.json | 2 +- src/main/deploy/paths/SixBallMid1.wpilib.json | 2 +- src/main/deploy/paths/SixBallMidComplete.wpilib.json | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- 13 files changed, 21 insertions(+), 20 deletions(-) diff --git a/PathWeaver/Paths/EightBallMidComplete b/PathWeaver/Paths/EightBallMidComplete index 22711d6..8a05706 100644 --- a/PathWeaver/Paths/EightBallMidComplete +++ b/PathWeaver/Paths/EightBallMidComplete @@ -1,5 +1,6 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.4,0.2,2.5,true, -5.0,-0.715,3.0,0.0,true, -7.2,-0.715,1.5,0.0,true, -6.8,-2.6,-0.6,-0.3,true, +5.0,-1.1,3.0,0.0,true, +7.2,-1.1,1.5,0.0,true, +6.5,-2.78,-0.39262822032017564,-0.17846737287280634,true, +6.291072284729572,-2.036118403443617,0.13087607344005825,0.40452604517836144,true, diff --git a/PathWeaver/Paths/SixBallMid0 b/PathWeaver/Paths/SixBallMid0 index 18dd1ec..c392449 100644 --- a/PathWeaver/Paths/SixBallMid0 +++ b/PathWeaver/Paths/SixBallMid0 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.4,0.2,2.5,true, -5.006107200045366,-0.7154598441848491,2.0,0.0,true, +5.006107200045366,-1.3,2.0,0.0,true, diff --git a/PathWeaver/Paths/SixBallMid1 b/PathWeaver/Paths/SixBallMid1 index 4ae0aad..407b2e5 100644 --- a/PathWeaver/Paths/SixBallMid1 +++ b/PathWeaver/Paths/SixBallMid1 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -5.006,-0.715,3.048,0.0,true, -7.2,-0.7154598441848491,1.0,0.0,true, +5.006,-1.3,3.048,0.0,true, +7.2,-1.3,1.0,0.0,true, diff --git a/PathWeaver/Paths/SixBallMidComplete b/PathWeaver/Paths/SixBallMidComplete index 5fa9b7c..4378c83 100644 --- a/PathWeaver/Paths/SixBallMidComplete +++ b/PathWeaver/Paths/SixBallMidComplete @@ -1,4 +1,4 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.4,0.2,2.5,true, -5.0,-0.715,3.0,0.0,true, -7.2,-0.715,1.5,0.0,true, +5.0,-1.1,3.0,0.0,true, +7.2,-1.1,1.5,0.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index ddd52e0..4a585f0 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,8 +1,8 @@ { "lengthUnit": "Meter", "exportUnit": "Always Meters", - "maxVelocity": 1.7, - "maxAcceleration": 2.7, + "maxVelocity": 2.5, + "maxAcceleration": 3.0, "wheelBase": 0.648, "gameName": "Infinite Recharge", "outputDir": ".." diff --git a/src/main/deploy/paths/EightBallMid0.wpilib.json b/src/main/deploy/paths/EightBallMid0.wpilib.json index 9697957..a461431 100644 --- a/src/main/deploy/paths/EightBallMid0.wpilib.json +++ b/src/main/deploy/paths/EightBallMid0.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22868744691387022,"velocity":0.6860623407416107,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3238294613804855,"velocity":0.9714883841414567,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.39741314342202283,"velocity":1.1922394302660686,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.4601433693460262,"velocity":1.3804301080380785,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5161968408530254,"velocity":1.5485905225590764,"acceleration":2.9999999999999973,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.5677250216556059,"velocity":1.7031750649668176,"acceleration":2.9999999999999956,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.615996242927949,"velocity":1.8479887287838466,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.6618112290098469,"velocity":1.9854336870295404,"acceleration":1.7724319853927966,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7062618567953473,"velocity":2.064219401487351,"acceleration":0.5034639557065531,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.7506783242988111,"velocity":2.0865814919151564,"acceleration":0.49932665829872824,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.7956023350319041,"velocity":2.109013248071888,"acceleration":0.45895712191178695,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.8409364448700584,"velocity":2.12981966064764,"acceleration":0.39515203882870387,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":0.8865733255240144,"velocity":2.147853167083833,"acceleration":0.31701310713711306,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":0.9323919712522077,"velocity":2.162378278330942,"acceleration":0.23031078377585737,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":0.9782572476318948,"velocity":2.1729415460820443,"acceleration":0.13813925366720256,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.0240221425451959,"velocity":2.1792634745095256,"acceleration":0.041673045633118966,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.0695319790973778,"velocity":2.1811600080049205,"acceleration":-0.059065512402407955,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.1146298672309258,"velocity":2.178496278134046,"acceleration":-0.16441372455340814,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.159162714233256,"velocity":2.171174466893426,"acceleration":-0.27415348238761694,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.2029871254407334,"velocity":2.159159851947309,"acceleration":-0.3863225903265772,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.245974473802423,"velocity":2.14255286817695,"acceleration":-1.2804495582678201,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.2883463773411485,"velocity":2.0882977830078224,"acceleration":-3.0000000000000053,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.3310768450283628,"velocity":1.9601063799461798,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.3749668670018906,"velocity":1.8284363140255961,"acceleration":-3.0,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.4202251947318614,"velocity":1.6926613308356837,"acceleration":-3.0,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.4672211376575084,"velocity":1.5516735020587429,"acceleration":-3.0,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.516578191034963,"velocity":1.4036023419263792,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.5693496410384185,"velocity":1.2452879919160125,"acceleration":-3.0,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":1.627401271932817,"velocity":1.0711330992328167,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":1.6944454242441096,"velocity":0.8700006422989391,"acceleration":-3.0,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":1.984445638343756,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMid1.wpilib.json b/src/main/deploy/paths/EightBallMid1.wpilib.json index 1fcfcfc..11d97a8 100644 --- a/src/main/deploy/paths/EightBallMid1.wpilib.json +++ b/src/main/deploy/paths/EightBallMid1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.25197612978781475,"velocity":0.7559283893634442,"acceleration":3.0,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3562702409711053,"velocity":1.068810722913316,"acceleration":2.9999999999999987,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.4361595761515367,"velocity":1.30847872845461,"acceleration":3.0000000000000004,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5033083097241129,"velocity":1.5099249291723384,"acceleration":3.000000000000004,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5622027592166328,"velocity":1.6866082776498983,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6151186093010053,"velocity":1.8453558279030158,"acceleration":3.0,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.6633872747726501,"velocity":1.9901618243179504,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7078595240733458,"velocity":2.1235785722200378,"acceleration":3.0000000000000075,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.7491126584402565,"velocity":2.2473379753207703,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.7875559897937853,"velocity":2.3626679693813566,"acceleration":3.0000000000000027,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.8234897976794279,"velocity":2.4704693930382846,"acceleration":0.857610578891266,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.8576259992682255,"velocity":2.4997449606440023,"acceleration":-1.1655623083604212,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":0.8909295503524042,"velocity":2.460927596765728,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":0.9241768065402509,"velocity":2.3611858282021876,"acceleration":-3.0000000000000084,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":0.9577207352388619,"velocity":2.2605540421063544,"acceleration":-3.0000000000000058,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":0.9915165621380305,"velocity":2.159166561408848,"acceleration":-3.0,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.0255206194145319,"velocity":2.0571543895793445,"acceleration":-2.999999999999997,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.0596923443650952,"velocity":1.9546392147276543,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.093996980130577,"velocity":1.8517253074312092,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.1284092725993788,"velocity":1.7484884300248034,"acceleration":-3.0,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.197536234063257,"velocity":1.5411075456331693,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.267318590047229,"velocity":1.331760477681253,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.3388365417026034,"velocity":1.1172066227151305,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.415176951512201,"velocity":0.8881853932863374,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.5057126326753016,"velocity":0.6165783497970355,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.7112387492743135,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMid2.wpilib.json b/src/main/deploy/paths/EightBallMid2.wpilib.json index ee76706..26e5d4d 100644 --- a/src/main/deploy/paths/EightBallMid2.wpilib.json +++ b/src/main/deploy/paths/EightBallMid2.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999993,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.1857265193636004,"velocity":0.501461602281721,"acceleration":2.699999999999999,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":0.22660073611189058,"velocity":0.6118219875021044,"acceleration":2.7000000000000015,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":0.26036037214234553,"velocity":0.7029730047843328,"acceleration":1.4538431600368555,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":0.2901133482323059,"velocity":0.7462291655634617,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":0.30461963577730317,"velocity":0.7070621891919691,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":0.31975062184802994,"velocity":0.6662085268010068,"acceleration":-2.302451123186552,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":0.33557102199759564,"velocity":0.6297828287073786,"acceleration":-1.8675460534492743,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":0.3520637919772221,"velocity":0.5989818212214805,"acceleration":-1.4613709602031215,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":0.369181463367336,"velocity":0.5739665533456683,"acceleration":-1.079676100951105,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":0.38685538744616993,"velocity":0.554884439907727,"acceleration":-0.718302000748962,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":0.40499556745388565,"velocity":0.5418543123142384,"acceleration":-0.3729250173328537,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":0.4234931432073012,"velocity":0.5349561035557802,"acceleration":-0.03916357131337384,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":0.4422256519746503,"velocity":0.5342224716127917,"acceleration":0.28704156286608506,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":0.46106457684825486,"velocity":0.5396300260512279,"acceleration":0.6088741709257178,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":0.4798841040036331,"velocity":0.5510887500451729,"acceleration":0.9280335229053567,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":0.49856967238109146,"velocity":0.5684295838939945,"acceleration":1.244199218395254,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":0.5170249585196953,"velocity":0.5913916364829062,"acceleration":1.55470303079482,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":0.5351763702192629,"velocity":0.6196116912654284,"acceleration":1.8545226594628177,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":0.5529747450744766,"velocity":0.6526191807360355,"acceleration":2.1366633368747885,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":0.5703945330851697,"velocity":0.6898394031146146,"acceleration":2.3929144893008223,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":0.5874311208341066,"velocity":0.7306065007872906,"acceleration":2.614876812845669,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":0.6040970809732634,"velocity":0.7741859335189818,"acceleration":2.700000000000003,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":0.6204335317180859,"velocity":0.8182943505300027,"acceleration":2.699999999999989,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":0.6365062512871126,"velocity":0.8616906933663744,"acceleration":2.700000000000001,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":0.6680852946379339,"velocity":0.9469541104135919,"acceleration":2.7000000000000024,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":0.6991702640578552,"velocity":1.0308835278473796,"acceleration":2.7,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":0.7299416895512951,"velocity":1.1139663766796675,"acceleration":2.6999999999999993,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":0.7604917708724955,"velocity":1.1964515962469084,"acceleration":2.6999999999999957,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":0.7908583645653527,"velocity":1.2784413992176227,"acceleration":1.7317501674300086,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":0.8522944364989423,"velocity":1.3848333270748585,"acceleration":1.1714925683775692,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":0.9156777853297742,"velocity":1.4590864491890612,"acceleration":0.7858676493625434,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":0.9813100267777496,"velocity":1.5106647044981765,"acceleration":0.5250113170323235,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":1.0490457248506553,"velocity":1.5462267125535365,"acceleration":0.34981100784937563,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":1.1184519520705818,"velocity":1.5705057748483617,"acceleration":0.2288975554736658,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":1.1889035211319336,"velocity":1.5866319667857893,"acceleration":0.14022072554540496,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":1.2596548250824198,"velocity":1.59655276595901,"acceleration":0.06824791072810449,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":1.3298966361891713,"velocity":1.601346622812804,"acceleration":7.56678468946936E-4,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":1.398803911610538,"velocity":1.601398763464469,"acceleration":-0.07418428163614217,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":1.4655788655112532,"velocity":1.596445111478058,"acceleration":-0.1715480180400782,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":1.5294925579236458,"velocity":1.5854808442190889,"acceleration":-0.31394105304156766,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":1.5899279221203853,"velocity":1.5665077023422138,"acceleration":-1.3509793267586743,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":1.647297847739518,"velocity":1.4890021188530829,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":1.7032812155507002,"velocity":1.3378470257628907,"acceleration":-2.699999999999999,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":1.7586823776797251,"velocity":1.1882638880145233,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":1.8129609537084448,"velocity":1.0417117327369803,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":1.8656590466798884,"velocity":0.8994268817140824,"acceleration":-2.7,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":1.9165692937923622,"velocity":0.7619692145104032,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":1.9414487107808738,"velocity":0.6947947886414217,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":1.9661233789888195,"velocity":0.628173184479968,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":1.9908811602880785,"velocity":0.5613271749719686,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":2.0162223622736977,"velocity":0.4929059296107963,"acceleration":-2.7,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":2.0430294857607096,"velocity":0.42052669619586397,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":2.073014782085448,"velocity":0.33956639611906964,"acceleration":-2.7,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":2.1987801139813996,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.176195646925303,"velocity":0.5285869407759091,"acceleration":2.999999999999998,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":0.21497233367530247,"velocity":0.6449170010259073,"acceleration":3.0000000000000018,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":0.24699953652565942,"velocity":0.7409986095769783,"acceleration":2.9999999999999982,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":0.27455684939889846,"velocity":0.8236705481966954,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":0.28706924422183644,"velocity":0.8612077326655093,"acceleration":2.9999999999999947,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":0.2988897193555486,"velocity":0.8966691580666457,"acceleration":1.4372274216190066,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":0.3102197650367444,"velocity":0.9129530104078564,"acceleration":-3.0,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":0.32152890268001205,"velocity":0.8790255974780534,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":0.3331812815718787,"velocity":0.8440684608024535,"acceleration":-2.3349396646866505,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":0.3451995499454858,"velocity":0.8160065292760691,"acceleration":-1.5534212818965476,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":0.35753487235073245,"velocity":0.7968445769327036,"acceleration":-0.8064987399066847,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":0.370113223863055,"velocity":0.7867001522879121,"acceleration":-0.08469630474345373,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":0.3828513298248526,"velocity":0.7856212817835173,"acceleration":0.6207646255754383,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":0.3956617987389037,"velocity":0.7935735677223941,"acceleration":1.3167694007908965,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":0.408459077204561,"velocity":0.8104246324193718,"acceleration":2.006992912857606,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":0.42116526370123264,"velocity":0.8359258586676388,"acceleration":2.6907422543149933,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":0.4337148582754833,"velocity":0.8696935830630972,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":0.4460882493031231,"velocity":0.9068137561460166,"acceleration":3.0,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":0.4583258055081431,"velocity":0.9435264247610767,"acceleration":2.999999999999995,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":0.47048334862400293,"velocity":0.9799990541086561,"acceleration":2.999999999999982,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":0.4826051644207643,"velocity":1.01636450149894,"acceleration":3.0,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":0.4947258540646786,"velocity":1.052726570430683,"acceleration":2.9999999999999956,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":0.5068718843289584,"velocity":1.089164661223522,"acceleration":2.9999999999999876,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":0.5190629084365962,"velocity":1.1257377335464354,"acceleration":3.000000000000004,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":0.5436268496599665,"velocity":1.1994295572165465,"acceleration":3.0000000000000018,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":0.5684834454795932,"velocity":1.2739993446754265,"acceleration":3.0,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":0.593640987194308,"velocity":1.3494719698195712,"acceleration":2.999999999999995,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":0.6190742338239772,"velocity":1.4257717097085785,"acceleration":2.999999999999997,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":0.644736948531325,"velocity":1.5027598538306217,"acceleration":3.000000000000002,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":0.6965024202650136,"velocity":1.6580562690316876,"acceleration":3.0,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":0.7484217413230949,"velocity":1.8138142322059314,"acceleration":2.9999999999999987,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":0.7999552231413788,"velocity":1.968414677660783,"acceleration":3.0000000000000044,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":0.8505967339186178,"velocity":2.1203392099925003,"acceleration":3.000000000000002,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":0.899888738048394,"velocity":2.268215222381829,"acceleration":1.3460968931184591,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":0.9482263295305208,"velocity":2.333282304096749,"acceleration":0.30324551372276426,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":0.9963372162168512,"velocity":2.347871714645603,"acceleration":-0.17307880083034716,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":1.0442579994976287,"velocity":2.3395776429405153,"acceleration":-2.999999999999996,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":1.0929426509757927,"velocity":2.1935236885060236,"acceleration":-3.000000000000002,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":1.1433546887258714,"velocity":2.042287575255787,"acceleration":-2.999999999999998,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":1.1951115938389032,"velocity":1.8870168599166917,"acceleration":-3.0,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":1.2477918907181875,"velocity":1.7289759692788385,"acceleration":-3.0,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":1.300935198656413,"velocity":1.5695460454641619,"acceleration":-3.0,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":1.3540456846675004,"velocity":1.4102145874308998,"acceleration":-2.9999999999999982,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":1.4066038418718951,"velocity":1.2525401158177158,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":1.4580970203923065,"velocity":1.0980605802564818,"acceleration":-3.000000000000001,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":1.5080908210334283,"velocity":0.9480791783331165,"acceleration":-3.0,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":1.5563885221686564,"velocity":0.8031860749274318,"acceleration":-2.999999999999997,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":1.5799912095308917,"velocity":0.7323780128407261,"acceleration":-2.9999999999999982,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":1.6033996551447078,"velocity":0.6621526759992775,"acceleration":-3.0,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":1.6268869487601023,"velocity":0.5916907951530944,"acceleration":-3.0,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":1.650927723836384,"velocity":0.5195684699242497,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":1.67635919415729,"velocity":0.4432740589615319,"acceleration":-3.0,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":1.704805743967665,"velocity":0.35793440953040684,"acceleration":-3.0,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":1.824117213811134,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMidComplete.wpilib.json b/src/main/deploy/paths/EightBallMidComplete.wpilib.json index 9aca47f..1347f0f 100644 --- a/src/main/deploy/paths/EightBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/EightBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24104312286019983,"velocity":0.6508164317225397,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.2063923239707948,"y":-2.3218234953284265},"rotation":{"radians":1.4857527840978701}},"curvature":-0.13003316961730046},{"time":0.34125642343054197,"velocity":0.9213923432624636,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.2136058807373047,"y":-2.2433763408660887},"rotation":{"radians":1.4709813552860977}},"curvature":-0.2416411635394689},{"time":0.41862951975667484,"velocity":1.1302997033430222,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.222373878955841,"y":-2.1644892266392706},"rotation":{"radians":1.4480070351506305}},"curvature":-0.33393289305160856},{"time":0.4843752197211856,"velocity":1.3078130932472012,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.23333740234375,"y":-2.0850949096679687},"rotation":{"radians":1.418153476722264}},"curvature":-0.40799291240160246},{"time":0.5428188459431846,"velocity":1.4656108840465984,"acceleration":-0.12363267963904842,"pose":{"translation":{"x":3.247049701213837,"y":-2.005218879878521},"rotation":{"radians":1.3826311795834407}},"curvature":-0.4659998167211871},{"time":0.5989114443624675,"velocity":1.4586760057961055,"acceleration":-0.23878049786970917,"pose":{"translation":{"x":3.263980484008789,"y":-1.9249700260162352},"rotation":{"radians":1.3424938853817683}},"curvature":-0.5106186292535235},{"time":0.656093483931331,"velocity":1.4450220499186468,"acceleration":-0.17582458312750424,"pose":{"translation":{"x":3.284520208835602,"y":-1.8445313015580176},"rotation":{"radians":1.2986210341256466}},"curvature":-0.5446069018648376},{"time":0.7144460101119858,"velocity":1.4347622413284964,"acceleration":-0.13307656507771634,"pose":{"translation":{"x":3.308984375,"y":-1.7641503906249998},"rotation":{"radians":1.2517175479138372}},"curvature":-0.5705719275620846},{"time":0.7738450156145333,"velocity":1.426857625707185,"acceleration":-0.10571177853926464,"pose":{"translation":{"x":3.337617814540863,"y":-1.6841303738951683},"rotation":{"radians":1.2023248883292088}},"curvature":-0.5908312113511963},{"time":0.8341780829208127,"velocity":1.420479709857509,"acceleration":-0.08976520784153569,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6048203945159911},"rotation":{"radians":1.1508391887199425}},"curvature":-0.6073419696862633},{"time":0.8953429670423647,"velocity":1.4149892313217345,"acceleration":-0.08191722568263685,"pose":{"translation":{"x":3.4080442547798158,"y":-1.5266063240170478},"rotation":{"radians":1.0975335619661353}},"curvature":-0.621674601346085},{"time":0.9572461651384423,"velocity":1.4099182930728211,"acceleration":-0.07932481308319937,"pose":{"translation":{"x":3.45001220703125,"y":-1.449901428222656},"rotation":{"radians":1.0425826601190191}},"curvature":-0.6350112021866137},{"time":1.0198016941975718,"velocity":1.404956087422885,"acceleration":-0.07948863640008091,"pose":{"translation":{"x":3.4965079188346864,"y":-1.3751370331645012},"rotation":{"radians":0.9860883281959857}},"curvature":-0.6481550334261905},{"time":1.0829301018210549,"velocity":1.3999380963827859,"acceleration":-0.08015206912118296,"pose":{"translation":{"x":3.547487258911133,"y":-1.3027531909942627},"rotation":{"radians":0.9281057752826144}},"curvature":-0.6615413844839425},{"time":1.146557600543031,"velocity":1.394838220707214,"acceleration":-0.07923440840802286,"pose":{"translation":{"x":3.602861177921295,"y":-1.233189345896244},"rotation":{"radians":0.8686700639277289}},"curvature":-0.6752448631775445},{"time":1.2106151395445883,"velocity":1.3897626595003518,"acceleration":-0.07480385075761357,"pose":{"translation":{"x":3.6624999999999996,"y":-1.1668749999999999},"rotation":{"radians":0.8078228647022838}},"curvature":-0.6889828628777236},{"time":1.2750372072434193,"velocity":1.3849436407627116,"acceleration":-0.06509475812905682,"pose":{"translation":{"x":3.726237714290619,"y":-1.1042203792929648},"rotation":{"radians":0.745639315828271}},"curvature":-0.7021196688913226},{"time":1.3397601915745783,"velocity":1.380730513752284,"acceleration":-0.04856760891892339,"pose":{"translation":{"x":3.793876266479492,"y":-1.045607099533081},"rotation":{"radians":0.68225447169036}},"curvature":-0.71367992457469},{"time":1.4047202049596175,"velocity":1.3775755612268314,"acceleration":-0.023998991327275822,"pose":{"translation":{"x":3.8651898503303523,"y":-0.9913788321614265},"rotation":{"radians":0.6178882696500003}},"curvature":-0.7223829927855778},{"time":1.4698503918277483,"velocity":1.3760125024370393,"acceleration":0.009425020866650225,"pose":{"translation":{"x":3.9399291992187493,"y":-0.9418319702148437},"rotation":{"radians":0.5528673168364797}},"curvature":-0.7267095396737598},{"time":1.535077848801426,"velocity":1.3766272725800948,"acceleration":0.05205176529824429,"pose":{"translation":{"x":4.017825877666472,"y":-0.8972062942385672},"rotation":{"radians":0.48764131303933056}},"curvature":-0.7250066836520244},{"time":1.6003203604517349,"velocity":1.3800232604839846,"acceleration":0.10378762257480224,"pose":{"translation":{"x":4.098596572875976,"y":-0.8576756381988526},"rotation":{"radians":0.4227918675196265}},"curvature":-0.715627451832997},{"time":1.665483145837002,"velocity":1.3867863510594736,"acceleration":0.16424958779401677,"pose":{"translation":{"x":4.1819473862648,"y":-0.8233385553956034},"rotation":{"radians":0.3590321072445168}},"curvature":-0.6970855981440633},{"time":1.7304557025837932,"velocity":1.3974580667230574,"acceleration":0.2330731468720872,"pose":{"translation":{"x":4.267578124999998,"y":-0.794208984375},"rotation":{"radians":0.29719692634159106}},"curvature":-0.668192786058002},{"time":1.7951086483327998,"velocity":1.4125269322433287,"acceleration":0.31039116241082515,"pose":{"translation":{"x":4.35518659353256,"y":-0.7702069148421291},"rotation":{"radians":0.23822580998113968}},"curvature":-0.6281385045136234},{"time":1.859290265714348,"velocity":1.4324483390677942,"acceleration":0.3974925676686374,"pose":{"translation":{"x":4.444472885131834,"y":-0.751149053573609},"rotation":{"radians":0.18314236815259385}},"curvature":-0.5764792409963189},{"time":1.922822358232916,"velocity":1.4577018736523613,"acceleration":0.4976892074994749,"pose":{"translation":{"x":4.53514367341995,"y":-0.7367394903302198},"rotation":{"radians":0.1330363391804617}},"curvature":-0.5130224065785399},{"time":1.9854951125589597,"velocity":1.4888934270846992,"acceleration":0.6174792927963386,"pose":{"translation":{"x":4.626916503906247,"y":-0.7265603637695319},"rotation":{"radians":0.08905427201814982}},"curvature":-0.43761593999910725},{"time":2.047060989445588,"velocity":1.5269090812050408,"acceleration":0.7682064361829994,"pose":{"translation":{"x":4.7195240855216944,"y":-0.7200625273585326},"rotation":{"radians":0.052404113332160406}},"curvature":-0.3498775646988902},{"time":2.107228253203399,"velocity":1.5731299604713114,"acceleration":0.9685643952029747,"pose":{"translation":{"x":4.812718582153316,"y":-0.7165562152862552},"rotation":{"radians":0.024376580770117368}},"curvature":-0.24891407952009473},{"time":2.1656556590359175,"velocity":1.6297206654647636,"acceleration":1.248398336461174,"pose":{"translation":{"x":4.906275904178614,"y":-0.7152017083764088},"rotation":{"radians":0.0063825391669578325}},"curvature":-0.13309736505149056},{"time":2.221951259953214,"velocity":1.6999999999999984,"acceleration":2.843979367649469E-14,"pose":{"translation":{"x":4.999999999999995,"y":-0.7150000000000012},"rotation":{"radians":-2.0724163126336422E-15}},"curvature":-2.9605947323338176E-15},{"time":2.2770632519617626,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3319714486775105,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.386489936201094,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.440451733873254,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.49370816316683,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5461282165787624,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.597597926522085,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6480197342179226,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6973118585874882,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.7454076651440773,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.792255034885068,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8378157331839153,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.882064778682146,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.924989812181359,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9665904655352184,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.006877730541452,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0458733278338475,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.083609075774248,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.1554749990386948,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.222908019686529,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.286461829806158,"velocity":1.7,"acceleration":-0.8218715729622541,"pose":{"translation":{"x":6.80966796875,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.347697529046253,"velocity":1.6496721195440995,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.912227725982667,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.4105333918156933,"velocity":1.4800152900666113,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.010556030273439,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.479345141764698,"velocity":1.2942235652042982,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.106006145477297,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.5585075755357845,"velocity":1.0804849940223653,"acceleration":-2.7,"pose":{"translation":{"x":7.200000000000003,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.6042166199637675,"velocity":0.9570705740668113,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":3.628845057991022,"velocity":0.8905737913932243,"acceleration":-2.700000000000001,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":3.654785556334576,"velocity":0.8205344458656272,"acceleration":-2.700000000000001,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":3.682306030520562,"velocity":0.7462291655634643,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":3.6968123180655597,"velocity":0.7070621891919708,"acceleration":-2.700000000000004,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":3.711943304136287,"velocity":0.6662085268010068,"acceleration":-2.3024511231865272,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":3.727763704285853,"velocity":0.6297828287073786,"acceleration":-1.8675460534493051,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":3.7442564742654794,"velocity":0.5989818212214805,"acceleration":-1.461370960203089,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":3.761374145655594,"velocity":0.5739665533456683,"acceleration":-1.079676100951081,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":3.779048069734428,"velocity":0.554884439907727,"acceleration":-0.718302000748944,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":3.797188249742144,"velocity":0.5418543123142384,"acceleration":-0.37292501733286726,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":3.815685825495559,"velocity":0.5349561035557802,"acceleration":-0.03916357131337287,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":3.8344183342629083,"velocity":0.5342224716127917,"acceleration":0.2870415628660772,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":3.8532572591365133,"velocity":0.5396300260512279,"acceleration":0.6088741709257113,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":3.872076786291892,"velocity":0.5510887500451729,"acceleration":0.9280335229053246,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":3.890762354669351,"velocity":0.5684295838939945,"acceleration":1.2441992183952022,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":3.9092176408079555,"velocity":0.5913916364829062,"acceleration":1.5547030307947924,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":3.9273690525075233,"velocity":0.6196116912654284,"acceleration":1.8545226594627813,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":3.9451674273627373,"velocity":0.6526191807360355,"acceleration":2.136663336874829,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":3.9625872153734303,"velocity":0.6898394031146146,"acceleration":2.3929144893007783,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":3.9796238031223674,"velocity":0.7306065007872906,"acceleration":2.614876812845623,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":3.9962897632615246,"velocity":0.7741859335189818,"acceleration":2.7000000000000375,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":4.012626214006347,"velocity":0.8182943505300027,"acceleration":2.7000000000000224,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":4.028698933575373,"velocity":0.8616906933663744,"acceleration":2.6999999999999695,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":4.060277976926194,"velocity":0.9469541104135919,"acceleration":2.6999999999999877,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":4.091362946346115,"velocity":1.0308835278473796,"acceleration":2.7,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":4.1221343718395556,"velocity":1.1139663766796675,"acceleration":2.699999999999995,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":4.152684453160756,"velocity":1.1964515962469084,"acceleration":2.7000000000000197,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":4.183051046853612,"velocity":1.2784413992176227,"acceleration":1.731750167430012,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":4.244487118787202,"velocity":1.3848333270748585,"acceleration":1.171492568377572,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":4.3078704676180335,"velocity":1.4590864491890612,"acceleration":0.7858676493625462,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":4.373502709066009,"velocity":1.5106647044981765,"acceleration":0.5250113170323213,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":4.441238407138915,"velocity":1.5462267125535365,"acceleration":0.34981100784937635,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":4.510644634358841,"velocity":1.5705057748483617,"acceleration":0.22889755547366647,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":4.581096203420193,"velocity":1.5866319667857893,"acceleration":0.14022072554540496,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":4.651847507370679,"velocity":1.59655276595901,"acceleration":0.06824791072810449,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":4.722089318477431,"velocity":1.601346622812804,"acceleration":7.56678468946939E-4,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":4.790996593898797,"velocity":1.601398763464469,"acceleration":-0.07418428163614232,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":4.857771547799512,"velocity":1.596445111478058,"acceleration":-0.17154801804007855,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":4.921685240211905,"velocity":1.5854808442190889,"acceleration":-0.3139410530415669,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":4.982120604408644,"velocity":1.5665077023422138,"acceleration":-1.3509793267586387,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":5.039490530027777,"velocity":1.4890021188530849,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":5.09547389783896,"velocity":1.3378470257628925,"acceleration":-2.7,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":5.1508750599679844,"velocity":1.1882638880145262,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":5.205153635996704,"velocity":1.0417117327369836,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":5.257851728968148,"velocity":0.8994268817140864,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":5.308761976080621,"velocity":0.7619692145104063,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":5.333641393069134,"velocity":0.6947947886414234,"acceleration":-2.7,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":5.35831606127708,"velocity":0.6281731844799698,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":5.383073842576339,"velocity":0.5613271749719707,"acceleration":-2.7,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":5.408415044561957,"velocity":0.4929059296108012,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":5.43522216804897,"velocity":0.42052669619586686,"acceleration":-2.7,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":5.46520746437371,"velocity":0.33956639611906964,"acceleration":-2.7,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":5.590972796269662,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22851073171807768,"velocity":0.6855321951542331,"acceleration":3.0,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.32286738548734767,"velocity":0.9686021564620431,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.3948508967117983,"velocity":1.1845526901353949,"acceleration":3.000000000000002,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.45504953863530545,"velocity":1.3651486159059165,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5075769496703644,"velocity":1.5227308490110931,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5545708746789335,"velocity":1.6637126240368005,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.5973365928558361,"velocity":1.7920097785675082,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6367625752176861,"velocity":1.9102877256530584,"acceleration":-0.39480980187976056,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.6747121786647302,"velocity":1.8953048502347154,"acceleration":-0.7021730002147804,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.7127763389927304,"velocity":1.868577224576547,"acceleration":-0.4401004459228274,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.7511092582596577,"velocity":1.8517068897136486,"acceleration":-0.17400254925795713,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.7896601305578692,"velocity":1.8449989396576418,"acceleration":0.08372895263229682,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.8283828420132677,"velocity":1.848241151730885,"acceleration":0.32040119497795183,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":0.8672396362007758,"velocity":1.860690915021575,"acceleration":0.5251363087983144,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":0.906204315334355,"velocity":1.8811526827952934,"acceleration":0.6905017989409158,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":0.9452636685748346,"velocity":1.9081232364733132,"acceleration":0.8133740442414022,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":0.9844165273519864,"velocity":1.9399691555604976,"acceleration":0.8949317659517233,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.0236706441727126,"velocity":1.9750989116477453,"acceleration":0.9399001600451896,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.063038145418846,"velocity":2.0121004323695653,"acceleration":0.9553480479826941,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.1025304872768191,"velocity":2.049829364073845,"acceleration":0.9494024824000104,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.1421537132339343,"velocity":2.087447753158227,"acceleration":0.9301775648872205,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.1819045291156953,"velocity":2.1244230702774036,"acceleration":0.9050734432547365,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.221767420455895,"velocity":2.1605019146007676,"acceleration":0.8804626256424855,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.2617128175476713,"velocity":2.1956723438065247,"acceleration":0.8616916300805181,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.3016961872085873,"velocity":2.2301256787857513,"acceleration":0.8532946962433116,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.341657884138109,"velocity":2.264224782828595,"acceleration":0.8593254128101726,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.3815236046845703,"velocity":2.2984824095941576,"acceleration":0.8837378969826192,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.42120533048524,"velocity":2.3335506545018823,"acceleration":0.9307724426536456,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.4606027154283423,"velocity":2.3702206547195397,"acceleration":1.0053080180459415,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.4996049518717887,"velocity":2.40942991573786,"acceleration":1.113123412063664,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":1.5380932533029306,"velocity":2.4522721451514276,"acceleration":1.2609415748588182,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":1.575944217852111,"velocity":2.4999999999999964,"acceleration":9.47993122549823E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":1.6134203724179241,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6507579461846327,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6878305177006696,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7245245401177383,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.76073891203737,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.796384548357484,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8313839511189436,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.865670780352113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8991894249234176,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9318945733818982,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.963750784805772,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9947320596489881,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.024821410587785,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.05401043336725,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.082298877647874,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.109694217852113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.136211224010942,"velocity":2.5,"acceleration":-2.9297166719305383,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1622694050077933,"velocity":2.423656912693341,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.2143566851934087,"velocity":2.2673950721364946,"acceleration":-2.999999999999996,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.266729803510551,"velocity":2.110275717185067,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3199401431120066,"velocity":1.9506446983807002,"acceleration":-3.0,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3748347556748115,"velocity":1.7859608606922859,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.432703611156226,"velocity":1.6123542942480418,"acceleration":-3.0,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.495580885042337,"velocity":1.4237224725897093,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5669701453930265,"velocity":1.2095546915376412,"acceleration":-2.999999999999998,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6074049251020424,"velocity":1.0882503524105933,"acceleration":-3.000000000000005,"pose":{"translation":{"x":7.246453259419017,"y":-1.100468353001181},"rotation":{"radians":-0.03029627796319239}},"curvature":-1.319268880795511},{"time":2.628698299737897,"velocity":1.0243702285030287,"acceleration":-3.0000000000000027,"pose":{"translation":{"x":7.268919966550246,"y":-1.1015433468916538},"rotation":{"radians":-0.06812545247042903}},"curvature":-2.0669897873012513},{"time":2.6505796043600514,"velocity":0.9587263146365661,"acceleration":-3.0,"pose":{"translation":{"x":7.290521387883752,"y":-1.1035709212816442},"rotation":{"radians":-0.12227213585990128}},"curvature":-2.9587916487214816},{"time":2.673013681367624,"velocity":0.8914240836138477,"acceleration":-3.0,"pose":{"translation":{"x":7.311020896661181,"y":-1.1068060090328942},"rotation":{"radians":-0.19454364441226238}},"curvature":-4.050132379901242},{"time":2.684453521072769,"velocity":0.8571045644984139,"acceleration":-3.0,"pose":{"translation":{"x":7.320790317439845,"y":-1.108947806006542},"rotation":{"radians":-0.23814906997127733}},"curvature":-4.681287981030412},{"time":2.6960641268278294,"velocity":0.822272747233233,"acceleration":-3.0,"pose":{"translation":{"x":7.330206758370542,"y":-1.111473547983105},"rotation":{"radians":-0.2870838732590895}},"curvature":-5.368039436240083},{"time":2.707877775932446,"velocity":0.786831799919383,"acceleration":-3.0000000000000058,"pose":{"translation":{"x":7.339247504131675,"y":-1.1144068404739353},"rotation":{"radians":-0.34155174820354145}},"curvature":-6.10144429198697},{"time":2.7199428695455974,"velocity":0.7506365190799287,"acceleration":-3.0,"pose":{"translation":{"x":7.347891290084308,"y":-1.1177695476378748},"rotation":{"radians":-0.4016577762334248}},"curvature":-6.863367195390947},{"time":2.732328983652657,"velocity":0.7134781767587501,"acceleration":-2.999999999999997,"pose":{"translation":{"x":7.356118276001478,"y":-1.121581825615378},"rotation":{"radians":-0.4673549943172977}},"curvature":-7.6244722420236295},{"time":2.745133815359972,"velocity":0.6750636816368045,"acceleration":-2.6366830196581335,"pose":{"translation":{"x":7.363910019797516,"y":-1.125862155862635},"rotation":{"radians":-0.5383877761898741}},"curvature":-8.343686165228133},{"time":2.7584424810573904,"velocity":0.6399729487781146,"acceleration":-1.7769445288668568,"pose":{"translation":{"x":7.371249451257359,"y":-1.1306273784856944},"rotation":{"radians":-0.6142422608915585}},"curvature":-8.970417020168014},{"time":2.772233316981256,"velocity":0.6154673983347015,"acceleration":-0.9662749893703216,"pose":{"translation":{"x":7.378120845765868,"y":-1.1358927255745868},"rotation":{"radians":-0.694118349599752}},"curvature":-9.450474002120954},{"time":2.7863879742209994,"velocity":0.6017901070608277,"acceleration":-0.1875831048849451,"pose":{"translation":{"x":7.384509798037142,"y":-1.1416718545374471},"rotation":{"radians":-0.776938635525735}},"curvature":-9.735408476030893},{"time":2.800761549923939,"velocity":0.5990938671021715,"acceleration":0.5786763545271786,"pose":{"translation":{"x":7.390403195843834,"y":-1.1479768814346385},"rotation":{"radians":-0.8614041317901993}},"curvature":-9.793113499421619},{"time":2.8151949088373547,"velocity":0.6074461106217692,"acceleration":1.3512500497389404,"pose":{"translation":{"x":7.3957891937464675,"y":-1.154818414312874},"rotation":{"radians":-0.9460943844775829}},"curvature":-9.616022896217997},{"time":2.829530428985466,"velocity":0.6268169829349379,"acceleration":2.1444781995587228,"pose":{"translation":{"x":7.40065718682275,"y":-1.162205586539341},"rotation":{"radians":-1.0295945659911594}},"curvature":-9.223472277259154},{"time":2.843627613085366,"velocity":0.6570480869123403,"acceleration":2.964815882581206,"pose":{"translation":{"x":7.404997784396893,"y":-1.1701460901358236},"rotation":{"radians":-1.1106218383771238}},"curvature":-8.657088121620294},{"time":2.8573750588687528,"velocity":0.6978067325158486,"acceleration":3.0000000000000604,"pose":{"translation":{"x":7.408802783768922,"y":-1.178646209112826},"rotation":{"radians":-1.1881235850165326}},"curvature":-7.971153960622058},{"time":2.870793870807247,"velocity":0.7380631683313326,"acceleration":2.999999999999978,"pose":{"translation":{"x":7.412065143943997,"y":-1.1877108528036944},"rotation":{"radians":-1.2613308478052403}},"curvature":-7.221753953199051},{"time":2.8839989159104573,"velocity":0.7776783036409628,"acceleration":3.0000000000000213,"pose":{"translation":{"x":7.414778959361725,"y":-1.1973435891987416},"rotation":{"radians":-1.3297653307739847}},"curvature":-6.4581578382775735},{"time":2.8970797192684477,"velocity":0.8169207137149346,"acceleration":3.0,"pose":{"translation":{"x":7.416939433625478,"y":-1.207546678279369},"rotation":{"radians":-1.393210316952152}},"curvature":-5.718104209264924},{"time":2.9101026324067005,"velocity":0.8559894531296925,"acceleration":3.000000000000005,"pose":{"translation":{"x":7.418542853231706,"y":-1.2183211053521896},"rotation":{"radians":-1.4516609735687045}},"curvature":-5.0267761645186875},{"time":2.9231160981259343,"velocity":0.8950298502873935,"acceleration":3.0000000000000324,"pose":{"translation":{"x":7.419586561299257,"y":-1.2296666143831518},"rotation":{"radians":-1.5052684214084193}},"curvature":-4.398243194153372},{"time":2.9361546373251755,"velocity":0.9341454678851169,"acceleration":2.9999999999999654,"pose":{"translation":{"x":7.420068931298684,"y":-1.2415817413316619},"rotation":{"radians":-1.5542875714605175}},"curvature":-3.838016276555127},{"time":2.962387530612709,"velocity":1.0128441477477164,"acceleration":3.000000000000036,"pose":{"translation":{"x":7.419348145109851,"y":-1.2671091527909772},"rotation":{"radians":-1.6398507277113328}},"curvature":-2.917417225974206},{"time":2.988908873228704,"velocity":1.0924081755957016,"acceleration":2.9999999999999782,"pose":{"translation":{"x":7.416387091177872,"y":-1.294868733971218},"rotation":{"radians":-1.7110773325418644}},"curvature":-2.2285872384297742},{"time":3.01573516597123,"velocity":1.172887053823279,"acceleration":2.9999999999999627,"pose":{"translation":{"x":7.411207170318989,"y":-1.3248086843926672},"rotation":{"radians":-1.770576272089184}},"curvature":-1.7200119058360206},{"time":3.0428355369759528,"velocity":1.254188166837447,"acceleration":3.0000000000000124,"pose":{"translation":{"x":7.4038437469911225,"y":-1.3568610755467443},"rotation":{"radians":-1.820592107940096}},"curvature":-1.344952042416304},{"time":3.070153054044293,"velocity":1.3361407180424683,"acceleration":2.9999999999999902,"pose":{"translation":{"x":7.394345308631966,"y":-1.3909429175879429},"rotation":{"radians":-1.8629610881600354}},"curvature":-1.066776993002733},{"time":3.1251464334866763,"velocity":1.5011208563696172,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.369197907498064,"y":-1.4647940884166677},"rotation":{"radians":-1.9303207828865319}},"curvature":-0.700913398882251},{"time":3.1801254299921244,"velocity":1.6660578458859614,"acceleration":3.000000000000007,"pose":{"translation":{"x":7.336383380862248,"y":-1.545437585669859},"rotation":{"radians":-1.9810854450552438}},"curvature":-0.48678344759105546},{"time":3.234502974744613,"velocity":1.8291904801434278,"acceleration":2.9999999999999933,"pose":{"translation":{"x":7.296676307984395,"y":-1.6317760850316636},"rotation":{"radians":-2.0205587822636133}},"curvature":-0.35581674367284205},{"time":3.2877480590960606,"velocity":1.9889257331977697,"acceleration":2.9999999999999876,"pose":{"translation":{"x":7.250980532258531,"y":-1.7225736832213316},"rotation":{"radians":-2.052154330745463}},"curvature":-0.272676387194201},{"time":3.3393922916510848,"velocity":2.1438584308628412,"acceleration":2.999999999999992,"pose":{"translation":{"x":7.200302260032064,"y":-1.8164900321351913},"rotation":{"radians":-2.078142236038709}},"curvature":-0.21848488416291567},{"time":3.389026520853825,"velocity":2.2927611184710606,"acceleration":1.7602617899017508,"pose":{"translation":{"x":7.145723159425027,"y":-1.9121144729886241},"rotation":{"radians":-2.1000895426348007}},"curvature":-0.18279950109004067},{"time":3.4368781221491154,"velocity":2.376992463816774,"acceleration":-0.448180883447394,"pose":{"translation":{"x":7.088373459149306,"y":-2.008000170458039},"rotation":{"radians":-2.119124178347165}},"curvature":-0.15971985407340575},{"time":3.4840196932847407,"velocity":2.355864512818111,"acceleration":-2.999999999999996,"pose":{"translation":{"x":7.029405047327874,"y":-2.1026982468228463},"rotation":{"radians":-2.136099285795778}},"curvature":-0.14596242302645385},{"time":3.532012815317769,"velocity":2.2118851467190273,"acceleration":-3.000000000000004,"pose":{"translation":{"x":6.969964570314035,"y":-2.194791916107434},"rotation":{"radians":-2.151701710677999}},"curvature":-0.1399205308761633},{"time":3.5815797871141575,"velocity":2.0631842313298616,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":6.911166531510655,"y":-2.28293061822314},"rotation":{"radians":-2.1665305881291537}},"curvature":-0.14127792706547193},{"time":3.6322493880323914,"velocity":1.91117542857516,"acceleration":-3.0,"pose":{"translation":{"x":6.854066390189391,"y":-2.365864153110232},"rotation":{"radians":-2.1811630644764497}},"curvature":-0.15102290082887804},{"time":3.6834840642992823,"velocity":1.7574713997744869,"acceleration":-3.0,"pose":{"translation":{"x":6.799633660309938,"y":-2.442476814879872},"rotation":{"radians":-2.196220892033287}},"curvature":-0.17190256606202614},{"time":3.7346685295655555,"velocity":1.6039180039756675,"acceleration":-3.0,"pose":{"translation":{"x":6.748725009339258,"y":-2.5118215259561016},"rotation":{"radians":-2.212452725775109}},"curvature":-0.20959133303036753},{"time":3.7850968736454673,"velocity":1.4526329717359314,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.702057357070814,"y":-2.5731539712178138},"rotation":{"radians":-2.230852906292372}},"curvature":-0.2752960353954891},{"time":3.8803425077482427,"velocity":1.1668960694276058,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.623452582362421,"y":-2.670023420939335},"rotation":{"radians":-2.2806214520910677}},"curvature":-0.6053526530580257},{"time":3.9232680649365714,"velocity":1.0381193978626204,"acceleration":-3.000000000000003,"pose":{"translation":{"x":6.592008450515033,"y":-2.7053928147089477},"rotation":{"radians":-2.3176021111430174}},"curvature":-1.0180108404391697},{"time":3.961759655139707,"velocity":0.9226446272532131,"acceleration":-3.0,"pose":{"translation":{"x":6.565737496193482,"y":-2.7324829895675955},"rotation":{"radians":-2.3691895004088437}},"curvature":-1.8453908893564248},{"time":3.995077693076158,"velocity":0.8226905134438611,"acceleration":-3.0,"pose":{"translation":{"x":6.544254383112274,"y":-2.752075454798034},"rotation":{"radians":-2.4429519712858796}},"curvature":-3.4712152639078635},{"time":4.009724957254997,"velocity":0.7787487209073424,"acceleration":-3.0,"pose":{"translation":{"x":6.535106238022585,"y":-2.759414778783516},"rotation":{"radians":-2.4903369045713104}},"curvature":-4.677026935702093},{"time":4.023110582984799,"velocity":0.7385918437179384,"acceleration":-3.0,"pose":{"translation":{"x":6.526872620227842,"y":-2.7653592869897214},"rotation":{"radians":-2.5444599796315197}},"curvature":-6.007139028985739},{"time":4.035399953953152,"velocity":0.7017237308128799,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.519416249441682,"y":-2.7701268965146633},"rotation":{"radians":-2.6024368965833506}},"curvature":-6.995883284614772},{"time":4.0468560526731014,"velocity":0.6673554346530308,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.512577660557761,"y":-2.773965264180783},"rotation":{"radians":-2.657556932845574}},"curvature":-6.765796527922793},{"time":4.0688471551397685,"velocity":0.6013821272530296,"acceleration":-3.0,"pose":{"translation":{"x":6.499999999999997,"y":-2.7800000000000016},"rotation":{"radians":-2.714965160462927}},"curvature":-2.6555993490677373E-13},{"time":4.09253186587704,"velocity":0.5303279950412128,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.487723377682077,"y":-2.785375982625394},"rotation":{"radians":-2.7564018109602286}},"curvature":-6.15200093903577},{"time":4.105398508519192,"velocity":0.4917280671147566,"acceleration":-3.0,"pose":{"translation":{"x":6.481573363494451,"y":-2.787702090646249},"rotation":{"radians":-2.8069964803170873}},"curvature":-9.262660823828876},{"time":4.1190929136917545,"velocity":0.45064485159707074,"acceleration":-3.0,"pose":{"translation":{"x":6.475411413096477,"y":-2.789616940520493},"rotation":{"radians":-2.8770511086188177}},"curvature":-12.466539743546049},{"time":4.126323741848238,"velocity":0.4289523671276198,"acceleration":-3.0,"pose":{"translation":{"x":6.472325400347914,"y":-2.790384803317061},"rotation":{"radians":-2.919246803028493}},"curvature":-14.067221772378913},{"time":4.133870956794675,"velocity":0.40631072228830745,"acceleration":-2.9999999999999956,"pose":{"translation":{"x":6.469235868121625,"y":-2.7910089917291048},"rotation":{"radians":-2.9660566541833933}},"curvature":-15.623161265956721},{"time":4.141802655384934,"velocity":0.38251562651753157,"acceleration":-2.778995935541285,"pose":{"translation":{"x":6.466142779184198,"y":-2.791477475751396},"rotation":{"radians":-3.0172537129004535}},"curvature":-17.085434277986494},{"time":4.150191997169828,"velocity":0.3592016797954446,"acceleration":-1.9621857442111865,"pose":{"translation":{"x":6.463046161219631,"y":-2.791778965285789},"rotation":{"radians":-3.0725012120644393}},"curvature":-18.394686312759717},{"time":4.159043281527767,"velocity":0.3418338158103368,"acceleration":-1.2827823996625713,"pose":{"translation":{"x":6.459946105193779,"y":-2.7919028969218647},"rotation":{"radians":-3.1313372571696307}},"curvature":-19.486096556994575},{"time":4.1682839075227145,"velocity":0.3299801034221532,"acceleration":-0.6940623511654422,"pose":{"translation":{"x":6.456842763718804,"y":-2.7918394207175727},"rotation":{"radians":3.090014628006359}},"curvature":-20.29695791936882},{"time":4.177826543964612,"velocity":0.32335691873697314,"acceleration":-0.16050191244357367,"pose":{"translation":{"x":6.453736349417611,"y":-2.7915793869798775},"rotation":{"radians":3.0258936564331744}},"curvature":-20.775909875903174},{"time":4.187572511375943,"velocity":0.3217926723288418,"acceleration":0.3463169056488674,"pose":{"translation":{"x":6.450627133288312,"y":-2.791114333045399},"rotation":{"radians":2.9602868160099893}},"curvature":-20.891905566984803},{"time":4.197417008769472,"velocity":0.32520198820383717,"acceleration":0.8499934016959338,"pose":{"translation":{"x":6.447515443068656,"y":-2.790436470061059},"rotation":{"radians":2.8940465389525842}},"curvature":-20.640524308152266},{"time":4.207255484348972,"velocity":0.3335646275291587,"acceleration":1.370466137587277,"pose":{"translation":{"x":6.444401661600485,"y":-2.789538669764723},"rotation":{"radians":2.82802600857349}},"curvature":-20.04567743561068},{"time":4.21699013611279,"velocity":0.34690563813267716,"acceleration":1.9244106184216545,"pose":{"translation":{"x":6.4412862251941805,"y":-2.7884144512658433},"rotation":{"radians":2.763028051590503}},"curvature":-19.156082341287096},{"time":4.226535495067351,"velocity":0.36527482826148017,"acceleration":2.5250992915133477,"pose":{"translation":{"x":6.438169621993106,"y":-2.7870579678261054},"rotation":{"radians":2.6997609889715117}},"curvature":-18.037536199313763},{"time":4.235822305577407,"velocity":0.38872494690084025,"acceleration":2.9999999999999347,"pose":{"translation":{"x":6.435052390338058,"y":-2.7854639936400676},"rotation":{"radians":2.6388075458793896}},"curvature":-16.763218002949678},{"time":4.244816993183615,"velocity":0.4157090097194641,"acceleration":2.999999999999937,"pose":{"translation":{"x":6.431935117131708,"y":-2.783627910615809},"rotation":{"radians":2.580609550420515}},"curvature":-15.404481952570594},{"time":4.253557817463828,"velocity":0.44193148256010456,"acceleration":2.999999999999754,"pose":{"translation":{"x":6.4288184362030565,"y":-2.781545695155569},"rotation":{"radians":2.5254676721113087}},"curvature":-14.02394310711014},{"time":4.262114737385764,"velocity":0.46760224232591113,"acceleration":3.0000000000002505,"pose":{"translation":{"x":6.42570302667187,"y":-2.779213904936394},"rotation":{"radians":2.473552943635361}},"curvature":-12.671584893555588},{"time":4.2705400811810454,"velocity":0.4928782737117564,"acceleration":3.000000000000211,"pose":{"translation":{"x":6.422589611313139,"y":-2.7766296656907787},"rotation":{"radians":2.424925719879724}},"curvature":-11.383666243410126},{"time":4.278873308336619,"velocity":0.5178779551784787,"acceleration":2.999999999999947,"pose":{"translation":{"x":6.419478954921514,"y":-2.7737906579873117},"rotation":{"radians":2.3795578870961305}},"curvature":-10.183662676609572},{"time":4.2953720290809185,"velocity":0.5673741174113744,"acceleration":2.999999999999966,"pose":{"translation":{"x":6.413269178503195,"y":-2.7673417543454994},"rotation":{"radians":2.298176568793801}},"curvature":-8.090290466124452},{"time":4.311774881248613,"velocity":0.6165826739144591,"acceleration":2.999999999999905,"pose":{"translation":{"x":6.4070805940164055,"y":-2.7598592329459772},"rotation":{"radians":2.228196497731612}},"curvature":-6.409038928054524},{"time":4.328167351522133,"velocity":0.6657600847350147,"acceleration":3.0000000000000635,"pose":{"translation":{"x":6.4009206656998625,"y":-2.7513431640624413},"rotation":{"radians":2.168122245297885}},"curvature":-5.093159786686278},{"time":4.344589432692641,"velocity":0.7150263282465403,"acceleration":3.000000000000073,"pose":{"translation":{"x":6.39479737309381,"y":-2.7418012262878593},"rotation":{"radians":2.1164593903065523}},"curvature":-4.075911440438974},{"time":4.361052702476709,"velocity":0.7644161375987443,"acceleration":3.0000000000000164,"pose":{"translation":{"x":6.388719158702311,"y":-2.7312482835150664},"rotation":{"radians":2.071850864957236}},"curvature":-3.292517826037708},{"time":4.394062291691346,"velocity":0.8634449052426559,"acceleration":3.000000000000013,"pose":{"translation":{"x":6.376733735372094,"y":-2.707202226929091},"rotation":{"radians":1.999295180651931}},"curvature":-2.220128264384656},{"time":4.427048764659495,"velocity":0.9624043241471033,"acceleration":2.99999999999999,"pose":{"translation":{"x":6.365039206185345,"y":-2.6794515367071394},"rotation":{"radians":1.9432042411349209}},"curvature":-1.5681810532119427},{"time":4.459789132404415,"velocity":1.0606254273818627,"acceleration":3.0000000000000075,"pose":{"translation":{"x":6.353714439426232,"y":-2.6483306468077847},"rotation":{"radians":1.8986262626650925}},"curvature":-1.160299877845927},{"time":4.52358829592402,"velocity":1.2520229179406772,"acceleration":2.9999999999999836,"pose":{"translation":{"x":6.332499906528562,"y":-2.5776742418651493},"rotation":{"radians":1.8315691084032764}},"curvature":-0.7265746327828503},{"time":4.583735724786862,"velocity":1.4324652045292052,"acceleration":1.5279486263191557,"pose":{"translation":{"x":6.3137385964647486,"y":-2.4991519232923034},"rotation":{"radians":1.78154648707069}},"curvature":-0.5414226677913919},{"time":4.640266930323582,"velocity":1.5188419823732027,"acceleration":-3.0,"pose":{"translation":{"x":6.298036706330595,"y":-2.417222523434155},"rotation":{"radians":1.7395970696539222}},"curvature":-0.4804143150401365},{"time":4.697249475366745,"velocity":1.3478943472437126,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.2859045767228015,"y":-2.336451623918702},"rotation":{"radians":1.6997834499194502}},"curvature":-0.5096799592935836},{"time":4.7575445403092,"velocity":1.1670091524163468,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.277703097929729,"y":-2.2610783837863337},"rotation":{"radians":1.6568769709629299}},"curvature":-0.6464570557332997},{"time":4.819579738139942,"velocity":0.9809035589241215,"acceleration":-3.0,"pose":{"translation":{"x":6.273590116122153,"y":-2.194582367619131},"rotation":{"radians":1.6047453360448525}},"curvature":-0.9688050763451119},{"time":4.881934875316774,"velocity":0.7938381473936263,"acceleration":-3.0,"pose":{"translation":{"x":6.273466839544032,"y":-2.139250373670167},"rotation":{"radians":1.5350878663689433}},"curvature":-1.6539869192725656},{"time":4.9130314508955655,"velocity":0.7005484206572529,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.274787104757643,"y":-2.116052761548466},"rotation":{"radians":1.4906625351913902}},"curvature":-2.2070158970322034},{"time":4.944272122980406,"velocity":0.606826404402731,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.276924244703256,"y":-2.0957432619928063},"rotation":{"radians":1.4389161500512866}},"curvature":-2.889519609489578},{"time":4.976290130944924,"velocity":0.5107723805091781,"acceleration":-3.0,"pose":{"translation":{"x":6.279769865001267,"y":-2.078079362406115},"rotation":{"radians":1.3812845576470563}},"curvature":-3.531419002153032},{"time":5.01067971909592,"velocity":0.40760361605619033,"acceleration":-3.0,"pose":{"translation":{"x":6.283189482562416,"y":-2.0626627825700066},"rotation":{"radians":1.3233406589190928}},"curvature":-3.668111811549847},{"time":5.1465475911146505,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":6.29107228472956,"y":-2.036118403443619},"rotation":{"radians":1.257895098914613}},"curvature":6.632633907880963E-13}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid0.wpilib.json b/src/main/deploy/paths/SixBallMid0.wpilib.json index 9697957..2b339ea 100644 --- a/src/main/deploy/paths/SixBallMid0.wpilib.json +++ b/src/main/deploy/paths/SixBallMid0.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22844018981162115,"velocity":0.6853205694348635,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2065095854674475,"y":-2.321993759274483},"rotation":{"radians":1.4808013128224615}},"curvature":-0.25415239252139715},{"time":0.3224986666860846,"velocity":0.9674960000582539,"acceleration":2.999999999999999,"pose":{"translation":{"x":3.2144920400116974,"y":-2.244674015045166},"rotation":{"radians":1.4521039763788177}},"curvature":-0.48102289742329407},{"time":0.39393292568025073,"velocity":1.181798777040752,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2251942805954354,"y":-2.168657049536705},"rotation":{"radians":1.407083956538682}},"curvature":-0.6895659762146151},{"time":0.45338587226770455,"velocity":1.3601576168031135,"acceleration":2.999999999999999,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0944854736328122},"rotation":{"radians":1.3476355522918868}},"curvature":-0.8811082747650646},{"time":0.5050764160290824,"velocity":1.515229248087247,"acceleration":2.999999999999998,"pose":{"translation":{"x":3.2585973431481516,"y":-2.0226314455270766},"rotation":{"radians":1.2757042704692263}},"curvature":-1.0503191132212641},{"time":0.5512787765713811,"velocity":1.6538363297141432,"acceleration":2.999999999999997,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9534998893737792},"rotation":{"radians":1.1935567050390803}},"curvature":-1.1871900193146532},{"time":0.5728003854706883,"velocity":1.7184011564120647,"acceleration":2.339470754230812,"pose":{"translation":{"x":3.29679072395957,"y":-1.9200639848597347},"rotation":{"radians":1.1494812806441845}},"curvature":-1.239928580956994},{"time":0.5935363700192974,"velocity":1.7669123858237175,"acceleration":-0.5436899956399454,"pose":{"translation":{"x":3.3123124503261456,"y":-1.887431713938713},"rotation":{"radians":1.1039001836454154}},"curvature":-1.2805479837540814},{"time":0.613997248730107,"velocity":1.755788010766648,"acceleration":-0.27821667617747475,"pose":{"translation":{"x":3.3292782820223583,"y":-1.8556362158618867},"rotation":{"radians":1.0572084270862476}},"curvature":-1.308216350703189},{"time":0.6345348059123607,"velocity":1.7500741198705965,"acceleration":-0.022743667612852538,"pose":{"translation":{"x":3.347702503129696,"y":-1.82470703125},"rotation":{"radians":1.009808701952868}},"curvature":-1.3225645836950348},{"time":0.6551218107400116,"velocity":1.7496058958756522,"acceleration":0.21836662236809756,"pose":{"translation":{"x":3.3675900610791567,"y":-1.7946702026762067},"rotation":{"radians":0.96210010066623}},"curvature":-1.3237445021353418},{"time":0.6757329851623971,"velocity":1.7541066884173082,"acceleration":0.44094172089548617,"pose":{"translation":{"x":3.388937040133019,"y":-1.765548375248909},"rotation":{"radians":0.9144663557730784}},"curvature":-1.312428637189129},{"time":0.6963457035139206,"velocity":1.763195695919563,"acceleration":0.6413101666108914,"pose":{"translation":{"x":3.411731134866622,"y":-1.737360897194594},"rotation":{"radians":0.8672647742046862}},"curvature":-1.2897532381321657},{"time":0.7169405333077539,"velocity":1.7764033696459691,"acceleration":0.8165558577958478,"pose":{"translation":{"x":3.435952123650134,"y":-1.7101239204406737},"rotation":{"radians":0.8208169033196313}},"curvature":-1.2572161094450898},{"time":0.7375015438150363,"velocity":1.7931925832178925,"acceleration":0.9647011548897362,"pose":{"translation":{"x":3.461572342130331,"y":-1.6838505011983216},"rotation":{"radians":0.7754016677331865}},"curvature":-1.2165477334674017},{"time":0.7580163507583728,"velocity":1.8129832411684692,"acceleration":1.1313619301261402,"pose":{"translation":{"x":3.4885571567123694,"y":-1.6585507005453108},"rotation":{"radians":0.7312513370847253}},"curvature":-1.1695762249806743},{"time":0.7988546318429788,"velocity":1.859186117679383,"acceleration":1.2628229077088666,"pose":{"translation":{"x":3.5464500344851415,"y":-1.6108978271484373},"rotation":{"radians":0.6474363689665855}},"curvature":-1.0638099142624002},{"time":0.83944138598433,"velocity":1.910440000558629,"acceleration":1.300058189471748,"pose":{"translation":{"x":3.6092322956847354,"y":-1.567189708352089},"rotation":{"radians":0.5703086691540483}},"curvature":-0.9524662524724097},{"time":0.8797609609692681,"velocity":1.9628577942138181,"acceleration":1.265808259413599,"pose":{"translation":{"x":3.676424273995083,"y":-1.527409267425537},"rotation":{"radians":0.5001906802940327}},"curvature":-0.8446084678380373},{"time":0.9198040484146826,"velocity":2.0135446650346447,"acceleration":1.1846535633197064,"pose":{"translation":{"x":3.7474802494845436,"y":-1.491501161456108},"rotation":{"radians":0.4369591767127109}},"curvature":-0.7456528682493916},{"time":0.9595516846457711,"velocity":2.060631843929339,"acceleration":1.078307049602526,"pose":{"translation":{"x":3.8218036000226827,"y":-1.459375},"rotation":{"radians":0.3802110136115068}},"curvature":-0.6580867707002909},{"time":0.9989652615913103,"velocity":2.1031317817997657,"acceleration":0.9634000918400376,"pose":{"translation":{"x":3.8987619526970523,"y":-1.430908563733101},"rotation":{"radians":0.3293970719543265}},"curvature":-0.5824180484673191},{"time":1.0379824885977187,"velocity":2.140720981881083,"acceleration":0.8513310740584539,"pose":{"translation":{"x":3.9777023352299703,"y":-1.4059510231018066},"rotation":{"radians":0.2839173004495175}},"curvature":-0.5179964450188761},{"time":1.0765180844428441,"velocity":2.1735275320813963,"acceleration":0.7492837612540003,"pose":{"translation":{"x":4.057966327395298,"y":-1.384326156973839},"rotation":{"radians":0.2431817004482161}},"curvature":-0.4635925052478776},{"time":1.1144677175905173,"velocity":2.2019625759444943,"acceleration":-0.6986847648307831,"pose":{"translation":{"x":4.138905212435224,"y":-1.3658355712890626},"rotation":{"radians":0.2066461302891754}},"curvature":-0.4177494217263637},{"time":1.152147581490706,"velocity":2.175636229096535,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":4.219895128477039,"y":-1.3502619177103043},"rotation":{"radians":0.17383156520200765}},"curvature":-0.37896519682071883},{"time":1.1906206177395262,"velocity":2.060217120350075,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":4.300352219949918,"y":-1.3373721122741702},"rotation":{"radians":0.14433362331149865}},"curvature":-0.3457618351949617},{"time":1.2306576488017948,"velocity":1.940106027163269,"acceleration":-3.0,"pose":{"translation":{"x":4.3797477890017005,"y":-1.326920554041863},"rotation":{"radians":0.11782713826751266}},"curvature":-0.3166843715937036},{"time":1.2723682645918366,"velocity":1.814974179793144,"acceleration":-3.000000000000003,"pose":{"translation":{"x":4.457623446915669,"y":-1.3186523437500004},"rotation":{"radians":0.09406880478221523}},"curvature":-0.29025966227029737},{"time":1.315948027173534,"velocity":1.6842348920480514,"acceleration":-2.999999999999997,"pose":{"translation":{"x":4.5336062655273235,"y":-1.312306502461434},"rotation":{"radians":0.07289944611301012}},"curvature":-0.26493890076434345},{"time":1.3617317581047963,"velocity":1.546883699254265,"acceleration":-3.0,"pose":{"translation":{"x":4.607423928641174,"y":-1.307619190216065},"rotation":{"radians":0.05424603147602765}},"curvature":-0.2390516451714226},{"time":1.4102861588151152,"velocity":1.4012204971233082,"acceleration":-3.0,"pose":{"translation":{"x":4.678919883447504,"y":-1.3043269246816642},"rotation":{"radians":0.03812190138197012}},"curvature":-0.2108146530013771},{"time":1.4625872434216978,"velocity":1.2443172433035607,"acceleration":-3.0,"pose":{"translation":{"x":4.748068491939165,"y":-1.3021697998046884},"rotation":{"radians":0.02462148220789151}},"curvature":-0.1784645562722071},{"time":1.5204092440874486,"velocity":1.0708512413063085,"acceleration":-3.000000000000001,"pose":{"translation":{"x":4.814990182328344,"y":-1.3008947044610988},"rotation":{"radians":0.013903147168922325}},"curvature":-0.14060587812306488},{"time":1.58737015857728,"velocity":0.8699684978368145,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":4.87996660046335,"y":-1.3002585411071794},"rotation":{"radians":0.006151724781559076}},"curvature":-0.09684470081187932},{"time":1.8773596578562182,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":5.006107200045358,"y":-1.300000000000002},"rotation":{"radians":-5.329070518200854E-15}},"curvature":-1.06581410364021E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid1.wpilib.json b/src/main/deploy/paths/SixBallMid1.wpilib.json index 1fcfcfc..3971ad8 100644 --- a/src/main/deploy/paths/SixBallMid1.wpilib.json +++ b/src/main/deploy/paths/SixBallMid1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2519761297876903,"velocity":0.7559283893630709,"acceleration":2.9999999999999987,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3562702409671562,"velocity":1.0688107229014685,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4361595761278285,"velocity":1.3084787283834856,"acceleration":3.0,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.503308309644587,"velocity":1.5099249289337608,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.562202759020208,"velocity":1.686608277060624,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6151186089000533,"velocity":1.8453558267001602,"acceleration":3.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6633872740545509,"velocity":1.990161822163653,"acceleration":3.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7078595229045569,"velocity":2.123578568713671,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7491126566722462,"velocity":2.2473379700167393,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7875559872701902,"velocity":2.3626679618105713,"acceleration":3.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.823489794243851,"velocity":2.470469382731554,"acceleration":0.8651265291285504,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8576242431067156,"velocity":2.5,"acceleration":-1.1732826408603372,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8909260810512718,"velocity":2.460927531630908,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9241733364212149,"velocity":2.361185765521079,"acceleration":-3.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9577172640528673,"velocity":2.2605539826261216,"acceleration":-3.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9915130896437078,"velocity":2.1591665058536003,"acceleration":-3.000000000000006,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.025517145389847,"velocity":2.0571543386151823,"acceleration":-3.0000000000000098,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0596888686194887,"velocity":1.9546391689262566,"acceleration":-3.0000000000000036,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0939935025170637,"velocity":1.8517252672335311,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1284057930259193,"velocity":1.7484883957069648,"acceleration":-3.0,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1975327505604134,"velocity":1.5411075231034823,"acceleration":-2.999999999999999,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.267315103090996,"velocity":1.3317604655117345,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3388330522864487,"velocity":1.1172066179253766,"acceleration":-3.0,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4151734608431514,"velocity":0.8881853922552679,"acceleration":-3.0,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.505709141675879,"velocity":0.6165783497570856,"acceleration":-3.0,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7112352582615742,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMidComplete.wpilib.json b/src/main/deploy/paths/SixBallMidComplete.wpilib.json index 40512d6..eec2b0a 100644 --- a/src/main/deploy/paths/SixBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/SixBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24104312286019983,"velocity":0.6508164317225397,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.2063923239707948,"y":-2.3218234953284265},"rotation":{"radians":1.4857527840978701}},"curvature":-0.13003316961730046},{"time":0.34125642343054197,"velocity":0.9213923432624636,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.2136058807373047,"y":-2.2433763408660887},"rotation":{"radians":1.4709813552860977}},"curvature":-0.2416411635394689},{"time":0.41862951975667484,"velocity":1.1302997033430222,"acceleration":2.700000000000001,"pose":{"translation":{"x":3.222373878955841,"y":-2.1644892266392706},"rotation":{"radians":1.4480070351506305}},"curvature":-0.33393289305160856},{"time":0.4843752197211856,"velocity":1.3078130932472012,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.23333740234375,"y":-2.0850949096679687},"rotation":{"radians":1.418153476722264}},"curvature":-0.40799291240160246},{"time":0.5428188459431846,"velocity":1.4656108840465984,"acceleration":-0.12363267963904842,"pose":{"translation":{"x":3.247049701213837,"y":-2.005218879878521},"rotation":{"radians":1.3826311795834407}},"curvature":-0.4659998167211871},{"time":0.5989114443624675,"velocity":1.4586760057961055,"acceleration":-0.23878049786970917,"pose":{"translation":{"x":3.263980484008789,"y":-1.9249700260162352},"rotation":{"radians":1.3424938853817683}},"curvature":-0.5106186292535235},{"time":0.656093483931331,"velocity":1.4450220499186468,"acceleration":-0.17582458312750424,"pose":{"translation":{"x":3.284520208835602,"y":-1.8445313015580176},"rotation":{"radians":1.2986210341256466}},"curvature":-0.5446069018648376},{"time":0.7144460101119858,"velocity":1.4347622413284964,"acceleration":-0.13307656507771634,"pose":{"translation":{"x":3.308984375,"y":-1.7641503906249998},"rotation":{"radians":1.2517175479138372}},"curvature":-0.5705719275620846},{"time":0.7738450156145333,"velocity":1.426857625707185,"acceleration":-0.10571177853926464,"pose":{"translation":{"x":3.337617814540863,"y":-1.6841303738951683},"rotation":{"radians":1.2023248883292088}},"curvature":-0.5908312113511963},{"time":0.8341780829208127,"velocity":1.420479709857509,"acceleration":-0.08976520784153569,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6048203945159911},"rotation":{"radians":1.1508391887199425}},"curvature":-0.6073419696862633},{"time":0.8953429670423647,"velocity":1.4149892313217345,"acceleration":-0.08191722568263685,"pose":{"translation":{"x":3.4080442547798158,"y":-1.5266063240170478},"rotation":{"radians":1.0975335619661353}},"curvature":-0.621674601346085},{"time":0.9572461651384423,"velocity":1.4099182930728211,"acceleration":-0.07932481308319937,"pose":{"translation":{"x":3.45001220703125,"y":-1.449901428222656},"rotation":{"radians":1.0425826601190191}},"curvature":-0.6350112021866137},{"time":1.0198016941975718,"velocity":1.404956087422885,"acceleration":-0.07948863640008091,"pose":{"translation":{"x":3.4965079188346864,"y":-1.3751370331645012},"rotation":{"radians":0.9860883281959857}},"curvature":-0.6481550334261905},{"time":1.0829301018210549,"velocity":1.3999380963827859,"acceleration":-0.08015206912118296,"pose":{"translation":{"x":3.547487258911133,"y":-1.3027531909942627},"rotation":{"radians":0.9281057752826144}},"curvature":-0.6615413844839425},{"time":1.146557600543031,"velocity":1.394838220707214,"acceleration":-0.07923440840802286,"pose":{"translation":{"x":3.602861177921295,"y":-1.233189345896244},"rotation":{"radians":0.8686700639277289}},"curvature":-0.6752448631775445},{"time":1.2106151395445883,"velocity":1.3897626595003518,"acceleration":-0.07480385075761357,"pose":{"translation":{"x":3.6624999999999996,"y":-1.1668749999999999},"rotation":{"radians":0.8078228647022838}},"curvature":-0.6889828628777236},{"time":1.2750372072434193,"velocity":1.3849436407627116,"acceleration":-0.06509475812905682,"pose":{"translation":{"x":3.726237714290619,"y":-1.1042203792929648},"rotation":{"radians":0.745639315828271}},"curvature":-0.7021196688913226},{"time":1.3397601915745783,"velocity":1.380730513752284,"acceleration":-0.04856760891892339,"pose":{"translation":{"x":3.793876266479492,"y":-1.045607099533081},"rotation":{"radians":0.68225447169036}},"curvature":-0.71367992457469},{"time":1.4047202049596175,"velocity":1.3775755612268314,"acceleration":-0.023998991327275822,"pose":{"translation":{"x":3.8651898503303523,"y":-0.9913788321614265},"rotation":{"radians":0.6178882696500003}},"curvature":-0.7223829927855778},{"time":1.4698503918277483,"velocity":1.3760125024370393,"acceleration":0.009425020866650225,"pose":{"translation":{"x":3.9399291992187493,"y":-0.9418319702148437},"rotation":{"radians":0.5528673168364797}},"curvature":-0.7267095396737598},{"time":1.535077848801426,"velocity":1.3766272725800948,"acceleration":0.05205176529824429,"pose":{"translation":{"x":4.017825877666472,"y":-0.8972062942385672},"rotation":{"radians":0.48764131303933056}},"curvature":-0.7250066836520244},{"time":1.6003203604517349,"velocity":1.3800232604839846,"acceleration":0.10378762257480224,"pose":{"translation":{"x":4.098596572875976,"y":-0.8576756381988526},"rotation":{"radians":0.4227918675196265}},"curvature":-0.715627451832997},{"time":1.665483145837002,"velocity":1.3867863510594736,"acceleration":0.16424958779401677,"pose":{"translation":{"x":4.1819473862648,"y":-0.8233385553956034},"rotation":{"radians":0.3590321072445168}},"curvature":-0.6970855981440633},{"time":1.7304557025837932,"velocity":1.3974580667230574,"acceleration":0.2330731468720872,"pose":{"translation":{"x":4.267578124999998,"y":-0.794208984375},"rotation":{"radians":0.29719692634159106}},"curvature":-0.668192786058002},{"time":1.7951086483327998,"velocity":1.4125269322433287,"acceleration":0.31039116241082515,"pose":{"translation":{"x":4.35518659353256,"y":-0.7702069148421291},"rotation":{"radians":0.23822580998113968}},"curvature":-0.6281385045136234},{"time":1.859290265714348,"velocity":1.4324483390677942,"acceleration":0.3974925676686374,"pose":{"translation":{"x":4.444472885131834,"y":-0.751149053573609},"rotation":{"radians":0.18314236815259385}},"curvature":-0.5764792409963189},{"time":1.922822358232916,"velocity":1.4577018736523613,"acceleration":0.4976892074994749,"pose":{"translation":{"x":4.53514367341995,"y":-0.7367394903302198},"rotation":{"radians":0.1330363391804617}},"curvature":-0.5130224065785399},{"time":1.9854951125589597,"velocity":1.4888934270846992,"acceleration":0.6174792927963386,"pose":{"translation":{"x":4.626916503906247,"y":-0.7265603637695319},"rotation":{"radians":0.08905427201814982}},"curvature":-0.43761593999910725},{"time":2.047060989445588,"velocity":1.5269090812050408,"acceleration":0.7682064361829994,"pose":{"translation":{"x":4.7195240855216944,"y":-0.7200625273585326},"rotation":{"radians":0.052404113332160406}},"curvature":-0.3498775646988902},{"time":2.107228253203399,"velocity":1.5731299604713114,"acceleration":0.9685643952029747,"pose":{"translation":{"x":4.812718582153316,"y":-0.7165562152862552},"rotation":{"radians":0.024376580770117368}},"curvature":-0.24891407952009473},{"time":2.1656556590359175,"velocity":1.6297206654647636,"acceleration":1.248398336461174,"pose":{"translation":{"x":4.906275904178614,"y":-0.7152017083764088},"rotation":{"radians":0.0063825391669578325}},"curvature":-0.13309736505149056},{"time":2.221951259953214,"velocity":1.6999999999999984,"acceleration":2.843979367649469E-14,"pose":{"translation":{"x":4.999999999999995,"y":-0.7150000000000012},"rotation":{"radians":-2.0724163126336422E-15}},"curvature":-2.9605947323338176E-15},{"time":2.2770632519617626,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3319714486775105,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.386489936201094,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.440451733873254,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.49370816316683,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5461282165787624,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.597597926522085,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6480197342179226,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6973118585874882,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.7454076651440773,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.792255034885068,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8378157331839153,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.882064778682146,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.924989812181359,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9665904655352184,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.006877730541452,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0458733278338475,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.083609075774248,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.1554749990386948,"velocity":1.7,"acceleration":-0.8670174294348908,"pose":{"translation":{"x":6.5869903564453125,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2241092626820116,"velocity":1.6404928971648147,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.701626491546631,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.293986626673893,"velocity":1.4518240143867347,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.80966796875,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.370001653212677,"velocity":1.2465834427320184,"acceleration":-2.7,"pose":{"translation":{"x":6.912227725982667,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.457094277398368,"velocity":1.0114333574306535,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.010556030273439,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.5678335818442313,"velocity":0.7124372354268221,"acceleration":-2.7,"pose":{"translation":{"x":7.106006145477297,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.831699224594906,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":7.200000000000003,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22851073171807768,"velocity":0.6855321951542331,"acceleration":3.0,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.32286738548734767,"velocity":0.9686021564620431,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.3948508967117983,"velocity":1.1845526901353949,"acceleration":3.000000000000002,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.45504953863530545,"velocity":1.3651486159059165,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5075769496703644,"velocity":1.5227308490110931,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5545708746789335,"velocity":1.6637126240368005,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.5973365928558361,"velocity":1.7920097785675082,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6367625752176861,"velocity":1.9102877256530584,"acceleration":-0.39480980187976056,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.6747121786647302,"velocity":1.8953048502347154,"acceleration":-0.7021730002147804,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.7127763389927304,"velocity":1.868577224576547,"acceleration":-0.4401004459228274,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.7511092582596577,"velocity":1.8517068897136486,"acceleration":-0.17400254925795713,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.7896601305578692,"velocity":1.8449989396576418,"acceleration":0.08372895263229682,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.8283828420132677,"velocity":1.848241151730885,"acceleration":0.32040119497795183,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":0.8672396362007758,"velocity":1.860690915021575,"acceleration":0.5251363087983144,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":0.906204315334355,"velocity":1.8811526827952934,"acceleration":0.6905017989409158,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":0.9452636685748346,"velocity":1.9081232364733132,"acceleration":0.8133740442414022,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":0.9844165273519864,"velocity":1.9399691555604976,"acceleration":0.8949317659517233,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.0236706441727126,"velocity":1.9750989116477453,"acceleration":0.9399001600451896,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.063038145418846,"velocity":2.0121004323695653,"acceleration":0.9553480479826941,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.1025304872768191,"velocity":2.049829364073845,"acceleration":0.9494024824000104,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.1421537132339343,"velocity":2.087447753158227,"acceleration":0.9301775648872205,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.1819045291156953,"velocity":2.1244230702774036,"acceleration":0.9050734432547365,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.221767420455895,"velocity":2.1605019146007676,"acceleration":0.8804626256424855,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.2617128175476713,"velocity":2.1956723438065247,"acceleration":0.8616916300805181,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.3016961872085873,"velocity":2.2301256787857513,"acceleration":0.8532946962433116,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.341657884138109,"velocity":2.264224782828595,"acceleration":0.8593254128101726,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.3815236046845703,"velocity":2.2984824095941576,"acceleration":0.8837378969826192,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.42120533048524,"velocity":2.3335506545018823,"acceleration":0.9307724426536456,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.4606027154283423,"velocity":2.3702206547195397,"acceleration":1.0053080180459415,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.4996049518717887,"velocity":2.40942991573786,"acceleration":1.113123412063664,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":1.5380932533029306,"velocity":2.4522721451514276,"acceleration":1.2609415748588182,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":1.575944217852111,"velocity":2.4999999999999964,"acceleration":9.47993122549823E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":1.6134203724179241,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6507579461846327,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6878305177006696,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7245245401177383,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.76073891203737,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.796384548357484,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8313839511189436,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.865670780352113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8991894249234176,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9318945733818982,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.963750784805772,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9947320596489881,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.024821410587785,"velocity":2.5,"acceleration":-1.5142215235962606,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.0542731215093437,"velocity":2.4554035854158403,"acceleration":-3.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.0836007969891943,"velocity":2.367420558976288,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.113080971642734,"velocity":2.278980035015669,"acceleration":-3.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.142748981256642,"velocity":2.1899760061739455,"acceleration":-2.999999999999993,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.172654458612498,"velocity":2.1002595741063774,"acceleration":-3.0000000000000036,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.2334655374361456,"velocity":1.917826337635434,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.296330534384713,"velocity":1.729231346789732,"acceleration":-3.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3626220225156076,"velocity":1.5303568823970488,"acceleration":-2.999999999999999,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4347362085938182,"velocity":1.3140143241624165,"acceleration":-3.0,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5173595265421733,"velocity":1.0661443703173517,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.622416055107695,"velocity":0.7509747846207866,"acceleration":-3.0,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.872740983314624,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c982f8e..84d250f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -269,9 +269,9 @@ public class RobotContainer { // Run path following command, then stop at the end. try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + + //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); From 8b554eb38d712bf3495f1e9cb8aadf0e03fc3fde Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Mar 2020 18:44:53 -0600 Subject: [PATCH 21/39] Added IDs --- src/main/java/frc4388/robot/Constants.java | 1 + src/main/java/frc4388/robot/RobotContainer.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c20a589..4c0689a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -215,5 +215,6 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTON_BOX_ID = 2; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1a0365e..955fc51 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -88,6 +88,7 @@ public class RobotContainer { /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private final XboxController m_buttonBox = new XboxController(OIConstants. BUTTON_BOX_ID); /** From 6cc3e63b8648cf74c9b2d048137a58ae23aa3ec9 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Mar 2020 18:47:22 -0600 Subject: [PATCH 22/39] fox --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c0689a..9a1cf41 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -215,6 +215,6 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - public static final int BUTTON_BOX_ID = 2; + public static final int BUTTON_FOX_ID = 2; } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 955fc51..b7cbfee 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -88,7 +88,7 @@ public class RobotContainer { /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - private final XboxController m_buttonBox = new XboxController(OIConstants. BUTTON_BOX_ID); + private final XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID); /** From 69657ace52217259c241173e2ce4d5d3d09d6409 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Mar 2020 19:37:34 -0600 Subject: [PATCH 23/39] Added Storage Manual and ButtonFox.java --- .../java/frc4388/robot/RobotContainer.java | 50 +++++++++++++++++++ .../robot/commands/storage/ManageStorage.java | 11 +++- .../frc4388/utility/controller/ButtonFox.java | 20 ++++++++ 3 files changed, 80 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/utility/controller/ButtonFox.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b7cbfee..8e9854f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc4388.robot.commands.climber.RunClimberWithTriggers; import frc4388.robot.commands.climber.RunLevelerWithJoystick; import frc4388.robot.commands.drive.DriveStraightToPositionMM; import frc4388.robot.commands.drive.DriveWithJoystick; +import frc4388.robot.commands.drive.PlaySongDrive; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; @@ -56,6 +57,7 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; +import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxTriggerButton; @@ -169,6 +171,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) .whileHeld(new DisengageRachet(m_robotClimber)); + + + /* Operator Buttons */ // shoots until released new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) @@ -235,6 +240,35 @@ public class RobotContainer { .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); + + + + + /* Button Fox */ + // Storage Manual + new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH) + .whileHeld(new ManageStorage(m_robotStorage, StorageMode.MANUAL)) + .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + + // Meg + new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) + .whileHeld(new PlaySongDrive(m_robotDrive)) + .whenReleased(new InterruptSubystem(m_robotDrive)); + + // Shooter Manual + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH) + .whileHeld(new PlaySongDrive(m_robotDrive)) + .whenReleased(new InterruptSubystem(m_robotDrive)); + + // Goal Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) + .whileHeld(new PlaySongDrive(m_robotDrive)) + .whenReleased(new InterruptSubystem(m_robotDrive)); + + // Trench Shooter Position + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) + .whileHeld(new PlaySongDrive(m_robotDrive)) + .whenReleased(new InterruptSubystem(m_robotDrive)); } /** @@ -339,6 +373,11 @@ public class RobotContainer { return m_operatorXbox; } + public IHandController getButtonFoxObject() + { + return m_buttonFox; + } + /** * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller. * Generic HIDs/Joysticks can be used to set up JoystickButtons. @@ -358,4 +397,15 @@ public class RobotContainer { { return m_driverXbox.getJoyStick(); } + + /** + * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Button Fox. + * Generic HIDs/Joysticks can be used to set up JoystickButtons. + * @return The IHandController interface for the Button Fox. + */ + public Joystick getButtonFox() + { + return m_buttonFox.getJoyStick(); + } + } diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java index 68cfd71..5f3537c 100644 --- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java @@ -27,7 +27,7 @@ public class ManageStorage extends CommandBase { /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ boolean m_isStorageEmpty = true; - public enum StorageMode{IDLE, INTAKE, RESET}; + public enum StorageMode{IDLE, INTAKE, RESET, MANUAL}; StorageMode m_storageMode = StorageMode.IDLE; /** @@ -74,6 +74,8 @@ public class ManageStorage extends CommandBase { runIntake(); } else if (m_storageMode == StorageMode.RESET) { runReset(); + } else if (m_storageMode == StorageMode.MANUAL) { + runManual(); } } @@ -127,6 +129,13 @@ public class ManageStorage extends CommandBase { m_isStorageEmpty = !m_isBallInStorage; } + /** + * Switches Storage to Manual only + */ + private void runManual() { + m_storage.runStorage(0); + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc4388/utility/controller/ButtonFox.java b/src/main/java/frc4388/utility/controller/ButtonFox.java new file mode 100644 index 0000000..5b4da00 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonFox.java @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility.controller; + +/** + * button fox + * @author Ryan Manley + */ +public class ButtonFox { + public static final int RIGHT_SWITCH = 1; + public static final int MIDDLE_SWITCH = 2; + public static final int LEFT_SWITCH = 3; + public static final int RIGHT_BUTTON = 4; + public static final int LEFT_BUTTON = 5; +} From 59fa78f6e396022d28be9f5e565e922d51b3f038 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 10 Mar 2020 20:13:52 -0600 Subject: [PATCH 24/39] did a lot --- PathWeaver/Paths/DriveOffLineBackward | 3 + PathWeaver/Paths/DriveOffLineForward | 3 + PathWeaver/Paths/EightBallMid2 | 2 +- PathWeaver/Paths/EightBallMidComplete | 4 +- PathWeaver/pathweaver.json | 4 +- .../paths/DriveOffLineBackward.wpilib.json | 1 + .../paths/DriveOffLineForward.wpilib.json | 1 + .../deploy/paths/EightBallMid0.wpilib.json | 2 +- .../deploy/paths/EightBallMid1.wpilib.json | 2 +- .../deploy/paths/EightBallMid2.wpilib.json | 2 +- .../paths/EightBallMidComplete.wpilib.json | 2 +- src/main/deploy/paths/SixBallMid0.wpilib.json | 2 +- src/main/deploy/paths/SixBallMid1.wpilib.json | 2 +- .../paths/SixBallMidComplete.wpilib.json | 2 +- .../java/frc4388/robot/RobotContainer.java | 35 ++++++++-- .../commands/auto/DriveOffLineBackward.java | 31 +++++++++ .../commands/auto/DriveOffLineForward.java | 29 ++++++++ .../commands/auto/TankDriveVelocity.java | 67 +++++++++++++++++++ 18 files changed, 177 insertions(+), 17 deletions(-) create mode 100644 PathWeaver/Paths/DriveOffLineBackward create mode 100644 PathWeaver/Paths/DriveOffLineForward create mode 100644 src/main/deploy/paths/DriveOffLineBackward.wpilib.json create mode 100644 src/main/deploy/paths/DriveOffLineForward.wpilib.json create mode 100644 src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java create mode 100644 src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java create mode 100644 src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java diff --git a/PathWeaver/Paths/DriveOffLineBackward b/PathWeaver/Paths/DriveOffLineBackward new file mode 100644 index 0000000..f5cce7f --- /dev/null +++ b/PathWeaver/Paths/DriveOffLineBackward @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.3,0.5,0.0,true, +3.7,-2.3,0.5,0.0,true, diff --git a/PathWeaver/Paths/DriveOffLineForward b/PathWeaver/Paths/DriveOffLineForward new file mode 100644 index 0000000..f5cce7f --- /dev/null +++ b/PathWeaver/Paths/DriveOffLineForward @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.3,0.5,0.0,true, +3.7,-2.3,0.5,0.0,true, diff --git a/PathWeaver/Paths/EightBallMid2 b/PathWeaver/Paths/EightBallMid2 index 671f3d6..7acacf8 100644 --- a/PathWeaver/Paths/EightBallMid2 +++ b/PathWeaver/Paths/EightBallMid2 @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 7.2,-0.715,1.5,0.0,true, -6.8,-2.6,-0.6,-0.3,true, +6.612,-2.7,-0.6,-0.3,true, diff --git a/PathWeaver/Paths/EightBallMidComplete b/PathWeaver/Paths/EightBallMidComplete index 8a05706..e8f4cf2 100644 --- a/PathWeaver/Paths/EightBallMidComplete +++ b/PathWeaver/Paths/EightBallMidComplete @@ -2,5 +2,5 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.4,0.2,2.5,true, 5.0,-1.1,3.0,0.0,true, 7.2,-1.1,1.5,0.0,true, -6.5,-2.78,-0.39262822032017564,-0.17846737287280634,true, -6.291072284729572,-2.036118403443617,0.13087607344005825,0.40452604517836144,true, +6.6123135559006245,-2.7023965955020937,-0.39262822032017564,-0.17846737287280634,true, +6.279174459871384,-2.2621770757491713,0.13087607344005825,0.40452604517836144,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index 4a585f0..ddd52e0 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,8 +1,8 @@ { "lengthUnit": "Meter", "exportUnit": "Always Meters", - "maxVelocity": 2.5, - "maxAcceleration": 3.0, + "maxVelocity": 1.7, + "maxAcceleration": 2.7, "wheelBase": 0.648, "gameName": "Infinite Recharge", "outputDir": ".." diff --git a/src/main/deploy/paths/DriveOffLineBackward.wpilib.json b/src/main/deploy/paths/DriveOffLineBackward.wpilib.json new file mode 100644 index 0000000..25c61f9 --- /dev/null +++ b/src/main/deploy/paths/DriveOffLineBackward.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/deploy/paths/DriveOffLineForward.wpilib.json b/src/main/deploy/paths/DriveOffLineForward.wpilib.json new file mode 100644 index 0000000..25c61f9 --- /dev/null +++ b/src/main/deploy/paths/DriveOffLineForward.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMid0.wpilib.json b/src/main/deploy/paths/EightBallMid0.wpilib.json index a461431..9697957 100644 --- a/src/main/deploy/paths/EightBallMid0.wpilib.json +++ b/src/main/deploy/paths/EightBallMid0.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22868744691387022,"velocity":0.6860623407416107,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3238294613804855,"velocity":0.9714883841414567,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.39741314342202283,"velocity":1.1922394302660686,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.4601433693460262,"velocity":1.3804301080380785,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5161968408530254,"velocity":1.5485905225590764,"acceleration":2.9999999999999973,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.5677250216556059,"velocity":1.7031750649668176,"acceleration":2.9999999999999956,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.615996242927949,"velocity":1.8479887287838466,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.6618112290098469,"velocity":1.9854336870295404,"acceleration":1.7724319853927966,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7062618567953473,"velocity":2.064219401487351,"acceleration":0.5034639557065531,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.7506783242988111,"velocity":2.0865814919151564,"acceleration":0.49932665829872824,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.7956023350319041,"velocity":2.109013248071888,"acceleration":0.45895712191178695,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.8409364448700584,"velocity":2.12981966064764,"acceleration":0.39515203882870387,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":0.8865733255240144,"velocity":2.147853167083833,"acceleration":0.31701310713711306,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":0.9323919712522077,"velocity":2.162378278330942,"acceleration":0.23031078377585737,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":0.9782572476318948,"velocity":2.1729415460820443,"acceleration":0.13813925366720256,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.0240221425451959,"velocity":2.1792634745095256,"acceleration":0.041673045633118966,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.0695319790973778,"velocity":2.1811600080049205,"acceleration":-0.059065512402407955,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.1146298672309258,"velocity":2.178496278134046,"acceleration":-0.16441372455340814,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.159162714233256,"velocity":2.171174466893426,"acceleration":-0.27415348238761694,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.2029871254407334,"velocity":2.159159851947309,"acceleration":-0.3863225903265772,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.245974473802423,"velocity":2.14255286817695,"acceleration":-1.2804495582678201,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.2883463773411485,"velocity":2.0882977830078224,"acceleration":-3.0000000000000053,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.3310768450283628,"velocity":1.9601063799461798,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.3749668670018906,"velocity":1.8284363140255961,"acceleration":-3.0,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.4202251947318614,"velocity":1.6926613308356837,"acceleration":-3.0,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.4672211376575084,"velocity":1.5516735020587429,"acceleration":-3.0,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.516578191034963,"velocity":1.4036023419263792,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.5693496410384185,"velocity":1.2452879919160125,"acceleration":-3.0,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":1.627401271932817,"velocity":1.0711330992328167,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":1.6944454242441096,"velocity":0.8700006422989391,"acceleration":-3.0,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":1.984445638343756,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.241057734845558,"velocity":0.6508558840830065,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2065095854674475,"y":-2.3218236291658374},"rotation":{"radians":1.4813756721292652}},"curvature":-0.23682019043437622},{"time":0.3413462238092895,"velocity":0.9216348042850815,"acceleration":2.7000000000000015,"pose":{"translation":{"x":3.2144920400116974,"y":-2.243377360913824},"rotation":{"radians":1.454917761458827}},"curvature":-0.42576201763689586},{"time":0.41891023510025166,"velocity":1.1310576347706796,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2251942805954354,"y":-2.1644925027916173},"rotation":{"radians":1.4151325512096895}},"curvature":-0.5652942275784931},{"time":0.485033699119182,"velocity":1.3095909876217915,"acceleration":1.198557825708184,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0851022911999886},"rotation":{"radians":1.3654713587791596}},"curvature":-0.6577310942169727},{"time":0.5460162839325096,"velocity":1.3826821418817183,"acceleration":-0.09664254151193248,"pose":{"translation":{"x":3.2585973431481516,"y":-2.005232567172635},"rotation":{"radians":1.3091243144298805}},"curvature":-0.7083161600470856},{"time":0.6067366321916278,"velocity":1.3768139731044675,"acceleration":0.056078716638971786,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9249924521559951},"rotation":{"radians":1.2488824795564184}},"curvature":-0.7244898416325155},{"time":0.6689099696895163,"velocity":1.3803005740805108,"acceleration":0.1547804484445089,"pose":{"translation":{"x":3.3123124503261456,"y":-1.844565023789062},"rotation":{"radians":1.1870383284988666}},"curvature":-0.7148635896682205},{"time":0.732304190496085,"velocity":1.3901127600057417,"acceleration":0.2103982966969226,"pose":{"translation":{"x":3.347702503129696,"y":-1.7641979916831971},"rotation":{"radians":1.1253401051571525}},"curvature":-0.6880320260809347},{"time":0.7967364279166413,"velocity":1.4036691930113985,"acceleration":0.23280173311870972,"pose":{"translation":{"x":3.388937040133019,"y":-1.684194373201945},"rotation":{"radians":1.065004679160645}},"curvature":-0.6515789195141447},{"time":0.8620547624805587,"velocity":1.4188754145023061,"acceleration":0.23088864679733498,"pose":{"translation":{"x":3.435952123650134,"y":-1.6049031692408469},"rotation":{"radians":1.006777432997644}},"curvature":-0.6115184355792523},{"time":0.9281194841468721,"velocity":1.4341290086888838,"acceleration":0.21222177317200774,"pose":{"translation":{"x":3.4885571567123694,"y":-1.526710040007253},"rotation":{"radians":0.951018464595247}},"curvature":-0.5721866543272143},{"time":0.9947872927323935,"velocity":1.4482773692403952,"acceleration":0.18271830275439588,"pose":{"translation":{"x":3.5464500344851415,"y":-1.4500279808001393},"rotation":{"radians":0.8977946082603232}},"curvature":-0.5364453773677413},{"time":1.061900352517623,"velocity":1.4605401536170066,"acceleration":0.14658686074019905,"pose":{"translation":{"x":3.6092322956847354,"y":-1.3752879977899195},"rotation":{"radians":0.8469628668079059}},"curvature":-0.5060275803559428},{"time":1.1292807138826122,"velocity":1.4704172292650406,"acceleration":0.10649570641795872,"pose":{"translation":{"x":3.676424273995083,"y":-1.3029297837982592},"rotation":{"radians":0.7982381560354028}},"curvature":-0.4818964199153621},{"time":1.1967296497350939,"velocity":1.4776002513357902,"acceleration":0.06387559089571403,"pose":{"translation":{"x":3.7474802494845436,"y":-1.2333923940778901},"rotation":{"radians":0.751243975385745}},"curvature":-0.46454985151642403},{"time":1.2640309657840656,"velocity":1.4818991626664775,"acceleration":0.019269616300747743,"pose":{"translation":{"x":3.8218036000226827,"y":-1.1671049220924246},"rotation":{"radians":0.7055480104280688}},"curvature":-0.45424867593529666},{"time":1.3309571960078612,"velocity":1.4831888054433455,"acceleration":-0.027311892934865002,"pose":{"translation":{"x":3.8987619526970523,"y":-1.1044771752961677},"rotation":{"radians":0.6606862029065328}},"curvature":-0.45117004060039156},{"time":1.3972776197336674,"velocity":1.4813774691311514,"acceleration":-0.07602490623349939,"pose":{"translation":{"x":3.9777023352299703,"y":-1.0458903509139337},"rotation":{"radians":0.6161792808148264}},"curvature":-0.4554955855641011},{"time":1.4627671006194463,"velocity":1.4763986374875295,"acceleration":-0.12676857025603222,"pose":{"translation":{"x":4.057966327395298,"y":-0.9916877117208573},"rotation":{"radians":0.5715458045066106}},"curvature":-0.4674399207316766},{"time":1.5272147641598541,"velocity":1.4682286993241702,"acceleration":-0.17863556576701026,"pose":{"translation":{"x":4.138905212435224,"y":-0.9421652618222098},"rotation":{"radians":0.5263159118657924}},"curvature":-0.48721532342589935},{"time":1.590431452927045,"velocity":1.456935950360326,"acceleration":-0.2291597090436514,"pose":{"translation":{"x":4.219895128477039,"y":-0.8975624224332113},"rotation":{"radians":0.4800503874072753}},"curvature":-0.514914663124024},{"time":1.6522547165397012,"velocity":1.4427685492587208,"acceleration":-0.2733089917486314,"pose":{"translation":{"x":4.300352219949918,"y":-0.858052707658846},"rotation":{"radians":0.43237050210611794}},"curvature":-0.5502783042303422},{"time":1.7125498410614521,"velocity":1.4262893495683229,"acceleration":-0.3022406193756772,"pose":{"translation":{"x":4.3797477890017005,"y":-0.8237344002736755},"rotation":{"radians":0.38300509676508365}},"curvature":-0.5922963376106968},{"time":1.771205213726376,"velocity":1.408561313404365,"acceleration":-0.301932659771223,"pose":{"translation":{"x":4.457623446915669,"y":-0.794621227501652},"rotation":{"radians":0.3318620281588008}},"curvature":-0.638596353997764},{"time":1.8281203979112544,"velocity":1.3913767604620557,"acceleration":-0.25183985881265725,"pose":{"translation":{"x":4.5336062655273235,"y":-0.770633036795934},"rotation":{"radians":0.2791301165865979}},"curvature":-0.6846031138647943},{"time":1.883186076418791,"velocity":1.3775090277612945,"acceleration":-0.853135730581628,"pose":{"translation":{"x":4.607423928641174,"y":-0.751586471618698},"rotation":{"radians":0.22541304675400947}},"curvature":-0.7225669573485112},{"time":1.9370285392585973,"velocity":1.3315740988901423,"acceleration":-2.699999999999998,"pose":{"translation":{"x":4.678919883447504,"y":-0.7371856472209546},"rotation":{"radians":0.17188576739499878}},"curvature":-0.7407736342310233},{"time":1.992654531738804,"velocity":1.1813839191935844,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.748068491939165,"y":-0.7270128264223612},"rotation":{"radians":0.1204455235478345}},"curvature":-0.7235395890805306},{"time":2.0538463235767033,"velocity":1.016166081231257,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":4.814990182328344,"y":-0.7205190953910354},"rotation":{"radians":0.07380665332979482}},"curvature":-0.6528042487689714},{"time":2.124517065276345,"velocity":0.825355078642223,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-0.7170150394233701},"rotation":{"radians":0.03546965101273601}},"curvature":-0.5119937898610332},{"time":2.4302041314401315,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-0.7154598441848505},"rotation":{"radians":-3.99680288865064E-15}},"curvature":-7.549516567451497E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMid1.wpilib.json b/src/main/deploy/paths/EightBallMid1.wpilib.json index 11d97a8..1fcfcfc 100644 --- a/src/main/deploy/paths/EightBallMid1.wpilib.json +++ b/src/main/deploy/paths/EightBallMid1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.25197612978781475,"velocity":0.7559283893634442,"acceleration":3.0,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3562702409711053,"velocity":1.068810722913316,"acceleration":2.9999999999999987,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.4361595761515367,"velocity":1.30847872845461,"acceleration":3.0000000000000004,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5033083097241129,"velocity":1.5099249291723384,"acceleration":3.000000000000004,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5622027592166328,"velocity":1.6866082776498983,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6151186093010053,"velocity":1.8453558279030158,"acceleration":3.0,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.6633872747726501,"velocity":1.9901618243179504,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7078595240733458,"velocity":2.1235785722200378,"acceleration":3.0000000000000075,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.7491126584402565,"velocity":2.2473379753207703,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.7875559897937853,"velocity":2.3626679693813566,"acceleration":3.0000000000000027,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.8234897976794279,"velocity":2.4704693930382846,"acceleration":0.857610578891266,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.8576259992682255,"velocity":2.4997449606440023,"acceleration":-1.1655623083604212,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":0.8909295503524042,"velocity":2.460927596765728,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":0.9241768065402509,"velocity":2.3611858282021876,"acceleration":-3.0000000000000084,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":0.9577207352388619,"velocity":2.2605540421063544,"acceleration":-3.0000000000000058,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":0.9915165621380305,"velocity":2.159166561408848,"acceleration":-3.0,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.0255206194145319,"velocity":2.0571543895793445,"acceleration":-2.999999999999997,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.0596923443650952,"velocity":1.9546392147276543,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.093996980130577,"velocity":1.8517253074312092,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.1284092725993788,"velocity":1.7484884300248034,"acceleration":-3.0,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.197536234063257,"velocity":1.5411075456331693,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.267318590047229,"velocity":1.331760477681253,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.3388365417026034,"velocity":1.1172066227151305,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.415176951512201,"velocity":0.8881853932863374,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.5057126326753016,"velocity":0.6165783497970355,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.7112387492743135,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26560616204123155,"velocity":0.7171366375113252,"acceleration":2.700000000000001,"pose":{"translation":{"x":5.101237954974175,"y":-0.715000133837411},"rotation":{"radians":-4.1496514143772375E-6}},"curvature":-8.439120542891601E-5},{"time":0.3755418080019105,"velocity":1.0139628816051585,"acceleration":2.7000000000000015,"pose":{"translation":{"x":5.1963927268981935,"y":-0.7150010200477351},"rotation":{"radians":-1.5566916778703903E-5}},"curvature":-1.531433592359381E-4},{"time":0.45975256131083786,"velocity":1.2413319155392624,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-0.7150032761523467},"rotation":{"radians":-3.281919824111288E-5}},"curvature":-2.0809210046916836E-4},{"time":0.5305335413392231,"velocity":1.4324405616159026,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-0.7150073815320199},"rotation":{"radians":-5.4628064762566565E-5}},"curvature":-2.5102787463788406E-4},{"time":0.5926137419852601,"velocity":1.6000571033602022,"acceleration":1.7616233327671682,"pose":{"translation":{"x":5.480107913374901,"y":-0.7150136872941142},"rotation":{"radians":-7.986386339002337E-5}},"curvature":-2.83602941002837E-4},{"time":0.6492510855336531,"velocity":1.6998307692610013,"acceleration":-1.618642537235751E-4,"pose":{"translation":{"x":5.573556354522705,"y":-0.7150224261397599},"rotation":{"radians":-1.0753486872715826E-4}},"curvature":-3.07275938946559E-4},{"time":0.703708212041964,"velocity":1.699821954598859,"acceleration":-9.558060076856542E-5,"pose":{"translation":{"x":5.666124013066292,"y":-0.7150337222310442},"rotation":{"radians":-1.3677316456550684E-4}},"curvature":-3.2328258941494175E-4},{"time":0.7575219555253974,"velocity":1.6998168110489273,"acceleration":-3.565667757010362E-5,"pose":{"translation":{"x":5.757597656250001,"y":-0.7150476010581972},"rotation":{"radians":-1.6681884584259307E-4}},"curvature":-3.326228999871516E-4},{"time":0.8105612321047558,"velocity":1.6998149198445438,"acceleration":2.0620985976386642E-5,"pose":{"translation":{"x":5.847754658579826,"y":-0.715063999306777},"rotation":{"radians":-1.970036246728839E-4}},"curvature":-3.3605720307272754E-4},{"time":0.8626915928733692,"velocity":1.6998159948239822,"acceleration":7.612797407786101E-5,"pose":{"translation":{"x":5.936366649627686,"y":-0.7150827747248557},"rotation":{"radians":-2.267345283137577E-4}},"curvature":-3.3410511000065975E-4},{"time":0.9137773666638485,"velocity":1.699819883880445,"acceleration":1.3403797148642016E-4,"pose":{"translation":{"x":6.023203161835671,"y":-0.7151037159902053},"rotation":{"radians":-2.554780823579629E-4}},"curvature":-3.270428558434214E-4},{"time":0.9636838028938515,"velocity":1.6998265732379214,"acceleration":1.9801974397020216E-4,"pose":{"translation":{"x":6.108035278320313,"y":-0.7151265525774831},"rotation":{"radians":-2.827451666545957E-4}},"curvature":-3.1489552676741375E-4},{"time":1.0122792145971,"velocity":1.699836196088905,"acceleration":2.725041863281043E-4,"pose":{"translation":{"x":6.190639280676843,"y":-0.7151509646254184},"rotation":{"radians":-3.080766026243413E-4}},"curvature":-2.9742137977722E-4},{"time":1.059437121565497,"velocity":1.6998490468159724,"acceleration":3.630343580963836E-4,"pose":{"translation":{"x":6.270800296783448,"y":-0.7151765928039965},"rotation":{"radians":-3.310294717533898E-4}},"curvature":-2.740860371375139E-4},{"time":1.105038393643873,"velocity":1.6998656016445097,"acceleration":4.7670999504396565E-4,"pose":{"translation":{"x":6.3483159486055385,"y":-0.7152030481816463},"rotation":{"radians":-3.511641773574364E-4}},"curvature":-2.4402502101658282E-4},{"time":1.148973394189874,"velocity":1.6998865458984023,"acceleration":6.227242300883394E-4,"pose":{"translation":{"x":6.423000000000001,"y":-0.7152299220924245},"rotation":{"radians":-3.6803235840971186E-4}},"curvature":-2.059943242003666E-4},{"time":1.1911441237674736,"velocity":1.6999128066335107,"acceleration":8.129491144905005E-4,"pose":{"translation":{"x":6.494686004519464,"y":-0.7152567960032029},"rotation":{"radians":-3.811659755577217E-4}},"curvature":-1.58311254330657E-4},{"time":1.231466364211058,"velocity":1.6999455865631736,"acceleration":0.0010624275510308508,"pose":{"translation":{"x":6.563230953216554,"y":-0.7152832513808527},"rotation":{"radians":-3.900682698212409E-4}},"curvature":-9.879298936442655E-5},{"time":1.269871823331935,"velocity":1.6999863895810536,"acceleration":-1.1176149536288589,"pose":{"translation":{"x":6.628518922448159,"y":-0.7153088795594308},"rotation":{"radians":-3.942079319109873E-4}},"curvature":-2.471047188454908E-5},{"time":1.3067580684990234,"velocity":1.6587617703990953,"acceleration":-2.7,"pose":{"translation":{"x":6.690464721679689,"y":-0.715333291607366},"rotation":{"radians":-3.930188464700522E-4}},"curvature":6.720568814785149E-5},{"time":1.3796242838165373,"velocity":1.4620229890418077,"acceleration":-2.7,"pose":{"translation":{"x":6.804164600372316,"y":-0.7153770694599936},"rotation":{"radians":-3.722874810951608E-4}},"curvature":3.192984868163592E-4},{"time":1.4531813456172147,"velocity":1.2634189221799788,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.904402343750003,"y":-0.715412243126652},"rotation":{"radians":-3.234795622395437E-4}},"curvature":6.815272352510909E-4},{"time":1.528567885890812,"velocity":1.059875263441266,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.9919748954772984,"y":-0.7154374180450894},"rotation":{"radians":-2.4489770884975466E-4}},"curvature":0.0011296207077533735},{"time":1.6090377433938088,"velocity":0.8426066481831752,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-0.7154524626528294},"rotation":{"radians":-1.43629614104435E-4}},"curvature":0.0014770805957295724},{"time":1.7044707307238756,"velocity":0.5849375823919951,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-0.7154588241371141},"rotation":{"radians":-4.551855926430377E-5}},"curvature":0.0012543019646483913},{"time":1.9211142797579477,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-0.7154598441848493},"rotation":{"radians":0.0}},"curvature":3.552713678800261E-15}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMid2.wpilib.json b/src/main/deploy/paths/EightBallMid2.wpilib.json index 26e5d4d..1fb827b 100644 --- a/src/main/deploy/paths/EightBallMid2.wpilib.json +++ b/src/main/deploy/paths/EightBallMid2.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.176195646925303,"velocity":0.5285869407759091,"acceleration":2.999999999999998,"pose":{"translation":{"x":7.246564522385597,"y":-0.7155139830708503},"rotation":{"radians":-0.03301074916623491}},"curvature":-1.4169320623994563},{"time":0.21497233367530247,"velocity":0.6449170010259073,"acceleration":3.0000000000000018,"pose":{"translation":{"x":7.269286205526441,"y":-0.7166938776057213},"rotation":{"radians":-0.07356868815491435}},"curvature":-2.160240564890358},{"time":0.24699953652565942,"velocity":0.7409986095769783,"acceleration":2.9999999999999982,"pose":{"translation":{"x":7.291367816925049,"y":-0.7189196109771728},"rotation":{"radians":-0.13034803790848082}},"curvature":-2.9724600222132436},{"time":0.27455684939889846,"velocity":0.8236705481966954,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.312632273230702,"y":-0.7224713781941682},"rotation":{"radians":-0.20384375848792302}},"curvature":-3.859783847923219},{"time":0.28706924422183644,"velocity":0.8612077326655093,"acceleration":2.9999999999999947,"pose":{"translation":{"x":7.322907519803266,"y":-0.7248230876060552},"rotation":{"radians":-0.2469633412322504}},"curvature":-4.323361501562987},{"time":0.2988897193555486,"velocity":0.8966691580666457,"acceleration":1.4372274216190066,"pose":{"translation":{"x":7.332919952273369,"y":-0.7275965842604637},"rotation":{"radians":-0.29430316015307745}},"curvature":-4.789362932316134},{"time":0.3102197650367444,"velocity":0.9129530104078564,"acceleration":-3.0,"pose":{"translation":{"x":7.342652252668631,"y":-0.730817861216201},"rotation":{"radians":-0.34575274856226007}},"curvature":-5.2448864386307115},{"time":0.32152890268001205,"velocity":0.8790255974780534,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":7.3520881230942905,"y":-0.7345110075827688},"rotation":{"radians":-0.4011030452093752}},"curvature":-5.673301150541483},{"time":0.3331812815718787,"velocity":0.8440684608024535,"acceleration":-2.3349396646866505,"pose":{"translation":{"x":7.361212267921656,"y":-0.7386982448768685},"rotation":{"radians":-0.460027741375734}},"curvature":-6.05507734228002},{"time":0.3451995499454858,"velocity":0.8160065292760691,"acceleration":-1.5534212818965476,"pose":{"translation":{"x":7.3700103759765625,"y":-0.7433999633789062},"rotation":{"radians":-0.5220721667264541}},"curvature":-6.369447456164275},{"time":0.35753487235073245,"velocity":0.7968445769327036,"acceleration":-0.8064987399066847,"pose":{"translation":{"x":7.3784691027278315,"y":-0.7486347584894975},"rotation":{"radians":-0.5866537738316515}},"curvature":-6.596835433787617},{"time":0.370113223863055,"velocity":0.7867001522879121,"acceleration":-0.08469630474345373,"pose":{"translation":{"x":7.3865760524757205,"y":-0.7544194670859724},"rotation":{"radians":-0.6530772005654553}},"curvature":-6.721700100807001},{"time":0.3828513298248526,"velocity":0.7856212817835173,"acceleration":0.6207646255754383,"pose":{"translation":{"x":7.394319760540384,"y":-0.7607692038788809},"rotation":{"radians":-0.7205644949723411}},"curvature":-6.735169302137587},{"time":0.3956617987389037,"velocity":0.7935735677223941,"acceleration":1.3167694007908965,"pose":{"translation":{"x":7.401689675450325,"y":-0.7676973977684974},"rotation":{"radians":-0.7882978171901851}},"curvature":-6.636748578819588},{"time":0.408459077204561,"velocity":0.8104246324193718,"acceleration":2.006992912857606,"pose":{"translation":{"x":7.408676141130854,"y":-0.7752158282013261},"rotation":{"radians":-0.8554687993814396}},"curvature":-6.434575875688117},{"time":0.42116526370123264,"velocity":0.8359258586676388,"acceleration":2.6907422543149933,"pose":{"translation":{"x":7.4152703790925445,"y":-0.7833346615266055},"rotation":{"radians":-0.9213269426131445}},"curvature":-6.144123007026858},{"time":0.4337148582754833,"velocity":0.8696935830630972,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.421464470619685,"y":-0.792062487352814},"rotation":{"radians":-0.9852197163442566}},"curvature":-5.78572732604897},{"time":0.4460882493031231,"velocity":0.9068137561460166,"acceleration":3.0,"pose":{"translation":{"x":7.42725133895874,"y":-0.8014063549041748},"rotation":{"radians":-1.0466192793077236}},"curvature":-5.381647674000342},{"time":0.4583258055081431,"velocity":0.9435264247610767,"acceleration":2.999999999999995,"pose":{"translation":{"x":7.432624731506803,"y":-0.8113718093771604},"rotation":{"radians":-1.1051340316663605}},"curvature":-4.953358627820729},{"time":0.47048334862400293,"velocity":0.9799990541086561,"acceleration":2.999999999999982,"pose":{"translation":{"x":7.437579202000052,"y":-0.8219629282969981},"rotation":{"radians":-1.1605063335021422}},"curvature":-4.519573115046623},{"time":0.4826051644207643,"velocity":1.01636450149894,"acceleration":3.0,"pose":{"translation":{"x":7.4421100927022055,"y":-0.833182357874175},"rotation":{"radians":-1.2125997996055733}},"curvature":-4.095166469583263},{"time":0.4947258540646786,"velocity":1.052726570430683,"acceleration":2.9999999999999956,"pose":{"translation":{"x":7.446213516592979,"y":-0.8450313493609428},"rotation":{"radians":-1.2613803298943025}},"curvature":-3.690910282345266},{"time":0.5068718843289584,"velocity":1.089164661223522,"acceleration":2.9999999999999876,"pose":{"translation":{"x":7.449886339556542,"y":-0.8575097954078228},"rotation":{"radians":-1.306894703300488}},"curvature":-3.3137824106753158},{"time":0.5190629084365962,"velocity":1.1257377335464354,"acceleration":3.000000000000004,"pose":{"translation":{"x":7.45312616256997,"y":-0.8706162664201111},"rotation":{"radians":-1.3492496293615444}},"curvature":-2.9675944638423344},{"time":0.5436268496599665,"velocity":1.1994295572165465,"acceleration":3.0000000000000018,"pose":{"translation":{"x":7.45830078125,"y":-0.898701171875},"rotation":{"radians":-1.4250986701590653}},"curvature":-2.3718206794667163},{"time":0.5684834454795932,"velocity":1.2739993446754265,"acceleration":3.0,"pose":{"translation":{"x":7.461732205469161,"y":-0.9292495656106621},"rotation":{"radians":-1.4903490993636863}},"curvature":-1.8972644682319115},{"time":0.593640987194308,"velocity":1.3494719698195712,"acceleration":2.999999999999995,"pose":{"translation":{"x":7.463425889611244,"y":-0.9622061184048654},"rotation":{"radians":-1.5465120233189014}},"curvature":-1.5258519692589507},{"time":0.6190742338239772,"velocity":1.4257717097085785,"acceleration":2.999999999999997,"pose":{"translation":{"x":7.463397339638322,"y":-0.9974978353362531},"rotation":{"radians":-1.5950046216979124}},"curvature":-1.2372943445536448},{"time":0.644736948531325,"velocity":1.5027598538306217,"acceleration":3.000000000000002,"pose":{"translation":{"x":7.461671543121338,"y":-1.035035219192505},"rotation":{"radians":-1.637074129015746}},"curvature":-1.0133098607932398},{"time":0.6965024202650136,"velocity":1.6580562690316876,"acceleration":3.0,"pose":{"translation":{"x":7.453272148966789,"y":-1.1164134678244593},"rotation":{"radians":-1.7060109535050387}},"curvature":-0.7024214581009294},{"time":0.7484217413230949,"velocity":1.8138142322059314,"acceleration":2.9999999999999987,"pose":{"translation":{"x":7.438595581054687,"y":-1.2053390502929688},"rotation":{"radians":-1.7598414148638106}},"curvature":-0.5096067765020571},{"time":0.7999552231413788,"velocity":1.968414677660783,"acceleration":3.0000000000000044,"pose":{"translation":{"x":7.418124940991402,"y":-1.3006205740571022},"rotation":{"radians":-1.8030214069226786}},"curvature":-0.38682852273787793},{"time":0.8505967339186178,"velocity":2.1203392099925003,"acceleration":3.000000000000002,"pose":{"translation":{"x":7.392440319061279,"y":-1.4009142971038822},"rotation":{"radians":-1.83861094793171}},"curvature":-0.30694652214874857},{"time":0.899888738048394,"velocity":2.268215222381829,"acceleration":1.3460968931184591,"pose":{"translation":{"x":7.362200555205345,"y":-1.5047613570094112},"rotation":{"radians":-1.8687586562244822}},"curvature":-0.25448714727408545},{"time":0.9482263295305208,"velocity":2.333282304096749,"acceleration":0.30324551372276426,"pose":{"translation":{"x":7.328125,"y":-1.6106250000000006},"rotation":{"radians":-1.8950241676184996}},"curvature":-0.22053087572017724},{"time":0.9963372162168512,"velocity":2.347871714645603,"acceleration":-0.17307880083034716,"pose":{"translation":{"x":7.290975275635719,"y":-1.7169278100132948},"rotation":{"radians":-1.9185903109253033}},"curvature":-0.19998185675653543},{"time":1.0442579994976287,"velocity":2.3395776429405153,"acceleration":-2.999999999999996,"pose":{"translation":{"x":7.251537036895752,"y":-1.8220889377593998},"rotation":{"radians":-1.9404067180938465}},"curvature":-0.19014355026047416},{"time":1.0929426509757927,"velocity":2.1935236885060236,"acceleration":-3.000000000000002,"pose":{"translation":{"x":7.210601732134819,"y":-1.9245613297820099},"rotation":{"radians":-1.9612934639053132}},"curvature":-0.19003686718455529},{"time":1.1433546887258714,"velocity":2.042287575255787,"acceleration":-2.999999999999998,"pose":{"translation":{"x":7.168948364257812,"y":-2.022868957519533},"rotation":{"radians":-1.982024379952998}},"curvature":-0.20020347155366428},{"time":1.1951115938389032,"velocity":1.8870168599166917,"acceleration":-3.0,"pose":{"translation":{"x":7.127325251698494,"y":-2.115644046366216},"rotation":{"radians":-2.003404981709161}},"curvature":-0.22293185426852294},{"time":1.2477918907181875,"velocity":1.7289759692788385,"acceleration":-3.0,"pose":{"translation":{"x":7.086431789398192,"y":-2.201664304733278},"rotation":{"radians":-2.0263586820248065}},"curvature":-0.2630138771484163},{"time":1.300935198656413,"velocity":1.5695460454641619,"acceleration":-3.0,"pose":{"translation":{"x":7.046900209784507,"y":-2.2798901531100286},"rotation":{"radians":-2.0520365622478876}},"curvature":-0.329365520014453},{"time":1.3540456846675004,"velocity":1.4102145874308998,"acceleration":-2.9999999999999982,"pose":{"translation":{"x":7.009277343749999,"y":-2.349501953125003},"rotation":{"radians":-2.081969632892714}},"curvature":-0.43822447094387634},{"time":1.4066038418718951,"velocity":1.2525401158177158,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.974006381630896,"y":-2.409937236607078},"rotation":{"radians":-2.1182847059349985}},"curvature":-0.6192512361373247},{"time":1.4580970203923065,"velocity":1.0980605802564818,"acceleration":-3.000000000000001,"pose":{"translation":{"x":6.941408634185789,"y":-2.460927934646609},"rotation":{"radians":-2.1639902292367137}},"curvature":-0.9263958246762897},{"time":1.5080908210334283,"velocity":0.9480791783331165,"acceleration":-3.0,"pose":{"translation":{"x":6.911665293574332,"y":-2.5025376066565563},"rotation":{"radians":-2.2232479255908064}},"curvature":-1.452969931643203},{"time":1.5563885221686564,"velocity":0.8031860749274318,"acceleration":-2.999999999999997,"pose":{"translation":{"x":6.884799194335936,"y":-2.5351986694335977},"rotation":{"radians":-2.301188771290306}},"curvature":-2.3303573338487307},{"time":1.5799912095308917,"velocity":0.7323780128407261,"acceleration":-2.9999999999999982,"pose":{"translation":{"x":6.8724044566042695,"y":-2.5484186238888693},"rotation":{"radians":-2.34859494979231}},"curvature":-2.9284243203517053},{"time":1.6033996551447078,"velocity":0.6621526759992775,"acceleration":-3.0,"pose":{"translation":{"x":6.860656574368474,"y":-2.5597496262192774},"rotation":{"radians":-2.401760797928686}},"curvature":-3.6052758969197294},{"time":1.6268869487601023,"velocity":0.5916907951530944,"acceleration":-3.0,"pose":{"translation":{"x":6.84950574180111,"y":-2.5693660482298633},"rotation":{"radians":-2.4597729804382578}},"curvature":-4.270419440523796},{"time":1.650927723836384,"velocity":0.5195684699242497,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.838888835906978,"y":-2.5774722957611154},"rotation":{"radians":-2.520276046909255}},"curvature":-4.741626980469247},{"time":1.67635919415729,"velocity":0.4432740589615319,"acceleration":-3.0,"pose":{"translation":{"x":6.828728846553709,"y":-2.5843039720971195},"rotation":{"radians":-2.578999131141017}},"curvature":-4.746848371603827},{"time":1.704805743967665,"velocity":0.35793440953040684,"acceleration":-3.0,"pose":{"translation":{"x":6.818934306502339,"y":-2.590129041373734},"rotation":{"radians":-2.629713920493405}},"curvature":-4.00098049945812},{"time":1.824117213811134,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":6.799999999999996,"y":-2.6000000000000068},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576416074E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":7.2,"y":-0.715},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.18561803822360862,"velocity":0.5011687032037432,"acceleration":2.6999999999999997,"pose":{"translation":{"x":7.246509805083275,"y":-0.7155430880188942},"rotation":{"radians":-0.0350011181299882}},"curvature":-1.5127100780557379},{"time":0.22631257760103962,"velocity":0.6110439595228069,"acceleration":2.7000000000000015,"pose":{"translation":{"x":7.269105931106955,"y":-0.7167897682543843},"rotation":{"radians":-0.07832761851893318}},"curvature":-2.334420656075931},{"time":0.2597927090945532,"velocity":0.7014403145552937,"acceleration":-0.08416797278978987,"pose":{"translation":{"x":7.290950786590576,"y":-0.7191414356231689},"rotation":{"radians":-0.1395525129244173}},"curvature":-3.263282772933744},{"time":0.29010162656065624,"velocity":0.6988892744147187,"acceleration":-2.7000000000000037,"pose":{"translation":{"x":7.311837589647621,"y":-0.7228940822277218},"rotation":{"radians":-0.21970640501294192}},"curvature":-4.314442138931049},{"time":0.30532803573759026,"velocity":0.6577779696369969,"acceleration":-2.700000000000003,"pose":{"translation":{"x":7.321862860926544,"y":-0.7253787572213332},"rotation":{"radians":-0.26715373954274585}},"curvature":-4.8768249166265605},{"time":0.32128090008101395,"velocity":0.6147052359097529,"acceleration":-2.2718220683025194,"pose":{"translation":{"x":7.331580549359321,"y":-0.7283090326189995},"rotation":{"radians":-0.3195540309110183}},"curvature":-5.44923810979411},{"time":0.33804851075922254,"velocity":0.5766122079382936,"acceleration":-1.808467530328736,"pose":{"translation":{"x":7.3409705967268675,"y":-0.7317123590575647},"rotation":{"radians":-0.3768130423863286}},"curvature":-6.013133652151949},{"time":0.3556138293735418,"velocity":0.5448458995644183,"acceleration":-1.377717479971638,"pose":{"translation":{"x":7.3500141697414225,"y":-0.7356141742598266},"rotation":{"radians":-0.4386948535617238}},"curvature":-6.54366755130846},{"time":0.3739173384361787,"velocity":0.5196288351840042,"acceleration":-0.9750542191131898,"pose":{"translation":{"x":7.3586936382955175,"y":-0.7400379414865165},"rotation":{"radians":-0.5047911520002755}},"curvature":-7.011005995792478},{"time":0.39286733812909297,"velocity":0.5011515580312345,"acceleration":-0.5952264574689293,"pose":{"translation":{"x":7.366992553710937,"y":-0.7450051879882812},"rotation":{"radians":-0.5745030191870489}},"curvature":-7.383294440478745},{"time":0.41233999209292455,"velocity":0.4895609191948247,"acceleration":-0.23219580202763299,"pose":{"translation":{"x":7.37489562698768,"y":-0.7505355434576632},"rotation":{"radians":-0.6470431198312998}},"curvature":-7.631170999207397},{"time":0.4321841392506889,"velocity":0.48495319152997324,"acceleration":0.12042438759254688,"pose":{"translation":{"x":7.382388707052916,"y":-0.7566467784810811},"rotation":{"radians":-0.7214638781945694}},"curvature":-7.733002970359292},{"time":0.45223094758257126,"velocity":0.48736731614652534,"acceleration":0.46856974738435647,"pose":{"translation":{"x":7.389458759009955,"y":-0.7633548429908115},"rotation":{"radians":-0.7967118933901203}},"curvature":-7.67941005620971},{"time":0.47230715486631836,"velocity":0.4967744195219067,"acceleration":0.8167215181565207,"pose":{"translation":{"x":7.3960938423872,"y":-0.7706739047169685},"rotation":{"radians":-0.8717017970267}},"curvature":-7.475544337770991},{"time":0.492249500109957,"velocity":0.5130617620048927,"acceleration":1.1668842026742907,"pose":{"translation":{"x":7.402283089387114,"y":-0.7786163876394857},"rotation":{"radians":-0.945396735690179}},"curvature":-7.140250735362253},{"time":0.5119175444245033,"velocity":0.5360120922130347,"acceleration":1.5177593154443771,"pose":{"translation":{"x":7.408016683135182,"y":-0.7871930104400963},"rotation":{"radians":-1.0168804926180126}},"curvature":-6.702377284278848},{"time":0.5312026214692818,"velocity":0.5652821975468096,"acceleration":1.8643410335216777,"pose":{"translation":{"x":7.413285835928866,"y":-0.7964128249543136},"rotation":{"radians":-1.0854090438103314}},"curvature":-6.195516955724278},{"time":0.5500318690235017,"velocity":0.6003863363924795,"acceleration":2.198120405334121,"pose":{"translation":{"x":7.418082767486572,"y":-0.8062832546234131},"rotation":{"radians":-1.1504356775241285}},"curvature":-5.652809077092928},{"time":0.5683676070722279,"velocity":0.6406904963442459,"acceleration":2.5079745092906225,"pose":{"translation":{"x":7.422400683196611,"y":-0.8168101329464116},"rotation":{"radians":-1.2116107716220759}},"curvature":-5.103047095861068},{"time":0.5862032473753316,"velocity":0.6854218275813063,"acceleration":2.700000000000005,"pose":{"translation":{"x":7.426233752366156,"y":-0.8279977419320493},"rotation":{"radians":-1.2687623098382128}},"curvature":-4.568594092565457},{"time":0.6035740431139919,"velocity":0.7323229760756892,"acceleration":2.7000000000000033,"pose":{"translation":{"x":7.429577086470206,"y":-0.8398488505507703},"rotation":{"radians":-1.321865159656409}},"curvature":-4.064939827552741},{"time":0.6205696312402517,"velocity":0.7782110640165907,"acceleration":2.699999999999995,"pose":{"translation":{"x":7.432426717400551,"y":-0.8523647531867027},"rotation":{"radians":-1.3710064961660495}},"curvature":-3.6013796997049092},{"time":0.653798500637175,"velocity":0.8679290113882836,"acceleration":2.699999999999998,"pose":{"translation":{"x":7.436633468884975,"y":-0.8793889758270235},"rotation":{"radians":-1.4581210241861995}},"curvature":-2.8082821953841726},{"time":0.6864157481106786,"velocity":0.9559955795667432,"acceleration":2.700000000000003,"pose":{"translation":{"x":7.43883984375,"y":-0.909052734375},"rotation":{"radians":-1.5319155465954188}},"curvature":-2.187808044399357},{"time":0.7186946971645294,"velocity":1.0431487420121404,"acceleration":2.700000000000001,"pose":{"translation":{"x":7.439045013528317,"y":-0.9413172208983451},"rotation":{"radians":-1.5944160241119611}},"curvature":-1.7137886686522077},{"time":0.7507735226736758,"velocity":1.1297615708868358,"acceleration":2.7,"pose":{"translation":{"x":7.437260788321495,"y":-0.9761237254738808},"rotation":{"radians":-1.647530756225237}},"curvature":-1.3550438251889343},{"time":0.8145076192946602,"velocity":1.3018436317634936,"acceleration":1.9009265353098717,"pose":{"translation":{"x":7.427830410003662,"y":-1.053035821914673},"rotation":{"radians":-1.7319621716711908}},"curvature":-0.8776200100484256},{"time":0.8787738090352114,"velocity":1.4240089371645663,"acceleration":1.0320377589657466,"pose":{"translation":{"x":7.410869511723519,"y":-1.1389680621027947},"rotation":{"radians":-1.7953267099312762}},"curvature":-0.5981874451621899},{"time":0.9452304081993644,"velocity":1.4925946568344237,"acceleration":0.6704363837971291,"pose":{"translation":{"x":7.386856567382813,"y":-1.232859802246094},"rotation":{"radians":-1.8443193056718796}},"curvature":-0.42887728768877253},{"time":1.014564015143465,"velocity":1.539078429549638,"acceleration":0.4379762366154687,"pose":{"translation":{"x":7.356405448794365,"y":-1.3334500911831857},"rotation":{"radians":-1.8833328796774655}},"curvature":-0.3227070850970345},{"time":1.086515574956774,"velocity":1.570591502935284,"acceleration":0.28764631771309085,"pose":{"translation":{"x":7.320243152618408,"y":-1.4393170452117925},"rotation":{"radians":-1.9153030970810474}},"curvature":-0.2543047895084811},{"time":1.160531968955257,"velocity":1.5918820461193488,"acceleration":0.18693718068502835,"pose":{"translation":{"x":7.2791875272989275,"y":-1.5489172229170802},"rotation":{"radians":-1.9422516839062522}},"curvature":-0.209624443804745},{"time":1.2358666468993684,"velocity":1.6059648984220356,"acceleration":0.11490586723990336,"pose":{"translation":{"x":7.234125000000001,"y":-1.6606250000000005},"rotation":{"radians":-1.965624524729717}},"curvature":-0.1807211323727493},{"time":1.3116536756420727,"velocity":1.6146732726852515,"acceleration":0.057708477203834777,"pose":{"translation":{"x":7.185988303542137,"y":-1.7727719441056258},"rotation":{"radians":-1.9865046578778909}},"curvature":-0.16310054864071177},{"time":1.3869657142924408,"velocity":1.6190194157508806,"acceleration":0.0049975529370985036,"pose":{"translation":{"x":7.1357342033386235,"y":-1.88368618965149},"rotation":{"radians":-2.0057518949614077}},"curvature":-0.15437744131502087},{"time":1.4608626989337086,"velocity":1.6193887198435173,"acceleration":-0.05297813546347199,"pose":{"translation":{"x":7.084321224331855,"y":-1.9917318126559267},"rotation":{"radians":-2.024100963913834}},"curvature":-0.15363837252157278},{"time":1.5324356797043124,"velocity":1.6155969167727278,"acceleration":-0.12834859710760868,"pose":{"translation":{"x":7.032687377929688,"y":-2.095348205566407},"rotation":{"radians":-2.042238626552442}},"curvature":-0.16124278314075052},{"time":1.6008490316547188,"velocity":1.606816159026464,"acceleration":-0.23974686245403207,"pose":{"translation":{"x":6.981727888941764,"y":-2.1930894520878805},"rotation":{"radians":-2.0608745532745756}},"curvature":-0.17899026334377835},{"time":1.6653838574096969,"velocity":1.5913441370326904,"acceleration":-0.9141348079534928,"pose":{"translation":{"x":6.932272922515868,"y":-2.2836637020111104},"rotation":{"radians":-2.0808190837865843}},"curvature":-0.21073857875662377},{"time":1.7260677199073722,"velocity":1.5358709060425018,"acceleration":-2.7,"pose":{"translation":{"x":6.885065311074255,"y":-2.3659725460410135},"rotation":{"radians":-2.1030825654130876}},"curvature":-0.26376144837191884},{"time":1.7848057436642981,"velocity":1.3772782418988014,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.840738281249999,"y":-2.4391503906250023},"rotation":{"radians":-2.129015304185589}},"curvature":-0.3514924199646048},{"time":1.8429502898001922,"velocity":1.2202879673318878,"acceleration":-2.699999999999998,"pose":{"translation":{"x":6.7997931808233245,"y":-2.502603832781319},"rotation":{"radians":-2.1605122947808555}},"curvature":-0.4989493268459689},{"time":1.8999104848927477,"velocity":1.0664954405819882,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":6.762577205657957,"y":-2.5560510349273713},"rotation":{"radians":-2.200302020305231}},"curvature":-0.7530310265685853},{"time":1.9551577746428328,"velocity":0.9173277582567586,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.729261126637456,"y":-2.599561099708085},"rotation":{"radians":-2.2522772528856647}},"curvature":-1.1992166376442492},{"time":2.008384559314525,"velocity":0.7736154396431905,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.69981701660156,"y":-2.633593444824225},"rotation":{"radians":-2.3215377094330125}},"curvature":-1.9722505146070455},{"time":2.0342925478455514,"velocity":0.7036638706094184,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.686478409957137,"y":-2.6473154572118123},"rotation":{"radians":-2.364268687217763}},"curvature":-2.522324344886889},{"time":2.059883585157657,"velocity":0.634568069866733,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.67399597728252,"y":-2.6590371778607427},"rotation":{"radians":-2.412814916615144}},"curvature":-3.1726839328595746},{"time":2.085424932797736,"velocity":0.5656064312385193,"acceleration":-2.7,"pose":{"translation":{"x":6.662300425384189,"y":-2.668943344196311},"rotation":{"radians":-2.4666153391310455}},"curvature":-3.854197267072015},{"time":2.1113983772358043,"velocity":0.4954781312557351,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.651305866241448,"y":-2.677250471115121},"rotation":{"radians":-2.5237315022575926}},"curvature":-4.401365184508673},{"time":2.1386737463254133,"velocity":0.42183463471379096,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.640909120973196,"y":-2.684208081448457},"rotation":{"radians":-2.5802250024313738}},"curvature":-4.534047099645387},{"time":2.1689610863530175,"velocity":0.3400588166392591,"acceleration":-2.7,"pose":{"translation":{"x":6.630989023804658,"y":-2.6900999364256926},"rotation":{"radians":-2.6299027273834015}},"curvature":-3.9202737890595847},{"time":2.29490879621941,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.611999999999994,"y":-2.70000000000001},"rotation":{"radians":-2.6779450445889683}},"curvature":8.473716576415405E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/EightBallMidComplete.wpilib.json b/src/main/deploy/paths/EightBallMidComplete.wpilib.json index 1347f0f..0dca329 100644 --- a/src/main/deploy/paths/EightBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/EightBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22851073171807768,"velocity":0.6855321951542331,"acceleration":3.0,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.32286738548734767,"velocity":0.9686021564620431,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.3948508967117983,"velocity":1.1845526901353949,"acceleration":3.000000000000002,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.45504953863530545,"velocity":1.3651486159059165,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5075769496703644,"velocity":1.5227308490110931,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5545708746789335,"velocity":1.6637126240368005,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.5973365928558361,"velocity":1.7920097785675082,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6367625752176861,"velocity":1.9102877256530584,"acceleration":-0.39480980187976056,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.6747121786647302,"velocity":1.8953048502347154,"acceleration":-0.7021730002147804,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.7127763389927304,"velocity":1.868577224576547,"acceleration":-0.4401004459228274,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.7511092582596577,"velocity":1.8517068897136486,"acceleration":-0.17400254925795713,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.7896601305578692,"velocity":1.8449989396576418,"acceleration":0.08372895263229682,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.8283828420132677,"velocity":1.848241151730885,"acceleration":0.32040119497795183,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":0.8672396362007758,"velocity":1.860690915021575,"acceleration":0.5251363087983144,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":0.906204315334355,"velocity":1.8811526827952934,"acceleration":0.6905017989409158,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":0.9452636685748346,"velocity":1.9081232364733132,"acceleration":0.8133740442414022,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":0.9844165273519864,"velocity":1.9399691555604976,"acceleration":0.8949317659517233,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.0236706441727126,"velocity":1.9750989116477453,"acceleration":0.9399001600451896,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.063038145418846,"velocity":2.0121004323695653,"acceleration":0.9553480479826941,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.1025304872768191,"velocity":2.049829364073845,"acceleration":0.9494024824000104,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.1421537132339343,"velocity":2.087447753158227,"acceleration":0.9301775648872205,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.1819045291156953,"velocity":2.1244230702774036,"acceleration":0.9050734432547365,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.221767420455895,"velocity":2.1605019146007676,"acceleration":0.8804626256424855,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.2617128175476713,"velocity":2.1956723438065247,"acceleration":0.8616916300805181,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.3016961872085873,"velocity":2.2301256787857513,"acceleration":0.8532946962433116,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.341657884138109,"velocity":2.264224782828595,"acceleration":0.8593254128101726,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.3815236046845703,"velocity":2.2984824095941576,"acceleration":0.8837378969826192,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.42120533048524,"velocity":2.3335506545018823,"acceleration":0.9307724426536456,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.4606027154283423,"velocity":2.3702206547195397,"acceleration":1.0053080180459415,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.4996049518717887,"velocity":2.40942991573786,"acceleration":1.113123412063664,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":1.5380932533029306,"velocity":2.4522721451514276,"acceleration":1.2609415748588182,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":1.575944217852111,"velocity":2.4999999999999964,"acceleration":9.47993122549823E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":1.6134203724179241,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6507579461846327,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6878305177006696,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7245245401177383,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.76073891203737,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.796384548357484,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8313839511189436,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.865670780352113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8991894249234176,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9318945733818982,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.963750784805772,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9947320596489881,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.024821410587785,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.05401043336725,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.082298877647874,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.109694217852113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.136211224010942,"velocity":2.5,"acceleration":-2.9297166719305383,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1622694050077933,"velocity":2.423656912693341,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.2143566851934087,"velocity":2.2673950721364946,"acceleration":-2.999999999999996,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.266729803510551,"velocity":2.110275717185067,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3199401431120066,"velocity":1.9506446983807002,"acceleration":-3.0,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3748347556748115,"velocity":1.7859608606922859,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.432703611156226,"velocity":1.6123542942480418,"acceleration":-3.0,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.495580885042337,"velocity":1.4237224725897093,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5669701453930265,"velocity":1.2095546915376412,"acceleration":-2.999999999999998,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6074049251020424,"velocity":1.0882503524105933,"acceleration":-3.000000000000005,"pose":{"translation":{"x":7.246453259419017,"y":-1.100468353001181},"rotation":{"radians":-0.03029627796319239}},"curvature":-1.319268880795511},{"time":2.628698299737897,"velocity":1.0243702285030287,"acceleration":-3.0000000000000027,"pose":{"translation":{"x":7.268919966550246,"y":-1.1015433468916538},"rotation":{"radians":-0.06812545247042903}},"curvature":-2.0669897873012513},{"time":2.6505796043600514,"velocity":0.9587263146365661,"acceleration":-3.0,"pose":{"translation":{"x":7.290521387883752,"y":-1.1035709212816442},"rotation":{"radians":-0.12227213585990128}},"curvature":-2.9587916487214816},{"time":2.673013681367624,"velocity":0.8914240836138477,"acceleration":-3.0,"pose":{"translation":{"x":7.311020896661181,"y":-1.1068060090328942},"rotation":{"radians":-0.19454364441226238}},"curvature":-4.050132379901242},{"time":2.684453521072769,"velocity":0.8571045644984139,"acceleration":-3.0,"pose":{"translation":{"x":7.320790317439845,"y":-1.108947806006542},"rotation":{"radians":-0.23814906997127733}},"curvature":-4.681287981030412},{"time":2.6960641268278294,"velocity":0.822272747233233,"acceleration":-3.0,"pose":{"translation":{"x":7.330206758370542,"y":-1.111473547983105},"rotation":{"radians":-0.2870838732590895}},"curvature":-5.368039436240083},{"time":2.707877775932446,"velocity":0.786831799919383,"acceleration":-3.0000000000000058,"pose":{"translation":{"x":7.339247504131675,"y":-1.1144068404739353},"rotation":{"radians":-0.34155174820354145}},"curvature":-6.10144429198697},{"time":2.7199428695455974,"velocity":0.7506365190799287,"acceleration":-3.0,"pose":{"translation":{"x":7.347891290084308,"y":-1.1177695476378748},"rotation":{"radians":-0.4016577762334248}},"curvature":-6.863367195390947},{"time":2.732328983652657,"velocity":0.7134781767587501,"acceleration":-2.999999999999997,"pose":{"translation":{"x":7.356118276001478,"y":-1.121581825615378},"rotation":{"radians":-0.4673549943172977}},"curvature":-7.6244722420236295},{"time":2.745133815359972,"velocity":0.6750636816368045,"acceleration":-2.6366830196581335,"pose":{"translation":{"x":7.363910019797516,"y":-1.125862155862635},"rotation":{"radians":-0.5383877761898741}},"curvature":-8.343686165228133},{"time":2.7584424810573904,"velocity":0.6399729487781146,"acceleration":-1.7769445288668568,"pose":{"translation":{"x":7.371249451257359,"y":-1.1306273784856944},"rotation":{"radians":-0.6142422608915585}},"curvature":-8.970417020168014},{"time":2.772233316981256,"velocity":0.6154673983347015,"acceleration":-0.9662749893703216,"pose":{"translation":{"x":7.378120845765868,"y":-1.1358927255745868},"rotation":{"radians":-0.694118349599752}},"curvature":-9.450474002120954},{"time":2.7863879742209994,"velocity":0.6017901070608277,"acceleration":-0.1875831048849451,"pose":{"translation":{"x":7.384509798037142,"y":-1.1416718545374471},"rotation":{"radians":-0.776938635525735}},"curvature":-9.735408476030893},{"time":2.800761549923939,"velocity":0.5990938671021715,"acceleration":0.5786763545271786,"pose":{"translation":{"x":7.390403195843834,"y":-1.1479768814346385},"rotation":{"radians":-0.8614041317901993}},"curvature":-9.793113499421619},{"time":2.8151949088373547,"velocity":0.6074461106217692,"acceleration":1.3512500497389404,"pose":{"translation":{"x":7.3957891937464675,"y":-1.154818414312874},"rotation":{"radians":-0.9460943844775829}},"curvature":-9.616022896217997},{"time":2.829530428985466,"velocity":0.6268169829349379,"acceleration":2.1444781995587228,"pose":{"translation":{"x":7.40065718682275,"y":-1.162205586539341},"rotation":{"radians":-1.0295945659911594}},"curvature":-9.223472277259154},{"time":2.843627613085366,"velocity":0.6570480869123403,"acceleration":2.964815882581206,"pose":{"translation":{"x":7.404997784396893,"y":-1.1701460901358236},"rotation":{"radians":-1.1106218383771238}},"curvature":-8.657088121620294},{"time":2.8573750588687528,"velocity":0.6978067325158486,"acceleration":3.0000000000000604,"pose":{"translation":{"x":7.408802783768922,"y":-1.178646209112826},"rotation":{"radians":-1.1881235850165326}},"curvature":-7.971153960622058},{"time":2.870793870807247,"velocity":0.7380631683313326,"acceleration":2.999999999999978,"pose":{"translation":{"x":7.412065143943997,"y":-1.1877108528036944},"rotation":{"radians":-1.2613308478052403}},"curvature":-7.221753953199051},{"time":2.8839989159104573,"velocity":0.7776783036409628,"acceleration":3.0000000000000213,"pose":{"translation":{"x":7.414778959361725,"y":-1.1973435891987416},"rotation":{"radians":-1.3297653307739847}},"curvature":-6.4581578382775735},{"time":2.8970797192684477,"velocity":0.8169207137149346,"acceleration":3.0,"pose":{"translation":{"x":7.416939433625478,"y":-1.207546678279369},"rotation":{"radians":-1.393210316952152}},"curvature":-5.718104209264924},{"time":2.9101026324067005,"velocity":0.8559894531296925,"acceleration":3.000000000000005,"pose":{"translation":{"x":7.418542853231706,"y":-1.2183211053521896},"rotation":{"radians":-1.4516609735687045}},"curvature":-5.0267761645186875},{"time":2.9231160981259343,"velocity":0.8950298502873935,"acceleration":3.0000000000000324,"pose":{"translation":{"x":7.419586561299257,"y":-1.2296666143831518},"rotation":{"radians":-1.5052684214084193}},"curvature":-4.398243194153372},{"time":2.9361546373251755,"velocity":0.9341454678851169,"acceleration":2.9999999999999654,"pose":{"translation":{"x":7.420068931298684,"y":-1.2415817413316619},"rotation":{"radians":-1.5542875714605175}},"curvature":-3.838016276555127},{"time":2.962387530612709,"velocity":1.0128441477477164,"acceleration":3.000000000000036,"pose":{"translation":{"x":7.419348145109851,"y":-1.2671091527909772},"rotation":{"radians":-1.6398507277113328}},"curvature":-2.917417225974206},{"time":2.988908873228704,"velocity":1.0924081755957016,"acceleration":2.9999999999999782,"pose":{"translation":{"x":7.416387091177872,"y":-1.294868733971218},"rotation":{"radians":-1.7110773325418644}},"curvature":-2.2285872384297742},{"time":3.01573516597123,"velocity":1.172887053823279,"acceleration":2.9999999999999627,"pose":{"translation":{"x":7.411207170318989,"y":-1.3248086843926672},"rotation":{"radians":-1.770576272089184}},"curvature":-1.7200119058360206},{"time":3.0428355369759528,"velocity":1.254188166837447,"acceleration":3.0000000000000124,"pose":{"translation":{"x":7.4038437469911225,"y":-1.3568610755467443},"rotation":{"radians":-1.820592107940096}},"curvature":-1.344952042416304},{"time":3.070153054044293,"velocity":1.3361407180424683,"acceleration":2.9999999999999902,"pose":{"translation":{"x":7.394345308631966,"y":-1.3909429175879429},"rotation":{"radians":-1.8629610881600354}},"curvature":-1.066776993002733},{"time":3.1251464334866763,"velocity":1.5011208563696172,"acceleration":2.9999999999999973,"pose":{"translation":{"x":7.369197907498064,"y":-1.4647940884166677},"rotation":{"radians":-1.9303207828865319}},"curvature":-0.700913398882251},{"time":3.1801254299921244,"velocity":1.6660578458859614,"acceleration":3.000000000000007,"pose":{"translation":{"x":7.336383380862248,"y":-1.545437585669859},"rotation":{"radians":-1.9810854450552438}},"curvature":-0.48678344759105546},{"time":3.234502974744613,"velocity":1.8291904801434278,"acceleration":2.9999999999999933,"pose":{"translation":{"x":7.296676307984395,"y":-1.6317760850316636},"rotation":{"radians":-2.0205587822636133}},"curvature":-0.35581674367284205},{"time":3.2877480590960606,"velocity":1.9889257331977697,"acceleration":2.9999999999999876,"pose":{"translation":{"x":7.250980532258531,"y":-1.7225736832213316},"rotation":{"radians":-2.052154330745463}},"curvature":-0.272676387194201},{"time":3.3393922916510848,"velocity":2.1438584308628412,"acceleration":2.999999999999992,"pose":{"translation":{"x":7.200302260032064,"y":-1.8164900321351913},"rotation":{"radians":-2.078142236038709}},"curvature":-0.21848488416291567},{"time":3.389026520853825,"velocity":2.2927611184710606,"acceleration":1.7602617899017508,"pose":{"translation":{"x":7.145723159425027,"y":-1.9121144729886241},"rotation":{"radians":-2.1000895426348007}},"curvature":-0.18279950109004067},{"time":3.4368781221491154,"velocity":2.376992463816774,"acceleration":-0.448180883447394,"pose":{"translation":{"x":7.088373459149306,"y":-2.008000170458039},"rotation":{"radians":-2.119124178347165}},"curvature":-0.15971985407340575},{"time":3.4840196932847407,"velocity":2.355864512818111,"acceleration":-2.999999999999996,"pose":{"translation":{"x":7.029405047327874,"y":-2.1026982468228463},"rotation":{"radians":-2.136099285795778}},"curvature":-0.14596242302645385},{"time":3.532012815317769,"velocity":2.2118851467190273,"acceleration":-3.000000000000004,"pose":{"translation":{"x":6.969964570314035,"y":-2.194791916107434},"rotation":{"radians":-2.151701710677999}},"curvature":-0.1399205308761633},{"time":3.5815797871141575,"velocity":2.0631842313298616,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":6.911166531510655,"y":-2.28293061822314},"rotation":{"radians":-2.1665305881291537}},"curvature":-0.14127792706547193},{"time":3.6322493880323914,"velocity":1.91117542857516,"acceleration":-3.0,"pose":{"translation":{"x":6.854066390189391,"y":-2.365864153110232},"rotation":{"radians":-2.1811630644764497}},"curvature":-0.15102290082887804},{"time":3.6834840642992823,"velocity":1.7574713997744869,"acceleration":-3.0,"pose":{"translation":{"x":6.799633660309938,"y":-2.442476814879872},"rotation":{"radians":-2.196220892033287}},"curvature":-0.17190256606202614},{"time":3.7346685295655555,"velocity":1.6039180039756675,"acceleration":-3.0,"pose":{"translation":{"x":6.748725009339258,"y":-2.5118215259561016},"rotation":{"radians":-2.212452725775109}},"curvature":-0.20959133303036753},{"time":3.7850968736454673,"velocity":1.4526329717359314,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.702057357070814,"y":-2.5731539712178138},"rotation":{"radians":-2.230852906292372}},"curvature":-0.2752960353954891},{"time":3.8803425077482427,"velocity":1.1668960694276058,"acceleration":-2.999999999999998,"pose":{"translation":{"x":6.623452582362421,"y":-2.670023420939335},"rotation":{"radians":-2.2806214520910677}},"curvature":-0.6053526530580257},{"time":3.9232680649365714,"velocity":1.0381193978626204,"acceleration":-3.000000000000003,"pose":{"translation":{"x":6.592008450515033,"y":-2.7053928147089477},"rotation":{"radians":-2.3176021111430174}},"curvature":-1.0180108404391697},{"time":3.961759655139707,"velocity":0.9226446272532131,"acceleration":-3.0,"pose":{"translation":{"x":6.565737496193482,"y":-2.7324829895675955},"rotation":{"radians":-2.3691895004088437}},"curvature":-1.8453908893564248},{"time":3.995077693076158,"velocity":0.8226905134438611,"acceleration":-3.0,"pose":{"translation":{"x":6.544254383112274,"y":-2.752075454798034},"rotation":{"radians":-2.4429519712858796}},"curvature":-3.4712152639078635},{"time":4.009724957254997,"velocity":0.7787487209073424,"acceleration":-3.0,"pose":{"translation":{"x":6.535106238022585,"y":-2.759414778783516},"rotation":{"radians":-2.4903369045713104}},"curvature":-4.677026935702093},{"time":4.023110582984799,"velocity":0.7385918437179384,"acceleration":-3.0,"pose":{"translation":{"x":6.526872620227842,"y":-2.7653592869897214},"rotation":{"radians":-2.5444599796315197}},"curvature":-6.007139028985739},{"time":4.035399953953152,"velocity":0.7017237308128799,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.519416249441682,"y":-2.7701268965146633},"rotation":{"radians":-2.6024368965833506}},"curvature":-6.995883284614772},{"time":4.0468560526731014,"velocity":0.6673554346530308,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.512577660557761,"y":-2.773965264180783},"rotation":{"radians":-2.657556932845574}},"curvature":-6.765796527922793},{"time":4.0688471551397685,"velocity":0.6013821272530296,"acceleration":-3.0,"pose":{"translation":{"x":6.499999999999997,"y":-2.7800000000000016},"rotation":{"radians":-2.714965160462927}},"curvature":-2.6555993490677373E-13},{"time":4.09253186587704,"velocity":0.5303279950412128,"acceleration":-3.000000000000002,"pose":{"translation":{"x":6.487723377682077,"y":-2.785375982625394},"rotation":{"radians":-2.7564018109602286}},"curvature":-6.15200093903577},{"time":4.105398508519192,"velocity":0.4917280671147566,"acceleration":-3.0,"pose":{"translation":{"x":6.481573363494451,"y":-2.787702090646249},"rotation":{"radians":-2.8069964803170873}},"curvature":-9.262660823828876},{"time":4.1190929136917545,"velocity":0.45064485159707074,"acceleration":-3.0,"pose":{"translation":{"x":6.475411413096477,"y":-2.789616940520493},"rotation":{"radians":-2.8770511086188177}},"curvature":-12.466539743546049},{"time":4.126323741848238,"velocity":0.4289523671276198,"acceleration":-3.0,"pose":{"translation":{"x":6.472325400347914,"y":-2.790384803317061},"rotation":{"radians":-2.919246803028493}},"curvature":-14.067221772378913},{"time":4.133870956794675,"velocity":0.40631072228830745,"acceleration":-2.9999999999999956,"pose":{"translation":{"x":6.469235868121625,"y":-2.7910089917291048},"rotation":{"radians":-2.9660566541833933}},"curvature":-15.623161265956721},{"time":4.141802655384934,"velocity":0.38251562651753157,"acceleration":-2.778995935541285,"pose":{"translation":{"x":6.466142779184198,"y":-2.791477475751396},"rotation":{"radians":-3.0172537129004535}},"curvature":-17.085434277986494},{"time":4.150191997169828,"velocity":0.3592016797954446,"acceleration":-1.9621857442111865,"pose":{"translation":{"x":6.463046161219631,"y":-2.791778965285789},"rotation":{"radians":-3.0725012120644393}},"curvature":-18.394686312759717},{"time":4.159043281527767,"velocity":0.3418338158103368,"acceleration":-1.2827823996625713,"pose":{"translation":{"x":6.459946105193779,"y":-2.7919028969218647},"rotation":{"radians":-3.1313372571696307}},"curvature":-19.486096556994575},{"time":4.1682839075227145,"velocity":0.3299801034221532,"acceleration":-0.6940623511654422,"pose":{"translation":{"x":6.456842763718804,"y":-2.7918394207175727},"rotation":{"radians":3.090014628006359}},"curvature":-20.29695791936882},{"time":4.177826543964612,"velocity":0.32335691873697314,"acceleration":-0.16050191244357367,"pose":{"translation":{"x":6.453736349417611,"y":-2.7915793869798775},"rotation":{"radians":3.0258936564331744}},"curvature":-20.775909875903174},{"time":4.187572511375943,"velocity":0.3217926723288418,"acceleration":0.3463169056488674,"pose":{"translation":{"x":6.450627133288312,"y":-2.791114333045399},"rotation":{"radians":2.9602868160099893}},"curvature":-20.891905566984803},{"time":4.197417008769472,"velocity":0.32520198820383717,"acceleration":0.8499934016959338,"pose":{"translation":{"x":6.447515443068656,"y":-2.790436470061059},"rotation":{"radians":2.8940465389525842}},"curvature":-20.640524308152266},{"time":4.207255484348972,"velocity":0.3335646275291587,"acceleration":1.370466137587277,"pose":{"translation":{"x":6.444401661600485,"y":-2.789538669764723},"rotation":{"radians":2.82802600857349}},"curvature":-20.04567743561068},{"time":4.21699013611279,"velocity":0.34690563813267716,"acceleration":1.9244106184216545,"pose":{"translation":{"x":6.4412862251941805,"y":-2.7884144512658433},"rotation":{"radians":2.763028051590503}},"curvature":-19.156082341287096},{"time":4.226535495067351,"velocity":0.36527482826148017,"acceleration":2.5250992915133477,"pose":{"translation":{"x":6.438169621993106,"y":-2.7870579678261054},"rotation":{"radians":2.6997609889715117}},"curvature":-18.037536199313763},{"time":4.235822305577407,"velocity":0.38872494690084025,"acceleration":2.9999999999999347,"pose":{"translation":{"x":6.435052390338058,"y":-2.7854639936400676},"rotation":{"radians":2.6388075458793896}},"curvature":-16.763218002949678},{"time":4.244816993183615,"velocity":0.4157090097194641,"acceleration":2.999999999999937,"pose":{"translation":{"x":6.431935117131708,"y":-2.783627910615809},"rotation":{"radians":2.580609550420515}},"curvature":-15.404481952570594},{"time":4.253557817463828,"velocity":0.44193148256010456,"acceleration":2.999999999999754,"pose":{"translation":{"x":6.4288184362030565,"y":-2.781545695155569},"rotation":{"radians":2.5254676721113087}},"curvature":-14.02394310711014},{"time":4.262114737385764,"velocity":0.46760224232591113,"acceleration":3.0000000000002505,"pose":{"translation":{"x":6.42570302667187,"y":-2.779213904936394},"rotation":{"radians":2.473552943635361}},"curvature":-12.671584893555588},{"time":4.2705400811810454,"velocity":0.4928782737117564,"acceleration":3.000000000000211,"pose":{"translation":{"x":6.422589611313139,"y":-2.7766296656907787},"rotation":{"radians":2.424925719879724}},"curvature":-11.383666243410126},{"time":4.278873308336619,"velocity":0.5178779551784787,"acceleration":2.999999999999947,"pose":{"translation":{"x":6.419478954921514,"y":-2.7737906579873117},"rotation":{"radians":2.3795578870961305}},"curvature":-10.183662676609572},{"time":4.2953720290809185,"velocity":0.5673741174113744,"acceleration":2.999999999999966,"pose":{"translation":{"x":6.413269178503195,"y":-2.7673417543454994},"rotation":{"radians":2.298176568793801}},"curvature":-8.090290466124452},{"time":4.311774881248613,"velocity":0.6165826739144591,"acceleration":2.999999999999905,"pose":{"translation":{"x":6.4070805940164055,"y":-2.7598592329459772},"rotation":{"radians":2.228196497731612}},"curvature":-6.409038928054524},{"time":4.328167351522133,"velocity":0.6657600847350147,"acceleration":3.0000000000000635,"pose":{"translation":{"x":6.4009206656998625,"y":-2.7513431640624413},"rotation":{"radians":2.168122245297885}},"curvature":-5.093159786686278},{"time":4.344589432692641,"velocity":0.7150263282465403,"acceleration":3.000000000000073,"pose":{"translation":{"x":6.39479737309381,"y":-2.7418012262878593},"rotation":{"radians":2.1164593903065523}},"curvature":-4.075911440438974},{"time":4.361052702476709,"velocity":0.7644161375987443,"acceleration":3.0000000000000164,"pose":{"translation":{"x":6.388719158702311,"y":-2.7312482835150664},"rotation":{"radians":2.071850864957236}},"curvature":-3.292517826037708},{"time":4.394062291691346,"velocity":0.8634449052426559,"acceleration":3.000000000000013,"pose":{"translation":{"x":6.376733735372094,"y":-2.707202226929091},"rotation":{"radians":1.999295180651931}},"curvature":-2.220128264384656},{"time":4.427048764659495,"velocity":0.9624043241471033,"acceleration":2.99999999999999,"pose":{"translation":{"x":6.365039206185345,"y":-2.6794515367071394},"rotation":{"radians":1.9432042411349209}},"curvature":-1.5681810532119427},{"time":4.459789132404415,"velocity":1.0606254273818627,"acceleration":3.0000000000000075,"pose":{"translation":{"x":6.353714439426232,"y":-2.6483306468077847},"rotation":{"radians":1.8986262626650925}},"curvature":-1.160299877845927},{"time":4.52358829592402,"velocity":1.2520229179406772,"acceleration":2.9999999999999836,"pose":{"translation":{"x":6.332499906528562,"y":-2.5776742418651493},"rotation":{"radians":1.8315691084032764}},"curvature":-0.7265746327828503},{"time":4.583735724786862,"velocity":1.4324652045292052,"acceleration":1.5279486263191557,"pose":{"translation":{"x":6.3137385964647486,"y":-2.4991519232923034},"rotation":{"radians":1.78154648707069}},"curvature":-0.5414226677913919},{"time":4.640266930323582,"velocity":1.5188419823732027,"acceleration":-3.0,"pose":{"translation":{"x":6.298036706330595,"y":-2.417222523434155},"rotation":{"radians":1.7395970696539222}},"curvature":-0.4804143150401365},{"time":4.697249475366745,"velocity":1.3478943472437126,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.2859045767228015,"y":-2.336451623918702},"rotation":{"radians":1.6997834499194502}},"curvature":-0.5096799592935836},{"time":4.7575445403092,"velocity":1.1670091524163468,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":6.277703097929729,"y":-2.2610783837863337},"rotation":{"radians":1.6568769709629299}},"curvature":-0.6464570557332997},{"time":4.819579738139942,"velocity":0.9809035589241215,"acceleration":-3.0,"pose":{"translation":{"x":6.273590116122153,"y":-2.194582367619131},"rotation":{"radians":1.6047453360448525}},"curvature":-0.9688050763451119},{"time":4.881934875316774,"velocity":0.7938381473936263,"acceleration":-3.0,"pose":{"translation":{"x":6.273466839544032,"y":-2.139250373670167},"rotation":{"radians":1.5350878663689433}},"curvature":-1.6539869192725656},{"time":4.9130314508955655,"velocity":0.7005484206572529,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.274787104757643,"y":-2.116052761548466},"rotation":{"radians":1.4906625351913902}},"curvature":-2.2070158970322034},{"time":4.944272122980406,"velocity":0.606826404402731,"acceleration":-3.0000000000000013,"pose":{"translation":{"x":6.276924244703256,"y":-2.0957432619928063},"rotation":{"radians":1.4389161500512866}},"curvature":-2.889519609489578},{"time":4.976290130944924,"velocity":0.5107723805091781,"acceleration":-3.0,"pose":{"translation":{"x":6.279769865001267,"y":-2.078079362406115},"rotation":{"radians":1.3812845576470563}},"curvature":-3.531419002153032},{"time":5.01067971909592,"velocity":0.40760361605619033,"acceleration":-3.0,"pose":{"translation":{"x":6.283189482562416,"y":-2.0626627825700066},"rotation":{"radians":1.3233406589190928}},"curvature":-3.668111811549847},{"time":5.1465475911146505,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":6.29107228472956,"y":-2.036118403443619},"rotation":{"radians":1.257895098914613}},"curvature":6.632633907880963E-13}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24087146067360232,"velocity":0.6503529438187263,"acceleration":2.7,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.3403321067745373,"velocity":0.9188966882912508,"acceleration":2.700000000000002,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.4162093899230572,"velocity":1.1237653527922549,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.4796643300987847,"velocity":1.295093691266719,"acceleration":2.187682579886264,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5355873525736204,"velocity":1.4174355133495053,"acceleration":-0.734989108404782,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5891532438960738,"velocity":1.378065166645508,"acceleration":-0.6426464146474123,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.643461958301142,"velocity":1.3431638660489806,"acceleration":-0.5453266663691863,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6984118145503199,"velocity":1.3131982441231504,"acceleration":-0.4394478338280287,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.7539154354262596,"velocity":1.2888072981596066,"acceleration":-0.32468479529931726,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.809892141790966,"velocity":1.270632512712052,"acceleration":-0.20350244619471486,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.8662640818893885,"velocity":1.259160685005281,"acceleration":-0.08045877877688212,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.9229565411514642,"velocity":1.2545992789671963,"acceleration":0.03871626769718015,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.9799017050564615,"velocity":1.256803983177002,"acceleration":0.14815351255780274,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":1.037044049449856,"velocity":1.265269822214671,"acceleration":0.24282302918833878,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":1.0943450481757073,"velocity":1.2791838243007994,"acceleration":0.3192880318302778,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":1.151785273529354,"velocity":1.2975238008018528,"acceleration":0.37610415805722364,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":1.2093630070251655,"velocity":1.3191790257811382,"acceleration":0.4138164485760789,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.2670896494085861,"velocity":1.3430672599204667,"acceleration":0.43460983400489717,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.3249830335940764,"velocity":1.3682282940113044,"acceleration":0.4417529373871998,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.383060006914625,"velocity":1.3938839675702148,"acceleration":0.43900370786176246,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.441329456851559,"velocity":1.4194644721475942,"acceleration":0.43011410600385014,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.4997865390306198,"velocity":1.4446076877886345,"acceleration":0.41850596016098784,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.5584084380603251,"velocity":1.4691413019285218,"acceleration":0.4071259180970876,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.6171516690776433,"velocity":1.4930571937884367,"acceleration":0.3984462097492305,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.6759507421084023,"velocity":1.5164854615743109,"acceleration":0.39456346754290916,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.7347179434753464,"velocity":1.5396728523234446,"acceleration":0.397352070883421,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.7933440031024948,"velocity":1.562968038524027,"acceleration":0.4086404035647724,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.8516994822211263,"velocity":1.5868144450612802,"acceleration":0.43038917748303396,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.909636813019807,"velocity":1.6117500452092868,"acceleration":0.46485442754444356,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.9669930430836986,"velocity":1.6384123427017445,"acceleration":0.5147082657382434,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":2.0235934863647898,"velocity":1.6675450587029708,"acceleration":0.5830593842147167,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":2.079256669525349,"velocity":1.6999999999999975,"acceleration":4.265969051474203E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":2.1343686615338977,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1892768582496456,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.243795345773229,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.297757143445389,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.351013572738965,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4034336261508975,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4549033360942203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5053251437900577,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5546172681596233,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6027130747162124,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.649560444457203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6951211427560504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.739370188254281,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.782295221753494,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8238958751073535,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.864183140113587,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9031787374059825,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.940914485346383,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.01278040861083,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0802134292586643,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.143767239378293,"velocity":1.7,"acceleration":-0.9806847122121223,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2051845150767986,"velocity":1.6397690166567587,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.268443823133224,"velocity":1.4689688849044102,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.337848318037038,"velocity":1.2815767486641116,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.41794941674062,"velocity":1.0653037821644409,"acceleration":-2.699999999999997,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.464311638861809,"velocity":0.9401257824372306,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.246485948221108,"y":-1.1004457665706215},"rotation":{"radians":-0.02877542075414042}},"curvature":-1.2479062966462087},{"time":3.5152466704157184,"velocity":0.8026011972416758,"acceleration":-2.699999999999998,"pose":{"translation":{"x":7.290770527031534,"y":-1.103398777804336},"rotation":{"radians":-0.11545308104819059}},"curvature":-2.7518045032639415},{"time":3.5426121847539718,"velocity":0.7287143085283914,"acceleration":-2.7,"pose":{"translation":{"x":7.3114956505922,"y":-1.1064779763119066},"rotation":{"radians":-0.18293230201129468}},"curvature":-3.725227733364007},{"time":3.5716326709523103,"velocity":0.6503589957928775,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.331006934455969,"y":-1.1109206638015918},"rotation":{"radians":-0.26869329900096417}},"curvature":-4.883076044748539},{"time":3.5869706546331592,"velocity":0.6089464398545852,"acceleration":-2.7,"pose":{"translation":{"x":7.340252146464765,"y":-1.1137126796958767},"rotation":{"radians":-0.31891235544080615}},"curvature":-5.523919809501616},{"time":3.6030413007858946,"velocity":0.5655556952421994,"acceleration":-2.261497896342809,"pose":{"translation":{"x":7.349130295806822,"y":-1.1169134527391915},"rotation":{"radians":-0.37417386340026293}},"curvature":-6.1910282938294925},{"time":3.6199421735253385,"velocity":0.5273344070955894,"acceleration":-1.8200753979085174,"pose":{"translation":{"x":7.357622936902054,"y":-1.1205421754363483},"rotation":{"radians":-0.4344600800162828}},"curvature":-6.863459317284596},{"time":3.637660919757719,"velocity":0.49508495299624905,"acceleration":-1.404368604220338,"pose":{"translation":{"x":7.365712904636497,"y":-1.1246164469159217},"rotation":{"radians":-0.499598871724775}},"curvature":-7.511586808196944},{"time":3.6561465833657167,"velocity":0.4691242673969987,"acceleration":-1.013471060094217,"pose":{"translation":{"x":7.373384290445131,"y":-1.1291523046382135},"rotation":{"radians":-0.5692244956270529}},"curvature":-8.098065776430404},{"time":3.675310118112155,"velocity":0.44970257952237364,"acceleration":-0.6441793281202971,"pose":{"translation":{"x":7.380622418394695,"y":-1.1341642561032126},"rotation":{"radians":-0.6427505179428795}},"curvature":-8.58109966790429},{"time":3.695023152409384,"velocity":0.43700385033357203,"acceleration":-0.2912186052656897,"pose":{"translation":{"x":7.387413821266509,"y":-1.1396653105585597},"rotation":{"radians":-0.7193652922148932}},"curvature":-8.92014169080443},{"time":3.715122472825929,"velocity":0.43115055427507765,"acceleration":0.052067701331746194,"pose":{"translation":{"x":7.3937462166392915,"y":-1.1456670107075082},"rotation":{"radians":-0.7980584699587645}},"curvature":-9.08314266129781},{"time":3.7354206996909998,"velocity":0.43220743628905217,"acceleration":0.3926861267115369,"pose":{"translation":{"x":7.399608482971979,"y":-1.152179464416887},"rotation":{"radians":-0.8776808332180518}},"curvature":-9.053384284754978},{"time":3.755721886671196,"velocity":0.4401794307719521,"acceleration":0.7367582925439635,"pose":{"translation":{"x":7.404990635686545,"y":-1.1592113764250642},"rotation":{"radians":-0.957030545537142}},"curvature":-8.833522919030923},{"time":3.7758391314968773,"velocity":0.45500097772041015,"acceleration":1.0882423303428184,"pose":{"translation":{"x":7.409883803250819,"y":-1.1667700800499075},"rotation":{"radians":-1.0349502992109227}},"curvature":-8.445233665625674},{"time":3.7956105572131413,"velocity":0.4765170801160774,"acceleration":1.4478029542435038,"pose":{"translation":{"x":7.414280203261306,"y":-1.1748615688967483},"rotation":{"radians":-1.110416006752432}},"curvature":-7.924546693213447},{"time":3.8149106115079463,"velocity":0.5044597557411559,"acceleration":1.8120558532837037,"pose":{"translation":{"x":7.4181731185260045,"y":-1.1834905285663433},"rotation":{"radians":-1.1826006954592512}},"curvature":-7.314635079400883},{"time":3.833655213614978,"velocity":0.5384260217066767,"acceleration":2.1734101921253286,"pose":{"translation":{"x":7.421556873147227,"y":-1.192660368362838},"rotation":{"radians":-1.2509054956221384}},"curvature":-6.658491095790275},{"time":3.851801053675725,"velocity":0.5778643754393804,"acceleration":2.5206585401034607,"pose":{"translation":{"x":7.424426808604417,"y":-1.2023732530017275},"rotation":{"radians":-1.3149589717095316}},"curvature":-5.99341593717812},{"time":3.869340590588412,"velocity":0.6220755589478055,"acceleration":2.7000000000000313,"pose":{"translation":{"x":7.4267792598369695,"y":-1.2126301343178203},"rotation":{"radians":-1.3745927334979915}},"curvature":-5.3481080221595265},{"time":3.8863249147004435,"velocity":0.667933234050291,"acceleration":2.700000000000086,"pose":{"translation":{"x":7.428611531327051,"y":-1.2234307829732003},"rotation":{"radians":-1.4298037347140933}},"curvature":-4.742101270083783},{"time":3.90286706743035,"velocity":0.71259704642104,"acceleration":2.7000000000000104,"pose":{"translation":{"x":7.429921873182416,"y":-1.2347738201651899},"rotation":{"radians":-1.480712629900542}},"curvature":-4.186771633619186},{"time":3.919081127481485,"velocity":0.7563750085591059,"acceleration":2.7000000000000726,"pose":{"translation":{"x":7.430709457219227,"y":-1.2466567493343108},"rotation":{"radians":-1.527524747550707}},"curvature":-3.687079506807228},{"time":3.935049187697399,"velocity":0.7994887711420753,"acceleration":2.699999999999971,"pose":{"translation":{"x":7.430974353044876,"y":-1.2590759878722486},"rotation":{"radians":-1.5704972091395932}},"curvature":-3.2434293445605835},{"time":3.9664639427506723,"velocity":0.8843086097859121,"acceleration":2.699999999999964,"pose":{"translation":{"x":7.429940703945299,"y":-1.2855038226249045},"rotation":{"radians":-1.6460641038472028}},"curvature":-2.5125016951673764},{"time":3.997424393909201,"velocity":0.9679018279139381,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.426838529714478,"y":-1.3140081474824699},"rotation":{"radians":-1.7096996276847836}},"curvature":-1.9589426080510612},{"time":4.028085598335015,"velocity":1.0506870798636372,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.421698268143265,"y":-1.3445244380324612},"rotation":{"radians":-1.7635069388369267}},"curvature":-1.5430718610586651},{"time":4.058517591989222,"velocity":1.132853462729996,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.414562425632777,"y":-1.3769738370453977},"rotation":{"radians":-1.80928153691283}},"curvature":-1.2304194023042416},{"time":4.118736716721557,"velocity":1.2954450995073,"acceleration":2.3581795493155604,"pose":{"translation":{"x":7.394529774351022,"y":-1.4472909553859892},"rotation":{"radians":-1.8823557940783588}},"curvature":-0.8137689160789616},{"time":4.178392474069149,"velocity":1.4361240864833222,"acceleration":1.0280501369863269,"pose":{"translation":{"x":7.367292915991394,"y":-1.52408054521081},"rotation":{"radians":-1.9376153749830112}},"curvature":-0.5671040820963597},{"time":4.238964013252306,"velocity":1.4983946656380396,"acceleration":0.6806598794331884,"pose":{"translation":{"x":7.333548306053704,"y":-1.6062992620615995},"rotation":{"radians":-1.980664464363438}},"curvature":-0.415270222573429},{"time":4.301503977300484,"velocity":1.5409631100268286,"acceleration":0.4541105461405023,"pose":{"translation":{"x":7.294112024222084,"y":-1.692771843268838},"rotation":{"radians":-2.0151600783392727}},"curvature":-0.31853754024915065},{"time":4.365651597320475,"velocity":1.5700932207877196,"acceleration":0.3039509005891818,"pose":{"translation":{"x":7.249895283171679,"y":-1.7822235769053099},"rotation":{"radians":-2.0435542741178185}},"curvature":-0.2553649962385433},{"time":4.4308222574998855,"velocity":1.589901901641243,"acceleration":0.19979119065916273,"pose":{"translation":{"x":7.20187993737534,"y":-1.873312770739671},"rotation":{"radians":-2.0675482771424347}},"curvature":-0.21372950444359917},{"time":4.496291907487063,"velocity":1.6029821609642199,"acceleration":0.12148010098068647,"pose":{"translation":{"x":7.151093991910315,"y":-1.9646632211900141},"rotation":{"radians":-2.088370637372372}},"curvature":-0.18680044113633412},{"time":4.561259656850615,"velocity":1.6108744497173921,"acceleration":0.05463868552157288,"pose":{"translation":{"x":7.098587111264945,"y":-2.054896682277434},"rotation":{"radians":-2.106952696193974}},"curvature":-0.17076368611172438},{"time":4.624897589019494,"velocity":1.6143515426804107,"acceleration":-0.013181927539404118,"pose":{"translation":{"x":7.045406128145351,"y":-2.1426653345795916},"rotation":{"radians":-2.124045424613487}},"curvature":-0.1637481573893422},{"time":4.6863934918413115,"velocity":1.6135409081454433,"acceleration":-0.09636542124448583,"pose":{"translation":{"x":6.9925705522821335,"y":-2.226684254184283},"rotation":{"radians":-2.140304501136336}},"curvature":-0.16538102479256336},{"time":4.744990364133843,"velocity":1.6078941958633641,"acceleration":-0.6374308786638251,"pose":{"translation":{"x":6.941048079237058,"y":-2.3057638816430024},"rotation":{"radians":-2.156361587928427}},"curvature":-0.1768009201056777},{"time":4.800431081060775,"velocity":1.5725545709588773,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.891730099209751,"y":-2.37884249092451},"rotation":{"radians":-2.1728963784151825}},"curvature":-0.2011726549273176},{"time":4.854288620919112,"velocity":1.427139213341367,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.845407205844394,"y":-2.4450186583683924},"rotation":{"radians":-2.1907250385502053}},"curvature":-0.24500666492416812},{"time":4.907764200265851,"velocity":1.282755149105172,"acceleration":-2.7,"pose":{"translation":{"x":6.802744705036412,"y":-2.5035837316386345},"rotation":{"radians":-2.2109265322563663}},"curvature":-0.32107362952933843},{"time":5.010558252841902,"velocity":1.0052112071498347,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.7302887187706615,"y":-2.596204656657504},"rotation":{"radians":-2.26538823363029}},"curvature":-0.6983733281623699},{"time":5.058181507945903,"velocity":0.876628418369029,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.700978985620202,"y":-2.6300992809381722},"rotation":{"radians":-2.3055766625940555}},"curvature":-1.161706924289958},{"time":5.102107797041266,"velocity":0.7580274378115504,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.6762481672551255,"y":-2.6561252940163986},"rotation":{"radians":-2.3611253774593677}},"curvature":-2.069483430581481},{"time":5.1416578127195125,"velocity":0.6512423954802853,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.655767762927474,"y":-2.6750249344816397},"rotation":{"radians":-2.4393793407849826}},"curvature":-3.786562605724007},{"time":5.159738948600884,"velocity":0.6024233286005831,"acceleration":-2.7,"pose":{"translation":{"x":6.646945039992192,"y":-2.682139407006597},"rotation":{"radians":-2.4888731070782244}},"curvature":-5.0067041726191235},{"time":5.176792965437063,"velocity":0.5563774831429,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.638937036980688,"y":-2.68792802596912},"rotation":{"radians":-2.54463825964339}},"curvature":-6.289792037747411},{"time":5.1930309236619525,"velocity":0.5125349959356977,"acceleration":1.3760054548561336,"pose":{"translation":{"x":6.631622107145021,"y":-2.6925979064247128},"rotation":{"radians":-2.603460002905891}},"curvature":-7.150761360113481},{"time":5.207859374190757,"velocity":0.5329390247503961,"acceleration":2.7000000000000117,"pose":{"translation":{"x":6.624858527656298,"y":-2.6963844461134348},"rotation":{"radians":-2.658512232190288}},"curvature":-6.75882207866783},{"time":5.232432613506469,"velocity":0.5992867709028199,"acceleration":-1.2954847148587278,"pose":{"translation":{"x":6.6123135559006245,"y":-2.7023965955020928},"rotation":{"radians":-2.714965160462928}},"curvature":-2.782056460928346E-13},{"time":5.255485128727874,"velocity":0.5694225897944417,"acceleration":-2.7,"pose":{"translation":{"x":6.600000781924867,"y":-2.7078609588171707},"rotation":{"radians":-2.741416782477967}},"curvature":-3.815339203835633},{"time":5.280595583209917,"velocity":0.5016243626929267,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.587449437541447,"y":-2.7126871333494806},"rotation":{"radians":-2.814639860740997}},"curvature":-6.953564103889301},{"time":5.294534391040094,"velocity":0.463989581551448,"acceleration":-2.51320967641981,"pose":{"translation":{"x":6.581024377505649,"y":-2.7146891790782193},"rotation":{"radians":-2.8658404550015253}},"curvature":-8.22183756317215},{"time":5.3097153265706005,"velocity":0.42583670747907426,"acceleration":-1.420637008833131,"pose":{"translation":{"x":6.574474775176925,"y":-2.716338996269565},"rotation":{"radians":-2.924962178570092}},"curvature":-9.235001787363444},{"time":5.32614333242187,"velocity":0.4024984743854337,"acceleration":-0.6675417501345704,"pose":{"translation":{"x":6.567786061057784,"y":-2.7175859150618242},"rotation":{"radians":-2.990422599769865}},"curvature":-9.949439794600845},{"time":5.343500843112221,"velocity":0.39091161132121716,"acceleration":-0.08934168753605815,"pose":{"translation":{"x":6.560946817537351,"y":-2.7183853869336816},"rotation":{"radians":-3.0604727521050985}},"curvature":-10.335830771816653},{"time":5.361457822012166,"velocity":0.3893073045232467,"acceleration":0.3993183425367512,"pose":{"translation":{"x":6.553948643263447,"y":-2.718698765306084},"rotation":{"radians":-3.133275977941982}},"curvature":-10.391142880043173},{"time":5.37969325023703,"velocity":0.39658904549744733,"acceleration":0.8452012281683092,"pose":{"translation":{"x":6.546786017514653,"y":-2.7184930861441257},"rotation":{"radians":3.0761577385964625}},"curvature":-10.143682388705306},{"time":5.39791861238858,"velocity":0.4119931439717496,"acceleration":1.273109766770183,"pose":{"translation":{"x":6.539456164572393,"y":-2.717740848558933},"rotation":{"radians":3.00310010912821}},"curvature":-9.649019311906114},{"time":5.415897054585348,"velocity":0.4348816743237688,"acceleration":1.6931105855795832,"pose":{"translation":{"x":6.531958918093006,"y":-2.716419795409549},"rotation":{"radians":2.9321083621384485}},"curvature":-8.978732425160104},{"time":5.4334539084799225,"velocity":0.4646073695021467,"acceleration":2.104166578935371,"pose":{"translation":{"x":6.52429658547982,"y":-2.7145126939048203},"rotation":{"radians":2.8642215501788977}},"curvature":-8.20680098482241},{"time":5.450477610009569,"velocity":0.5004280733106014,"acceleration":2.4968026150305582,"pose":{"translation":{"x":6.5164738122552235,"y":-2.7120071162052803},"rotation":{"radians":2.800134866123928}},"curvature":-7.398430838000034},{"time":5.466912924524007,"velocity":0.5414638095690992,"acceleration":2.7000000000000237,"pose":{"translation":{"x":6.508497446432745,"y":-2.7088952200250334},"rotation":{"radians":2.740230748578606}},"curvature":-6.603818980361795},{"time":5.482783228379204,"velocity":0.5843136299781293,"acceleration":2.699999999999973,"pose":{"translation":{"x":6.500376402889126,"y":-2.7051735292336434},"rotation":{"radians":2.6846350981540272}},"curvature":-5.856777204223596},{"time":5.4981886016609645,"velocity":0.6259081378388831,"acceleration":2.699999999999989,"pose":{"translation":{"x":6.49212152773639,"y":-2.700842714458014},"rotation":{"radians":2.633281828218556}},"curvature":-5.176639926128518},{"time":5.513232961900598,"velocity":0.666527910485892,"acceleration":2.7000000000000526,"pose":{"translation":{"x":6.483745462693925,"y":-2.6959073736842782},"rotation":{"radians":2.5859736858732676}},"curvature":-4.571644906700683},{"time":5.5424876306213875,"velocity":0.7455155160320258,"acceleration":2.699999999999983,"pose":{"translation":{"x":6.466688494086605,"y":-2.684259826494461},"rotation":{"radians":2.502339906825371}},"curvature":-3.5851423375100553},{"time":5.57086869677463,"velocity":0.8221443946457809,"acceleration":2.7000000000000477,"pose":{"translation":{"x":6.4493373891083,"y":-2.670337881609422},"rotation":{"radians":2.4311633245994693}},"curvature":-2.859842251329955},{"time":5.59848508337457,"velocity":0.8967086384656198,"acceleration":2.700000000000045,"pose":{"translation":{"x":6.4318440893306725,"y":-2.6542973292426666},"rotation":{"radians":2.3698700235605896}},"curvature":-2.3389424471387197},{"time":5.6253401968092005,"velocity":0.9692174447391245,"acceleration":2.7000000000000273,"pose":{"translation":{"x":6.414376245762293,"y":-2.636335735136296},"rotation":{"radians":2.316179665573082}},"curvature":-1.971245985355279},{"time":5.651382693812427,"velocity":1.0395321866478364,"acceleration":2.3397973634705633,"pose":{"translation":{"x":6.397112878755023,"y":-2.6166854198213247},"rotation":{"radians":2.2681655710867354}},"curvature":-1.7177072822338848},{"time":5.701076468298373,"velocity":1.1558055491709527,"acceleration":0.13185620101355705,"pose":{"translation":{"x":6.363946461985967,"y":-2.5733795571961275},"rotation":{"radians":2.1829634101425417}},"curvature":-1.4531964340918389},{"time":5.749028266621835,"velocity":1.1621282911296527,"acceleration":-1.2418908296492508,"pose":{"translation":{"x":6.333839465146571,"y":-2.5266666132856277},"rotation":{"radians":2.1038693326688813}},"curvature":-1.428497937409373},{"time":5.79687438905331,"velocity":1.1027086304477278,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.3081906578607985,"y":-2.4789402092914683},"rotation":{"radians":2.0222056755935585}},"curvature":-1.6267050425406355},{"time":5.821052508603361,"velocity":1.0374277076625913,"acceleration":-2.699999999999994,"pose":{"translation":{"x":6.297414801809275,"y":-2.4554188673206423},"rotation":{"radians":1.977719022211628}},"curvature":-1.826133388334632},{"time":5.845687364250436,"velocity":0.9709135974154887,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.288164070257655,"y":-2.432476052028613},"rotation":{"radians":1.929238799528292}},"curvature":-2.111200891597752},{"time":5.870673275595948,"velocity":0.9034516367826055,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.280524503434468,"y":-2.4103409437610113},"rotation":{"radians":1.875500722191483}},"curvature":-2.501078261352885},{"time":5.8959358064369916,"velocity":0.8352428035117866,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.274550109975329,"y":-2.3892072702555778},"rotation":{"radians":1.8152269082626638}},"curvature":-3.014788560841303},{"time":5.921456442251981,"velocity":0.7663370868113146,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.270258526829311,"y":-2.369226285902487},"rotation":{"radians":1.7473051168809193}},"curvature":-3.6604764302923014},{"time":5.947310546313835,"velocity":0.6965310058443089,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.267626679165335,"y":-2.350499751004661},"rotation":{"radians":1.6711659569549777}},"curvature":-4.412655527221165},{"time":5.97372707891214,"velocity":0.6252063678288854,"acceleration":-2.7,"pose":{"translation":{"x":6.266586440278536,"y":-2.333072911038095},"rotation":{"radians":1.5874406145062498}},"curvature":-5.1745095089891295},{"time":5.98728642428764,"velocity":0.5885961353150362,"acceleration":-2.7,"pose":{"translation":{"x":6.266628497589658,"y":-2.3248438348321385},"rotation":{"radians":1.5434847754496874}},"curvature":-5.498692515200691},{"time":6.001196215504899,"velocity":0.5510396990284375,"acceleration":-2.699999999999995,"pose":{"translation":{"x":6.267020291496656,"y":-2.316927475912173},"rotation":{"radians":1.4988894184050614}},"curvature":-5.736779218810578},{"time":6.015587967438983,"velocity":0.5121819688064083,"acceleration":-2.700000000000005,"pose":{"translation":{"x":6.267738702623978,"y":-2.309310468744486},"rotation":{"radians":1.4544878154614598}},"curvature":-5.844205776954959},{"time":6.030645697244969,"velocity":0.4715260983302458,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.268756982086413,"y":-2.3019745992299896},"rotation":{"radians":1.4113367378835124}},"curvature":-5.7736468362624205},{"time":6.063952573576684,"velocity":0.3815975322346138,"acceleration":-2.7,"pose":{"translation":{"x":6.271567189159886,"y":-2.2880478575486682},"rotation":{"radians":1.3340215256225776}},"curvature":-4.927882985766305},{"time":6.205284992922838,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.279174459871376,"y":-2.2621770757491753},"rotation":{"radians":1.257895098914576}},"curvature":4.89472983251582E-13}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid0.wpilib.json b/src/main/deploy/paths/SixBallMid0.wpilib.json index 2b339ea..24f8dfa 100644 --- a/src/main/deploy/paths/SixBallMid0.wpilib.json +++ b/src/main/deploy/paths/SixBallMid0.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22844018981162115,"velocity":0.6853205694348635,"acceleration":2.9999999999999996,"pose":{"translation":{"x":3.2065095854674475,"y":-2.321993759274483},"rotation":{"radians":1.4808013128224615}},"curvature":-0.25415239252139715},{"time":0.3224986666860846,"velocity":0.9674960000582539,"acceleration":2.999999999999999,"pose":{"translation":{"x":3.2144920400116974,"y":-2.244674015045166},"rotation":{"radians":1.4521039763788177}},"curvature":-0.48102289742329407},{"time":0.39393292568025073,"velocity":1.181798777040752,"acceleration":3.000000000000001,"pose":{"translation":{"x":3.2251942805954354,"y":-2.168657049536705},"rotation":{"radians":1.407083956538682}},"curvature":-0.6895659762146151},{"time":0.45338587226770455,"velocity":1.3601576168031135,"acceleration":2.999999999999999,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0944854736328122},"rotation":{"radians":1.3476355522918868}},"curvature":-0.8811082747650646},{"time":0.5050764160290824,"velocity":1.515229248087247,"acceleration":2.999999999999998,"pose":{"translation":{"x":3.2585973431481516,"y":-2.0226314455270766},"rotation":{"radians":1.2757042704692263}},"curvature":-1.0503191132212641},{"time":0.5512787765713811,"velocity":1.6538363297141432,"acceleration":2.999999999999997,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9534998893737792},"rotation":{"radians":1.1935567050390803}},"curvature":-1.1871900193146532},{"time":0.5728003854706883,"velocity":1.7184011564120647,"acceleration":2.339470754230812,"pose":{"translation":{"x":3.29679072395957,"y":-1.9200639848597347},"rotation":{"radians":1.1494812806441845}},"curvature":-1.239928580956994},{"time":0.5935363700192974,"velocity":1.7669123858237175,"acceleration":-0.5436899956399454,"pose":{"translation":{"x":3.3123124503261456,"y":-1.887431713938713},"rotation":{"radians":1.1039001836454154}},"curvature":-1.2805479837540814},{"time":0.613997248730107,"velocity":1.755788010766648,"acceleration":-0.27821667617747475,"pose":{"translation":{"x":3.3292782820223583,"y":-1.8556362158618867},"rotation":{"radians":1.0572084270862476}},"curvature":-1.308216350703189},{"time":0.6345348059123607,"velocity":1.7500741198705965,"acceleration":-0.022743667612852538,"pose":{"translation":{"x":3.347702503129696,"y":-1.82470703125},"rotation":{"radians":1.009808701952868}},"curvature":-1.3225645836950348},{"time":0.6551218107400116,"velocity":1.7496058958756522,"acceleration":0.21836662236809756,"pose":{"translation":{"x":3.3675900610791567,"y":-1.7946702026762067},"rotation":{"radians":0.96210010066623}},"curvature":-1.3237445021353418},{"time":0.6757329851623971,"velocity":1.7541066884173082,"acceleration":0.44094172089548617,"pose":{"translation":{"x":3.388937040133019,"y":-1.765548375248909},"rotation":{"radians":0.9144663557730784}},"curvature":-1.312428637189129},{"time":0.6963457035139206,"velocity":1.763195695919563,"acceleration":0.6413101666108914,"pose":{"translation":{"x":3.411731134866622,"y":-1.737360897194594},"rotation":{"radians":0.8672647742046862}},"curvature":-1.2897532381321657},{"time":0.7169405333077539,"velocity":1.7764033696459691,"acceleration":0.8165558577958478,"pose":{"translation":{"x":3.435952123650134,"y":-1.7101239204406737},"rotation":{"radians":0.8208169033196313}},"curvature":-1.2572161094450898},{"time":0.7375015438150363,"velocity":1.7931925832178925,"acceleration":0.9647011548897362,"pose":{"translation":{"x":3.461572342130331,"y":-1.6838505011983216},"rotation":{"radians":0.7754016677331865}},"curvature":-1.2165477334674017},{"time":0.7580163507583728,"velocity":1.8129832411684692,"acceleration":1.1313619301261402,"pose":{"translation":{"x":3.4885571567123694,"y":-1.6585507005453108},"rotation":{"radians":0.7312513370847253}},"curvature":-1.1695762249806743},{"time":0.7988546318429788,"velocity":1.859186117679383,"acceleration":1.2628229077088666,"pose":{"translation":{"x":3.5464500344851415,"y":-1.6108978271484373},"rotation":{"radians":0.6474363689665855}},"curvature":-1.0638099142624002},{"time":0.83944138598433,"velocity":1.910440000558629,"acceleration":1.300058189471748,"pose":{"translation":{"x":3.6092322956847354,"y":-1.567189708352089},"rotation":{"radians":0.5703086691540483}},"curvature":-0.9524662524724097},{"time":0.8797609609692681,"velocity":1.9628577942138181,"acceleration":1.265808259413599,"pose":{"translation":{"x":3.676424273995083,"y":-1.527409267425537},"rotation":{"radians":0.5001906802940327}},"curvature":-0.8446084678380373},{"time":0.9198040484146826,"velocity":2.0135446650346447,"acceleration":1.1846535633197064,"pose":{"translation":{"x":3.7474802494845436,"y":-1.491501161456108},"rotation":{"radians":0.4369591767127109}},"curvature":-0.7456528682493916},{"time":0.9595516846457711,"velocity":2.060631843929339,"acceleration":1.078307049602526,"pose":{"translation":{"x":3.8218036000226827,"y":-1.459375},"rotation":{"radians":0.3802110136115068}},"curvature":-0.6580867707002909},{"time":0.9989652615913103,"velocity":2.1031317817997657,"acceleration":0.9634000918400376,"pose":{"translation":{"x":3.8987619526970523,"y":-1.430908563733101},"rotation":{"radians":0.3293970719543265}},"curvature":-0.5824180484673191},{"time":1.0379824885977187,"velocity":2.140720981881083,"acceleration":0.8513310740584539,"pose":{"translation":{"x":3.9777023352299703,"y":-1.4059510231018066},"rotation":{"radians":0.2839173004495175}},"curvature":-0.5179964450188761},{"time":1.0765180844428441,"velocity":2.1735275320813963,"acceleration":0.7492837612540003,"pose":{"translation":{"x":4.057966327395298,"y":-1.384326156973839},"rotation":{"radians":0.2431817004482161}},"curvature":-0.4635925052478776},{"time":1.1144677175905173,"velocity":2.2019625759444943,"acceleration":-0.6986847648307831,"pose":{"translation":{"x":4.138905212435224,"y":-1.3658355712890626},"rotation":{"radians":0.2066461302891754}},"curvature":-0.4177494217263637},{"time":1.152147581490706,"velocity":2.175636229096535,"acceleration":-2.9999999999999973,"pose":{"translation":{"x":4.219895128477039,"y":-1.3502619177103043},"rotation":{"radians":0.17383156520200765}},"curvature":-0.37896519682071883},{"time":1.1906206177395262,"velocity":2.060217120350075,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":4.300352219949918,"y":-1.3373721122741702},"rotation":{"radians":0.14433362331149865}},"curvature":-0.3457618351949617},{"time":1.2306576488017948,"velocity":1.940106027163269,"acceleration":-3.0,"pose":{"translation":{"x":4.3797477890017005,"y":-1.326920554041863},"rotation":{"radians":0.11782713826751266}},"curvature":-0.3166843715937036},{"time":1.2723682645918366,"velocity":1.814974179793144,"acceleration":-3.000000000000003,"pose":{"translation":{"x":4.457623446915669,"y":-1.3186523437500004},"rotation":{"radians":0.09406880478221523}},"curvature":-0.29025966227029737},{"time":1.315948027173534,"velocity":1.6842348920480514,"acceleration":-2.999999999999997,"pose":{"translation":{"x":4.5336062655273235,"y":-1.312306502461434},"rotation":{"radians":0.07289944611301012}},"curvature":-0.26493890076434345},{"time":1.3617317581047963,"velocity":1.546883699254265,"acceleration":-3.0,"pose":{"translation":{"x":4.607423928641174,"y":-1.307619190216065},"rotation":{"radians":0.05424603147602765}},"curvature":-0.2390516451714226},{"time":1.4102861588151152,"velocity":1.4012204971233082,"acceleration":-3.0,"pose":{"translation":{"x":4.678919883447504,"y":-1.3043269246816642},"rotation":{"radians":0.03812190138197012}},"curvature":-0.2108146530013771},{"time":1.4625872434216978,"velocity":1.2443172433035607,"acceleration":-3.0,"pose":{"translation":{"x":4.748068491939165,"y":-1.3021697998046884},"rotation":{"radians":0.02462148220789151}},"curvature":-0.1784645562722071},{"time":1.5204092440874486,"velocity":1.0708512413063085,"acceleration":-3.000000000000001,"pose":{"translation":{"x":4.814990182328344,"y":-1.3008947044610988},"rotation":{"radians":0.013903147168922325}},"curvature":-0.14060587812306488},{"time":1.58737015857728,"velocity":0.8699684978368145,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":4.87996660046335,"y":-1.3002585411071794},"rotation":{"radians":0.006151724781559076}},"curvature":-0.09684470081187932},{"time":1.8773596578562182,"velocity":0.0,"acceleration":-2.9999999999999996,"pose":{"translation":{"x":5.006107200045358,"y":-1.300000000000002},"rotation":{"radians":-5.329070518200854E-15}},"curvature":-1.06581410364021E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24079710297530457,"velocity":0.6501521780333225,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.2065095854674475,"y":-2.321993759274483},"rotation":{"radians":1.4808013128224615}},"curvature":-0.25415239252139715},{"time":0.3399434430318312,"velocity":0.9178472961859444,"acceleration":2.7000000000000024,"pose":{"translation":{"x":3.2144920400116974,"y":-2.244674015045166},"rotation":{"radians":1.4521039763788177}},"curvature":-0.48102289742329407},{"time":0.4152417634944757,"velocity":1.1211527614350847,"acceleration":2.7000000000000024,"pose":{"translation":{"x":3.2251942805954354,"y":-2.168657049536705},"rotation":{"radians":1.407083956538682}},"curvature":-0.6895659762146151},{"time":0.4779106717693721,"velocity":1.290358813777305,"acceleration":-0.37855383338541415,"pose":{"translation":{"x":3.2396305049811973,"y":-2.0944854736328122},"rotation":{"radians":1.3476355522918868}},"curvature":-0.8811082747650646},{"time":0.5359982410296074,"velocity":1.2683695417618022,"acceleration":-0.6925189022102917,"pose":{"translation":{"x":3.2585973431481516,"y":-2.0226314455270766},"rotation":{"radians":1.2757042704692263}},"curvature":-1.0503191132212641},{"time":0.5946566732242955,"velocity":1.22774746869296,"acceleration":-0.5032767383908354,"pose":{"translation":{"x":3.2826890087088776,"y":-1.9534998893737792},"rotation":{"radians":1.1935567050390803}},"curvature":-1.1871900193146532},{"time":0.6243944835549278,"velocity":1.212781120502874,"acceleration":-0.3768403323138947,"pose":{"translation":{"x":3.29679072395957,"y":-1.9200639848597347},"rotation":{"radians":1.1494812806441845}},"curvature":-1.239928580956994},{"time":0.6543294378988004,"velocity":1.2015004223601278,"acceleration":-0.2514022539839045,"pose":{"translation":{"x":3.3123124503261456,"y":-1.887431713938713},"rotation":{"radians":1.1039001836454154}},"curvature":-1.2805479837540814},{"time":0.684418965414697,"velocity":1.1939358473213206,"acceleration":-0.1286473910644702,"pose":{"translation":{"x":3.3292782820223583,"y":-1.8556362158618867},"rotation":{"radians":1.0572084270862476}},"curvature":-1.308216350703189},{"time":0.7146212553885996,"velocity":1.1900504015120055,"acceleration":-0.010516671904178118,"pose":{"translation":{"x":3.347702503129696,"y":-1.82470703125},"rotation":{"radians":1.009808701952868}},"curvature":-1.3225645836950348},{"time":0.7448962624880843,"velocity":1.1897320091954435,"acceleration":0.10097272618299795,"pose":{"translation":{"x":3.3675900610791567,"y":-1.7946702026762067},"rotation":{"radians":0.96210010066623}},"curvature":-1.3237445021353418},{"time":0.7752068131092396,"velocity":1.1927925481237693,"acceleration":0.20389145174208068,"pose":{"translation":{"x":3.388937040133019,"y":-1.765548375248909},"rotation":{"radians":0.9144663557730784}},"curvature":-1.312428637189129},{"time":0.8055196342144211,"velocity":1.1989730732253028,"acceleration":0.29654182104087534,"pose":{"translation":{"x":3.411731134866622,"y":-1.737360897194594},"rotation":{"radians":0.8672647742046862}},"curvature":-1.2897532381321657},{"time":0.835806148617117,"velocity":1.207954291359259,"acceleration":0.3775754286448021,"pose":{"translation":{"x":3.435952123650134,"y":-1.7101239204406737},"rotation":{"radians":0.8208169033196313}},"curvature":-1.2572161094450898},{"time":0.8660429287748854,"velocity":1.219370956588167,"acceleration":0.44607781402101293,"pose":{"translation":{"x":3.461572342130331,"y":-1.6838505011983216},"rotation":{"radians":0.7754016677331865}},"curvature":-1.2165477334674017},{"time":0.8962117625150862,"velocity":1.232828603994559,"acceleration":0.5231417564903263,"pose":{"translation":{"x":3.4885571567123694,"y":-1.6585507005453108},"rotation":{"radians":0.7312513370847253}},"curvature":-1.1695762249806743},{"time":0.9562680582277421,"velocity":1.2642465600219803,"acceleration":0.5839293125245819,"pose":{"translation":{"x":3.5464500344851415,"y":-1.6108978271484373},"rotation":{"radians":0.6474363689665855}},"curvature":-1.0638099142624002},{"time":1.0159544613767881,"velocity":1.2990992003798678,"acceleration":0.6011469068117358,"pose":{"translation":{"x":3.6092322956847354,"y":-1.567189708352089},"rotation":{"radians":0.5703086691540483}},"curvature":-0.9524662524724097},{"time":1.075247954001697,"velocity":1.3347433000653963,"acceleration":0.5853097391528487,"pose":{"translation":{"x":3.676424273995083,"y":-1.527409267425537},"rotation":{"radians":0.5001906802940327}},"curvature":-0.8446084678380373},{"time":1.1341348473037771,"velocity":1.3692103722235585,"acceleration":0.5477838076790317,"pose":{"translation":{"x":3.7474802494845436,"y":-1.491501161456108},"rotation":{"radians":0.4369591767127109}},"curvature":-0.7456528682493916},{"time":1.192587253525966,"velocity":1.4012296538719506,"acceleration":0.49860917973620345,"pose":{"translation":{"x":3.8218036000226827,"y":-1.459375},"rotation":{"radians":0.3802110136115068}},"curvature":-0.6580867707002909},{"time":1.2505483960929356,"velocity":1.4301296116238404,"acceleration":0.44547620246683517,"pose":{"translation":{"x":3.8987619526970523,"y":-1.430908563733101},"rotation":{"radians":0.3293970719543265}},"curvature":-0.5824180484673191},{"time":1.3079266711023596,"velocity":1.4556902676791363,"acceleration":0.39365548864463024,"pose":{"translation":{"x":3.9777023352299703,"y":-1.4059510231018066},"rotation":{"radians":0.2839173004495175}},"curvature":-0.5179964450188761},{"time":1.36459666499225,"velocity":1.4779987218153494,"acceleration":0.3464688112038548,"pose":{"translation":{"x":4.057966327395298,"y":-1.384326156973839},"rotation":{"radians":0.2431817004482161}},"curvature":-0.4635925052478776},{"time":1.4204049490329456,"velocity":1.4973345516422563,"acceleration":0.30594936186666,"pose":{"translation":{"x":4.138905212435224,"y":-1.3658355712890626},"rotation":{"radians":0.2066461302891754}},"curvature":-0.4177494217263637},{"time":1.4751787559057503,"velocity":1.5140925629019986,"acceleration":0.27349218095525435,"pose":{"translation":{"x":4.219895128477039,"y":-1.3502619177103043},"rotation":{"radians":0.17383156520200765}},"curvature":-0.37896519682071883},{"time":1.5287361342444172,"velocity":1.5287400871100862,"acceleration":0.2504223497687081,"pose":{"translation":{"x":4.300352219949918,"y":-1.3373721122741702},"rotation":{"radians":0.14433362331149865}},"curvature":-0.3457618351949617},{"time":1.5808966468770733,"velocity":1.5418022452486964,"acceleration":0.23847228579086874,"pose":{"translation":{"x":4.3797477890017005,"y":-1.326920554041863},"rotation":{"radians":0.11782713826751266}},"curvature":-0.3166843715937036},{"time":1.6314920601476874,"velocity":1.5538678491018734,"acceleration":0.2401578562175825,"pose":{"translation":{"x":4.457623446915669,"y":-1.3186523437500004},"rotation":{"radians":0.09406880478221523}},"curvature":-0.29025966227029737},{"time":1.6803767808178705,"velocity":1.5656078988198199,"acceleration":-2.0114815977744067,"pose":{"translation":{"x":4.5336062655273235,"y":-1.312306502461434},"rotation":{"radians":0.07289944611301012}},"curvature":-0.26493890076434345},{"time":1.7291493718369744,"velocity":1.4675027295091152,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":4.607423928641174,"y":-1.307619190216065},"rotation":{"radians":0.05424603147602765}},"curvature":-0.2390516451714226},{"time":1.7803302040600095,"velocity":1.3293144825069205,"acceleration":-2.700000000000001,"pose":{"translation":{"x":4.678919883447504,"y":-1.3043269246816642},"rotation":{"radians":0.03812190138197012}},"curvature":-0.2108146530013771},{"time":1.8354603878780003,"velocity":1.1804629861983456,"acceleration":-2.700000000000001,"pose":{"translation":{"x":4.748068491939165,"y":-1.3021697998046884},"rotation":{"radians":0.02462148220789151}},"curvature":-0.1784645562722071},{"time":1.8964101282018484,"velocity":1.0158986873239553,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":4.814990182328344,"y":-1.3008947044610988},"rotation":{"radians":0.013903147168922325}},"curvature":-0.14060587812306488},{"time":1.966993129533728,"velocity":0.8253245837278805,"acceleration":-2.7,"pose":{"translation":{"x":4.87996660046335,"y":-1.3002585411071794},"rotation":{"radians":0.006151724781559076}},"curvature":-0.09684470081187932},{"time":2.2726689012847947,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.006107200045358,"y":-1.300000000000002},"rotation":{"radians":-5.329070518200854E-15}},"curvature":-1.06581410364021E-14}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMid1.wpilib.json b/src/main/deploy/paths/SixBallMid1.wpilib.json index 3971ad8..758d46b 100644 --- a/src/main/deploy/paths/SixBallMid1.wpilib.json +++ b/src/main/deploy/paths/SixBallMid1.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2519761297876903,"velocity":0.7559283893630709,"acceleration":2.9999999999999987,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3562702409671562,"velocity":1.0688107229014685,"acceleration":3.0000000000000013,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.4361595761278285,"velocity":1.3084787283834856,"acceleration":3.0,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.503308309644587,"velocity":1.5099249289337608,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.562202759020208,"velocity":1.686608277060624,"acceleration":3.000000000000002,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6151186089000533,"velocity":1.8453558267001602,"acceleration":3.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6633872740545509,"velocity":1.990161822163653,"acceleration":3.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7078595229045569,"velocity":2.123578568713671,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7491126566722462,"velocity":2.2473379700167393,"acceleration":3.000000000000005,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7875559872701902,"velocity":2.3626679618105713,"acceleration":3.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.823489794243851,"velocity":2.470469382731554,"acceleration":0.8651265291285504,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8576242431067156,"velocity":2.5,"acceleration":-1.1732826408603372,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8909260810512718,"velocity":2.460927531630908,"acceleration":-2.9999999999999947,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9241733364212149,"velocity":2.361185765521079,"acceleration":-3.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9577172640528673,"velocity":2.2605539826261216,"acceleration":-3.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9915130896437078,"velocity":2.1591665058536003,"acceleration":-3.000000000000006,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.025517145389847,"velocity":2.0571543386151823,"acceleration":-3.0000000000000098,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0596888686194887,"velocity":1.9546391689262566,"acceleration":-3.0000000000000036,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0939935025170637,"velocity":1.8517252672335311,"acceleration":-2.9999999999999964,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1284057930259193,"velocity":1.7484883957069648,"acceleration":-3.0,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1975327505604134,"velocity":1.5411075231034823,"acceleration":-2.999999999999999,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.267315103090996,"velocity":1.3317604655117345,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3388330522864487,"velocity":1.1172066179253766,"acceleration":-3.0,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4151734608431514,"velocity":0.8881853922552679,"acceleration":-3.0,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.505709141675879,"velocity":0.6165783497570856,"acceleration":-3.0,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7112352582615742,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2656061620411004,"velocity":0.717136637510971,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3755418079977478,"velocity":1.013962881593919,"acceleration":2.699999999999999,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45975256128584724,"velocity":1.2413319154717875,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5305335412553954,"velocity":1.4324405613895677,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5926137417782101,"velocity":1.6000571028011674,"acceleration":1.7647018169830884,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6492481806601815,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7036997445093501,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7575077699115317,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.810541300693782,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8626660013101702,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9137463026089846,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9636475475999504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0122381372214386,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0593916761076771,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1049891183559657,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1489209132938847,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1910891512465098,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2314097093036216,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2698143970869191,"velocity":1.7,"acceleration":-1.1179894261480032,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3067004902763237,"velocity":1.6587617378423356,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3795667014519033,"velocity":1.4620229676682703,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4531237596123885,"velocity":1.26341891063496,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.528510297293001,"velocity":1.0598752588973066,"acceleration":-2.7,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6089801534753305,"velocity":0.8426066472050169,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7044131404571534,"velocity":0.5849375823540952,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9210566894771886,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/deploy/paths/SixBallMidComplete.wpilib.json b/src/main/deploy/paths/SixBallMidComplete.wpilib.json index eec2b0a..4300f3a 100644 --- a/src/main/deploy/paths/SixBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/SixBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.22851073171807768,"velocity":0.6855321951542331,"acceleration":3.0,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.32286738548734767,"velocity":0.9686021564620431,"acceleration":3.0000000000000004,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.3948508967117983,"velocity":1.1845526901353949,"acceleration":3.000000000000002,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.45504953863530545,"velocity":1.3651486159059165,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5075769496703644,"velocity":1.5227308490110931,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5545708746789335,"velocity":1.6637126240368005,"acceleration":2.9999999999999987,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.5973365928558361,"velocity":1.7920097785675082,"acceleration":3.0000000000000013,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6367625752176861,"velocity":1.9102877256530584,"acceleration":-0.39480980187976056,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.6747121786647302,"velocity":1.8953048502347154,"acceleration":-0.7021730002147804,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.7127763389927304,"velocity":1.868577224576547,"acceleration":-0.4401004459228274,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.7511092582596577,"velocity":1.8517068897136486,"acceleration":-0.17400254925795713,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.7896601305578692,"velocity":1.8449989396576418,"acceleration":0.08372895263229682,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.8283828420132677,"velocity":1.848241151730885,"acceleration":0.32040119497795183,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":0.8672396362007758,"velocity":1.860690915021575,"acceleration":0.5251363087983144,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":0.906204315334355,"velocity":1.8811526827952934,"acceleration":0.6905017989409158,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":0.9452636685748346,"velocity":1.9081232364733132,"acceleration":0.8133740442414022,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":0.9844165273519864,"velocity":1.9399691555604976,"acceleration":0.8949317659517233,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.0236706441727126,"velocity":1.9750989116477453,"acceleration":0.9399001600451896,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.063038145418846,"velocity":2.0121004323695653,"acceleration":0.9553480479826941,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.1025304872768191,"velocity":2.049829364073845,"acceleration":0.9494024824000104,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.1421537132339343,"velocity":2.087447753158227,"acceleration":0.9301775648872205,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.1819045291156953,"velocity":2.1244230702774036,"acceleration":0.9050734432547365,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.221767420455895,"velocity":2.1605019146007676,"acceleration":0.8804626256424855,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.2617128175476713,"velocity":2.1956723438065247,"acceleration":0.8616916300805181,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.3016961872085873,"velocity":2.2301256787857513,"acceleration":0.8532946962433116,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.341657884138109,"velocity":2.264224782828595,"acceleration":0.8593254128101726,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.3815236046845703,"velocity":2.2984824095941576,"acceleration":0.8837378969826192,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.42120533048524,"velocity":2.3335506545018823,"acceleration":0.9307724426536456,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.4606027154283423,"velocity":2.3702206547195397,"acceleration":1.0053080180459415,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.4996049518717887,"velocity":2.40942991573786,"acceleration":1.113123412063664,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":1.5380932533029306,"velocity":2.4522721451514276,"acceleration":1.2609415748588182,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":1.575944217852111,"velocity":2.4999999999999964,"acceleration":9.47993122549823E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":1.6134203724179241,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6507579461846327,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6878305177006696,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7245245401177383,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.76073891203737,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.796384548357484,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8313839511189436,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.865670780352113,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8991894249234176,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9318945733818982,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.963750784805772,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9947320596489881,"velocity":2.5,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.024821410587785,"velocity":2.5,"acceleration":-1.5142215235962606,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.0542731215093437,"velocity":2.4554035854158403,"acceleration":-3.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.0836007969891943,"velocity":2.367420558976288,"acceleration":-2.9999999999999933,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.113080971642734,"velocity":2.278980035015669,"acceleration":-3.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.142748981256642,"velocity":2.1899760061739455,"acceleration":-2.999999999999993,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.172654458612498,"velocity":2.1002595741063774,"acceleration":-3.0000000000000036,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.2334655374361456,"velocity":1.917826337635434,"acceleration":-3.0000000000000018,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.296330534384713,"velocity":1.729231346789732,"acceleration":-3.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.3626220225156076,"velocity":1.5303568823970488,"acceleration":-2.999999999999999,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4347362085938182,"velocity":1.3140143241624165,"acceleration":-3.0,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5173595265421733,"velocity":1.0661443703173517,"acceleration":-2.9999999999999987,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.622416055107695,"velocity":0.7509747846207866,"acceleration":-3.0,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.872740983314624,"velocity":0.0,"acceleration":-3.0,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24087146067360232,"velocity":0.6503529438187263,"acceleration":2.7,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.3403321067745373,"velocity":0.9188966882912508,"acceleration":2.700000000000002,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.4162093899230572,"velocity":1.1237653527922549,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.4796643300987847,"velocity":1.295093691266719,"acceleration":2.187682579886264,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5355873525736204,"velocity":1.4174355133495053,"acceleration":-0.734989108404782,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5891532438960738,"velocity":1.378065166645508,"acceleration":-0.6426464146474123,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.643461958301142,"velocity":1.3431638660489806,"acceleration":-0.5453266663691863,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6984118145503199,"velocity":1.3131982441231504,"acceleration":-0.4394478338280287,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.7539154354262596,"velocity":1.2888072981596066,"acceleration":-0.32468479529931726,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.809892141790966,"velocity":1.270632512712052,"acceleration":-0.20350244619471486,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.8662640818893885,"velocity":1.259160685005281,"acceleration":-0.08045877877688212,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.9229565411514642,"velocity":1.2545992789671963,"acceleration":0.03871626769718015,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.9799017050564615,"velocity":1.256803983177002,"acceleration":0.14815351255780274,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":1.037044049449856,"velocity":1.265269822214671,"acceleration":0.24282302918833878,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":1.0943450481757073,"velocity":1.2791838243007994,"acceleration":0.3192880318302778,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":1.151785273529354,"velocity":1.2975238008018528,"acceleration":0.37610415805722364,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":1.2093630070251655,"velocity":1.3191790257811382,"acceleration":0.4138164485760789,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.2670896494085861,"velocity":1.3430672599204667,"acceleration":0.43460983400489717,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.3249830335940764,"velocity":1.3682282940113044,"acceleration":0.4417529373871998,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.383060006914625,"velocity":1.3938839675702148,"acceleration":0.43900370786176246,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.441329456851559,"velocity":1.4194644721475942,"acceleration":0.43011410600385014,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.4997865390306198,"velocity":1.4446076877886345,"acceleration":0.41850596016098784,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.5584084380603251,"velocity":1.4691413019285218,"acceleration":0.4071259180970876,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.6171516690776433,"velocity":1.4930571937884367,"acceleration":0.3984462097492305,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.6759507421084023,"velocity":1.5164854615743109,"acceleration":0.39456346754290916,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.7347179434753464,"velocity":1.5396728523234446,"acceleration":0.397352070883421,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.7933440031024948,"velocity":1.562968038524027,"acceleration":0.4086404035647724,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.8516994822211263,"velocity":1.5868144450612802,"acceleration":0.43038917748303396,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.909636813019807,"velocity":1.6117500452092868,"acceleration":0.46485442754444356,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.9669930430836986,"velocity":1.6384123427017445,"acceleration":0.5147082657382434,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":2.0235934863647898,"velocity":1.6675450587029708,"acceleration":0.5830593842147167,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":2.079256669525349,"velocity":1.6999999999999975,"acceleration":4.265969051474203E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":2.1343686615338977,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1892768582496456,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.243795345773229,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.297757143445389,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.351013572738965,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4034336261508975,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4549033360942203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5053251437900577,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5546172681596233,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6027130747162124,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.649560444457203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6951211427560504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.739370188254281,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.782295221753494,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8238958751073535,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.864183140113587,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9031787374059825,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.940914485346383,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.01278040861083,"velocity":1.7,"acceleration":-0.8670174294348908,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0814146722541467,"velocity":1.6404928971648147,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.151292036246028,"velocity":1.4518240143867347,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.227307062784812,"velocity":1.2465834427320184,"acceleration":-2.7,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.314399686970503,"velocity":1.0114333574306535,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.4251389914163664,"velocity":0.7124372354268221,"acceleration":-2.7,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.6890046341670413,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 84d250f..5f82551 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.trajectory.Trajectory; @@ -33,6 +34,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.auto.DriveOffLineBackward; +import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.auto.Wait; @@ -92,11 +95,16 @@ public class RobotContainer { private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); /* Autos */ - SixBallAutoMiddle m_sixBallAutoMiddle; double m_totalTimeAuto; + SixBallAutoMiddle m_sixBallAutoMiddle; + EightBallAutoMiddle m_eightBallAutoMiddle; + DriveOffLineForward m_driveOffLineForward; + + DriveOffLineBackward m_driveOffLineBackward; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -247,12 +255,26 @@ public class RobotContainer { String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; - m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + + m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths, false)); String[] eightBallAutoMiddlePaths = new String[]{ "EightBallMidComplete" }; - m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); + + m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths, false)); + + String[] driveOffLineForwardPaths = new String[]{ + "DriveOffLineForward" + }; + + m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths, false)); + + String[] driveOffLineBackwardPaths = new String[]{ + "DriveOffLineBackward" + }; + + m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths, true)); } /** @@ -271,7 +293,9 @@ public class RobotContainer { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); @@ -314,7 +338,7 @@ public class RobotContainer { return ramseteCommand; } - public RamseteCommand[] buildPaths(String[] paths) { + public RamseteCommand[] buildPaths(String[] paths, boolean isReversed) { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; double[] times = new double[paths.length]; Trajectory initialTrajectory; @@ -327,6 +351,7 @@ public class RobotContainer { Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); initialTrajectory = trajectory; + RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose())); ramseteCommands[0] = ramseteCommand; times[0] = initialTrajectory.getTotalTimeSeconds(); diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java new file mode 100644 index 0000000..fed1212 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.commands.drive.TurnDegrees; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class DriveOffLineBackward extends SequentialCommandGroup { + /** + * Creates a new DriveOffLineBackward. + */ + public DriveOffLineBackward(Drive drive, RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands( + new TankDriveVelocity(drive, -1, -1, 2) + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java new file mode 100644 index 0000000..a5fb618 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class DriveOffLineForward extends SequentialCommandGroup { + /** + * Creates a new DriveOffLineForward. + */ + public DriveOffLineForward(Drive drive, RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands( + paths[0] + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java b/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java new file mode 100644 index 0000000..595d37c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class TankDriveVelocity extends CommandBase { + Drive m_drive; + double m_leftTargetVel; + double m_rightTargetVel; + + double m_targetTime; + double m_firstTimeSec; + double m_currentTimeSec; + double m_diffSec; + + /** + * Creates a new TankDriveVelocity. + */ + public TankDriveVelocity(Drive subsystem, double leftTargetVel, double rightTargetVel, double targetTime) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_leftTargetVel = leftTargetVel; + m_rightTargetVel = rightTargetVel; + m_targetTime = targetTime; + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_firstTimeSec = (System.currentTimeMillis() / 1000); + m_diffSec = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_currentTimeSec = (System.currentTimeMillis() / 1000); + m_diffSec = m_currentTimeSec - m_firstTimeSec; + + if (m_diffSec < m_targetTime) { + m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_diffSec >= m_targetTime) { + return true; + } + return false; + } +} From 713ab60c424e4c10b1b8c2fead19c9a4ca7d7f97 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Mar 2020 20:14:28 -0600 Subject: [PATCH 25/39] Fixed Enums for Manual --- .../java/frc4388/robot/RobotContainer.java | 8 ++-- .../commands/shooter/ShooterGoalPosition.java | 40 +++++++++++++++++++ .../shooter/ShooterTrenchPosition.java | 40 +++++++++++++++++++ .../robot/commands/storage/ManageStorage.java | 29 ++++++-------- .../commands/storage/ManageStoragePID.java | 26 ++++++------ .../frc4388/robot/subsystems/Storage.java | 7 ++++ 6 files changed, 116 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java create mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8e9854f..1ba7be0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -44,7 +44,6 @@ import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.storage.ManageStorage; import frc4388.robot.commands.storage.StoragePrep; -import frc4388.robot.commands.storage.ManageStorage.StorageMode; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; @@ -57,6 +56,7 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -129,7 +129,7 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage, StorageMode.IDLE)); + m_robotStorage.setDefaultCommand(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.IDLE))); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -247,8 +247,8 @@ public class RobotContainer { /* Button Fox */ // Storage Manual new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH) - .whileHeld(new ManageStorage(m_robotStorage, StorageMode.MANUAL)) - .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); + .whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))) + .whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); // Meg new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java new file mode 100644 index 0000000..acf2366 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterGoalPosition extends CommandBase { + /** + * Creates a new ShooterGoalPosition. + */ + public ShooterGoalPosition() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java new file mode 100644 index 0000000..f5408f2 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterTrenchPosition extends CommandBase { + /** + * Creates a new ShooterTrenchPosition. + */ + public ShooterTrenchPosition() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java index 5f3537c..8b55380 100644 --- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Storage.StorageMode; public class ManageStorage extends CommandBase { Storage m_storage; @@ -26,17 +27,13 @@ public class ManageStorage extends CommandBase { /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ boolean m_isStorageEmpty = true; - - public enum StorageMode{IDLE, INTAKE, RESET, MANUAL}; - StorageMode m_storageMode = StorageMode.IDLE; /** * Creates a new ManageStorage. */ - public ManageStorage(Storage m_robotStorage, StorageMode storageMode) { + public ManageStorage(Storage m_robotStorage) { // Use addRequirements() here to declare subsystem dependencies. m_storage = m_robotStorage; - m_storageMode = storageMode; addRequirements(m_storage); } @@ -50,7 +47,7 @@ public class ManageStorage extends CommandBase { m_isStorageEmpty = !m_isBallInStorage; - if (m_storageMode == StorageMode.RESET) { + if (m_storage.m_storageMode == StorageMode.RESET) { m_resetStartTime = System.currentTimeMillis(); } } @@ -68,13 +65,13 @@ public class ManageStorage extends CommandBase { SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); - if (m_storageMode == StorageMode.IDLE) { + if (m_storage.m_storageMode == StorageMode.IDLE) { runIdle(); - } else if (m_storageMode == StorageMode.INTAKE) { + } else if (m_storage.m_storageMode == StorageMode.INTAKE) { runIntake(); - } else if (m_storageMode == StorageMode.RESET) { + } else if (m_storage.m_storageMode == StorageMode.RESET) { runReset(); - } else if (m_storageMode == StorageMode.MANUAL) { + } else if (m_storage.m_storageMode == StorageMode.MANUAL) { runManual(); } } @@ -93,10 +90,10 @@ public class ManageStorage extends CommandBase { } if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode m_isStorageEmpty = false; - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } } else { - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } } @@ -108,7 +105,7 @@ public class ManageStorage extends CommandBase { m_storage.runStorage(0); if (m_isBallInIntake) { - m_storageMode = StorageMode.INTAKE; + m_storage.changeStorageMode(StorageMode.INTAKE); } m_isStorageEmpty = !m_isBallInStorage; } @@ -122,9 +119,9 @@ public class ManageStorage extends CommandBase { m_storage.runStorage(-StorageConstants.STORAGE_SPEED); if (m_isBallInIntake) { - m_storageMode = StorageMode.INTAKE; + m_storage.changeStorageMode(StorageMode.INTAKE); } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } m_isStorageEmpty = !m_isBallInStorage; } @@ -139,7 +136,7 @@ public class ManageStorage extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_storageMode = StorageMode.RESET; + m_storage.changeStorageMode(StorageMode.RESET); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java index 6ed4fd3..97ff356 100644 --- a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java +++ b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Storage.StorageMode; public class ManageStoragePID extends CommandBase { Storage m_storage; @@ -30,16 +31,13 @@ public class ManageStoragePID extends CommandBase { /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ boolean m_isStorageEmpty = true; - public enum StorageMode{IDLE, INTAKE, RESET}; - StorageMode m_storageMode = StorageMode.IDLE; /** * Creates a new ManageStorage. */ - public ManageStoragePID(Storage m_robotStorage, StorageMode storageMode) { + public ManageStoragePID(Storage m_robotStorage) { // Use addRequirements() here to declare subsystem dependencies. m_storage = m_robotStorage; - m_storageMode = storageMode; addRequirements(m_storage); } @@ -55,7 +53,7 @@ public class ManageStoragePID extends CommandBase { m_intakeStartPos = m_storage.getEncoderPosInches(); - if (m_storageMode == StorageMode.RESET) { + if (m_storage.m_storageMode == StorageMode.RESET) { m_resetStartTime = System.currentTimeMillis(); } } @@ -73,11 +71,11 @@ public class ManageStoragePID extends CommandBase { SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); - if (m_storageMode == StorageMode.IDLE) { + if (m_storage.m_storageMode == StorageMode.IDLE) { runIdle(); - } else if (m_storageMode == StorageMode.INTAKE) { + } else if (m_storage.m_storageMode == StorageMode.INTAKE) { runIntake(); - } else if (m_storageMode == StorageMode.RESET) { + } else if (m_storage.m_storageMode == StorageMode.RESET) { runReset(); } } @@ -93,10 +91,10 @@ public class ManageStoragePID extends CommandBase { double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches(); if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) { - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } } else { - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } } @@ -108,7 +106,7 @@ public class ManageStoragePID extends CommandBase { m_storage.runStorage(0); if (m_isBallInIntake) { - m_storageMode = StorageMode.INTAKE; + m_storage.changeStorageMode(StorageMode.INTAKE); m_intakeStartPos = m_storage.getEncoderPosInches(); } m_isStorageEmpty = !m_isBallInStorage; @@ -123,10 +121,10 @@ public class ManageStoragePID extends CommandBase { m_storage.runStorage(-StorageConstants.STORAGE_SPEED); if (m_isBallInIntake) { - m_storageMode = StorageMode.INTAKE; + m_storage.changeStorageMode(StorageMode.INTAKE); m_intakeStartPos = m_storage.getEncoderPosInches(); } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { - m_storageMode = StorageMode.IDLE; + m_storage.changeStorageMode(StorageMode.IDLE); } m_isStorageEmpty = !m_isBallInStorage; } @@ -134,7 +132,7 @@ public class ManageStoragePID extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_storageMode = StorageMode.RESET; + m_storage.changeStorageMode(StorageMode.RESET); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 403de25..98bc3b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -36,6 +36,9 @@ public class Storage extends SubsystemBase { public boolean m_isStorageReadyToFire = false; + public enum StorageMode{IDLE, INTAKE, RESET, MANUAL}; + public StorageMode m_storageMode = StorageMode.IDLE; + /** * Creates a new Storage. */ @@ -134,4 +137,8 @@ public class Storage extends SubsystemBase { public boolean getBeamIntake(){ return m_beamIntake.get(); } + + public void changeStorageMode(StorageMode storageMode){ + m_storageMode = storageMode; + } } From 427be2c9bf55779b434cc0f84945fa2bd4fb4903 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Tue, 10 Mar 2020 20:16:55 -0600 Subject: [PATCH 26/39] added stuff --- PathWeaver/Paths/FiveBallMidComplete | 3 +++ src/main/deploy/paths/FiveBallMidComplete.wpilib.json | 1 + 2 files changed, 4 insertions(+) create mode 100644 PathWeaver/Paths/FiveBallMidComplete create mode 100644 src/main/deploy/paths/FiveBallMidComplete.wpilib.json diff --git a/PathWeaver/Paths/FiveBallMidComplete b/PathWeaver/Paths/FiveBallMidComplete new file mode 100644 index 0000000..17bb9b1 --- /dev/null +++ b/PathWeaver/Paths/FiveBallMidComplete @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,3.048,0.0,true, +3.048,-3.048,0.0,-3.048,true, diff --git a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json new file mode 100644 index 0000000..faf1565 --- /dev/null +++ b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.0,"y":0.0},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26611539250248006,"velocity":0.7185115597566961,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.09560199522972107,"y":-5.351235866546631E-4},"rotation":{"radians":-0.01643557600211064}},"curvature":-0.3280139914006408},{"time":0.37832612257576836,"velocity":1.0214805309545745,"acceleration":2.6999999999999993,"pose":{"translation":{"x":0.19315972137451173,"y":-0.004101493835449219},"rotation":{"radians":-0.060227767837218976}},"curvature":-0.5482112858961018},{"time":0.4672171543421533,"velocity":1.2614863167238137,"acceleration":1.7117954491622207,"pose":{"translation":{"x":0.2942139637470245,"y":-0.01325146222114563},"rotation":{"radians":-0.122128208991722}},"curvature":-0.6527862400129346},{"time":0.5476447009514351,"velocity":1.399161824996865,"acceleration":0.2256047834474202,"pose":{"translation":{"x":0.399882568359375,"y":-0.030044677734375},"rotation":{"radians":-0.19326096007443874}},"curvature":-0.6636207972685537},{"time":0.6286099324930101,"velocity":1.4174279685255722,"acceleration":0.3445398373025691,"pose":{"translation":{"x":0.5108931434154511,"y":-0.056080788373947144},"rotation":{"radians":-0.26655398309896333}},"curvature":-0.6152946879689695},{"time":0.7139941674480041,"velocity":1.4468462389451702,"acceleration":0.36702991751433095,"pose":{"translation":{"x":0.6276157608032227,"y":-0.09253214263916015},"rotation":{"radians":-0.3372911276165204}},"curvature":-0.5400288901862749},{"time":0.7583500845207223,"velocity":1.4631261875296424,"acceleration":0.35939189955348283,"pose":{"translation":{"x":0.6881450166329742,"y":-0.11492110296338795},"rotation":{"radians":-0.3708348624485024}},"curvature":-0.49967803189401166},{"time":0.8038207086533624,"velocity":1.4794679615105544,"acceleration":0.34110673226584154,"pose":{"translation":{"x":0.7500956575870513,"y":-0.14017649102210997},"rotation":{"radians":-0.40292427332100755}},"curvature":-0.46006703591423725},{"time":0.8504194911370777,"velocity":1.4953631199311412,"acceleration":0.316223700987995,"pose":{"translation":{"x":0.8134279479458928,"y":-0.1683392347767949},"rotation":{"radians":-0.43348589624470896}},"curvature":-0.42236918942041696},{"time":0.8981565124814388,"velocity":1.510458697494798,"acceleration":0.28785953309578993,"pose":{"translation":{"x":0.8780859375000001,"y":-0.1994296875},"rotation":{"radians":-0.46250735863979175}},"curvature":-0.3873022288183412},{"time":0.9470356598236636,"velocity":1.5245290260268511,"acceleration":0.2582886030433786,"pose":{"translation":{"x":0.9439984834715724,"y":-0.23344864969700577},"rotation":{"radians":-0.49002121151097894}},"curvature":-0.3552422229542392},{"time":0.9970525446129715,"velocity":1.5374478173276631,"acceleration":0.22907304524839023,"pose":{"translation":{"x":1.011080272436142,"y":-0.27037839102745054},"rotation":{"radians":-0.5160916282809809}},"curvature":-0.32632279408302584},{"time":1.0481931014539378,"velocity":1.5491627404189217,"acceleration":0.20120691663021545,"pose":{"translation":{"x":1.079232842244208,"y":-0.31018367222696547},"rotation":{"radians":-0.5408038933963396}},"curvature":-0.3005152946982005},{"time":1.1004327744652074,"velocity":1.55967372395129,"acceleration":0.1752545092620447,"pose":{"translation":{"x":1.1483456039428712,"y":-0.3528127670288086},"rotation":{"radians":-0.5642563851793583}},"curvature":-0.2776899960695386},{"time":1.1537361868964333,"velocity":1.5690153873389168,"acceleration":0.15147085717835027,"pose":{"translation":{"x":1.2182968636974691,"y":-0.3981984840855003},"rotation":{"radians":-0.5865546641462306}},"curvature":-0.2576606317119665},{"time":1.208057193853282,"velocity":1.5772434368254618,"acceleration":0.12989944308354476,"pose":{"translation":{"x":1.288954844713211,"y":-0.4462591888904572},"rotation":{"radians":-0.6078072654168749}},"curvature":-0.24021547502233934},{"time":1.2633392297495507,"velocity":1.5844245425009116,"acceleration":0.11044695104753953,"pose":{"translation":{"x":1.3601787091568114,"y":-0.4968998256996274},"rotation":{"radians":-0.6281228263232282}},"curvature":-0.2251381277104783},{"time":1.319515876771435,"velocity":1.5906290818845525,"acceleration":0.09293763772828834,"pose":{"translation":{"x":1.431819580078125,"y":-0.550012939453125},"rotation":{"radians":-0.6476082331693351}},"curvature":-0.21222079108774547},{"time":1.3765115953423626,"velocity":1.595926129329161,"acceleration":0.07715101466312033,"pose":{"translation":{"x":1.5037215633317829,"y":-0.6054796976968646},"rotation":{"radians":-0.6663675289892361}},"curvature":-0.20127225459593204},{"time":1.434242570829637,"velocity":1.6003801326654958,"acceleration":0.06284663997261843,"pose":{"translation":{"x":1.5757227694988252,"y":-0.6631709125041961},"rotation":{"radians":-0.6845013786777033}},"curvature":-0.19212230898477914},{"time":1.4926176419327786,"velocity":1.604048809742491,"acceleration":0.049779393953850636,"pose":{"translation":{"x":1.647656335808337,"y":-0.7229480623975397},"rotation":{"radians":-0.7021069351713791}},"curvature":-0.18462383884095865},{"time":1.5515392852483902,"velocity":1.606981893437507,"acceleration":0.037707987143503895,"pose":{"translation":{"x":1.719351448059082,"y":-0.7846643142700194},"rotation":{"radians":-0.7192779892828585}},"curvature":-0.1786534886681594},{"time":1.610904637591002,"velocity":1.6092204413804119,"acceleration":0.026398811602806938,"pose":{"translation":{"x":1.7906343625411392,"y":-0.8481655453070999},"rotation":{"radians":-0.73610531663522}},"curvature":-0.1741115236266933},{"time":1.6706065430426809,"velocity":1.6107965007347593,"acceleration":0.015626689192205216,"pose":{"translation":{"x":1.8613294279575348,"y":-0.9132913649082184},"rotation":{"radians":-0.7526771588590878}},"curvature":-0.17092130635439215},{"time":1.7305346157034076,"velocity":1.6117329781001164,"acceleration":0.005173627968943801,"pose":{"translation":{"x":1.9312601073458793,"y":-0.9798761366084217},"rotation":{"radians":-0.769079793937514}},"curvature":-0.16902866891701063},{"time":1.7905763119866465,"velocity":1.6120436114993102,"acceleration":-0.005173627968943801,"pose":{"translation":{"x":2.0002500000000003,"y":-1.04775},"rotation":{"radians":-0.7853981633974483}},"curvature":-0.16840135895962896},{"time":1.8506180082698853,"velocity":1.6117329781001164,"acceleration":-0.01562668919220529,"pose":{"translation":{"x":2.0681238633915786,"y":-1.116739892654121},"rotation":{"radians":-0.8017165328573825}},"curvature":-0.16902866891701068},{"time":1.9105460809306118,"velocity":1.6107965007347593,"acceleration":-0.026398811602806938,"pose":{"translation":{"x":2.1347086350917817,"y":-1.1866705720424653},"rotation":{"radians":-0.8181191679358089}},"curvature":-0.1709213063543919},{"time":1.9702479863822906,"velocity":1.6092204413804119,"acceleration":-0.03770798714350157,"pose":{"translation":{"x":2.1998344546929003,"y":-1.2573656374588609},"rotation":{"radians":-0.8346910101596765}},"curvature":-0.1741115236266933},{"time":2.0296133387249005,"velocity":1.6069818934375073,"acceleration":-0.049779393953852634,"pose":{"translation":{"x":2.263335685729981,"y":-1.3286485519409177},"rotation":{"radians":-0.8515183375120382}},"curvature":-0.1786534886681591},{"time":2.0885349820405144,"velocity":1.604048809742491,"acceleration":-0.06284663997261858,"pose":{"translation":{"x":2.3250519376024608,"y":-1.4003436641916633},"rotation":{"radians":-0.8686893916235177}},"curvature":-0.1846238388409585},{"time":2.146910053143656,"velocity":1.6003801326654958,"acceleration":-0.0771510146631207,"pose":{"translation":{"x":2.3848290874958042,"y":-1.472277230501175},"rotation":{"radians":-0.8862949481171933}},"curvature":-0.19212230898477875},{"time":2.2046410286309297,"velocity":1.595926129329161,"acceleration":-0.09293763772828544,"pose":{"translation":{"x":2.4425203023031354,"y":-1.544278436668217},"rotation":{"radians":-0.9044287978056609}},"curvature":-0.20127225459593212},{"time":2.2616367472018566,"velocity":1.5906290818845528,"acceleration":-0.11044695104754229,"pose":{"translation":{"x":2.4979870605468752,"y":-1.616180419921875},"rotation":{"radians":-0.9231880936255614}},"curvature":-0.21222079108774536},{"time":2.3178133942237413,"velocity":1.5844245425009116,"acceleration":-0.1298994430835441,"pose":{"translation":{"x":2.551100174300373,"y":-1.6878212908431882},"rotation":{"radians":-0.9426735004716682}},"curvature":-0.2251381277104782},{"time":2.3730954301200105,"velocity":1.5772434368254618,"acceleration":-0.15147085717835143,"pose":{"translation":{"x":2.601740811109543,"y":-1.7590451552867892},"rotation":{"radians":-0.962989061378022}},"curvature":-0.24021547502233934},{"time":2.4274164370768587,"velocity":1.5690153873389168,"acceleration":-0.17525450926204203,"pose":{"translation":{"x":2.6498015159145,"y":-1.8297031363025305},"rotation":{"radians":-0.9842416626486657}},"curvature":-0.2576606317119665},{"time":2.480719849508084,"velocity":1.5596737239512901,"acceleration":-0.20120691663021434,"pose":{"translation":{"x":2.6951872329711914,"y":-1.8996543960571284},"rotation":{"radians":-1.0065399416155383}},"curvature":-0.2776899960695381},{"time":2.532959522519354,"velocity":1.5491627404189219,"acceleration":-0.22907304524839303,"pose":{"translation":{"x":2.7378163277730345,"y":-1.9687671577557921},"rotation":{"radians":-1.029992433398557}},"curvature":-0.3005152946982001},{"time":2.584100079360321,"velocity":1.5374478173276631,"acceleration":-0.25828860304337636,"pose":{"translation":{"x":2.77762160897255,"y":-2.0369197275638573},"rotation":{"radians":-1.054704698513916}},"curvature":-0.326322794083026},{"time":2.634116964149629,"velocity":1.5245290260268511,"acceleration":-0.28785953309577883,"pose":{"translation":{"x":2.814551350302995,"y":-2.1040015165284274},"rotation":{"radians":-1.0807751152839182}},"curvature":-0.35524222295423874},{"time":2.682996111491854,"velocity":1.5104586974947984,"acceleration":-0.3162237009879981,"pose":{"translation":{"x":2.848570312500001,"y":-2.1699140625},"rotation":{"radians":-1.108288968155105}},"curvature":-0.38730222881834014},{"time":2.7307331328362148,"velocity":1.4953631199311417,"acceleration":-0.3411067322658357,"pose":{"translation":{"x":2.8796607652232056,"y":-2.2345720520541077},"rotation":{"radians":-1.137310430550188}},"curvature":-0.42236918942041607},{"time":2.7773319153199303,"velocity":1.479467961510555,"acceleration":-0.35939189955349155,"pose":{"translation":{"x":2.907823508977891,"y":-2.2979043424129486},"rotation":{"radians":-1.1678720534738884}},"curvature":-0.46006703591423553},{"time":2.82280253945257,"velocity":1.4631261875296429,"acceleration":-0.3670299175143166,"pose":{"translation":{"x":2.9330788970366135,"y":-2.3598549833670246},"rotation":{"radians":-1.199961464346393}},"curvature":-0.4996780318940104},{"time":2.867158456525288,"velocity":1.4468462389451713,"acceleration":-0.34453983730256904,"pose":{"translation":{"x":2.955467857360841,"y":-2.4203842391967765},"rotation":{"radians":-1.2335051991783754}},"curvature":-0.5400288901862721},{"time":2.952542691480283,"velocity":1.417427968525573,"acceleration":-0.22560478344741489,"pose":{"translation":{"x":2.9919192116260533,"y":-2.5371068565845487},"rotation":{"radians":-1.3042423436959323}},"curvature":-0.6152946879689668},{"time":3.033507923021858,"velocity":1.3991618249968663,"acceleration":-1.7117954491622256,"pose":{"translation":{"x":3.0179553222656255,"y":-2.6481174316406246},"rotation":{"radians":-1.3775353667204573}},"curvature":-0.6636207972685509},{"time":3.1139354696311403,"velocity":1.261486316723814,"acceleration":-2.700000000000002,"pose":{"translation":{"x":3.0347485377788566,"y":-2.7537860362529756},"rotation":{"radians":-1.4486681178031742}},"curvature":-0.6527862400129302},{"time":3.202826501397525,"velocity":1.0214805309545756,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":3.0438985061645507,"y":-2.8548402786254883},"rotation":{"radians":-1.5105685589576778}},"curvature":-0.5482112858960992},{"time":3.315037231470813,"velocity":0.7185115597566977,"acceleration":-2.7,"pose":{"translation":{"x":3.0474648764133487,"y":-2.952398004770279},"rotation":{"radians":-1.554360750792784}},"curvature":-0.32801399140063797},{"time":3.581152623973294,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.048000000000002,"y":-3.048},"rotation":{"radians":-1.5707963267948948}},"curvature":1.5296436709931115E-15}] \ No newline at end of file From b55974b01c0f8680fcc1f29be5739b8527897ca1 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 10 Mar 2020 20:23:32 -0600 Subject: [PATCH 27/39] Added Goal and Trench Position Commands Untested --- src/main/java/frc4388/robot/RobotContainer.java | 14 ++++++++++---- .../commands/shooter/ShooterGoalPosition.java | 16 ++++++++++++++-- .../commands/shooter/ShooterTrenchPosition.java | 16 ++++++++++++++-- 3 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1ba7be0..3612c9d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,8 @@ import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.shooter.ShooterGoalPosition; +import frc4388.robot.commands.shooter.ShooterTrenchPosition; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.storage.ManageStorage; @@ -262,13 +264,17 @@ public class RobotContainer { // Goal Shooter Position new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + .whileHeld(new ShooterGoalPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) + .whenReleased(new InterruptSubystem(m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotShooterHood)) + .whenReleased(new InterruptSubystem(m_robotShooterAim)); // Trench Shooter Position new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) - .whileHeld(new PlaySongDrive(m_robotDrive)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) + .whenReleased(new InterruptSubystem(m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotShooterHood)) + .whenReleased(new InterruptSubystem(m_robotShooterAim)); } /** diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java index acf2366..b750c66 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java @@ -8,13 +8,22 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class ShooterGoalPosition extends CommandBase { + Shooter m_shooter; + ShooterHood m_hood; + ShooterAim m_aim; /** * Creates a new ShooterGoalPosition. */ - public ShooterGoalPosition() { - // Use addRequirements() here to declare subsystem dependencies. + public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { + m_shooter = shooterSub; + m_hood = hoodSub; + m_aim = aimSub; + addRequirements(m_shooter,m_hood,m_aim); } // Called when the command is initially scheduled. @@ -25,6 +34,9 @@ public class ShooterGoalPosition extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + m_shooter.runDrumShooterVelocityPID(5000); + m_hood.runAngleAdjustPID(3); + m_aim.runshooterRotatePID(-26.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java index f5408f2..ba452b7 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java @@ -8,13 +8,22 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; public class ShooterTrenchPosition extends CommandBase { + Shooter m_shooter; + ShooterHood m_hood; + ShooterAim m_aim; /** * Creates a new ShooterTrenchPosition. */ - public ShooterTrenchPosition() { - // Use addRequirements() here to declare subsystem dependencies. + public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { + m_shooter = shooterSub; + m_hood = hoodSub; + m_aim = aimSub; + addRequirements(m_shooter,m_hood,m_aim); } // Called when the command is initially scheduled. @@ -25,6 +34,9 @@ public class ShooterTrenchPosition extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + m_shooter.runDrumShooterVelocityPID(5000); + m_hood.runAngleAdjustPID(3); + m_aim.runshooterRotatePID(-26.5); } // Called once the command ends or is interrupted. From f4e184a2f842102a64c6ef2f5b3daa35a9835658 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 16:35:46 -0600 Subject: [PATCH 28/39] Added storage command Co-Authored-By: kyrarivera --- .../java/frc4388/robot/RobotContainer.java | 1 - .../robot/commands/intake/RunIntake.java | 46 +++++++++++++++++++ .../robot/commands/storage/RunStorage.java | 46 +++++++++++++++++++ 3 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/commands/intake/RunIntake.java create mode 100644 src/main/java/frc4388/robot/commands/storage/RunStorage.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3ff294f..d3c5d05 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,7 +49,6 @@ import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.ShootFireGroup; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.shooter.ShootPrepGroup; diff --git a/src/main/java/frc4388/robot/commands/intake/RunIntake.java b/src/main/java/frc4388/robot/commands/intake/RunIntake.java new file mode 100644 index 0000000..c88b1f7 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/intake/RunIntake.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.subsystems.Intake; + +public class RunIntake extends CommandBase { + Intake m_intake; + /** + * Creates a new RunIntake. + */ + public RunIntake(Intake subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = subsystem; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_intake.runIntake(IntakeConstants.INTAKE_SPEED); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storage/RunStorage.java b/src/main/java/frc4388/robot/commands/storage/RunStorage.java new file mode 100644 index 0000000..3a259d1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storage/RunStorage.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.storage; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.StorageConstants; +import frc4388.robot.subsystems.Storage; + +public class RunStorage extends CommandBase { + Storage m_storage; + /** + * Creates a new RunStorage. + */ + public RunStorage(Storage subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_storage = subsystem; + addRequirements(m_storage); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_storage.runStorage(StorageConstants.STORAGE_SPEED); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From ef62fc1119cb17c4eb819d950e09e13d77f1b334 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 16:46:40 -0600 Subject: [PATCH 29/39] Added a five ball auto path Co-Authored-By: kyrarivera --- .../java/frc4388/robot/RobotContainer.java | 22 ++++++++++----- .../commands/auto/FiveBallAutoMiddle.java | 27 +++++++++++++++++++ 2 files changed, 43 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d3c5d05..5bb9e90 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,6 +37,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.commands.auto.DriveOffLineBackward; import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; +import frc4388.robot.commands.auto.FiveBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; import frc4388.robot.commands.InterruptSubystem; import frc4388.robot.commands.auto.AutoPath1FromCenter; @@ -112,6 +113,8 @@ public class RobotContainer { DriveOffLineBackward m_driveOffLineBackward; + FiveBallAutoMiddle m_fiveBallAutoMiddle; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -267,25 +270,31 @@ public class RobotContainer { "SixBallMidComplete" }; - m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths, false)); + m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); String[] eightBallAutoMiddlePaths = new String[]{ "EightBallMidComplete" }; - m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths, false)); + m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); String[] driveOffLineForwardPaths = new String[]{ "DriveOffLineForward" }; - m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths, false)); + m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths)); String[] driveOffLineBackwardPaths = new String[]{ "DriveOffLineBackward" }; - m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths, true)); + m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); + + String[] fiveBallAutoMiddlePaths = new String[]{ + "FiveBallMidComplete" + }; + + m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); } /** @@ -306,7 +315,8 @@ public class RobotContainer { //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); @@ -349,7 +359,7 @@ public class RobotContainer { return ramseteCommand; } - public RamseteCommand[] buildPaths(String[] paths, boolean isReversed) { + public RamseteCommand[] buildPaths(String[] paths) { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; double[] times = new double[paths.length]; Trajectory initialTrajectory; diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java new file mode 100644 index 0000000..5e5e18f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class FiveBallAutoMiddle extends SequentialCommandGroup { + /** + * Creates a new FiveBallAutoMiddle. + */ + public FiveBallAutoMiddle(Drive drive, RamseteCommand[] paths) { + // Use addRequirements() here to declare subsystem dependencies. + addCommands( + paths[0] + ); + } +} From 70c9dabbce08fa10789f60dacb6d6fcb2972923c Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 16:49:42 -0600 Subject: [PATCH 30/39] Added the path "FiveBallMidComplete" Co-Authored-By: kyrarivera --- PathWeaver/Paths/FiveBallMidComplete | 5 +++-- src/main/deploy/paths/FiveBallMidComplete.wpilib.json | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/PathWeaver/Paths/FiveBallMidComplete b/PathWeaver/Paths/FiveBallMidComplete index 17bb9b1..c2f4041 100644 --- a/PathWeaver/Paths/FiveBallMidComplete +++ b/PathWeaver/Paths/FiveBallMidComplete @@ -1,3 +1,4 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -0.0,0.0,3.048,0.0,true, -3.048,-3.048,0.0,-3.048,true, +3.2,-2.3,0.5,0.0,true, +5.8746484146930245,-2.2145857763164236,0.9797262118384756,-0.6607896052402182,false, +6.5409266067515,-2.857068318658526,-0.24985432202192825,-0.10708042372368398,true, diff --git a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json index faf1565..8b8d170 100644 --- a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.0,"y":0.0},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.26611539250248006,"velocity":0.7185115597566961,"acceleration":2.6999999999999997,"pose":{"translation":{"x":0.09560199522972107,"y":-5.351235866546631E-4},"rotation":{"radians":-0.01643557600211064}},"curvature":-0.3280139914006408},{"time":0.37832612257576836,"velocity":1.0214805309545745,"acceleration":2.6999999999999993,"pose":{"translation":{"x":0.19315972137451173,"y":-0.004101493835449219},"rotation":{"radians":-0.060227767837218976}},"curvature":-0.5482112858961018},{"time":0.4672171543421533,"velocity":1.2614863167238137,"acceleration":1.7117954491622207,"pose":{"translation":{"x":0.2942139637470245,"y":-0.01325146222114563},"rotation":{"radians":-0.122128208991722}},"curvature":-0.6527862400129346},{"time":0.5476447009514351,"velocity":1.399161824996865,"acceleration":0.2256047834474202,"pose":{"translation":{"x":0.399882568359375,"y":-0.030044677734375},"rotation":{"radians":-0.19326096007443874}},"curvature":-0.6636207972685537},{"time":0.6286099324930101,"velocity":1.4174279685255722,"acceleration":0.3445398373025691,"pose":{"translation":{"x":0.5108931434154511,"y":-0.056080788373947144},"rotation":{"radians":-0.26655398309896333}},"curvature":-0.6152946879689695},{"time":0.7139941674480041,"velocity":1.4468462389451702,"acceleration":0.36702991751433095,"pose":{"translation":{"x":0.6276157608032227,"y":-0.09253214263916015},"rotation":{"radians":-0.3372911276165204}},"curvature":-0.5400288901862749},{"time":0.7583500845207223,"velocity":1.4631261875296424,"acceleration":0.35939189955348283,"pose":{"translation":{"x":0.6881450166329742,"y":-0.11492110296338795},"rotation":{"radians":-0.3708348624485024}},"curvature":-0.49967803189401166},{"time":0.8038207086533624,"velocity":1.4794679615105544,"acceleration":0.34110673226584154,"pose":{"translation":{"x":0.7500956575870513,"y":-0.14017649102210997},"rotation":{"radians":-0.40292427332100755}},"curvature":-0.46006703591423725},{"time":0.8504194911370777,"velocity":1.4953631199311412,"acceleration":0.316223700987995,"pose":{"translation":{"x":0.8134279479458928,"y":-0.1683392347767949},"rotation":{"radians":-0.43348589624470896}},"curvature":-0.42236918942041696},{"time":0.8981565124814388,"velocity":1.510458697494798,"acceleration":0.28785953309578993,"pose":{"translation":{"x":0.8780859375000001,"y":-0.1994296875},"rotation":{"radians":-0.46250735863979175}},"curvature":-0.3873022288183412},{"time":0.9470356598236636,"velocity":1.5245290260268511,"acceleration":0.2582886030433786,"pose":{"translation":{"x":0.9439984834715724,"y":-0.23344864969700577},"rotation":{"radians":-0.49002121151097894}},"curvature":-0.3552422229542392},{"time":0.9970525446129715,"velocity":1.5374478173276631,"acceleration":0.22907304524839023,"pose":{"translation":{"x":1.011080272436142,"y":-0.27037839102745054},"rotation":{"radians":-0.5160916282809809}},"curvature":-0.32632279408302584},{"time":1.0481931014539378,"velocity":1.5491627404189217,"acceleration":0.20120691663021545,"pose":{"translation":{"x":1.079232842244208,"y":-0.31018367222696547},"rotation":{"radians":-0.5408038933963396}},"curvature":-0.3005152946982005},{"time":1.1004327744652074,"velocity":1.55967372395129,"acceleration":0.1752545092620447,"pose":{"translation":{"x":1.1483456039428712,"y":-0.3528127670288086},"rotation":{"radians":-0.5642563851793583}},"curvature":-0.2776899960695386},{"time":1.1537361868964333,"velocity":1.5690153873389168,"acceleration":0.15147085717835027,"pose":{"translation":{"x":1.2182968636974691,"y":-0.3981984840855003},"rotation":{"radians":-0.5865546641462306}},"curvature":-0.2576606317119665},{"time":1.208057193853282,"velocity":1.5772434368254618,"acceleration":0.12989944308354476,"pose":{"translation":{"x":1.288954844713211,"y":-0.4462591888904572},"rotation":{"radians":-0.6078072654168749}},"curvature":-0.24021547502233934},{"time":1.2633392297495507,"velocity":1.5844245425009116,"acceleration":0.11044695104753953,"pose":{"translation":{"x":1.3601787091568114,"y":-0.4968998256996274},"rotation":{"radians":-0.6281228263232282}},"curvature":-0.2251381277104783},{"time":1.319515876771435,"velocity":1.5906290818845525,"acceleration":0.09293763772828834,"pose":{"translation":{"x":1.431819580078125,"y":-0.550012939453125},"rotation":{"radians":-0.6476082331693351}},"curvature":-0.21222079108774547},{"time":1.3765115953423626,"velocity":1.595926129329161,"acceleration":0.07715101466312033,"pose":{"translation":{"x":1.5037215633317829,"y":-0.6054796976968646},"rotation":{"radians":-0.6663675289892361}},"curvature":-0.20127225459593204},{"time":1.434242570829637,"velocity":1.6003801326654958,"acceleration":0.06284663997261843,"pose":{"translation":{"x":1.5757227694988252,"y":-0.6631709125041961},"rotation":{"radians":-0.6845013786777033}},"curvature":-0.19212230898477914},{"time":1.4926176419327786,"velocity":1.604048809742491,"acceleration":0.049779393953850636,"pose":{"translation":{"x":1.647656335808337,"y":-0.7229480623975397},"rotation":{"radians":-0.7021069351713791}},"curvature":-0.18462383884095865},{"time":1.5515392852483902,"velocity":1.606981893437507,"acceleration":0.037707987143503895,"pose":{"translation":{"x":1.719351448059082,"y":-0.7846643142700194},"rotation":{"radians":-0.7192779892828585}},"curvature":-0.1786534886681594},{"time":1.610904637591002,"velocity":1.6092204413804119,"acceleration":0.026398811602806938,"pose":{"translation":{"x":1.7906343625411392,"y":-0.8481655453070999},"rotation":{"radians":-0.73610531663522}},"curvature":-0.1741115236266933},{"time":1.6706065430426809,"velocity":1.6107965007347593,"acceleration":0.015626689192205216,"pose":{"translation":{"x":1.8613294279575348,"y":-0.9132913649082184},"rotation":{"radians":-0.7526771588590878}},"curvature":-0.17092130635439215},{"time":1.7305346157034076,"velocity":1.6117329781001164,"acceleration":0.005173627968943801,"pose":{"translation":{"x":1.9312601073458793,"y":-0.9798761366084217},"rotation":{"radians":-0.769079793937514}},"curvature":-0.16902866891701063},{"time":1.7905763119866465,"velocity":1.6120436114993102,"acceleration":-0.005173627968943801,"pose":{"translation":{"x":2.0002500000000003,"y":-1.04775},"rotation":{"radians":-0.7853981633974483}},"curvature":-0.16840135895962896},{"time":1.8506180082698853,"velocity":1.6117329781001164,"acceleration":-0.01562668919220529,"pose":{"translation":{"x":2.0681238633915786,"y":-1.116739892654121},"rotation":{"radians":-0.8017165328573825}},"curvature":-0.16902866891701068},{"time":1.9105460809306118,"velocity":1.6107965007347593,"acceleration":-0.026398811602806938,"pose":{"translation":{"x":2.1347086350917817,"y":-1.1866705720424653},"rotation":{"radians":-0.8181191679358089}},"curvature":-0.1709213063543919},{"time":1.9702479863822906,"velocity":1.6092204413804119,"acceleration":-0.03770798714350157,"pose":{"translation":{"x":2.1998344546929003,"y":-1.2573656374588609},"rotation":{"radians":-0.8346910101596765}},"curvature":-0.1741115236266933},{"time":2.0296133387249005,"velocity":1.6069818934375073,"acceleration":-0.049779393953852634,"pose":{"translation":{"x":2.263335685729981,"y":-1.3286485519409177},"rotation":{"radians":-0.8515183375120382}},"curvature":-0.1786534886681591},{"time":2.0885349820405144,"velocity":1.604048809742491,"acceleration":-0.06284663997261858,"pose":{"translation":{"x":2.3250519376024608,"y":-1.4003436641916633},"rotation":{"radians":-0.8686893916235177}},"curvature":-0.1846238388409585},{"time":2.146910053143656,"velocity":1.6003801326654958,"acceleration":-0.0771510146631207,"pose":{"translation":{"x":2.3848290874958042,"y":-1.472277230501175},"rotation":{"radians":-0.8862949481171933}},"curvature":-0.19212230898477875},{"time":2.2046410286309297,"velocity":1.595926129329161,"acceleration":-0.09293763772828544,"pose":{"translation":{"x":2.4425203023031354,"y":-1.544278436668217},"rotation":{"radians":-0.9044287978056609}},"curvature":-0.20127225459593212},{"time":2.2616367472018566,"velocity":1.5906290818845528,"acceleration":-0.11044695104754229,"pose":{"translation":{"x":2.4979870605468752,"y":-1.616180419921875},"rotation":{"radians":-0.9231880936255614}},"curvature":-0.21222079108774536},{"time":2.3178133942237413,"velocity":1.5844245425009116,"acceleration":-0.1298994430835441,"pose":{"translation":{"x":2.551100174300373,"y":-1.6878212908431882},"rotation":{"radians":-0.9426735004716682}},"curvature":-0.2251381277104782},{"time":2.3730954301200105,"velocity":1.5772434368254618,"acceleration":-0.15147085717835143,"pose":{"translation":{"x":2.601740811109543,"y":-1.7590451552867892},"rotation":{"radians":-0.962989061378022}},"curvature":-0.24021547502233934},{"time":2.4274164370768587,"velocity":1.5690153873389168,"acceleration":-0.17525450926204203,"pose":{"translation":{"x":2.6498015159145,"y":-1.8297031363025305},"rotation":{"radians":-0.9842416626486657}},"curvature":-0.2576606317119665},{"time":2.480719849508084,"velocity":1.5596737239512901,"acceleration":-0.20120691663021434,"pose":{"translation":{"x":2.6951872329711914,"y":-1.8996543960571284},"rotation":{"radians":-1.0065399416155383}},"curvature":-0.2776899960695381},{"time":2.532959522519354,"velocity":1.5491627404189219,"acceleration":-0.22907304524839303,"pose":{"translation":{"x":2.7378163277730345,"y":-1.9687671577557921},"rotation":{"radians":-1.029992433398557}},"curvature":-0.3005152946982001},{"time":2.584100079360321,"velocity":1.5374478173276631,"acceleration":-0.25828860304337636,"pose":{"translation":{"x":2.77762160897255,"y":-2.0369197275638573},"rotation":{"radians":-1.054704698513916}},"curvature":-0.326322794083026},{"time":2.634116964149629,"velocity":1.5245290260268511,"acceleration":-0.28785953309577883,"pose":{"translation":{"x":2.814551350302995,"y":-2.1040015165284274},"rotation":{"radians":-1.0807751152839182}},"curvature":-0.35524222295423874},{"time":2.682996111491854,"velocity":1.5104586974947984,"acceleration":-0.3162237009879981,"pose":{"translation":{"x":2.848570312500001,"y":-2.1699140625},"rotation":{"radians":-1.108288968155105}},"curvature":-0.38730222881834014},{"time":2.7307331328362148,"velocity":1.4953631199311417,"acceleration":-0.3411067322658357,"pose":{"translation":{"x":2.8796607652232056,"y":-2.2345720520541077},"rotation":{"radians":-1.137310430550188}},"curvature":-0.42236918942041607},{"time":2.7773319153199303,"velocity":1.479467961510555,"acceleration":-0.35939189955349155,"pose":{"translation":{"x":2.907823508977891,"y":-2.2979043424129486},"rotation":{"radians":-1.1678720534738884}},"curvature":-0.46006703591423553},{"time":2.82280253945257,"velocity":1.4631261875296429,"acceleration":-0.3670299175143166,"pose":{"translation":{"x":2.9330788970366135,"y":-2.3598549833670246},"rotation":{"radians":-1.199961464346393}},"curvature":-0.4996780318940104},{"time":2.867158456525288,"velocity":1.4468462389451713,"acceleration":-0.34453983730256904,"pose":{"translation":{"x":2.955467857360841,"y":-2.4203842391967765},"rotation":{"radians":-1.2335051991783754}},"curvature":-0.5400288901862721},{"time":2.952542691480283,"velocity":1.417427968525573,"acceleration":-0.22560478344741489,"pose":{"translation":{"x":2.9919192116260533,"y":-2.5371068565845487},"rotation":{"radians":-1.3042423436959323}},"curvature":-0.6152946879689668},{"time":3.033507923021858,"velocity":1.3991618249968663,"acceleration":-1.7117954491622256,"pose":{"translation":{"x":3.0179553222656255,"y":-2.6481174316406246},"rotation":{"radians":-1.3775353667204573}},"curvature":-0.6636207972685509},{"time":3.1139354696311403,"velocity":1.261486316723814,"acceleration":-2.700000000000002,"pose":{"translation":{"x":3.0347485377788566,"y":-2.7537860362529756},"rotation":{"radians":-1.4486681178031742}},"curvature":-0.6527862400129302},{"time":3.202826501397525,"velocity":1.0214805309545756,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":3.0438985061645507,"y":-2.8548402786254883},"rotation":{"radians":-1.5105685589576778}},"curvature":-0.5482112858960992},{"time":3.315037231470813,"velocity":0.7185115597566977,"acceleration":-2.7,"pose":{"translation":{"x":3.0474648764133487,"y":-2.952398004770279},"rotation":{"radians":-1.554360750792784}},"curvature":-0.32801399140063797},{"time":3.581152623973294,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.048000000000002,"y":-3.048},"rotation":{"radians":-1.5707963267948948}},"curvature":1.5296436709931115E-15}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.16253437312191035,"velocity":0.43884280742915804,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.235655291298622,"y":-2.299233917261998},"rotation":{"radians":0.050203030073159544}},"curvature":1.48318905993772},{"time":0.26475953875061276,"velocity":0.7148507546266544,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2944360548439127,"y":-2.2945352730858968},"rotation":{"radians":0.09912820467000355}},"curvature":0.4235442403261895},{"time":0.37690622350689507,"velocity":1.0176468034686166,"acceleration":2.699999999999998,"pose":{"translation":{"x":3.390973335372,"y":-2.28366884622584},"rotation":{"radians":0.12024382125741907}},"curvature":0.10475034952100395},{"time":0.43634063761718933,"velocity":1.178119721566411,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.455732936198907,"y":-2.2756673507299965},"rotation":{"radians":0.12502925945028812}},"curvature":0.04928050745126129},{"time":0.4972142356672923,"velocity":1.342478436301689,"acceleration":2.6999999999999975,"pose":{"translation":{"x":3.5318392672810157,"y":-2.265991501645676},"rotation":{"radians":0.12749945581685104}},"curvature":0.018712898811020638},{"time":0.5589028508420205,"velocity":1.5090376972734552,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.6190725996188573,"y":-2.254762828521436},"rotation":{"radians":0.12827013383137298}},"curvature":6.382389439987909E-4},{"time":0.6208199608340345,"velocity":1.6762138942518927,"acceleration":0.20474125842093605,"pose":{"translation":{"x":3.716875586271945,"y":-2.2421666771189357},"rotation":{"radians":0.12771169861921847}},"curvature":-0.011011542191749546},{"time":0.6852310387503929,"velocity":1.689401499400737,"acceleration":-0.05305258852571156,"pose":{"translation":{"x":3.8243947783475503,"y":-2.22844328711919},"rotation":{"radians":0.12603866367906338}},"curvature":-0.019362727933097953},{"time":0.7545838260008766,"velocity":1.6857221545156258,"acceleration":-0.045806482602144795,"pose":{"translation":{"x":3.9405221409894784,"y":-2.2138788698288154},"rotation":{"radians":0.12336189171989319}},"curvature":-0.026141570374716056},{"time":0.8284141634951516,"velocity":1.6823402464456838,"acceleration":-0.0445672480823384,"pose":{"translation":{"x":4.063936569366847,"y":-2.198796685886281},"rotation":{"radians":0.11971802636802259}},"curvature":-0.032398566413562334},{"time":0.8667325821531024,"velocity":1.680632499975232,"acceleration":-0.04542286286009097,"pose":{"translation":{"x":4.127915543450831,"y":-2.191170385325926},"rotation":{"radians":0.11752858387876963}},"curvature":-0.03556770123464028},{"time":0.9058300099382188,"velocity":1.6788565828747664,"acceleration":-0.04739491083650032,"pose":{"translation":{"x":4.193145404662863,"y":-2.183548122968157},"rotation":{"radians":0.11508490225444941}},"curvature":-0.03887018160379375},{"time":0.9455848714943765,"velocity":1.6769724047559948,"acceleration":-0.050456872119439125,"pose":{"translation":{"x":4.259419576557454,"y":-2.1759767533084307},"rotation":{"radians":0.11237604173534803}},"curvature":-0.04238163050602945},{"time":0.9858730340914761,"velocity":1.6749395900879058,"acceleration":-0.05464666529357269,"pose":{"translation":{"x":4.326525950063592,"y":-2.1685037734953654},"rotation":{"radians":0.1093877910236385}},"curvature":-0.04617894557562544},{"time":1.0265686275990085,"velocity":1.6727157116105764,"acceleration":-0.06006268471382118,"pose":{"translation":{"x":4.394248180859394,"y":-2.1611770445090626},"rotation":{"radians":0.10610251404539708}},"curvature":-0.050343741049063785},{"time":1.0675448829204444,"velocity":1.670254567706452,"acceleration":-0.0668652748270841,"pose":{"translation":{"x":4.46236698674675,"y":-2.1540445123394276},"rotation":{"radians":0.10249890819515217}},"curvature":-0.05496580675182263},{"time":1.1086749898666093,"velocity":1.6675043918018293,"acceleration":-0.07528311421565198,"pose":{"translation":{"x":4.530661445025976,"y":-2.1471539291644883},"rotation":{"radians":0.09855167028286257}},"curvature":-0.0601468203169265},{"time":1.1498329761782073,"velocity":1.664405890417447,"acceleration":-0.08562453140808468,"pose":{"translation":{"x":4.598910289870464,"y":-2.140552574528716},"rotation":{"radians":0.09423106285647165}},"curvature":-0.06600455065774892},{"time":1.1908946101489744,"velocity":1.6608900072498498,"acceleration":-0.09829421482571662,"pose":{"translation":{"x":4.6668932097013265,"y":-2.1342869765213477},"rotation":{"radians":0.08950237001869939}},"curvature":-0.07267781348567748},{"time":1.231738330074408,"velocity":1.6568753058692178,"acceleration":-0.11381615220473237,"pose":{"translation":{"x":4.734392144562055,"y":-2.128402632954702},"rotation":{"radians":0.08432522818272103}},"curvature":-0.08033248328318213},{"time":1.2722462045685698,"velocity":1.6522648554603,"acceleration":-0.1328639423684381,"pose":{"translation":{"x":4.801192583493158,"y":-2.1229437325425042},"rotation":{"radians":0.07865281336102048}},"curvature":-0.08916893229125794},{"time":1.312304928648127,"velocity":1.6469424954528407,"acceleration":-0.15629981595439885,"pose":{"translation":{"x":4.86708486190682,"y":-2.117952876078204},"rotation":{"radians":0.07243086257884969}},"curvature":-0.09943135873654044},{"time":1.3518068613474916,"velocity":1.640768350642087,"acceleration":-0.18522365479157954,"pose":{"translation":{"x":4.931865458961542,"y":-2.1134707976132963},"rotation":{"radians":0.0655965030028932}},"curvature":-0.11141958736259769},{"time":1.3906511114248492,"velocity":1.6335734766751207,"acceleration":-0.24295562039015778,"pose":{"translation":{"x":4.9953382949368,"y":-2.109536085635641},"rotation":{"radians":0.058076858751224265}},"curvature":-0.12550407841834513},{"time":1.466022363588721,"velocity":1.6152616073460642,"acceleration":-0.3546562825081202,"pose":{"translation":{"x":5.117621354619559,"y":-2.1034507143452768},"rotation":{"radians":0.040630021789150914}},"curvature":-0.1619169599168633},{"time":1.537780719636484,"velocity":1.5898120555512705,"acceleration":-0.5275364639006982,"pose":{"translation":{"x":5.232563525846965,"y":-2.0999519636134742},"rotation":{"radians":0.019237266076289655}},"curvature":-0.21391600794008497},{"time":1.6054896443553452,"velocity":1.554093128830564,"acceleration":-0.7912251824906273,"pose":{"translation":{"x":5.338996383423962,"y":-2.099242861240941},"rotation":{"radians":-0.0072515924490507595}},"curvature":-0.28977018232314705},{"time":1.668946075148854,"velocity":1.503884802795766,"acceleration":-1.1751373492597204,"pose":{"translation":{"x":5.435995108057348,"y":-2.101465339128981},"rotation":{"radians":-0.04037803733154881}},"curvature":-0.40248682439394784},{"time":1.7282235242951927,"velocity":1.4342256583350599,"acceleration":-1.6778561230701603,"pose":{"translation":{"x":5.5229200023445495,"y":-2.1066913109857426},"rotation":{"radians":-0.08216377445719576}},"curvature":-0.5719401080374314},{"time":1.7836959264372831,"velocity":1.341150948739543,"acceleration":-2.20080945964638,"pose":{"translation":{"x":5.599458006762405,"y":-2.114913750032468},"rotation":{"radians":-0.1351143705125221}},"curvature":-0.825827101138403},{"time":1.8359975323484463,"velocity":1.226045079695558,"acceleration":-2.4790139320701097,"pose":{"translation":{"x":5.665664215655931,"y":-2.1260377667097456},"rotation":{"radians":-0.20191341877489544}},"curvature":-1.1931240150348847},{"time":1.885824507260314,"velocity":1.1025233146961304,"acceleration":-2.311980572815219,"pose":{"translation":{"x":5.722003393227111,"y":-2.139871686383754},"rotation":{"radians":-0.284297039501025}},"curvature":-1.6725848958928446},{"time":1.9099737856104797,"velocity":1.0466906523030397,"acceleration":-1.893950979741696,"pose":{"translation":{"x":5.746740320006809,"y":-2.147716301131953},"rotation":{"radians":-0.3309830197554157}},"curvature":-1.9264401293459859},{"time":1.9335585280660952,"velocity":1.002022306222271,"acceleration":-1.1632371776425636,"pose":{"translation":{"x":5.769391489523653,"y":-2.156118127052516},"rotation":{"radians":-0.380302946666284}},"curvature":-2.149904376291824},{"time":1.9564021415443753,"velocity":0.9754497657526388,"acceleration":0.011912567692432518,"pose":{"translation":{"x":5.790148704538905,"y":-2.1650220108051035},"rotation":{"radians":-0.4306688379447418}},"curvature":-2.2925487642707947},{"time":1.978186463436481,"velocity":0.9757092729618122,"acceleration":2.699999999999997,"pose":{"translation":{"x":5.809237156427796,"y":-2.1743650770521468},"rotation":{"radians":-0.47969473744756086}},"curvature":-2.291118132066251},{"time":2.016638423599818,"velocity":1.079529565402822,"acceleration":2.699999999999993,"pose":{"translation":{"x":5.843483263645056,"y":-2.194076972763101},"rotation":{"radians":-0.5605108950017286}},"curvature":-1.622465522268993},{"time":2.0498208647278267,"velocity":1.1691221564484455,"acceleration":2.6999999999999957,"pose":{"translation":{"x":5.874648414693021,"y":-2.2145857763164267},"rotation":{"radians":-0.5933810296164905}},"curvature":-3.247134184938767E-14},{"time":2.109378975559902,"velocity":1.329929055695049,"acceleration":2.699999999999996,"pose":{"translation":{"x":5.936258944700883,"y":-2.2563276903425025},"rotation":{"radians":-0.5994171739895844}},"curvature":-0.1487431329178877},{"time":2.164360211611288,"velocity":1.4783783930337904,"acceleration":1.8517760132138024,"pose":{"translation":{"x":5.999699981882348,"y":-2.300320873723265},"rotation":{"radians":-0.6141274879698528}},"curvature":-0.22030767448755514},{"time":2.2175748158508273,"velocity":1.5769199207172349,"acceleration":-0.014315562132053861,"pose":{"translation":{"x":6.065697950852353,"y":-2.347785174236805},"rotation":{"radians":-0.6331059611884475}},"curvature":-0.24089795741624495},{"time":2.2716979386639586,"velocity":1.5761451177898227,"acceleration":-0.02723730289590519,"pose":{"translation":{"x":6.133962927343928,"y":-2.3989766438490805},"rotation":{"radians":-0.6537784130941335}},"curvature":-0.24253360344491967},{"time":2.327683437678907,"velocity":1.5746202237953741,"acceleration":-0.5520101968227636,"pose":{"translation":{"x":6.203395553508525,"y":-2.453365069212477},"rotation":{"radians":-0.675248684023209}},"curvature":-0.24575742904074352},{"time":2.3848207185559724,"velocity":1.5430798621325077,"acceleration":-2.7,"pose":{"translation":{"x":6.272293953216331,"y":-2.509811502164368},"rotation":{"radians":-0.6977715414955855}},"curvature":-0.2633146109498168},{"time":2.4445608693304597,"velocity":1.381781455041392,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.338560647356597,"y":-2.5667457902256756},"rotation":{"radians":-0.7224590858174899}},"curvature":-0.30800993487295275},{"time":2.5084695729111095,"velocity":1.2092279553736378,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.399909469137951,"y":-2.6223441070994333},"rotation":{"radians":-0.7513632514930497}},"curvature":-0.40265674918292893},{"time":2.5758368121059863,"velocity":1.027336409547471,"acceleration":-2.7,"pose":{"translation":{"x":6.454072479388717,"y":-2.6747064831693455},"rotation":{"radians":-0.7880733683333389}},"curvature":-0.6004987770491809},{"time":2.645792282217101,"velocity":0.8384566402474619,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.499006881857241,"y":-2.722034335998349},"rotation":{"radians":-0.8393292542522007}},"curvature":-1.0462955538581296},{"time":2.7174508148262406,"velocity":0.6449786022027844,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.533101938512206,"y":-2.762808000827175},"rotation":{"radians":-0.9192964022695238}},"curvature":-2.204183013486819},{"time":2.753784984556833,"velocity":0.5468763439301851,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.545750418006628,"y":-2.780382073202902},"rotation":{"radians":-0.9791815976966345}},"curvature":-3.485726715025795},{"time":2.7906382659669196,"velocity":0.44737248412295083,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.555385884842951,"y":-2.79596426107291},"rotation":{"radians":-1.061990677947966}},"curvature":-5.881009321372131},{"time":2.809457424798199,"velocity":0.3965607552784971,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.559075092744607,"y":-2.8029963396245963},"rotation":{"radians":-1.1158613221217693}},"curvature":-7.827718392765945},{"time":2.8287835191728945,"velocity":0.34438030046681856,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.562020655111404,"y":-2.8095221084893103},"rotation":{"radians":-1.1809980974647505}},"curvature":-10.569436189018688},{"time":2.8490238264989167,"velocity":0.28973147068655825,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.5642344432613005,"y":-2.8155454789102445},"rotation":{"radians":-1.2602788043979845}},"curvature":-14.417182716046018},{"time":2.859729139788011,"velocity":0.26082712480600356,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.56507186128735,"y":-2.818370943824043},"rotation":{"radians":-1.306244968667926}},"curvature":-16.86263361015955},{"time":2.87106221383231,"velocity":0.23022782488639748,"acceleration":-2.3612718102537897,"pose":{"translation":{"x":6.565732845159796,"y":-2.8210738788275567},"rotation":{"radians":-1.3569924039235672}},"curvature":-19.70367342021235},{"time":2.8832346538097156,"velocity":0.2014853855057428,"acceleration":-1.8823900772771511,"pose":{"translation":{"x":6.566220182251913,"y":-2.823655790340661},"rotation":{"radians":-1.412912280427388}},"curvature":-22.95474232463341},{"time":2.8963628627940183,"velocity":0.17677297518127044,"acceleration":-1.4714194647731251,"pose":{"translation":{"x":6.5665369674856615,"y":-2.826118426247995},"rotation":{"radians":-1.474319022768083}},"curvature":-26.59523025516007},{"time":2.9104878075921956,"velocity":0.1559892564663864,"acceleration":-1.1254439629148367,"pose":{"translation":{"x":6.566686609646243,"y":-2.8284637813167604},"rotation":{"radians":-1.541389152233492}},"curvature":-30.54995815591113},{"time":2.925611064449408,"velocity":0.13896887833682636,"acceleration":-0.8377391423775556,"pose":{"translation":{"x":6.566672837696655,"y":-2.830694102614526},"rotation":{"radians":-1.6140863178872185}},"curvature":-34.669613418093725},{"time":2.941679457699172,"velocity":0.12550775635638367,"acceleration":-0.5989931096992901,"pose":{"translation":{"x":6.566499707092241,"y":-2.8328118949270262},"rotation":{"radians":-1.6920812424865532}},"curvature":-38.71907285207275},{"time":2.958571811664889,"velocity":0.11538935272431772,"acceleration":-0.39836294880725803,"pose":{"translation":{"x":6.56617160609525,"y":-2.8348199261759635},"rotation":{"radians":-1.7746838185185858}},"curvature":-42.38496435964513},{"time":2.9760924888977693,"velocity":0.10840976407672742,"acceleration":-0.2650213103080076,"pose":{"translation":{"x":6.565693262089384,"y":-2.8367212328368074},"rotation":{"radians":-1.8608117438581422}},"curvature":-45.31248254996997},{"time":2.985025684936199,"velocity":0.10604227675738442,"acceleration":-0.1827783906326452,"pose":{"translation":{"x":6.565399320548026,"y":-2.8376328881112363},"rotation":{"radians":-1.9047580036990877}},"curvature":-46.393030714121075},{"time":2.9940118538728644,"velocity":0.1043997992611877,"acceleration":-0.10338126455167747,"pose":{"translation":{"x":6.5650697478943565,"y":-2.838519125356596},"rotation":{"radians":-1.9490200030342122}},"curvature":-47.171469796300315},{"time":3.0030101711910997,"velocity":0.10346954183799129,"acceleration":-0.025273988080138532,"pose":{"translation":{"x":6.5647052332412,"y":-2.8393804020756814},"rotation":{"radians":-1.9933772497009323}},"curvature":-47.623320398873744},{"time":3.011978572681497,"velocity":0.10324287456562509,"acceleration":0.09326386027098398,"pose":{"translation":{"x":6.564306488080446,"y":-2.8402171935717373},"rotation":{"radians":-2.037602908125817}},"curvature":-47.73465242571826},{"time":3.0296236200714812,"velocity":0.10488851979987945,"acceleration":0.26257573607129786,"pose":{"translation":{"x":6.5634092652830445,"y":-2.841819312125811},"rotation":{"radians":-2.1247564340256364}},"curvature":-46.93729676286493},{"time":3.046662742188624,"velocity":0.10936257983179687,"acceleration":0.45877295584260974,"pose":{"translation":{"x":6.562384226517221,"y":-2.843329645887367},"rotation":{"radians":-2.208763935210407}},"curvature":-44.89080964582525},{"time":3.062823621750542,"velocity":0.1167767543174345,"acceleration":0.7007886401669247,"pose":{"translation":{"x":6.561237889492266,"y":-2.844752655367728},"rotation":{"radians":-2.288157818489723}},"curvature":-41.84472781061636},{"time":3.077899735774915,"velocity":0.12734192376357656,"acceleration":1.0140300260544806,"pose":{"translation":{"x":6.55997714892625,"y":-2.8460931021387887},"rotation":{"radians":-2.361820387383494}},"curvature":-38.11692809320769},{"time":3.0917554325870618,"velocity":0.14139201636300058,"acceleration":1.4356063758184543,"pose":{"translation":{"x":6.558609282860578,"y":-2.84735605425082},"rotation":{"radians":-2.4290117557070676}},"curvature":-34.022560762308004},{"time":3.104319349300483,"velocity":0.15942885530204032,"acceleration":2.0243220425740582,"pose":{"translation":{"x":6.557141958974539,"y":-2.8485468916502654},"rotation":{"radians":-2.489336635086955}},"curvature":-29.824269910378586},{"time":3.11557089520828,"velocity":0.18220560769622773,"acceleration":2.700000000000096,"pose":{"translation":{"x":6.555583240899867,"y":-2.849671311597545},"rotation":{"radians":-2.5426742752711875}},"curvature":-25.71024378865562},{"time":3.134558205623386,"velocity":0.2334713458170163,"acceleration":-1.1102273481572658,"pose":{"translation":{"x":6.55222589436107,"y":-2.851745307253974},"rotation":{"radians":-2.6287861328487576}},"curvature":-18.12857913974966},{"time":3.1528165809971296,"velocity":0.21320039814416503,"acceleration":-2.7,"pose":{"translation":{"x":6.548609911299899,"y":-2.8536301714594146},"rotation":{"radians":-2.6889048955956647}},"curvature":-11.538793868227126},{"time":3.2317796914208943,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.540926606751496,"y":-2.857068318658525},"rotation":{"radians":-2.736700867304774}},"curvature":-1.0037780381732805E-12}] \ No newline at end of file From 2a3ee216ae266ce0ab44ce82e7d9c1346858760f Mon Sep 17 00:00:00 2001 From: Kyra <101209@psdschools.org> Date: Wed, 11 Mar 2020 17:49:09 -0600 Subject: [PATCH 31/39] Added the ten ball auto command --- .../java/frc4388/robot/RobotContainer.java | 12 +++++++- .../commands/auto/TenBallAutoMiddle.java | 28 +++++++++++++++++++ 2 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5bb9e90..771b2f1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc4388.robot.commands.auto.DriveOffLineForward; import frc4388.robot.commands.auto.EightBallAutoMiddle; import frc4388.robot.commands.auto.FiveBallAutoMiddle; import frc4388.robot.commands.auto.SixBallAutoMiddle; +import frc4388.robot.commands.auto.TenBallAutoMiddle; import frc4388.robot.commands.InterruptSubystem; import frc4388.robot.commands.auto.AutoPath1FromCenter; import frc4388.robot.commands.auto.Wait; @@ -115,6 +116,8 @@ public class RobotContainer { FiveBallAutoMiddle m_fiveBallAutoMiddle; + TenBallAutoMiddle m_tenBallAutoMiddle; + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -295,6 +298,12 @@ public class RobotContainer { }; m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); + + String[] tenBallAutoMiddlePaths = new String[]{ + "TenBallMidComplete" + }; + + m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); } /** @@ -316,7 +325,8 @@ public class RobotContainer { //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); diff --git a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java new file mode 100644 index 0000000..7ff19da --- /dev/null +++ b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class TenBallAutoMiddle extends SequentialCommandGroup { + /** + * Creates a new TenBallAutoMiddle. + */ + public TenBallAutoMiddle(Drive drive, RamseteCommand[] paths) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + addCommands( + paths[0] + ); + } +} From 9f4d0d47a013f3c86816abe04ad465b278443e4b Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 17:57:03 -0600 Subject: [PATCH 32/39] working on 5 ball auto --- PathWeaver/Paths/FiveBallMidComplete | 5 +++-- src/main/deploy/paths/FiveBallMidComplete.wpilib.json | 2 +- .../java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java | 3 ++- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/PathWeaver/Paths/FiveBallMidComplete b/PathWeaver/Paths/FiveBallMidComplete index c2f4041..6783265 100644 --- a/PathWeaver/Paths/FiveBallMidComplete +++ b/PathWeaver/Paths/FiveBallMidComplete @@ -1,4 +1,5 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.3,0.5,0.0,true, -5.8746484146930245,-2.2145857763164236,0.9797262118384756,-0.6607896052402182,false, -6.5409266067515,-2.857068318658526,-0.24985432202192825,-0.10708042372368398,true, +5.910341889267586,-1.8695488554289974,0.2974456214546777,-0.16656954801461854,true, +6.374357058736883,-2.3097683751819202,-0.09518259886549707,-0.16656954801461987,true, +5.85085276497665,-2.4763379231965392,-0.22605867230555443,-0.11897824858187134,true, diff --git a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json index 8b8d170..859b771 100644 --- a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.16253437312191035,"velocity":0.43884280742915804,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.235655291298622,"y":-2.299233917261998},"rotation":{"radians":0.050203030073159544}},"curvature":1.48318905993772},{"time":0.26475953875061276,"velocity":0.7148507546266544,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2944360548439127,"y":-2.2945352730858968},"rotation":{"radians":0.09912820467000355}},"curvature":0.4235442403261895},{"time":0.37690622350689507,"velocity":1.0176468034686166,"acceleration":2.699999999999998,"pose":{"translation":{"x":3.390973335372,"y":-2.28366884622584},"rotation":{"radians":0.12024382125741907}},"curvature":0.10475034952100395},{"time":0.43634063761718933,"velocity":1.178119721566411,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.455732936198907,"y":-2.2756673507299965},"rotation":{"radians":0.12502925945028812}},"curvature":0.04928050745126129},{"time":0.4972142356672923,"velocity":1.342478436301689,"acceleration":2.6999999999999975,"pose":{"translation":{"x":3.5318392672810157,"y":-2.265991501645676},"rotation":{"radians":0.12749945581685104}},"curvature":0.018712898811020638},{"time":0.5589028508420205,"velocity":1.5090376972734552,"acceleration":2.699999999999999,"pose":{"translation":{"x":3.6190725996188573,"y":-2.254762828521436},"rotation":{"radians":0.12827013383137298}},"curvature":6.382389439987909E-4},{"time":0.6208199608340345,"velocity":1.6762138942518927,"acceleration":0.20474125842093605,"pose":{"translation":{"x":3.716875586271945,"y":-2.2421666771189357},"rotation":{"radians":0.12771169861921847}},"curvature":-0.011011542191749546},{"time":0.6852310387503929,"velocity":1.689401499400737,"acceleration":-0.05305258852571156,"pose":{"translation":{"x":3.8243947783475503,"y":-2.22844328711919},"rotation":{"radians":0.12603866367906338}},"curvature":-0.019362727933097953},{"time":0.7545838260008766,"velocity":1.6857221545156258,"acceleration":-0.045806482602144795,"pose":{"translation":{"x":3.9405221409894784,"y":-2.2138788698288154},"rotation":{"radians":0.12336189171989319}},"curvature":-0.026141570374716056},{"time":0.8284141634951516,"velocity":1.6823402464456838,"acceleration":-0.0445672480823384,"pose":{"translation":{"x":4.063936569366847,"y":-2.198796685886281},"rotation":{"radians":0.11971802636802259}},"curvature":-0.032398566413562334},{"time":0.8667325821531024,"velocity":1.680632499975232,"acceleration":-0.04542286286009097,"pose":{"translation":{"x":4.127915543450831,"y":-2.191170385325926},"rotation":{"radians":0.11752858387876963}},"curvature":-0.03556770123464028},{"time":0.9058300099382188,"velocity":1.6788565828747664,"acceleration":-0.04739491083650032,"pose":{"translation":{"x":4.193145404662863,"y":-2.183548122968157},"rotation":{"radians":0.11508490225444941}},"curvature":-0.03887018160379375},{"time":0.9455848714943765,"velocity":1.6769724047559948,"acceleration":-0.050456872119439125,"pose":{"translation":{"x":4.259419576557454,"y":-2.1759767533084307},"rotation":{"radians":0.11237604173534803}},"curvature":-0.04238163050602945},{"time":0.9858730340914761,"velocity":1.6749395900879058,"acceleration":-0.05464666529357269,"pose":{"translation":{"x":4.326525950063592,"y":-2.1685037734953654},"rotation":{"radians":0.1093877910236385}},"curvature":-0.04617894557562544},{"time":1.0265686275990085,"velocity":1.6727157116105764,"acceleration":-0.06006268471382118,"pose":{"translation":{"x":4.394248180859394,"y":-2.1611770445090626},"rotation":{"radians":0.10610251404539708}},"curvature":-0.050343741049063785},{"time":1.0675448829204444,"velocity":1.670254567706452,"acceleration":-0.0668652748270841,"pose":{"translation":{"x":4.46236698674675,"y":-2.1540445123394276},"rotation":{"radians":0.10249890819515217}},"curvature":-0.05496580675182263},{"time":1.1086749898666093,"velocity":1.6675043918018293,"acceleration":-0.07528311421565198,"pose":{"translation":{"x":4.530661445025976,"y":-2.1471539291644883},"rotation":{"radians":0.09855167028286257}},"curvature":-0.0601468203169265},{"time":1.1498329761782073,"velocity":1.664405890417447,"acceleration":-0.08562453140808468,"pose":{"translation":{"x":4.598910289870464,"y":-2.140552574528716},"rotation":{"radians":0.09423106285647165}},"curvature":-0.06600455065774892},{"time":1.1908946101489744,"velocity":1.6608900072498498,"acceleration":-0.09829421482571662,"pose":{"translation":{"x":4.6668932097013265,"y":-2.1342869765213477},"rotation":{"radians":0.08950237001869939}},"curvature":-0.07267781348567748},{"time":1.231738330074408,"velocity":1.6568753058692178,"acceleration":-0.11381615220473237,"pose":{"translation":{"x":4.734392144562055,"y":-2.128402632954702},"rotation":{"radians":0.08432522818272103}},"curvature":-0.08033248328318213},{"time":1.2722462045685698,"velocity":1.6522648554603,"acceleration":-0.1328639423684381,"pose":{"translation":{"x":4.801192583493158,"y":-2.1229437325425042},"rotation":{"radians":0.07865281336102048}},"curvature":-0.08916893229125794},{"time":1.312304928648127,"velocity":1.6469424954528407,"acceleration":-0.15629981595439885,"pose":{"translation":{"x":4.86708486190682,"y":-2.117952876078204},"rotation":{"radians":0.07243086257884969}},"curvature":-0.09943135873654044},{"time":1.3518068613474916,"velocity":1.640768350642087,"acceleration":-0.18522365479157954,"pose":{"translation":{"x":4.931865458961542,"y":-2.1134707976132963},"rotation":{"radians":0.0655965030028932}},"curvature":-0.11141958736259769},{"time":1.3906511114248492,"velocity":1.6335734766751207,"acceleration":-0.24295562039015778,"pose":{"translation":{"x":4.9953382949368,"y":-2.109536085635641},"rotation":{"radians":0.058076858751224265}},"curvature":-0.12550407841834513},{"time":1.466022363588721,"velocity":1.6152616073460642,"acceleration":-0.3546562825081202,"pose":{"translation":{"x":5.117621354619559,"y":-2.1034507143452768},"rotation":{"radians":0.040630021789150914}},"curvature":-0.1619169599168633},{"time":1.537780719636484,"velocity":1.5898120555512705,"acceleration":-0.5275364639006982,"pose":{"translation":{"x":5.232563525846965,"y":-2.0999519636134742},"rotation":{"radians":0.019237266076289655}},"curvature":-0.21391600794008497},{"time":1.6054896443553452,"velocity":1.554093128830564,"acceleration":-0.7912251824906273,"pose":{"translation":{"x":5.338996383423962,"y":-2.099242861240941},"rotation":{"radians":-0.0072515924490507595}},"curvature":-0.28977018232314705},{"time":1.668946075148854,"velocity":1.503884802795766,"acceleration":-1.1751373492597204,"pose":{"translation":{"x":5.435995108057348,"y":-2.101465339128981},"rotation":{"radians":-0.04037803733154881}},"curvature":-0.40248682439394784},{"time":1.7282235242951927,"velocity":1.4342256583350599,"acceleration":-1.6778561230701603,"pose":{"translation":{"x":5.5229200023445495,"y":-2.1066913109857426},"rotation":{"radians":-0.08216377445719576}},"curvature":-0.5719401080374314},{"time":1.7836959264372831,"velocity":1.341150948739543,"acceleration":-2.20080945964638,"pose":{"translation":{"x":5.599458006762405,"y":-2.114913750032468},"rotation":{"radians":-0.1351143705125221}},"curvature":-0.825827101138403},{"time":1.8359975323484463,"velocity":1.226045079695558,"acceleration":-2.4790139320701097,"pose":{"translation":{"x":5.665664215655931,"y":-2.1260377667097456},"rotation":{"radians":-0.20191341877489544}},"curvature":-1.1931240150348847},{"time":1.885824507260314,"velocity":1.1025233146961304,"acceleration":-2.311980572815219,"pose":{"translation":{"x":5.722003393227111,"y":-2.139871686383754},"rotation":{"radians":-0.284297039501025}},"curvature":-1.6725848958928446},{"time":1.9099737856104797,"velocity":1.0466906523030397,"acceleration":-1.893950979741696,"pose":{"translation":{"x":5.746740320006809,"y":-2.147716301131953},"rotation":{"radians":-0.3309830197554157}},"curvature":-1.9264401293459859},{"time":1.9335585280660952,"velocity":1.002022306222271,"acceleration":-1.1632371776425636,"pose":{"translation":{"x":5.769391489523653,"y":-2.156118127052516},"rotation":{"radians":-0.380302946666284}},"curvature":-2.149904376291824},{"time":1.9564021415443753,"velocity":0.9754497657526388,"acceleration":0.011912567692432518,"pose":{"translation":{"x":5.790148704538905,"y":-2.1650220108051035},"rotation":{"radians":-0.4306688379447418}},"curvature":-2.2925487642707947},{"time":1.978186463436481,"velocity":0.9757092729618122,"acceleration":2.699999999999997,"pose":{"translation":{"x":5.809237156427796,"y":-2.1743650770521468},"rotation":{"radians":-0.47969473744756086}},"curvature":-2.291118132066251},{"time":2.016638423599818,"velocity":1.079529565402822,"acceleration":2.699999999999993,"pose":{"translation":{"x":5.843483263645056,"y":-2.194076972763101},"rotation":{"radians":-0.5605108950017286}},"curvature":-1.622465522268993},{"time":2.0498208647278267,"velocity":1.1691221564484455,"acceleration":2.6999999999999957,"pose":{"translation":{"x":5.874648414693021,"y":-2.2145857763164267},"rotation":{"radians":-0.5933810296164905}},"curvature":-3.247134184938767E-14},{"time":2.109378975559902,"velocity":1.329929055695049,"acceleration":2.699999999999996,"pose":{"translation":{"x":5.936258944700883,"y":-2.2563276903425025},"rotation":{"radians":-0.5994171739895844}},"curvature":-0.1487431329178877},{"time":2.164360211611288,"velocity":1.4783783930337904,"acceleration":1.8517760132138024,"pose":{"translation":{"x":5.999699981882348,"y":-2.300320873723265},"rotation":{"radians":-0.6141274879698528}},"curvature":-0.22030767448755514},{"time":2.2175748158508273,"velocity":1.5769199207172349,"acceleration":-0.014315562132053861,"pose":{"translation":{"x":6.065697950852353,"y":-2.347785174236805},"rotation":{"radians":-0.6331059611884475}},"curvature":-0.24089795741624495},{"time":2.2716979386639586,"velocity":1.5761451177898227,"acceleration":-0.02723730289590519,"pose":{"translation":{"x":6.133962927343928,"y":-2.3989766438490805},"rotation":{"radians":-0.6537784130941335}},"curvature":-0.24253360344491967},{"time":2.327683437678907,"velocity":1.5746202237953741,"acceleration":-0.5520101968227636,"pose":{"translation":{"x":6.203395553508525,"y":-2.453365069212477},"rotation":{"radians":-0.675248684023209}},"curvature":-0.24575742904074352},{"time":2.3848207185559724,"velocity":1.5430798621325077,"acceleration":-2.7,"pose":{"translation":{"x":6.272293953216331,"y":-2.509811502164368},"rotation":{"radians":-0.6977715414955855}},"curvature":-0.2633146109498168},{"time":2.4445608693304597,"velocity":1.381781455041392,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.338560647356597,"y":-2.5667457902256756},"rotation":{"radians":-0.7224590858174899}},"curvature":-0.30800993487295275},{"time":2.5084695729111095,"velocity":1.2092279553736378,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.399909469137951,"y":-2.6223441070994333},"rotation":{"radians":-0.7513632514930497}},"curvature":-0.40265674918292893},{"time":2.5758368121059863,"velocity":1.027336409547471,"acceleration":-2.7,"pose":{"translation":{"x":6.454072479388717,"y":-2.6747064831693455},"rotation":{"radians":-0.7880733683333389}},"curvature":-0.6004987770491809},{"time":2.645792282217101,"velocity":0.8384566402474619,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.499006881857241,"y":-2.722034335998349},"rotation":{"radians":-0.8393292542522007}},"curvature":-1.0462955538581296},{"time":2.7174508148262406,"velocity":0.6449786022027844,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.533101938512206,"y":-2.762808000827175},"rotation":{"radians":-0.9192964022695238}},"curvature":-2.204183013486819},{"time":2.753784984556833,"velocity":0.5468763439301851,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.545750418006628,"y":-2.780382073202902},"rotation":{"radians":-0.9791815976966345}},"curvature":-3.485726715025795},{"time":2.7906382659669196,"velocity":0.44737248412295083,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.555385884842951,"y":-2.79596426107291},"rotation":{"radians":-1.061990677947966}},"curvature":-5.881009321372131},{"time":2.809457424798199,"velocity":0.3965607552784971,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.559075092744607,"y":-2.8029963396245963},"rotation":{"radians":-1.1158613221217693}},"curvature":-7.827718392765945},{"time":2.8287835191728945,"velocity":0.34438030046681856,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.562020655111404,"y":-2.8095221084893103},"rotation":{"radians":-1.1809980974647505}},"curvature":-10.569436189018688},{"time":2.8490238264989167,"velocity":0.28973147068655825,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.5642344432613005,"y":-2.8155454789102445},"rotation":{"radians":-1.2602788043979845}},"curvature":-14.417182716046018},{"time":2.859729139788011,"velocity":0.26082712480600356,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.56507186128735,"y":-2.818370943824043},"rotation":{"radians":-1.306244968667926}},"curvature":-16.86263361015955},{"time":2.87106221383231,"velocity":0.23022782488639748,"acceleration":-2.3612718102537897,"pose":{"translation":{"x":6.565732845159796,"y":-2.8210738788275567},"rotation":{"radians":-1.3569924039235672}},"curvature":-19.70367342021235},{"time":2.8832346538097156,"velocity":0.2014853855057428,"acceleration":-1.8823900772771511,"pose":{"translation":{"x":6.566220182251913,"y":-2.823655790340661},"rotation":{"radians":-1.412912280427388}},"curvature":-22.95474232463341},{"time":2.8963628627940183,"velocity":0.17677297518127044,"acceleration":-1.4714194647731251,"pose":{"translation":{"x":6.5665369674856615,"y":-2.826118426247995},"rotation":{"radians":-1.474319022768083}},"curvature":-26.59523025516007},{"time":2.9104878075921956,"velocity":0.1559892564663864,"acceleration":-1.1254439629148367,"pose":{"translation":{"x":6.566686609646243,"y":-2.8284637813167604},"rotation":{"radians":-1.541389152233492}},"curvature":-30.54995815591113},{"time":2.925611064449408,"velocity":0.13896887833682636,"acceleration":-0.8377391423775556,"pose":{"translation":{"x":6.566672837696655,"y":-2.830694102614526},"rotation":{"radians":-1.6140863178872185}},"curvature":-34.669613418093725},{"time":2.941679457699172,"velocity":0.12550775635638367,"acceleration":-0.5989931096992901,"pose":{"translation":{"x":6.566499707092241,"y":-2.8328118949270262},"rotation":{"radians":-1.6920812424865532}},"curvature":-38.71907285207275},{"time":2.958571811664889,"velocity":0.11538935272431772,"acceleration":-0.39836294880725803,"pose":{"translation":{"x":6.56617160609525,"y":-2.8348199261759635},"rotation":{"radians":-1.7746838185185858}},"curvature":-42.38496435964513},{"time":2.9760924888977693,"velocity":0.10840976407672742,"acceleration":-0.2650213103080076,"pose":{"translation":{"x":6.565693262089384,"y":-2.8367212328368074},"rotation":{"radians":-1.8608117438581422}},"curvature":-45.31248254996997},{"time":2.985025684936199,"velocity":0.10604227675738442,"acceleration":-0.1827783906326452,"pose":{"translation":{"x":6.565399320548026,"y":-2.8376328881112363},"rotation":{"radians":-1.9047580036990877}},"curvature":-46.393030714121075},{"time":2.9940118538728644,"velocity":0.1043997992611877,"acceleration":-0.10338126455167747,"pose":{"translation":{"x":6.5650697478943565,"y":-2.838519125356596},"rotation":{"radians":-1.9490200030342122}},"curvature":-47.171469796300315},{"time":3.0030101711910997,"velocity":0.10346954183799129,"acceleration":-0.025273988080138532,"pose":{"translation":{"x":6.5647052332412,"y":-2.8393804020756814},"rotation":{"radians":-1.9933772497009323}},"curvature":-47.623320398873744},{"time":3.011978572681497,"velocity":0.10324287456562509,"acceleration":0.09326386027098398,"pose":{"translation":{"x":6.564306488080446,"y":-2.8402171935717373},"rotation":{"radians":-2.037602908125817}},"curvature":-47.73465242571826},{"time":3.0296236200714812,"velocity":0.10488851979987945,"acceleration":0.26257573607129786,"pose":{"translation":{"x":6.5634092652830445,"y":-2.841819312125811},"rotation":{"radians":-2.1247564340256364}},"curvature":-46.93729676286493},{"time":3.046662742188624,"velocity":0.10936257983179687,"acceleration":0.45877295584260974,"pose":{"translation":{"x":6.562384226517221,"y":-2.843329645887367},"rotation":{"radians":-2.208763935210407}},"curvature":-44.89080964582525},{"time":3.062823621750542,"velocity":0.1167767543174345,"acceleration":0.7007886401669247,"pose":{"translation":{"x":6.561237889492266,"y":-2.844752655367728},"rotation":{"radians":-2.288157818489723}},"curvature":-41.84472781061636},{"time":3.077899735774915,"velocity":0.12734192376357656,"acceleration":1.0140300260544806,"pose":{"translation":{"x":6.55997714892625,"y":-2.8460931021387887},"rotation":{"radians":-2.361820387383494}},"curvature":-38.11692809320769},{"time":3.0917554325870618,"velocity":0.14139201636300058,"acceleration":1.4356063758184543,"pose":{"translation":{"x":6.558609282860578,"y":-2.84735605425082},"rotation":{"radians":-2.4290117557070676}},"curvature":-34.022560762308004},{"time":3.104319349300483,"velocity":0.15942885530204032,"acceleration":2.0243220425740582,"pose":{"translation":{"x":6.557141958974539,"y":-2.8485468916502654},"rotation":{"radians":-2.489336635086955}},"curvature":-29.824269910378586},{"time":3.11557089520828,"velocity":0.18220560769622773,"acceleration":2.700000000000096,"pose":{"translation":{"x":6.555583240899867,"y":-2.849671311597545},"rotation":{"radians":-2.5426742752711875}},"curvature":-25.71024378865562},{"time":3.134558205623386,"velocity":0.2334713458170163,"acceleration":-1.1102273481572658,"pose":{"translation":{"x":6.55222589436107,"y":-2.851745307253974},"rotation":{"radians":-2.6287861328487576}},"curvature":-18.12857913974966},{"time":3.1528165809971296,"velocity":0.21320039814416503,"acceleration":-2.7,"pose":{"translation":{"x":6.548609911299899,"y":-2.8536301714594146},"rotation":{"radians":-2.6889048955956647}},"curvature":-11.538793868227126},{"time":3.2317796914208943,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":6.540926606751496,"y":-2.857068318658525},"rotation":{"radians":-2.736700867304774}},"curvature":-1.0037780381732805E-12}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.16408328867307112,"velocity":0.443024879417292,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2363298344524436,"y":-2.298899802686018},"rotation":{"radians":0.0692577651470374}},"curvature":1.8950587512445145},{"time":0.27165252251845773,"velocity":0.733461810799836,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.2992357901794267,"y":-2.292058382559469},"rotation":{"radians":0.13336588761066984}},"curvature":0.5081353245388296},{"time":0.39146527816103716,"velocity":1.0569562510348003,"acceleration":2.700000000000003,"pose":{"translation":{"x":3.4052753217803797,"y":-2.2759406702466065},"rotation":{"radians":0.1622880652133495}},"curvature":0.14308012858303878},{"time":0.45511058967808826,"velocity":1.2287985921308384,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.4770070197266802,"y":-2.2638785389701312},"rotation":{"radians":0.17018012232912796}},"curvature":0.08218998056047282},{"time":0.5202669048089674,"velocity":1.404720642984212,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.5615193955328435,"y":-2.249097623342679},"rotation":{"radians":0.17564390475973646}},"curvature":0.04917485937853854},{"time":0.5862126558482816,"velocity":1.5827741707903604,"acceleration":1.5865644228393085,"pose":{"translation":{"x":3.658474466495589,"y":-2.231684860998854},"rotation":{"radians":0.17945016920991327}},"curvature":0.030140843221192568},{"time":0.6537218964528062,"velocity":1.6898819301463979,"acceleration":0.05788196551324373,"pose":{"translation":{"x":3.7671397522485237,"y":-2.211813471951235},"rotation":{"radians":0.18207557565508717}},"curvature":0.018479758912245518},{"time":0.7254285040062793,"velocity":1.6940324495318797,"acceleration":0.04142453277389165,"pose":{"translation":{"x":3.8864378767263954,"y":-2.1897319350134796},"rotation":{"radians":0.18381956783311507}},"curvature":0.010872498721873012},{"time":0.7633704400602459,"velocity":1.6956041765054521,"acceleration":0.03427111458881184,"pose":{"translation":{"x":3.949655472782524,"y":-2.177957876460112},"rotation":{"radians":0.18442327195140867}},"curvature":0.008001487996222786},{"time":0.8025567568325975,"velocity":1.6969471352578709,"acceleration":0.02906850160180063,"pose":{"translation":{"x":4.014996170129343,"y":-2.1657529642234286},"rotation":{"radians":0.18487131031758927}},"curvature":0.0055525725273563005},{"time":0.8428636062198919,"velocity":1.698118794973849,"acceleration":0.0253231394733626,"pose":{"translation":{"x":4.082250383218097,"y":-2.1531645157563517},"rotation":{"radians":0.185176376794578}},"curvature":0.0034191885570685615},{"time":0.8841591774035676,"velocity":1.6991645284825654,"acceleration":0.016949828884694675,"pose":{"translation":{"x":4.151196270887151,"y":-2.140242485266206},"rotation":{"radians":0.185348127438105}},"curvature":0.001517578640164743},{"time":0.9263077178976394,"velocity":1.6998789390316795,"acceleration":-0.020915772434514122,"pose":{"translation":{"x":4.2216012864233665,"y":-2.127039119227935},"rotation":{"radians":0.185393608906121}},"curvature":-2.1980680822174232E-4},{"time":0.9691872552083657,"velocity":1.6989820803871911,"acceleration":-0.019853875112120833,"pose":{"translation":{"x":4.293223727623496,"y":-2.1136086118973263},"rotation":{"radians":0.18531757186828496}},"curvature":-0.001849182069837596},{"time":1.0126678049640467,"velocity":1.6981188229825355,"acceleration":-0.019438079067074412,"pose":{"translation":{"x":4.365814286855553,"y":-2.100006760824233},"rotation":{"radians":0.1851226905021804}},"curvature":-0.003419137593420175},{"time":1.0565953995481074,"velocity":1.697264954925784,"acceleration":-0.01961598803052627,"pose":{"translation":{"x":4.439117601120204,"y":-2.0862906223657953},"rotation":{"radians":0.18480970272003633}},"curvature":-0.004973588312268373},{"time":1.1008137297010812,"velocity":1.6963975686907735,"acceleration":-0.020376770398937767,"pose":{"translation":{"x":4.512873802112148,"y":-2.072518167199662},"rotation":{"radians":0.18437748064958973}},"curvature":-0.006554250817816681},{"time":1.1451650787587457,"velocity":1.6954938314341423,"acceleration":-0.02174865906857091,"pose":{"translation":{"x":4.586820066281499,"y":-2.0587479358372143},"rotation":{"radians":0.1838230366305232}},"curvature":-0.008202877188079381},{"time":1.1894912691710187,"velocity":1.6945297962310573,"acceleration":-0.023800679579925297,"pose":{"translation":{"x":4.6606921648951705,"y":-2.0450386941367853},"rotation":{"radians":0.1831414662100924}},"curvature":-0.00996343941748574},{"time":1.2336346217613723,"velocity":1.6934791544404706,"acceleration":-0.02664873108330119,"pose":{"translation":{"x":4.734226014098256,"y":-2.031449088816883},"rotation":{"radians":0.18232582600629615}},"curvature":-0.011884448939914765},{"time":1.2774389289250685,"velocity":1.692311825238575,"acceleration":-0.03046680385892747,"pose":{"translation":{"x":4.807159224975414,"y":-2.018037302969414},"rotation":{"radians":0.1813669405494641}},"curvature":-0.014021608840024123},{"time":1.3207504437966078,"velocity":1.6909922618101507,"acceleration":-0.03550490906982497,"pose":{"translation":{"x":4.87923265361225,"y":-2.0048607115729036},"rotation":{"radians":0.18025312799179333}},"curvature":-0.016441033887417626},{"time":1.3634188884165654,"velocity":1.6894773225637683,"acceleration":-0.04211637972877536,"pose":{"translation":{"x":4.950191951156699,"y":-1.9919755370057164},"rotation":{"radians":0.17896982951364038}},"curvature":-0.019223341480108017},{"time":1.4052984851902914,"velocity":1.687713505563158,"acceleration":-0.05079880035097404,"pose":{"translation":{"x":5.019789113880411,"y":-1.9794365045592819},"rotation":{"radians":0.1774991208800654}},"curvature":-0.022469026289744613},{"time":1.446249017581491,"velocity":1.6856332676439514,"acceleration":-0.06225527958543467,"pose":{"translation":{"x":5.087784033240128,"y":-1.9672964979513143},"rotation":{"radians":0.17581907627481713}},"curvature":-0.026305702065901784},{"time":1.48613692819827,"velocity":1.683150034616425,"acceleration":-0.07748664065103303,"pose":{"translation":{"x":5.153946045939074,"y":-1.955606214839035},"rotation":{"radians":0.1739029433768764}},"curvature":-0.030898057172033474},{"time":1.5248364654577076,"velocity":1.6801513374794417,"acceleration":-0.09793124518239772,"pose":{"translation":{"x":5.218055483988332,"y":-1.944413822332393},"rotation":{"radians":0.17171807338504597}},"curvature":-0.036461777406136126},{"time":1.5622308942193142,"velocity":1.676489254507933,"acceleration":-0.14430405981446515,"pose":{"translation":{"x":5.27990522476823,"y":-1.9337646125072896},"rotation":{"radians":0.16922452850747088}},"curvature":-0.04328332502065509},{"time":1.6327045242352638,"velocity":1.666319623586769,"acceleration":-2.5496325060863407,"pose":{"translation":{"x":5.396069151255783,"y":-1.9142604671143888},"rotation":{"radians":0.16310370436497154}},"curvature":-0.062384057405159905},{"time":1.700004059382801,"velocity":1.4947305411301082,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.501090654161787,"y":-1.897385524088994},"rotation":{"radians":0.15498967537946623}},"curvature":-0.09338424002657549},{"time":1.766850386843393,"velocity":1.3142454569865103,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.593922492082721,"y":-1.8833634811611886},"rotation":{"radians":0.14401549016527215}},"curvature":-0.14664144594343798},{"time":1.8327108342658995,"velocity":1.136422248945742,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":5.673866955415721,"y":-1.8723389647855806},"rotation":{"radians":0.12876563152136272}},"curvature":-0.24432674544242147},{"time":1.89674347147306,"velocity":0.9635341284864088,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.740625468322835,"y":-1.8643665065644082},"rotation":{"radians":0.10682738395944273}},"curvature":-0.43772210750117546},{"time":2.015399258918648,"velocity":0.6431635023833218,"acceleration":-2.7,"pose":{"translation":{"x":5.835683620117707,"y":-1.8572792752710763},"rotation":{"radians":0.02197361283131086}},"curvature":-1.8474877858052587},{"time":2.068104204745642,"velocity":0.500860148650437,"acceleration":-2.700000000000003,"pose":{"translation":{"x":5.865828193832404,"y":-1.8577238789494634},"rotation":{"radians":-0.06367453439929643}},"curvature":-4.361539640139643},{"time":2.09260418539275,"velocity":0.4347102009032456,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.87723983042111,"y":-1.8587835158903134},"rotation":{"radians":-0.12606797335766265}},"curvature":-6.765173997576896},{"time":2.1160800812268463,"velocity":0.37132528215118565,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.886575890703612,"y":-1.8603172471295863},"rotation":{"radians":-0.20493559496852556}},"curvature":-10.161883925389565},{"time":2.127589396456522,"velocity":0.3402501310310618,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":5.890565694061718,"y":-1.8612389048670965},"rotation":{"radians":-0.2504220751916043}},"curvature":-12.085865426392672},{"time":2.1390945597750513,"velocity":0.30918619007103243,"acceleration":-1.8924820462055663,"pose":{"translation":{"x":5.894162125738611,"y":-1.8622502866084267},"rotation":{"radians":-0.29897795833341534}},"curvature":-13.883657658978745},{"time":2.150587779303661,"velocity":0.2874354784600397,"acceleration":-0.3671051506100357,"pose":{"translation":{"x":5.897412720882245,"y":-1.8633405156596639},"rotation":{"radians":-0.3489700280234347}},"curvature":-15.167811103722578},{"time":2.1617083394694956,"velocity":0.28335306354549294,"acceleration":2.6999999999999984,"pose":{"translation":{"x":5.900367833181723,"y":-1.864498083498372},"rotation":{"radians":-0.39786739118221953}},"curvature":-15.430809298875147},{"time":2.1804709377657456,"velocity":0.3340120789453682,"acceleration":2.6999999999999567,"pose":{"translation":{"x":5.905607407346868,"y":-1.8669659777145693},"rotation":{"radians":-0.47832444070195745}},"curvature":-11.041099262767903},{"time":2.1956824280374048,"velocity":0.3750831026788478,"acceleration":2.699999999999993,"pose":{"translation":{"x":5.910341889267582,"y":-1.8695488554289996},"rotation":{"radians":-0.5104883219168177}},"curvature":-3.7340349071888874E-13},{"time":2.2929148364286727,"velocity":0.63761060533527,"acceleration":2.7000000000000037,"pose":{"translation":{"x":5.952628765990579,"y":-1.8947627456207157},"rotation":{"radians":-0.5725205397695268}},"curvature":-1.2729792340387263},{"time":2.3940778583761566,"velocity":0.9107507645934767,"acceleration":2.6999999999999997,"pose":{"translation":{"x":6.016899459359807,"y":-1.939518290288375},"rotation":{"radians":-0.6341961814427468}},"curvature":-0.4951458179596235},{"time":2.499582823433522,"velocity":1.1956141702483631,"acceleration":2.700000000000001,"pose":{"translation":{"x":6.104929493165445,"y":-2.0073229658763747},"rotation":{"radians":-0.6751734667579602}},"curvature":-0.30598432766039185},{"time":2.549871630357917,"velocity":1.331393948944229,"acceleration":-2.3703085481660073,"pose":{"translation":{"x":6.154145099583976,"y":-2.0475121420294213},"rotation":{"radians":-0.6943965009615569}},"curvature":-0.30719753844441644},{"time":2.601064659117595,"velocity":1.210050675268655,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.203697633427262,"y":-2.0896586153054586},"rotation":{"radians":-0.7157907005915123}},"curvature":-0.36026510676615386},{"time":2.7165416194940355,"velocity":0.8982628822522657,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.293359406274425,"y":-2.171994264734542},"rotation":{"radians":-0.7777408162553153}},"curvature":-0.7709264287429204},{"time":2.778891049560791,"velocity":0.7299194210720268,"acceleration":-2.7,"pose":{"translation":{"x":6.328670510091029,"y":-2.208456747165414},"rotation":{"radians":-0.8305100864157245}},"curvature":-1.4222448009063526},{"time":2.842695131750204,"velocity":0.5576483991606114,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.355220615825325,"y":-2.239798940322541},"rotation":{"radians":-0.9168250753018049}},"curvature":-3.1442989963643533},{"time":2.8749582012569603,"velocity":0.4705381114923698,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.36490619532735,"y":-2.2532634290570734},"rotation":{"radians":-0.9827537906480719}},"curvature":-5.034987197192603},{"time":2.891239770434675,"velocity":0.42657787471254105,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.3688352966753286,"y":-2.259419669934672},"rotation":{"radians":-1.0244849695664913}},"curvature":-6.4853718103581715},{"time":2.907713203045178,"velocity":0.3820996066641824,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.372160835205086,"y":-2.2651909502257763},"rotation":{"radians":-1.0737622012988202}},"curvature":-8.435878662280569},{"time":2.924526900044579,"velocity":0.3367026247657997,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.374893052354147,"y":-2.270580863405573},"rotation":{"radians":-1.1321258392888536}},"curvature":-11.04518545418917},{"time":2.941957874367017,"velocity":0.28963899409521776,"acceleration":-2.7,"pose":{"translation":{"x":6.377047337038721,"y":-2.2755966728813544},"rotation":{"radians":-1.2012322168330674}},"curvature":-14.476500929092555},{"time":2.96055398358937,"velocity":0.23942949919486523,"acceleration":-2.448027153795687,"pose":{"translation":{"x":6.3786444689861534,"y":-2.280249495489123},"rotation":{"radians":-1.2826082131505}},"curvature":-18.827812193648736},{"time":2.970567416073978,"velocity":0.21491634456984543,"acceleration":-1.9961771142903886,"pose":{"translation":{"x":6.379242130280754,"y":-2.2824443595580353},"rotation":{"radians":-1.3282237761827782}},"curvature":-21.327328725413906},{"time":2.981144665114215,"velocity":0.19380228210357486,"acceleration":-1.6000282816415057,"pose":{"translation":{"x":6.379710862067385,"y":-2.2845544849901995},"rotation":{"radians":-1.3771567268610405}},"curvature":-23.98711892404214},{"time":2.992268979172749,"velocity":0.17600306499605822,"acceleration":-1.2553869860651006,"pose":{"translation":{"x":6.380054886336579,"y":-2.286582426880274},"rotation":{"radians":-1.429290793886386}},"curvature":-26.72507006593706},{"time":3.0038952409682596,"velocity":0.16140760724138803,"acceleration":-0.9551676954781074,"pose":{"translation":{"x":6.380278807629404,"y":-2.288531015567824},"rotation":{"radians":-1.484358701225411}},"curvature":-29.420806330750263},{"time":3.0159445119389567,"velocity":0.14989853285611587,"acceleration":-0.6898335911409661,"pose":{"translation":{"x":6.380387620641595,"y":-2.290403362371591},"rotation":{"radians":-1.5419143492300567}},"curvature":-31.91668188022513},{"time":3.0283006223384077,"velocity":0.14137487284672845,"acceleration":-0.44743303545741403,"pose":{"translation":{"x":6.380386717827701,"y":-2.2922028653237634},"rotation":{"radians":-1.6013160616668083}},"curvature":-34.02706070206741},{"time":3.0408101331571995,"velocity":0.13577770444898907,"acceleration":-0.212921320679107,"pose":{"translation":{"x":6.380281897005218,"y":-2.2939332149042446},"rotation":{"radians":-1.6617277302102624}},"curvature":-35.55699082407614},{"time":3.0532866132644054,"velocity":0.13312119582713622,"acceleration":0.03378972806083901,"pose":{"translation":{"x":6.380079368958727,"y":-2.295598399774919},"rotation":{"radians":-1.7221425921088367}},"curvature":-36.3281418998923},{"time":3.065519296764733,"velocity":0.1335345348760666,"acceleration":0.32225443065354187,"pose":{"translation":{"x":6.379785765044043,"y":-2.2972027125139265},"rotation":{"radians":-1.781429772113809}},"curvature":-36.206139172711836},{"time":3.077285012514349,"velocity":0.13732608890619047,"acceleration":0.7012784582506274,"pose":{"translation":{"x":6.379408144792344,"y":-2.2987507553499262},"rotation":{"radians":-1.8383976967879823}},"curvature":-35.12127713858841},{"time":3.0883612496811925,"velocity":0.14509361542977273,"acceleration":1.2617209489418255,"pose":{"translation":{"x":6.3789540035143135,"y":-2.3002474458963693},"rotation":{"radians":-1.8918633161545486}},"curvature":-33.07584393236491},{"time":3.098537803842263,"velocity":0.157933587002836,"acceleration":2.7000000000000473,"pose":{"translation":{"x":6.378431279904281,"y":-2.3016980228857653},"rotation":{"radians":-1.940714098059974}},"curvature":-30.13585854641613},{"time":3.1153645172946947,"velocity":0.20336571332440306,"acceleration":2.6999999999999815,"pose":{"translation":{"x":6.3772141030085905,"y":-2.304483431124363},"rotation":{"radians":-2.0207139247079544}},"curvature":-22.0271903748647},{"time":3.1406589917681282,"velocity":0.2716607944026734,"acceleration":0.8024179910851177,"pose":{"translation":{"x":6.374357058736877,"y":-2.309768375181916},"rotation":{"radians":-2.089942441041602}},"curvature":-3.6876165474616885E-12},{"time":3.1622589055614276,"velocity":0.2889929538363045,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.371273053807943,"y":-2.3149791695909654},"rotation":{"radians":-2.134140449789665}},"curvature":-13.455982774139883},{"time":3.1737418742140093,"velocity":0.25798893847433346,"acceleration":-1.4383587980679373,"pose":{"translation":{"x":6.369534239034446,"y":-2.3175943536800085},"rotation":{"radians":-2.1829835922357548}},"curvature":-17.25132655214485},{"time":3.186933448272058,"velocity":0.23901472186757433,"acceleration":0.1738347345922304,"pose":{"translation":{"x":6.367572227256497,"y":-2.3202205006717915},"rotation":{"radians":-2.242778936332965}},"curvature":-18.865841343006046},{"time":3.201357086015986,"velocity":0.2415220511066446,"acceleration":1.2074390857232837,"pose":{"translation":{"x":6.365327025522202,"y":-2.3228603994487775},"rotation":{"radians":-2.30822787316994}},"curvature":-18.637946847005626},{"time":3.2161458477403038,"velocity":0.2593785800420343,"acceleration":2.1336945716205817,"pose":{"translation":{"x":6.362745547798263,"y":-2.3255164261590364},"rotation":{"radians":-2.374763726729108}},"curvature":-17.142365443426765},{"time":3.2306694918655126,"velocity":0.2903676006721412,"acceleration":2.700000000000074,"pose":{"translation":{"x":6.359781371637524,"y":-2.3281905601724713},"rotation":{"radians":-2.4389742624979664}},"curvature":-14.983480497841681},{"time":3.2446628052837676,"velocity":0.3281495469014306,"acceleration":2.6999999999998945,"pose":{"translation":{"x":6.356394494846521,"y":-2.330884400037045},"rotation":{"radians":-2.4986892553126565}},"curvature":-12.6428063971659},{"time":3.2582435491927177,"velocity":0.364817555455595,"acceleration":2.7000000000000877,"pose":{"translation":{"x":6.352551092153018,"y":-2.333599179435007},"rotation":{"radians":-2.552808677946185}},"curvature":-10.425243073509822},{"time":3.2716173390145897,"velocity":0.400926787974651,"acceleration":2.7000000000000393,"pose":{"translation":{"x":6.34822327187356,"y":-2.336335783139118},"rotation":{"radians":-2.6010138665326594}},"curvature":-8.480273098517067},{"time":3.2981771443200105,"velocity":0.4726382622992881,"acceleration":2.6999999999999944,"pose":{"translation":{"x":6.338031019772121,"y":-2.341876353746762},"rotation":{"radians":-2.680649682418315}},"curvature":-5.526691285519326},{"time":3.3248223581697265,"velocity":0.5445803396935209,"acceleration":2.7000000000000046,"pose":{"translation":{"x":6.3257040302168415,"y":-2.3475068181889385},"rotation":{"radians":-2.7413418618503456}},"curvature":-3.621027769968688},{"time":3.3516517412505586,"velocity":0.6170196740117675,"acceleration":2.6999999999999864,"pose":{"translation":{"x":6.311207959025704,"y":-2.3532233214413143},"rotation":{"radians":-2.787500682952535}},"curvature":-2.421126879189059},{"time":3.4055288550540843,"velocity":0.7624878812812861,"acceleration":2.6999999999999966,"pose":{"translation":{"x":6.275921295893175,"y":-2.3648792811083497},"rotation":{"radians":-2.8500310809579763}},"curvature":-1.1659746173848968},{"time":3.458689461018175,"velocity":0.9060215173843309,"acceleration":2.700000000000006,"pose":{"translation":{"x":6.23318766752019,"y":-2.3767415682587734},"rotation":{"radians":-2.887571818016487}},"curvature":-0.6087466403424915},{"time":3.557675298971284,"velocity":1.1732832798577255,"acceleration":-1.7282800282634867,"pose":{"translation":{"x":6.133054298331776,"y":-2.4004892897255963},"rotation":{"radians":-2.922362067786142}},"curvature":-0.15135507094459147},{"time":3.654622402712532,"velocity":1.005731536663738,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.029885473338478,"y":-2.423133209081552},"rotation":{"radians":-2.9237451596146884}},"curvature":0.1383938594851771},{"time":3.7573878685624846,"velocity":0.7282647788688656,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":5.943142240414719,"y":-2.443479477201466},"rotation":{"radians":-2.8895449379618605}},"curvature":0.7880473353914925},{"time":3.809391276974363,"velocity":0.5878555761567938,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.910154408569832,"y":-2.452584709951119},"rotation":{"radians":-2.850242690365192}},"curvature":1.6400683797905078},{"time":3.861155536589642,"velocity":0.44809207519554023,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.884695555551516,"y":-2.4609965262266837},"rotation":{"radians":-2.787675545983974}},"curvature":3.22050410776737},{"time":3.915948019182654,"velocity":0.30015237219440816,"acceleration":-2.7,"pose":{"translation":{"x":5.8657554437247095,"y":-2.468837742167634},"rotation":{"radians":-2.7058643030528273}},"curvature":4.433577634625505},{"time":4.027115564439842,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.850852764976659,"y":-2.476337923196535},"rotation":{"radians":-2.657114724552762}},"curvature":7.099564484227281E-14}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java index 5e5e18f..bce3980 100644 --- a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java @@ -21,7 +21,8 @@ public class FiveBallAutoMiddle extends SequentialCommandGroup { public FiveBallAutoMiddle(Drive drive, RamseteCommand[] paths) { // Use addRequirements() here to declare subsystem dependencies. addCommands( - paths[0] + paths[0], + new TankDriveVelocity(drive, -3.1, -0.3, 0.97) ); } } From 8b3ce633d69bb18f769728cdb24df3850476a365 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 18:27:06 -0600 Subject: [PATCH 33/39] made ten ball path --- PathWeaver/Paths/TenBallMidComplete | 9 +++++++++ src/main/deploy/paths/TenBallMidComplete.wpilib.json | 1 + 2 files changed, 10 insertions(+) create mode 100644 PathWeaver/Paths/TenBallMidComplete create mode 100644 src/main/deploy/paths/TenBallMidComplete.wpilib.json diff --git a/PathWeaver/Paths/TenBallMidComplete b/PathWeaver/Paths/TenBallMidComplete new file mode 100644 index 0000000..9363e21 --- /dev/null +++ b/PathWeaver/Paths/TenBallMidComplete @@ -0,0 +1,9 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +3.2,-2.4,0.2,2.5,true, +5.0,-1.1,3.0,0.0,true, +7.2,-1.1,1.5,0.0,true, +6.6123135559006245,-2.7023965955020937,-0.39262822032017564,-0.17846737287280634,true, +6.314867934445947,-2.8094770192257776,-0.3292650584281963,-0.18335832609507596,false, +5.862750589834837,-3.511448685858817,-0.1090388144136282,-0.46596647260741486,false, +5.886546239551211,-4.177726877917294,-0.07863878356842902,-0.48088836895271725,false, +5.553407143521973,-4.939187668841268,0.3688325706038009,-0.5235042937602339,true, diff --git a/src/main/deploy/paths/TenBallMidComplete.wpilib.json b/src/main/deploy/paths/TenBallMidComplete.wpilib.json new file mode 100644 index 0000000..a26f0c7 --- /dev/null +++ b/src/main/deploy/paths/TenBallMidComplete.wpilib.json @@ -0,0 +1 @@ +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24087146067360232,"velocity":0.6503529438187263,"acceleration":2.7,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.3403321067745373,"velocity":0.9188966882912508,"acceleration":2.700000000000002,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.4162093899230572,"velocity":1.1237653527922549,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.4796643300987847,"velocity":1.295093691266719,"acceleration":2.187682579886264,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5355873525736204,"velocity":1.4174355133495053,"acceleration":-0.734989108404782,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5891532438960738,"velocity":1.378065166645508,"acceleration":-0.6426464146474123,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.643461958301142,"velocity":1.3431638660489806,"acceleration":-0.5453266663691863,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6984118145503199,"velocity":1.3131982441231504,"acceleration":-0.4394478338280287,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.7539154354262596,"velocity":1.2888072981596066,"acceleration":-0.32468479529931726,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.809892141790966,"velocity":1.270632512712052,"acceleration":-0.20350244619471486,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.8662640818893885,"velocity":1.259160685005281,"acceleration":-0.08045877877688212,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.9229565411514642,"velocity":1.2545992789671963,"acceleration":0.03871626769718015,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.9799017050564615,"velocity":1.256803983177002,"acceleration":0.14815351255780274,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":1.037044049449856,"velocity":1.265269822214671,"acceleration":0.24282302918833878,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":1.0943450481757073,"velocity":1.2791838243007994,"acceleration":0.3192880318302778,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":1.151785273529354,"velocity":1.2975238008018528,"acceleration":0.37610415805722364,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":1.2093630070251655,"velocity":1.3191790257811382,"acceleration":0.4138164485760789,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.2670896494085861,"velocity":1.3430672599204667,"acceleration":0.43460983400489717,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.3249830335940764,"velocity":1.3682282940113044,"acceleration":0.4417529373871998,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.383060006914625,"velocity":1.3938839675702148,"acceleration":0.43900370786176246,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.441329456851559,"velocity":1.4194644721475942,"acceleration":0.43011410600385014,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.4997865390306198,"velocity":1.4446076877886345,"acceleration":0.41850596016098784,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.5584084380603251,"velocity":1.4691413019285218,"acceleration":0.4071259180970876,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.6171516690776433,"velocity":1.4930571937884367,"acceleration":0.3984462097492305,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.6759507421084023,"velocity":1.5164854615743109,"acceleration":0.39456346754290916,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.7347179434753464,"velocity":1.5396728523234446,"acceleration":0.397352070883421,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.7933440031024948,"velocity":1.562968038524027,"acceleration":0.4086404035647724,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.8516994822211263,"velocity":1.5868144450612802,"acceleration":0.43038917748303396,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.909636813019807,"velocity":1.6117500452092868,"acceleration":0.46485442754444356,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.9669930430836986,"velocity":1.6384123427017445,"acceleration":0.5147082657382434,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":2.0235934863647898,"velocity":1.6675450587029708,"acceleration":0.5830593842147167,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":2.079256669525349,"velocity":1.6999999999999975,"acceleration":4.265969051474203E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":2.1343686615338977,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1892768582496456,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.243795345773229,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.297757143445389,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.351013572738965,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4034336261508975,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4549033360942203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5053251437900577,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5546172681596233,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6027130747162124,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.649560444457203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6951211427560504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.739370188254281,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.782295221753494,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8238958751073535,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.864183140113587,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9031787374059825,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.940914485346383,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.01278040861083,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0802134292586643,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.143767239378293,"velocity":1.7,"acceleration":-0.9806847122121223,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2051845150767986,"velocity":1.6397690166567587,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.268443823133224,"velocity":1.4689688849044102,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.337848318037038,"velocity":1.2815767486641116,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.41794941674062,"velocity":1.0653037821644409,"acceleration":-2.699999999999997,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.464311638861809,"velocity":0.9401257824372306,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.246485948221108,"y":-1.1004457665706215},"rotation":{"radians":-0.02877542075414042}},"curvature":-1.2479062966462087},{"time":3.5152466704157184,"velocity":0.8026011972416758,"acceleration":-2.699999999999998,"pose":{"translation":{"x":7.290770527031534,"y":-1.103398777804336},"rotation":{"radians":-0.11545308104819059}},"curvature":-2.7518045032639415},{"time":3.5426121847539718,"velocity":0.7287143085283914,"acceleration":-2.7,"pose":{"translation":{"x":7.3114956505922,"y":-1.1064779763119066},"rotation":{"radians":-0.18293230201129468}},"curvature":-3.725227733364007},{"time":3.5716326709523103,"velocity":0.6503589957928775,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.331006934455969,"y":-1.1109206638015918},"rotation":{"radians":-0.26869329900096417}},"curvature":-4.883076044748539},{"time":3.5869706546331592,"velocity":0.6089464398545852,"acceleration":-2.7,"pose":{"translation":{"x":7.340252146464765,"y":-1.1137126796958767},"rotation":{"radians":-0.31891235544080615}},"curvature":-5.523919809501616},{"time":3.6030413007858946,"velocity":0.5655556952421994,"acceleration":-2.261497896342809,"pose":{"translation":{"x":7.349130295806822,"y":-1.1169134527391915},"rotation":{"radians":-0.37417386340026293}},"curvature":-6.1910282938294925},{"time":3.6199421735253385,"velocity":0.5273344070955894,"acceleration":-1.8200753979085174,"pose":{"translation":{"x":7.357622936902054,"y":-1.1205421754363483},"rotation":{"radians":-0.4344600800162828}},"curvature":-6.863459317284596},{"time":3.637660919757719,"velocity":0.49508495299624905,"acceleration":-1.404368604220338,"pose":{"translation":{"x":7.365712904636497,"y":-1.1246164469159217},"rotation":{"radians":-0.499598871724775}},"curvature":-7.511586808196944},{"time":3.6561465833657167,"velocity":0.4691242673969987,"acceleration":-1.013471060094217,"pose":{"translation":{"x":7.373384290445131,"y":-1.1291523046382135},"rotation":{"radians":-0.5692244956270529}},"curvature":-8.098065776430404},{"time":3.675310118112155,"velocity":0.44970257952237364,"acceleration":-0.6441793281202971,"pose":{"translation":{"x":7.380622418394695,"y":-1.1341642561032126},"rotation":{"radians":-0.6427505179428795}},"curvature":-8.58109966790429},{"time":3.695023152409384,"velocity":0.43700385033357203,"acceleration":-0.2912186052656897,"pose":{"translation":{"x":7.387413821266509,"y":-1.1396653105585597},"rotation":{"radians":-0.7193652922148932}},"curvature":-8.92014169080443},{"time":3.715122472825929,"velocity":0.43115055427507765,"acceleration":0.052067701331746194,"pose":{"translation":{"x":7.3937462166392915,"y":-1.1456670107075082},"rotation":{"radians":-0.7980584699587645}},"curvature":-9.08314266129781},{"time":3.7354206996909998,"velocity":0.43220743628905217,"acceleration":0.3926861267115369,"pose":{"translation":{"x":7.399608482971979,"y":-1.152179464416887},"rotation":{"radians":-0.8776808332180518}},"curvature":-9.053384284754978},{"time":3.755721886671196,"velocity":0.4401794307719521,"acceleration":0.7367582925439635,"pose":{"translation":{"x":7.404990635686545,"y":-1.1592113764250642},"rotation":{"radians":-0.957030545537142}},"curvature":-8.833522919030923},{"time":3.7758391314968773,"velocity":0.45500097772041015,"acceleration":1.0882423303428184,"pose":{"translation":{"x":7.409883803250819,"y":-1.1667700800499075},"rotation":{"radians":-1.0349502992109227}},"curvature":-8.445233665625674},{"time":3.7956105572131413,"velocity":0.4765170801160774,"acceleration":1.4478029542435038,"pose":{"translation":{"x":7.414280203261306,"y":-1.1748615688967483},"rotation":{"radians":-1.110416006752432}},"curvature":-7.924546693213447},{"time":3.8149106115079463,"velocity":0.5044597557411559,"acceleration":1.8120558532837037,"pose":{"translation":{"x":7.4181731185260045,"y":-1.1834905285663433},"rotation":{"radians":-1.1826006954592512}},"curvature":-7.314635079400883},{"time":3.833655213614978,"velocity":0.5384260217066767,"acceleration":2.1734101921253286,"pose":{"translation":{"x":7.421556873147227,"y":-1.192660368362838},"rotation":{"radians":-1.2509054956221384}},"curvature":-6.658491095790275},{"time":3.851801053675725,"velocity":0.5778643754393804,"acceleration":2.5206585401034607,"pose":{"translation":{"x":7.424426808604417,"y":-1.2023732530017275},"rotation":{"radians":-1.3149589717095316}},"curvature":-5.99341593717812},{"time":3.869340590588412,"velocity":0.6220755589478055,"acceleration":2.7000000000000313,"pose":{"translation":{"x":7.4267792598369695,"y":-1.2126301343178203},"rotation":{"radians":-1.3745927334979915}},"curvature":-5.3481080221595265},{"time":3.8863249147004435,"velocity":0.667933234050291,"acceleration":2.700000000000086,"pose":{"translation":{"x":7.428611531327051,"y":-1.2234307829732003},"rotation":{"radians":-1.4298037347140933}},"curvature":-4.742101270083783},{"time":3.90286706743035,"velocity":0.71259704642104,"acceleration":2.7000000000000104,"pose":{"translation":{"x":7.429921873182416,"y":-1.2347738201651899},"rotation":{"radians":-1.480712629900542}},"curvature":-4.186771633619186},{"time":3.919081127481485,"velocity":0.7563750085591059,"acceleration":2.7000000000000726,"pose":{"translation":{"x":7.430709457219227,"y":-1.2466567493343108},"rotation":{"radians":-1.527524747550707}},"curvature":-3.687079506807228},{"time":3.935049187697399,"velocity":0.7994887711420753,"acceleration":2.699999999999971,"pose":{"translation":{"x":7.430974353044876,"y":-1.2590759878722486},"rotation":{"radians":-1.5704972091395932}},"curvature":-3.2434293445605835},{"time":3.9664639427506723,"velocity":0.8843086097859121,"acceleration":2.699999999999964,"pose":{"translation":{"x":7.429940703945299,"y":-1.2855038226249045},"rotation":{"radians":-1.6460641038472028}},"curvature":-2.5125016951673764},{"time":3.997424393909201,"velocity":0.9679018279139381,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.426838529714478,"y":-1.3140081474824699},"rotation":{"radians":-1.7096996276847836}},"curvature":-1.9589426080510612},{"time":4.028085598335015,"velocity":1.0506870798636372,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.421698268143265,"y":-1.3445244380324612},"rotation":{"radians":-1.7635069388369267}},"curvature":-1.5430718610586651},{"time":4.058517591989222,"velocity":1.132853462729996,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.414562425632777,"y":-1.3769738370453977},"rotation":{"radians":-1.80928153691283}},"curvature":-1.2304194023042416},{"time":4.118736716721557,"velocity":1.2954450995073,"acceleration":2.3581795493155604,"pose":{"translation":{"x":7.394529774351022,"y":-1.4472909553859892},"rotation":{"radians":-1.8823557940783588}},"curvature":-0.8137689160789616},{"time":4.178392474069149,"velocity":1.4361240864833222,"acceleration":1.0280501369863269,"pose":{"translation":{"x":7.367292915991394,"y":-1.52408054521081},"rotation":{"radians":-1.9376153749830112}},"curvature":-0.5671040820963597},{"time":4.238964013252306,"velocity":1.4983946656380396,"acceleration":0.6806598794331884,"pose":{"translation":{"x":7.333548306053704,"y":-1.6062992620615995},"rotation":{"radians":-1.980664464363438}},"curvature":-0.415270222573429},{"time":4.301503977300484,"velocity":1.5409631100268286,"acceleration":0.4541105461405023,"pose":{"translation":{"x":7.294112024222084,"y":-1.692771843268838},"rotation":{"radians":-2.0151600783392727}},"curvature":-0.31853754024915065},{"time":4.365651597320475,"velocity":1.5700932207877196,"acceleration":0.3039509005891818,"pose":{"translation":{"x":7.249895283171679,"y":-1.7822235769053099},"rotation":{"radians":-2.0435542741178185}},"curvature":-0.2553649962385433},{"time":4.4308222574998855,"velocity":1.589901901641243,"acceleration":0.19979119065916273,"pose":{"translation":{"x":7.20187993737534,"y":-1.873312770739671},"rotation":{"radians":-2.0675482771424347}},"curvature":-0.21372950444359917},{"time":4.496291907487063,"velocity":1.6029821609642199,"acceleration":0.12148010098068647,"pose":{"translation":{"x":7.151093991910315,"y":-1.9646632211900141},"rotation":{"radians":-2.088370637372372}},"curvature":-0.18680044113633412},{"time":4.561259656850615,"velocity":1.6108744497173921,"acceleration":0.05463868552157288,"pose":{"translation":{"x":7.098587111264945,"y":-2.054896682277434},"rotation":{"radians":-2.106952696193974}},"curvature":-0.17076368611172438},{"time":4.624897589019494,"velocity":1.6143515426804107,"acceleration":-0.013181927539404118,"pose":{"translation":{"x":7.045406128145351,"y":-2.1426653345795916},"rotation":{"radians":-2.124045424613487}},"curvature":-0.1637481573893422},{"time":4.6863934918413115,"velocity":1.6135409081454433,"acceleration":-0.09636542124448583,"pose":{"translation":{"x":6.9925705522821335,"y":-2.226684254184283},"rotation":{"radians":-2.140304501136336}},"curvature":-0.16538102479256336},{"time":4.744990364133843,"velocity":1.6078941958633641,"acceleration":-0.6374308786638251,"pose":{"translation":{"x":6.941048079237058,"y":-2.3057638816430024},"rotation":{"radians":-2.156361587928427}},"curvature":-0.1768009201056777},{"time":4.800431081060775,"velocity":1.5725545709588773,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.891730099209751,"y":-2.37884249092451},"rotation":{"radians":-2.1728963784151825}},"curvature":-0.2011726549273176},{"time":4.854288620919112,"velocity":1.427139213341367,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.845407205844394,"y":-2.4450186583683924},"rotation":{"radians":-2.1907250385502053}},"curvature":-0.24500666492416812},{"time":4.907764200265851,"velocity":1.282755149105172,"acceleration":-2.7,"pose":{"translation":{"x":6.802744705036412,"y":-2.5035837316386345},"rotation":{"radians":-2.2109265322563663}},"curvature":-0.32107362952933843},{"time":5.010558252841902,"velocity":1.0052112071498347,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.7302887187706615,"y":-2.596204656657504},"rotation":{"radians":-2.26538823363029}},"curvature":-0.6983733281623699},{"time":5.058181507945903,"velocity":0.876628418369029,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.700978985620202,"y":-2.6300992809381722},"rotation":{"radians":-2.3055766625940555}},"curvature":-1.161706924289958},{"time":5.102107797041266,"velocity":0.7580274378115504,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.6762481672551255,"y":-2.6561252940163986},"rotation":{"radians":-2.3611253774593677}},"curvature":-2.069483430581481},{"time":5.1416578127195125,"velocity":0.6512423954802853,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.655767762927474,"y":-2.6750249344816397},"rotation":{"radians":-2.4393793407849826}},"curvature":-3.786562605724007},{"time":5.159738948600884,"velocity":0.6024233286005831,"acceleration":-2.7,"pose":{"translation":{"x":6.646945039992192,"y":-2.682139407006597},"rotation":{"radians":-2.4888731070782244}},"curvature":-5.0067041726191235},{"time":5.176792965437063,"velocity":0.5563774831429,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.638937036980688,"y":-2.68792802596912},"rotation":{"radians":-2.54463825964339}},"curvature":-6.289792037747411},{"time":5.1930309236619525,"velocity":0.5125349959356977,"acceleration":1.3760054548561336,"pose":{"translation":{"x":6.631622107145021,"y":-2.6925979064247128},"rotation":{"radians":-2.603460002905891}},"curvature":-7.150761360113481},{"time":5.207859374190757,"velocity":0.5329390247503961,"acceleration":2.7000000000000117,"pose":{"translation":{"x":6.624858527656298,"y":-2.6963844461134348},"rotation":{"radians":-2.658512232190288}},"curvature":-6.75882207866783},{"time":5.232432613506469,"velocity":0.5992867709028199,"acceleration":2.6999999999999997,"pose":{"translation":{"x":6.6123135559006245,"y":-2.7023965955020928},"rotation":{"radians":-2.714965160462928}},"curvature":-2.782056460928346E-13},{"time":5.307249981064505,"velocity":0.8012936633095167,"acceleration":2.6999999999999944,"pose":{"translation":{"x":6.564370383742047,"y":-2.723528796446016},"rotation":{"radians":-2.7486246800604213}},"curvature":-1.2481483002418547},{"time":5.359588764109113,"velocity":0.942608377529959,"acceleration":-1.3037969736295336,"pose":{"translation":{"x":6.521596141607646,"y":-2.739437497523542},"rotation":{"radians":-2.830970184843895}},"curvature":-2.3571662142433545},{"time":5.380819682559197,"velocity":0.9149275703073636,"acceleration":2.1653287500554654,"pose":{"translation":{"x":6.502679354153565,"y":-2.7450033060483143},"rotation":{"radians":-2.8808935518583048}},"curvature":-2.648365983542163},{"time":5.3999635475683805,"velocity":0.9563803315989277,"acceleration":-1.9880304077630273,"pose":{"translation":{"x":6.485268832011468,"y":-2.749212182681022},"rotation":{"radians":-2.927188261447052}},"curvature":-2.3998009552324597},{"time":5.434828841887997,"velocity":0.8870670663159218,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.453690251127663,"y":-2.7551725959229563},"rotation":{"radians":-2.9677547229044667}},"curvature":0.39238321378028623},{"time":5.471501485855907,"velocity":0.7880509276025651,"acceleration":-1.6821792794414314,"pose":{"translation":{"x":6.423581274828754,"y":-2.7612464468006617},"rotation":{"radians":-2.9005726138477717}},"curvature":3.571669713554079},{"time":5.492544888288298,"velocity":0.7526521520618497,"acceleration":2.4550269205084136,"pose":{"translation":{"x":6.4079642503873036,"y":-2.7655937621270232},"rotation":{"radians":-2.838791299614375}},"curvature":3.884813326993507},{"time":5.514928540897344,"velocity":0.8076046217963667,"acceleration":2.6999999999999975,"pose":{"translation":{"x":6.391477144723909,"y":-2.7713471159009355},"rotation":{"radians":-2.7743073993901626}},"curvature":3.410464289721715},{"time":5.537683301071879,"velocity":0.8690424742676104,"acceleration":2.6999999999999487,"pose":{"translation":{"x":6.373886771801721,"y":-2.778727284101776},"rotation":{"radians":-2.717036888422856}},"curvature":2.5791011531493675},{"time":5.560811154289309,"velocity":0.9314876779546685,"acceleration":-2.232737302146218,"pose":{"translation":{"x":6.355122754197213,"y":-2.787751360334723},"rotation":{"radians":-2.6726507558686987}},"curvature":1.702709676404945},{"time":5.613209538187445,"velocity":0.8144958516531218,"acceleration":-2.7,"pose":{"translation":{"x":6.3148679344459335,"y":-2.8094770192257847},"rotation":{"radians":-2.633489132868386}},"curvature":2.6424201040934643E-13},{"time":5.644832808940525,"velocity":0.7291130206198057,"acceleration":0.014726220632563273,"pose":{"translation":{"x":6.293824179582127,"y":-2.8218407195536503},"rotation":{"radians":-2.5704807412571062}},"curvature":4.109876886612331},{"time":5.6636248279584125,"velocity":0.7293897560379944,"acceleration":2.7000000000000006,"pose":{"translation":{"x":6.282512531848517,"y":-2.8295769425123694},"rotation":{"radians":-2.5130508700960563}},"curvature":4.107146562881415},{"time":5.683907608455922,"velocity":0.7841532633812693,"acceleration":2.700000000000044,"pose":{"translation":{"x":6.270373430375236,"y":-2.8389709421709193},"rotation":{"radians":-2.454602498726585}},"curvature":3.4662114222347586},{"time":5.70526422526559,"velocity":0.8418161287673747,"acceleration":2.7000000000000424,"pose":{"translation":{"x":6.2572605014198635,"y":-2.850351234968828},"rotation":{"radians":-2.401258195704806}},"curvature":2.6992882596325503},{"time":5.727782637168609,"velocity":0.9026158409055279,"acceleration":2.700000000000005,"pose":{"translation":{"x":6.2430848376947585,"y":-2.8639459490095134},"rotation":{"radians":-2.355136423565373}},"curvature":2.035175527413349},{"time":5.775996784507287,"velocity":1.0327940387199588,"acceleration":2.7000000000000086,"pose":{"translation":{"x":6.211447015686596,"y":-2.898237788623612},"rotation":{"radians":-2.283511963581287}},"curvature":1.1584351087573401},{"time":5.827249030133743,"velocity":1.1711751019113907,"acceleration":2.7000000000000006,"pose":{"translation":{"x":6.175708077122055,"y":-2.9419713739030793},"rotation":{"radians":-2.232503020651801}},"curvature":0.7114010646875015},{"time":5.879716810848121,"velocity":1.3128381098402124,"acceleration":2.700000000000001,"pose":{"translation":{"x":6.136715668746837,"y":-2.9941835597868995},"rotation":{"radians":-2.1943873167816736}},"curvature":0.48850726496729824},{"time":5.931594500419193,"velocity":1.4529078716821067,"acceleration":1.5624737156913744,"pose":{"translation":{"x":6.095757006630035,"y":-3.0530822209766115},"rotation":{"radians":-2.163759911299562}},"curvature":0.380438022405832},{"time":5.982215145525289,"velocity":1.5320012991317222,"acceleration":-0.057841945126272415,"pose":{"translation":{"x":6.054398911513116,"y":-3.116305329649745},"rotation":{"radians":-2.1369461734365096}},"curvature":0.3384556587168576},{"time":6.032035019002884,"velocity":1.5291196207438333,"acceleration":-0.5090616981546786,"pose":{"translation":{"x":6.014327844158916,"y":-3.181180033173254},"rotation":{"radians":-2.1111850358192785}},"curvature":0.34490995393452345},{"time":6.080707664405129,"velocity":1.504342241221686,"acceleration":-1.7656263145663706,"pose":{"translation":{"x":5.97718994070063,"y":-3.244981731816954},"rotation":{"radians":-2.0840011566593573}},"curvature":0.4014259222340187},{"time":6.127561407010178,"velocity":1.4216160403422935,"acceleration":-2.7,"pose":{"translation":{"x":5.944431047990803,"y":-3.305193156466954},"rotation":{"radians":-2.052707137462513}},"curvature":0.5281984550986057},{"time":6.172389573744138,"velocity":1.300579990160599,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.917136758950319,"y":-3.3597634463390964},"rotation":{"radians":-2.0140182231966675}},"curvature":0.7696908429217094},{"time":6.214300523930543,"velocity":1.1874204246573072,"acceleration":-2.7000000000000037,"pose":{"translation":{"x":5.895872447917389,"y":-3.4073672266923865},"rotation":{"radians":-1.9641560802129352}},"curvature":1.188984427881291},{"time":6.252252771580072,"velocity":1.0849493560035777,"acceleration":-0.4333623742254068,"pose":{"translation":{"x":5.880523305996552,"y":-3.4476636865424304},"rotation":{"radians":-1.901225210539506}},"curvature":1.7496710295967226},{"time":6.285141745600643,"velocity":1.0706965121361853,"acceleration":1.504053099473219,"pose":{"translation":{"x":5.870134376407649,"y":-3.4815556563748715},"rotation":{"radians":-1.8346198081438863}},"curvature":1.8140478591397617},{"time":6.313341537856752,"velocity":1.1131104970834875,"acceleration":-2.7,"pose":{"translation":{"x":5.862750589834832,"y":-3.511448685858822},"rotation":{"radians":-1.8006658230463592}},"curvature":-5.360757530368065E-13},{"time":6.341497677084923,"velocity":1.0370889211674263,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.856203796082101,"y":-3.541002910133535},"rotation":{"radians":-1.766968921320334}},"curvature":1.963419840399414},{"time":6.373942418174615,"velocity":0.9494881202252553,"acceleration":1.9757710956120564,"pose":{"translation":{"x":5.851064699271866,"y":-3.5728175057829237},"rotation":{"radians":-1.6924633524514152}},"curvature":2.43962472127956},{"time":6.410085221881796,"velocity":1.020898027104284,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.848224330419191,"y":-3.6083116788576466},"rotation":{"radians":-1.61121420277845}},"curvature":2.0530882496173066},{"time":6.4472409528531545,"velocity":1.1212185007269526,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.848083515132619,"y":-3.648107382432883},"rotation":{"radians":-1.541358784235409}},"curvature":1.468599708411283},{"time":6.484891418261424,"velocity":1.2228747573292795,"acceleration":2.7000000000000095,"pose":{"translation":{"x":5.850633646779956,"y":-3.6921617367241057},"rotation":{"radians":-1.4882866880902128}},"curvature":0.9656511797992369},{"time":6.522567110762013,"velocity":1.3245991270808724,"acceleration":2.699999999999986,"pose":{"translation":{"x":5.855537459654031,"y":-3.7398994492028543},"rotation":{"radians":-1.4515974408881707}},"curvature":0.5866238096609577},{"time":6.5952183344984405,"velocity":1.520757431169224,"acceleration":-1.890804527454655,"pose":{"translation":{"x":5.869898409873462,"y":-3.8422562355841015},"rotation":{"radians":-1.4201823229158446}},"curvature":0.05849443045663204},{"time":6.666646906859882,"velocity":1.3856999631585873,"acceleration":-2.7,"pose":{"translation":{"x":5.884964438933297,"y":-3.944959110869841},"rotation":{"radians":-1.4385731313207648}},"curvature":-0.43793035518534235},{"time":6.739064849944098,"velocity":1.1901715168312037,"acceleration":-2.700000000000003,"pose":{"translation":{"x":5.894444557385554,"y":-4.0377457278600914},"rotation":{"radians":-1.5112165860236555}},"curvature":-1.2058440033677431},{"time":6.774536895566012,"velocity":1.0943969936520368,"acceleration":-2.1274280542372015,"pose":{"translation":{"x":5.895735701559724,"y":-4.078244310677544},"rotation":{"radians":-1.5700473110704392}},"curvature":-1.7079223464270294},{"time":6.808872494354494,"velocity":1.021350477530387,"acceleration":-2.373467065825907,"pose":{"translation":{"x":5.8945434544964925,"y":-4.114547466566519},"rotation":{"radians":-1.6393519162414487}},"curvature":-2.0508114870006136},{"time":6.842357555890162,"velocity":0.9418747867783246,"acceleration":-2.7,"pose":{"translation":{"x":5.891207411273528,"y":-4.14724709351312},"rotation":{"radians":-1.7035828110055375}},"curvature":-1.669967448090788},{"time":6.87679427990919,"velocity":0.848895631926948,"acceleration":-2.7,"pose":{"translation":{"x":5.886546239551215,"y":-4.177726877917286},"rotation":{"radians":-1.7328897632897347}},"curvature":1.1065428326033853E-13},{"time":6.915988445631466,"velocity":0.7430713844768021,"acceleration":-0.9800934374084039,"pose":{"translation":{"x":5.88067630228792,"y":-4.208367592591282},"rotation":{"radians":-1.8080776127277007}},"curvature":-3.9746967020187314},{"time":6.939149798238455,"velocity":0.7203710947851905,"acceleration":2.105068867779344,"pose":{"translation":{"x":5.876118091119996,"y":-4.224690756343688},"rotation":{"radians":-1.8787325484751447}},"curvature":-4.197206170037974},{"time":6.963906716861135,"velocity":0.7724861134399396,"acceleration":2.7000000000001143,"pose":{"translation":{"x":5.869858974330641,"y":-4.2420777330336055},"rotation":{"radians":-1.952406601416692}},"curvature":-3.7058234846360123},{"time":6.989199800601645,"velocity":0.8407774395393204,"acceleration":2.7000000000000917,"pose":{"translation":{"x":5.861597813055334,"y":-4.260732576536634},"rotation":{"radians":-2.0203938686193776}},"curvature":-2.956673651080127},{"time":7.015028262074048,"velocity":0.9105142855148116,"acceleration":2.700000000000028,"pose":{"translation":{"x":5.851151489499009,"y":-4.2807920944866344},"rotation":{"radians":-2.078685177167563}},"curvature":-2.2258692105513505},{"time":7.068415357591023,"velocity":1.0546594434106475,"acceleration":2.6999999999999766,"pose":{"translation":{"x":5.8234994187239835,"y":-4.325369532324868},"rotation":{"radians":-2.1640426184738857}},"curvature":-1.1525883783768047},{"time":7.123332995545581,"velocity":1.2029370658879515,"acceleration":2.700000000000027,"pose":{"translation":{"x":5.787408291453949,"y":-4.375770984831428},"rotation":{"radians":-2.2141065150229546}},"curvature":-0.5349613600815684},{"time":7.17819258339559,"velocity":1.351057953082978,"acceleration":2.699999999999995,"pose":{"translation":{"x":5.744615217060818,"y":-4.431237495974559},"rotation":{"radians":-2.2374556498732163}},"curvature":-0.16441177996834094},{"time":7.231275738982579,"velocity":1.49438247316785,"acceleration":-1.695930601341739,"pose":{"translation":{"x":5.697760506246135,"y":-4.490468238184125},"rotation":{"radians":-2.2392837121801517}},"curvature":0.10580284594353764},{"time":7.284900098907512,"velocity":1.4034392801937925,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.650059292447182,"y":-4.551798535128107},"rotation":{"radians":-2.2209710474953113}},"curvature":0.3716502298548522},{"time":7.342468924065651,"velocity":1.2480034522668173,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":5.604973153243076,"y":-4.613377884489101},"rotation":{"radians":-2.180152623521608}},"curvature":0.7236839842025061},{"time":7.40391328710216,"velocity":1.0821036720682429,"acceleration":-2.699999999999996,"pose":{"translation":{"x":5.56588173176088,"y":-4.6733479807408145},"rotation":{"radians":-2.110033357039896}},"curvature":1.2953678674001186},{"time":7.435819801119151,"velocity":0.9959560842223683,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.549536962771379,"y":-4.702190515076448},"rotation":{"radians":-2.060289209923918}},"curvature":1.7314463248379408},{"time":7.468444759800843,"velocity":0.9078686957817996,"acceleration":-2.7,"pose":{"translation":{"x":5.5357543580817,"y":-4.730020737924564},"rotation":{"radians":-1.9979270126197324}},"curvature":2.320021023322054},{"time":7.501861141036805,"velocity":0.8176444664447036,"acceleration":-2.6999999999999953,"pose":{"translation":{"x":5.5247870979254925,"y":-4.756683442209535},"rotation":{"radians":-1.9203821291983572}},"curvature":3.103746880369516},{"time":7.518946087216342,"velocity":0.7715151117599537,"acceleration":-2.7,"pose":{"translation":{"x":5.520420385403833,"y":-4.769537314349319},"rotation":{"radians":-1.87511361445659}},"curvature":3.5772510721477797},{"time":7.536360358041236,"velocity":0.7244965805327409,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":5.5168216706467925,"y":-4.782056312425773},"rotation":{"radians":-1.8251635891717155}},"curvature":4.103054297437957},{"time":7.55420292968893,"velocity":0.6763216370839664,"acceleration":-2.607992884955388,"pose":{"translation":{"x":5.514002160507967,"y":-4.794231199022947},"rotation":{"radians":-1.770388976089039}},"curvature":4.671595505555233},{"time":7.572595310766079,"velocity":0.6283544380973731,"acceleration":-2.168275051067394,"pose":{"translation":{"x":5.51196761086897,"y":-4.80605548784949},"rotation":{"radians":-1.710811712710651}},"curvature":5.263825366744856},{"time":7.591579364783137,"velocity":0.58719178740407,"acceleration":-1.7049346554482947,"pose":{"translation":{"x":5.510718005957207,"y":-4.817525617589015},"rotation":{"radians":-1.6466861683784764}},"curvature":5.849184750926004},{"time":7.611078252080525,"velocity":0.5539474587080722,"acceleration":-1.2273047399076786,"pose":{"translation":{"x":5.5102472376636635,"y":-4.82864112575047},"rotation":{"radians":-1.5785641919150275}},"curvature":6.385441698329703},{"time":7.630954101985592,"velocity":0.5295537339098895,"acceleration":-0.7356699178498,"pose":{"translation":{"x":5.5105427848606805,"y":-4.839404822518501},"rotation":{"radians":-1.507341453940095}},"curvature":6.821759991973681},{"time":7.651005074056508,"velocity":0.5148028369336706,"acceleration":-0.21868006238280333,"pose":{"translation":{"x":5.511585392719732,"y":-4.8498229646038284},"rotation":{"radians":-1.4342657220142343}},"curvature":7.105663902666528},{"time":7.670972126524015,"velocity":0.5104364406544754,"acceleration":0.3496940236859671,"pose":{"translation":{"x":5.513348752029203,"y":-4.859905429093603},"rotation":{"radians":-1.360891999701801}},"curvature":7.192849441564701},{"time":7.690555950909508,"velocity":0.5172847870029977,"acceleration":1.0159549694998586,"pose":{"translation":{"x":5.515799178512178,"y":-4.8696658873017835},"rotation":{"radians":-1.2889830914151401}},"curvature":7.056761937305157},{"time":7.709440884685059,"velocity":0.5364710293209449,"acceleration":1.7204991229826834,"pose":{"translation":{"x":5.518895292144211,"y":-4.879121978619499},"rotation":{"radians":-1.2203733973856572}},"curvature":6.694003221269455},{"time":7.7273589882858,"velocity":0.5672991108515307,"acceleration":-2.699999999999996,"pose":{"translation":{"x":5.522587696471107,"y":-4.888295484365418},"rotation":{"radians":-1.1568292312130675}},"curvature":6.123051113372027},{"time":7.7455439259682795,"velocity":0.5181997791088344,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.5268186579267065,"y":-4.897212501636115},"rotation":{"radians":-1.0999415472086687}},"curvature":5.37683627323114},{"time":7.7656690330091696,"velocity":0.4638619900984307,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.531521785150656,"y":-4.905903617156444},"rotation":{"radians":-1.0510750755719702}},"curvature":4.49233832354661},{"time":7.815797793645661,"velocity":0.32851433637990307,"acceleration":-2.7,"pose":{"translation":{"x":5.542033758397939,"y":-4.922753981088973},"rotation":{"radians":-0.9818462210463513}},"curvature":2.421643800402231},{"time":7.937469770082663,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.553407143521984,"y":-4.939187668841276},"rotation":{"radians":-0.9570262317783049}},"curvature":1.1443843838492492E-13}] \ No newline at end of file From 18021fbdeb88c2af12e939f292601c67c0fcb0bf Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 19:41:56 -0600 Subject: [PATCH 34/39] finished 5 ball --- PathWeaver/Paths/FiveBallMidComplete | 4 ++-- src/main/deploy/paths/FiveBallMidComplete.wpilib.json | 2 +- src/main/java/frc4388/robot/Robot.java | 2 ++ src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++-- .../frc4388/robot/commands/auto/FiveBallAutoMiddle.java | 2 +- 5 files changed, 12 insertions(+), 6 deletions(-) diff --git a/PathWeaver/Paths/FiveBallMidComplete b/PathWeaver/Paths/FiveBallMidComplete index 6783265..b325387 100644 --- a/PathWeaver/Paths/FiveBallMidComplete +++ b/PathWeaver/Paths/FiveBallMidComplete @@ -1,5 +1,5 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.3,0.5,0.0,true, 5.910341889267586,-1.8695488554289974,0.2974456214546777,-0.16656954801461854,true, -6.374357058736883,-2.3097683751819202,-0.09518259886549707,-0.16656954801461987,true, -5.85085276497665,-2.4763379231965392,-0.22605867230555443,-0.11897824858187134,true, +6.398152708453256,-2.178892301741862,-0.09518259886549707,-0.16656954801461987,true, +5.886546239551211,-2.3573596746146683,-0.22605867230555443,-0.11897824858187134,true, diff --git a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json index 859b771..be9f201 100644 --- a/src/main/deploy/paths/FiveBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/FiveBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.16408328867307112,"velocity":0.443024879417292,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2363298344524436,"y":-2.298899802686018},"rotation":{"radians":0.0692577651470374}},"curvature":1.8950587512445145},{"time":0.27165252251845773,"velocity":0.733461810799836,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.2992357901794267,"y":-2.292058382559469},"rotation":{"radians":0.13336588761066984}},"curvature":0.5081353245388296},{"time":0.39146527816103716,"velocity":1.0569562510348003,"acceleration":2.700000000000003,"pose":{"translation":{"x":3.4052753217803797,"y":-2.2759406702466065},"rotation":{"radians":0.1622880652133495}},"curvature":0.14308012858303878},{"time":0.45511058967808826,"velocity":1.2287985921308384,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.4770070197266802,"y":-2.2638785389701312},"rotation":{"radians":0.17018012232912796}},"curvature":0.08218998056047282},{"time":0.5202669048089674,"velocity":1.404720642984212,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.5615193955328435,"y":-2.249097623342679},"rotation":{"radians":0.17564390475973646}},"curvature":0.04917485937853854},{"time":0.5862126558482816,"velocity":1.5827741707903604,"acceleration":1.5865644228393085,"pose":{"translation":{"x":3.658474466495589,"y":-2.231684860998854},"rotation":{"radians":0.17945016920991327}},"curvature":0.030140843221192568},{"time":0.6537218964528062,"velocity":1.6898819301463979,"acceleration":0.05788196551324373,"pose":{"translation":{"x":3.7671397522485237,"y":-2.211813471951235},"rotation":{"radians":0.18207557565508717}},"curvature":0.018479758912245518},{"time":0.7254285040062793,"velocity":1.6940324495318797,"acceleration":0.04142453277389165,"pose":{"translation":{"x":3.8864378767263954,"y":-2.1897319350134796},"rotation":{"radians":0.18381956783311507}},"curvature":0.010872498721873012},{"time":0.7633704400602459,"velocity":1.6956041765054521,"acceleration":0.03427111458881184,"pose":{"translation":{"x":3.949655472782524,"y":-2.177957876460112},"rotation":{"radians":0.18442327195140867}},"curvature":0.008001487996222786},{"time":0.8025567568325975,"velocity":1.6969471352578709,"acceleration":0.02906850160180063,"pose":{"translation":{"x":4.014996170129343,"y":-2.1657529642234286},"rotation":{"radians":0.18487131031758927}},"curvature":0.0055525725273563005},{"time":0.8428636062198919,"velocity":1.698118794973849,"acceleration":0.0253231394733626,"pose":{"translation":{"x":4.082250383218097,"y":-2.1531645157563517},"rotation":{"radians":0.185176376794578}},"curvature":0.0034191885570685615},{"time":0.8841591774035676,"velocity":1.6991645284825654,"acceleration":0.016949828884694675,"pose":{"translation":{"x":4.151196270887151,"y":-2.140242485266206},"rotation":{"radians":0.185348127438105}},"curvature":0.001517578640164743},{"time":0.9263077178976394,"velocity":1.6998789390316795,"acceleration":-0.020915772434514122,"pose":{"translation":{"x":4.2216012864233665,"y":-2.127039119227935},"rotation":{"radians":0.185393608906121}},"curvature":-2.1980680822174232E-4},{"time":0.9691872552083657,"velocity":1.6989820803871911,"acceleration":-0.019853875112120833,"pose":{"translation":{"x":4.293223727623496,"y":-2.1136086118973263},"rotation":{"radians":0.18531757186828496}},"curvature":-0.001849182069837596},{"time":1.0126678049640467,"velocity":1.6981188229825355,"acceleration":-0.019438079067074412,"pose":{"translation":{"x":4.365814286855553,"y":-2.100006760824233},"rotation":{"radians":0.1851226905021804}},"curvature":-0.003419137593420175},{"time":1.0565953995481074,"velocity":1.697264954925784,"acceleration":-0.01961598803052627,"pose":{"translation":{"x":4.439117601120204,"y":-2.0862906223657953},"rotation":{"radians":0.18480970272003633}},"curvature":-0.004973588312268373},{"time":1.1008137297010812,"velocity":1.6963975686907735,"acceleration":-0.020376770398937767,"pose":{"translation":{"x":4.512873802112148,"y":-2.072518167199662},"rotation":{"radians":0.18437748064958973}},"curvature":-0.006554250817816681},{"time":1.1451650787587457,"velocity":1.6954938314341423,"acceleration":-0.02174865906857091,"pose":{"translation":{"x":4.586820066281499,"y":-2.0587479358372143},"rotation":{"radians":0.1838230366305232}},"curvature":-0.008202877188079381},{"time":1.1894912691710187,"velocity":1.6945297962310573,"acceleration":-0.023800679579925297,"pose":{"translation":{"x":4.6606921648951705,"y":-2.0450386941367853},"rotation":{"radians":0.1831414662100924}},"curvature":-0.00996343941748574},{"time":1.2336346217613723,"velocity":1.6934791544404706,"acceleration":-0.02664873108330119,"pose":{"translation":{"x":4.734226014098256,"y":-2.031449088816883},"rotation":{"radians":0.18232582600629615}},"curvature":-0.011884448939914765},{"time":1.2774389289250685,"velocity":1.692311825238575,"acceleration":-0.03046680385892747,"pose":{"translation":{"x":4.807159224975414,"y":-2.018037302969414},"rotation":{"radians":0.1813669405494641}},"curvature":-0.014021608840024123},{"time":1.3207504437966078,"velocity":1.6909922618101507,"acceleration":-0.03550490906982497,"pose":{"translation":{"x":4.87923265361225,"y":-2.0048607115729036},"rotation":{"radians":0.18025312799179333}},"curvature":-0.016441033887417626},{"time":1.3634188884165654,"velocity":1.6894773225637683,"acceleration":-0.04211637972877536,"pose":{"translation":{"x":4.950191951156699,"y":-1.9919755370057164},"rotation":{"radians":0.17896982951364038}},"curvature":-0.019223341480108017},{"time":1.4052984851902914,"velocity":1.687713505563158,"acceleration":-0.05079880035097404,"pose":{"translation":{"x":5.019789113880411,"y":-1.9794365045592819},"rotation":{"radians":0.1774991208800654}},"curvature":-0.022469026289744613},{"time":1.446249017581491,"velocity":1.6856332676439514,"acceleration":-0.06225527958543467,"pose":{"translation":{"x":5.087784033240128,"y":-1.9672964979513143},"rotation":{"radians":0.17581907627481713}},"curvature":-0.026305702065901784},{"time":1.48613692819827,"velocity":1.683150034616425,"acceleration":-0.07748664065103303,"pose":{"translation":{"x":5.153946045939074,"y":-1.955606214839035},"rotation":{"radians":0.1739029433768764}},"curvature":-0.030898057172033474},{"time":1.5248364654577076,"velocity":1.6801513374794417,"acceleration":-0.09793124518239772,"pose":{"translation":{"x":5.218055483988332,"y":-1.944413822332393},"rotation":{"radians":0.17171807338504597}},"curvature":-0.036461777406136126},{"time":1.5622308942193142,"velocity":1.676489254507933,"acceleration":-0.14430405981446515,"pose":{"translation":{"x":5.27990522476823,"y":-1.9337646125072896},"rotation":{"radians":0.16922452850747088}},"curvature":-0.04328332502065509},{"time":1.6327045242352638,"velocity":1.666319623586769,"acceleration":-2.5496325060863407,"pose":{"translation":{"x":5.396069151255783,"y":-1.9142604671143888},"rotation":{"radians":0.16310370436497154}},"curvature":-0.062384057405159905},{"time":1.700004059382801,"velocity":1.4947305411301082,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.501090654161787,"y":-1.897385524088994},"rotation":{"radians":0.15498967537946623}},"curvature":-0.09338424002657549},{"time":1.766850386843393,"velocity":1.3142454569865103,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.593922492082721,"y":-1.8833634811611886},"rotation":{"radians":0.14401549016527215}},"curvature":-0.14664144594343798},{"time":1.8327108342658995,"velocity":1.136422248945742,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":5.673866955415721,"y":-1.8723389647855806},"rotation":{"radians":0.12876563152136272}},"curvature":-0.24432674544242147},{"time":1.89674347147306,"velocity":0.9635341284864088,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.740625468322835,"y":-1.8643665065644082},"rotation":{"radians":0.10682738395944273}},"curvature":-0.43772210750117546},{"time":2.015399258918648,"velocity":0.6431635023833218,"acceleration":-2.7,"pose":{"translation":{"x":5.835683620117707,"y":-1.8572792752710763},"rotation":{"radians":0.02197361283131086}},"curvature":-1.8474877858052587},{"time":2.068104204745642,"velocity":0.500860148650437,"acceleration":-2.700000000000003,"pose":{"translation":{"x":5.865828193832404,"y":-1.8577238789494634},"rotation":{"radians":-0.06367453439929643}},"curvature":-4.361539640139643},{"time":2.09260418539275,"velocity":0.4347102009032456,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.87723983042111,"y":-1.8587835158903134},"rotation":{"radians":-0.12606797335766265}},"curvature":-6.765173997576896},{"time":2.1160800812268463,"velocity":0.37132528215118565,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.886575890703612,"y":-1.8603172471295863},"rotation":{"radians":-0.20493559496852556}},"curvature":-10.161883925389565},{"time":2.127589396456522,"velocity":0.3402501310310618,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":5.890565694061718,"y":-1.8612389048670965},"rotation":{"radians":-0.2504220751916043}},"curvature":-12.085865426392672},{"time":2.1390945597750513,"velocity":0.30918619007103243,"acceleration":-1.8924820462055663,"pose":{"translation":{"x":5.894162125738611,"y":-1.8622502866084267},"rotation":{"radians":-0.29897795833341534}},"curvature":-13.883657658978745},{"time":2.150587779303661,"velocity":0.2874354784600397,"acceleration":-0.3671051506100357,"pose":{"translation":{"x":5.897412720882245,"y":-1.8633405156596639},"rotation":{"radians":-0.3489700280234347}},"curvature":-15.167811103722578},{"time":2.1617083394694956,"velocity":0.28335306354549294,"acceleration":2.6999999999999984,"pose":{"translation":{"x":5.900367833181723,"y":-1.864498083498372},"rotation":{"radians":-0.39786739118221953}},"curvature":-15.430809298875147},{"time":2.1804709377657456,"velocity":0.3340120789453682,"acceleration":2.6999999999999567,"pose":{"translation":{"x":5.905607407346868,"y":-1.8669659777145693},"rotation":{"radians":-0.47832444070195745}},"curvature":-11.041099262767903},{"time":2.1956824280374048,"velocity":0.3750831026788478,"acceleration":2.699999999999993,"pose":{"translation":{"x":5.910341889267582,"y":-1.8695488554289996},"rotation":{"radians":-0.5104883219168177}},"curvature":-3.7340349071888874E-13},{"time":2.2929148364286727,"velocity":0.63761060533527,"acceleration":2.7000000000000037,"pose":{"translation":{"x":5.952628765990579,"y":-1.8947627456207157},"rotation":{"radians":-0.5725205397695268}},"curvature":-1.2729792340387263},{"time":2.3940778583761566,"velocity":0.9107507645934767,"acceleration":2.6999999999999997,"pose":{"translation":{"x":6.016899459359807,"y":-1.939518290288375},"rotation":{"radians":-0.6341961814427468}},"curvature":-0.4951458179596235},{"time":2.499582823433522,"velocity":1.1956141702483631,"acceleration":2.700000000000001,"pose":{"translation":{"x":6.104929493165445,"y":-2.0073229658763747},"rotation":{"radians":-0.6751734667579602}},"curvature":-0.30598432766039185},{"time":2.549871630357917,"velocity":1.331393948944229,"acceleration":-2.3703085481660073,"pose":{"translation":{"x":6.154145099583976,"y":-2.0475121420294213},"rotation":{"radians":-0.6943965009615569}},"curvature":-0.30719753844441644},{"time":2.601064659117595,"velocity":1.210050675268655,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.203697633427262,"y":-2.0896586153054586},"rotation":{"radians":-0.7157907005915123}},"curvature":-0.36026510676615386},{"time":2.7165416194940355,"velocity":0.8982628822522657,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.293359406274425,"y":-2.171994264734542},"rotation":{"radians":-0.7777408162553153}},"curvature":-0.7709264287429204},{"time":2.778891049560791,"velocity":0.7299194210720268,"acceleration":-2.7,"pose":{"translation":{"x":6.328670510091029,"y":-2.208456747165414},"rotation":{"radians":-0.8305100864157245}},"curvature":-1.4222448009063526},{"time":2.842695131750204,"velocity":0.5576483991606114,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.355220615825325,"y":-2.239798940322541},"rotation":{"radians":-0.9168250753018049}},"curvature":-3.1442989963643533},{"time":2.8749582012569603,"velocity":0.4705381114923698,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.36490619532735,"y":-2.2532634290570734},"rotation":{"radians":-0.9827537906480719}},"curvature":-5.034987197192603},{"time":2.891239770434675,"velocity":0.42657787471254105,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.3688352966753286,"y":-2.259419669934672},"rotation":{"radians":-1.0244849695664913}},"curvature":-6.4853718103581715},{"time":2.907713203045178,"velocity":0.3820996066641824,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.372160835205086,"y":-2.2651909502257763},"rotation":{"radians":-1.0737622012988202}},"curvature":-8.435878662280569},{"time":2.924526900044579,"velocity":0.3367026247657997,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.374893052354147,"y":-2.270580863405573},"rotation":{"radians":-1.1321258392888536}},"curvature":-11.04518545418917},{"time":2.941957874367017,"velocity":0.28963899409521776,"acceleration":-2.7,"pose":{"translation":{"x":6.377047337038721,"y":-2.2755966728813544},"rotation":{"radians":-1.2012322168330674}},"curvature":-14.476500929092555},{"time":2.96055398358937,"velocity":0.23942949919486523,"acceleration":-2.448027153795687,"pose":{"translation":{"x":6.3786444689861534,"y":-2.280249495489123},"rotation":{"radians":-1.2826082131505}},"curvature":-18.827812193648736},{"time":2.970567416073978,"velocity":0.21491634456984543,"acceleration":-1.9961771142903886,"pose":{"translation":{"x":6.379242130280754,"y":-2.2824443595580353},"rotation":{"radians":-1.3282237761827782}},"curvature":-21.327328725413906},{"time":2.981144665114215,"velocity":0.19380228210357486,"acceleration":-1.6000282816415057,"pose":{"translation":{"x":6.379710862067385,"y":-2.2845544849901995},"rotation":{"radians":-1.3771567268610405}},"curvature":-23.98711892404214},{"time":2.992268979172749,"velocity":0.17600306499605822,"acceleration":-1.2553869860651006,"pose":{"translation":{"x":6.380054886336579,"y":-2.286582426880274},"rotation":{"radians":-1.429290793886386}},"curvature":-26.72507006593706},{"time":3.0038952409682596,"velocity":0.16140760724138803,"acceleration":-0.9551676954781074,"pose":{"translation":{"x":6.380278807629404,"y":-2.288531015567824},"rotation":{"radians":-1.484358701225411}},"curvature":-29.420806330750263},{"time":3.0159445119389567,"velocity":0.14989853285611587,"acceleration":-0.6898335911409661,"pose":{"translation":{"x":6.380387620641595,"y":-2.290403362371591},"rotation":{"radians":-1.5419143492300567}},"curvature":-31.91668188022513},{"time":3.0283006223384077,"velocity":0.14137487284672845,"acceleration":-0.44743303545741403,"pose":{"translation":{"x":6.380386717827701,"y":-2.2922028653237634},"rotation":{"radians":-1.6013160616668083}},"curvature":-34.02706070206741},{"time":3.0408101331571995,"velocity":0.13577770444898907,"acceleration":-0.212921320679107,"pose":{"translation":{"x":6.380281897005218,"y":-2.2939332149042446},"rotation":{"radians":-1.6617277302102624}},"curvature":-35.55699082407614},{"time":3.0532866132644054,"velocity":0.13312119582713622,"acceleration":0.03378972806083901,"pose":{"translation":{"x":6.380079368958727,"y":-2.295598399774919},"rotation":{"radians":-1.7221425921088367}},"curvature":-36.3281418998923},{"time":3.065519296764733,"velocity":0.1335345348760666,"acceleration":0.32225443065354187,"pose":{"translation":{"x":6.379785765044043,"y":-2.2972027125139265},"rotation":{"radians":-1.781429772113809}},"curvature":-36.206139172711836},{"time":3.077285012514349,"velocity":0.13732608890619047,"acceleration":0.7012784582506274,"pose":{"translation":{"x":6.379408144792344,"y":-2.2987507553499262},"rotation":{"radians":-1.8383976967879823}},"curvature":-35.12127713858841},{"time":3.0883612496811925,"velocity":0.14509361542977273,"acceleration":1.2617209489418255,"pose":{"translation":{"x":6.3789540035143135,"y":-2.3002474458963693},"rotation":{"radians":-1.8918633161545486}},"curvature":-33.07584393236491},{"time":3.098537803842263,"velocity":0.157933587002836,"acceleration":2.7000000000000473,"pose":{"translation":{"x":6.378431279904281,"y":-2.3016980228857653},"rotation":{"radians":-1.940714098059974}},"curvature":-30.13585854641613},{"time":3.1153645172946947,"velocity":0.20336571332440306,"acceleration":2.6999999999999815,"pose":{"translation":{"x":6.3772141030085905,"y":-2.304483431124363},"rotation":{"radians":-2.0207139247079544}},"curvature":-22.0271903748647},{"time":3.1406589917681282,"velocity":0.2716607944026734,"acceleration":0.8024179910851177,"pose":{"translation":{"x":6.374357058736877,"y":-2.309768375181916},"rotation":{"radians":-2.089942441041602}},"curvature":-3.6876165474616885E-12},{"time":3.1622589055614276,"velocity":0.2889929538363045,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.371273053807943,"y":-2.3149791695909654},"rotation":{"radians":-2.134140449789665}},"curvature":-13.455982774139883},{"time":3.1737418742140093,"velocity":0.25798893847433346,"acceleration":-1.4383587980679373,"pose":{"translation":{"x":6.369534239034446,"y":-2.3175943536800085},"rotation":{"radians":-2.1829835922357548}},"curvature":-17.25132655214485},{"time":3.186933448272058,"velocity":0.23901472186757433,"acceleration":0.1738347345922304,"pose":{"translation":{"x":6.367572227256497,"y":-2.3202205006717915},"rotation":{"radians":-2.242778936332965}},"curvature":-18.865841343006046},{"time":3.201357086015986,"velocity":0.2415220511066446,"acceleration":1.2074390857232837,"pose":{"translation":{"x":6.365327025522202,"y":-2.3228603994487775},"rotation":{"radians":-2.30822787316994}},"curvature":-18.637946847005626},{"time":3.2161458477403038,"velocity":0.2593785800420343,"acceleration":2.1336945716205817,"pose":{"translation":{"x":6.362745547798263,"y":-2.3255164261590364},"rotation":{"radians":-2.374763726729108}},"curvature":-17.142365443426765},{"time":3.2306694918655126,"velocity":0.2903676006721412,"acceleration":2.700000000000074,"pose":{"translation":{"x":6.359781371637524,"y":-2.3281905601724713},"rotation":{"radians":-2.4389742624979664}},"curvature":-14.983480497841681},{"time":3.2446628052837676,"velocity":0.3281495469014306,"acceleration":2.6999999999998945,"pose":{"translation":{"x":6.356394494846521,"y":-2.330884400037045},"rotation":{"radians":-2.4986892553126565}},"curvature":-12.6428063971659},{"time":3.2582435491927177,"velocity":0.364817555455595,"acceleration":2.7000000000000877,"pose":{"translation":{"x":6.352551092153018,"y":-2.333599179435007},"rotation":{"radians":-2.552808677946185}},"curvature":-10.425243073509822},{"time":3.2716173390145897,"velocity":0.400926787974651,"acceleration":2.7000000000000393,"pose":{"translation":{"x":6.34822327187356,"y":-2.336335783139118},"rotation":{"radians":-2.6010138665326594}},"curvature":-8.480273098517067},{"time":3.2981771443200105,"velocity":0.4726382622992881,"acceleration":2.6999999999999944,"pose":{"translation":{"x":6.338031019772121,"y":-2.341876353746762},"rotation":{"radians":-2.680649682418315}},"curvature":-5.526691285519326},{"time":3.3248223581697265,"velocity":0.5445803396935209,"acceleration":2.7000000000000046,"pose":{"translation":{"x":6.3257040302168415,"y":-2.3475068181889385},"rotation":{"radians":-2.7413418618503456}},"curvature":-3.621027769968688},{"time":3.3516517412505586,"velocity":0.6170196740117675,"acceleration":2.6999999999999864,"pose":{"translation":{"x":6.311207959025704,"y":-2.3532233214413143},"rotation":{"radians":-2.787500682952535}},"curvature":-2.421126879189059},{"time":3.4055288550540843,"velocity":0.7624878812812861,"acceleration":2.6999999999999966,"pose":{"translation":{"x":6.275921295893175,"y":-2.3648792811083497},"rotation":{"radians":-2.8500310809579763}},"curvature":-1.1659746173848968},{"time":3.458689461018175,"velocity":0.9060215173843309,"acceleration":2.700000000000006,"pose":{"translation":{"x":6.23318766752019,"y":-2.3767415682587734},"rotation":{"radians":-2.887571818016487}},"curvature":-0.6087466403424915},{"time":3.557675298971284,"velocity":1.1732832798577255,"acceleration":-1.7282800282634867,"pose":{"translation":{"x":6.133054298331776,"y":-2.4004892897255963},"rotation":{"radians":-2.922362067786142}},"curvature":-0.15135507094459147},{"time":3.654622402712532,"velocity":1.005731536663738,"acceleration":-2.699999999999999,"pose":{"translation":{"x":6.029885473338478,"y":-2.423133209081552},"rotation":{"radians":-2.9237451596146884}},"curvature":0.1383938594851771},{"time":3.7573878685624846,"velocity":0.7282647788688656,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":5.943142240414719,"y":-2.443479477201466},"rotation":{"radians":-2.8895449379618605}},"curvature":0.7880473353914925},{"time":3.809391276974363,"velocity":0.5878555761567938,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.910154408569832,"y":-2.452584709951119},"rotation":{"radians":-2.850242690365192}},"curvature":1.6400683797905078},{"time":3.861155536589642,"velocity":0.44809207519554023,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.884695555551516,"y":-2.4609965262266837},"rotation":{"radians":-2.787675545983974}},"curvature":3.22050410776737},{"time":3.915948019182654,"velocity":0.30015237219440816,"acceleration":-2.7,"pose":{"translation":{"x":5.8657554437247095,"y":-2.468837742167634},"rotation":{"radians":-2.7058643030528273}},"curvature":4.433577634625505},{"time":4.027115564439842,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.850852764976659,"y":-2.476337923196535},"rotation":{"radians":-2.657114724552762}},"curvature":7.099564484227281E-14}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.16408328867307112,"velocity":0.443024879417292,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.2363298344524436,"y":-2.298899802686018},"rotation":{"radians":0.0692577651470374}},"curvature":1.8950587512445145},{"time":0.27165252251845773,"velocity":0.733461810799836,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.2992357901794267,"y":-2.292058382559469},"rotation":{"radians":0.13336588761066984}},"curvature":0.5081353245388296},{"time":0.39146527816103716,"velocity":1.0569562510348003,"acceleration":2.700000000000003,"pose":{"translation":{"x":3.4052753217803797,"y":-2.2759406702466065},"rotation":{"radians":0.1622880652133495}},"curvature":0.14308012858303878},{"time":0.45511058967808826,"velocity":1.2287985921308384,"acceleration":2.6999999999999993,"pose":{"translation":{"x":3.4770070197266802,"y":-2.2638785389701312},"rotation":{"radians":0.17018012232912796}},"curvature":0.08218998056047282},{"time":0.5202669048089674,"velocity":1.404720642984212,"acceleration":2.6999999999999997,"pose":{"translation":{"x":3.5615193955328435,"y":-2.249097623342679},"rotation":{"radians":0.17564390475973646}},"curvature":0.04917485937853854},{"time":0.5862126558482816,"velocity":1.5827741707903604,"acceleration":1.5865644228393085,"pose":{"translation":{"x":3.658474466495589,"y":-2.231684860998854},"rotation":{"radians":0.17945016920991327}},"curvature":0.030140843221192568},{"time":0.6537218964528062,"velocity":1.6898819301463979,"acceleration":0.05788196551324373,"pose":{"translation":{"x":3.7671397522485237,"y":-2.211813471951235},"rotation":{"radians":0.18207557565508717}},"curvature":0.018479758912245518},{"time":0.7254285040062793,"velocity":1.6940324495318797,"acceleration":0.04142453277389165,"pose":{"translation":{"x":3.8864378767263954,"y":-2.1897319350134796},"rotation":{"radians":0.18381956783311507}},"curvature":0.010872498721873012},{"time":0.7633704400602459,"velocity":1.6956041765054521,"acceleration":0.03427111458881184,"pose":{"translation":{"x":3.949655472782524,"y":-2.177957876460112},"rotation":{"radians":0.18442327195140867}},"curvature":0.008001487996222786},{"time":0.8025567568325975,"velocity":1.6969471352578709,"acceleration":0.02906850160180063,"pose":{"translation":{"x":4.014996170129343,"y":-2.1657529642234286},"rotation":{"radians":0.18487131031758927}},"curvature":0.0055525725273563005},{"time":0.8428636062198919,"velocity":1.698118794973849,"acceleration":0.0253231394733626,"pose":{"translation":{"x":4.082250383218097,"y":-2.1531645157563517},"rotation":{"radians":0.185176376794578}},"curvature":0.0034191885570685615},{"time":0.8841591774035676,"velocity":1.6991645284825654,"acceleration":0.016949828884694675,"pose":{"translation":{"x":4.151196270887151,"y":-2.140242485266206},"rotation":{"radians":0.185348127438105}},"curvature":0.001517578640164743},{"time":0.9263077178976394,"velocity":1.6998789390316795,"acceleration":-0.020915772434514122,"pose":{"translation":{"x":4.2216012864233665,"y":-2.127039119227935},"rotation":{"radians":0.185393608906121}},"curvature":-2.1980680822174232E-4},{"time":0.9691872552083657,"velocity":1.6989820803871911,"acceleration":-0.019853875112120833,"pose":{"translation":{"x":4.293223727623496,"y":-2.1136086118973263},"rotation":{"radians":0.18531757186828496}},"curvature":-0.001849182069837596},{"time":1.0126678049640467,"velocity":1.6981188229825355,"acceleration":-0.019438079067074412,"pose":{"translation":{"x":4.365814286855553,"y":-2.100006760824233},"rotation":{"radians":0.1851226905021804}},"curvature":-0.003419137593420175},{"time":1.0565953995481074,"velocity":1.697264954925784,"acceleration":-0.01961598803052627,"pose":{"translation":{"x":4.439117601120204,"y":-2.0862906223657953},"rotation":{"radians":0.18480970272003633}},"curvature":-0.004973588312268373},{"time":1.1008137297010812,"velocity":1.6963975686907735,"acceleration":-0.020376770398937767,"pose":{"translation":{"x":4.512873802112148,"y":-2.072518167199662},"rotation":{"radians":0.18437748064958973}},"curvature":-0.006554250817816681},{"time":1.1451650787587457,"velocity":1.6954938314341423,"acceleration":-0.02174865906857091,"pose":{"translation":{"x":4.586820066281499,"y":-2.0587479358372143},"rotation":{"radians":0.1838230366305232}},"curvature":-0.008202877188079381},{"time":1.1894912691710187,"velocity":1.6945297962310573,"acceleration":-0.023800679579925297,"pose":{"translation":{"x":4.6606921648951705,"y":-2.0450386941367853},"rotation":{"radians":0.1831414662100924}},"curvature":-0.00996343941748574},{"time":1.2336346217613723,"velocity":1.6934791544404706,"acceleration":-0.02664873108330119,"pose":{"translation":{"x":4.734226014098256,"y":-2.031449088816883},"rotation":{"radians":0.18232582600629615}},"curvature":-0.011884448939914765},{"time":1.2774389289250685,"velocity":1.692311825238575,"acceleration":-0.03046680385892747,"pose":{"translation":{"x":4.807159224975414,"y":-2.018037302969414},"rotation":{"radians":0.1813669405494641}},"curvature":-0.014021608840024123},{"time":1.3207504437966078,"velocity":1.6909922618101507,"acceleration":-0.03550490906982497,"pose":{"translation":{"x":4.87923265361225,"y":-2.0048607115729036},"rotation":{"radians":0.18025312799179333}},"curvature":-0.016441033887417626},{"time":1.3634188884165654,"velocity":1.6894773225637683,"acceleration":-0.04211637972877536,"pose":{"translation":{"x":4.950191951156699,"y":-1.9919755370057164},"rotation":{"radians":0.17896982951364038}},"curvature":-0.019223341480108017},{"time":1.4052984851902914,"velocity":1.687713505563158,"acceleration":-0.05079880035097404,"pose":{"translation":{"x":5.019789113880411,"y":-1.9794365045592819},"rotation":{"radians":0.1774991208800654}},"curvature":-0.022469026289744613},{"time":1.446249017581491,"velocity":1.6856332676439514,"acceleration":-0.06225527958543467,"pose":{"translation":{"x":5.087784033240128,"y":-1.9672964979513143},"rotation":{"radians":0.17581907627481713}},"curvature":-0.026305702065901784},{"time":1.48613692819827,"velocity":1.683150034616425,"acceleration":-0.07748664065103303,"pose":{"translation":{"x":5.153946045939074,"y":-1.955606214839035},"rotation":{"radians":0.1739029433768764}},"curvature":-0.030898057172033474},{"time":1.5248364654577076,"velocity":1.6801513374794417,"acceleration":-0.09793124518239772,"pose":{"translation":{"x":5.218055483988332,"y":-1.944413822332393},"rotation":{"radians":0.17171807338504597}},"curvature":-0.036461777406136126},{"time":1.5622308942193142,"velocity":1.676489254507933,"acceleration":-0.14430405981446515,"pose":{"translation":{"x":5.27990522476823,"y":-1.9337646125072896},"rotation":{"radians":0.16922452850747088}},"curvature":-0.04328332502065509},{"time":1.6327045242352638,"velocity":1.666319623586769,"acceleration":-2.5496325060863407,"pose":{"translation":{"x":5.396069151255783,"y":-1.9142604671143888},"rotation":{"radians":0.16310370436497154}},"curvature":-0.062384057405159905},{"time":1.700004059382801,"velocity":1.4947305411301082,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.501090654161787,"y":-1.897385524088994},"rotation":{"radians":0.15498967537946623}},"curvature":-0.09338424002657549},{"time":1.766850386843393,"velocity":1.3142454569865103,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.593922492082721,"y":-1.8833634811611886},"rotation":{"radians":0.14401549016527215}},"curvature":-0.14664144594343798},{"time":1.8327108342658995,"velocity":1.136422248945742,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":5.673866955415721,"y":-1.8723389647855806},"rotation":{"radians":0.12876563152136272}},"curvature":-0.24432674544242147},{"time":1.89674347147306,"velocity":0.9635341284864088,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.740625468322835,"y":-1.8643665065644082},"rotation":{"radians":0.10682738395944273}},"curvature":-0.43772210750117546},{"time":2.015399258918648,"velocity":0.6431635023833218,"acceleration":-2.7,"pose":{"translation":{"x":5.835683620117707,"y":-1.8572792752710763},"rotation":{"radians":0.02197361283131086}},"curvature":-1.8474877858052587},{"time":2.068104204745642,"velocity":0.500860148650437,"acceleration":-2.700000000000003,"pose":{"translation":{"x":5.865828193832404,"y":-1.8577238789494634},"rotation":{"radians":-0.06367453439929643}},"curvature":-4.361539640139643},{"time":2.09260418539275,"velocity":0.4347102009032456,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.87723983042111,"y":-1.8587835158903134},"rotation":{"radians":-0.12606797335766265}},"curvature":-6.765173997576896},{"time":2.1160800812268463,"velocity":0.37132528215118565,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.886575890703612,"y":-1.8603172471295863},"rotation":{"radians":-0.20493559496852556}},"curvature":-10.161883925389565},{"time":2.127589396456522,"velocity":0.3402501310310618,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":5.890565694061718,"y":-1.8612389048670965},"rotation":{"radians":-0.2504220751916043}},"curvature":-12.085865426392672},{"time":2.1390945597750513,"velocity":0.30918619007103243,"acceleration":-1.8924820462055663,"pose":{"translation":{"x":5.894162125738611,"y":-1.8622502866084267},"rotation":{"radians":-0.29897795833341534}},"curvature":-13.883657658978745},{"time":2.150587779303661,"velocity":0.2874354784600397,"acceleration":-0.3671051506100357,"pose":{"translation":{"x":5.897412720882245,"y":-1.8633405156596639},"rotation":{"radians":-0.3489700280234347}},"curvature":-15.167811103722578},{"time":2.1617083394694956,"velocity":0.28335306354549294,"acceleration":2.6999999999999984,"pose":{"translation":{"x":5.900367833181723,"y":-1.864498083498372},"rotation":{"radians":-0.39786739118221953}},"curvature":-15.430809298875147},{"time":2.1804709377657456,"velocity":0.3340120789453682,"acceleration":2.6999999999999567,"pose":{"translation":{"x":5.905607407346868,"y":-1.8669659777145693},"rotation":{"radians":-0.47832444070195745}},"curvature":-11.041099262767903},{"time":2.1956824280374048,"velocity":0.3750831026788478,"acceleration":2.699999999999998,"pose":{"translation":{"x":5.910341889267582,"y":-1.8695488554289996},"rotation":{"radians":-0.5104883219168177}},"curvature":-3.7340349071888874E-13},{"time":2.388769937245301,"velocity":0.8964193775401674,"acceleration":2.699999999999994,"pose":{"translation":{"x":6.019362680912478,"y":-1.9259705717486812},"rotation":{"radians":-0.457653588008675}},"curvature":0.08815631236043758},{"time":2.4883624125512283,"velocity":1.165319060866171,"acceleration":-0.46150344014918243,"pose":{"translation":{"x":6.111478234899523,"y":-1.9713048863389462},"rotation":{"radians":-0.4603174376989857}},"curvature":-0.11129184244351757},{"time":2.5906580769043837,"velocity":1.1181092598548437,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.215595458285448,"y":-2.024220578585429},"rotation":{"radians":-0.48395842928784394}},"curvature":-0.3145673826484365},{"time":2.70322104072566,"velocity":0.814189257537398,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.310606314256721,"y":-2.0771362708319114},"rotation":{"radians":-0.542721169737204}},"curvature":-0.9207690115471886},{"time":2.764138320436643,"velocity":0.6497126023177433,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.348182799436799,"y":-2.1011391557636725},"rotation":{"radians":-0.6009884368276502}},"curvature":-1.8648205143316683},{"time":2.795342233849168,"velocity":0.565462036103926,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.363599732828538,"y":-2.1121739062004825},"rotation":{"radians":-0.6444585240143699}},"curvature":-2.8243017945696063},{"time":2.826997761556873,"velocity":0.4799921112931212,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.376553043989027,"y":-2.1224705854221755},"rotation":{"radians":-0.7033422239728307}},"curvature":-4.491242440772023},{"time":2.8593054766288244,"velocity":0.3927612805988525,"acceleration":-2.7,"pose":{"translation":{"x":6.38695681395706,"y":-2.131985026593671},"rotation":{"radians":-0.7852287507835739}},"curvature":-7.515004253757817},{"time":2.8759119946945018,"velocity":0.34792368182152367,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.39119492327807,"y":-2.1364417236195927},"rotation":{"radians":-0.8382640323882593}},"curvature":-9.890568239576622},{"time":2.89307895299441,"velocity":0.3015728944117711,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":6.394795994947252,"y":-2.140697571643865},"rotation":{"radians":-0.9018170428636533}},"curvature":-13.122491542036409},{"time":2.9112357590757947,"velocity":0.2525495179920331,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.397770582878232,"y":-2.144754445523107},"rotation":{"radians":-0.9780469927814595}},"curvature":-17.454602821326706},{"time":2.920942595671849,"velocity":0.22634105918268688,"acceleration":-2.4860393459854366,"pose":{"translation":{"x":6.399027731224297,"y":-2.1467093739969005},"rotation":{"radians":-1.02159743124877}},"curvature":-20.09502862925046},{"time":2.9312692866346604,"velocity":0.20066849913530505,"acceleration":-2.0189603351796466,"pose":{"translation":{"x":6.400134707587854,"y":-2.1486161348611237},"rotation":{"radians":-1.069089430112289}},"curvature":-23.060751341810086},{"time":2.9423098653006123,"velocity":0.17837800873131748,"acceleration":-1.6143542252704404,"pose":{"translation":{"x":6.401094411229809,"y":-2.1504755167024108},"rotation":{"radians":-1.1206887834018617}},"curvature":-26.32815672730302},{"time":2.954081692119802,"velocity":0.15937411036660665,"acceleration":-1.2688547292128112,"pose":{"translation":{"x":6.4019101154848554,"y":-2.1522884397462643},"rotation":{"radians":-1.1764606640265038}},"curvature":-29.835574717517932},{"time":2.9665754482745217,"velocity":0.14352134878405876,"acceleration":-0.9763706463097248,"pose":{"translation":{"x":6.402585475864242,"y":-2.154055958848849},"rotation":{"radians":-1.2363268615194787}},"curvature":-33.47199907937164},{"time":2.979746600520624,"velocity":0.13066142235288813,"acceleration":-0.7288747558571244,"pose":{"translation":{"x":6.4031245381585515,"y":-2.155779266488783},"rotation":{"radians":-1.3000214411236208}},"curvature":-37.0701428019739},{"time":2.993507961719939,"velocity":0.12063111356847578,"acceleration":-0.5170296138934241,"pose":{"translation":{"x":6.40353174654046,"y":-2.1574596957589307},"rotation":{"radians":-1.3670526349960594}},"curvature":-40.409104950567425},{"time":3.007725368222374,"velocity":0.11328029337395595,"acceleration":-0.33053844199721205,"pose":{"translation":{"x":6.403811951667518,"y":-2.159098723358196},"rotation":{"radians":-1.4366815305525695}},"curvature":-43.23155333801457},{"time":3.022218591265303,"velocity":0.1084897260098282,"acceleration":-0.15811487091757606,"pose":{"translation":{"x":6.403970418784919,"y":-2.160697972583314},"rotation":{"radians":-1.5079284352910682}},"curvature":-45.276810326152486},{"time":3.036768883040763,"velocity":0.10618910850393826,"acceleration":0.01307341057034199,"pose":{"translation":{"x":6.4040128358282695,"y":-2.1622592163206433},"rotation":{"radians":-1.579613900421947}},"curvature":-46.324613583278015},{"time":3.051132960822136,"velocity":0.10637689599023847,"acceleration":0.19876164239942998,"pose":{"translation":{"x":6.40394532152636,"y":-2.16378438003796},"rotation":{"radians":-1.6504330500073743}},"curvature":-46.23738812271767},{"time":3.0650612219497555,"velocity":0.10914530004773226,"acceleration":0.4203441776464799,"pose":{"translation":{"x":6.403774433503942,"y":-2.1652755447762466},"rotation":{"radians":-1.7190515676349232}},"curvature":-44.98631977809164},{"time":3.0783164615029475,"velocity":0.11471706281722582,"acceleration":0.7104111631016262,"pose":{"translation":{"x":6.403507176384488,"y":-2.16673495014149},"rotation":{"radians":-1.7842040253596458}},"curvature":-42.651445664603145},{"time":3.0906891386144704,"velocity":0.12350675075470352,"acceleration":1.1244324574076787,"pose":{"translation":{"x":6.403151009892975,"y":-2.168164997296468},"rotation":{"radians":-1.8447742063981305}},"curvature":-39.39638825688053},{"time":3.102006427709611,"velocity":0.13623227794314552,"acceleration":1.7674578002780963,"pose":{"translation":{"x":6.402713856958646,"y":-2.169568251952547},"rotation":{"radians":-1.8998432378181598}},"curvature":-35.42804729881617},{"time":3.112134237651494,"velocity":0.15413275462466103,"acceleration":2.700000000000108,"pose":{"translation":{"x":6.40220411181779,"y":-2.1709474473614714},"rotation":{"radians":-1.9487014078318636}},"curvature":-30.955102394649295},{"time":3.128848467974634,"velocity":0.19926117649714015,"acceleration":2.7000000000000246,"pose":{"translation":{"x":6.40100282701348,"y":-2.1736454490974797},"rotation":{"radians":-2.0258546267077726}},"curvature":-21.160549507806387},{"time":3.1543931615383447,"velocity":0.2682318491191599,"acceleration":1.4900650944686238,"pose":{"translation":{"x":6.398152708453254,"y":-2.1788923017418553},"rotation":{"radians":-2.0899424410415515}},"curvature":-2.945304125570491E-12},{"time":3.1757095034106175,"velocity":0.29999458608479385,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":6.39507216638006,"y":-2.18410655900665},"rotation":{"radians":-2.1318581991327763}},"curvature":-12.749079953629735},{"time":3.186732200511364,"velocity":0.27023330391277856,"acceleration":-1.5622710730940672,"pose":{"translation":{"x":6.393341297652253,"y":-2.186729689141383},"rotation":{"radians":-2.178143871089744}},"curvature":-16.32981615964336},{"time":3.1993334231752146,"velocity":0.2505467782594274,"acceleration":0.1719853395949932,"pose":{"translation":{"x":6.3913942692807435,"y":-2.189370819539606},"rotation":{"radians":-2.2347884625292824}},"curvature":-17.85543237008885},{"time":3.2131117080900946,"velocity":0.2529164412695496,"acceleration":1.270986424445406,"pose":{"translation":{"x":6.389172967824155,"y":-2.1920346185943},"rotation":{"radians":-2.296809961481927}},"curvature":-17.659220798825885},{"time":3.2272574885484624,"velocity":0.27089553619532014,"acceleration":2.238272955222904,"pose":{"translation":{"x":6.3866259633725395,"y":-2.1947251185768817},"rotation":{"radians":-2.359939148599949}},"curvature":-16.28235115373208},{"time":3.241182801861523,"velocity":0.3020641883769496,"acceleration":2.6999999999999478,"pose":{"translation":{"x":6.383708274193028,"y":-2.197445739571543},"rotation":{"radians":-2.4209897906426305}},"curvature":-14.283774338572098},{"time":3.254667728224852,"velocity":0.338473489557936,"acceleration":2.7000000000000957,"pose":{"translation":{"x":6.380381131375498,"y":-2.2001993134095907},"rotation":{"radians":-2.477923759670583}},"curvature":-12.106402683981111},{"time":3.267829800330271,"velocity":0.37401108424256774,"acceleration":2.6999999999999553,"pose":{"translation":{"x":6.376611743478227,"y":-2.202988107603785},"rotation":{"radians":-2.5296884017885226}},"curvature":-10.032118530851717},{"time":3.2808394361059166,"velocity":0.40913710083681054,"acceleration":2.699999999999969,"pose":{"translation":{"x":6.372373061173554,"y":-2.2058138492826806},"rotation":{"radians":-2.5759534641472417}},"curvature":-8.20177097536335},{"time":3.306764556237397,"velocity":0.4791349251918062,"acceleration":2.700000000000016,"pose":{"translation":{"x":6.362406914475598,"y":-2.211580525293808},"rotation":{"radians":-2.6527662519604673}},"curvature":-5.3971712559868905},{"time":3.332842938639764,"velocity":0.5495465576781979,"acceleration":2.700000000000018,"pose":{"translation":{"x":6.350372195476547,"y":-2.217503260292213},"rotation":{"radians":-2.711679723460505}},"curvature":-3.566532900623742},{"time":3.359143548662012,"velocity":0.6205582047382671,"acceleration":2.699999999999992,"pose":{"translation":{"x":6.336235219518413,"y":-2.223578858777592},"rotation":{"radians":-2.7567394084285106}},"curvature":-2.4023315616831367},{"time":3.412038202869246,"velocity":0.7633737710977991,"acceleration":2.6999999999999913,"pose":{"translation":{"x":6.30185862579485,"y":-2.236144887853593},"rotation":{"radians":-2.8182177716287375}},"curvature":-1.1714173696573198},{"time":3.4643008903546346,"velocity":0.9044830273083475,"acceleration":2.6999999999999935,"pose":{"translation":{"x":6.260257688103602,"y":-2.249139865685754},"rotation":{"radians":-2.855491724158334}},"curvature":-0.6192913719087216},{"time":3.561763628351173,"velocity":1.1676324198990002,"acceleration":-1.756930706235751,"pose":{"translation":{"x":6.162798860477242,"y":-2.2755621287146326},"rotation":{"radians":-2.890852210433827}},"curvature":-0.16501260308299173},{"time":3.6574011703443383,"velocity":0.9996038857022966,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.062304577045997,"y":-2.3008805896326443},"rotation":{"radians":-2.8940333591153284}},"curvature":0.1141648975399554},{"time":3.7589808786552377,"velocity":0.7253386732628685,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.97760410421294,"y":-2.3232696178432635},"rotation":{"radians":-2.8640180725634394}},"curvature":0.7144006666381507},{"time":3.8104841074968494,"velocity":0.5862799553905166,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.945267638157282,"y":-2.3330262163821494},"rotation":{"radians":-2.8288298217795607}},"curvature":1.4855537428430912},{"time":3.8618873556309987,"velocity":0.44749118542831345,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.920198043313465,"y":-2.3418272908322164},"rotation":{"radians":-2.7729042131356465}},"curvature":2.8905757965382564},{"time":3.9164961820908406,"velocity":0.3000473539867403,"acceleration":-2.7,"pose":{"translation":{"x":5.901422525991386,"y":-2.3498331012778997},"rotation":{"radians":-2.7002183095133314}},"curvature":3.93415202030435},{"time":4.027624831715559,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.886546239551206,"y":-2.3573596746146754},"rotation":{"radians":-2.6571147245527174}},"curvature":4.1076051658711784E-13}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 084e7b8..4504f62 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -82,6 +82,8 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); m_robotContainer.resetOdometry(new Pose2d()); + m_robotContainer.resetGyroYawRobotContainer(0); + //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); /* diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 771b2f1..b7db8a8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -325,8 +325,8 @@ public class RobotContainer { //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); @@ -438,6 +438,10 @@ public class RobotContainer { m_robotDrive.setOdometry(pose); } + public void resetGyroYawRobotContainer(double angle) { + m_robotDrive.resetGyroYaw(angle); + } + /** * Used for analog inputs like triggers and axises. * @return IHandController interface for the Driver Controller. diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java index bce3980..9a655de 100644 --- a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java @@ -22,7 +22,7 @@ public class FiveBallAutoMiddle extends SequentialCommandGroup { // Use addRequirements() here to declare subsystem dependencies. addCommands( paths[0], - new TankDriveVelocity(drive, -3.1, -0.3, 0.97) + new TankDriveVelocity(drive, -3.2, -0.2, 0.8) ); } } From 3ec37422c63bc3011b4a2dbdffa50f2e77afb5ca Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 11 Mar 2020 20:52:52 -0600 Subject: [PATCH 35/39] fixed reimporting paths problem --- PathWeaver/Paths/TenBallMidComplete | 11 ++++++----- src/main/deploy/paths/TenBallMidComplete.wpilib.json | 2 +- src/main/java/frc4388/robot/Robot.java | 5 ++++- src/main/java/frc4388/robot/RobotContainer.java | 9 ++++++--- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/PathWeaver/Paths/TenBallMidComplete b/PathWeaver/Paths/TenBallMidComplete index 9363e21..ead609f 100644 --- a/PathWeaver/Paths/TenBallMidComplete +++ b/PathWeaver/Paths/TenBallMidComplete @@ -2,8 +2,9 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name 3.2,-2.4,0.2,2.5,true, 5.0,-1.1,3.0,0.0,true, 7.2,-1.1,1.5,0.0,true, -6.6123135559006245,-2.7023965955020937,-0.39262822032017564,-0.17846737287280634,true, -6.314867934445947,-2.8094770192257776,-0.3292650584281963,-0.18335832609507596,false, -5.862750589834837,-3.511448685858817,-0.1090388144136282,-0.46596647260741486,false, -5.886546239551211,-4.177726877917294,-0.07863878356842902,-0.48088836895271725,false, -5.553407143521973,-4.939187668841268,0.3688325706038009,-0.5235042937602339,true, +6.981146126504424,-1.7267749571307522,-0.16656954801462032,-0.6424825423421034,true, +6.766985279057057,-2.6072139966365975,-0.1189782485818709,-0.2974456214546768,true, +6.564722256467875,-3.1783095898295777,-0.2022630225891815,-0.44021951975292284,true, +6.267276635013197,-3.880281256462616,-0.206563341919694,-0.4559320155220153,false, +5.946035363842147,-4.546559448521093,-0.14277389829824472,-0.2974456214546777,true, +5.660487567245656,-5.046268092564953,0.13087607344005825,-0.42832169489473504,true, diff --git a/src/main/deploy/paths/TenBallMidComplete.wpilib.json b/src/main/deploy/paths/TenBallMidComplete.wpilib.json index a26f0c7..d662e4f 100644 --- a/src/main/deploy/paths/TenBallMidComplete.wpilib.json +++ b/src/main/deploy/paths/TenBallMidComplete.wpilib.json @@ -1 +1 @@ -[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24087146067360232,"velocity":0.6503529438187263,"acceleration":2.7,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.3403321067745373,"velocity":0.9188966882912508,"acceleration":2.700000000000002,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.4162093899230572,"velocity":1.1237653527922549,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.4796643300987847,"velocity":1.295093691266719,"acceleration":2.187682579886264,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5355873525736204,"velocity":1.4174355133495053,"acceleration":-0.734989108404782,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5891532438960738,"velocity":1.378065166645508,"acceleration":-0.6426464146474123,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.643461958301142,"velocity":1.3431638660489806,"acceleration":-0.5453266663691863,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6984118145503199,"velocity":1.3131982441231504,"acceleration":-0.4394478338280287,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.7539154354262596,"velocity":1.2888072981596066,"acceleration":-0.32468479529931726,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.809892141790966,"velocity":1.270632512712052,"acceleration":-0.20350244619471486,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.8662640818893885,"velocity":1.259160685005281,"acceleration":-0.08045877877688212,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.9229565411514642,"velocity":1.2545992789671963,"acceleration":0.03871626769718015,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.9799017050564615,"velocity":1.256803983177002,"acceleration":0.14815351255780274,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":1.037044049449856,"velocity":1.265269822214671,"acceleration":0.24282302918833878,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":1.0943450481757073,"velocity":1.2791838243007994,"acceleration":0.3192880318302778,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":1.151785273529354,"velocity":1.2975238008018528,"acceleration":0.37610415805722364,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":1.2093630070251655,"velocity":1.3191790257811382,"acceleration":0.4138164485760789,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.2670896494085861,"velocity":1.3430672599204667,"acceleration":0.43460983400489717,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.3249830335940764,"velocity":1.3682282940113044,"acceleration":0.4417529373871998,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.383060006914625,"velocity":1.3938839675702148,"acceleration":0.43900370786176246,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.441329456851559,"velocity":1.4194644721475942,"acceleration":0.43011410600385014,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.4997865390306198,"velocity":1.4446076877886345,"acceleration":0.41850596016098784,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.5584084380603251,"velocity":1.4691413019285218,"acceleration":0.4071259180970876,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.6171516690776433,"velocity":1.4930571937884367,"acceleration":0.3984462097492305,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.6759507421084023,"velocity":1.5164854615743109,"acceleration":0.39456346754290916,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.7347179434753464,"velocity":1.5396728523234446,"acceleration":0.397352070883421,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.7933440031024948,"velocity":1.562968038524027,"acceleration":0.4086404035647724,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.8516994822211263,"velocity":1.5868144450612802,"acceleration":0.43038917748303396,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.909636813019807,"velocity":1.6117500452092868,"acceleration":0.46485442754444356,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.9669930430836986,"velocity":1.6384123427017445,"acceleration":0.5147082657382434,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":2.0235934863647898,"velocity":1.6675450587029708,"acceleration":0.5830593842147167,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":2.079256669525349,"velocity":1.6999999999999975,"acceleration":4.265969051474203E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":2.1343686615338977,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1892768582496456,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.243795345773229,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.297757143445389,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.351013572738965,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4034336261508975,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4549033360942203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5053251437900577,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5546172681596233,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6027130747162124,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.649560444457203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6951211427560504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.739370188254281,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.782295221753494,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8238958751073535,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.864183140113587,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9031787374059825,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.940914485346383,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.01278040861083,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0802134292586643,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.143767239378293,"velocity":1.7,"acceleration":-0.9806847122121223,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2051845150767986,"velocity":1.6397690166567587,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.268443823133224,"velocity":1.4689688849044102,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.337848318037038,"velocity":1.2815767486641116,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.41794941674062,"velocity":1.0653037821644409,"acceleration":-2.699999999999997,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.464311638861809,"velocity":0.9401257824372306,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.246485948221108,"y":-1.1004457665706215},"rotation":{"radians":-0.02877542075414042}},"curvature":-1.2479062966462087},{"time":3.5152466704157184,"velocity":0.8026011972416758,"acceleration":-2.699999999999998,"pose":{"translation":{"x":7.290770527031534,"y":-1.103398777804336},"rotation":{"radians":-0.11545308104819059}},"curvature":-2.7518045032639415},{"time":3.5426121847539718,"velocity":0.7287143085283914,"acceleration":-2.7,"pose":{"translation":{"x":7.3114956505922,"y":-1.1064779763119066},"rotation":{"radians":-0.18293230201129468}},"curvature":-3.725227733364007},{"time":3.5716326709523103,"velocity":0.6503589957928775,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.331006934455969,"y":-1.1109206638015918},"rotation":{"radians":-0.26869329900096417}},"curvature":-4.883076044748539},{"time":3.5869706546331592,"velocity":0.6089464398545852,"acceleration":-2.7,"pose":{"translation":{"x":7.340252146464765,"y":-1.1137126796958767},"rotation":{"radians":-0.31891235544080615}},"curvature":-5.523919809501616},{"time":3.6030413007858946,"velocity":0.5655556952421994,"acceleration":-2.261497896342809,"pose":{"translation":{"x":7.349130295806822,"y":-1.1169134527391915},"rotation":{"radians":-0.37417386340026293}},"curvature":-6.1910282938294925},{"time":3.6199421735253385,"velocity":0.5273344070955894,"acceleration":-1.8200753979085174,"pose":{"translation":{"x":7.357622936902054,"y":-1.1205421754363483},"rotation":{"radians":-0.4344600800162828}},"curvature":-6.863459317284596},{"time":3.637660919757719,"velocity":0.49508495299624905,"acceleration":-1.404368604220338,"pose":{"translation":{"x":7.365712904636497,"y":-1.1246164469159217},"rotation":{"radians":-0.499598871724775}},"curvature":-7.511586808196944},{"time":3.6561465833657167,"velocity":0.4691242673969987,"acceleration":-1.013471060094217,"pose":{"translation":{"x":7.373384290445131,"y":-1.1291523046382135},"rotation":{"radians":-0.5692244956270529}},"curvature":-8.098065776430404},{"time":3.675310118112155,"velocity":0.44970257952237364,"acceleration":-0.6441793281202971,"pose":{"translation":{"x":7.380622418394695,"y":-1.1341642561032126},"rotation":{"radians":-0.6427505179428795}},"curvature":-8.58109966790429},{"time":3.695023152409384,"velocity":0.43700385033357203,"acceleration":-0.2912186052656897,"pose":{"translation":{"x":7.387413821266509,"y":-1.1396653105585597},"rotation":{"radians":-0.7193652922148932}},"curvature":-8.92014169080443},{"time":3.715122472825929,"velocity":0.43115055427507765,"acceleration":0.052067701331746194,"pose":{"translation":{"x":7.3937462166392915,"y":-1.1456670107075082},"rotation":{"radians":-0.7980584699587645}},"curvature":-9.08314266129781},{"time":3.7354206996909998,"velocity":0.43220743628905217,"acceleration":0.3926861267115369,"pose":{"translation":{"x":7.399608482971979,"y":-1.152179464416887},"rotation":{"radians":-0.8776808332180518}},"curvature":-9.053384284754978},{"time":3.755721886671196,"velocity":0.4401794307719521,"acceleration":0.7367582925439635,"pose":{"translation":{"x":7.404990635686545,"y":-1.1592113764250642},"rotation":{"radians":-0.957030545537142}},"curvature":-8.833522919030923},{"time":3.7758391314968773,"velocity":0.45500097772041015,"acceleration":1.0882423303428184,"pose":{"translation":{"x":7.409883803250819,"y":-1.1667700800499075},"rotation":{"radians":-1.0349502992109227}},"curvature":-8.445233665625674},{"time":3.7956105572131413,"velocity":0.4765170801160774,"acceleration":1.4478029542435038,"pose":{"translation":{"x":7.414280203261306,"y":-1.1748615688967483},"rotation":{"radians":-1.110416006752432}},"curvature":-7.924546693213447},{"time":3.8149106115079463,"velocity":0.5044597557411559,"acceleration":1.8120558532837037,"pose":{"translation":{"x":7.4181731185260045,"y":-1.1834905285663433},"rotation":{"radians":-1.1826006954592512}},"curvature":-7.314635079400883},{"time":3.833655213614978,"velocity":0.5384260217066767,"acceleration":2.1734101921253286,"pose":{"translation":{"x":7.421556873147227,"y":-1.192660368362838},"rotation":{"radians":-1.2509054956221384}},"curvature":-6.658491095790275},{"time":3.851801053675725,"velocity":0.5778643754393804,"acceleration":2.5206585401034607,"pose":{"translation":{"x":7.424426808604417,"y":-1.2023732530017275},"rotation":{"radians":-1.3149589717095316}},"curvature":-5.99341593717812},{"time":3.869340590588412,"velocity":0.6220755589478055,"acceleration":2.7000000000000313,"pose":{"translation":{"x":7.4267792598369695,"y":-1.2126301343178203},"rotation":{"radians":-1.3745927334979915}},"curvature":-5.3481080221595265},{"time":3.8863249147004435,"velocity":0.667933234050291,"acceleration":2.700000000000086,"pose":{"translation":{"x":7.428611531327051,"y":-1.2234307829732003},"rotation":{"radians":-1.4298037347140933}},"curvature":-4.742101270083783},{"time":3.90286706743035,"velocity":0.71259704642104,"acceleration":2.7000000000000104,"pose":{"translation":{"x":7.429921873182416,"y":-1.2347738201651899},"rotation":{"radians":-1.480712629900542}},"curvature":-4.186771633619186},{"time":3.919081127481485,"velocity":0.7563750085591059,"acceleration":2.7000000000000726,"pose":{"translation":{"x":7.430709457219227,"y":-1.2466567493343108},"rotation":{"radians":-1.527524747550707}},"curvature":-3.687079506807228},{"time":3.935049187697399,"velocity":0.7994887711420753,"acceleration":2.699999999999971,"pose":{"translation":{"x":7.430974353044876,"y":-1.2590759878722486},"rotation":{"radians":-1.5704972091395932}},"curvature":-3.2434293445605835},{"time":3.9664639427506723,"velocity":0.8843086097859121,"acceleration":2.699999999999964,"pose":{"translation":{"x":7.429940703945299,"y":-1.2855038226249045},"rotation":{"radians":-1.6460641038472028}},"curvature":-2.5125016951673764},{"time":3.997424393909201,"velocity":0.9679018279139381,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.426838529714478,"y":-1.3140081474824699},"rotation":{"radians":-1.7096996276847836}},"curvature":-1.9589426080510612},{"time":4.028085598335015,"velocity":1.0506870798636372,"acceleration":2.7000000000000095,"pose":{"translation":{"x":7.421698268143265,"y":-1.3445244380324612},"rotation":{"radians":-1.7635069388369267}},"curvature":-1.5430718610586651},{"time":4.058517591989222,"velocity":1.132853462729996,"acceleration":2.7000000000000037,"pose":{"translation":{"x":7.414562425632777,"y":-1.3769738370453977},"rotation":{"radians":-1.80928153691283}},"curvature":-1.2304194023042416},{"time":4.118736716721557,"velocity":1.2954450995073,"acceleration":2.3581795493155604,"pose":{"translation":{"x":7.394529774351022,"y":-1.4472909553859892},"rotation":{"radians":-1.8823557940783588}},"curvature":-0.8137689160789616},{"time":4.178392474069149,"velocity":1.4361240864833222,"acceleration":1.0280501369863269,"pose":{"translation":{"x":7.367292915991394,"y":-1.52408054521081},"rotation":{"radians":-1.9376153749830112}},"curvature":-0.5671040820963597},{"time":4.238964013252306,"velocity":1.4983946656380396,"acceleration":0.6806598794331884,"pose":{"translation":{"x":7.333548306053704,"y":-1.6062992620615995},"rotation":{"radians":-1.980664464363438}},"curvature":-0.415270222573429},{"time":4.301503977300484,"velocity":1.5409631100268286,"acceleration":0.4541105461405023,"pose":{"translation":{"x":7.294112024222084,"y":-1.692771843268838},"rotation":{"radians":-2.0151600783392727}},"curvature":-0.31853754024915065},{"time":4.365651597320475,"velocity":1.5700932207877196,"acceleration":0.3039509005891818,"pose":{"translation":{"x":7.249895283171679,"y":-1.7822235769053099},"rotation":{"radians":-2.0435542741178185}},"curvature":-0.2553649962385433},{"time":4.4308222574998855,"velocity":1.589901901641243,"acceleration":0.19979119065916273,"pose":{"translation":{"x":7.20187993737534,"y":-1.873312770739671},"rotation":{"radians":-2.0675482771424347}},"curvature":-0.21372950444359917},{"time":4.496291907487063,"velocity":1.6029821609642199,"acceleration":0.12148010098068647,"pose":{"translation":{"x":7.151093991910315,"y":-1.9646632211900141},"rotation":{"radians":-2.088370637372372}},"curvature":-0.18680044113633412},{"time":4.561259656850615,"velocity":1.6108744497173921,"acceleration":0.05463868552157288,"pose":{"translation":{"x":7.098587111264945,"y":-2.054896682277434},"rotation":{"radians":-2.106952696193974}},"curvature":-0.17076368611172438},{"time":4.624897589019494,"velocity":1.6143515426804107,"acceleration":-0.013181927539404118,"pose":{"translation":{"x":7.045406128145351,"y":-2.1426653345795916},"rotation":{"radians":-2.124045424613487}},"curvature":-0.1637481573893422},{"time":4.6863934918413115,"velocity":1.6135409081454433,"acceleration":-0.09636542124448583,"pose":{"translation":{"x":6.9925705522821335,"y":-2.226684254184283},"rotation":{"radians":-2.140304501136336}},"curvature":-0.16538102479256336},{"time":4.744990364133843,"velocity":1.6078941958633641,"acceleration":-0.6374308786638251,"pose":{"translation":{"x":6.941048079237058,"y":-2.3057638816430024},"rotation":{"radians":-2.156361587928427}},"curvature":-0.1768009201056777},{"time":4.800431081060775,"velocity":1.5725545709588773,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.891730099209751,"y":-2.37884249092451},"rotation":{"radians":-2.1728963784151825}},"curvature":-0.2011726549273176},{"time":4.854288620919112,"velocity":1.427139213341367,"acceleration":-2.699999999999997,"pose":{"translation":{"x":6.845407205844394,"y":-2.4450186583683924},"rotation":{"radians":-2.1907250385502053}},"curvature":-0.24500666492416812},{"time":4.907764200265851,"velocity":1.282755149105172,"acceleration":-2.7,"pose":{"translation":{"x":6.802744705036412,"y":-2.5035837316386345},"rotation":{"radians":-2.2109265322563663}},"curvature":-0.32107362952933843},{"time":5.010558252841902,"velocity":1.0052112071498347,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.7302887187706615,"y":-2.596204656657504},"rotation":{"radians":-2.26538823363029}},"curvature":-0.6983733281623699},{"time":5.058181507945903,"velocity":0.876628418369029,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.700978985620202,"y":-2.6300992809381722},"rotation":{"radians":-2.3055766625940555}},"curvature":-1.161706924289958},{"time":5.102107797041266,"velocity":0.7580274378115504,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.6762481672551255,"y":-2.6561252940163986},"rotation":{"radians":-2.3611253774593677}},"curvature":-2.069483430581481},{"time":5.1416578127195125,"velocity":0.6512423954802853,"acceleration":-2.6999999999999966,"pose":{"translation":{"x":6.655767762927474,"y":-2.6750249344816397},"rotation":{"radians":-2.4393793407849826}},"curvature":-3.786562605724007},{"time":5.159738948600884,"velocity":0.6024233286005831,"acceleration":-2.7,"pose":{"translation":{"x":6.646945039992192,"y":-2.682139407006597},"rotation":{"radians":-2.4888731070782244}},"curvature":-5.0067041726191235},{"time":5.176792965437063,"velocity":0.5563774831429,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":6.638937036980688,"y":-2.68792802596912},"rotation":{"radians":-2.54463825964339}},"curvature":-6.289792037747411},{"time":5.1930309236619525,"velocity":0.5125349959356977,"acceleration":1.3760054548561336,"pose":{"translation":{"x":6.631622107145021,"y":-2.6925979064247128},"rotation":{"radians":-2.603460002905891}},"curvature":-7.150761360113481},{"time":5.207859374190757,"velocity":0.5329390247503961,"acceleration":2.7000000000000117,"pose":{"translation":{"x":6.624858527656298,"y":-2.6963844461134348},"rotation":{"radians":-2.658512232190288}},"curvature":-6.75882207866783},{"time":5.232432613506469,"velocity":0.5992867709028199,"acceleration":2.6999999999999997,"pose":{"translation":{"x":6.6123135559006245,"y":-2.7023965955020928},"rotation":{"radians":-2.714965160462928}},"curvature":-2.782056460928346E-13},{"time":5.307249981064505,"velocity":0.8012936633095167,"acceleration":2.6999999999999944,"pose":{"translation":{"x":6.564370383742047,"y":-2.723528796446016},"rotation":{"radians":-2.7486246800604213}},"curvature":-1.2481483002418547},{"time":5.359588764109113,"velocity":0.942608377529959,"acceleration":-1.3037969736295336,"pose":{"translation":{"x":6.521596141607646,"y":-2.739437497523542},"rotation":{"radians":-2.830970184843895}},"curvature":-2.3571662142433545},{"time":5.380819682559197,"velocity":0.9149275703073636,"acceleration":2.1653287500554654,"pose":{"translation":{"x":6.502679354153565,"y":-2.7450033060483143},"rotation":{"radians":-2.8808935518583048}},"curvature":-2.648365983542163},{"time":5.3999635475683805,"velocity":0.9563803315989277,"acceleration":-1.9880304077630273,"pose":{"translation":{"x":6.485268832011468,"y":-2.749212182681022},"rotation":{"radians":-2.927188261447052}},"curvature":-2.3998009552324597},{"time":5.434828841887997,"velocity":0.8870670663159218,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":6.453690251127663,"y":-2.7551725959229563},"rotation":{"radians":-2.9677547229044667}},"curvature":0.39238321378028623},{"time":5.471501485855907,"velocity":0.7880509276025651,"acceleration":-1.6821792794414314,"pose":{"translation":{"x":6.423581274828754,"y":-2.7612464468006617},"rotation":{"radians":-2.9005726138477717}},"curvature":3.571669713554079},{"time":5.492544888288298,"velocity":0.7526521520618497,"acceleration":2.4550269205084136,"pose":{"translation":{"x":6.4079642503873036,"y":-2.7655937621270232},"rotation":{"radians":-2.838791299614375}},"curvature":3.884813326993507},{"time":5.514928540897344,"velocity":0.8076046217963667,"acceleration":2.6999999999999975,"pose":{"translation":{"x":6.391477144723909,"y":-2.7713471159009355},"rotation":{"radians":-2.7743073993901626}},"curvature":3.410464289721715},{"time":5.537683301071879,"velocity":0.8690424742676104,"acceleration":2.6999999999999487,"pose":{"translation":{"x":6.373886771801721,"y":-2.778727284101776},"rotation":{"radians":-2.717036888422856}},"curvature":2.5791011531493675},{"time":5.560811154289309,"velocity":0.9314876779546685,"acceleration":-2.232737302146218,"pose":{"translation":{"x":6.355122754197213,"y":-2.787751360334723},"rotation":{"radians":-2.6726507558686987}},"curvature":1.702709676404945},{"time":5.613209538187445,"velocity":0.8144958516531218,"acceleration":-2.7,"pose":{"translation":{"x":6.3148679344459335,"y":-2.8094770192257847},"rotation":{"radians":-2.633489132868386}},"curvature":2.6424201040934643E-13},{"time":5.644832808940525,"velocity":0.7291130206198057,"acceleration":0.014726220632563273,"pose":{"translation":{"x":6.293824179582127,"y":-2.8218407195536503},"rotation":{"radians":-2.5704807412571062}},"curvature":4.109876886612331},{"time":5.6636248279584125,"velocity":0.7293897560379944,"acceleration":2.7000000000000006,"pose":{"translation":{"x":6.282512531848517,"y":-2.8295769425123694},"rotation":{"radians":-2.5130508700960563}},"curvature":4.107146562881415},{"time":5.683907608455922,"velocity":0.7841532633812693,"acceleration":2.700000000000044,"pose":{"translation":{"x":6.270373430375236,"y":-2.8389709421709193},"rotation":{"radians":-2.454602498726585}},"curvature":3.4662114222347586},{"time":5.70526422526559,"velocity":0.8418161287673747,"acceleration":2.7000000000000424,"pose":{"translation":{"x":6.2572605014198635,"y":-2.850351234968828},"rotation":{"radians":-2.401258195704806}},"curvature":2.6992882596325503},{"time":5.727782637168609,"velocity":0.9026158409055279,"acceleration":2.700000000000005,"pose":{"translation":{"x":6.2430848376947585,"y":-2.8639459490095134},"rotation":{"radians":-2.355136423565373}},"curvature":2.035175527413349},{"time":5.775996784507287,"velocity":1.0327940387199588,"acceleration":2.7000000000000086,"pose":{"translation":{"x":6.211447015686596,"y":-2.898237788623612},"rotation":{"radians":-2.283511963581287}},"curvature":1.1584351087573401},{"time":5.827249030133743,"velocity":1.1711751019113907,"acceleration":2.7000000000000006,"pose":{"translation":{"x":6.175708077122055,"y":-2.9419713739030793},"rotation":{"radians":-2.232503020651801}},"curvature":0.7114010646875015},{"time":5.879716810848121,"velocity":1.3128381098402124,"acceleration":2.700000000000001,"pose":{"translation":{"x":6.136715668746837,"y":-2.9941835597868995},"rotation":{"radians":-2.1943873167816736}},"curvature":0.48850726496729824},{"time":5.931594500419193,"velocity":1.4529078716821067,"acceleration":1.5624737156913744,"pose":{"translation":{"x":6.095757006630035,"y":-3.0530822209766115},"rotation":{"radians":-2.163759911299562}},"curvature":0.380438022405832},{"time":5.982215145525289,"velocity":1.5320012991317222,"acceleration":-0.057841945126272415,"pose":{"translation":{"x":6.054398911513116,"y":-3.116305329649745},"rotation":{"radians":-2.1369461734365096}},"curvature":0.3384556587168576},{"time":6.032035019002884,"velocity":1.5291196207438333,"acceleration":-0.5090616981546786,"pose":{"translation":{"x":6.014327844158916,"y":-3.181180033173254},"rotation":{"radians":-2.1111850358192785}},"curvature":0.34490995393452345},{"time":6.080707664405129,"velocity":1.504342241221686,"acceleration":-1.7656263145663706,"pose":{"translation":{"x":5.97718994070063,"y":-3.244981731816954},"rotation":{"radians":-2.0840011566593573}},"curvature":0.4014259222340187},{"time":6.127561407010178,"velocity":1.4216160403422935,"acceleration":-2.7,"pose":{"translation":{"x":5.944431047990803,"y":-3.305193156466954},"rotation":{"radians":-2.052707137462513}},"curvature":0.5281984550986057},{"time":6.172389573744138,"velocity":1.300579990160599,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.917136758950319,"y":-3.3597634463390964},"rotation":{"radians":-2.0140182231966675}},"curvature":0.7696908429217094},{"time":6.214300523930543,"velocity":1.1874204246573072,"acceleration":-2.7000000000000037,"pose":{"translation":{"x":5.895872447917389,"y":-3.4073672266923865},"rotation":{"radians":-1.9641560802129352}},"curvature":1.188984427881291},{"time":6.252252771580072,"velocity":1.0849493560035777,"acceleration":-0.4333623742254068,"pose":{"translation":{"x":5.880523305996552,"y":-3.4476636865424304},"rotation":{"radians":-1.901225210539506}},"curvature":1.7496710295967226},{"time":6.285141745600643,"velocity":1.0706965121361853,"acceleration":1.504053099473219,"pose":{"translation":{"x":5.870134376407649,"y":-3.4815556563748715},"rotation":{"radians":-1.8346198081438863}},"curvature":1.8140478591397617},{"time":6.313341537856752,"velocity":1.1131104970834875,"acceleration":-2.7,"pose":{"translation":{"x":5.862750589834832,"y":-3.511448685858822},"rotation":{"radians":-1.8006658230463592}},"curvature":-5.360757530368065E-13},{"time":6.341497677084923,"velocity":1.0370889211674263,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.856203796082101,"y":-3.541002910133535},"rotation":{"radians":-1.766968921320334}},"curvature":1.963419840399414},{"time":6.373942418174615,"velocity":0.9494881202252553,"acceleration":1.9757710956120564,"pose":{"translation":{"x":5.851064699271866,"y":-3.5728175057829237},"rotation":{"radians":-1.6924633524514152}},"curvature":2.43962472127956},{"time":6.410085221881796,"velocity":1.020898027104284,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.848224330419191,"y":-3.6083116788576466},"rotation":{"radians":-1.61121420277845}},"curvature":2.0530882496173066},{"time":6.4472409528531545,"velocity":1.1212185007269526,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.848083515132619,"y":-3.648107382432883},"rotation":{"radians":-1.541358784235409}},"curvature":1.468599708411283},{"time":6.484891418261424,"velocity":1.2228747573292795,"acceleration":2.7000000000000095,"pose":{"translation":{"x":5.850633646779956,"y":-3.6921617367241057},"rotation":{"radians":-1.4882866880902128}},"curvature":0.9656511797992369},{"time":6.522567110762013,"velocity":1.3245991270808724,"acceleration":2.699999999999986,"pose":{"translation":{"x":5.855537459654031,"y":-3.7398994492028543},"rotation":{"radians":-1.4515974408881707}},"curvature":0.5866238096609577},{"time":6.5952183344984405,"velocity":1.520757431169224,"acceleration":-1.890804527454655,"pose":{"translation":{"x":5.869898409873462,"y":-3.8422562355841015},"rotation":{"radians":-1.4201823229158446}},"curvature":0.05849443045663204},{"time":6.666646906859882,"velocity":1.3856999631585873,"acceleration":-2.7,"pose":{"translation":{"x":5.884964438933297,"y":-3.944959110869841},"rotation":{"radians":-1.4385731313207648}},"curvature":-0.43793035518534235},{"time":6.739064849944098,"velocity":1.1901715168312037,"acceleration":-2.700000000000003,"pose":{"translation":{"x":5.894444557385554,"y":-4.0377457278600914},"rotation":{"radians":-1.5112165860236555}},"curvature":-1.2058440033677431},{"time":6.774536895566012,"velocity":1.0943969936520368,"acceleration":-2.1274280542372015,"pose":{"translation":{"x":5.895735701559724,"y":-4.078244310677544},"rotation":{"radians":-1.5700473110704392}},"curvature":-1.7079223464270294},{"time":6.808872494354494,"velocity":1.021350477530387,"acceleration":-2.373467065825907,"pose":{"translation":{"x":5.8945434544964925,"y":-4.114547466566519},"rotation":{"radians":-1.6393519162414487}},"curvature":-2.0508114870006136},{"time":6.842357555890162,"velocity":0.9418747867783246,"acceleration":-2.7,"pose":{"translation":{"x":5.891207411273528,"y":-4.14724709351312},"rotation":{"radians":-1.7035828110055375}},"curvature":-1.669967448090788},{"time":6.87679427990919,"velocity":0.848895631926948,"acceleration":-2.7,"pose":{"translation":{"x":5.886546239551215,"y":-4.177726877917286},"rotation":{"radians":-1.7328897632897347}},"curvature":1.1065428326033853E-13},{"time":6.915988445631466,"velocity":0.7430713844768021,"acceleration":-0.9800934374084039,"pose":{"translation":{"x":5.88067630228792,"y":-4.208367592591282},"rotation":{"radians":-1.8080776127277007}},"curvature":-3.9746967020187314},{"time":6.939149798238455,"velocity":0.7203710947851905,"acceleration":2.105068867779344,"pose":{"translation":{"x":5.876118091119996,"y":-4.224690756343688},"rotation":{"radians":-1.8787325484751447}},"curvature":-4.197206170037974},{"time":6.963906716861135,"velocity":0.7724861134399396,"acceleration":2.7000000000001143,"pose":{"translation":{"x":5.869858974330641,"y":-4.2420777330336055},"rotation":{"radians":-1.952406601416692}},"curvature":-3.7058234846360123},{"time":6.989199800601645,"velocity":0.8407774395393204,"acceleration":2.7000000000000917,"pose":{"translation":{"x":5.861597813055334,"y":-4.260732576536634},"rotation":{"radians":-2.0203938686193776}},"curvature":-2.956673651080127},{"time":7.015028262074048,"velocity":0.9105142855148116,"acceleration":2.700000000000028,"pose":{"translation":{"x":5.851151489499009,"y":-4.2807920944866344},"rotation":{"radians":-2.078685177167563}},"curvature":-2.2258692105513505},{"time":7.068415357591023,"velocity":1.0546594434106475,"acceleration":2.6999999999999766,"pose":{"translation":{"x":5.8234994187239835,"y":-4.325369532324868},"rotation":{"radians":-2.1640426184738857}},"curvature":-1.1525883783768047},{"time":7.123332995545581,"velocity":1.2029370658879515,"acceleration":2.700000000000027,"pose":{"translation":{"x":5.787408291453949,"y":-4.375770984831428},"rotation":{"radians":-2.2141065150229546}},"curvature":-0.5349613600815684},{"time":7.17819258339559,"velocity":1.351057953082978,"acceleration":2.699999999999995,"pose":{"translation":{"x":5.744615217060818,"y":-4.431237495974559},"rotation":{"radians":-2.2374556498732163}},"curvature":-0.16441177996834094},{"time":7.231275738982579,"velocity":1.49438247316785,"acceleration":-1.695930601341739,"pose":{"translation":{"x":5.697760506246135,"y":-4.490468238184125},"rotation":{"radians":-2.2392837121801517}},"curvature":0.10580284594353764},{"time":7.284900098907512,"velocity":1.4034392801937925,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.650059292447182,"y":-4.551798535128107},"rotation":{"radians":-2.2209710474953113}},"curvature":0.3716502298548522},{"time":7.342468924065651,"velocity":1.2480034522668173,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":5.604973153243076,"y":-4.613377884489101},"rotation":{"radians":-2.180152623521608}},"curvature":0.7236839842025061},{"time":7.40391328710216,"velocity":1.0821036720682429,"acceleration":-2.699999999999996,"pose":{"translation":{"x":5.56588173176088,"y":-4.6733479807408145},"rotation":{"radians":-2.110033357039896}},"curvature":1.2953678674001186},{"time":7.435819801119151,"velocity":0.9959560842223683,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.549536962771379,"y":-4.702190515076448},"rotation":{"radians":-2.060289209923918}},"curvature":1.7314463248379408},{"time":7.468444759800843,"velocity":0.9078686957817996,"acceleration":-2.7,"pose":{"translation":{"x":5.5357543580817,"y":-4.730020737924564},"rotation":{"radians":-1.9979270126197324}},"curvature":2.320021023322054},{"time":7.501861141036805,"velocity":0.8176444664447036,"acceleration":-2.6999999999999953,"pose":{"translation":{"x":5.5247870979254925,"y":-4.756683442209535},"rotation":{"radians":-1.9203821291983572}},"curvature":3.103746880369516},{"time":7.518946087216342,"velocity":0.7715151117599537,"acceleration":-2.7,"pose":{"translation":{"x":5.520420385403833,"y":-4.769537314349319},"rotation":{"radians":-1.87511361445659}},"curvature":3.5772510721477797},{"time":7.536360358041236,"velocity":0.7244965805327409,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":5.5168216706467925,"y":-4.782056312425773},"rotation":{"radians":-1.8251635891717155}},"curvature":4.103054297437957},{"time":7.55420292968893,"velocity":0.6763216370839664,"acceleration":-2.607992884955388,"pose":{"translation":{"x":5.514002160507967,"y":-4.794231199022947},"rotation":{"radians":-1.770388976089039}},"curvature":4.671595505555233},{"time":7.572595310766079,"velocity":0.6283544380973731,"acceleration":-2.168275051067394,"pose":{"translation":{"x":5.51196761086897,"y":-4.80605548784949},"rotation":{"radians":-1.710811712710651}},"curvature":5.263825366744856},{"time":7.591579364783137,"velocity":0.58719178740407,"acceleration":-1.7049346554482947,"pose":{"translation":{"x":5.510718005957207,"y":-4.817525617589015},"rotation":{"radians":-1.6466861683784764}},"curvature":5.849184750926004},{"time":7.611078252080525,"velocity":0.5539474587080722,"acceleration":-1.2273047399076786,"pose":{"translation":{"x":5.5102472376636635,"y":-4.82864112575047},"rotation":{"radians":-1.5785641919150275}},"curvature":6.385441698329703},{"time":7.630954101985592,"velocity":0.5295537339098895,"acceleration":-0.7356699178498,"pose":{"translation":{"x":5.5105427848606805,"y":-4.839404822518501},"rotation":{"radians":-1.507341453940095}},"curvature":6.821759991973681},{"time":7.651005074056508,"velocity":0.5148028369336706,"acceleration":-0.21868006238280333,"pose":{"translation":{"x":5.511585392719732,"y":-4.8498229646038284},"rotation":{"radians":-1.4342657220142343}},"curvature":7.105663902666528},{"time":7.670972126524015,"velocity":0.5104364406544754,"acceleration":0.3496940236859671,"pose":{"translation":{"x":5.513348752029203,"y":-4.859905429093603},"rotation":{"radians":-1.360891999701801}},"curvature":7.192849441564701},{"time":7.690555950909508,"velocity":0.5172847870029977,"acceleration":1.0159549694998586,"pose":{"translation":{"x":5.515799178512178,"y":-4.8696658873017835},"rotation":{"radians":-1.2889830914151401}},"curvature":7.056761937305157},{"time":7.709440884685059,"velocity":0.5364710293209449,"acceleration":1.7204991229826834,"pose":{"translation":{"x":5.518895292144211,"y":-4.879121978619499},"rotation":{"radians":-1.2203733973856572}},"curvature":6.694003221269455},{"time":7.7273589882858,"velocity":0.5672991108515307,"acceleration":-2.699999999999996,"pose":{"translation":{"x":5.522587696471107,"y":-4.888295484365418},"rotation":{"radians":-1.1568292312130675}},"curvature":6.123051113372027},{"time":7.7455439259682795,"velocity":0.5181997791088344,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.5268186579267065,"y":-4.897212501636115},"rotation":{"radians":-1.0999415472086687}},"curvature":5.37683627323114},{"time":7.7656690330091696,"velocity":0.4638619900984307,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":5.531521785150656,"y":-4.905903617156444},"rotation":{"radians":-1.0510750755719702}},"curvature":4.49233832354661},{"time":7.815797793645661,"velocity":0.32851433637990307,"acceleration":-2.7,"pose":{"translation":{"x":5.542033758397939,"y":-4.922753981088973},"rotation":{"radians":-0.9818462210463513}},"curvature":2.421643800402231},{"time":7.937469770082663,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.553407143521984,"y":-4.939187668841276},"rotation":{"radians":-0.9570262317783049}},"curvature":1.1443843838492492E-13}] \ No newline at end of file +[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.4},"rotation":{"radians":1.4909663410826592}},"curvature":0.0},{"time":0.24087146067360232,"velocity":0.6503529438187263,"acceleration":2.7,"pose":{"translation":{"x":3.2063923239707948,"y":-2.321935549378395},"rotation":{"radians":1.4853936094671658}},"curvature":-0.14003454519850286},{"time":0.3403321067745373,"velocity":0.9188966882912508,"acceleration":2.700000000000002,"pose":{"translation":{"x":3.2136058807373047,"y":-2.244230365753174},"rotation":{"radians":1.4693943312421711}},"curvature":-0.26854009181439953},{"time":0.4162093899230572,"velocity":1.1237653527922549,"acceleration":2.7000000000000006,"pose":{"translation":{"x":3.222373878955841,"y":-2.167232152819633},"rotation":{"radians":1.4438709569483001}},"curvature":-0.3892554699617604},{"time":0.4796643300987847,"velocity":1.295093691266719,"acceleration":2.187682579886264,"pose":{"translation":{"x":3.23333740234375,"y":-2.0912750244140623},"rotation":{"radians":1.4095508622566484}},"curvature":-0.5045238953483497},{"time":0.5355873525736204,"velocity":1.4174355133495053,"acceleration":-0.734989108404782,"pose":{"translation":{"x":3.247049701213837,"y":-2.0166784316301345},"rotation":{"radians":1.3670615930214727}},"curvature":-0.6152749842269323},{"time":0.5891532438960738,"velocity":1.378065166645508,"acceleration":-0.6426464146474123,"pose":{"translation":{"x":3.263980484008789,"y":-1.9437460899353027},"rotation":{"radians":1.31700394182167}},"curvature":-0.7210297835846021},{"time":0.643461958301142,"velocity":1.3431638660489806,"acceleration":-0.5453266663691863,"pose":{"translation":{"x":3.284520208835602,"y":-1.8727649062871932},"rotation":{"radians":1.2600195729181376}},"curvature":-0.8199640567171538},{"time":0.6984118145503199,"velocity":1.3131982441231504,"acceleration":-0.4394478338280287,"pose":{"translation":{"x":3.308984375,"y":-1.80400390625},"rotation":{"radians":1.1968484614725936}},"curvature":-0.9091030887449644},{"time":0.7539154354262596,"velocity":1.2888072981596066,"acceleration":-0.32468479529931726,"pose":{"translation":{"x":3.337617814540863,"y":-1.7377131611108778},"rotation":{"radians":1.1283693350753876}},"curvature":-0.984719188894597},{"time":0.809892141790966,"velocity":1.270632512712052,"acceleration":-0.20350244619471486,"pose":{"translation":{"x":3.3705989837646486,"y":-1.6741227149963378},"rotation":{"radians":1.0556153835823303}},"curvature":-1.0429516644982315},{"time":0.8662640818893885,"velocity":1.259160685005281,"acceleration":-0.08045877877688212,"pose":{"translation":{"x":3.4080442547798158,"y":-1.6134415119886398},"rotation":{"radians":0.9797593479141627}},"curvature":-1.0805731039252389},{"time":0.9229565411514642,"velocity":1.2545992789671963,"acceleration":0.03871626769718015,"pose":{"translation":{"x":3.45001220703125,"y":-1.5558563232421874},"rotation":{"radians":0.9020673863864839}},"curvature":-1.0957232372764043},{"time":0.9799017050564615,"velocity":1.256803983177002,"acceleration":0.14815351255780274,"pose":{"translation":{"x":3.4965079188346864,"y":-1.5015306740999221},"rotation":{"radians":0.8238287103495565}},"curvature":-1.0883868599412896},{"time":1.037044049449856,"velocity":1.265269822214671,"acceleration":0.24282302918833878,"pose":{"translation":{"x":3.547487258911133,"y":-1.4506037712097166},"rotation":{"radians":0.7462749142641318}},"curvature":-1.060453497287128},{"time":1.0943450481757073,"velocity":1.2791838243007994,"acceleration":0.3192880318302778,"pose":{"translation":{"x":3.602861177921295,"y":-1.4031894296407699},"rotation":{"radians":0.6705058542365749}},"curvature":-1.0153469207650658},{"time":1.151785273529354,"velocity":1.2975238008018528,"acceleration":0.37610415805722364,"pose":{"translation":{"x":3.6624999999999996,"y":-1.359375},"rotation":{"radians":0.5974362859535688}},"curvature":-0.957370100328515},{"time":1.2093630070251655,"velocity":1.3191790257811382,"acceleration":0.4138164485760789,"pose":{"translation":{"x":3.726237714290619,"y":-1.3192202955484391},"rotation":{"radians":0.5277705788228475}},"curvature":-0.8909885271430263},{"time":1.2670896494085861,"velocity":1.3430672599204667,"acceleration":0.43460983400489717,"pose":{"translation":{"x":3.793876266479492,"y":-1.282756519317627},"rotation":{"radians":0.46200494353630284}},"curvature":-0.8202450408700819},{"time":1.3249830335940764,"velocity":1.3682282940113044,"acceleration":0.4417529373871998,"pose":{"translation":{"x":3.8651898503303523,"y":-1.2499851912260056},"rotation":{"radians":0.4004508235379794}},"curvature":-0.7484034289896291},{"time":1.383060006914625,"velocity":1.3938839675702148,"acceleration":0.43900370786176246,"pose":{"translation":{"x":3.9399291992187493,"y":-1.2208770751953126},"rotation":{"radians":0.3432707503966877}},"curvature":-0.6778200992401754},{"time":1.441329456851559,"velocity":1.4194644721475942,"acceleration":0.43011410600385014,"pose":{"translation":{"x":4.017825877666472,"y":-1.1953711062669754},"rotation":{"radians":0.2905185837502133}},"curvature":-0.6099838436225127},{"time":1.4997865390306198,"velocity":1.4446076877886345,"acceleration":0.41850596016098784,"pose":{"translation":{"x":4.098596572875976,"y":-1.173373317718506},"rotation":{"radians":0.24217829113794748}},"curvature":-0.5456484025792503},{"time":1.5584084380603251,"velocity":1.4691413019285218,"acceleration":0.4071259180970876,"pose":{"translation":{"x":4.1819473862648,"y":-1.1547557681798937},"rotation":{"radians":0.19819797352787924}},"curvature":-0.4849954493582745},{"time":1.6171516690776433,"velocity":1.4930571937884367,"acceleration":0.3984462097492305,"pose":{"translation":{"x":4.267578124999998,"y":-1.1393554687500003},"rotation":{"radians":0.1585179297328586}},"curvature":-0.42778827730628},{"time":1.6759507421084023,"velocity":1.5164854615743109,"acceleration":0.39456346754290916,"pose":{"translation":{"x":4.35518659353256,"y":-1.1269733101129535},"rotation":{"radians":0.12309291857098917}},"curvature":-0.37349708304330403},{"time":1.7347179434753464,"velocity":1.5396728523234446,"acceleration":0.397352070883421,"pose":{"translation":{"x":4.444472885131834,"y":-1.1173729896545417},"rotation":{"radians":0.09190948239783109}},"curvature":-0.32139092067395386},{"time":1.7933440031024948,"velocity":1.562968038524027,"acceleration":0.4086404035647724,"pose":{"translation":{"x":4.53514367341995,"y":-1.1102799385786064},"rotation":{"radians":0.06499941950371821}},"curvature":-0.27059936113793964},{"time":1.8516994822211263,"velocity":1.5868144450612802,"acceleration":0.43038917748303396,"pose":{"translation":{"x":4.626916503906247,"y":-1.1053802490234386},"rotation":{"radians":0.042450411457934975}},"curvature":-0.2201505876217445},{"time":1.909636813019807,"velocity":1.6117500452092868,"acceleration":0.46485442754444356,"pose":{"translation":{"x":4.7195240855216944,"y":-1.1023196011781704},"rotation":{"radians":0.024414544524684806}},"curvature":-0.16899419639207908},{"time":1.9669930430836986,"velocity":1.6384123427017445,"acceleration":0.5147082657382434,"pose":{"translation":{"x":4.812718582153316,"y":-1.1007021903991712},"rotation":{"radians":0.011115066097951428}},"curvature":-0.11601802371569106},{"time":2.0235934863647898,"velocity":1.6675450587029708,"acceleration":0.5830593842147167,"pose":{"translation":{"x":4.906275904178614,"y":-1.1000896543264405},"rotation":{"radians":0.0028511840826505935}},"curvature":-0.06007008409255472},{"time":2.079256669525349,"velocity":1.6999999999999975,"acceleration":4.265969051474203E-14,"pose":{"translation":{"x":4.999999999999995,"y":-1.1000000000000019},"rotation":{"radians":-3.2566542055671518E-15}},"curvature":-4.342205607422935E-15},{"time":2.1343686615338977,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.0936903864145275,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.1892768582496456,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.187034320831299,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.243795345773229,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.279715749621391,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.297757143445389,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.371450805664063,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.351013572738965,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.461986735463142,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4034336261508975,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.551100826263427,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.4549033360942203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.638599333167076,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5053251437900577,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.72431640625,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.5546172681596233,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.808113017678261,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6027130747162124,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.889875888824463,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.649560444457203,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.969516417384147,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.6951211427560504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.046969604492188,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.739370188254281,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.12219298183918,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.782295221753494,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.195165538787842,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.8238958751073535,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.265886649489403,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.864183140113587,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.334375,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.9031787374059825,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.400667515397072,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.940914485346383,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.464818286895753,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.01278040861083,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.5869903564453125,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.0802134292586643,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.701626491546631,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.143767239378293,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.80966796875,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.2040965083386856,"velocity":1.7,"acceleration":-2.1491555180394277,"pose":{"translation":{"x":6.912227725982667,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.264221775517693,"velocity":1.5707814502686412,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.010556030273439,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.328543555301386,"velocity":1.3971126448526707,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.106006145477297,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.4008763809671625,"velocity":1.201814015555074,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.200000000000003,"y":-1.1},"rotation":{"radians":0.0}},"curvature":0.0},{"time":3.4848439889787057,"velocity":0.9751014739239071,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.291391427065546,"y":-1.100829703353923},"rotation":{"radians":-0.027821850230778215}},"curvature":-0.650210314192843},{"time":3.5304430389191896,"velocity":0.8519840390846011,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.33300693575994,"y":-1.102681343908726},"rotation":{"radians":-0.06466356345347674}},"curvature":-1.1671521341710898},{"time":3.5778846168647904,"velocity":0.7238917786314792,"acceleration":-2.7000000000000024,"pose":{"translation":{"x":7.370233046899057,"y":-1.1060809225877481},"rotation":{"radians":-0.12296931421995656}},"curvature":-2.060099762721178},{"time":3.6022571287580676,"velocity":0.6580859965196303,"acceleration":-2.7,"pose":{"translation":{"x":7.386904400841855,"y":-1.1084662489341992},"rotation":{"radians":-0.16325383468827934}},"curvature":-2.7730985925232075},{"time":3.6271103997518286,"velocity":0.5909821648364754,"acceleration":-2.7,"pose":{"translation":{"x":7.402155192351948,"y":-1.111353633071441},"rotation":{"radians":-0.21358032077615297}},"curvature":-3.7862951832682605},{"time":3.652572439380677,"velocity":0.5222346578385852,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.415909561237784,"y":-1.114770334002192},"rotation":{"radians":-0.2768284721084359}},"curvature":-5.252936086916672},{"time":3.678927181450754,"velocity":0.4510768542493773,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.428105903379064,"y":-1.1187386439112075},"rotation":{"radians":-0.35686632281713865}},"curvature":-7.397416721857951},{"time":3.6926141362001688,"velocity":0.41412207642595716,"acceleration":-2.6999999999999984,"pose":{"translation":{"x":7.433604315141906,"y":-1.1209353108447915},"rotation":{"radians":-0.40469686961167517}},"curvature":-8.81393295104801},{"time":3.7068032402609243,"velocity":0.3758114954619176,"acceleration":-2.699999999999999,"pose":{"translation":{"x":7.438696276906384,"y":-1.123276093041583},"rotation":{"radians":-0.45867228530255166}},"curvature":-10.5116637682121},{"time":3.7217061475405706,"velocity":0.3355736458068724,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.443377833241871,"y":-1.125762471341103},"rotation":{"radians":-0.5195379709979431}},"curvature":-12.521757406781552},{"time":3.7376764841691847,"velocity":0.29245373690961385,"acceleration":-2.1511698401327632,"pose":{"translation":{"x":7.44764580838088,"y":-1.1283956545710556},"rotation":{"radians":-0.5880100735834901}},"curvature":-14.85458396152343},{"time":3.755028959973427,"velocity":0.2551256143078946,"acceleration":-1.633997026424236,"pose":{"translation":{"x":7.4514977876621735,"y":-1.1311765859497145},"rotation":{"radians":-0.6646802737127656}},"curvature":-17.4795810166942},{"time":3.773857213312967,"velocity":0.224360304338324,"acceleration":-1.2920260604607863,"pose":{"translation":{"x":7.454932098973876,"y":-1.134105949488305},"rotation":{"radians":-0.7498755855456528}},"curvature":-20.29968500248048},{"time":3.7838551285067523,"velocity":0.21144273735767627,"acceleration":-1.0899098955310096,"pose":{"translation":{"x":7.456492305080279,"y":-1.1356264377697427},"rotation":{"radians":-0.7956562154609756}},"curvature":-21.728400778541683},{"time":3.7942143449657375,"velocity":0.2001521248290804,"acceleration":-0.9049130468186761,"pose":{"translation":{"x":7.457947794196594,"y":-1.1371841763933905},"rotation":{"radians":-0.8434788689153079}},"curvature":-23.12820866880771},{"time":3.80491588861381,"velocity":0.19046815836083997,"acceleration":-0.735425697805756,"pose":{"translation":{"x":7.459298557887232,"y":-1.1387791808097316},"rotation":{"radians":-0.8932284950696684}},"curvature":-24.46103818109851},{"time":3.815931670737958,"velocity":0.1823668691053123,"acceleration":-0.5794957153271393,"pose":{"translation":{"x":7.460544630646523,"y":-1.1404114514692556},"rotation":{"radians":-0.944741230203777}},"curvature":-25.684779785449013},{"time":3.8272241906318096,"velocity":0.1758229022115787,"acceleration":-0.4349050122088543,"pose":{"translation":{"x":7.461686089318814,"y":-1.1420809740225333},"rotation":{"radians":-0.9978029559869008}},"curvature":-26.755617400486308},{"time":3.8387467925622083,"velocity":0.17081166487836086,"acceleration":-0.2992389113278968,"pose":{"translation":{"x":7.462723052518566,"y":-1.1437877195202912},"rotation":{"radians":-1.0521507937987378}},"curvature":-27.631116920906926},{"time":3.8504445642069376,"velocity":0.16731123642642975,"acceleration":-0.16994683082951736,"pose":{"translation":{"x":7.463655680050451,"y":-1.1455316446134858},"rotation":{"radians":-1.1074779835281168}},"curvature":-28.2737787148395},{"time":3.862255893716841,"velocity":0.16530393840833846,"acceleration":-0.04439424774012141,"pose":{"translation":{"x":7.464484172329446,"y":-1.1473126917533785},"rotation":{"radians":-1.1634422271483342}},"curvature":-28.65458793715891},{"time":3.874114613746176,"velocity":0.16477747945347543,"acceleration":0.08009266419378874,"pose":{"translation":{"x":7.46520876980094,"y":-1.1491307893916096},"rotation":{"radians":-1.2196771131358384}},"curvature":-28.755999475861472},{"time":3.885952570160363,"velocity":0.16572561292129762,"acceleration":0.20618723801495994,"pose":{"translation":{"x":7.46582975236082,"y":-1.1509858521802734},"rotation":{"radians":-1.2758057659201345}},"curvature":-28.573825683680486},{"time":3.897702381789645,"velocity":0.16814827412833533,"acceleration":0.33652424457678903,"pose":{"translation":{"x":7.466347438775576,"y":-1.1528777811719926},"rotation":{"radians":-1.331455494132833}},"curvature":-28.117668468730983},{"time":3.909300124085043,"velocity":0.17205119559309046,"acceleration":0.4736603241313466,"pose":{"translation":{"x":7.466762186102392,"y":-1.1548064640199922},"rotation":{"radians":-1.3862720377899758}},"curvature":-27.409814592509886},{"time":3.9206876830333157,"velocity":0.177445030455594,"acceleration":0.6200300943560695,"pose":{"translation":{"x":7.467074389109252,"y":-1.1567717751781745},"rotation":{"radians":-1.4399320847953063}},"curvature":-26.482813979610114},{"time":3.9318145824632764,"velocity":0.18434404295904294,"acceleration":0.7778959727669047,"pose":{"translation":{"x":7.467284479695027,"y":-1.158773576101194},"rotation":{"radians":-1.4921530220635075}},"curvature":-25.376195561326828},{"time":3.942639172736599,"velocity":0.19276444813951232,"acceleration":0.949290967398619,"pose":{"translation":{"x":7.467392926309582,"y":-1.1608117154445305},"rotation":{"radians":-1.542699328039103}},"curvature":-24.13288147640941},{"time":3.953129160861284,"velocity":0.20272249911439444,"acceleration":1.135955117911212,"pose":{"translation":{"x":7.4674002333738665,"y":-1.1628860292645662},"rotation":{"radians":-1.5913854936628287}},"curvature":-22.79582618985712},{"time":3.9632615418252266,"velocity":0.21423242912701146,"acceleration":1.3392677471373278,"pose":{"translation":{"x":7.4673069407000146,"y":-1.1649963412186568},"rotation":{"radians":-1.6380757769664533}},"curvature":-21.405267157377484},{"time":3.973022045764004,"velocity":0.227304357248023,"acceleration":1.6813681577996658,"pose":{"translation":{"x":7.467113622911442,"y":-1.1671424627652092},"rotation":{"radians":-1.6826813861112742}},"curvature":-19.99678746639396},{"time":3.9913629212613007,"velocity":0.25814212129534597,"acceleration":2.195217952570515,"pose":{"translation":{"x":7.466429381060786,"y":-1.1715413206750205},"rotation":{"radians":-1.765489142784833}},"curvature":-17.239258032151128},{"time":4.00822689644302,"velocity":0.2951622223659588,"acceleration":2.700000000000086,"pose":{"translation":{"x":7.465352778998552,"y":-1.176080858285079},"rotation":{"radians":-1.839838206677028}},"curvature":-14.689952636946256},{"time":4.023734047095584,"velocity":0.3370315291278809,"acceleration":2.6999999999998843,"pose":{"translation":{"x":7.463889607768119,"y":-1.180759148508029},"rotation":{"radians":-1.9061486326525838}},"curvature":-12.431514400354017},{"time":4.038194096624452,"velocity":0.3760736628558241,"acceleration":2.700000000000073,"pose":{"translation":{"x":7.462046159722717,"y":-1.1855740882804664},"rotation":{"radians":-1.9650609451952596}},"curvature":-10.486462314410268},{"time":4.051936610928297,"velocity":0.4131784514762061,"acceleration":2.7000000000000557,"pose":{"translation":{"x":7.459829209968529,"y":-1.1905234049653206},"rotation":{"radians":-2.0173089249512017}},"curvature":-8.841322943841373},{"time":4.065161141127738,"velocity":0.44888468301469747,"acceleration":2.7000000000001942,"pose":{"translation":{"x":7.457245997807815,"y":-1.195604662754241},"rotation":{"radians":-2.063636181634097}},"curvature":-7.464887532413894},{"time":4.077995850981517,"velocity":0.4835383996199028,"acceleration":2.6999999999999784,"pose":{"translation":{"x":7.454304208182021,"y":-1.2008152690699803},"rotation":{"radians":-2.10474771146483}},"curvature":-6.319871017969904},{"time":4.102807927364417,"velocity":0.5505310058537315,"acceleration":2.70000000000004,"pose":{"translation":{"x":7.447377753155588,"y":-1.211613411542752},"rotation":{"radians":-2.1738212970746114}},"curvature":-4.5801372088327215},{"time":4.126791929649074,"velocity":0.615287812022308,"acceleration":2.700000000000023,"pose":{"translation":{"x":7.4391195320428505,"y":-1.2228941996783393},"rotation":{"radians":-2.2288160276989615}},"curvature":-3.37494202009036},{"time":4.150151464976035,"velocity":0.6783585574051022,"acceleration":2.7000000000000117,"pose":{"translation":{"x":7.429605174583705,"y":-1.234631902221708},"rotation":{"radians":-2.2729705708237473}},"curvature":-2.527916987831134},{"time":4.195346040111186,"velocity":0.8003839102700119,"acceleration":2.7000000000000144,"pose":{"translation":{"x":7.407136719595017,"y":-1.2593658620207715},"rotation":{"radians":-2.337722173229997}},"curvature":-1.4772175377638754},{"time":4.280221360101839,"velocity":1.0295472742447767,"acceleration":2.6999999999999953,"pose":{"translation":{"x":7.350974555129498,"y":-1.3129995813244224},"rotation":{"radians":-2.408909455392808}},"curvature":-0.5347997001052112},{"time":4.357308365158846,"velocity":1.2376821878986943,"acceleration":2.7000000000000033,"pose":{"translation":{"x":7.285072764551772,"y":-1.3703880432690192},"rotation":{"radians":-2.4353803498498174}},"curvature":-0.1169882682753502},{"time":4.4256128254794405,"velocity":1.422104230764299,"acceleration":-1.2978122319032706,"pose":{"translation":{"x":7.215918010652873,"y":-1.4292870796027968},"rotation":{"radians":-2.432432107294122}},"curvature":0.17737888614083197},{"time":4.489497129805158,"velocity":1.3391943991837518,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.149694042178108,"y":-1.487544673339861},"rotation":{"radians":-2.402842074915739}},"curvature":0.5161747347143254},{"time":4.5537511090136045,"velocity":1.1657086553209464,"acceleration":-2.7,"pose":{"translation":{"x":7.091673621783036,"y":-1.5433107520931373},"rotation":{"radians":-2.3420150749522866}},"curvature":1.0553635894564308},{"time":4.586028361710854,"velocity":1.0785600730383729,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":7.066988739956747,"y":-1.5698155175341644},"rotation":{"radians":-2.296842094749197}},"curvature":1.46358366454697},{"time":4.618120918759651,"velocity":0.9919101690066217,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":7.045610453989442,"y":-1.5952469814073202},"rotation":{"radians":-2.239856166388038}},"curvature":1.996087590213061},{"time":4.649955434645675,"velocity":0.9059569761143562,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":7.027670862293224,"y":-1.6195522301762182},"rotation":{"radians":-2.170149313599266}},"curvature":2.643464806503541},{"time":4.681660456826774,"velocity":0.8203534162253896,"acceleration":-2.0864951796849778,"pose":{"translation":{"x":7.013131113141328,"y":-1.6427365580918252},"rotation":{"radians":-2.0886536159341755}},"curvature":3.309498733349602},{"time":4.697491663437206,"velocity":0.787321679944126,"acceleration":-1.209308415376104,"pose":{"translation":{"x":7.007072617840989,"y":-1.6539275607758022},"rotation":{"radians":-2.0447575100318383}},"curvature":3.5778366924102567},{"time":4.713127866378614,"velocity":0.7684126881425526,"acceleration":0.8313216487286121,"pose":{"translation":{"x":7.001762402416748,"y":-1.6648700232341174},"rotation":{"radians":-2.0001134513348635}},"curvature":3.741829781587491},{"time":4.742481042049709,"velocity":0.7928146185368677,"acceleration":2.700000000000043,"pose":{"translation":{"x":6.993126971360874,"y":-1.6860940035537344},"rotation":{"radians":-1.9150753113486085}},"curvature":3.5316640430096276},{"time":4.768519029037876,"velocity":0.8631171834049169,"acceleration":2.700000000000008,"pose":{"translation":{"x":6.986559104322092,"y":-1.706627752913635},"rotation":{"radians":-1.8500885056813583}},"curvature":2.2972998269570555},{"time":4.791838638584075,"velocity":0.9260801291796544,"acceleration":2.699999999999998,"pose":{"translation":{"x":6.981146126504432,"y":-1.7267749571307505},"rotation":{"radians":-1.8244704229335087}},"curvature":6.389810104651532E-14},{"time":4.8772957207031595,"velocity":1.1568142509011834,"acceleration":2.700000000000002,"pose":{"translation":{"x":6.9592661543988825,"y":-1.8130425384870001},"rotation":{"radians":-1.811400282697703}},"curvature":0.18305419903007367},{"time":4.967415245295342,"velocity":1.4001369673000763,"acceleration":2.70000000000001,"pose":{"translation":{"x":6.932764737139686,"y":-1.9251688628471044},"rotation":{"radians":-1.7963819825529694}},"curvature":0.08233336529423564},{"time":5.014618955459946,"velocity":1.5275869847445067,"acceleration":2.7000000000000037,"pose":{"translation":{"x":6.9174684998367875,"y":-1.9925542910646845},"rotation":{"radians":-1.7920610655072007}},"curvature":0.04542948812004931},{"time":5.061839473007546,"velocity":1.6550823821230267,"acceleration":0.9094428271800233,"pose":{"translation":{"x":6.901075449959414,"y":-2.0658880189922586},"rotation":{"radians":-1.7897169508357862}},"curvature":0.018473484793554897},{"time":5.108908049761268,"velocity":1.697888561637272,"acceleration":-0.26387371111017716,"pose":{"translation":{"x":6.883970919741171,"y":-2.142921707934635},"rotation":{"radians":-1.7891576401649198}},"curvature":-0.003838170076288228},{"time":5.15613368373084,"velocity":1.6854269583421906,"acceleration":-0.34147593973972523,"pose":{"translation":{"x":6.866629562244371,"y":-2.220906495772335},"rotation":{"radians":-1.7903543306679175}},"curvature":-0.02668672374829891},{"time":5.202548560881559,"velocity":1.6695773945492447,"acceleration":-2.149695336898206,"pose":{"translation":{"x":6.849566333832086,"y":-2.2968748477473047},"rotation":{"radians":-1.7935050152380263}},"curvature":-0.05623993874744346},{"time":5.2475067554213135,"velocity":1.572930973391772,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.833287476640194,"y":-2.367922407248626},"rotation":{"radians":-1.7991214804768882}},"curvature":-0.10257183437654888},{"time":5.330413349628705,"velocity":1.3490831690318168,"acceleration":-2.700000000000002,"pose":{"translation":{"x":6.8047701681574635,"y":-2.4856447178365855},"rotation":{"radians":-1.822526000943982}},"curvature":-0.3523138191517206},{"time":5.393855616593966,"velocity":1.1777890482256093,"acceleration":2.7000000000000206,"pose":{"translation":{"x":6.783090623277312,"y":-2.5628124674485733},"rotation":{"radians":-1.879661531385654}},"curvature":-1.3684642417610602},{"time":5.432266865113056,"velocity":1.2814994192271516,"acceleration":2.700000000000013,"pose":{"translation":{"x":6.766985279057036,"y":-2.6072139966365957},"rotation":{"radians":-1.9513027039074702}},"curvature":-1.5427633070819612E-12},{"time":5.50707600533672,"velocity":1.4834840978310444,"acceleration":2.7000000000000113,"pose":{"translation":{"x":6.731791420174782,"y":-2.7044647720887713},"rotation":{"radians":-1.8895360249931248}},"curvature":0.3028962384922683},{"time":5.558530510816899,"velocity":1.6224112626275276,"acceleration":0.5340583566499336,"pose":{"translation":{"x":6.707339661769482,"y":-2.7805377969098837},"rotation":{"radians":-1.8773477894358561}},"curvature":0.047626527361036446},{"time":5.616117894387626,"velocity":1.6531662860610792,"acceleration":-1.1577996380084765,"pose":{"translation":{"x":6.678867013701109,"y":-2.8704533716239875},"rotation":{"radians":-1.8796694858289695}},"curvature":-0.087437362490553},{"time":5.676720221788557,"velocity":1.5830009333338104,"acceleration":-1.7239520448331906,"pose":{"translation":{"x":6.64846271193872,"y":-2.9636803526706923},"rotation":{"radians":-1.8947048458847193}},"curvature":-0.22811624607870962},{"time":5.735921936029287,"velocity":1.4809400170108733,"acceleration":-1.7930050997699363,"pose":{"translation":{"x":6.618459990812718,"y":-3.049269313584438},"rotation":{"radians":-1.9246360307009078}},"curvature":-0.45654182535567983},{"time":5.789176920436449,"velocity":1.385453558380664,"acceleration":2.700000000000016,"pose":{"translation":{"x":6.5905210891597985,"y":-3.120296800861932},"rotation":{"radians":-1.9698094797322772}},"curvature":-0.7007252930308779},{"time":5.833121807218755,"velocity":1.5041047526928912,"acceleration":1.4644282265429305,"pose":{"translation":{"x":6.564722256467882,"y":-3.178309589829586},"rotation":{"radians":-2.001488838181345}},"curvature":2.5539066129365256E-13},{"time":5.875409835553279,"velocity":1.5660325350308155,"acceleration":1.2297898950302844,"pose":{"translation":{"x":6.537938124915615,"y":-3.237441399690594},"rotation":{"radians":-1.9880549453765368}},"curvature":0.2640301659784271},{"time":5.928175944872254,"velocity":1.630923763071354,"acceleration":0.666041558250595,"pose":{"translation":{"x":6.504467396303145,"y":-3.3148614817153645},"rotation":{"radians":-1.971607271981553}},"curvature":0.13072239607567918},{"time":5.993149187477557,"velocity":1.6741986428207842,"acceleration":0.333778151245732,"pose":{"translation":{"x":6.4630861417637835,"y":-3.4139391985919607},"rotation":{"radians":-1.9627263649341367}},"curvature":0.04756533449352125},{"time":6.065539106001328,"velocity":1.6983608159944779,"acceleration":-0.2750046569252419,"pose":{"translation":{"x":6.416671370635927,"y":-3.5268403456821757},"rotation":{"radians":-1.9598523044604}},"curvature":0.0029788781311610086},{"time":6.138060689585077,"velocity":1.6784170427813536,"acceleration":-0.5992029561921169,"pose":{"translation":{"x":6.370156860656412,"y":-3.6401059183334414},"rotation":{"radians":-1.9619346995648212}},"curvature":-0.03968862552733675},{"time":6.2034375840802065,"velocity":1.639243014333212,"acceleration":-1.0560587651523559,"pose":{"translation":{"x":6.3284889881538575,"y":-3.74023087919074},"rotation":{"radians":-1.9697003811835467}},"curvature":-0.11439521721935789},{"time":6.256806038038413,"velocity":1.582882790748018,"acceleration":2.699999999999979,"pose":{"translation":{"x":6.294582558242029,"y":-3.81924292550852},"rotation":{"radians":-1.984322806626164}},"curvature":-0.2283636351184617},{"time":6.29762897442008,"velocity":1.6931047189785187,"acceleration":-1.9259183125779071,"pose":{"translation":{"x":6.267276635013188,"y":-3.88028125646261},"rotation":{"radians":-1.9961898206316888}},"curvature":-3.718304897395597E-13},{"time":6.338419796270027,"velocity":1.6145449281926036,"acceleration":0.7545941402288411,"pose":{"translation":{"x":6.23922019896753,"y":-3.941631121038789},"rotation":{"radians":-2.0049477277332395}},"curvature":-0.16335886169672642},{"time":6.392801741348136,"velocity":1.6555812252827915,"acceleration":0.35274926917193355,"pose":{"translation":{"x":6.201335361250187,"y":-4.0220744791727014},"rotation":{"radians":-2.0156859277247685}},"curvature":-0.08280776660273527},{"time":6.460740596832295,"velocity":1.6795466069032061,"acceleration":-0.9153978571626162,"pose":{"translation":{"x":6.152210651921689,"y":-4.124162319704786},"rotation":{"radians":-2.022062347830648}},"curvature":-0.037586189160884666},{"time":6.537870890516304,"velocity":1.6089417013425409,"acceleration":-2.7,"pose":{"translation":{"x":6.09668889886182,"y":-4.238183851564877},"rotation":{"radians":-2.0254071580455033}},"curvature":-0.017583286099940882},{"time":6.619880238364254,"velocity":1.3875164621530773,"acceleration":-2.6999999999999993,"pose":{"translation":{"x":6.042646637292194,"y":-4.348529551531317},"rotation":{"radians":-2.026743957009689}},"curvature":-0.003811340975765062},{"time":6.699514882416592,"velocity":1.1725029232117652,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":5.997773519298815,"y":-4.440054211990066},"rotation":{"radians":-2.0261395522652093}},"curvature":0.02077742451270997},{"time":6.81616980216361,"velocity":0.8575346398948168,"acceleration":-2.7,"pose":{"translation":{"x":5.946035363842139,"y":-4.5465594485211},"rotation":{"radians":-2.0183163019521078}},"curvature":-3.012836197628505E-13},{"time":6.841893878536755,"velocity":0.7880796336873248,"acceleration":-1.2542204787890836,"pose":{"translation":{"x":5.936556497255027,"y":-4.565484265063717},"rotation":{"radians":-2.064628046880355}},"curvature":-3.571427190245016},{"time":6.856705904936342,"velocity":0.7695020868445985,"acceleration":2.699999999999826,"pose":{"translation":{"x":5.930873224440056,"y":-4.5755225665272805},"rotation":{"radians":-2.1075808705966947}},"curvature":-3.7321628991871814},{"time":6.87259832971659,"velocity":0.8124116337512672,"acceleration":2.700000000000061,"pose":{"translation":{"x":5.924201504522159,"y":-4.586176140795628},"rotation":{"radians":-2.1520402224255477}},"curvature":-3.2843626854041},{"time":6.906458393735083,"velocity":0.9038338066012014,"acceleration":2.700000000000035,"pose":{"translation":{"x":5.907264235271395,"y":-4.609785149466344},"rotation":{"radians":-2.2277586558349807}},"curvature":-1.9758845219610193},{"time":6.943144509816377,"velocity":1.002886320020695,"acceleration":2.6999999999999753,"pose":{"translation":{"x":5.885140344231054,"y":-4.636873699129188},"rotation":{"radians":-2.277838812922086}},"curvature":-0.9905387662992968},{"time":7.021316491852703,"velocity":1.213950671518773,"acceleration":-0.06898886651647455,"pose":{"translation":{"x":5.8272724292904545,"y":-4.701364426734989},"rotation":{"radians":-2.3124024921267456}},"curvature":0.013955931147822767},{"time":7.1039819311848795,"velocity":1.2082476765591597,"acceleration":-2.6999999999999975,"pose":{"translation":{"x":5.760503657459791,"y":-4.775964384068014},"rotation":{"radians":-2.278730146762986}},"curvature":0.661093781844257},{"time":7.147838555204729,"velocity":1.0898347917055649,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":5.728505684666588,"y":-4.814894978457702},"rotation":{"radians":-2.235162805251749}},"curvature":1.0961299502459416},{"time":7.194610750125419,"velocity":0.9635498654197036,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.700081747766614,"y":-4.853599797205923},"rotation":{"radians":-2.1680248787945833}},"curvature":1.757620630537024},{"time":7.218915175799948,"velocity":0.8979279160984739,"acceleration":-2.699999999999998,"pose":{"translation":{"x":5.687766218327823,"y":-4.8725745365264155},"rotation":{"radians":-2.123216480360578}},"curvature":2.2261447269644323},{"time":7.243783223456295,"velocity":0.830784187426338,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.676964548292993,"y":-4.89115819582592},"rotation":{"radians":-2.069282861912245}},"curvature":2.8197852070434726},{"time":7.269248244143494,"velocity":0.7620286315709007,"acceleration":-2.699999999999997,"pose":{"translation":{"x":5.6678240008681655,"y":-4.90926204359236},"rotation":{"radians":-2.0049285397303866}},"curvature":3.557955262784595},{"time":7.295445415528923,"velocity":0.6912962688302415,"acceleration":-2.699999999999999,"pose":{"translation":{"x":5.660452710336893,"y":-4.92681346622997},"rotation":{"radians":-1.9290964956932903}},"curvature":4.438007692288481},{"time":7.308908309801176,"velocity":0.65494645429516,"acceleration":-2.699999999999998,"pose":{"translation":{"x":5.657452152605622,"y":-4.935364456137411},"rotation":{"radians":-1.886727236623641}},"curvature":4.9177115386256585},{"time":7.322690018027458,"velocity":0.6177358420841967,"acceleration":-2.337828346394491,"pose":{"translation":{"x":5.654913682519064,"y":-4.943758904004977},"rotation":{"radians":-1.841461127225999}},"curvature":5.407362253384503},{"time":7.3368152816211785,"velocity":0.5847134004545045,"acceleration":-1.909268343064559,"pose":{"translation":{"x":5.652837140182536,"y":-4.951993573710473},"rotation":{"radians":-1.79349304281632}},"curvature":5.887059520979176},{"time":7.351236640696314,"velocity":0.5571791561083812,"acceleration":-1.4594671757952518,"pose":{"translation":{"x":5.651218795229721,"y":-4.960066786991287},"rotation":{"radians":-1.7431630871890897}},"curvature":6.330503910917793},{"time":7.3658705309478085,"velocity":0.5358214736321345,"acceleration":-0.9835096934742872,"pose":{"translation":{"x":5.650051159337019,"y":-4.967978515192694},"rotation":{"radians":-1.6909786461077827}},"curvature":6.705859650499326},{"time":7.380600810165643,"velocity":0.5213341012338114,"acceleration":-0.4664822988598888,"pose":{"translation":{"x":5.649322798737876,"y":-4.9757304710161625},"rotation":{"radians":-1.6376263776857742}},"curvature":6.977977660835564},{"time":7.3952787027424876,"velocity":0.5144871241621465,"acceleration":0.12164654678623932,"pose":{"translation":{"x":5.649018146737125,"y":-4.983326200267652},"rotation":{"radians":-1.5839693528694552}},"curvature":7.111918230185031},{"time":7.409725980774572,"velocity":0.5162445856452103,"acceleration":0.052672211309998954,"pose":{"translation":{"x":5.649117316225329,"y":-4.9907711736059195},"rotation":{"radians":-1.531026634408773}},"curvature":7.077199829846817},{"time":7.423889982741932,"velocity":0.5169906349498303,"acceleration":-2.699999999999995,"pose":{"translation":{"x":5.649595912193108,"y":-4.998072878290817},"rotation":{"radians":-1.4799363085462358}},"curvature":6.851731928515863},{"time":7.438396838317813,"velocity":0.47782212489495235,"acceleration":-2.700000000000001,"pose":{"translation":{"x":5.650424844245491,"y":-5.005240909931599},"rotation":{"radians":-1.4319073873778112}},"curvature":6.424251674858325},{"time":7.471192082814179,"velocity":0.389274964754765,"acceleration":-2.7,"pose":{"translation":{"x":5.6529927531822,"y":-5.019225428754662},"rotation":{"radians":-1.349929704456554}},"curvature":4.970754709570831},{"time":7.615367995686314,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":5.660487567245657,"y":-5.0462680925649535},"rotation":{"radians":-1.2742505177252024}},"curvature":-2.1831735954903813E-13}] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4504f62..c8ac8f8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,6 +64,8 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + /* Builds Autos */ + m_robotContainer.buildAutos(); SmartDashboard.putString("Is Auto Start?", "NAH"); } @@ -82,7 +84,8 @@ public class Robot extends TimedRobot { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); m_robotContainer.resetOdometry(new Pose2d()); - m_robotContainer.resetGyroYawRobotContainer(0); + + //m_robotContainer.resetGyroYawRobotContainer(0); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b7db8a8..c4a1d3e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -136,7 +136,7 @@ public class RobotContainer { configureButtonBindings(); /* Builds Autos */ - buildAutos(); + //buildAutos(); /* Default Commands */ // drives the robot with a two-axis input from the driver controller @@ -321,11 +321,11 @@ public class RobotContainer { try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { @@ -380,6 +380,9 @@ public class RobotContainer { String path = paths[0]; String trajectoryJSON = "paths/" + path + ".wpilib.json"; Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON); + + SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString()); + Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath); initialTrajectory = trajectory; From db128a328a938186c55240b93ca53f442ab10ac7 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 11 Mar 2020 20:56:18 -0600 Subject: [PATCH 36/39] Added Hood with joy, full manual all programmed but not might not work --- .../java/frc4388/robot/RobotContainer.java | 36 +++++++------ .../commands/shooter/RunHoodWithJoystick.java | 54 +++++++++++++++++++ .../robot/commands/shooter/ShooterManual.java | 45 ++++++++++++++++ .../frc4388/robot/subsystems/ShooterHood.java | 5 ++ .../controller/JoystickManualButton.java | 53 ++++++++++++++++++ .../utility/controller/XboxController.java | 4 ++ 6 files changed, 181 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java create mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterManual.java create mode 100644 src/main/java/frc4388/utility/controller/JoystickManualButton.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3612c9d..cfd064b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,9 +39,12 @@ import frc4388.robot.commands.drive.PlaySongDrive; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.RunHoodWithJoystick; import frc4388.robot.commands.shooter.ShootPrepGroup; import frc4388.robot.commands.shooter.ShooterGoalPosition; +import frc4388.robot.commands.shooter.ShooterManual; import frc4388.robot.commands.shooter.ShooterTrenchPosition; +import frc4388.robot.commands.shooter.ShooterVelocityControlPID; import frc4388.robot.commands.shooter.TrackTarget; import frc4388.robot.commands.shooter.TrimShooter; import frc4388.robot.commands.storage.ManageStorage; @@ -61,6 +64,7 @@ import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; +import frc4388.utility.controller.JoystickManualButton; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.XboxTriggerButton; @@ -90,10 +94,12 @@ public class RobotContainer { private final LimeLight m_robotLime = new LimeLight(); /* Controllers */ - private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - private final XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID); + private static XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); + private static XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private static XboxController m_buttonFox = new XboxController(OIConstants. BUTTON_FOX_ID); + private static XboxController m_manualXbox = new XboxController(3); + public static boolean m_isShooterManual = false; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -121,6 +127,8 @@ public class RobotContainer { m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); + // runs the hood with joystick + m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); // moves the drum not m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(1500), m_robotShooter)); // drives climber with input from triggers on the opperator controller @@ -226,23 +234,18 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); - //Prepares storage for intaking - //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.LEFT_TRIGGER) - //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.8))) - //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - //.whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); - - //Runs storage to outtake - //new XboxTriggerButton(m_operatorXbox, XboxTriggerButton.RIGHT_TRIGGER) - //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.8))) - //.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); //Run drum - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false) .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); + //Run drum manual + new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true) + .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000))) + .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + @@ -259,8 +262,8 @@ public class RobotContainer { // Shooter Manual new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH) - .whileHeld(new PlaySongDrive(m_robotDrive)) - .whenReleased(new InterruptSubystem(m_robotDrive)); + .whileHeld(new ShooterManual(true)) + .whenReleased(new ShooterManual(false)); // Goal Shooter Position new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) @@ -384,6 +387,7 @@ public class RobotContainer { return m_buttonFox; } + /** * Gets the {@link edu.wpi.first.wpilibj.GenericHID#GenericHID(int) Generic HID} for the Operator Xbox Controller. * Generic HIDs/Joysticks can be used to set up JoystickButtons. diff --git a/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java b/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java new file mode 100644 index 0000000..29ca105 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.utility.controller.IHandController; + +public class RunHoodWithJoystick extends CommandBase { + private ShooterHood m_hood; + private IHandController m_controller; + + /** + * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. + * @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) { + m_hood = subsystem; + m_controller = controller; + addRequirements(m_hood); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double input = m_controller.getRightXAxis(); + m_hood.runHood(input); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java b/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java new file mode 100644 index 0000000..2017ff4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; + +public class ShooterManual extends CommandBase { + public boolean isManual = false; + /** + * Creates a new ShooterManual. + */ + public ShooterManual(boolean man) { + isManual = man; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.m_isShooterManual = isManual; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.m_isShooterManual = isManual; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index c177c38..e286a2e 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -86,6 +86,11 @@ public class ShooterHood extends SubsystemBase { m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); } + public void runHood(double input) + { + m_angleAdjustMotor.set(input); + } + public void resetGyroAngleAdj(){ m_angleEncoder.setPosition(0); } diff --git a/src/main/java/frc4388/utility/controller/JoystickManualButton.java b/src/main/java/frc4388/utility/controller/JoystickManualButton.java new file mode 100644 index 0000000..6455639 --- /dev/null +++ b/src/main/java/frc4388/utility/controller/JoystickManualButton.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.Button; +import frc4388.robot.RobotContainer; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A {@link Button} that gets its state from a {@link GenericHID}. + */ +public class JoystickManualButton extends Button { + private final GenericHID m_joystick; + private final int m_buttonNumber; + private boolean m_buttonType; + + /** + * Creates a joystick button for triggering commands. + * + * @param joystick The GenericHID object that has the button (e.g. Joystick, + * KinectStick, etc) + * @param buttonNumber The button number (see + * {@link GenericHID#getRawButton(int) } + */ + public JoystickManualButton(GenericHID joystick, int buttonNumber, boolean buttonType) { + requireNonNullParam(joystick, "joystick", "JoystickButton"); + + m_joystick = joystick; + m_buttonNumber = buttonNumber; + } + + /** + * Gets the value of the joystick button. + * + * @return The value of the joystick button + */ + @Override + public boolean get() { + boolean m = RobotContainer.m_isShooterManual; + if (m_buttonType == m) { + return m_joystick.getRawButton(m_buttonNumber); + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 4353d1e..38c4736 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -60,6 +60,10 @@ public class XboxController implements IHandController m_stick = new Joystick(portNumber); } + public void setJoystick(Joystick joy) { + m_stick = joy; + } + /** * @return Joystick for Xbox Controller */ From d1254f1e1d6b27de70ab9151f197fd556e619e57 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 13 Mar 2020 08:37:28 -0600 Subject: [PATCH 37/39] Program Simple Auto --- PathWeaver/Groups/TenBallAuto | 2 + PathWeaver/Paths/TenBallMidComplete | 2 - .../driverStation/GOAT DRIVERSTATION.json | 159 +++++++++++++++++- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 12 +- .../commands/auto/TenBallAutoMiddle.java | 35 +++- .../commands/shooter/CalibrateShooter.java | 5 + .../robot/commands/shooter/PrepChecker.java | 52 ++++++ 8 files changed, 255 insertions(+), 14 deletions(-) create mode 100644 PathWeaver/Groups/TenBallAuto create mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java diff --git a/PathWeaver/Groups/TenBallAuto b/PathWeaver/Groups/TenBallAuto new file mode 100644 index 0000000..87ec839 --- /dev/null +++ b/PathWeaver/Groups/TenBallAuto @@ -0,0 +1,2 @@ +SixBallMidComplete +TenBallMidComplete diff --git a/PathWeaver/Paths/TenBallMidComplete b/PathWeaver/Paths/TenBallMidComplete index ead609f..a87ff34 100644 --- a/PathWeaver/Paths/TenBallMidComplete +++ b/PathWeaver/Paths/TenBallMidComplete @@ -1,6 +1,4 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Name -3.2,-2.4,0.2,2.5,true, -5.0,-1.1,3.0,0.0,true, 7.2,-1.1,1.5,0.0,true, 6.981146126504424,-1.7267749571307522,-0.16656954801462032,-0.6424825423421034,true, 6.766985279057057,-2.6072139966365975,-0.1189782485818709,-0.2974456214546768,true, diff --git a/src/main/driverStation/GOAT DRIVERSTATION.json b/src/main/driverStation/GOAT DRIVERSTATION.json index a191e6e..e61f688 100644 --- a/src/main/driverStation/GOAT DRIVERSTATION.json +++ b/src/main/driverStation/GOAT DRIVERSTATION.json @@ -335,6 +335,155 @@ "Colors/Color when true": "#7CFC00FF", "Colors/Color when false": "#8B0000FF" } + }, + "8,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance to Target", + "_title": "Distance to Target" + } + }, + "9,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball in Intake!", + "_title": "!Ball in Intake!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "0,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball Storage!", + "_title": "!Ball Storage!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "1,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/!Ball Shooter!", + "_title": "!Ball Shooter!", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "2,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Center Displacement", + "_title": "Center Displacement" + } + }, + "3,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Is Auto Start?", + "_title": "Is Auto Start?" + } + }, + "4,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/trajectoryPath Initial", + "_title": "trajectoryPath Initial" + } + }, + "5,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Motor Pos Inches", + "_title": "Left Motor Pos Inches" + } + }, + "6,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Motor Pos Inches", + "_title": "Right Motor Pos Inches" + } + }, + "7,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Motor Pos Meters", + "_title": "Left Motor Pos Meters" + } + }, + "8,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Motor Pos Meters", + "_title": "Right Motor Pos Meters" + } + }, + "9,4": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Odometry Values Meters", + "_title": "Odometry Values Meters" + } + }, + "0,5": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Trajectory Total Time", + "_title": "Trajectory Total Time" + } } } } @@ -645,8 +794,8 @@ "Controls/Rotation": "NONE", "compression": -1.0, "fps": -1, - "imageWidth": -1, - "imageHeight": -1 + "imageWidth": 0, + "imageHeight": 0 } }, "8,4": { @@ -857,9 +1006,9 @@ } ], "windowGeometry": { - "x": 40.0, - "y": 142.39999389648438, + "x": -6.400000095367432, + "y": 1.600000023841858, "width": 1547.199951171875, - "height": 1481.5999755859375 + "height": 828.7999877929688 } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a69160e..543cbb5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -171,7 +171,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 11; public static final double STORAGE_PARTIAL_BALL = 2; public static final double STORAGE_FULL_BALL = 7; - public static final double STORAGE_SPEED = 1.0; + public static final double STORAGE_SPEED = 0.5; public static final double STORAGE_TIMEOUT = 3000; /* Storage Characteristics */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dca2c7e..788f91b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -269,6 +269,8 @@ public class RobotContainer { } public void buildAutos() { + resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); + String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" }; @@ -300,10 +302,12 @@ public class RobotContainer { m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); String[] tenBallAutoMiddlePaths = new String[]{ + "SixBallMidComplete", "TenBallMidComplete" }; - m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); + m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, + m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths)); } /** @@ -321,12 +325,12 @@ public class RobotContainer { try { SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); - return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + //return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); + return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); diff --git a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java index 7ff19da..28ae59c 100644 --- a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java @@ -7,9 +7,21 @@ package frc4388.robot.commands.auto; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.commands.shooter.CalibrateShooter; +import frc4388.robot.commands.shooter.PrepChecker; +import frc4388.robot.commands.shooter.ShootPrepGroup; +import frc4388.robot.commands.storage.RunStorage; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; +import frc4388.robot.subsystems.ShooterHood; +import frc4388.robot.subsystems.Storage; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: @@ -18,11 +30,30 @@ public class TenBallAutoMiddle extends SequentialCommandGroup { /** * Creates a new TenBallAutoMiddle. */ - public TenBallAutoMiddle(Drive drive, RamseteCommand[] paths) { + public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( - paths[0] + new ParallelDeadlineGroup( + new Wait(drive, 0.1, 0), + new CalibrateShooter(shooter, shooterAim, shooterHood) + ), + new ParallelDeadlineGroup( + new Wait(drive, 1, 0), + new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim) + ), + new ParallelDeadlineGroup( + new Wait(drive, 4, 0), + new PrepChecker(shooter, shooterAim), + new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake), + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage) + ), + new ParallelDeadlineGroup( + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage), + new RunStorage(storage) + ) + //paths[0], + //paths[1] ); } } diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java index 23be5fa..5e28114 100644 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java @@ -7,6 +7,7 @@ package frc4388.robot.commands.shooter; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.SoftLimitDirection; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -59,6 +60,10 @@ public class CalibrateShooter extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { + if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() && + m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) { + return true; + } return false; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java new file mode 100644 index 0000000..cc978fd --- /dev/null +++ b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Shooter; +import frc4388.robot.subsystems.ShooterAim; + +public class PrepChecker extends CommandBase { + Shooter m_shooter; + ShooterAim m_shooterAim; + + /** + * Creates a new PrepChecker. + */ + public PrepChecker(Shooter shooter, ShooterAim shooterAim) { + // Use addRequirements() here to declare subsystem dependencies. + m_shooter = shooter; + m_shooterAim = shooterAim; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_shooterAim.m_isAimReady && m_shooter.m_isDrumReady) { + return true; + } + + return false; + } +} From bb9c325479daabcf8eabdc401c2e80752cf88626 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 13 Mar 2020 17:10:44 -0600 Subject: [PATCH 38/39] doneish --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- .../frc4388/robot/commands/shooter/RunHoodWithJoystick.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cfd064b..eaaab06 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -139,7 +139,7 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the storage not //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); - m_robotStorage.setDefaultCommand(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.IDLE))); + m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -257,7 +257,7 @@ public class RobotContainer { // Meg new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) - .whileHeld(new PlaySongDrive(m_robotDrive)) + .whenPressed(new PlaySongDrive(m_robotDrive)) .whenReleased(new InterruptSubystem(m_robotDrive)); // Shooter Manual diff --git a/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java b/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java index 29ca105..f406e04 100644 --- a/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java @@ -37,7 +37,7 @@ public class RunHoodWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double input = m_controller.getRightXAxis(); + double input = m_controller.getRightYAxis(); m_hood.runHood(input); } From a62a91f5f3c1ef52a5a753b87069f6fba85b9325 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 13 Mar 2020 20:51:06 -0600 Subject: [PATCH 39/39] highlanders fin --- .VSCodeCounter/details.md | 98 ++++++++++++++ .VSCodeCounter/results.csv | 85 ++++++++++++ .VSCodeCounter/results.md | 41 ++++++ .VSCodeCounter/results.txt | 127 ++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 22 +-- .../commands/drive/DriveWithJoystick.java | 2 +- .../robot/commands/drive/PlaySongDrive.java | 5 +- .../robot/commands/drive/SkipSong.java | 56 ++++++++ .../java/frc4388/robot/subsystems/Drive.java | 14 +- .../controller/JoystickManualButton.java | 1 + 10 files changed, 434 insertions(+), 17 deletions(-) create mode 100644 .VSCodeCounter/details.md create mode 100644 .VSCodeCounter/results.csv create mode 100644 .VSCodeCounter/results.md create mode 100644 .VSCodeCounter/results.txt create mode 100644 src/main/java/frc4388/robot/commands/drive/SkipSong.java diff --git a/.VSCodeCounter/details.md b/.VSCodeCounter/details.md new file mode 100644 index 0000000..1306275 --- /dev/null +++ b/.VSCodeCounter/details.md @@ -0,0 +1,98 @@ +# Details + +Date : 2020-03-13 19:46:18 + +Directory c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main + +Total : 83 files, 4888 codes, 1770 comments, 1110 blanks, all 7768 lines + +[summary](results.md) + +## Files +| filename | language | code | comment | blank | total | +| :--- | :--- | ---: | ---: | ---: | ---: | +| [src/main/deploy/paths/DriveOffLineBackward.wpilib.json](/src/main/deploy/paths/DriveOffLineBackward.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/DriveOffLineForward.wpilib.json](/src/main/deploy/paths/DriveOffLineForward.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/EightBallMid0.wpilib.json](/src/main/deploy/paths/EightBallMid0.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/EightBallMid1.wpilib.json](/src/main/deploy/paths/EightBallMid1.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/EightBallMid2.wpilib.json](/src/main/deploy/paths/EightBallMid2.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/EightBallMidComplete.wpilib.json](/src/main/deploy/paths/EightBallMidComplete.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/FiveBallMidComplete.wpilib.json](/src/main/deploy/paths/FiveBallMidComplete.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/SixBallMid0.wpilib.json](/src/main/deploy/paths/SixBallMid0.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/SixBallMid1.wpilib.json](/src/main/deploy/paths/SixBallMid1.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/SixBallMidComplete.wpilib.json](/src/main/deploy/paths/SixBallMidComplete.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/deploy/paths/TenBallMidComplete.wpilib.json](/src/main/deploy/paths/TenBallMidComplete.wpilib.json) | JSON | 1 | 0 | 0 | 1 | +| [src/main/driverStation/GOAT DRIVERSTATION.json](/src/main/driverStation/GOAT DRIVERSTATION.json) | JSON | 1,014 | 0 | 0 | 1,014 | +| [src/main/driverStation/themes/Ridgbotics/ridgeboticstheme.css](/src/main/driverStation/themes/Ridgbotics/ridgeboticstheme.css) | CSS | 8 | 0 | 1 | 9 | +| [src/main/java/frc4388/robot/Constants.java](/src/main/java/frc4388/robot/Constants.java) | Java | 157 | 32 | 41 | 230 | +| [src/main/java/frc4388/robot/Main.java](/src/main/java/frc4388/robot/Main.java) | Java | 9 | 16 | 5 | 30 | +| [src/main/java/frc4388/robot/Robot.java](/src/main/java/frc4388/robot/Robot.java) | Java | 59 | 63 | 24 | 146 | +| [src/main/java/frc4388/robot/RobotContainer.java](/src/main/java/frc4388/robot/RobotContainer.java) | Java | 312 | 145 | 96 | 553 | +| [src/main/java/frc4388/robot/commands/InterruptSubystem.java](/src/main/java/frc4388/robot/commands/InterruptSubystem.java) | Java | 21 | 14 | 8 | 43 | +| [src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java](/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java) | Java | 22 | 19 | 6 | 47 | +| [src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java](/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java) | Java | 26 | 23 | 6 | 55 | +| [src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java](/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java) | Java | 13 | 14 | 5 | 32 | +| [src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java](/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java) | Java | 11 | 14 | 5 | 30 | +| [src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java](/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java) | Java | 11 | 14 | 5 | 30 | +| [src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java](/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java) | Java | 12 | 13 | 4 | 29 | +| [src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java](/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java) | Java | 16 | 14 | 6 | 36 | +| [src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java](/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java) | Java | 42 | 14 | 12 | 68 | +| [src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java](/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java) | Java | 40 | 16 | 4 | 60 | +| [src/main/java/frc4388/robot/commands/auto/Wait.java](/src/main/java/frc4388/robot/commands/auto/Wait.java) | Java | 40 | 16 | 16 | 72 | +| [src/main/java/frc4388/robot/commands/climber/DisengageRachet.java](/src/main/java/frc4388/robot/commands/climber/DisengageRachet.java) | Java | 26 | 14 | 9 | 49 | +| [src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java](/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java) | Java | 42 | 15 | 9 | 66 | +| [src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java](/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java) | Java | 28 | 17 | 9 | 54 | +| [src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java](/src/main/java/frc4388/robot/commands/drive/DrivePositionMPAux.java) | Java | 60 | 25 | 9 | 94 | +| [src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java](/src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java) | Java | 29 | 17 | 7 | 53 | +| [src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java](/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java) | Java | 56 | 22 | 9 | 87 | +| [src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java](/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java) | Java | 49 | 22 | 9 | 80 | +| [src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java](/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java) | Java | 54 | 43 | 14 | 111 | +| [src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java](/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickAuxPID.java) | Java | 48 | 17 | 13 | 78 | +| [src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java](/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickDriveStraight.java) | Java | 93 | 31 | 19 | 143 | +| [src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java](/src/main/java/frc4388/robot/commands/drive/DriveWithJoystickUsingDeadAssistPID.java) | Java | 132 | 41 | 22 | 195 | +| [src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java](/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesFieldRelative.java) | Java | 10 | 14 | 5 | 29 | +| [src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java](/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java) | Java | 54 | 14 | 20 | 88 | +| [src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java](/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java) | Java | 28 | 16 | 9 | 53 | +| [src/main/java/frc4388/robot/commands/drive/SkipSong.java](/src/main/java/frc4388/robot/commands/drive/SkipSong.java) | Java | 32 | 14 | 11 | 57 | +| [src/main/java/frc4388/robot/commands/drive/TurnDegrees.java](/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java) | Java | 41 | 16 | 17 | 74 | +| [src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java](/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java) | Java | 46 | 16 | 16 | 78 | +| [src/main/java/frc4388/robot/commands/intake/RunIntake.java](/src/main/java/frc4388/robot/commands/intake/RunIntake.java) | Java | 25 | 14 | 8 | 47 | +| [src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java](/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java) | Java | 36 | 19 | 9 | 64 | +| [src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java](/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java) | Java | 44 | 15 | 11 | 70 | +| [src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java](/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java) | Java | 33 | 18 | 9 | 60 | +| [src/main/java/frc4388/robot/commands/shooter/PrepChecker.java](/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java) | Java | 28 | 14 | 11 | 53 | +| [src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java](/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java) | Java | 29 | 17 | 9 | 55 | +| [src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java](/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java) | Java | 16 | 15 | 4 | 35 | +| [src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java](/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java) | Java | 32 | 13 | 8 | 53 | +| [src/main/java/frc4388/robot/commands/shooter/ShooterManual.java](/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java) | Java | 25 | 13 | 8 | 46 | +| [src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java](/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java) | Java | 32 | 13 | 8 | 53 | +| [src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java](/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java) | Java | 37 | 15 | 10 | 62 | +| [src/main/java/frc4388/robot/commands/shooter/TrackTarget.java](/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java) | Java | 84 | 30 | 22 | 136 | +| [src/main/java/frc4388/robot/commands/shooter/TrimShooter.java](/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java) | Java | 41 | 14 | 11 | 66 | +| [src/main/java/frc4388/robot/commands/storage/ManageStorage.java](/src/main/java/frc4388/robot/commands/storage/ManageStorage.java) | Java | 90 | 35 | 23 | 148 | +| [src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java](/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java) | Java | 86 | 33 | 24 | 143 | +| [src/main/java/frc4388/robot/commands/storage/RunStorage.java](/src/main/java/frc4388/robot/commands/storage/RunStorage.java) | Java | 25 | 14 | 8 | 47 | +| [src/main/java/frc4388/robot/commands/storage/StoragePrep.java](/src/main/java/frc4388/robot/commands/storage/StoragePrep.java) | Java | 33 | 14 | 8 | 55 | +| [src/main/java/frc4388/robot/subsystems/Camera.java](/src/main/java/frc4388/robot/subsystems/Camera.java) | Java | 24 | 15 | 6 | 45 | +| [src/main/java/frc4388/robot/subsystems/Climber.java](/src/main/java/frc4388/robot/subsystems/Climber.java) | Java | 58 | 20 | 16 | 94 | +| [src/main/java/frc4388/robot/subsystems/Drive.java](/src/main/java/frc4388/robot/subsystems/Drive.java) | Java | 506 | 275 | 148 | 929 | +| [src/main/java/frc4388/robot/subsystems/Intake.java](/src/main/java/frc4388/robot/subsystems/Intake.java) | Java | 36 | 36 | 14 | 86 | +| [src/main/java/frc4388/robot/subsystems/LED.java](/src/main/java/frc4388/robot/subsystems/LED.java) | Java | 44 | 32 | 12 | 88 | +| [src/main/java/frc4388/robot/subsystems/Leveler.java](/src/main/java/frc4388/robot/subsystems/Leveler.java) | Java | 29 | 18 | 11 | 58 | +| [src/main/java/frc4388/robot/subsystems/LimeLight.java](/src/main/java/frc4388/robot/subsystems/LimeLight.java) | Java | 18 | 10 | 8 | 36 | +| [src/main/java/frc4388/robot/subsystems/Pneumatics.java](/src/main/java/frc4388/robot/subsystems/Pneumatics.java) | Java | 50 | 27 | 14 | 91 | +| [src/main/java/frc4388/robot/subsystems/Shooter.java](/src/main/java/frc4388/robot/subsystems/Shooter.java) | Java | 73 | 38 | 32 | 143 | +| [src/main/java/frc4388/robot/subsystems/ShooterAim.java](/src/main/java/frc4388/robot/subsystems/ShooterAim.java) | Java | 108 | 31 | 39 | 178 | +| [src/main/java/frc4388/robot/subsystems/ShooterHood.java](/src/main/java/frc4388/robot/subsystems/ShooterHood.java) | Java | 65 | 20 | 23 | 108 | +| [src/main/java/frc4388/robot/subsystems/Storage.java](/src/main/java/frc4388/robot/subsystems/Storage.java) | Java | 84 | 33 | 28 | 145 | +| [src/main/java/frc4388/utility/Gains.java](/src/main/java/frc4388/utility/Gains.java) | Java | 33 | 28 | 5 | 66 | +| [src/main/java/frc4388/utility/LEDPatterns.java](/src/main/java/frc4388/utility/LEDPatterns.java) | Java | 30 | 10 | 6 | 46 | +| [src/main/java/frc4388/utility/ShooterTables.java](/src/main/java/frc4388/utility/ShooterTables.java) | Java | 125 | 15 | 31 | 171 | +| [src/main/java/frc4388/utility/Trims.java](/src/main/java/frc4388/utility/Trims.java) | Java | 9 | 6 | 3 | 18 | +| [src/main/java/frc4388/utility/controller/ButtonFox.java](/src/main/java/frc4388/utility/controller/ButtonFox.java) | Java | 8 | 10 | 3 | 21 | +| [src/main/java/frc4388/utility/controller/IHandController.java](/src/main/java/frc4388/utility/controller/IHandController.java) | Java | 10 | 3 | 9 | 22 | +| [src/main/java/frc4388/utility/controller/JoystickManualButton.java](/src/main/java/frc4388/utility/controller/JoystickManualButton.java) | Java | 24 | 22 | 8 | 54 | +| [src/main/java/frc4388/utility/controller/XboxController.java](/src/main/java/frc4388/utility/controller/XboxController.java) | Java | 151 | 26 | 46 | 223 | +| [src/main/java/frc4388/utility/controller/XboxTriggerButton.java](/src/main/java/frc4388/utility/controller/XboxTriggerButton.java) | Java | 55 | 8 | 6 | 69 | + +[summary](results.md) \ No newline at end of file diff --git a/.VSCodeCounter/results.csv b/.VSCodeCounter/results.csv new file mode 100644 index 0000000..bcfdb2f --- /dev/null +++ b/.VSCodeCounter/results.csv @@ -0,0 +1,85 @@ +filename, language, JSON, Java, CSS, comment, blank, total +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\DriveOffLineBackward.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\DriveOffLineForward.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid0.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid1.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid2.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMidComplete.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\FiveBallMidComplete.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMid0.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMid1.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMidComplete.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\TenBallMidComplete.wpilib.json, JSON, 1, 0, 0, 0, 0, 1 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\driverStation\GOAT DRIVERSTATION.json, JSON, 1014, 0, 0, 0, 0, 1014 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\driverStation\themes\Ridgbotics\ridgeboticstheme.css, CSS, 0, 0, 8, 0, 1, 9 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Constants.java, Java, 0, 157, 0, 32, 41, 230 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Main.java, Java, 0, 9, 0, 16, 5, 30 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Robot.java, Java, 0, 59, 0, 63, 24, 146 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\RobotContainer.java, Java, 0, 312, 0, 145, 96, 553 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\InterruptSubystem.java, Java, 0, 21, 0, 14, 8, 43 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\AutoPath1FromCenter.java, Java, 0, 22, 0, 19, 6, 47 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\AutoPath2FromRight.java, Java, 0, 26, 0, 23, 6, 55 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\DriveOffLineBackward.java, Java, 0, 13, 0, 14, 5, 32 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\DriveOffLineForward.java, Java, 0, 11, 0, 14, 5, 30 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\EightBallAutoMiddle.java, Java, 0, 11, 0, 14, 5, 30 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\FiveBallAutoMiddle.java, Java, 0, 12, 0, 13, 4, 29 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\SixBallAutoMiddle.java, Java, 0, 16, 0, 14, 6, 36 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\TankDriveVelocity.java, Java, 0, 42, 0, 14, 12, 68 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\TenBallAutoMiddle.java, Java, 0, 40, 0, 16, 4, 60 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\Wait.java, Java, 0, 40, 0, 16, 16, 72 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\DisengageRachet.java, Java, 0, 26, 0, 14, 9, 49 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\RunClimberWithTriggers.java, Java, 0, 42, 0, 15, 9, 66 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\RunLevelerWithJoystick.java, Java, 0, 28, 0, 17, 9, 54 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DrivePositionMPAux.java, Java, 0, 60, 0, 25, 9, 94 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightAtVelocityPID.java, Java, 0, 29, 0, 17, 7, 53 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightToPositionMM.java, Java, 0, 56, 0, 22, 9, 87 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightToPositionPID.java, Java, 0, 49, 0, 22, 9, 80 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystick.java, Java, 0, 54, 0, 43, 14, 111 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickAuxPID.java, Java, 0, 48, 0, 17, 13, 78 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickDriveStraight.java, Java, 0, 93, 0, 31, 19, 143 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickUsingDeadAssistPID.java, Java, 0, 132, 0, 41, 22, 195 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\GotoCoordinatesFieldRelative.java, Java, 0, 10, 0, 14, 5, 29 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\GotoCoordinatesRobotRelative.java, Java, 0, 54, 0, 14, 20, 88 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\PlaySongDrive.java, Java, 0, 28, 0, 16, 9, 53 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\SkipSong.java, Java, 0, 32, 0, 14, 11, 57 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\TurnDegrees.java, Java, 0, 41, 0, 16, 17, 74 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunExtenderOutIn.java, Java, 0, 46, 0, 16, 16, 78 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunIntake.java, Java, 0, 25, 0, 14, 8, 47 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunIntakeWithTriggers.java, Java, 0, 36, 0, 19, 9, 64 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\CalibrateShooter.java, Java, 0, 44, 0, 15, 11, 70 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\HoodPositionPID.java, Java, 0, 33, 0, 18, 9, 60 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\PrepChecker.java, Java, 0, 28, 0, 14, 11, 53 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\RunHoodWithJoystick.java, Java, 0, 29, 0, 17, 9, 55 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShootPrepGroup.java, Java, 0, 16, 0, 15, 4, 35 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterGoalPosition.java, Java, 0, 32, 0, 13, 8, 53 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterManual.java, Java, 0, 25, 0, 13, 8, 46 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterTrenchPosition.java, Java, 0, 32, 0, 13, 8, 53 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterVelocityControlPID.java, Java, 0, 37, 0, 15, 10, 62 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\TrackTarget.java, Java, 0, 84, 0, 30, 22, 136 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\TrimShooter.java, Java, 0, 41, 0, 14, 11, 66 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\ManageStorage.java, Java, 0, 90, 0, 35, 23, 148 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\ManageStoragePID.java, Java, 0, 86, 0, 33, 24, 143 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\RunStorage.java, Java, 0, 25, 0, 14, 8, 47 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\StoragePrep.java, Java, 0, 33, 0, 14, 8, 55 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Camera.java, Java, 0, 24, 0, 15, 6, 45 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Climber.java, Java, 0, 58, 0, 20, 16, 94 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Drive.java, Java, 0, 506, 0, 275, 148, 929 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Intake.java, Java, 0, 36, 0, 36, 14, 86 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\LED.java, Java, 0, 44, 0, 32, 12, 88 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Leveler.java, Java, 0, 29, 0, 18, 11, 58 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\LimeLight.java, Java, 0, 18, 0, 10, 8, 36 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Pneumatics.java, Java, 0, 50, 0, 27, 14, 91 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Shooter.java, Java, 0, 73, 0, 38, 32, 143 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\ShooterAim.java, Java, 0, 108, 0, 31, 39, 178 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\ShooterHood.java, Java, 0, 65, 0, 20, 23, 108 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Storage.java, Java, 0, 84, 0, 33, 28, 145 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\Gains.java, Java, 0, 33, 0, 28, 5, 66 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\LEDPatterns.java, Java, 0, 30, 0, 10, 6, 46 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\ShooterTables.java, Java, 0, 125, 0, 15, 31, 171 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\Trims.java, Java, 0, 9, 0, 6, 3, 18 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\ButtonFox.java, Java, 0, 8, 0, 10, 3, 21 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\IHandController.java, Java, 0, 10, 0, 3, 9, 22 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\JoystickManualButton.java, Java, 0, 24, 0, 22, 8, 54 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\XboxController.java, Java, 0, 151, 0, 26, 46, 223 +c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\XboxTriggerButton.java, Java, 0, 55, 0, 8, 6, 69 +Total, -, 1025, 3855, 8, 1770, 1110, 7768 \ No newline at end of file diff --git a/.VSCodeCounter/results.md b/.VSCodeCounter/results.md new file mode 100644 index 0000000..47eec3f --- /dev/null +++ b/.VSCodeCounter/results.md @@ -0,0 +1,41 @@ +# Summary + +Date : 2020-03-13 19:46:18 + +Directory c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main + +Total : 83 files, 4888 codes, 1770 comments, 1110 blanks, all 7768 lines + +[details](details.md) + +## Languages +| language | files | code | comment | blank | total | +| :--- | ---: | ---: | ---: | ---: | ---: | +| Java | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| JSON | 12 | 1,025 | 0 | 0 | 1,025 | +| CSS | 1 | 8 | 0 | 1 | 9 | + +## Directories +| path | files | code | comment | blank | total | +| :--- | ---: | ---: | ---: | ---: | ---: | +| . | 83 | 4,888 | 1,770 | 1,110 | 7,768 | +| deploy | 11 | 11 | 0 | 0 | 11 | +| deploy\paths | 11 | 11 | 0 | 0 | 11 | +| driverStation | 2 | 1,022 | 0 | 1 | 1,023 | +| driverStation\themes | 1 | 8 | 0 | 1 | 9 | +| driverStation\themes\Ridgbotics | 1 | 8 | 0 | 1 | 9 | +| java | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| java\frc4388 | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| java\frc4388\robot | 61 | 3,410 | 1,642 | 992 | 6,044 | +| java\frc4388\robot\commands | 45 | 1,778 | 831 | 475 | 3,084 | +| java\frc4388\robot\commands\auto | 10 | 233 | 157 | 69 | 459 | +| java\frc4388\robot\commands\climber | 3 | 96 | 46 | 27 | 169 | +| java\frc4388\robot\commands\drive | 13 | 686 | 292 | 164 | 1,142 | +| java\frc4388\robot\commands\intake | 3 | 107 | 49 | 33 | 189 | +| java\frc4388\robot\commands\shooter | 11 | 401 | 177 | 111 | 689 | +| java\frc4388\robot\commands\storage | 4 | 234 | 96 | 63 | 393 | +| java\frc4388\robot\subsystems | 12 | 1,095 | 555 | 351 | 2,001 | +| java\frc4388\utility | 9 | 445 | 128 | 117 | 690 | +| java\frc4388\utility\controller | 5 | 248 | 69 | 72 | 389 | + +[details](details.md) \ No newline at end of file diff --git a/.VSCodeCounter/results.txt b/.VSCodeCounter/results.txt new file mode 100644 index 0000000..78f07a1 --- /dev/null +++ b/.VSCodeCounter/results.txt @@ -0,0 +1,127 @@ +Date : 2020-03-13 19:46:18 +Directory : c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main +Total : 83 files, 4888 codes, 1770 comments, 1110 blanks, all 7768 lines + +Languages ++----------+------------+------------+------------+------------+------------+ +| language | files | code | comment | blank | total | ++----------+------------+------------+------------+------------+------------+ +| Java | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| JSON | 12 | 1,025 | 0 | 0 | 1,025 | +| CSS | 1 | 8 | 0 | 1 | 9 | ++----------+------------+------------+------------+------------+------------+ + +Directories ++-------------------------------------------------------------------------------------------------------------------------------------------------+------------+------------+------------+------------+------------+ +| path | files | code | comment | blank | total | ++-------------------------------------------------------------------------------------------------------------------------------------------------+------------+------------+------------+------------+------------+ +| . | 83 | 4,888 | 1,770 | 1,110 | 7,768 | +| deploy | 11 | 11 | 0 | 0 | 11 | +| deploy\paths | 11 | 11 | 0 | 0 | 11 | +| driverStation | 2 | 1,022 | 0 | 1 | 1,023 | +| driverStation\themes | 1 | 8 | 0 | 1 | 9 | +| driverStation\themes\Ridgbotics | 1 | 8 | 0 | 1 | 9 | +| java | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| java\frc4388 | 70 | 3,855 | 1,770 | 1,109 | 6,734 | +| java\frc4388\robot | 61 | 3,410 | 1,642 | 992 | 6,044 | +| java\frc4388\robot\commands | 45 | 1,778 | 831 | 475 | 3,084 | +| java\frc4388\robot\commands\auto | 10 | 233 | 157 | 69 | 459 | +| java\frc4388\robot\commands\climber | 3 | 96 | 46 | 27 | 169 | +| java\frc4388\robot\commands\drive | 13 | 686 | 292 | 164 | 1,142 | +| java\frc4388\robot\commands\intake | 3 | 107 | 49 | 33 | 189 | +| java\frc4388\robot\commands\shooter | 11 | 401 | 177 | 111 | 689 | +| java\frc4388\robot\commands\storage | 4 | 234 | 96 | 63 | 393 | +| java\frc4388\robot\subsystems | 12 | 1,095 | 555 | 351 | 2,001 | +| java\frc4388\utility | 9 | 445 | 128 | 117 | 690 | +| java\frc4388\utility\controller | 5 | 248 | 69 | 72 | 389 | ++-------------------------------------------------------------------------------------------------------------------------------------------------+------------+------------+------------+------------+------------+ + +Files ++-------------------------------------------------------------------------------------------------------------------------------------------------+----------+------------+------------+------------+------------+ +| filename | language | code | comment | blank | total | ++-------------------------------------------------------------------------------------------------------------------------------------------------+----------+------------+------------+------------+------------+ +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\DriveOffLineBackward.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\DriveOffLineForward.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid0.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid1.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMid2.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\EightBallMidComplete.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\FiveBallMidComplete.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMid0.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMid1.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\SixBallMidComplete.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\deploy\paths\TenBallMidComplete.wpilib.json | JSON | 1 | 0 | 0 | 1 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\driverStation\GOAT DRIVERSTATION.json | JSON | 1,014 | 0 | 0 | 1,014 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\driverStation\themes\Ridgbotics\ridgeboticstheme.css | CSS | 8 | 0 | 1 | 9 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Constants.java | Java | 157 | 32 | 41 | 230 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Main.java | Java | 9 | 16 | 5 | 30 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\Robot.java | Java | 59 | 63 | 24 | 146 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\RobotContainer.java | Java | 312 | 145 | 96 | 553 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\InterruptSubystem.java | Java | 21 | 14 | 8 | 43 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\AutoPath1FromCenter.java | Java | 22 | 19 | 6 | 47 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\AutoPath2FromRight.java | Java | 26 | 23 | 6 | 55 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\DriveOffLineBackward.java | Java | 13 | 14 | 5 | 32 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\DriveOffLineForward.java | Java | 11 | 14 | 5 | 30 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\EightBallAutoMiddle.java | Java | 11 | 14 | 5 | 30 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\FiveBallAutoMiddle.java | Java | 12 | 13 | 4 | 29 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\SixBallAutoMiddle.java | Java | 16 | 14 | 6 | 36 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\TankDriveVelocity.java | Java | 42 | 14 | 12 | 68 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\TenBallAutoMiddle.java | Java | 40 | 16 | 4 | 60 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\auto\Wait.java | Java | 40 | 16 | 16 | 72 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\DisengageRachet.java | Java | 26 | 14 | 9 | 49 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\RunClimberWithTriggers.java | Java | 42 | 15 | 9 | 66 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\climber\RunLevelerWithJoystick.java | Java | 28 | 17 | 9 | 54 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DrivePositionMPAux.java | Java | 60 | 25 | 9 | 94 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightAtVelocityPID.java | Java | 29 | 17 | 7 | 53 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightToPositionMM.java | Java | 56 | 22 | 9 | 87 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveStraightToPositionPID.java | Java | 49 | 22 | 9 | 80 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystick.java | Java | 54 | 43 | 14 | 111 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickAuxPID.java | Java | 48 | 17 | 13 | 78 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickDriveStraight.java | Java | 93 | 31 | 19 | 143 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\DriveWithJoystickUsingDeadAssistPID.java | Java | 132 | 41 | 22 | 195 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\GotoCoordinatesFieldRelative.java | Java | 10 | 14 | 5 | 29 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\GotoCoordinatesRobotRelative.java | Java | 54 | 14 | 20 | 88 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\PlaySongDrive.java | Java | 28 | 16 | 9 | 53 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\SkipSong.java | Java | 32 | 14 | 11 | 57 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\drive\TurnDegrees.java | Java | 41 | 16 | 17 | 74 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunExtenderOutIn.java | Java | 46 | 16 | 16 | 78 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunIntake.java | Java | 25 | 14 | 8 | 47 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\intake\RunIntakeWithTriggers.java | Java | 36 | 19 | 9 | 64 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\CalibrateShooter.java | Java | 44 | 15 | 11 | 70 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\HoodPositionPID.java | Java | 33 | 18 | 9 | 60 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\PrepChecker.java | Java | 28 | 14 | 11 | 53 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\RunHoodWithJoystick.java | Java | 29 | 17 | 9 | 55 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShootPrepGroup.java | Java | 16 | 15 | 4 | 35 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterGoalPosition.java | Java | 32 | 13 | 8 | 53 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterManual.java | Java | 25 | 13 | 8 | 46 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterTrenchPosition.java | Java | 32 | 13 | 8 | 53 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\ShooterVelocityControlPID.java | Java | 37 | 15 | 10 | 62 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\TrackTarget.java | Java | 84 | 30 | 22 | 136 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\shooter\TrimShooter.java | Java | 41 | 14 | 11 | 66 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\ManageStorage.java | Java | 90 | 35 | 23 | 148 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\ManageStoragePID.java | Java | 86 | 33 | 24 | 143 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\RunStorage.java | Java | 25 | 14 | 8 | 47 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\commands\storage\StoragePrep.java | Java | 33 | 14 | 8 | 55 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Camera.java | Java | 24 | 15 | 6 | 45 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Climber.java | Java | 58 | 20 | 16 | 94 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Drive.java | Java | 506 | 275 | 148 | 929 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Intake.java | Java | 36 | 36 | 14 | 86 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\LED.java | Java | 44 | 32 | 12 | 88 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Leveler.java | Java | 29 | 18 | 11 | 58 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\LimeLight.java | Java | 18 | 10 | 8 | 36 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Pneumatics.java | Java | 50 | 27 | 14 | 91 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Shooter.java | Java | 73 | 38 | 32 | 143 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\ShooterAim.java | Java | 108 | 31 | 39 | 178 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\ShooterHood.java | Java | 65 | 20 | 23 | 108 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\robot\subsystems\Storage.java | Java | 84 | 33 | 28 | 145 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\Gains.java | Java | 33 | 28 | 5 | 66 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\LEDPatterns.java | Java | 30 | 10 | 6 | 46 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\ShooterTables.java | Java | 125 | 15 | 31 | 171 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\Trims.java | Java | 9 | 6 | 3 | 18 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\ButtonFox.java | Java | 8 | 10 | 3 | 21 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\IHandController.java | Java | 10 | 3 | 9 | 22 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\JoystickManualButton.java | Java | 24 | 22 | 8 | 54 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\XboxController.java | Java | 151 | 26 | 46 | 223 | +| c:\Users\Ridgebotics\Documents\GitHub\RiseOfRidgebotics2020\src\main\java\frc4388\utility\controller\XboxTriggerButton.java | Java | 55 | 8 | 6 | 69 | +| Total | | 4,888 | 1,770 | 1,110 | 7,768 | ++-------------------------------------------------------------------------------------------------------------------------------------------------+----------+------------+------------+------------+------------+ \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b2d88f1..940694c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -49,6 +49,7 @@ import frc4388.robot.commands.climber.RunLevelerWithJoystick; import frc4388.robot.commands.drive.DriveStraightToPositionMM; import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.PlaySongDrive; +import frc4388.robot.commands.drive.SkipSong; import frc4388.robot.commands.drive.TurnDegrees; import frc4388.robot.commands.intake.RunIntakeWithTriggers; import frc4388.robot.commands.shooter.CalibrateShooter; @@ -136,7 +137,7 @@ public class RobotContainer { public RobotContainer() { /* Passing Drive and Pneumatics Subsystems */ m_robotPneumatics.passRequiredSubsystem(m_robotDrive); - m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); m_robotShooterHood.passRequiredSubsystem(m_robotShooter); @@ -184,8 +185,8 @@ public class RobotContainer { private void configureButtonBindings() { /* Test Buttons */ // A driver test button - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive)); + /*new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotDrive.tankDriveVelocity(1, -1), m_robotDrive));*/ // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -289,7 +290,7 @@ public class RobotContainer { // Meg new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH) - .whenPressed(new PlaySongDrive(m_robotDrive)) + .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) .whenReleased(new InterruptSubystem(m_robotDrive)); // Shooter Manual @@ -298,22 +299,21 @@ public class RobotContainer { .whenReleased(new ShooterManual(false)); // Goal Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) - .whileHeld(new ShooterGoalPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) - .whenReleased(new InterruptSubystem(m_robotShooter)) - .whenReleased(new InterruptSubystem(m_robotShooterHood)) - .whenReleased(new InterruptSubystem(m_robotShooterAim)); + new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) + .whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)) + .whenReleased(new InterruptSubystem(m_robotDrive)); // Trench Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON) + new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON) .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)) .whenReleased(new InterruptSubystem(m_robotShooter)) .whenReleased(new InterruptSubystem(m_robotShooterHood)) .whenReleased(new InterruptSubystem(m_robotShooterAim)); + //.whenPressed(new SkipSong(m_robotDrive, 1)); } public void buildAutos() { - resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); + //resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); String[] sixBallAutoMiddlePaths = new String[]{ "SixBallMidComplete" diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java index 75ab279..b2aba9c 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java @@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase { } */ - m_drive.driveWithInput(moveOutput, steerOutput); + m_drive.driveWithInput(-moveOutput, steerOutput); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java index eaa0f07..10a99ee 100644 --- a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java +++ b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java @@ -9,6 +9,7 @@ package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Shooter; public class PlaySongDrive extends CommandBase { private Drive m_drive; @@ -16,10 +17,10 @@ public class PlaySongDrive extends CommandBase { /** * Creates a new PlaySongDrive. */ - public PlaySongDrive(Drive subsystem) { + public PlaySongDrive(Drive subsystem, Shooter shooter) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - addRequirements(m_drive); + addRequirements(m_drive, shooter); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/drive/SkipSong.java b/src/main/java/frc4388/robot/commands/drive/SkipSong.java new file mode 100644 index 0000000..0d020a1 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/drive/SkipSong.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands.drive; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class SkipSong extends CommandBase { + Drive m_drive; + int m_index; + + /** + * Creates a new SkipSong. + */ + public SkipSong(Drive m_robotDrive, int index) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = m_robotDrive; + m_index = index; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + String[] songs = m_drive.songsStrings; + String song = m_drive.m_currentSong; + + for (int i = 0; i < songs.length; i++) { + if (songs[i] == song) { + m_drive.selectSong(songs[i + m_index]); + break; + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index f833621..5860235 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -54,6 +54,7 @@ public class Drive extends SubsystemBase { /* Pneumatics Subsystem */ public Pneumatics m_pneumaticsSubsystem; + Shooter m_shooter; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -96,7 +97,8 @@ public class Drive extends SubsystemBase { SendableChooser m_songChooser = new SendableChooser(); /* Misc */ - String m_currentSong = ""; + public String m_currentSong = ""; + public String[] songsStrings; /** * Add your docs here. @@ -283,11 +285,12 @@ public class Drive extends SubsystemBase { /* Create chooser to choose song to play */ File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs"); System.err.println(songsDir.getPath()); - String[] songsStrings = songsDir.list(); + songsStrings = songsDir.list(); for (String songString : songsStrings) { m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString); } Shuffleboard.getTab("Songs").add(m_songChooser); + selectSong(songsStrings[0]); /* Start counting time */ m_lastTimeMs = System.currentTimeMillis(); @@ -306,8 +309,10 @@ public class Drive extends SubsystemBase { * * @param subsystem Subsystem needed. */ - public void passRequiredSubsystem(Pneumatics subsystem) { + public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) { m_pneumaticsSubsystem = subsystem; + m_shooter = shooter; + m_orchestra.addInstrument(m_shooter.m_shooterFalcon); } public void updateTime() { @@ -913,6 +918,7 @@ public class Drive extends SubsystemBase { if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected(); selectSong(m_currentSong); + //System.err.println(m_currentSong); } } catch (Exception e) { @@ -920,4 +926,6 @@ public class Drive extends SubsystemBase { // e.printStackTrace(System.err); } } + + } diff --git a/src/main/java/frc4388/utility/controller/JoystickManualButton.java b/src/main/java/frc4388/utility/controller/JoystickManualButton.java index 6455639..53db6f4 100644 --- a/src/main/java/frc4388/utility/controller/JoystickManualButton.java +++ b/src/main/java/frc4388/utility/controller/JoystickManualButton.java @@ -34,6 +34,7 @@ public class JoystickManualButton extends Button { m_joystick = joystick; m_buttonNumber = buttonNumber; + m_buttonType = buttonType; } /**