From cbdf73f90978d07c3de30c6d1275c15439df2427 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 26 Mar 2025 20:26:20 -0700 Subject: [PATCH 001/126] first pass at coral ground state --- src/main/java/frc/robot/Robot.java | 3 ++- .../subsystems/ManipulatorSubsystem.java | 11 ++++++++ .../frc/robot/subsystems/Superstructure.java | 27 +++++++++++++++++++ .../subsystems/climber/ClimberIOSim.java | 3 +-- .../elevator/ElevatorSubsystem.java | 1 + .../robot/subsystems/roller/RollerIOSim.java | 14 ++++++---- .../shoulder/ShoulderSubsystem.java | 1 + .../subsystems/wrist/WristSubsystem.java | 1 + 8 files changed, 53 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c40b48ce..93d4d1c3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -119,7 +119,7 @@ private RobotHardware(SwerveConstants swerveConstants) { } } - public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.REPLAY; + public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; @@ -448,6 +448,7 @@ public static enum AlgaeScoreTarget { , driver.rightTrigger().or(() -> Autos.autoPreScore), driver.leftTrigger(), + driver.leftBumper(), driver .x() .and(driver.pov(-1).negate()) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 5103ceca..43ccd92e 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -24,6 +24,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; + public static final double CORAL_INTAKE_VOLTAGE = -10.0; public static final double ALGAE_INTAKE_VOLTAGE = -10.0; public static final double ALGAE_HOLDING_VOLTAGE = -3.0; public static final double ALGAE_CURRENT_THRESHOLD = 30.0; @@ -75,6 +76,12 @@ public void periodic() { Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); zeroTimer.reset(); } + + if (!firstBBInputs.get && secondBBInputs.get) { + // Number calculated from coral length, may need tuning + Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.63)); + zeroTimer.reset(); + } } public Command index() { @@ -113,6 +120,10 @@ public Command backIndex() { setVelocity(-INDEXING_VELOCITY).until(() -> !secondBBInputs.get), index()); } + public Command intakeCoral() { + return setVoltage(CORAL_INTAKE_VOLTAGE).until(() -> secondBBInputs.get).andThen(hold()); + } + public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) .until(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index b4ab4447..900a2edd 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -35,6 +35,7 @@ public class Superstructure { public static enum SuperState { IDLE, HOME, + INTAKE_CORAL_GROUND, READY_CORAL, SPIT_CORAL, PRE_L1, @@ -73,6 +74,9 @@ public static enum SuperState { @AutoLogOutput(key = "Superstructure/Algae Intake Request") private final Trigger intakeAlgaeReq; + @AutoLogOutput(key = "Superstructure/Coral Intake Request") + private final Trigger intakeCoralReq; + @AutoLogOutput(key = "Superstructure/Pre Climb Request") private final Trigger preClimbReq; @@ -129,6 +133,7 @@ public Superstructure( Trigger scoreReq, Trigger preScoreReq, Trigger intakeAlgaeReq, + Trigger intakeCoralReq, Trigger climbReq, Trigger climbConfReq, Trigger climbCancelReq, @@ -155,6 +160,7 @@ public Superstructure( this.scoreReq = scoreReq; this.intakeAlgaeReq = intakeAlgaeReq; + this.intakeCoralReq = intakeCoralReq; this.preClimbReq = climbReq; this.climbConfReq = climbConfReq; @@ -221,6 +227,11 @@ private void configureStateTransitionCommands() { .and(manipulator::getFirstBeambreak) .onTrue(this.forceState(SuperState.READY_CORAL)); + stateTriggers + .get(SuperState.IDLE) + .and(intakeCoralReq) + .onTrue(this.forceState(SuperState.INTAKE_CORAL_GROUND)); + // IDLE -> INTAKE_ALGAE_{location} stateTriggers .get(SuperState.IDLE) @@ -278,6 +289,22 @@ private void configureStateTransitionCommands() { .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) .onTrue(this.forceState(prevState)); + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.GROUND_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, + WristSubsystem.WRIST_CORAL_GROUND)) + .whileTrue(manipulator.intakeCoral().repeatedly()) + .and(manipulator::getSecondBeambreak) + .onTrue(this.forceState(SuperState.READY_CORAL)); + + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) + .and(intakeCoralReq.negate()) + .onTrue(this.forceState(SuperState.IDLE)); + // READY_CORAL logic stateTriggers .get(SuperState.READY_CORAL) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 3f2ffaf4..f4b56ccb 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -68,7 +68,6 @@ public void resetEncoder(double position) { @Override public void setPosition(double position, double vel) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + // We dont simulate the climber really so im gonna ignore this } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 8a9e5a9c..e2787bc8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -55,6 +55,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); + public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); public static final double MAX_ACCELERATION = 10.0; public static final double SLOW_ACCELERATION = 5.0; diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java index 3fe028c8..3a37e16b 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java @@ -66,18 +66,22 @@ public void registerSimulationCallback(Consumer callba @Override public void setPosition(Rotation2d rot) { - // TODO Actually simulate + setVelocity(0.0); + // i cba to impl this properly rn + resetEncoder(rot.getRotations()); } @Override public void resetEncoder(double position) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'resetEncoder'"); + motorSim.setAngle(position); } @Override public void setCurrent(double amps) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setCurrent'"); + setVoltage( + DCMotor.getKrakenX60Foc(1) + .getVoltage( + DCMotor.getKrakenX60Foc(1).getTorque(amps), + motorSim.getAngularVelocityRadPerSec())); } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 270e6e47..da4bddee 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,6 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(90.0); + public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index ef5da9ae..56d62e5a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -22,6 +22,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(-30.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(-23.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = Rotation2d.fromDegrees(-70); From ea03c9739033b4bb183a90faa64cbb862057306f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 27 Mar 2025 22:20:46 -0700 Subject: [PATCH 002/126] adjust mech viz to use new ee --- src/main/java/frc/robot/Robot.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93d4d1c3..cf43509d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1067,12 +1067,15 @@ public void robotPeriodic() { 0, -Units.degreesToRadians(2.794042) - shoulder.getAngle().getRadians(), 0.0)), new Pose3d( // Manipulator new Translation3d( - ShoulderSubsystem.X_OFFSET_METERS - + shoulder.getAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - 0, - elevator.getExtensionMeters() - + ShoulderSubsystem.Z_OFFSET_METERS - + shoulder.getAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS), + ShoulderSubsystem.X_OFFSET_METERS + + shoulder.getAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + 0, + elevator.getExtensionMeters() + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulder.getAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation3d(Units.inchesToMeters(1.0), 0.0, Units.inchesToMeters(-8.0)) + .rotateBy(new Rotation3d(0.0, -wrist.getAngle().getRadians(), 0.0))), new Rotation3d(0, wrist.getAngle().getRadians(), Math.PI)) }); From 2e6685633cccc7c1b19f6bd4e4c32e2f218866f0 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 27 Mar 2025 22:49:35 -0700 Subject: [PATCH 003/126] adjust hp intake pose --- src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index 12086ad4..49be3de9 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -28,7 +28,7 @@ public class ShoulderIOSim implements ShoulderIO { private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 0.0, 0.0); // 1.31085, 0.278); private final ProfiledPIDController pid = - new ProfiledPIDController(100.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); + new ProfiledPIDController(60.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index da4bddee..45a5b551 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -28,7 +28,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(90.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(40.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 56d62e5a..3a1c998a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -21,7 +21,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(180 - 49.519570 + 5); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(-30.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(-23.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); From 83366b3562acfe206442839c2692b26551052fbe Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 00:19:56 -0700 Subject: [PATCH 004/126] add clearance for hp extension --- .../java/frc/robot/subsystems/Superstructure.java | 13 +++++++++---- .../subsystems/shoulder/ShoulderSubsystem.java | 5 +++++ .../frc/robot/subsystems/wrist/WristSubsystem.java | 4 ++-- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 900a2edd..0c620374 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -308,9 +308,11 @@ private void configureStateTransitionCommands() { // READY_CORAL logic stateTriggers .get(SuperState.READY_CORAL) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(elevator.setExtension(ElevatorSubsystem.HP_EXTENSION_METERS)) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_RETRACTED_POS, + WristSubsystem.WRIST_RETRACTED_POS)) .whileTrue(manipulator.index()); // keep indexing to make sure its chilling @@ -770,7 +772,10 @@ private Command extendWithClearance( return Commands.sequence( // Retract shoulder + wrist Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder + .hold() + .until(() -> wrist.getAngle().getDegrees() < 90.0) + .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), elevator.hold()) .until( diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 45a5b551..1ba9d3c4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -120,6 +120,11 @@ public Command setTargetAngleSlow(final Rotation2d target) { return setTargetAngle(() -> target); } + public Command hold() { + return Commands.sequence( + setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); + } + public Rotation2d getAngle() { return inputs.position; } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 3a1c998a..1bb64623 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -16,7 +16,7 @@ public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); // TODO: UPDATE WHEN CAD IS FINISHED - public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(180.0); + public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(180 - 49.519570 + 5); @@ -30,7 +30,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L2_POS = Rotation2d.fromDegrees(-35); public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35); public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-45); - public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(-50.0); + public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-44.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-44.0); From a6d19b91d9fd6fea61f8c98726873c0fde7ff6d5 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 01:00:23 -0700 Subject: [PATCH 005/126] begin trying to use ik to not have to redo setpoints (it brokey rn) --- src/main/java/frc/robot/Robot.java | 9 +++ .../robot/subsystems/ExtensionKinematics.java | 61 ++++++++----------- .../elevator/ElevatorSubsystem.java | 13 ++-- .../shoulder/ShoulderSubsystem.java | 11 +++- .../subsystems/wrist/WristSubsystem.java | 11 ++-- 5 files changed, 59 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cf43509d..35038c20 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -986,6 +986,15 @@ public Robot() { Stream.of(AlgaeIntakeTargets.values()) .map((target) -> AlgaeIntakeTargets.getRobotTargetLocation(target.location)) .toArray(Pose2d[]::new)); + + Logger.recordOutput("IK/L1 FK Pose", ExtensionKinematics.L1_POSE); + System.out.println("ExtensionKinematics.L1_POSE: " + ExtensionKinematics.L1_POSE); + Logger.recordOutput("IK/L2 FK Pose", ExtensionKinematics.L2_POSE); + System.out.println("ExtensionKinematics.L2_POSE: " + ExtensionKinematics.L2_POSE); + Logger.recordOutput("IK/L3 FK Pose", ExtensionKinematics.L3_POSE); + System.out.println("ExtensionKinematics.L3_POSE: " + ExtensionKinematics.L3_POSE); + Logger.recordOutput("IK/L4 FK Pose", ExtensionKinematics.L4_POSE); + System.out.println("ExtensionKinematics.L4_POSE: " + ExtensionKinematics.L4_POSE); } private void addAutos() { diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 51327652..6d3d606b 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -24,31 +24,27 @@ import java.util.function.Supplier; public class ExtensionKinematics { + + // These need to be here bc their main constants arent loaded when we need the constants in this + // class + private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); + private static final Transform2d IK_WRIST_TO_CORAL = + new Transform2d( + Units.inchesToMeters(11.0), Units.inchesToMeters(6.842), Rotation2d.fromDegrees(0.0)); + private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); + // Not super accurate bc of whack - public static final ExtensionState L1_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS); - public static final Pose2d L1_POSE = solveFK(L1_EXTENSION); - public static final ExtensionState L2_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L2_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_POS, - WristSubsystem.WRIST_SCORE_L2_POS); - public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); - public static final ExtensionState L3_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L3_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_POS, - WristSubsystem.WRIST_SCORE_L3_POS); - public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); - public static final ExtensionState L4_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L4_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L4_POS, - WristSubsystem.WRIST_SCORE_L4_POS); - public static final Pose2d L4_POSE = solveFK(L4_EXTENSION); + public static final Pose2d L1_POSE = new Pose2d(); // solveFK(L1_EXTENSION); + public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); + public static final Pose2d L2_POSE = + new Pose2d(new Translation2d(0.24, 0.78), new Rotation2d(-0.61)); + public static final ExtensionState L2_EXTENSION = solveIK(L2_POSE); + public static final Pose2d L3_POSE = + new Pose2d(new Translation2d(0.24, 1.14), new Rotation2d(-0.61)); + public static final ExtensionState L3_EXTENSION = solveIK(L3_POSE); + public static final Pose2d L4_POSE = + new Pose2d(new Translation2d(0.27, 1.79), new Rotation2d(-0.79)); + public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = new ExtensionState( @@ -75,9 +71,9 @@ private ExtensionKinematics() {} */ public static ExtensionState solveIK(Pose2d target) { // Offset wrist pose from target - final var wristPose = target.transformBy(ManipulatorSubsystem.IK_WRIST_TO_CORAL.inverse()); + final var wristPose = target.transformBy(IK_WRIST_TO_CORAL.inverse()); // Find shoulder angle from needed horizontal extension - var shoulderAngle = Math.acos(wristPose.getX() / ShoulderSubsystem.ARM_LENGTH_METERS); + var shoulderAngle = Math.acos(wristPose.getX() / ARM_LENGTH_METERS); // Set angle to horizontal if we can't reach if (Double.isNaN(shoulderAngle)) shoulderAngle = 0.0; // Elevator goes to remaining needed height @@ -86,16 +82,13 @@ public static ExtensionState solveIK(Pose2d target) { .getTranslation() .minus( new Translation2d( - ShoulderSubsystem.ARM_LENGTH_METERS * Math.cos(shoulderAngle), - ShoulderSubsystem.ARM_LENGTH_METERS * Math.sin(shoulderAngle))) + ARM_LENGTH_METERS * Math.cos(shoulderAngle), + ARM_LENGTH_METERS * Math.sin(shoulderAngle))) .getY(); // If we're extending higher than we can reach, prioritize matching Z instead of X - if (elevatorHeight > ElevatorSubsystem.MAX_EXTENSION_METERS) { - elevatorHeight = ElevatorSubsystem.MAX_EXTENSION_METERS; - shoulderAngle = - Math.asin( - (target.getY() - ElevatorSubsystem.MAX_EXTENSION_METERS) - / ShoulderSubsystem.ARM_LENGTH_METERS); + if (elevatorHeight > MAX_EXTENSION_METERS) { + elevatorHeight = MAX_EXTENSION_METERS; + shoulderAngle = Math.asin((target.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); // Limit shoulder angle if (Double.isNaN(shoulderAngle) || shoulderAngle > Units.degreesToRadians(85.0)) { shoulderAngle = Units.degreesToRadians(85.0); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index e2787bc8..91fac47b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import java.util.function.DoubleSupplier; import java.util.function.Function; import org.littletonrobotics.junction.Logger; @@ -40,11 +41,15 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - public static final double L1_EXTENSION_METERS = Units.inchesToMeters(12.0); + public static final double L1_EXTENSION_METERS = + ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); - public static final double L2_EXTENSION_METERS = Units.inchesToMeters(17.0); - public static final double L3_EXTENSION_METERS = Units.inchesToMeters(31.5); - public static final double L4_EXTENSION_METERS = Units.inchesToMeters(58.0); + public static final double L2_EXTENSION_METERS = + ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); + public static final double L3_EXTENSION_METERS = + ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); + public static final double L4_EXTENSION_METERS = + ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); public static final double INTAKE_ALGAE_GROUND_EXTENSION = Units.inchesToMeters(5.0); public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(12.5); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 1ba9d3c4..0be1605b 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import frc.robot.utils.Tracer; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -36,10 +37,14 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(34.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(60.0); - public static final Rotation2d SHOULDER_SCORE_POS = Rotation2d.fromDegrees(60); + // may be incorrect as l2-3 poses are derived from ExtensionKinematics now + public static final Rotation2d SHOULDER_SCORE_POS = + ExtensionKinematics.L2_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L4_POS = Rotation2d.fromDegrees(55); + public static final Rotation2d SHOULDER_SCORE_L1_POS = + ExtensionKinematics.L1_EXTENSION.shoulderAngle(); + public static final Rotation2d SHOULDER_SCORE_L4_POS = + ExtensionKinematics.L4_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(40); public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(75.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 1bb64623..df9d9932 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import frc.robot.subsystems.shoulder.ShoulderIOInputsAutoLogged; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -20,16 +21,16 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(180 - 49.519570 + 5); - public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(-30.0); + public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); - public static final Rotation2d WRIST_SCORE_L1_POS = Rotation2d.fromDegrees(-70); + public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - public static final Rotation2d WRIST_SCORE_L2_POS = Rotation2d.fromDegrees(-35); - public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35); - public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-45); + public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); + public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); + public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-44.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = From 28a2db9ee52eadbc2104a698b16df7678fd74ceb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 15:29:48 -0700 Subject: [PATCH 006/126] fix getManipulatorPose (?) --- .../java/frc/robot/subsystems/ExtensionKinematics.java | 7 ++++--- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 5 +---- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 6d3d606b..de58e4fd 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -28,9 +28,9 @@ public class ExtensionKinematics { // These need to be here bc their main constants arent loaded when we need the constants in this // class private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - private static final Transform2d IK_WRIST_TO_CORAL = + static final Transform2d IK_WRIST_TO_CORAL = new Transform2d( - Units.inchesToMeters(11.0), Units.inchesToMeters(6.842), Rotation2d.fromDegrees(0.0)); + Units.inchesToMeters(-11.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); // Not super accurate bc of whack @@ -128,7 +128,8 @@ public static Pose3d getManipulatorPose(Pose2d robotPose, ExtensionState state) 0, 0, fk.getY() + ElevatorSubsystem.Z_OFFSET_METERS, - new Rotation3d(0, -state.wristAngle().getRadians(), 0))); + new Rotation3d(0, -state.wristAngle().getRadians(), 0))) + .transformBy(new Transform3d(-IK_WRIST_TO_CORAL.getX() + 0.1, 0.0, 0.0, Rotation3d.kZero)); } public static Pose3d getBranchPose(Pose2d pose, ExtensionState state, ReefTarget level) { diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 43ccd92e..e48edd0a 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -28,9 +27,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double ALGAE_INTAKE_VOLTAGE = -10.0; public static final double ALGAE_HOLDING_VOLTAGE = -3.0; public static final double ALGAE_CURRENT_THRESHOLD = 30.0; - public static final Transform2d IK_WRIST_TO_CORAL = - new Transform2d( - Units.inchesToMeters(1.0), Units.inchesToMeters(3.0), Rotation2d.fromDegrees(0.0)); + public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; private final BeambreakIO firstBBIO, secondBBIO; private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), From 7296893b7d05a3dae36d3c3034510de6d253f35f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 16:37:14 -0700 Subject: [PATCH 007/126] tune reef setpoints --- src/main/java/frc/robot/Robot.java | 30 +++++++++++++------ .../robot/subsystems/ExtensionKinematics.java | 13 ++++---- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 35038c20..2e261bdc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -284,7 +284,9 @@ public static enum AlgaeScoreTarget { }, PhoenixOdometryThread.getInstance(), swerveDriveSimulation, - new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); + ROBOT_TYPE != RobotType.SIM + ? new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants()) + : new VisionIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); private final ElevatorSubsystem elevator = new ElevatorSubsystem( @@ -464,7 +466,7 @@ public static enum AlgaeScoreTarget { operator.rightBumper(), operator.leftBumper(), new Trigger(() -> killVisionIK) - .or(() -> currentTarget == ReefTarget.L4) + // .or(() -> currentTarget == ReefTarget.L4) .or(() -> DriverStation.isAutonomous()), () -> MathUtil.clamp(operator.getLeftY(), -1.0, 0.0)); @@ -1107,12 +1109,16 @@ public void robotPeriodic() { 0.0)), new Pose3d( // Manipulator new Translation3d( - ShoulderSubsystem.X_OFFSET_METERS - + shoulder.getSetpoint().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - 0, - elevator.getSetpoint() - + ShoulderSubsystem.Z_OFFSET_METERS - + shoulder.getSetpoint().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS), + ShoulderSubsystem.X_OFFSET_METERS + + shoulder.getSetpoint().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + 0, + elevator.getSetpoint() + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulder.getSetpoint().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation3d( + Units.inchesToMeters(1.0), 0.0, Units.inchesToMeters(-8.0)) + .rotateBy(new Rotation3d(0.0, -wrist.getAngle().getRadians(), 0.0))), new Rotation3d(0, wrist.getSetpoint().getRadians(), Math.PI)) }); @@ -1168,9 +1174,15 @@ public void robotPeriodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( - "Manipulator FK Pose", + "IK/Manipulator FK Pose", ExtensionKinematics.getManipulatorPose( swerve.getPose(), superstructure.getExtensionState())); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "IK/Extension FK Pose", + ExtensionKinematics.solveFK( + new ExtensionState( + elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()))); state = superstructure::getState; } diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index de58e4fd..4c024c44 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -30,20 +30,20 @@ public class ExtensionKinematics { private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); static final Transform2d IK_WRIST_TO_CORAL = new Transform2d( - Units.inchesToMeters(-11.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); + Units.inchesToMeters(8.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); // Not super accurate bc of whack public static final Pose2d L1_POSE = new Pose2d(); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final Pose2d L2_POSE = - new Pose2d(new Translation2d(0.24, 0.78), new Rotation2d(-0.61)); + new Pose2d(new Translation2d(0.26, 0.72), new Rotation2d(-0.61)); public static final ExtensionState L2_EXTENSION = solveIK(L2_POSE); public static final Pose2d L3_POSE = - new Pose2d(new Translation2d(0.24, 1.14), new Rotation2d(-0.61)); + new Pose2d(new Translation2d(0.26, 1.12), new Rotation2d(-0.61)); public static final ExtensionState L3_EXTENSION = solveIK(L3_POSE); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.27, 1.79), new Rotation2d(-0.79)); + new Pose2d(new Translation2d(0.4, 1.85), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = @@ -98,7 +98,7 @@ public static ExtensionState solveIK(Pose2d target) { return new ExtensionState( MathUtil.clamp(elevatorHeight, 0.0, ElevatorSubsystem.MAX_EXTENSION_METERS), Rotation2d.fromRadians(shoulderAngle), - Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 45.0))); + Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 120.0))); } public static Pose2d solveFK(ExtensionState state) { @@ -128,8 +128,7 @@ public static Pose3d getManipulatorPose(Pose2d robotPose, ExtensionState state) 0, 0, fk.getY() + ElevatorSubsystem.Z_OFFSET_METERS, - new Rotation3d(0, -state.wristAngle().getRadians(), 0))) - .transformBy(new Transform3d(-IK_WRIST_TO_CORAL.getX() + 0.1, 0.0, 0.0, Rotation3d.kZero)); + new Rotation3d(0, -state.wristAngle().getRadians(), 0))); } public static Pose3d getBranchPose(Pose2d pose, ExtensionState state, ReefTarget level) { From 28f21ba77d71a634df56edfdce66e05de44fcc58 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 20:31:08 -0700 Subject: [PATCH 008/126] first pass tuning reef algae intaking --- .../frc/robot/subsystems/Superstructure.java | 20 +++++++++++++------ .../elevator/ElevatorSubsystem.java | 4 ++-- .../shoulder/ShoulderSubsystem.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 4 ++-- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0c620374..17564c42 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -569,12 +569,20 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.INTAKE_ALGAE_LOW) - .whileTrue(this.extendWithClearance(() -> ExtensionKinematics.LOW_ALGAE_EXTENSION)) + .whileTrue( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers .get(SuperState.INTAKE_ALGAE_HIGH) - .whileTrue(this.extendWithClearance(() -> ExtensionKinematics.HIGH_ALGAE_EXTENSION)) + .whileTrue( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers @@ -772,10 +780,10 @@ private Command extendWithClearance( return Commands.sequence( // Retract shoulder + wrist Commands.parallel( - shoulder - .hold() - .until(() -> wrist.getAngle().getDegrees() < 90.0) - .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), + // shoulder + // .hold() + // .until(() -> true) + (shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), elevator.hold()) .until( diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 91fac47b..bd985e56 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -53,8 +53,8 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_GROUND_EXTENSION = Units.inchesToMeters(5.0); public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(12.5); - public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(25.4); - public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(40.5); + public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); + public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 0be1605b..cefa5196 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -34,7 +34,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(34.0); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(60.0); // may be incorrect as l2-3 poses are derived from ExtensionKinematics now diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index df9d9932..ebaae4ff 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -32,9 +32,9 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(0.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-44.0); + public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(-44.0); + Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(70); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = WRIST_RETRACTED_POS; From d4e16cb588c454e8f3452f35678162efac261b42 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 21:20:24 -0700 Subject: [PATCH 009/126] fix bindings for coral ground --- src/main/java/frc/robot/Robot.java | 22 +++++++++---------- .../subsystems/ManipulatorSubsystem.java | 14 ++++++------ 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2e261bdc..277a951f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -139,7 +139,7 @@ public static enum ReefTarget { ShoulderSubsystem.SHOULDER_SCORE_POS), L4( ElevatorSubsystem.L4_EXTENSION_METERS, - 20.0, + -20.0, WristSubsystem.WRIST_SCORE_L4_POS, ShoulderSubsystem.SHOULDER_SCORE_L4_POS); @@ -182,6 +182,7 @@ public static enum AlgaeScoreTarget { private static ReefTarget currentTarget = ReefTarget.L4; private AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + private boolean leftHandedTarget = false; @AutoLogOutput private boolean killVisionIK = false; @@ -307,13 +308,13 @@ public static enum AlgaeScoreTarget { .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(2)) - .withSlot0(new Slot0Configs().withKV(0.24).withKP(0.5)) + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(5.8667)) + .withSlot0(new Slot0Configs().withKV(0.70).withKP(0.5)) .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) : new RollerIOSim( 0.01, - 2, - new SimpleMotorFeedforward(0.0, 0.24), + 5.8677, + new SimpleMotorFeedforward(0.0, 0.7), new ProfiledPIDController( 0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(15, 1))), new BeambreakIOReal(1, true), @@ -696,7 +697,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and(() -> superstructure.stateIsCoralAlike()) .whileTrue( Commands.parallel( @@ -710,7 +710,7 @@ public Robot() { .plus( new Transform2d( twist.dx, twist.dy, Rotation2d.fromRadians(twist.dtheta))), - driver.leftBumper().getAsBoolean()); + leftHandedTarget); }, // Keeps the robot off the reef wall until it's aligned side-side new Transform2d( @@ -719,7 +719,6 @@ public Robot() { .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.INTAKE_ALGAE_HIGH @@ -776,7 +775,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) .whileTrue( swerve.driveToAlgae( @@ -791,7 +789,6 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.READY_ALGAE @@ -824,7 +821,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.PRE_CLIMB @@ -842,7 +838,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.READY_ALGAE @@ -942,6 +937,9 @@ public Robot() { .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); + operator.povLeft().onTrue(Commands.runOnce(() -> leftHandedTarget = true)); + operator.povRight().onTrue(Commands.runOnce(() -> leftHandedTarget = false)); + operator .back() .and(operator.start()) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index e48edd0a..0cb505e7 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -23,7 +23,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; - public static final double CORAL_INTAKE_VOLTAGE = -10.0; + public static final double CORAL_INTAKE_VELOCITY = -15.0; public static final double ALGAE_INTAKE_VOLTAGE = -10.0; public static final double ALGAE_HOLDING_VOLTAGE = -3.0; public static final double ALGAE_CURRENT_THRESHOLD = 30.0; @@ -81,6 +81,8 @@ public void periodic() { } } + /** For the old ee */ + @Deprecated public Command index() { return Commands.sequence( setVelocity(9.0) @@ -112,13 +114,11 @@ public Command hold() { return this.jog(inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); } - public Command backIndex() { - return Commands.sequence( - setVelocity(-INDEXING_VELOCITY).until(() -> !secondBBInputs.get), index()); - } - public Command intakeCoral() { - return setVoltage(CORAL_INTAKE_VOLTAGE).until(() -> secondBBInputs.get).andThen(hold()); + return Commands.sequence( + setVelocity(CORAL_INTAKE_VELOCITY).until(() -> secondBBInputs.get), + setVelocity(1.0).until(() -> !firstBBInputs.get), + jog(1.0)); } public Command intakeAlgae() { From d0a94c1f0a8b3e649bd80d2473e677c4fa057ff3 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 21:22:21 -0700 Subject: [PATCH 010/126] use proper indexing --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 4 +++- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 0cb505e7..239e6f76 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -29,6 +29,8 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double ALGAE_CURRENT_THRESHOLD = 30.0; public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; + public static final double CORAL_HOLD_POS = 1.0; + private final BeambreakIO firstBBIO, secondBBIO; private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), secondBBInputs = new BeambreakIOInputsAutoLogged(); @@ -118,7 +120,7 @@ public Command intakeCoral() { return Commands.sequence( setVelocity(CORAL_INTAKE_VELOCITY).until(() -> secondBBInputs.get), setVelocity(1.0).until(() -> !firstBBInputs.get), - jog(1.0)); + jog(CORAL_HOLD_POS)); } public Command intakeAlgae() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 17564c42..f01192de 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -205,7 +205,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) // ) - .whileTrue(manipulator.index().repeatedly()) + .whileTrue(manipulator.intakeCoral().repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -313,7 +313,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(manipulator.index()); + .whileTrue(manipulator.jog(ManipulatorSubsystem.CORAL_HOLD_POS)); // keep indexing to make sure its chilling stateTriggers From b4b7ec7814cc7628e08380cec7e4d62771b35b10 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 28 Mar 2025 21:31:50 -0700 Subject: [PATCH 011/126] import zeroing offset from cad --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index ebaae4ff..6de04d55 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,7 +19,7 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(180 - 49.519570 + 5); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(88.0); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); From 6b2c2b245dc7e269df8b14d0ba7b00ccbc3df9a4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 10:43:54 -0700 Subject: [PATCH 012/126] make shoulder wait for wrist to not be retracted to begin clearance extend --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f01192de..4956e3d3 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -780,10 +780,10 @@ private Command extendWithClearance( return Commands.sequence( // Retract shoulder + wrist Commands.parallel( - // shoulder - // .hold() - // .until(() -> true) - (shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), + shoulder + .hold() + .until(() -> wrist.getAngle().getDegrees() < 90.0) + .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), elevator.hold()) .until( From 10d83eb8599d26923825ec91e9ea520e389be891 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 11:05:36 -0700 Subject: [PATCH 013/126] =?UTF-8?q?make=20auto=20use=20ground=20intake=20f?= =?UTF-8?q?or=20=E2=9C=A8testing=E2=9C=A8=20reasons?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/deploy/choreo/LtoAGround.traj | 144 +++++++++++++++++++++++++ src/main/java/frc/robot/Autos.java | 11 +- src/main/java/frc/robot/Robot.java | 10 +- 3 files changed, 158 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/choreo/LtoAGround.traj diff --git a/src/main/deploy/choreo/LtoAGround.traj b/src/main/deploy/choreo/LtoAGround.traj new file mode 100644 index 00000000..0565bd94 --- /dev/null +++ b/src/main/deploy/choreo/LtoAGround.traj @@ -0,0 +1,144 @@ +{ + "name":"LtoAGround", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0471975511965976, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2501322031021118, "y":6.730594158172607, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.203567624092102, "y":6.226143836975098, "heading":-1.5344487260604618, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"(-60) deg", "val":-1.0471975511965976}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2501322031021118 m", "val":1.2501322031021118}, "y":{"exp":"6.730594158172607 m", "val":6.730594158172607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.203567624092102 m", "val":1.203567624092102}, "y":{"exp":"6.226143836975098 m", "val":6.226143836975098}, "heading":{"exp":"-1.5344487260604618 rad", "val":-1.5344487260604618}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.53916,1.97037,3.2716], + "samples":[ + {"t":0.0, "x":3.69166, "y":5.08934, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.21138, "ay":3.29527, "alpha":-1.07639, "fx":[-57.74783,-49.19857,-47.66356,-55.39005], "fy":[48.55353,57.19446,58.4952,51.24257]}, + {"t":0.0405, "x":3.68902, "y":5.09204, "heading":-1.0472, "vx":-0.13007, "vy":0.13347, "omega":-0.0436, "ax":-3.2169, "ay":3.28954, "alpha":-1.07568, "fx":[-57.82933,-49.30624,-47.75891,-55.46659], "fy":[48.44949,57.09549,58.41218,51.15398]}, + {"t":0.08101, "x":3.68112, "y":5.10015, "heading":-1.04896, "vx":-0.26037, "vy":0.26671, "omega":-0.08717, "ax":-3.22306, "ay":3.28313, "alpha":-1.07478, "fx":[-57.92403,-49.43632,-47.8594,-55.54402], "fy":[48.32849,56.97595,58.32403,51.06349]}, + {"t":0.12151, "x":3.66793, "y":5.11364, "heading":-1.05249, "vx":-0.39092, "vy":0.39969, "omega":-0.1307, "ax":-3.22998, "ay":3.27591, "alpha":-1.07365, "fx":[-58.03344,-49.5911,-47.96724,-55.62398], "fy":[48.18835,56.83344,58.22877,50.96918]}, + {"t":0.16202, "x":3.64944, "y":5.13252, "heading":-1.05779, "vx":-0.52175, "vy":0.53238, "omega":-0.17419, "ax":-3.23779, "ay":3.26771, "alpha":-1.07227, "fx":[-58.15945,-49.77351,-48.08523,-55.70852], "fy":[48.02634,56.66484,58.12386,50.8686]}, + {"t":0.20252, "x":3.62565, "y":5.15676, "heading":-1.06484, "vx":-0.65289, "vy":0.66474, "omega":-0.21762, "ax":-3.24669, "ay":3.25833, "alpha":-1.0706, "fx":[-58.30442,-49.98724,-48.217,-55.80026], "fy":[47.83898,56.46613,58.00598,50.75862]}, + {"t":0.24302, "x":3.59655, "y":5.18636, "heading":-1.07366, "vx":-0.7844, "vy":0.79671, "omega":-0.26098, "ax":-3.25693, "ay":3.24747, "alpha":-1.06858, "fx":[-58.47141,-50.23713,-48.36725,-55.90261], "fy":[47.62173,56.23204,57.87076,50.6351]}, + {"t":0.28353, "x":3.5621, "y":5.22129, "heading":-1.08423, "vx":-0.91632, "vy":0.92825, "omega":-0.30426, "ax":-3.26883, "ay":3.23476, "alpha":-1.06616, "fx":[-58.66451,-50.52952,-48.54221,-56.02009], "fy":[47.36847,55.9555,57.71236,50.4925]}, + {"t":0.32403, "x":3.52231, "y":5.26154, "heading":-1.09655, "vx":-1.04872, "vy":1.05927, "omega":-0.34745, "ax":-3.28282, "ay":3.2197, "alpha":-1.06324, "fx":[-58.88922,-50.87293,-48.75031,-56.15883], "fy":[47.07083,55.6269,57.52277,50.32324]}, + {"t":0.36454, "x":3.47714, "y":5.30709, "heading":-1.11063, "vx":-1.18168, "vy":1.18968, "omega":-0.39051, "ax":-3.29951, "ay":3.20155, "alpha":-1.05969, "fx":[-59.15326,-51.27904,-49.00317,-56.32736], "fy":[46.71696,55.23274,57.29072,50.11659]}, + {"t":0.40504, "x":3.42657, "y":5.3579, "heading":-1.12644, "vx":-1.31533, "vy":1.31936, "omega":-0.43344, "ax":-3.31976, "ay":3.17927, "alpha":-1.05536, "fx":[-59.46762,-51.76437,-49.31731,-56.53791], "fy":[46.28957,54.7535,56.99986,49.85696]}, + {"t":0.44555, "x":3.37057, "y":5.41395, "heading":-1.144, "vx":-1.44979, "vy":1.44813, "omega":-0.47618, "ax":-3.34485, "ay":3.15126, "alpha":-1.04997, "fx":[-59.84863,-52.35307,-49.71711,-56.80856], "fy":[45.7625,54.15979,56.62546,49.52074]}, + {"t":0.48605, "x":3.3091, "y":5.47519, "heading":-1.16329, "vx":-1.58527, "vy":1.57577, "omega":-0.51871, "ax":-3.3767, "ay":3.11502, "alpha":-1.04313, "fx":[-60.32136,-53.08193,-50.24001,-57.16723], "fy":[45.0943,53.40523,56.12827,49.07053]}, + {"t":0.52655, "x":3.24212, "y":5.54157, "heading":-1.1843, "vx":-1.72204, "vy":1.70194, "omega":-0.56096, "ax":-3.41847, "ay":3.0663, "alpha":-1.03417, "fx":[-60.92619,-54.0099,-50.94674,-57.65922], "fy":[44.21539,52.41196,55.44201,48.4434]}, + {"t":0.56706, "x":3.16957, "y":5.61302, "heading":-1.20702, "vx":-1.86051, "vy":1.82614, "omega":-0.60285, "ax":-3.47556, "ay":2.99743, "alpha":-1.02186, "fx":[-61.73214,-55.23753,-51.94268,-58.36305], "fy":[43.00003,51.03866,54.44522,47.52491]}, + {"t":0.60756, "x":3.09136, "y":5.68945, "heading":-1.23144, "vx":-2.00128, "vy":1.94755, "omega":-0.64424, "ax":-3.55807, "ay":2.89285, "alpha":-1.00371, "fx":[-62.86618,-56.95045,-53.42742,-59.42653], "fy":[41.1968,49.00013,52.88944,46.08373]}, + {"t":0.64807, "x":3.00738, "y":5.7707, "heading":-1.25753, "vx":-2.1454, "vy":2.06472, "omega":-0.68489, "ax":-3.68694, "ay":2.71601, "alpha":-0.97371, "fx":[-64.58462,-59.52654,-55.82587,-61.16093], "fy":[38.22487,45.62758,50.17879,43.57498]}, + {"t":0.68857, "x":2.91746, "y":5.85656, "heading":-1.28527, "vx":-2.29473, "vy":2.17473, "omega":-0.72433, "ax":-3.91144, "ay":2.35817, "alpha":-0.91314, "fx":[-67.45513,-63.81539,-60.18709,-64.32052], "fy":[32.40732,38.95023,44.48099,38.36752]}, + {"t":0.72907, "x":2.8213, "y":5.94658, "heading":-1.31461, "vx":-2.45316, "vy":2.27025, "omega":-0.76132, "ax":-4.3364, "ay":1.33733, "alpha":-0.72899, "fx":[-72.42017,-71.28298,-69.13358,-70.73068], "fy":[16.69566,20.5892,27.15563,23.01067]}, + {"t":0.76958, "x":2.71838, "y":6.03963, "heading":-1.34545, "vx":-2.6288, "vy":2.32441, "omega":-0.79085, "ax":-3.37255, "ay":-2.93537, "alpha":0.30308, "fx":[-55.52997,-56.57959,-54.78627,-53.64323], "fy":[-47.44414,-46.34201,-48.51952,-49.645]}, + {"t":0.81008, "x":2.60914, "y":6.13137, "heading":-1.37748, "vx":-2.76541, "vy":2.20552, "omega":-0.77857, "ax":0.88654, "ay":-4.44071, "alpha":1.10025, "fx":[20.69077,8.28553,9.16649,19.83028], "fy":[-71.22411,-73.70342,-73.79318,-71.66802]}, + {"t":0.85059, "x":2.49785, "y":6.21706, "heading":-1.40901, "vx":-2.7295, "vy":2.02565, "omega":-0.73401, "ax":1.98131, "ay":-4.10889, "alpha":1.12753, "fx":[39.4398,28.27147,25.95657,35.89475], "fy":[-63.54593,-69.19713,-70.22012,-65.7269]}, + {"t":0.89109, "x":2.38892, "y":6.29574, "heading":-1.43874, "vx":-2.64925, "vy":1.85922, "omega":-0.68834, "ax":2.36764, "ay":-3.91535, "alpha":1.1158, "fx":[45.66192,35.72188,32.12989,41.31172], "fy":[-59.53816,-65.93915,-67.8466,-62.71027]}, + {"t":0.9316, "x":2.28356, "y":6.36784, "heading":-1.46663, "vx":-2.55335, "vy":1.70064, "omega":-0.64314, "ax":2.55917, "ay":-3.80183, "alpha":1.10529, "fx":[48.65879,39.5396,35.24203,43.90993], "fy":[-57.27825,-63.87513,-66.40695,-61.05029]}, + {"t":0.9721, "x":2.18224, "y":6.4336, "heading":-1.49268, "vx":-2.44969, "vy":1.54665, "omega":-0.59837, "ax":2.67271, "ay":-3.72847, "alpha":1.09738, "fx":[50.40413,41.87521,37.10011,45.39539], "fy":[-55.8498,-62.46441,-65.46424,-60.03509]}, + {"t":1.0126, "x":2.08521, "y":6.49319, "heading":-1.51691, "vx":-2.34143, "vy":1.39563, "omega":-0.55392, "ax":2.74763, "ay":-3.67744, "alpha":1.09139, "fx":[51.5403,43.46667,38.33032,46.33648], "fy":[-54.8719,-61.43317,-64.80439,-59.36681]}, + {"t":1.05311, "x":1.99262, "y":6.5467, "heading":-1.53935, "vx":-2.23014, "vy":1.24668, "omega":-0.50972, "ax":2.8007, "ay":-3.63996, "alpha":1.08674, "fx":[52.3355,44.63068,39.2037,46.97429], "fy":[-54.16379,-60.64059,-64.31813,-58.90321]}, + {"t":1.09361, "x":1.90459, "y":6.59421, "heading":-1.55999, "vx":-2.1167, "vy":1.09924, "omega":-0.4657, "ax":2.84024, "ay":-3.61131, "alpha":1.08306, "fx":[52.92108,45.52477,39.85585,47.42806], "fy":[-53.62948,-60.00868,-63.94519,-58.56853]}, + {"t":1.13412, "x":1.82119, "y":6.63577, "heading":-1.57886, "vx":-2.00166, "vy":0.95297, "omega":-0.42183, "ax":2.87082, "ay":-3.5887, "alpha":1.0801, "fx":[53.36884,46.23585,40.36182,47.7634], "fy":[-53.21339,-59.49118,-63.64992,-58.31889]}, + {"t":1.17462, "x":1.74246, "y":6.67143, "heading":-1.59594, "vx":-1.88538, "vy":0.80761, "omega":-0.37808, "ax":2.89518, "ay":-3.57041, "alpha":1.07769, "fx":[53.72142,46.81578,40.76632,48.01938], "fy":[-52.88111,-59.05904,-63.41007,-58.12719]}, + {"t":1.21512, "x":1.66847, "y":6.70121, "heading":-1.61126, "vx":-1.76812, "vy":0.663, "omega":-0.33443, "ax":2.91504, "ay":-3.55531, "alpha":1.07572, "fx":[54.00572,47.29736,41.09761,48.22068], "fy":[-52.61016,-58.69317,-63.21108,-57.97581]}, + {"t":1.25563, "x":1.59925, "y":6.72515, "heading":-1.6248, "vx":-1.65004, "vy":0.51899, "omega":-0.29086, "ax":2.93153, "ay":-3.54264, "alpha":1.07411, "fx":[54.23958,47.70236,41.37432,48.38365], "fy":[-52.38524,-58.3805,-63.04307,-57.85282]}, + {"t":1.29613, "x":1.53482, "y":6.74326, "heading":-1.63658, "vx":-1.53131, "vy":0.3755, "omega":-0.24736, "ax":2.94545, "ay":-3.53185, "alpha":1.0728, "fx":[54.43535,48.04582,41.60925,48.51955], "fy":[-52.19552,-58.1118,-62.89912,-57.74988]}, + {"t":1.33664, "x":1.47521, "y":6.75557, "heading":-1.6466, "vx":-1.412, "vy":0.23244, "omega":-0.2039, "ax":2.95735, "ay":-3.52256, "alpha":1.07174, "fx":[54.60179,48.33845,41.81143,48.63639], "fy":[-52.03315,-57.88036,-62.77428,-57.66093]}, + {"t":1.37714, "x":1.42045, "y":6.7621, "heading":-1.65486, "vx":-1.29222, "vy":0.08977, "omega":-0.16049, "ax":2.96764, "ay":-3.51447, "alpha":1.07089, "fx":[54.74536,48.58808,41.98742,48.74002], "fy":[-51.89228,-57.68118,-62.66487,-57.58155]}, + {"t":1.41765, "x":1.37054, "y":6.76285, "heading":-1.66136, "vx":-1.17202, "vy":-0.05258, "omega":-0.11712, "ax":2.97662, "ay":-3.50737, "alpha":1.07023, "fx":[54.87086,48.80058,42.14211,48.83482], "fy":[-51.76847,-57.51047,-62.56812,-57.50835]}, + {"t":1.45815, "x":1.32551, "y":6.75785, "heading":-1.66611, "vx":-1.05145, "vy":-0.19465, "omega":-0.07377, "ax":2.98453, "ay":-3.50109, "alpha":1.06974, "fx":[54.98196,48.98043,42.27923,48.92409], "fy":[-51.65832,-57.36531,-62.48191,-57.43875]}, + {"t":1.49865, "x":1.28537, "y":6.74709, "heading":-1.66909, "vx":-0.93056, "vy":-0.33646, "omega":-0.03044, "ax":2.99155, "ay":-3.49548, "alpha":1.0694, "fx":[55.0815,49.13111,42.40169,49.01044], "fy":[-51.55915,-57.24338,-62.40457,-57.37073]}, + {"t":1.53916, "x":1.25013, "y":6.73059, "heading":-1.67033, "vx":-0.80939, "vy":-0.47804, "omega":0.01288, "ax":3.009, "ay":-3.47446, "alpha":1.0823, "fx":[55.39658,49.46716,42.63819,49.26385], "fy":[-51.12059,-56.85068,-62.16018,-57.072]}, + {"t":1.56072, "x":1.23338, "y":6.71948, "heading":-1.67005, "vx":-0.74452, "vy":-0.55295, "omega":0.03621, "ax":3.0433, "ay":-3.44219, "alpha":1.1366, "fx":[56.19068,50.12688,42.91052,49.78057], "fy":[-50.24203,-56.2634,-61.96839,-56.61889]}, + {"t":1.58228, "x":1.21804, "y":6.70676, "heading":-1.66927, "vx":-0.6789, "vy":-0.62717, "omega":0.06072, "ax":3.07871, "ay":-3.40805, "alpha":1.1931, "fx":[57.00257,50.81362,43.19472,50.31357], "fy":[-49.31436,-55.6371,-61.76634,-56.14269]}, + {"t":1.60384, "x":1.20411, "y":6.69244, "heading":-1.66796, "vx":-0.61252, "vy":-0.70065, "omega":0.08644, "ax":3.11528, "ay":-3.37191, "alpha":1.25193, "fx":[57.83193,51.52839,43.49151,50.86365], "fy":[-48.33423,-54.96846,-61.55322,-55.64162]}, + {"t":1.6254, "x":1.19163, "y":6.67655, "heading":-1.6661, "vx":-0.54535, "vy":-0.77335, "omega":0.11343, "ax":3.15302, "ay":-3.33363, "alpha":1.31321, "fx":[58.67829,52.2722,43.8017,51.43159], "fy":[-47.29806,-54.2538,-61.32817,-55.11375]}, + {"t":1.64696, "x":1.18061, "y":6.6591, "heading":-1.66365, "vx":-0.47737, "vy":-0.84522, "omega":0.14175, "ax":3.19198, "ay":-3.29302, "alpha":1.37707, "fx":[59.54095,53.04606,44.12613,52.01823], "fy":[-46.20202,-53.48903,-61.09023,-54.55696]}, + {"t":1.66852, "x":1.17106, "y":6.64012, "heading":-1.66059, "vx":-0.40855, "vy":-0.91622, "omega":0.17144, "ax":3.23218, "ay":-3.2499, "alpha":1.44366, "fx":[60.41902,53.85092,44.46571,52.62436], "fy":[-45.04201,-52.66965,-60.83834,-53.96896]}, + {"t":1.69008, "x":1.163, "y":6.61961, "heading":-1.6569, "vx":-0.33886, "vy":-0.98629, "omega":0.20256, "ax":3.27364, "ay":-3.20409, "alpha":1.51311, "fx":[61.31131,54.68767,44.82143,53.25081], "fy":[-43.81369,-51.79062,-60.57133,-53.34722]}, + {"t":1.71164, "x":1.15645, "y":6.5976, "heading":-1.65253, "vx":-0.26828, "vy":-1.05538, "omega":0.23519, "ax":3.31638, "ay":-3.15535, "alpha":1.58559, "fx":[62.21632,55.55709,45.19436,53.89836], "fy":[-42.51242,-50.84639,-60.2879,-52.68903]}, + {"t":1.73321, "x":1.15144, "y":6.57411, "heading":-1.64746, "vx":-0.19678, "vy":-1.12341, "omega":0.26937, "ax":3.36041, "ay":-3.10345, "alpha":1.66127, "fx":[63.13219,56.45985,45.58566,54.56777], "fy":[-41.13331,-49.83075,-59.98661,-51.99141]}, + {"t":1.75477, "x":1.14798, "y":6.54916, "heading":-1.64165, "vx":-0.12432, "vy":-1.19032, "omega":0.30519, "ax":3.40574, "ay":-3.04814, "alpha":1.7403, "fx":[64.05661,57.39638,45.99658,55.25977], "fy":[-39.67122,-48.73681,-59.66584,-51.25112]}, + {"t":1.77633, "x":1.14609, "y":6.52279, "heading":-1.63507, "vx":-0.05089, "vy":-1.25604, "omega":0.34271, "ax":3.45235, "ay":-2.98913, "alpha":1.82288, "fx":[64.98678,58.36687,46.42846,55.97497], "fy":[-38.12074,-47.55689,-59.32381,-50.46466]}, + {"t":1.79789, "x":1.14579, "y":6.49502, "heading":-1.62768, "vx":0.02354, "vy":-1.32049, "omega":0.38202, "ax":3.50021, "ay":-2.92611, "alpha":1.90919, "fx":[65.91933,59.37113,46.88277,56.71395], "fy":[-36.47629,-46.28245,-58.95851,-49.62819]}, + {"t":1.81945, "x":1.14711, "y":6.46586, "heading":-1.61945, "vx":0.09901, "vy":-1.38358, "omega":0.42318, "ax":3.5493, "ay":-2.85876, "alpha":1.99943, "fx":[66.85024,60.40851,47.36109,57.47709], "fy":[-34.73205,-44.904,-58.56768,-48.73758]}, + {"t":1.84101, "x":1.15007, "y":6.43537, "heading":-1.61032, "vx":0.17553, "vy":-1.44522, "omega":0.46629, "ax":3.59954, "ay":-2.78672, "alpha":2.09382, "fx":[67.77474,61.47771,47.86514,58.26467], "fy":[-32.88214,-43.411,-58.14881,-47.78834]}, + {"t":1.86257, "x":1.1547, "y":6.40356, "heading":-1.60027, "vx":0.25314, "vy":-1.5053, "omega":0.51143, "ax":3.65084, "ay":-2.7096, "alpha":2.19256, "fx":[68.68724,62.57667,48.39675,59.0767], "fy":[-30.9206,-41.79176,-57.69904,-46.77562]}, + {"t":1.88413, "x":1.161, "y":6.37048, "heading":-1.58924, "vx":0.33186, "vy":-1.56372, "omega":0.55871, "ax":3.7031, "ay":-2.62698, "alpha":2.2959, "fx":[69.58123,63.70226,48.95791,59.91297], "fy":[-28.84154,-40.03341,-57.21517,-45.69422]}, + {"t":1.90569, "x":1.16902, "y":6.33615, "heading":-1.57719, "vx":0.4117, "vy":-1.62036, "omega":0.60821, "ax":3.75614, "ay":-2.53842, "alpha":2.40407, "fx":[70.44919,64.85004,49.55074,60.77292], "fy":[-26.63922,-38.12177,-56.69358,-44.53851]}, + {"t":1.92725, "x":1.17877, "y":6.30062, "heading":-1.56408, "vx":0.49269, "vy":-1.67509, "omega":0.66004, "ax":3.80976, "ay":-2.44344, "alpha":2.51731, "fx":[71.28249,66.01392,50.17752,61.65561], "fy":[-24.30826,-36.04142,-56.13019,-43.30251]}, + {"t":1.94881, "x":1.19028, "y":6.26394, "heading":-1.54985, "vx":0.57483, "vy":-1.72777, "omega":0.71432, "ax":3.86371, "ay":-2.34155, "alpha":2.6359, "fx":[72.07136,67.18571,50.84063,62.55959], "fy":[-21.84378,-33.77571,-55.52037,-41.97984]}, + {"t":1.97037, "x":1.20357, "y":6.22614, "heading":-1.53445, "vx":0.65813, "vy":-1.77826, "omega":0.77115, "ax":3.90328, "ay":-2.28536, "alpha":2.59842, "fx":[72.36541,67.69406,51.94157,63.24341], "fy":[-20.99481,-32.88926,-54.55521,-41.00617]}, + {"t":1.99806, "x":1.22328, "y":6.17604, "heading":-1.5131, "vx":0.7662, "vy":-1.84153, "omega":0.84309, "ax":3.91306, "ay":-2.26766, "alpha":2.59323, "fx":[72.48701,67.62672,52.20185,63.56843], "fy":[-20.52719,-32.98811,-54.29007,-40.48258]}, + {"t":2.02574, "x":1.246, "y":6.12418, "heading":-1.48976, "vx":0.87453, "vy":-1.90431, "omega":0.91488, "ax":3.92402, "ay":-2.24752, "alpha":2.58797, "fx":[72.61789,67.55903,52.49984,63.92457], "fy":[-20.00793,-33.082,-53.98366,-39.89725]}, + {"t":2.05343, "x":1.27171, "y":6.0706, "heading":-1.46443, "vx":0.98317, "vy":-1.96654, "omega":0.98653, "ax":3.93642, "ay":-2.22439, "alpha":2.58277, "fx":[72.75855,67.49648,52.84263,64.31395], "fy":[-19.42949,-33.15875,-53.62719,-39.24251]}, + {"t":2.08112, "x":1.30044, "y":6.0153, "heading":-1.43712, "vx":1.09215, "vy":-2.02812, "omega":1.05804, "ax":3.95053, "ay":-2.19753, "alpha":2.57779, "fx":[72.90968,67.44644,53.2389,64.73938], "fy":[-18.78163,-33.20214,-53.20949,-38.50841]}, + {"t":2.1088, "x":1.33219, "y":5.95831, "heading":-1.40782, "vx":1.20152, "vy":-2.08896, "omega":1.12941, "ax":3.96675, "ay":-2.16597, "alpha":2.57313, "fx":[73.07222,67.41886,53.69959,65.20456], "fy":[-18.05026,-33.19028,-52.71594,-37.6817]}, + {"t":2.13649, "x":1.36698, "y":5.89965, "heading":-1.37656, "vx":1.31135, "vy":-2.14892, "omega":1.20064, "ax":3.9856, "ay":-2.12838, "alpha":2.56887, "fx":[73.2474,67.42731,54.23878,65.71448], "fy":[-17.21561,-33.09301,-52.12687,-36.74434]}, + {"t":2.16417, "x":1.40481, "y":5.83934, "heading":-1.34331, "vx":1.42169, "vy":-2.20785, "omega":1.27177, "ax":4.00778, "ay":-2.08286, "alpha":2.56498, "fx":[73.43672,67.49034,54.87513,66.27594], "fy":[-16.24932,-32.86791,-51.41499,-35.67111]}, + {"t":2.19186, "x":1.44571, "y":5.77741, "heading":-1.30811, "vx":1.53265, "vy":-2.26552, "omega":1.34278, "ax":4.03423, "ay":-2.02668, "alpha":2.56126, "fx":[73.64177,67.63362,55.6342,66.89839], "fy":[-15.10974,-32.45343,-50.54089,-34.42551]}, + {"t":2.21954, "x":1.48968, "y":5.71391, "heading":-1.27093, "vx":1.64434, "vy":-2.32163, "omega":1.41369, "ax":4.06629, "ay":-1.95571, "alpha":2.55725, "fx":[73.86377,67.89289,56.55232,67.59516], "fy":[-13.73374,-31.75698,-49.44509,-32.95272]}, + {"t":2.24723, "x":1.53677, "y":5.64889, "heading":-1.23179, "vx":1.75691, "vy":-2.37577, "omega":1.48449, "ax":4.10581, "ay":-1.86343, "alpha":2.552, "fx":[74.10187,68.31819,57.68324,68.38527], "fy":[-12.02213,-30.63258,-48.03283,-31.16653]}, + {"t":2.27491, "x":1.58698, "y":5.5824, "heading":-1.19069, "vx":1.87059, "vy":-2.42736, "omega":1.55514, "ax":4.15543, "ay":-1.73896, "alpha":2.54371, "fx":[74.34864,68.97886,59.11001,69.29589], "fy":[-9.81165,-28.83653,-46.14291,-28.92334]}, + {"t":2.3026, "x":1.64036, "y":5.51453, "heading":-1.14764, "vx":1.98563, "vy":-2.4755, "omega":1.62557, "ax":4.21872, "ay":-1.56277, "alpha":2.52891, "fx":[74.57644,69.96461,60.96663,70.36452], "fy":[-6.81772,-25.93208,-43.47742,-25.9663]}, + {"t":2.33029, "x":1.69695, "y":5.4454, "heading":-1.10263, "vx":2.10243, "vy":-2.51877, "omega":1.69558, "ax":4.29957, "ay":-1.29661, "alpha":2.50063, "fx":[74.69362,71.35667,63.47545,71.63331], "fy":[-2.5074,-21.06655,-39.42312,-21.79122]}, + {"t":2.35797, "x":1.75681, "y":5.37517, "heading":-1.05569, "vx":2.22147, "vy":-2.55467, "omega":1.76481, "ax":4.39623, "ay":-0.85649, "alpha":2.44287, "fx":[74.38533,73.02478,66.98071,73.08931], "fy":[4.20723,-12.41172,-32.52322,-15.28036]}, + {"t":2.38566, "x":1.81999, "y":5.30411, "heading":-1.00683, "vx":2.34318, "vy":-2.57838, "omega":1.83244, "ax":4.46045, "ay":-0.03538, "alpha":2.30696, "fx":[72.43726,73.43232,71.59025,74.2198], "fy":[15.73235,4.13886,-18.63577,-3.54902]}, + {"t":2.41334, "x":1.88658, "y":5.23271, "heading":-0.9561, "vx":2.46667, "vy":-2.57936, "omega":1.89631, "ax":4.12316, "ay":1.64783, "alpha":1.84747, "fx":[63.57578,64.04929,71.59135,70.40699], "fy":[36.90381,34.75365,14.64334,21.45452]}, + {"t":2.44103, "x":1.95645, "y":5.16193, "heading":-0.9036, "vx":2.58082, "vy":-2.53374, "omega":1.94746, "ax":2.0449, "ay":3.95487, "alpha":0.56278, "fx":[32.28272,29.7575,34.7593,36.92146], "fy":[65.46187,66.43792,63.84892,62.8694]}, + {"t":2.46871, "x":2.02868, "y":5.0933, "heading":-0.84968, "vx":2.63743, "vy":-2.42425, "omega":1.96304, "ax":-0.68371, "ay":4.43461, "alpha":-0.99068, "fx":[-13.01184,-3.77744,-9.69884,-18.22109], "fy":[72.17315,73.42204,73.08642,71.30828]}, + {"t":2.4964, "x":2.10144, "y":5.02788, "heading":-0.79533, "vx":2.61851, "vy":-2.30147, "omega":1.93561, "ax":-1.97649, "ay":4.04404, "alpha":-1.74231, "fx":[-38.59019,-20.80768,-27.91508,-41.93467], "fy":[63.07849,71.11363,68.93027,61.32727]}, + {"t":2.52409, "x":2.17318, "y":4.96572, "heading":-0.74174, "vx":2.56379, "vy":-2.18951, "omega":1.88738, "ax":-2.55314, "ay":3.71474, "alpha":-2.09165, "fx":[-49.94137,-29.32285,-36.23577,-51.45574], "fy":[55.08007,68.43143,65.31787,54.08652]}, + {"t":2.55177, "x":2.24318, "y":4.90652, "heading":-0.68949, "vx":2.4931, "vy":-2.08667, "omega":1.82947, "ax":-2.85727, "ay":3.49101, "alpha":-2.27758, "fx":[-55.54618,-34.05377,-40.9397,-56.30416], "fy":[49.79725,66.45802,62.70365,49.32673]}, + {"t":2.57946, "x":2.31111, "y":4.85009, "heading":-0.63884, "vx":2.41399, "vy":-1.99001, "omega":1.76641, "ax":-3.04082, "ay":3.33579, "alpha":-2.3879, "fx":[-58.66532,-36.93467,-44.01443,-59.23213], "fy":[46.34607,65.07729,60.73535,45.97679]}, + {"t":2.60714, "x":2.37677, "y":4.79627, "heading":-0.58994, "vx":2.32981, "vy":-1.89766, "omega":1.7003, "ax":-3.16253, "ay":3.22335, "alpha":-2.45802, "fx":[-60.551,-38.81659,-46.22923,-61.20883], "fy":[44.04786,64.10097,59.17392,43.45986]}, + {"t":2.63483, "x":2.44006, "y":4.74497, "heading":-0.54286, "vx":2.24225, "vy":-1.80842, "omega":1.63225, "ax":-3.24883, "ay":3.13865, "alpha":-2.50418, "fx":[-61.75026,-40.11597,-47.93531,-62.64741], "fy":[42.4979,63.39308,57.88038,41.4723]}, + {"t":2.66251, "x":2.5009, "y":4.69611, "heading":-0.49767, "vx":2.15231, "vy":-1.72153, "omega":1.56292, "ax":-3.31312, "ay":3.07276, "alpha":-2.5348, "fx":[-62.53256,-41.05561,-49.31355,-63.75078], "fy":[41.45386,62.86446,56.77264,39.84422]}, + {"t":2.6902, "x":2.55921, "y":4.64962, "heading":-0.4544, "vx":2.06058, "vy":-1.63645, "omega":1.49274, "ax":-3.36282, "ay":3.02017, "alpha":-2.5548, "fx":[-63.04435,-41.76327,-50.46583,-64.62958], "fy":[40.76412,62.45723,55.80028,38.47422]}, + {"t":2.71788, "x":2.61497, "y":4.60547, "heading":-0.41307, "vx":1.96748, "vy":-1.55284, "omega":1.42201, "ax":-3.4024, "ay":2.97729, "alpha":-2.56728, "fx":[-63.37183,-42.31615,-51.45363,-65.34922], "fy":[40.32915,62.13348,54.93113,37.29822]}, + {"t":2.74557, "x":2.66814, "y":4.56362, "heading":-0.37371, "vx":1.87328, "vy":-1.47041, "omega":1.35094, "ax":-3.43464, "ay":2.94172, "alpha":-2.57439, "fx":[-63.56924,-42.76281,-52.31614,-65.95096], "fy":[40.08058,61.86798,54.14394,36.27372]}, + {"t":2.77326, "x":2.71869, "y":4.52404, "heading":-0.3363, "vx":1.77819, "vy":-1.38897, "omega":1.27966, "ax":-3.4614, "ay":2.91179, "alpha":-2.57769, "fx":[-63.67265,-43.13473,-53.07947,-66.46216], "fy":[39.96958,61.6438,53.42426,35.37126]}, + {"t":2.80094, "x":2.76659, "y":4.4867, "heading":-0.30088, "vx":1.68236, "vy":-1.30835, "omega":1.2083, "ax":-3.48395, "ay":2.88629, "alpha":-2.57835, "fx":[-63.70731,-43.45275,-53.76172,-66.90182], "fy":[39.9601,61.44946,52.76201,34.56967]}, + {"t":2.82863, "x":2.81183, "y":4.45159, "heading":-0.26742, "vx":1.58591, "vy":-1.22845, "omega":1.13691, "ax":-3.50319, "ay":2.86433, "alpha":-2.57726, "fx":[-63.69159,-43.73091,-54.37581,-67.28361], "fy":[40.02475,61.27712,52.15003,33.85324]}, + {"t":2.85631, "x":2.8544, "y":4.41867, "heading":-0.23595, "vx":1.48892, "vy":-1.14914, "omega":1.06556, "ax":-3.51978, "ay":2.84524, "alpha":-2.5751, "fx":[-63.6394,-43.97876,-54.93125,-67.61767], "fy":[40.14227,61.12144,51.58312,33.20992]}, + {"t":2.884, "x":2.89427, "y":4.38795, "heading":-0.20645, "vx":1.39147, "vy":-1.07037, "omega":0.99427, "ax":-3.53422, "ay":2.8285, "alpha":-2.5724, "fx":[-63.5616,-44.20283,-55.43522,-67.91172], "fy":[40.29583,60.97879,51.05745,32.63025]}, + {"t":2.91168, "x":2.93144, "y":4.3594, "heading":-0.17892, "vx":1.29362, "vy":-0.99206, "omega":0.92305, "ax":-3.54689, "ay":2.81371, "alpha":-2.56954, "fx":[-63.46685,-44.40759,-55.89324,-68.1718], "fy":[40.47195,60.84676,50.57016,32.10662]}, + {"t":2.93937, "x":2.96589, "y":4.33301, "heading":-0.15336, "vx":1.19543, "vy":-0.91416, "omega":0.85191, "ax":-3.55807, "ay":2.80056, "alpha":-2.56682, "fx":[-63.36226,-44.59609,-56.30965,-68.4027], "fy":[40.65968,60.72373,50.11906,31.63281]}, + {"t":2.96705, "x":2.99763, "y":4.30878, "heading":-0.12978, "vx":1.09692, "vy":-0.83663, "omega":0.78085, "ax":-3.56801, "ay":2.78878, "alpha":-2.56444, "fx":[-63.25372,-44.77041,-56.68791,-68.60831], "fy":[40.85008,60.60868,49.70252,31.20361]}, + {"t":2.99474, "x":3.02663, "y":4.28668, "heading":-0.10816, "vx":0.99814, "vy":-0.75942, "omega":0.70985, "ax":-3.57688, "ay":2.77816, "alpha":-2.56254, "fx":[-63.14619,-44.93193,-57.03085,-68.7918], "fy":[41.03578,60.50099,49.31924,30.81463]}, + {"t":3.02243, "x":3.05289, "y":4.26672, "heading":-0.08851, "vx":0.89911, "vy":-0.68251, "omega":0.6389, "ax":-3.58485, "ay":2.76854, "alpha":-2.56121, "fx":[-63.04386,-45.08156,-57.34075,-68.95581], "fy":[41.21068,60.40033,48.96823,30.46211]}, + {"t":3.05011, "x":3.07641, "y":4.24889, "heading":-0.07082, "vx":0.79986, "vy":-0.60586, "omega":0.56799, "ax":-3.59204, "ay":2.75977, "alpha":-2.56049, "fx":[-62.95035,-45.2199,-57.61953,-69.10256], "fy":[41.36972,60.30654,48.64868,30.14283]}, + {"t":3.0778, "x":3.09718, "y":4.23317, "heading":-0.05509, "vx":0.70041, "vy":-0.52945, "omega":0.49711, "ax":-3.59856, "ay":2.75173, "alpha":-2.5604, "fx":[-62.86872,-45.34733,-57.8688,-69.23388], "fy":[41.50864,60.21958,48.35999,29.85399]}, + {"t":3.10548, "x":3.11519, "y":4.21957, "heading":-0.04133, "vx":0.60078, "vy":-0.45327, "omega":0.42622, "ax":-3.6045, "ay":2.74433, "alpha":-2.56094, "fx":[-62.80161,-45.46409,-58.08987,-69.35135], "fy":[41.62389,60.13948,48.10163,29.59316]}, + {"t":3.13317, "x":3.13044, "y":4.20807, "heading":-0.02953, "vx":0.50099, "vy":-0.37729, "omega":0.35532, "ax":-3.60993, "ay":2.73748, "alpha":-2.5621, "fx":[-62.75128,-45.57033,-58.2839,-69.45629], "fy":[41.71246,60.0663,47.8732,29.35828]}, + {"t":3.16085, "x":3.14293, "y":4.19868, "heading":-0.01969, "vx":0.40105, "vy":-0.3015, "omega":0.28439, "ax":-3.61491, "ay":2.73111, "alpha":-2.56387, "fx":[-62.71968,-45.6662,-58.45182,-69.54982], "fy":[41.77178,60.00011,47.67437,29.14755]}, + {"t":3.18854, "x":3.15265, "y":4.19138, "heading":-0.01182, "vx":0.30097, "vy":-0.22589, "omega":0.2134, "ax":-3.6195, "ay":2.72516, "alpha":-2.56624, "fx":[-62.70843,-45.75181,-58.59445,-69.63286], "fy":[41.79967,59.94096,47.50486,28.95946]}, + {"t":3.21622, "x":3.15959, "y":4.18617, "heading":-0.00591, "vx":0.20076, "vy":-0.15044, "omega":0.14236, "ax":-3.62374, "ay":2.71959, "alpha":-2.56918, "fx":[-62.71891,-45.82732,-58.71245,-69.70621], "fy":[41.79421,59.88886,47.36443,28.79275]}, + {"t":3.24391, "x":3.16376, "y":4.18304, "heading":-0.00197, "vx":0.10043, "vy":-0.07515, "omega":0.07123, "ax":-3.62767, "ay":2.71433, "alpha":-2.57271, "fx":[-62.75226,-45.89292,-58.80637,-69.77048], "fy":[41.75374,59.84375,47.25287,28.6464]}, + {"t":3.2716, "x":3.16515, "y":4.182, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 84d7d9f6..ab782e43 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -37,6 +37,7 @@ public class Autos { public static boolean autoPreScore = false; public static boolean autoScore = false; // TODO perhaps this should not be static + public static boolean autoGroundIntake = false; public Autos(SwerveSubsystem swerve, ManipulatorSubsystem manipulator, FunnelSubsystem funnel) { this.swerve = swerve; @@ -144,7 +145,7 @@ public Command LOtoJ() { HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { - "LO", "J", "PLO", "K", "PLO", "L", "PLM", "A", "PLO" // each stop we are going to, in order + "LO", "J", "PLO", "K", "PLO", "L", // "PLM", "A", "PLO" // each stop we are going to, in order }; // i don't love repeating the plos but ??? for (int i = 0; i < stops.length - 1; i++) { String name = stops[i] + "to" + stops[i + 1]; // concatenate the names of the stops @@ -164,9 +165,13 @@ public Command LOtoJ() { runPath(routine, startPos, endPos, nextPos, steps); } + final var groundTraj = routine.trajectory("LtoAGround"); + routine - .observe(steps.get("LtoPLM").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); + .observe(steps.get("PLOtoL").done()) + .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) + .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) + .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); return routine.cmd(); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 277a951f..08d14b05 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -427,7 +427,7 @@ public static enum AlgaeScoreTarget { }) .debounce(0.15)) // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore) + .or(() -> Autos.autoScore && DriverStation.isAutonomousEnabled()) // .or( // new Trigger( // () -> @@ -449,9 +449,11 @@ public static enum AlgaeScoreTarget { // .debounce(0.08) // .and(() -> swerve.hasFrontTags)) , - driver.rightTrigger().or(() -> Autos.autoPreScore), + driver.rightTrigger().or(() -> Autos.autoPreScore && DriverStation.isAutonomousEnabled()), driver.leftTrigger(), - driver.leftBumper(), + driver + .leftBumper() + .or(() -> Autos.autoGroundIntake && DriverStation.isAutonomousEnabled()), driver .x() .and(driver.pov(-1).negate()) @@ -650,7 +652,7 @@ public Robot() { elevator.setVoltage(0.0)) .withName("Elevator Default Command")); - manipulator.setDefaultCommand(manipulator.index()); + manipulator.setDefaultCommand(manipulator.hold()); shoulder.setDefaultCommand(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)); From 19e1e227afdc663a7d0c0f40e16c9fda2963432c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 13:21:58 -0700 Subject: [PATCH 014/126] turn down shoulder + wrist speed for bring up --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderIOReal.java | 4 ++-- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 08d14b05..50168176 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -334,7 +334,7 @@ public static enum AlgaeScoreTarget { .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(30.0) + .withStatorCurrentLimit(10.0) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 0ba8f4b0..14564126 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -58,13 +58,13 @@ public ShoulderIOReal() { config.Slot0.kP = 8000.0; config.Slot0.kD = 160.0; - config.CurrentLimits.StatorCurrentLimit = 40.0; + config.CurrentLimits.StatorCurrentLimit = 10.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 20.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; // guesses - config.MotionMagic.MotionMagicCruiseVelocity = 1.0; + config.MotionMagic.MotionMagicCruiseVelocity = 1.0 * 0.25; config.MotionMagic.MotionMagicAcceleration = 4.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 6de04d55..bad47ca2 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,13 +39,13 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = WRIST_RETRACTED_POS; public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(6); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4 * 0.25).withMotionMagicAcceleration(6); public static MotionMagicConfigs SLOW_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(4); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4 * 0.25).withMotionMagicAcceleration(4); public static MotionMagicConfigs CRAWL_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2 * 0.25).withMotionMagicAcceleration(2); private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); From 607ee08cbc60e733ea58661d871048a0d8c13f33 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 14:18:07 -0700 Subject: [PATCH 015/126] start tuning positions irl --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Robot.java | 5 ++-- .../subsystems/ManipulatorSubsystem.java | 8 +++++-- .../frc/robot/subsystems/Superstructure.java | 23 ++++++++++--------- .../subsystems/shoulder/ShoulderIOReal.java | 8 +++---- .../shoulder/ShoulderSubsystem.java | 2 +- .../robot/subsystems/wrist/WristIOReal.java | 4 ++-- .../subsystems/wrist/WristSubsystem.java | 18 ++++++++++----- 8 files changed, 41 insertions(+), 30 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 26bbbfd3..248c951e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.compile.nullAnalysis.mode": "automatic", - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 50168176..1cb28b4a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -306,8 +306,7 @@ public static enum AlgaeScoreTarget { .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)) + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(5.8667)) .withSlot0(new Slot0Configs().withKV(0.70).withKP(0.5)) .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) @@ -334,7 +333,7 @@ public static enum AlgaeScoreTarget { .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(10.0) + .withStatorCurrentLimit(30.0) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 239e6f76..88df2d1f 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -29,7 +29,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double ALGAE_CURRENT_THRESHOLD = 30.0; public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; - public static final double CORAL_HOLD_POS = 1.0; + public static final double CORAL_HOLD_POS = 0.5; private final BeambreakIO firstBBIO, secondBBIO; private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), @@ -117,8 +117,12 @@ public Command hold() { } public Command intakeCoral() { + return intakeCoral(CORAL_INTAKE_VELOCITY); + } + + public Command intakeCoral(double vel) { return Commands.sequence( - setVelocity(CORAL_INTAKE_VELOCITY).until(() -> secondBBInputs.get), + setVelocity(vel).until(() -> secondBBInputs.get), setVelocity(1.0).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS)); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4956e3d3..c375015f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -205,7 +205,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) // ) - .whileTrue(manipulator.intakeCoral().repeatedly()) + .whileTrue(manipulator.intakeCoral(-3.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -382,7 +382,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.L1_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_SCORE_L1_POS, WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) @@ -403,7 +403,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L2_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -421,7 +421,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L3_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -439,7 +439,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L4_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -781,15 +781,16 @@ private Command extendWithClearance( // Retract shoulder + wrist Commands.parallel( shoulder - .hold() + .run(() -> {}) .until(() -> wrist.getAngle().getDegrees() < 90.0) .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle( + () -> + wrist.getAngle().getDegrees() < 90.0 + ? Rotation2d.fromDegrees(45.0) + : WristSubsystem.WRIST_CLEARANCE_POS), elevator.hold()) - .until( - () -> - shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)), + .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), // extend elevator Commands.parallel( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 14564126..8ab3cac3 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -51,14 +51,14 @@ public ShoulderIOReal() { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.kG = 8.6; - config.Slot0.kS = 2.5; + config.Slot0.kG = 8.0; + config.Slot0.kS = 3.5; config.Slot0.kV = 0.0; config.Slot0.kA = 0.0; config.Slot0.kP = 8000.0; config.Slot0.kD = 160.0; - config.CurrentLimits.StatorCurrentLimit = 10.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 20.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; @@ -67,7 +67,7 @@ public ShoulderIOReal() { config.MotionMagic.MotionMagicCruiseVelocity = 1.0 * 0.25; config.MotionMagic.MotionMagicAcceleration = 4.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ShoulderSubsystem.SHOULDER_GEAR_RATIO; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index cefa5196..02f4ecfa 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(40.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 40fb204b..521dd93e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -57,7 +57,7 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { motor.optimizeBusUtilization(); - motor.setPosition(edu.wpi.first.math.util.Units.degreesToRotations(-10.0)); + motor.setPosition(edu.wpi.first.math.util.Units.degreesToRotations(10.0)); } @Override @@ -107,7 +107,7 @@ public static TalonFXConfiguration getDefaultConfiguration() { .withSlot0(new Slot0Configs().withGravityType(GravityTypeValue.Arm_Cosine)) .withMotorOutput( new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) + .withNeutralMode(NeutralModeValue.Coast) .withInverted(InvertedValue.Clockwise_Positive)); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index bad47ca2..1ef3b066 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,10 +19,10 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(88.0); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(91.0); - public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(0.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); + public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); @@ -39,13 +39,19 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = WRIST_RETRACTED_POS; public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4 * 0.25).withMotionMagicAcceleration(6); + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(4 * 0.25) + .withMotionMagicAcceleration(6); public static MotionMagicConfigs SLOW_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4 * 0.25).withMotionMagicAcceleration(4); + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(4 * 0.25) + .withMotionMagicAcceleration(4); public static MotionMagicConfigs CRAWL_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(2 * 0.25).withMotionMagicAcceleration(2); + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(2 * 0.25) + .withMotionMagicAcceleration(2); private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); From e3bfc09073d52f97df8d03c8af8b28116d0e6ba7 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 15:00:39 -0700 Subject: [PATCH 016/126] more tuning on robot --- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/Superstructure.java | 6 +++++- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 8 ++++---- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 88df2d1f..3211f950 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -124,7 +124,7 @@ public Command intakeCoral(double vel) { return Commands.sequence( setVelocity(vel).until(() -> secondBBInputs.get), setVelocity(1.0).until(() -> !firstBBInputs.get), - jog(CORAL_HOLD_POS)); + jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } public Command intakeAlgae() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index c375015f..a413252d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -296,8 +296,12 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.GROUND_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, WristSubsystem.WRIST_CORAL_GROUND)) + .whileTrue( + Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) + .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly()) .and(manipulator::getSecondBeambreak) + .debounce(0.060) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers @@ -313,7 +317,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(manipulator.jog(ManipulatorSubsystem.CORAL_HOLD_POS)); + .whileTrue(manipulator.hold()); // keep indexing to make sure its chilling stateTriggers diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 02f4ecfa..2f1a4dce 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -30,7 +30,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(45.0); - public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(0.0); + public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 1ef3b066..a79bbd89 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,11 +19,11 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(91.0); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(101.0); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-3.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); @@ -136,7 +136,7 @@ public Command currentZero(Supplier shoulderInputs) System.out.println("Wrist Zeroing"); }), this.run(() -> io.setMotorVoltage(-1.0)) - .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 10.0), + .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 15.0), this.runOnce( () -> { hasZeroed = true; From 03a3b60c478c207a4401661e86a0683b78fde517 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 15:11:28 -0700 Subject: [PATCH 017/126] adjust l1 --- src/main/java/frc/robot/Robot.java | 2 +- .../robot/subsystems/ExtensionKinematics.java | 3 +- .../frc/robot/subsystems/Superstructure.java | 83 +++++++++---------- 3 files changed, 44 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 08d14b05..7d6c3e3d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -179,7 +179,7 @@ public static enum AlgaeScoreTarget { PROCESSOR } - private static ReefTarget currentTarget = ReefTarget.L4; + private static ReefTarget currentTarget = ReefTarget.L1; private AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 4c024c44..e019b6c0 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -34,7 +34,8 @@ public class ExtensionKinematics { private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); // Not super accurate bc of whack - public static final Pose2d L1_POSE = new Pose2d(); // solveFK(L1_EXTENSION); + public static final Pose2d L1_POSE = + new Pose2d(0.26, 0.35, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final Pose2d L2_POSE = new Pose2d(new Translation2d(0.26, 0.72), new Rotation2d(-0.61)); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4956e3d3..da4ad826 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,10 +1,8 @@ package frc.robot.subsystems; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -377,11 +375,18 @@ private void configureStateTransitionCommands() { // PRE_L{1-4} logic + -> SCORE_CORAL stateTriggers .get(SuperState.PRE_L1) + // .whileTrue( + // this.extendWithClearance( + // ElevatorSubsystem.L1_EXTENSION_METERS, + // ShoulderSubsystem.SHOULDER_SCORE_L1_POS, + // WristSubsystem.WRIST_SCORE_L1_POS)) .whileTrue( this.extendWithClearance( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS)) + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.L1_EXTENSION + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.L1_EXTENSION))) .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) @@ -452,45 +457,39 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_CORAL) .whileTrue( - Commands.either( - Commands.parallel( - elevator.setExtension(ElevatorSubsystem.L1_EXTENSION_METERS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_L1_POS)), - // Score on L2-4 - Commands.either( - Commands.parallel( - ExtensionKinematics.holdStateCommand( - elevator, - shoulder, - wrist, - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel( - reefTarget.get())))), - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), - // End L2-4 - () -> - MathUtil.isNear( - elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0))), - () -> reefTarget.get() == ReefTarget.L1)) + // Commands.either( + // Commands.parallel( + // ExtensionKinematics.holdStateCommand( + // elevator, + // shoulder, + // wrist, + // () -> + // killVisionIK.getAsBoolean() + // ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + // : ExtensionKinematics.getPoseCompensatedExtension( + // pose.get(), + // ExtensionKinematics.getExtensionForLevel( + // reefTarget.get())))), + this.extendWithClearance( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.getExtensionForLevel(reefTarget.get()))) + // // End + // () -> + // MathUtil.isNear( + // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) + ) .and(() -> elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) .whileTrue(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed)) - .and(() -> reefTarget.get() == ReefTarget.L1) - .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) - .whileTrue( - Commands.waitSeconds(0.1) - .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))); + // .and(() -> reefTarget.get() == ReefTarget.L1) + // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) + // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) + // .whileTrue( + // Commands.waitSeconds(0.1) + // .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))) + ; stateTriggers .get(SuperState.SCORE_CORAL) From 7e02572936aa5452724bf70e8357d5e7347b42b1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 16:20:57 -0700 Subject: [PATCH 018/126] adjust algae on robot --- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 6 +++--- src/main/java/frc/robot/subsystems/Superstructure.java | 5 ++++- .../frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 4 ++-- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 4 ++-- 6 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 33d4631a..55d67c68 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -302,7 +302,7 @@ public static enum AlgaeScoreTarget { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(60.0) + .withStatorCurrentLimit(30.0) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 3211f950..64e3cb0b 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -24,9 +24,9 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; public static final double CORAL_INTAKE_VELOCITY = -15.0; - public static final double ALGAE_INTAKE_VOLTAGE = -10.0; - public static final double ALGAE_HOLDING_VOLTAGE = -3.0; - public static final double ALGAE_CURRENT_THRESHOLD = 30.0; + public static final double ALGAE_INTAKE_VOLTAGE = 10.0; + public static final double ALGAE_HOLDING_VOLTAGE = 1.0; + public static final double ALGAE_CURRENT_THRESHOLD = 6.0; public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; public static final double CORAL_HOLD_POS = 0.5; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9909fd42..6fe8edbb 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -636,7 +636,10 @@ private void configureStateTransitionCommands() { ? WristSubsystem.WRIST_RETRACTED_POS : WristSubsystem.WRIST_INTAKE_ALGAE_REEF_RETRACT_POS)) .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() > 20.0 || Robot.ROBOT_TYPE == RobotType.SIM) + .and( + () -> + manipulator.getStatorCurrentAmps() > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD + || Robot.ROBOT_TYPE == RobotType.SIM) .and( () -> AlgaeIntakeTargets.getClosestTargetPose(pose.get()) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index bd985e56..d149f235 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -51,7 +51,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double L4_EXTENSION_METERS = ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - public static final double INTAKE_ALGAE_GROUND_EXTENSION = Units.inchesToMeters(5.0); + public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.135; public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(12.5); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 2f1a4dce..9f359b7e 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -32,7 +32,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = @@ -47,7 +47,7 @@ public class ShoulderSubsystem extends SubsystemBase { ExtensionKinematics.L4_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(40); public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(75.0); + public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a79bbd89..75b47f8a 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -24,7 +24,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-3.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); + public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); @@ -36,7 +36,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(70); - public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = WRIST_RETRACTED_POS; + public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = new MotionMagicConfigs() From 3d92a1e63d3161791bb17d8a2b741dd102d266f0 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 17:06:23 -0700 Subject: [PATCH 019/126] use stops for floor intake --- .../frc/robot/subsystems/Superstructure.java | 25 +++++++++++++------ .../shoulder/ShoulderSubsystem.java | 4 +++ .../subsystems/wrist/WristSubsystem.java | 3 ++- 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 6fe8edbb..2a0a3353 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -291,14 +291,21 @@ private void configureStateTransitionCommands() { .get(SuperState.INTAKE_CORAL_GROUND) .whileTrue( extendWithClearance( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - WristSubsystem.WRIST_CORAL_GROUND)) + ElevatorSubsystem.GROUND_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, + WristSubsystem.WRIST_CORAL_GROUND) + .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS)) + .andThen( + Commands.parallel( + shoulder.setVoltage(-1.0), + elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), + wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)))) .whileTrue( Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly()) .and(manipulator::getSecondBeambreak) + .and(intakeCoralReq.negate()) .debounce(0.060) .onTrue(this.forceState(SuperState.READY_CORAL)); @@ -562,9 +569,7 @@ private void configureStateTransitionCommands() { .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( Commands.waitUntil( - () -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION)) - .andThen( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS))) + () -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION))) .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS)) @@ -656,7 +661,7 @@ private void configureStateTransitionCommands() { .get(SuperState.READY_ALGAE) .whileTrue( extendWithClearance( - 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS)) + 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_READY_ALGAE)) .whileTrue(manipulator.intakeAlgae()); // READY_ALGAE -> PRE_NET stateTriggers @@ -806,7 +811,11 @@ private Command extendWithClearance( // re-extend joints Commands.parallel( shoulder.setTargetAngle(shoulderAngle), - wrist.setTargetAngle(wristAngle), + wrist + .hold() + .until(() -> shoulder.isNearTarget()) + .unless(() -> wristAngle.get().getDegrees() < 90.0) + .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 9f359b7e..62f3b5d9 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -125,6 +125,10 @@ public Command setTargetAngleSlow(final Rotation2d target) { return setTargetAngle(() -> target); } + public Command setVoltage(final double volts) { + return this.run(() -> io.setMotorVoltage(volts)); + } + public Command hold() { return Commands.sequence( setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 75b47f8a..0025e7a0 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -22,8 +22,9 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(101.0); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); + public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(5.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-3.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-5.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); From 67b4183f8ec73524873e9845f4296320aa6d1353 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 17:24:12 -0700 Subject: [PATCH 020/126] ground intake tuning --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/ManipulatorSubsystem.java | 11 ++++++++++- .../java/frc/robot/subsystems/Superstructure.java | 7 +++++-- .../frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 55d67c68..cf09d21a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -301,7 +301,7 @@ public static enum AlgaeScoreTarget { RollerIOReal.getDefaultConfig() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(false) .withStatorCurrentLimit(30.0) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 64e3cb0b..64aaa556 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.beambreak.BeambreakIO; @@ -120,13 +121,21 @@ public Command intakeCoral() { return intakeCoral(CORAL_INTAKE_VELOCITY); } - public Command intakeCoral(double vel) { + public Command intakeCoralAir(double vel) { return Commands.sequence( setVelocity(vel).until(() -> secondBBInputs.get), setVelocity(1.0).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } + public Command intakeCoral(double vel) { + return Commands.sequence( + setVelocity(vel).until(new Trigger(() -> secondBBInputs.get).debounce(0.5)), + Commands.runOnce(() -> io.setPosition(Rotation2d.fromRotations(0.5))), + setVelocity(1.0).until(() -> !firstBBInputs.get), + jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); + } + public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) .until(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2a0a3353..2d8d6aaa 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -203,7 +203,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) // ) - .whileTrue(manipulator.intakeCoral(-3.0).repeatedly()) + .whileTrue(manipulator.intakeCoralAir(-3.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -283,7 +283,10 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - elevator.runCurrentZeroing(), wrist.currentZero(() -> shoulder.getInputs()))) + shoulder.setTargetAngle(Rotation2d.fromDegrees(50.0)), + elevator.runCurrentZeroing(), + Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) + .andThen(wrist.currentZero(() -> shoulder.getInputs())))) .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) .onTrue(this.forceState(prevState)); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 0025e7a0..2ef15235 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -24,7 +24,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(5.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-5.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-6.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); From 1863c23f5c363f80f8db667120b66f3d40d3f9b6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 17:31:28 -0700 Subject: [PATCH 021/126] tune hp pose --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 62f3b5d9..b58712d0 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(45.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 2ef15235..db4a43fd 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(5.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(160.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(176.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-6.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); From 37b458fedc6c408eb0495ceca21957a527e14242 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 29 Mar 2025 18:43:45 -0700 Subject: [PATCH 022/126] more adjustments for new ee robustness --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 73 ++++++++++++++++--- .../subsystems/shoulder/ShoulderIOReal.java | 2 +- .../subsystems/wrist/WristSubsystem.java | 14 +--- 4 files changed, 69 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cf09d21a..8e946717 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -329,7 +329,7 @@ public static enum AlgaeScoreTarget { 12, WristIOReal.getDefaultConfiguration() .withSlot0( - new Slot0Configs().withKP(1000.0).withKD(5.0).withKS(0.3).withKV(3.6)) + new Slot0Configs().withKP(1000.0).withKD(10.0).withKS(0.3).withKV(3.6)) .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2d8d6aaa..a24fcc63 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -307,7 +307,7 @@ private void configureStateTransitionCommands() { Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly()) - .and(manipulator::getSecondBeambreak) + .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) .onTrue(this.forceState(SuperState.READY_CORAL)); @@ -315,6 +315,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.INTAKE_CORAL_GROUND) .and(intakeCoralReq.negate()) + .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .onTrue(this.forceState(SuperState.IDLE)); // READY_CORAL logic @@ -568,11 +569,11 @@ private void configureStateTransitionCommands() { // INTAKE_ALGAE_{location} -> READY_ALGAE stateTriggers .get(SuperState.INTAKE_ALGAE_GROUND) - .whileTrue(elevator.setExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( - Commands.waitUntil( - () -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION))) + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS)) @@ -598,13 +599,15 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.INTAKE_ALGAE_STACK) - .whileTrue(elevator.setExtension(ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION)) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) .andThen(manipulator.setVoltage(-12.0))) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) .and( () -> Robot.ROBOT_TYPE == RobotType.REAL @@ -663,8 +666,10 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.READY_ALGAE) .whileTrue( - extendWithClearance( - 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_READY_ALGAE)) + extendWithClearanceSlow( + () -> 0.1, + () -> ShoulderSubsystem.SHOULDER_RETRACTED_POS, + () -> WristSubsystem.WRIST_READY_ALGAE)) .whileTrue(manipulator.intakeAlgae()); // READY_ALGAE -> PRE_NET stateTriggers @@ -804,6 +809,11 @@ private Command extendWithClearance( ? Rotation2d.fromDegrees(45.0) : WristSubsystem.WRIST_CLEARANCE_POS), elevator.hold()) + // .unless( + // () -> + // shoulder.getAngle().getDegrees() + // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() + // && wrist.getAngle().getDegrees() < 90.0) .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), // extend elevator Commands.parallel( @@ -822,6 +832,49 @@ private Command extendWithClearance( elevator.setExtension(elevatorExtension))); } + private Command extendWithClearanceSlow( + DoubleSupplier elevatorExtension, + Supplier shoulderAngle, + Supplier wristAngle) { + return Commands.sequence( + // Retract shoulder + wrist + Commands.parallel( + shoulder + .run(() -> {}) + .until(() -> wrist.getAngle().getDegrees() < 90.0) + .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), + wrist.setTargetAngle( + () -> + wrist.getAngle().getDegrees() < 90.0 + ? Rotation2d.fromDegrees(45.0) + : WristSubsystem.WRIST_CLEARANCE_POS), + elevator.hold()) + // .unless( + // () -> + // shoulder.getAngle().getDegrees() + // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() + // && wrist.getAngle().getDegrees() < 90.0) + .until( + () -> + shoulder.getAngle().getDegrees() + < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees()), + // extend elevator + Commands.parallel( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + elevator.setExtensionSlow(elevatorExtension)) + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), + // re-extend joints + Commands.parallel( + shoulder.setTargetAngle(shoulderAngle), + wrist + .hold() + .until(() -> shoulder.isNearTarget()) + .unless(() -> wristAngle.get().getDegrees() < 90.0) + .andThen(wrist.setTargetAngle(wristAngle)), + elevator.setExtension(elevatorExtension))); + } + public SuperState getState() { return state; } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 8ab3cac3..6b4936b6 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -64,7 +64,7 @@ public ShoulderIOReal() { config.CurrentLimits.SupplyCurrentLimitEnable = true; // guesses - config.MotionMagic.MotionMagicCruiseVelocity = 1.0 * 0.25; + config.MotionMagic.MotionMagicCruiseVelocity = 1.0; config.MotionMagic.MotionMagicAcceleration = 4.0; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index db4a43fd..9dc478e9 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -22,7 +22,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(101.0); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); - public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(5.0); + public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(176.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-6.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); @@ -40,19 +40,13 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(4 * 0.25) - .withMotionMagicAcceleration(6); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); public static MotionMagicConfigs SLOW_MOTION_MAGIC = - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(4 * 0.25) - .withMotionMagicAcceleration(4); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); public static MotionMagicConfigs CRAWL_MOTION_MAGIC = - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(2 * 0.25) - .withMotionMagicAcceleration(2); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); From 47150070346eaa1c071eeb2bc714ccffb68e888c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 11:48:53 -0700 Subject: [PATCH 023/126] adjust l2 and l3 setpoints --- src/main/java/frc/robot/Robot.java | 2 ++ .../robot/subsystems/ExtensionKinematics.java | 21 +++++++++---------- .../subsystems/ManipulatorSubsystem.java | 4 ++++ .../frc/robot/subsystems/Superstructure.java | 10 ++++++++- .../subsystems/wrist/WristSubsystem.java | 4 ++-- 5 files changed, 27 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8e946717..d83de2ed 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -131,10 +131,12 @@ public static enum ReefTarget { ShoulderSubsystem.SHOULDER_SCORE_POS), L2( ElevatorSubsystem.L2_EXTENSION_METERS, + -15.0, WristSubsystem.WRIST_SCORE_L2_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L3( ElevatorSubsystem.L3_EXTENSION_METERS, + -15.0, WristSubsystem.WRIST_SCORE_L3_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L4( diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index e019b6c0..0e29d926 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -37,14 +37,14 @@ public class ExtensionKinematics { public static final Pose2d L1_POSE = new Pose2d(0.26, 0.35, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); - public static final Pose2d L2_POSE = - new Pose2d(new Translation2d(0.26, 0.72), new Rotation2d(-0.61)); - public static final ExtensionState L2_EXTENSION = solveIK(L2_POSE); - public static final Pose2d L3_POSE = - new Pose2d(new Translation2d(0.26, 1.12), new Rotation2d(-0.61)); - public static final ExtensionState L3_EXTENSION = solveIK(L3_POSE); + public static final ExtensionState L2_EXTENSION = + new ExtensionState(0.27, Rotation2d.fromRadians(0.569), Rotation2d.fromRadians(2.447)); + public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); + public static final ExtensionState L3_EXTENSION = + new ExtensionState(0.634, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); + public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.4, 1.85), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.4, 1.9), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = @@ -104,11 +104,10 @@ public static ExtensionState solveIK(Pose2d target) { public static Pose2d solveFK(ExtensionState state) { return new Pose2d( - state.shoulderAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - state.elevatorHeightMeters() - + state.shoulderAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS, + state.shoulderAngle().getCos() * ARM_LENGTH_METERS, + state.elevatorHeightMeters() + state.shoulderAngle().getSin() * ARM_LENGTH_METERS, state.wristAngle()) - .transformBy(ManipulatorSubsystem.IK_WRIST_TO_CORAL); + .transformBy(IK_WRIST_TO_CORAL); } public static ExtensionState getPoseCompensatedExtension(Pose2d pose, ExtensionState target) { diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 64aaa556..d81aff32 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -117,6 +117,10 @@ public Command hold() { return this.jog(inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); } + public void resetPosition(final double rotations) { + io.resetEncoder(rotations); + } + public Command intakeCoral() { return intakeCoral(CORAL_INTAKE_VELOCITY); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a24fcc63..0a47da01 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.utils.autoaim.CoralTargets; import frc.robot.utils.autoaim.HumanPlayerTargets; import java.util.HashMap; import java.util.Map; @@ -203,7 +204,7 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) // ) - .whileTrue(manipulator.intakeCoralAir(-3.0).repeatedly()) + .whileTrue(manipulator.intakeCoralAir(-6.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -310,6 +311,7 @@ private void configureStateTransitionCommands() { .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) + .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.5))) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers @@ -510,6 +512,12 @@ private void configureStateTransitionCommands() { .get(SuperState.SCORE_CORAL) .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) + .and( + () -> + CoralTargets.getClosestTarget(pose.get()) + .getTranslation() + .getDistance(pose.get().getTranslation()) + > 0.2) .debounce(0.15) .onTrue(forceState(SuperState.IDLE)); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 9dc478e9..2706047c 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(176.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-6.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); @@ -32,7 +32,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(0.0); + public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); From 8f83f7d348da1e40935da5b579f8c1d1a4c8bb2d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 12:39:46 -0700 Subject: [PATCH 024/126] adjust setpoints, clearance extension --- .../frc/robot/subsystems/Superstructure.java | 55 +++++++++++++------ .../shoulder/ShoulderSubsystem.java | 1 + .../subsystems/wrist/WristSubsystem.java | 5 +- 3 files changed, 43 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0a47da01..51e3996a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -809,24 +809,40 @@ private Command extendWithClearance( Commands.parallel( shoulder .run(() -> {}) - .until(() -> wrist.getAngle().getDegrees() < 90.0) - .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), - wrist.setTargetAngle( - () -> - wrist.getAngle().getDegrees() < 90.0 - ? Rotation2d.fromDegrees(45.0) - : WristSubsystem.WRIST_CLEARANCE_POS), + .until( + () -> + wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS)) + .andThen( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle( + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0)), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0), elevator.hold()) // .unless( // () -> // shoulder.getAngle().getDegrees() // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() // && wrist.getAngle().getDegrees() < 90.0) - .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), + .until( + () -> + shoulder.getAngle().getDegrees() + < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees()), // extend elevator Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0), elevator.setExtension(elevatorExtension)) .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), // re-extend joints @@ -849,13 +865,15 @@ private Command extendWithClearanceSlow( Commands.parallel( shoulder .run(() -> {}) - .until(() -> wrist.getAngle().getDegrees() < 90.0) + .until( + () -> + wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS)) .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), - wrist.setTargetAngle( - () -> - wrist.getAngle().getDegrees() < 90.0 - ? Rotation2d.fromDegrees(45.0) - : WristSubsystem.WRIST_CLEARANCE_POS), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0), elevator.hold()) // .unless( // () -> @@ -869,7 +887,10 @@ private Command extendWithClearanceSlow( // extend elevator Commands.parallel( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> wrist.getAngle().getDegrees() < 90.0), elevator.setExtensionSlow(elevatorExtension)) .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), // re-extend joints diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index b58712d0..39676822 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -49,6 +49,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); + public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(70.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 2706047c..26320b33 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -24,7 +24,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-6.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-5.5); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); @@ -32,7 +32,10 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); + public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); + public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(160.0); + public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); From b22061b0e4b8e0fba951bf274f3e04f07788ec3b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 14:27:51 -0700 Subject: [PATCH 025/126] fix not retracting after l2/3 scoring --- src/main/java/frc/robot/Robot.java | 2 +- .../robot/subsystems/ExtensionKinematics.java | 8 ++++---- .../frc/robot/subsystems/Superstructure.java | 16 ++++++++-------- .../subsystems/shoulder/ShoulderSubsystem.java | 2 +- .../robot/subsystems/wrist/WristSubsystem.java | 2 +- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d83de2ed..2dff15ea 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -126,7 +126,7 @@ private RobotHardware(SwerveConstants swerveConstants) { public static enum ReefTarget { L1( ElevatorSubsystem.L1_EXTENSION_METERS, - 12.0, + 3.0, WristSubsystem.WRIST_SCORE_L1_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L2( diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 0e29d926..034f87bb 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -35,16 +35,16 @@ public class ExtensionKinematics { // Not super accurate bc of whack public static final Pose2d L1_POSE = - new Pose2d(0.26, 0.35, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); + new Pose2d(0.26, 0.4, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final ExtensionState L2_EXTENSION = - new ExtensionState(0.27, Rotation2d.fromRadians(0.569), Rotation2d.fromRadians(2.447)); + new ExtensionState(0.33, Rotation2d.fromRadians(0.569), Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = - new ExtensionState(0.634, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); + new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.4, 1.9), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.37, 1.9), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 51e3996a..e1eade96 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -201,9 +201,10 @@ private void configureStateTransitionCommands() { // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) // .andThen( extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) // ) + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS) + .repeatedly()) // ) .whileTrue(manipulator.intakeCoralAir(-6.0).repeatedly()) .whileTrue( funnel.setVoltage( @@ -812,7 +813,9 @@ private Command extendWithClearance( .until( () -> wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS)) + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees()) .andThen( Commands.either( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), @@ -829,10 +832,7 @@ private Command extendWithClearance( // shoulder.getAngle().getDegrees() // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() // && wrist.getAngle().getDegrees() < 90.0) - .until( - () -> - shoulder.getAngle().getDegrees() - < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees()), + .until(() -> shoulder.isNearTarget() && wrist.isNearTarget()), // extend elevator Commands.parallel( Commands.either( diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 39676822..05b640a2 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -49,7 +49,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(70.0); + public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(45.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 26320b33..db0efebb 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -34,7 +34,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(160.0); + public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = From 1c147423a60e1085bd81f9a361d04ffdf4aefa2d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 15:01:27 -0700 Subject: [PATCH 026/126] reenable current limits, adjust l4 pose --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 4 ++-- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2dff15ea..db4dbd53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -303,8 +303,8 @@ public static enum AlgaeScoreTarget { RollerIOReal.getDefaultConfig() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(30.0) + .withStatorCurrentLimitEnable(true) + .withStatorCurrentLimit(60.0) .withSupplyCurrentLimit(20.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 034f87bb..91b527b9 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -30,7 +30,7 @@ public class ExtensionKinematics { private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); static final Transform2d IK_WRIST_TO_CORAL = new Transform2d( - Units.inchesToMeters(8.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); + Units.inchesToMeters(12.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); // Not super accurate bc of whack @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.37, 1.9), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.37, 2.0), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index db0efebb..37ec1267 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -43,7 +43,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(5); public static MotionMagicConfigs SLOW_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); From ed037bc03ff44dea249cbc6e4a4a9b5e41d07338 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 15:16:39 -0700 Subject: [PATCH 027/126] tune barge shot --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 +++++--- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index e1eade96..143d4e0c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -726,7 +726,7 @@ private void configureStateTransitionCommands() { // PRE_NET logic stateTriggers .get(SuperState.PRE_NET) - .whileTrue(manipulator.setVoltage(2 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) + .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) .whileTrue( Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), @@ -741,11 +741,13 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_ALGAE_NET) .onTrue(Commands.runOnce(() -> stateTimer.reset())) - .whileTrue(manipulator.setVoltage(13.0)) + .whileTrue(manipulator.setVoltage(-13.0)) .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> stateTimer.hasElapsed(1)) + .and(() -> stateTimer.hasElapsed(0.5)) + .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS)) + .and(() -> stateTimer.hasElapsed(1.0)) .onTrue(forceState(SuperState.IDLE)); stateTriggers diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 37ec1267..d03b3e0e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,7 +39,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(70); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = From a2a8f1680f0146595be8ac00c4fe50449955a662 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 16:03:41 -0700 Subject: [PATCH 028/126] tune stack intake, fix ik at max extension (?) --- .../frc/robot/subsystems/ExtensionKinematics.java | 4 ++-- .../frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- .../java/frc/robot/subsystems/Superstructure.java | 12 +----------- .../robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- .../frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 5 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 91b527b9..c6725d5e 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -88,8 +88,8 @@ public static ExtensionState solveIK(Pose2d target) { .getY(); // If we're extending higher than we can reach, prioritize matching Z instead of X if (elevatorHeight > MAX_EXTENSION_METERS) { - elevatorHeight = MAX_EXTENSION_METERS; - shoulderAngle = Math.asin((target.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); + elevatorHeight = MAX_EXTENSION_METERS - Units.inchesToMeters(1.0); + shoulderAngle = Math.asin((wristPose.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); // Limit shoulder angle if (Double.isNaN(shoulderAngle) || shoulderAngle > Units.degreesToRadians(85.0)) { shoulderAngle = Units.degreesToRadians(85.0); diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index d81aff32..a2efd564 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -24,7 +24,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; - public static final double CORAL_INTAKE_VELOCITY = -15.0; + public static final double CORAL_INTAKE_VELOCITY = -18.0; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; public static final double ALGAE_CURRENT_THRESHOLD = 6.0; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 143d4e0c..0b9bc9d3 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -616,17 +616,7 @@ private void configureStateTransitionCommands() { .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .andThen(manipulator.setVoltage(-12.0))) - .and( - () -> - Robot.ROBOT_TYPE == RobotType.REAL - ? manipulator.getStatorCurrentAmps() - > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD - : manipulator.hasAlgae()) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION)) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .onTrue(forceState(SuperState.CHECK_ALGAE)); + .andThen(manipulator.setVoltage(12.0))); // leave intake stateTriggers diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d149f235..8a592119 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -52,7 +52,7 @@ public class ElevatorSubsystem extends SubsystemBase { ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.135; - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(12.5); + public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index d03b3e0e..bf5b136d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -26,7 +26,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-5.5); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); - public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); + public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); From 20fa56289631d970d2b0d7649ca493cb66f78d5a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 16:50:58 -0700 Subject: [PATCH 029/126] update ratio --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index db4dbd53..fb9a89e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -309,8 +309,8 @@ public static enum AlgaeScoreTarget { .withSupplyCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(5.8667)) - .withSlot0(new Slot0Configs().withKV(0.70).withKP(0.5)) + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) + .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) : new RollerIOSim( 0.01, diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index a2efd564..da5d6020 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -32,6 +32,8 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final double CORAL_HOLD_POS = 0.5; + public static final double GEAR_RATIO = (58.0 / 10.0) * (24.0 / 18.0); + private final BeambreakIO firstBBIO, secondBBIO; private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), secondBBInputs = new BeambreakIOInputsAutoLogged(); From 3477e2f5a28ac2d2e7b2f5455095bd109af1e20b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden <70111132+Lewis-Seiden@users.noreply.github.com> Date: Sun, 30 Mar 2025 23:52:37 +0000 Subject: [PATCH 030/126] run spotless --- src/main/java/frc/robot/Robot.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fb9a89e6..18bf07e7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -309,7 +309,9 @@ public static enum AlgaeScoreTarget { .withSupplyCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) + .withFeedback( + new FeedbackConfigs() + .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) : new RollerIOSim( From 385f718d8ec60ece9cdd743c1c09270625d7cdfe Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 18:13:38 -0700 Subject: [PATCH 031/126] update more setpoints --- src/main/java/frc/robot/Robot.java | 6 ++++-- .../java/frc/robot/subsystems/Superstructure.java | 13 +++++++++---- .../subsystems/elevator/ElevatorSubsystem.java | 2 +- .../frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fb9a89e6..2dfda447 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -305,11 +305,13 @@ public static enum AlgaeScoreTarget { new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) .withStatorCurrentLimit(60.0) - .withSupplyCurrentLimit(20.0) + .withSupplyCurrentLimit(30.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) + .withFeedback( + new FeedbackConfigs() + .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) : new RollerIOSim( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0b9bc9d3..81a14cd0 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -285,7 +285,7 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - shoulder.setTargetAngle(Rotation2d.fromDegrees(50.0)), + shoulder.setTargetAngle(Rotation2d.fromDegrees(55.0)), elevator.runCurrentZeroing(), Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) .andThen(wrist.currentZero(() -> shoulder.getInputs())))) @@ -824,7 +824,8 @@ private Command extendWithClearance( // shoulder.getAngle().getDegrees() // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() // && wrist.getAngle().getDegrees() < 90.0) - .until(() -> shoulder.isNearTarget() && wrist.isNearTarget()), + .until(() -> shoulder.isNearTarget() && wrist.isNearTarget()) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // extend elevator Commands.parallel( Commands.either( @@ -836,14 +837,18 @@ private Command extendWithClearance( wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), () -> wrist.getAngle().getDegrees() < 90.0), elevator.setExtension(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // re-extend joints Commands.parallel( shoulder.setTargetAngle(shoulderAngle), wrist .hold() .until(() -> shoulder.isNearTarget()) - .unless(() -> wristAngle.get().getDegrees() < 90.0) + .unless( + () -> + wristAngle.get().getDegrees() < 90.0 + || shoulder.getAngle().getDegrees() < 60.0) .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 8a592119..f009bab5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -57,7 +57,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); - public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; + public static final double ALGAE_PROCESSOR_EXTENSION = 0.1; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index bf5b136d..4ed00d52 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -24,7 +24,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(-5.5); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); From af86ef0426a088bda1f9551755a00f215a207604 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 19:12:51 -0700 Subject: [PATCH 032/126] make wrist use hardstop for ground, reenable left bumper align controls --- src/main/java/frc/robot/Robot.java | 5 ++++- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 4 ++++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2dfda447..7ffff0c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -702,6 +702,7 @@ public Robot() { driver .rightBumper() + .or(driver.leftBumper()) .and(() -> superstructure.stateIsCoralAlike()) .whileTrue( Commands.parallel( @@ -715,7 +716,7 @@ public Robot() { .plus( new Transform2d( twist.dx, twist.dy, Rotation2d.fromRadians(twist.dtheta))), - leftHandedTarget); + driver.leftBumper().getAsBoolean()); }, // Keeps the robot off the reef wall until it's aligned side-side new Transform2d( @@ -780,6 +781,7 @@ public Robot() { driver .rightBumper() + .or(driver.leftBumper()) .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) .whileTrue( swerve.driveToAlgae( @@ -794,6 +796,7 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); driver .rightBumper() + .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.READY_ALGAE diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 81a14cd0..cb070412 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -304,7 +304,7 @@ private void configureStateTransitionCommands() { Commands.parallel( shoulder.setVoltage(-1.0), elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), - wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)))) + wrist.setVoltage(-1.0)))) .whileTrue( Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 4ed00d52..ca894c7b 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -110,6 +110,10 @@ public Command hold() { setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); } + public Command setVoltage(final double volts) { + return this.run(() -> io.setMotorVoltage(volts)); + } + public Rotation2d getAngle() { return inputs.position; } From 7c2dd023a425e175254b8a422ff99b9212d2d21d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 19:15:18 -0700 Subject: [PATCH 033/126] make ready coral use hp pose, wrist use pid before hardstop --- src/main/java/frc/robot/subsystems/Superstructure.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index cb070412..6e974f84 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -304,7 +304,10 @@ private void configureStateTransitionCommands() { Commands.parallel( shoulder.setVoltage(-1.0), elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), - wrist.setVoltage(-1.0)))) + wrist + .setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND) + .until(wrist::isNearTarget) + .andThen(wrist.setVoltage(-1.0))))) .whileTrue( Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) @@ -327,8 +330,8 @@ private void configureStateTransitionCommands() { .whileTrue( extendWithClearance( ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_RETRACTED_POS, - WristSubsystem.WRIST_RETRACTED_POS)) + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS)) .whileTrue(manipulator.hold()); // keep indexing to make sure its chilling From 8baa4a78668c8a82a9538c7fcb7824a3fe762af8 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 30 Mar 2025 20:03:26 -0700 Subject: [PATCH 034/126] adjust wrist zero offset with shims --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index ca894c7b..b20b7b1d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,7 +19,7 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(101.0); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(85); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); From 6738f79d2f7d36cca9c7b0c80f428a116cc9bd66 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 1 Apr 2025 18:44:56 -0700 Subject: [PATCH 035/126] redo zeroing offset --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index b20b7b1d..537f247d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -19,7 +19,7 @@ public class WristSubsystem extends SubsystemBase { // TODO: UPDATE WHEN CAD IS FINISHED public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(85); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); From 2428fdecaa69b8d7e145358e28fbaa247998febc Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 1 Apr 2025 18:49:59 -0700 Subject: [PATCH 036/126] increase manipulator current limit --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7ffff0c9..2bf6da2e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -304,7 +304,7 @@ public static enum AlgaeScoreTarget { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(60.0) + .withStatorCurrentLimit(120.0) .withSupplyCurrentLimit(30.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( From 9cf7b35681a73393377d9c36b459f2e96f22401d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 1 Apr 2025 19:13:39 -0700 Subject: [PATCH 037/126] increase wrist accel and add soft limit to try to avoid catching it --- src/main/java/frc/robot/Robot.java | 7 ++++++- .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2bf6da2e..65fc37b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -343,7 +344,11 @@ public static enum AlgaeScoreTarget { .withSupplyCurrentLimitEnable(true)) .withFeedback( new FeedbackConfigs() - .withSensorToMechanismRatio(WristSubsystem.WRIST_GEAR_RATIO))) + .withSensorToMechanismRatio(WristSubsystem.WRIST_GEAR_RATIO)) + .withSoftwareLimitSwitch( + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(0.5))) : new WristIOSim()); private final FunnelSubsystem funnel = diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 537f247d..a449e7c5 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -43,7 +43,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(5); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(8); public static MotionMagicConfigs SLOW_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); From 5e8533b9e28b2ea18ff28f1bd805276cf5cc0edb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 1 Apr 2025 19:20:25 -0700 Subject: [PATCH 038/126] revert accel increase --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index a449e7c5..537f247d 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -43,7 +43,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(8); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(5); public static MotionMagicConfigs SLOW_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); From 012b434a14df845c5af5ffa51864f3e0d2eea447 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Tue, 1 Apr 2025 19:37:18 -0700 Subject: [PATCH 039/126] adjust tucked clearance shoulder pose, increase filtering for ik --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 6 +++--- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index c6725d5e..7fc40a6a 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -158,9 +158,9 @@ public static Command holdStateCommand( ShoulderSubsystem shoulder, WristSubsystem wrist, Supplier target) { - final LinearFilter elevatorFilter = LinearFilter.movingAverage(5); - final LinearFilter shoulderFilter = LinearFilter.movingAverage(5); - final LinearFilter wristFilter = LinearFilter.movingAverage(5); + final LinearFilter elevatorFilter = LinearFilter.movingAverage(8); + final LinearFilter shoulderFilter = LinearFilter.movingAverage(8); + final LinearFilter wristFilter = LinearFilter.movingAverage(8); return Commands.runOnce( () -> { elevatorFilter.reset(); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 05b640a2..0c4e9dd4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -49,7 +49,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(45.0); + public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(40.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); From dae0be77a7b442d3f6a269f9870ccfa4fd424ea6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 16:00:36 -0700 Subject: [PATCH 040/126] try to avoid jitters when starting extendWithClearance near target --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 6e974f84..3291e7cb 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -828,7 +828,7 @@ private Command extendWithClearance( // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() // && wrist.getAngle().getDegrees() < 90.0) .until(() -> shoulder.isNearTarget() && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), // extend elevator Commands.parallel( Commands.either( @@ -851,7 +851,8 @@ private Command extendWithClearance( .unless( () -> wristAngle.get().getDegrees() < 90.0 - || shoulder.getAngle().getDegrees() < 60.0) + || shoulder.getAngle().getDegrees() < 60.0 + || shoulder.isNearTarget()) .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))); } From 1e59a9268a5f35e69a086a2961ada4800a12ca8c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 17:19:29 -0700 Subject: [PATCH 041/126] hardcode auto prescore to true because it wasnt working otherwise for some reason???? --- src/main/java/frc/robot/Autos.java | 22 ++++++++++--------- src/main/java/frc/robot/Robot.java | 14 +++++------- .../subsystems/shoulder/ShoulderIOReal.java | 2 +- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ab782e43..656f4e5b 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -27,6 +27,7 @@ import java.util.HashMap; import java.util.Optional; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Autos { @@ -35,9 +36,9 @@ public class Autos { private final FunnelSubsystem funnel; private final AutoFactory factory; - public static boolean autoPreScore = false; - public static boolean autoScore = false; // TODO perhaps this should not be static - public static boolean autoGroundIntake = false; + @AutoLogOutput public static boolean autoPreScore = true; + @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static + @AutoLogOutput public static boolean autoGroundIntake = false; public Autos(SwerveSubsystem swerve, ManipulatorSubsystem manipulator, FunnelSubsystem funnel) { this.swerve = swerve; @@ -142,6 +143,7 @@ public void runPath( public Command LOtoJ() { final var routine = factory.newRoutine("LO to J"); bindElevatorExtension(routine); + routine.active().onTrue(Commands.print("Auto!")); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -165,15 +167,15 @@ public Command LOtoJ() { runPath(routine, startPos, endPos, nextPos, steps); } - final var groundTraj = routine.trajectory("LtoAGround"); + // final var groundTraj = routine.trajectory("LtoAGround"); - routine - .observe(steps.get("PLOtoL").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) - .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) - .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + // routine + // .observe(steps.get("PLOtoL").done()) + // .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) + // .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) + // .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); - return routine.cmd(); + return routine.cmd().alongWith(Commands.print("auto :(")); } public Command ROtoE() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 65fc37b5..267b5552 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -182,7 +182,7 @@ public static enum AlgaeScoreTarget { PROCESSOR } - private static ReefTarget currentTarget = ReefTarget.L1; + private static ReefTarget currentTarget = ReefTarget.L4; private AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; @@ -435,7 +435,7 @@ public static enum AlgaeScoreTarget { }) .debounce(0.15)) // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore && DriverStation.isAutonomousEnabled()) + .or(() -> Autos.autoScore && DriverStation.isAutonomous()) // .or( // new Trigger( // () -> @@ -457,11 +457,9 @@ public static enum AlgaeScoreTarget { // .debounce(0.08) // .and(() -> swerve.hasFrontTags)) , - driver.rightTrigger().or(() -> Autos.autoPreScore && DriverStation.isAutonomousEnabled()), + driver.rightTrigger().or(() -> DriverStation.isAutonomous()), driver.leftTrigger(), - driver - .leftBumper() - .or(() -> Autos.autoGroundIntake && DriverStation.isAutonomousEnabled()), + driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), driver .x() .and(driver.pov(-1).negate()) @@ -605,10 +603,10 @@ public Robot() { || superstructure.getState() == SuperState.READY_CORAL) .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.5)); - new Trigger(() -> DriverStation.isEnabled()) + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoScore = false)); - new Trigger(() -> DriverStation.isEnabled()) + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoPreScore = false)); new Trigger( diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 6b4936b6..fae583d4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -67,7 +67,7 @@ public ShoulderIOReal() { config.MotionMagic.MotionMagicCruiseVelocity = 1.0; config.MotionMagic.MotionMagicAcceleration = 4.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = ShoulderSubsystem.SHOULDER_GEAR_RATIO; From 78695e289159a79cb1a07402707e14d22f8228f5 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 17:45:26 -0700 Subject: [PATCH 042/126] move auto prescore condition to Robot instead of goofy ah binding --- src/main/java/frc/robot/Robot.java | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 267b5552..2f95dcec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -457,7 +457,22 @@ public static enum AlgaeScoreTarget { // .debounce(0.08) // .and(() -> swerve.hasFrontTags)) , - driver.rightTrigger().or(() -> DriverStation.isAutonomous()), + driver + .rightTrigger() + .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) + .or( + () -> + swerve + .getPose() + .getTranslation() + .minus( + DriverStation.getAlliance().orElse(Alliance.Blue) + == Alliance.Blue + ? AutoAim.BLUE_REEF_CENTER + : AutoAim.RED_REEF_CENTER) + .getNorm() + < 3.5 + && DriverStation.isAutonomous()), driver.leftTrigger(), driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), driver From 9f4142c3393f4212f5633d09439b23e3c12f87e1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 21:48:56 -0700 Subject: [PATCH 043/126] abstract checking for tuck --- src/main/java/frc/robot/Robot.java | 6 ++++++ .../java/frc/robot/subsystems/Superstructure.java | 14 +++++++++----- .../frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2f95dcec..2388b88b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -603,6 +603,12 @@ public Robot() { .setCurrent(60.0) .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); + SmartDashboard.putData( + "Check Clear", + Commands.parallel( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))); + // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3291e7cb..ac492c2f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -521,7 +521,7 @@ private void configureStateTransitionCommands() { CoralTargets.getClosestTarget(pose.get()) .getTranslation() .getDistance(pose.get().getTranslation()) - > 0.2) + > 0.5) .debounce(0.15) .onTrue(forceState(SuperState.IDLE)); @@ -796,6 +796,10 @@ private Command extendWithClearance( return extendWithClearance(() -> elevatorExtension, () -> shoulderAngle, () -> wristAngle); } + private boolean shouldntTuck() { + return wrist.getAngle().getDegrees() < 100.0 || elevator.getExtensionMeters() > 0.75; + } + private Command extendWithClearance( DoubleSupplier elevatorExtension, Supplier shoulderAngle, @@ -816,11 +820,11 @@ private Command extendWithClearance( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle( ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0)), + this::shouldntTuck)), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0), + this::shouldntTuck), elevator.hold()) // .unless( // () -> @@ -834,11 +838,11 @@ private Command extendWithClearance( Commands.either( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0), + this::shouldntTuck), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0), + this::shouldntTuck), elevator.setExtension(elevatorExtension)) .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 537f247d..9d858008 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(165.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); From ea6ed26ad06a945b3db723c9270fbe34b055a569 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 22:00:02 -0700 Subject: [PATCH 044/126] =?UTF-8?q?adjust=20climb=20so=20it=20works=20?= =?UTF-8?q?=F0=9F=94=A5=F0=9F=94=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/subsystems/Superstructure.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ac492c2f..54a7ec89 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -754,6 +754,11 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.PRE_CLIMB) + .whileTrue( + Commands.parallel( + elevator.setExtension(0), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_HP_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS))) .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) .onTrue(funnel.unlatch()) // !! .and(climbConfReq) @@ -761,9 +766,14 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CLIMB) + .whileTrue( + Commands.parallel( + elevator.setExtension(0), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_HP_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS))) .whileTrue( climber - .setPositionSlow(1.35) + .setPositionSlow(1.3) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // May need more checks to see if canceling is safe From 6f807d980cf5de75d5f788ec742d9bf9151fa49f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 22:27:52 -0700 Subject: [PATCH 045/126] wait better for reextension --- .../frc/robot/subsystems/Superstructure.java | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 54a7ec89..193f5027 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -858,15 +858,23 @@ private Command extendWithClearance( .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // re-extend joints Commands.parallel( - shoulder.setTargetAngle(shoulderAngle), + shoulder + .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) + .unless( + () -> + shouldntTuck() + || shoulderAngle.get().getDegrees() + < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until(() -> wrist.isNearTarget()) + .andThen(shoulder.setTargetAngle(shoulderAngle)), wrist .hold() .until(() -> shoulder.isNearTarget()) .unless( () -> wristAngle.get().getDegrees() < 90.0 - || shoulder.getAngle().getDegrees() < 60.0 - || shoulder.isNearTarget()) + || shoulderAngle.get().getDegrees() + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))); } @@ -910,11 +918,23 @@ private Command extendWithClearanceSlow( .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), // re-extend joints Commands.parallel( - shoulder.setTargetAngle(shoulderAngle), + shoulder + .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) + .unless( + () -> + shouldntTuck() + || shoulderAngle.get().getDegrees() + < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until(() -> wrist.isNearTarget()) + .andThen(shoulder.setTargetAngle(shoulderAngle)), wrist .hold() .until(() -> shoulder.isNearTarget()) - .unless(() -> wristAngle.get().getDegrees() < 90.0) + .unless( + () -> + wristAngle.get().getDegrees() < 90.0 + || shoulderAngle.get().getDegrees() + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))); } From 06d5a8cd4b9cc634c9049c2452c2af1584466b64 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 22:57:41 -0700 Subject: [PATCH 046/126] make sure that robot waits to retract after coral score --- .../java/frc/robot/subsystems/Superstructure.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 193f5027..f903201e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -521,7 +521,7 @@ private void configureStateTransitionCommands() { CoralTargets.getClosestTarget(pose.get()) .getTranslation() .getDistance(pose.get().getTranslation()) - > 0.5) + > 0.3) .debounce(0.15) .onTrue(forceState(SuperState.IDLE)); @@ -530,6 +530,12 @@ private void configureStateTransitionCommands() { .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) .and(killVisionIK) + .and( + () -> + CoralTargets.getClosestTarget(pose.get()) + .getTranslation() + .getDistance(pose.get().getTranslation()) + > 0.3) .onTrue(forceState(SuperState.IDLE)); stateTriggers @@ -841,7 +847,12 @@ private Command extendWithClearance( // shoulder.getAngle().getDegrees() // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() // && wrist.getAngle().getDegrees() < 90.0) - .until(() -> shoulder.isNearTarget() && wrist.isNearTarget()) + .until( + () -> + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() + && wrist.isNearTarget()) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), // extend elevator Commands.parallel( From e289799e2c910f6802a09de0bc6c6b321f16d78a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 23:38:02 -0700 Subject: [PATCH 047/126] tune processor a bit --- src/main/java/frc/robot/Robot.java | 5 +- .../subsystems/ManipulatorSubsystem.java | 6 ++- .../frc/robot/subsystems/Superstructure.java | 50 ++++++++++++------- .../elevator/ElevatorSubsystem.java | 2 +- 4 files changed, 41 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2388b88b..184058e9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -779,7 +779,10 @@ public Robot() { Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) && elevator.isNearTarget() - && shoulder.isNearTarget()), + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + && wrist.isNearAngle( + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index da5d6020..fdb0dca0 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -130,7 +130,7 @@ public Command intakeCoral() { public Command intakeCoralAir(double vel) { return Commands.sequence( setVelocity(vel).until(() -> secondBBInputs.get), - setVelocity(1.0).until(() -> !firstBBInputs.get), + setVelocity(0.25).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } @@ -144,7 +144,9 @@ public Command intakeCoral(double vel) { public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) - .until(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + .until( + new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + .debounce(0.75)) .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f903201e..0673244d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -205,7 +205,7 @@ private void configureStateTransitionCommands() { ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) - .whileTrue(manipulator.intakeCoralAir(-6.0).repeatedly()) + .whileTrue(manipulator.intakeCoralAir(-9.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -715,8 +715,8 @@ private void configureStateTransitionCommands() { // PRE_PROCESSOR logic stateTriggers .get(SuperState.PRE_PROCESSOR) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) + .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) + .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) @@ -725,7 +725,7 @@ private void configureStateTransitionCommands() { // PRE_NET logic stateTriggers .get(SuperState.PRE_NET) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) + .whileTrue(manipulator.setVoltage(3 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) .whileTrue( Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), @@ -751,11 +751,11 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_ALGAE_PROCESSOR) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(manipulator.setVoltage(-ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)) - .and(() -> stateTimer.hasElapsed(1.0)) + .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) + .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) + .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) + .whileTrue(manipulator.setVoltage(-2.0)) + .and(() -> stateTimer.hasElapsed(2.0)) .onTrue(this.forceState(SuperState.IDLE)); stateTriggers @@ -902,12 +902,19 @@ private Command extendWithClearanceSlow( .until( () -> wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS)) - .andThen(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS)), + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees()) + .andThen( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle( + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + this::shouldntTuck)), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0), + this::shouldntTuck), elevator.hold()) // .unless( // () -> @@ -916,17 +923,24 @@ private Command extendWithClearanceSlow( // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - shoulder.getAngle().getDegrees() - < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees()), + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() + && wrist.isNearTarget()) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), // extend elevator Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + this::shouldntTuck), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> wrist.getAngle().getDegrees() < 90.0), + this::shouldntTuck), elevator.setExtensionSlow(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // re-extend joints Commands.parallel( shoulder @@ -947,7 +961,7 @@ private Command extendWithClearanceSlow( || shoulderAngle.get().getDegrees() > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) .andThen(wrist.setTargetAngle(wristAngle)), - elevator.setExtension(elevatorExtension))); + elevator.setExtensionSlow(elevatorExtension))); } public SuperState getState() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index f009bab5..8a592119 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -57,7 +57,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); - public static final double ALGAE_PROCESSOR_EXTENSION = 0.1; + public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); From 869c96acbc9ddebb411c9df3b51f8c093fd2ac99 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Wed, 2 Apr 2025 23:49:24 -0700 Subject: [PATCH 048/126] try to improve funnel behavior --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 184058e9..81fee299 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -314,7 +314,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) + .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.39))) : new RollerIOSim( 0.01, 5.8677, diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index fdb0dca0..a24a114a 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -130,7 +130,7 @@ public Command intakeCoral() { public Command intakeCoralAir(double vel) { return Commands.sequence( setVelocity(vel).until(() -> secondBBInputs.get), - setVelocity(0.25).until(() -> !firstBBInputs.get), + setVoltage(0.5).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } From 0a59d4781505331c66b1831b6e00eac5814eeba4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 08:32:06 -0700 Subject: [PATCH 049/126] reenable ground piece in auto --- src/main/java/frc/robot/Autos.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 656f4e5b..482af859 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -167,13 +167,13 @@ public Command LOtoJ() { runPath(routine, startPos, endPos, nextPos, steps); } - // final var groundTraj = routine.trajectory("LtoAGround"); + final var groundTraj = routine.trajectory("LtoAGround"); - // routine - // .observe(steps.get("PLOtoL").done()) - // .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) - // .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) - // .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + routine + .observe(steps.get("PLOtoL").done()) + .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) + .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) + .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); return routine.cmd().alongWith(Commands.print("auto :(")); } From 6265e8571bb6dd663521c54d524e319d748ed637 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 08:39:54 -0700 Subject: [PATCH 050/126] turn off ik by default and when in l1 --- src/main/java/frc/robot/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 81fee299..73c02417 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -187,7 +187,7 @@ public static enum AlgaeScoreTarget { private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; - @AutoLogOutput private boolean killVisionIK = false; + @AutoLogOutput private boolean killVisionIK = true; @AutoLogOutput private boolean haveAutosGenerated = false; @@ -490,9 +490,9 @@ public static enum AlgaeScoreTarget { operator.rightBumper(), operator.leftBumper(), new Trigger(() -> killVisionIK) - // .or(() -> currentTarget == ReefTarget.L4) + .or(() -> currentTarget == ReefTarget.L1) .or(() -> DriverStation.isAutonomous()), - () -> MathUtil.clamp(operator.getLeftY(), -1.0, 0.0)); + () -> MathUtil.clamp(operator.getLeftY(), -1.0, 1.0)); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); From eaa7ec4153c8996e49949e61b6ac29649296f4d8 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 15:41:21 -0700 Subject: [PATCH 051/126] make ground intake traj wait for prev scoring --- src/main/java/frc/robot/Autos.java | 5 ++++- src/main/java/frc/robot/Robot.java | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 482af859..d2e46c36 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -170,10 +170,13 @@ public Command LOtoJ() { final var groundTraj = routine.trajectory("LtoAGround"); routine - .observe(steps.get("PLOtoL").done()) + .observe(steps.get("PLOtoL").recentlyDone()) + .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak()) .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + + routine.observe(groundTraj.done()).onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); return routine.cmd().alongWith(Commands.print("auto :(")); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 73c02417..8a895437 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -471,7 +471,7 @@ public static enum AlgaeScoreTarget { ? AutoAim.BLUE_REEF_CENTER : AutoAim.RED_REEF_CENTER) .getNorm() - < 3.5 + < 3.0 && DriverStation.isAutonomous()), driver.leftTrigger(), driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), @@ -630,6 +630,9 @@ public Robot() { new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoPreScore = false)); + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) + .onTrue(Commands.runOnce(() -> Autos.autoGroundIntake = false)); + new Trigger( () -> { var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); From 5c97978c76f863ab6ae9547dc932f1fd90f4ab6d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 16:09:06 -0700 Subject: [PATCH 052/126] try to avoid slams when entering score_coral when alr extended --- src/main/java/frc/robot/Autos.java | 7 ++- .../frc/robot/subsystems/Superstructure.java | 43 ++++++++++++++++--- .../elevator/ElevatorSubsystem.java | 3 +- .../shoulder/ShoulderSubsystem.java | 3 +- .../subsystems/wrist/WristSubsystem.java | 3 +- 5 files changed, 46 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index d2e46c36..4457841c 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -171,12 +171,15 @@ public Command LOtoJ() { routine .observe(steps.get("PLOtoL").recentlyDone()) + .onTrue(scoreInAuto(() -> steps.get("PLOtoL").getFinalPose().get())) .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak()) .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); - - routine.observe(groundTraj.done()).onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); + + routine + .observe(groundTraj.done()) + .onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); return routine.cmd().alongWith(Commands.print("auto :(")); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0673244d..eeae32fb 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -491,12 +491,38 @@ private void configureStateTransitionCommands() { // pose.get(), // ExtensionKinematics.getExtensionForLevel( // reefTarget.get())))), - this.extendWithClearance( + Commands.either( + this.holdExtension( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), + this.extendWithClearance( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.getExtensionForLevel(reefTarget.get()))) + elevator.isNearExtension( + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + .elevatorHeightMeters() + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get())) + .elevatorHeightMeters()) + && shoulder.isNearAngle( + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + .shoulderAngle() + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get())) + .shoulderAngle())) // // End // () -> // MathUtil.isNear( @@ -890,6 +916,13 @@ private Command extendWithClearance( elevator.setExtension(elevatorExtension))); } + private Command holdExtension(Supplier state) { + return Commands.parallel( + elevator.setExtension(() -> state.get().elevatorHeightMeters()), + shoulder.setTargetAngle(() -> state.get().shoulderAngle()), + wrist.setTargetAngle(() -> state.get().wristAngle())); + } + private Command extendWithClearanceSlow( DoubleSupplier elevatorExtension, Supplier shoulderAngle, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 8a592119..beb817aa 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -132,8 +132,7 @@ public Command setExtension(DoubleSupplier meters) { () -> { io.setTarget(meters.getAsDouble(), MAX_ACCELERATION); setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); + Logger.recordOutput("Elevator/Setpoint", setpoint); }); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 0c4e9dd4..fdc4bd6e 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -100,8 +100,7 @@ public Command setTargetAngle(final Supplier target) { () -> { io.setMotorPosition(target.get()); setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); + Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); }); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 9d858008..cc8b44e1 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -76,8 +76,7 @@ public Command setTargetAngle(final Supplier target) { () -> { io.setMotorPosition(target.get()); setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); + Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); }); } From 31358819e57bd3b2164b8a7289c92af0b6da1404 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 17:43:44 -0700 Subject: [PATCH 053/126] fix hop when switching from clearance to ik hold --- .../robot/subsystems/ExtensionKinematics.java | 42 ++++- .../frc/robot/subsystems/Superstructure.java | 156 ++++++++++-------- 2 files changed, 123 insertions(+), 75 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 7fc40a6a..54eae9f4 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -163,9 +163,45 @@ public static Command holdStateCommand( final LinearFilter wristFilter = LinearFilter.movingAverage(8); return Commands.runOnce( () -> { - elevatorFilter.reset(); - shoulderFilter.reset(); - wristFilter.reset(); + elevatorFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters() + }, + new double[0]); + shoulderFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations() + }, + new double[0]); + wristFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations() + }, + new double[0]); }) .andThen( Commands.parallel( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index eeae32fb..3844aa78 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -460,11 +460,12 @@ private void configureStateTransitionCommands() { .get(SuperState.PRE_L4) .whileTrue( this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L4_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L4_EXTENSION))) + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.L4_EXTENSION + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.L4_EXTENSION)) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)) .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -824,12 +825,16 @@ private Command extendWithClearance(ExtensionState extension) { extension.elevatorHeightMeters(), extension.shoulderAngle(), extension.wristAngle()); } - private Command extendWithClearance(Supplier extension) { + public Command extendWithClearance(Supplier extension) { return extendWithClearance( () -> extension.get().elevatorHeightMeters(), () -> extension.get().shoulderAngle(), () -> extension.get().wristAngle()) - .until(() -> elevator.isNearExtension(extension.get().elevatorHeightMeters())) + .until( + () -> + elevator.isNearExtension(extension.get().elevatorHeightMeters()) + && shoulder.isNearAngle(extension.get().shoulderAngle()) + && wrist.isNearAngle(extension.get().wristAngle())) .andThen(ExtensionKinematics.holdStateCommand(elevator, shoulder, wrist, extension)); } @@ -847,73 +852,80 @@ private Command extendWithClearance( Supplier shoulderAngle, Supplier wristAngle) { return Commands.sequence( - // Retract shoulder + wrist - Commands.parallel( + // Retract shoulder + wrist + Commands.parallel( + shoulder + .run(() -> {}) + .until( + () -> + wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees()) + .andThen( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle( + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + this::shouldntTuck)), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + this::shouldntTuck), + elevator.hold()) + // .unless( + // () -> + // shoulder.getAngle().getDegrees() + // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() + // && wrist.getAngle().getDegrees() < 90.0) + .until( + () -> + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees() + && wrist.isNearTarget()) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), + // extend elevator + Commands.parallel( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + this::shouldntTuck), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + this::shouldntTuck), + elevator.setExtension(elevatorExtension)) + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), + // re-extend joints + Commands.parallel( shoulder - .run(() -> {}) - .until( + .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) + .unless( () -> - wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 - > shoulder.getAngle().getDegrees()) - .andThen( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle( - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck)), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), - elevator.hold()) - // .unless( - // () -> - // shoulder.getAngle().getDegrees() - // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() - // && wrist.getAngle().getDegrees() < 90.0) - .until( - () -> - shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), - // extend elevator - Commands.parallel( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), - elevator.setExtension(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), - // re-extend joints - Commands.parallel( - shoulder - .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) - .unless( - () -> - shouldntTuck() - || shoulderAngle.get().getDegrees() - < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .until(() -> wrist.isNearTarget()) - .andThen(shoulder.setTargetAngle(shoulderAngle)), - wrist - .hold() - .until(() -> shoulder.isNearTarget()) - .unless( - () -> - wristAngle.get().getDegrees() < 90.0 - || shoulderAngle.get().getDegrees() - > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .andThen(wrist.setTargetAngle(wristAngle)), - elevator.setExtension(elevatorExtension))); + shouldntTuck() + || shoulderAngle.get().getDegrees() + < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until(() -> wrist.isNearTarget()) + .andThen(shoulder.setTargetAngle(shoulderAngle)), + wrist + .hold() + .until(new Trigger(() -> shoulder.isNearTarget()).debounce(0.040)) + .unless( + () -> + wristAngle.get().getDegrees() < 90.0 + || shoulderAngle.get().getDegrees() + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .andThen(wrist.setTargetAngle(wristAngle)), + elevator.setExtension(elevatorExtension))) + .finallyDo( + (interrupted) -> { + if (interrupted) { + System.out.println("interrupted clearance extend"); + } + }); } private Command holdExtension(Supplier state) { From 368665bcd5f8331d75d67d60b2e9b42e753a9eeb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 17:46:48 -0700 Subject: [PATCH 054/126] allow cancelling out of l4 --- .../java/frc/robot/subsystems/Superstructure.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3844aa78..b38ad4cc 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -460,12 +460,11 @@ private void configureStateTransitionCommands() { .get(SuperState.PRE_L4) .whileTrue( this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L4_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L4_EXTENSION)) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)) + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.L4_EXTENSION + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.L4_EXTENSION))) .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From e1ba93ad59e63ad26db7663b584c3a18d7dccb27 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 17:53:51 -0700 Subject: [PATCH 055/126] delay wrist movement in some cases --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index b38ad4cc..67dba794 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -911,12 +911,16 @@ private Command extendWithClearance( .andThen(shoulder.setTargetAngle(shoulderAngle)), wrist .hold() - .until(new Trigger(() -> shoulder.isNearTarget()).debounce(0.040)) + .until( + new Trigger(() -> shoulder.isNearAngle(shoulderAngle.get())) + .debounce(0.040)) + .withTimeout(0.5) .unless( () -> wristAngle.get().getDegrees() < 90.0 || shoulderAngle.get().getDegrees() - > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees() + + 5) .andThen(wrist.setTargetAngle(wristAngle)), elevator.setExtension(elevatorExtension))) .finallyDo( From 911d3734d7d3b573951eaf55e0340ad708ae050f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 19:23:22 -0700 Subject: [PATCH 056/126] try to avoid l4 fucking itself again --- .../java/frc/robot/subsystems/Superstructure.java | 15 ++++++++++----- .../subsystems/shoulder/ShoulderSubsystem.java | 2 +- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 67dba794..188c5ae1 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -902,12 +902,17 @@ private Command extendWithClearance( Commands.parallel( shoulder .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) - .unless( + // .unless( + // () -> + // shouldntTuck() + // || shoulderAngle.get().getDegrees() + // < + // ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until( () -> - shouldntTuck() - || shoulderAngle.get().getDegrees() - < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .until(() -> wrist.isNearTarget()) + wrist.isNearTarget() + && (wrist.getAngle().getDegrees() < 120.0 + || !elevator.isNearExtension(1.4, 0.25))) .andThen(shoulder.setTargetAngle(shoulderAngle)), wrist .hold() diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index fdc4bd6e..4fb543a5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -49,7 +49,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(40.0); + public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); From 72dbd07fdd82ef765b3327fba1c2aee07f69d9c4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 19:26:58 -0700 Subject: [PATCH 057/126] reduce skipping steps elevator tolerance --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 188c5ae1..403056ec 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -884,7 +884,7 @@ private Command extendWithClearance( || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // extend elevator Commands.parallel( Commands.either( From 58a54117d3683787f840e9adbe016c5c657daf4a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 20:10:54 -0700 Subject: [PATCH 058/126] try to avoid l1 killing itself --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 24 ++++++++++--------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8a895437..e1231708 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -334,7 +334,7 @@ public static enum AlgaeScoreTarget { 12, WristIOReal.getDefaultConfiguration() .withSlot0( - new Slot0Configs().withKP(1000.0).withKD(10.0).withKS(0.3).withKV(3.6)) + new Slot0Configs().withKP(1000.0).withKD(30.0).withKS(0.3).withKV(3.6)) .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 403056ec..14002562 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -842,8 +842,10 @@ private Command extendWithClearance( return extendWithClearance(() -> elevatorExtension, () -> shoulderAngle, () -> wristAngle); } - private boolean shouldntTuck() { - return wrist.getAngle().getDegrees() < 100.0 || elevator.getExtensionMeters() > 0.75; + private boolean shouldntTuck(Rotation2d shoulderAngle) { + return wrist.getAngle().getDegrees() < 100.0 + || elevator.getExtensionMeters() > 0.75 + || shoulderAngle.getDegrees() > 60.0; } private Command extendWithClearance( @@ -866,11 +868,11 @@ private Command extendWithClearance( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle( ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck)), + () -> shouldntTuck(shoulderAngle.get()))), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), elevator.hold()) // .unless( // () -> @@ -890,11 +892,11 @@ private Command extendWithClearance( Commands.either( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), elevator.setExtension(elevatorExtension)) .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), @@ -963,11 +965,11 @@ private Command extendWithClearanceSlow( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle( ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck)), + () -> shouldntTuck(shoulderAngle.get()))), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), elevator.hold()) // .unless( // () -> @@ -986,11 +988,11 @@ private Command extendWithClearanceSlow( Commands.either( shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), Commands.either( wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - this::shouldntTuck), + () -> shouldntTuck(shoulderAngle.get())), elevator.setExtensionSlow(elevatorExtension)) .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), @@ -1000,7 +1002,7 @@ private Command extendWithClearanceSlow( .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) .unless( () -> - shouldntTuck() + shouldntTuck(shoulderAngle.get()) || shoulderAngle.get().getDegrees() < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) .until(() -> wrist.isNearTarget()) From cff7bbcff51b0802ae8212b9cca9239c35ed3f67 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 20:58:01 -0700 Subject: [PATCH 059/126] =?UTF-8?q?tune=20hp=20intake=20more=20again=20?= =?UTF-8?q?=F0=9F=98=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 6 ++++-- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index a24a114a..96dfc3dd 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -129,8 +129,10 @@ public Command intakeCoral() { public Command intakeCoralAir(double vel) { return Commands.sequence( - setVelocity(vel).until(() -> secondBBInputs.get), - setVoltage(0.5).until(() -> !firstBBInputs.get), + setVelocity(vel) + .until(() -> secondBBInputs.get) + .finallyDo(() -> io.setPosition(Rotation2d.fromRotations(0.63))), + setVoltage(0.25).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 14002562..688d6335 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -205,7 +205,7 @@ private void configureStateTransitionCommands() { ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) - .whileTrue(manipulator.intakeCoralAir(-9.0).repeatedly()) + .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> From 79073457abe83b0847f4d58d66e8c02d21ac8997 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 21:15:27 -0700 Subject: [PATCH 060/126] add manual zeroing --- src/main/java/frc/robot/Robot.java | 8 ++++++++ .../frc/robot/subsystems/elevator/ElevatorSubsystem.java | 5 +++++ .../java/frc/robot/subsystems/wrist/WristSubsystem.java | 5 +++++ 3 files changed, 18 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e1231708..953538f8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -609,6 +609,14 @@ public Robot() { shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))); + SmartDashboard.putData( + "Manual Zero Extension", + Commands.runOnce( + () -> { + elevator.resetExtension(0.0); + wrist.resetPosition(Rotation2d.k180deg); + })); + // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index beb817aa..b412b20d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -269,4 +269,9 @@ public boolean isNearExtension(double expected) { public boolean isNearExtension(double expected, double toleranceMeters) { return MathUtil.isNear(expected, inputs.positionMeters, toleranceMeters); } + + public void resetExtension(double extension) { + io.resetEncoder(extension); + hasZeroed = true; + } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index cc8b44e1..f74e9def 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -144,4 +144,9 @@ public Command currentZero(Supplier shoulderInputs) io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); })); } + + public void resetPosition(Rotation2d angle) { + io.resetEncoder(angle); + hasZeroed = true; + } } From 00befae53ed3e41b3554d62a9d65f8a30966aef2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 21:41:25 -0700 Subject: [PATCH 061/126] tune auto to be slower and smoother --- src/main/deploy/choreo/LOtoJ.traj | 132 +++++++++++++++-------------- src/main/deploy/choreo/PLOtoK.traj | 120 +++++++++++++------------- src/main/deploy/choreo/PLOtoL.traj | 120 +++++++++++++------------- src/main/deploy/choreo/choreo.chor | 10 ++- src/main/java/frc/robot/Autos.java | 26 +++--- src/main/java/frc/robot/Robot.java | 2 +- 6 files changed, 215 insertions(+), 195 deletions(-) diff --git a/src/main/deploy/choreo/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 5e328416..5ad5aebe 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":4.484625190496445, "y":4.0126113295555115, "r":3.25}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.484625190496445 m", "val":4.484625190496445}, "y":{"exp":"4.0126113295555115 m", "val":4.0126113295555115}, "r":{"exp":"3.25 m", "val":3.25}}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], "targetDt":{ @@ -36,66 +36,68 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.87859,1.805], + "waypoints":[0.0,0.96573,2.04281], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.49989, "ay":-2.10796, "alpha":0.00896, "fx":[-73.55183,-73.59107,-73.57728,-73.53804], "fy":[-34.48806,-34.40435,-34.4341,-34.51771]}, - {"t":0.03254, "x":7.10893, "y":6.23445, "heading":-2.35619, "vx":-0.14643, "vy":-0.06859, "omega":0.00029, "ax":-4.49955, "ay":-2.10781, "alpha":0.00923, "fx":[-73.54597,-73.58637,-73.57218,-73.53178], "fy":[-34.48644,-34.40026,-34.43089,-34.51697]}, - {"t":0.06508, "x":7.10178, "y":6.2311, "heading":-2.35618, "vx":-0.29284, "vy":-0.13718, "omega":0.00059, "ax":-4.49917, "ay":-2.10764, "alpha":0.00953, "fx":[-73.53938,-73.58108,-73.56644,-73.52474], "fy":[-34.48462,-34.39565,-34.42728,-34.51613]}, - {"t":0.09762, "x":7.08987, "y":6.22552, "heading":-2.35617, "vx":-0.43925, "vy":-0.20576, "omega":0.0009, "ax":-4.49875, "ay":-2.10745, "alpha":0.00987, "fx":[-73.5319,-73.57509,-73.55993,-73.51675], "fy":[-34.48255,-34.39043,-34.42319,-34.51519]}, - {"t":0.13016, "x":7.0732, "y":6.21771, "heading":-2.35614, "vx":-0.58564, "vy":-0.27434, "omega":0.00122, "ax":-4.49826, "ay":-2.10724, "alpha":0.01025, "fx":[-73.52335,-73.56823,-73.55249,-73.50761], "fy":[-34.48019,-34.38446,-34.41851,-34.51411]}, - {"t":0.1627, "x":7.05176, "y":6.20767, "heading":-2.3561, "vx":-0.73201, "vy":-0.34291, "omega":0.00156, "ax":-4.49769, "ay":-2.10699, "alpha":0.0107, "fx":[-73.51347,-73.56031,-73.5439,-73.49707], "fy":[-34.47745,-34.37756,-34.41311,-34.51286]}, - {"t":0.19524, "x":7.02556, "y":6.19539, "heading":-2.35605, "vx":-0.87837, "vy":-0.41147, "omega":0.0019, "ax":-4.49703, "ay":-2.1067, "alpha":0.01122, "fx":[-73.50195,-73.55107,-73.53387,-73.48475], "fy":[-34.47427,-34.36951,-34.4068,-34.5114]}, - {"t":0.22778, "x":6.9946, "y":6.18089, "heading":-2.35598, "vx":-1.0247, "vy":-0.48003, "omega":0.00227, "ax":-4.49625, "ay":-2.10635, "alpha":0.01184, "fx":[-73.48831,-73.54014,-73.52201,-73.47019], "fy":[-34.4705,-34.35999,-34.39934,-34.50968]}, - {"t":0.26032, "x":6.95887, "y":6.16415, "heading":-2.35591, "vx":-1.17101, "vy":-0.54857, "omega":0.00266, "ax":-4.49532, "ay":-2.10594, "alpha":0.01258, "fx":[-73.47193,-73.52701,-73.50777,-73.45269], "fy":[-34.46596,-34.34855,-34.39039,-34.50761]}, - {"t":0.29286, "x":6.91839, "y":6.14519, "heading":-2.35582, "vx":-1.31729, "vy":-0.6171, "omega":0.00306, "ax":-4.49417, "ay":-2.10544, "alpha":0.01349, "fx":[-73.45188,-73.51094,-73.49034,-73.43129], "fy":[-34.46042,-34.33455,-34.37943,-34.50509]}, - {"t":0.3254, "x":6.87314, "y":6.12399, "heading":-2.35572, "vx":-1.46353, "vy":-0.68561, "omega":0.0035, "ax":-4.49273, "ay":-2.1048, "alpha":0.01463, "fx":[-73.42678,-73.49083,-73.46853,-73.40449], "fy":[-34.45348,-34.31701,-34.36572,-34.50193]}, - {"t":0.35794, "x":6.82314, "y":6.10057, "heading":-2.35561, "vx":-1.60973, "vy":-0.7541, "omega":0.00398, "ax":-4.49089, "ay":-2.10399, "alpha":0.0161, "fx":[-73.39445,-73.46492,-73.44044,-73.36998], "fy":[-34.44453,-34.29442,-34.34806,-34.49786]}, - {"t":0.39048, "x":6.76838, "y":6.07491, "heading":-2.35548, "vx":-1.75586, "vy":-0.82256, "omega":0.0045, "ax":-4.48842, "ay":-2.1029, "alpha":0.01807, "fx":[-73.35122,-73.43029,-73.40291,-73.32385], "fy":[-34.43258,-34.26423,-34.32446,-34.49242]}, - {"t":0.42302, "x":6.70887, "y":6.04703, "heading":-2.35533, "vx":-1.90191, "vy":-0.89099, "omega":0.00509, "ax":-4.48495, "ay":-2.10137, "alpha":0.02084, "fx":[-73.29047,-73.38164,-73.35022,-73.25907], "fy":[-34.41578,-34.22179,-34.29133,-34.4848]}, - {"t":0.45556, "x":6.64461, "y":6.01693, "heading":-2.35517, "vx":-2.04786, "vy":-0.95937, "omega":0.00577, "ax":-4.47972, "ay":-2.09907, "alpha":0.02502, "fx":[-73.19887,-73.30831,-73.27086,-73.16146], "fy":[-34.39046,-34.15781,-34.24141,-34.47332]}, - {"t":0.4881, "x":6.5756, "y":5.9846, "heading":-2.35498, "vx":-2.19363, "vy":-1.02767, "omega":0.00658, "ax":-4.47094, "ay":-2.0952, "alpha":0.03206, "fx":[-73.04489,-73.18511,-73.13774,-72.99758], "fy":[-34.3479,-34.05027,-34.15766,-34.45407]}, - {"t":0.52064, "x":6.50185, "y":5.95005, "heading":-2.35477, "vx":-2.33911, "vy":-1.09585, "omega":0.00763, "ax":-4.45313, "ay":-2.08734, "alpha":0.04646, "fx":[-72.73195,-72.93502,-72.86822,-72.66528], "fy":[-34.26147,-33.83177,-33.98799,-34.41514]}, - {"t":0.55318, "x":6.42338, "y":5.91329, "heading":-2.35452, "vx":-2.48402, "vy":-1.16377, "omega":0.00914, "ax":-4.39758, "ay":-2.06285, "alpha":0.09257, "fx":[-71.7508,-72.15502,-72.03306,-71.62963], "fy":[-33.99243,-33.1452,-33.46015,-34.29714]}, - {"t":0.58572, "x":6.34022, "y":5.87432, "heading":-2.35422, "vx":-2.62712, "vy":-1.2309, "omega":0.01215, "ax":3.22933, "ay":1.48263, "alpha":1.33572, "fx":[56.17561,50.92751,49.14119,54.9295], "fy":[22.47913,30.87511,26.46115,17.13753]}, - {"t":0.61826, "x":6.25644, "y":5.83506, "heading":-2.35383, "vx":-2.52203, "vy":-1.18266, "omega":0.05562, "ax":4.41293, "ay":2.06445, "alpha":0.07275, "fx":[72.25229,71.9358,72.03357,72.35044], "fy":[33.54179,34.20252,33.9611,33.29415]}, - {"t":0.6508, "x":6.17671, "y":5.79766, "heading":-2.35202, "vx":-2.37844, "vy":-1.11548, "omega":0.05798, "ax":4.45754, "ay":2.0866, "alpha":0.03527, "fx":[72.92343,72.76966,72.821,72.97483], "fy":[34.00902,34.3335,34.21555,33.88961]}, - {"t":0.68334, "x":6.10167, "y":5.76247, "heading":-2.35013, "vx":-2.23339, "vy":-1.04758, "omega":0.05913, "ax":4.47315, "ay":2.09435, "alpha":0.02237, "fx":[73.15945,73.06187,73.09545,73.19305], "fy":[34.17311,34.3798,34.30452,34.09724]}, - {"t":0.71588, "x":6.03137, "y":5.72949, "heading":-2.3482, "vx":-2.08783, "vy":-0.97943, "omega":0.05986, "ax":4.4811, "ay":2.0983, "alpha":0.01583, "fx":[73.27989,73.21085,73.23505,73.30409], "fy":[34.25684,34.40337,34.34979,34.20297]}, - {"t":0.74843, "x":5.9658, "y":5.69873, "heading":-2.34626, "vx":-1.94201, "vy":-0.91115, "omega":0.06037, "ax":4.48592, "ay":2.1007, "alpha":0.01187, "fx":[73.35295,73.3012,73.31958,73.37134], "fy":[34.30764,34.41761,34.37721,34.26706]}, - {"t":0.78097, "x":5.90498, "y":5.67019, "heading":-2.34429, "vx":-1.79604, "vy":-0.84279, "omega":0.06076, "ax":4.48915, "ay":2.1023, "alpha":0.00922, "fx":[73.402,73.36183,73.37625,73.41642], "fy":[34.34173,34.42714,34.3956,34.31008]}, - {"t":0.81351, "x":5.84892, "y":5.64388, "heading":-2.34231, "vx":-1.64996, "vy":-0.77438, "omega":0.06106, "ax":4.49147, "ay":2.10345, "alpha":0.00731, "fx":[73.4372,73.40535,73.41689,73.44874], "fy":[34.36619,34.43395,34.40879,34.34096]}, - {"t":0.84605, "x":5.7976, "y":5.6198, "heading":-2.34033, "vx":-1.50381, "vy":-0.70594, "omega":0.0613, "ax":4.49322, "ay":2.10432, "alpha":0.00588, "fx":[73.46369,73.4381,73.44746,73.47305], "fy":[34.38459,34.43906,34.41872,34.36421]}, - {"t":0.87859, "x":5.75105, "y":5.59794, "heading":-2.33833, "vx":-1.3576, "vy":-0.63746, "omega":0.06149, "ax":0.05832, "ay":-0.10122, "alpha":1.51111, "fx":[6.73272,0.62347,-4.79461,1.25233], "fy":[-1.50228,4.37411,-1.81317,-7.67764]}, - {"t":0.90947, "x":5.70915, "y":5.57821, "heading":-2.33643, "vx":-1.3558, "vy":-0.64059, "omega":0.10815, "ax":0.02493, "ay":-0.05267, "alpha":1.38357, "fx":[5.68474,0.13097,-4.86759,0.68199], "fy":[-0.70223,4.65891,-1.02368,-6.37725]}, - {"t":0.94035, "x":5.6673, "y":5.5584, "heading":-2.33309, "vx":-1.35503, "vy":-0.64221, "omega":0.15088, "ax":0.00696, "ay":-0.01468, "alpha":1.25968, "fx":[4.91612,-0.16085,-4.6853,0.38505], "fy":[-0.07236,4.78604,-0.40866,-5.26465]}, - {"t":0.97123, "x":5.62546, "y":5.53856, "heading":-2.32844, "vx":-1.35481, "vy":-0.64267, "omega":0.18978, "ax":0.00131, "ay":-0.00276, "alpha":1.13959, "fx":[4.3644,-0.25506,-4.31524,0.29161], "fy":[0.13102,4.5023,-0.22172,-4.59228]}, - {"t":1.00211, "x":5.58362, "y":5.51871, "heading":-2.32257, "vx":-1.35477, "vy":-0.64275, "omega":0.22497, "ax":0.0003, "ay":-0.0003, "alpha":1.02307, "fx":[3.91265,-0.30751,-3.87093,0.28512], "fy":[0.17857,4.07794,-0.1884,-4.08778]}, - {"t":1.03299, "x":5.54178, "y":5.49887, "heading":-2.31563, "vx":-1.35476, "vy":-0.64276, "omega":0.25656, "ax":0.55648, "ay":0.27049, "alpha":0.8834, "fx":[12.47808,8.60369,5.84522,9.46226], "fy":[4.56691,7.97959,4.28226,0.85941]}, - {"t":1.06387, "x":5.50021, "y":5.47915, "heading":-2.3077, "vx":-1.33758, "vy":-0.63441, "omega":0.28384, "ax":1.79229, "ay":0.85037, "alpha":0.66647, "fx":[31.62643,29.04632,26.87604,29.65333], "fy":[13.88336,16.70929,13.94088,11.07409]}, - {"t":1.09475, "x":5.45976, "y":5.45996, "heading":-2.29894, "vx":-1.28223, "vy":-0.60815, "omega":0.30442, "ax":1.79971, "ay":0.85382, "alpha":0.57317, "fx":[31.42802,29.14591,27.35953,29.75361], "fy":[13.95822,16.37976,13.97137,11.52394]}, - {"t":1.12563, "x":5.42102, "y":5.44159, "heading":-2.28954, "vx":-1.22666, "vy":-0.58178, "omega":0.32212, "ax":1.80216, "ay":0.85494, "alpha":0.48421, "fx":[31.15922,29.20603,27.72678,29.75547], "fy":[13.99603,16.02311,13.96581,11.9217]}, - {"t":1.15651, "x":5.384, "y":5.42403, "heading":-2.27959, "vx":-1.171, "vy":-0.55538, "omega":0.33708, "ax":1.80339, "ay":0.85549, "alpha":0.39663, "fx":[30.87415,29.26137,28.06271,29.72942], "fy":[14.0204,15.66051,13.95626,12.30527]}, - {"t":1.18739, "x":5.3487, "y":5.40729, "heading":-2.26918, "vx":-1.11531, "vy":-0.52896, "omega":0.34932, "ax":1.80412, "ay":0.85581, "alpha":0.30958, "fx":[30.58288,29.3186,28.38524,29.68909], "fy":[14.03561,15.29531,13.9494,12.68302]}, - {"t":1.21827, "x":5.31512, "y":5.39136, "heading":-2.2584, "vx":-1.0596, "vy":-0.50253, "omega":0.35888, "ax":1.80461, "ay":0.85602, "alpha":0.22267, "fx":[30.28837,29.38016,28.70077,29.63874], "fy":[14.04279,14.92818,13.94764,13.05826]}, - {"t":1.24915, "x":5.28326, "y":5.37625, "heading":-2.24731, "vx":-1.00387, "vy":-0.4761, "omega":0.36576, "ax":1.80497, "ay":0.85616, "alpha":0.13563, "fx":[29.99167,29.44727,29.01223,29.57999], "fy":[14.04224,14.55911,13.95218,13.43271]}, - {"t":1.28003, "x":5.25312, "y":5.36196, "heading":-2.23602, "vx":-0.94814, "vy":-0.44966, "omega":0.36995, "ax":1.80523, "ay":0.85626, "alpha":0.04824, "fx":[29.69302,29.5207,29.32135,29.51351], "fy":[14.03399,14.18779,13.96369,13.80757]}, - {"t":1.31091, "x":5.2247, "y":5.34848, "heading":-2.22459, "vx":-0.89239, "vy":-0.42322, "omega":0.37144, "ax":1.80544, "ay":0.85634, "alpha":-0.03969, "fx":[29.3924,29.60097,29.62922,29.43961], "fy":[14.01802,13.81389,13.98256,14.18369]}, - {"t":1.34179, "x":5.19801, "y":5.33582, "heading":-2.21312, "vx":-0.83664, "vy":-0.39678, "omega":0.37021, "ax":1.80561, "ay":0.8564, "alpha":-0.12834, "fx":[29.0896,29.68844,29.93668,29.35841], "fy":[13.99424,13.43694,14.00903,14.56188]}, - {"t":1.37268, "x":5.17303, "y":5.32397, "heading":-2.20169, "vx":-0.78088, "vy":-0.37033, "omega":0.36625, "ax":1.80575, "ay":0.85645, "alpha":-0.21785, "fx":[28.78435,29.78337,30.24441,29.27002], "fy":[13.96267,13.05655,14.04321,14.9428]}, - {"t":1.40356, "x":5.14978, "y":5.31295, "heading":-2.19038, "vx":-0.72512, "vy":-0.34388, "omega":0.35952, "ax":1.80586, "ay":0.85649, "alpha":-0.30841, "fx":[28.47625,29.88591,30.55302,29.1745], "fy":[13.92334,12.67221,14.08509,15.3271]}, - {"t":1.43444, "x":5.12825, "y":5.30273, "heading":-2.17928, "vx":-0.66935, "vy":-0.31743, "omega":0.35, "ax":1.80596, "ay":0.85652, "alpha":-0.40014, "fx":[28.16493,29.99615,30.86304,29.07197], "fy":[13.87637,12.28346,14.13457,15.7154]}, - {"t":1.46532, "x":5.10844, "y":5.29334, "heading":-2.16847, "vx":-0.61358, "vy":-0.29098, "omega":0.33764, "ax":1.80605, "ay":0.85655, "alpha":-0.49322, "fx":[27.84989,30.11409,31.17504,28.96258], "fy":[13.82197,11.88977,14.19143,16.10835]}, - {"t":1.4962, "x":5.09035, "y":5.28476, "heading":-2.15804, "vx":-0.55781, "vy":-0.26453, "omega":0.32241, "ax":1.80612, "ay":0.85657, "alpha":-0.58779, "fx":[27.53063,30.23963,31.48956,28.84657], "fy":[13.76043,11.49059,14.25538,16.50657]}, - {"t":1.52708, "x":5.07399, "y":5.277, "heading":-2.14809, "vx":-0.50203, "vy":-0.23808, "omega":0.30426, "ax":1.80618, "ay":0.85659, "alpha":-0.68401, "fx":[27.20657,30.37263,31.80721,28.7242], "fy":[13.69215,11.0853,14.32598,16.91077]}, - {"t":1.55796, "x":5.05935, "y":5.27006, "heading":-2.13869, "vx":-0.44626, "vy":-0.21163, "omega":0.28314, "ax":1.80624, "ay":0.8566, "alpha":-0.78203, "fx":[26.87706,30.51282,32.12859,28.59586], "fy":[13.61765,10.67326,14.40271,17.32165]}, - {"t":1.58884, "x":5.04643, "y":5.26393, "heading":-2.12995, "vx":-0.39048, "vy":-0.18518, "omega":0.25899, "ax":1.80629, "ay":0.85662, "alpha":-0.88203, "fx":[26.54138,30.6599,32.45437,28.462], "fy":[13.53756,10.25373,14.48494,17.73997]}, - {"t":1.61972, "x":5.03523, "y":5.25862, "heading":-2.12195, "vx":-0.3347, "vy":-0.15872, "omega":0.23175, "ax":1.80634, "ay":0.85663, "alpha":-0.98415, "fx":[26.19873,30.81342,32.78527,28.32319], "fy":[13.45269,9.82592,14.57188,18.16654]}, - {"t":1.6506, "x":5.02575, "y":5.25413, "heading":-2.11479, "vx":-0.27892, "vy":-0.13227, "omega":0.20136, "ax":1.80638, "ay":0.85664, "alpha":-1.08858, "fx":[25.84823,30.97287,33.12211,28.18009], "fy":[13.36395,9.38893,14.66262,18.60225]}, - {"t":1.68148, "x":5.018, "y":5.25045, "heading":-2.10858, "vx":-0.22314, "vy":-0.10582, "omega":0.16774, "ax":1.80641, "ay":0.85665, "alpha":-1.19549, "fx":[25.48887,31.13761,33.46573,28.03351], "fy":[13.27249,8.94179,14.75611,19.04802]}, - {"t":1.71236, "x":5.01197, "y":5.24759, "heading":-2.1034, "vx":-0.16736, "vy":-0.07936, "omega":0.13083, "ax":1.80645, "ay":0.85666, "alpha":-1.30507, "fx":[25.11955,31.30687,33.8171,27.8844], "fy":[13.17962,8.48342,14.85111,19.50486]}, - {"t":1.74324, "x":5.00767, "y":5.24555, "heading":-2.09936, "vx":-0.11157, "vy":-0.05291, "omega":0.09052, "ax":1.80648, "ay":0.85667, "alpha":-1.41752, "fx":[24.73903,31.47979,34.1773,27.73383], "fy":[13.0869,8.01262,14.94619,19.97385]}, - {"t":1.77412, "x":5.00508, "y":5.24433, "heading":-2.09656, "vx":-0.05579, "vy":-0.02645, "omega":0.04675, "ax":1.80651, "ay":0.85668, "alpha":-1.51391, "fx":[24.34243,31.64265,34.54806,27.59864], "fy":[13.02489,7.67471,15.00881,20.31166]}, - {"t":1.805, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17863, "ay":-1.95783, "alpha":0.00583, "fx":[-68.30429,-68.32983,-68.32085,-68.29531], "fy":[-32.02433,-31.96985,-31.9892,-32.04364]}, + {"t":0.03449, "x":7.10883, "y":6.2344, "heading":-2.35619, "vx":-0.14412, "vy":-0.06753, "omega":0.0002, "ax":-4.17832, "ay":-1.95769, "alpha":0.00603, "fx":[-68.29893,-68.32535,-68.31607,-68.28965], "fy":[-32.02264,-31.96629,-31.9863,-32.04261]}, + {"t":0.06898, "x":7.10137, "y":6.23091, "heading":-2.35619, "vx":-0.28824, "vy":-0.13505, "omega":0.00041, "ax":-4.17796, "ay":-1.95753, "alpha":0.00626, "fx":[-68.29286,-68.32028,-68.31065,-68.28323], "fy":[-32.02072,-31.96225,-31.98302,-32.04144]}, + {"t":0.10347, "x":7.08895, "y":6.22508, "heading":-2.35617, "vx":-0.43234, "vy":-0.20256, "omega":0.00063, "ax":-4.17756, "ay":-1.95735, "alpha":0.00652, "fx":[-68.28591,-68.31447,-68.30445,-68.27589], "fy":[-32.01853,-31.95764,-31.97927,-32.04011]}, + {"t":0.13796, "x":7.07155, "y":6.21693, "heading":-2.35615, "vx":-0.57642, "vy":-0.27007, "omega":0.00085, "ax":-4.1771, "ay":-1.95714, "alpha":0.00682, "fx":[-68.2779,-68.30777,-68.29729,-68.26743], "fy":[-32.01599,-31.95231,-31.97495,-32.03857]}, + {"t":0.17245, "x":7.04918, "y":6.20645, "heading":-2.35612, "vx":-0.72049, "vy":-0.33758, "omega":0.00109, "ax":-4.17656, "ay":-1.95689, "alpha":0.00717, "fx":[-68.26854,-68.29994,-68.28894,-68.25754], "fy":[-32.01304,-31.94609,-31.96989,-32.03677]}, + {"t":0.20694, "x":7.02185, "y":6.19365, "heading":-2.35609, "vx":-0.86454, "vy":-0.40507, "omega":0.00133, "ax":-4.17592, "ay":-1.9566, "alpha":0.00759, "fx":[-68.25747,-68.29069,-68.27906,-68.24585], "fy":[-32.00954,-31.93874,-31.96392,-32.03465]}, + {"t":0.24143, "x":6.98955, "y":6.17851, "heading":-2.35604, "vx":-1.00857, "vy":-0.47255, "omega":0.00159, "ax":-4.17515, "ay":-1.95625, "alpha":0.00808, "fx":[-68.24418,-68.27957,-68.26719,-68.2318], "fy":[-32.00534,-31.9299,-31.95674,-32.0321]}, + {"t":0.27592, "x":6.95228, "y":6.16105, "heading":-2.35598, "vx":-1.15258, "vy":-0.54003, "omega":0.00187, "ax":-4.1742, "ay":-1.95583, "alpha":0.00869, "fx":[-68.22791,-68.26598,-68.25268,-68.21462], "fy":[-32.0002,-31.91909,-31.94797,-32.02898]}, + {"t":0.31041, "x":6.91004, "y":6.14126, "heading":-2.35592, "vx":-1.29655, "vy":-0.60748, "omega":0.00217, "ax":-4.17303, "ay":-1.95529, "alpha":0.00946, "fx":[-68.20755,-68.24896,-68.23452,-68.19312], "fy":[-31.99376,-31.90555,-31.93698,-32.02507]}, + {"t":0.3449, "x":6.86284, "y":6.11914, "heading":-2.35584, "vx":-1.44048, "vy":-0.67492, "omega":0.0025, "ax":-4.17151, "ay":-1.95461, "alpha":0.01044, "fx":[-68.18133,-68.22704,-68.21113,-68.16543], "fy":[-31.98547,-31.88812,-31.92284,-32.02005]}, + {"t":0.3794, "x":6.81068, "y":6.0947, "heading":-2.35576, "vx":-1.58435, "vy":-0.74234, "omega":0.00286, "ax":-4.16948, "ay":-1.95369, "alpha":0.01176, "fx":[-68.14629,-68.19775,-68.1799,-68.12844], "fy":[-31.9744,-31.86483,-31.90395,-32.01334]}, + {"t":0.41389, "x":6.75355, "y":6.06794, "heading":-2.35566, "vx":-1.72816, "vy":-0.80972, "omega":0.00327, "ax":-4.16664, "ay":-1.9524, "alpha":0.01361, "fx":[-68.09707,-68.15663,-68.13606,-68.07651], "fy":[-31.95885,-31.83212,-31.87743,-32.00392]}, + {"t":0.44838, "x":6.69147, "y":6.03885, "heading":-2.35555, "vx":-1.87187, "vy":-0.87706, "omega":0.00373, "ax":-4.16235, "ay":-1.95046, "alpha":0.0164, "fx":[-68.02292,-68.09469,-68.07006,-67.99831], "fy":[-31.93542,-31.78283,-31.8375,-31.98975]}, + {"t":0.48287, "x":6.62443, "y":6.00744, "heading":-2.35542, "vx":-2.01543, "vy":-0.94433, "omega":0.0043, "ax":-4.15516, "ay":-1.94721, "alpha":0.0211, "fx":[-67.89843,-67.99075,-67.95942,-67.86714], "fy":[-31.8961,-31.7001,-31.77056,-31.96599]}, + {"t":0.51736, "x":6.55245, "y":5.97371, "heading":-2.35527, "vx":-2.15874, "vy":-1.01149, "omega":0.00503, "ax":-4.1406, "ay":-1.94062, "alpha":0.03068, "fx":[-67.64604,-67.78019,-67.73573,-67.60164], "fy":[-31.81642,-31.53239,-31.63516,-31.918]}, + {"t":0.55185, "x":6.47553, "y":5.93767, "heading":-2.3551, "vx":-2.30156, "vy":-1.07843, "omega":0.00609, "ax":-4.09544, "ay":-1.92019, "alpha":0.06119, "fx":[-66.85996,-67.12714,-67.045,-66.77819], "fy":[-31.56973,-31.0088,-31.21566,-31.77171]}, + {"t":0.58634, "x":6.39371, "y":5.89933, "heading":-2.35489, "vx":-2.44281, "vy":-1.14466, "omega":0.0082, "ax":2.98228, "ay":1.38131, "alpha":1.02202, "fx":[51.39917,47.33681,45.9398,50.3424], "fy":[21.22059,27.65747,24.22716,17.22224]}, + {"t":0.62083, "x":6.31123, "y":5.86067, "heading":-2.3546, "vx":-2.33995, "vy":-1.09701, "omega":0.04345, "ax":4.10601, "ay":1.92246, "alpha":0.04948, "fx":[67.19943,66.98384,67.05127,67.26705], "fy":[31.28581,31.73694,31.5728,31.11855]}, + {"t":0.65532, "x":6.23297, "y":5.82398, "heading":-2.35311, "vx":-2.19833, "vy":-1.03071, "omega":0.04515, "ax":4.14352, "ay":1.94065, "alpha":0.02405, "fx":[67.77357,67.66858,67.70377,67.80879], "fy":[31.65528,31.87706,31.79681,31.57429]}, + {"t":0.68981, "x":6.15961, "y":5.78958, "heading":-2.35155, "vx":-2.05542, "vy":-0.96377, "omega":0.04598, "ax":4.15658, "ay":1.94698, "alpha":0.01535, "fx":[67.97411,67.90708,67.93015,67.9972], "fy":[31.78412,31.92621,31.87473,31.73234]}, + {"t":0.7243, "x":6.09119, "y":5.7575, "heading":-2.34996, "vx":-1.91206, "vy":-0.89662, "omega":0.04651, "ax":4.16322, "ay":1.95019, "alpha":0.01095, "fx":[68.07618,68.02837,68.04509,68.09291], "fy":[31.84968,31.9512,31.91431,31.81264]}, + {"t":0.75879, "x":6.02772, "y":5.72774, "heading":-2.34836, "vx":-1.76847, "vy":-0.82936, "omega":0.04689, "ax":4.16723, "ay":1.95214, "alpha":0.00829, "fx":[68.13801,68.10182,68.11462,68.15081], "fy":[31.88939,31.96631,31.93825,31.86124]}, + {"t":0.79328, "x":5.9692, "y":5.70029, "heading":-2.34674, "vx":-1.62474, "vy":-0.76203, "omega":0.04718, "ax":4.16993, "ay":1.95345, "alpha":0.00651, "fx":[68.17947,68.15108,68.16122,68.18962], "fy":[31.91602,31.97643,31.9543,31.89384]}, + {"t":0.82777, "x":5.91564, "y":5.67517, "heading":-2.34511, "vx":-1.48091, "vy":-0.69465, "omega":0.0474, "ax":4.17186, "ay":1.95438, "alpha":0.00523, "fx":[68.20922,68.1864,68.19461,68.21743], "fy":[31.93512,31.98367,31.96581,31.91722]}, + {"t":0.86226, "x":5.86705, "y":5.65238, "heading":-2.34348, "vx":-1.33702, "vy":-0.62725, "omega":0.04758, "ax":4.17331, "ay":1.95509, "alpha":0.00427, "fx":[68.23159,68.21297,68.21972,68.23834], "fy":[31.94948,31.98911,31.97446,31.93481]}, + {"t":0.89675, "x":5.82342, "y":5.6319, "heading":-2.34184, "vx":-1.19308, "vy":-0.55981, "omega":0.04773, "ax":4.17444, "ay":1.95564, "alpha":0.00352, "fx":[68.24903,68.23369,68.23929,68.25463], "fy":[31.96067,31.99334,31.98121,31.94853]}, + {"t":0.93124, "x":5.78475, "y":5.61376, "heading":-2.34019, "vx":-1.04911, "vy":-0.49236, "omega":0.04785, "ax":4.17535, "ay":1.95608, "alpha":0.00292, "fx":[68.26301,68.2503,68.25497,68.26769], "fy":[31.96964,31.99672,31.98662,31.95952]}, + {"t":0.96573, "x":5.75105, "y":5.59794, "heading":-2.33854, "vx":-0.9051, "vy":-0.4249, "omega":0.04795, "ax":0.04195, "ay":-0.07851, "alpha":1.15701, "fx":[5.11239,0.4484,-3.7255,0.90799], "fy":[-1.16555,3.3266,-1.4048,-5.88993]}, + {"t":1.00048, "x":5.71963, "y":5.58313, "heading":-2.33687, "vx":-0.90364, "vy":-0.42762, "omega":0.08815, "ax":0.00953, "ay":-0.02011, "alpha":1.0362, "fx":[4.13041,-0.05691,-3.8142,0.36374], "fy":[-0.2105,3.78377,-0.4485,-4.43995]}, + {"t":1.03522, "x":5.68824, "y":5.56826, "heading":-2.33381, "vx":-0.90331, "vy":-0.42832, "omega":0.12415, "ax":0.00102, "ay":-0.00215, "alpha":0.95776, "fx":[3.67126,-0.1913,-3.6341,0.22073], "fy":[0.09073,3.78355,-0.16128,-3.85343]}, + {"t":1.06997, "x":5.65685, "y":5.55338, "heading":-2.3295, "vx":-0.90327, "vy":-0.4284, "omega":0.15743, "ax":-0.00008, "ay":0.00016, "alpha":0.856, "fx":[3.28227,-0.20447,-3.28308,0.20019], "fy":[0.13179,3.39585,-0.12649,-3.39042]}, + {"t":1.10471, "x":5.62547, "y":5.53849, "heading":-2.32403, "vx":-0.90327, "vy":-0.42839, "omega":0.18717, "ax":-0.00013, "ay":0.00028, "alpha":0.76889, "fx":[2.92523,-0.19643,-2.92913,0.19161], "fy":[0.13921,3.07233,-0.13001,-3.06313]}, + {"t":1.13946, "x":5.59408, "y":5.52361, "heading":-2.31753, "vx":-0.90328, "vy":-0.42838, "omega":0.21389, "ax":-0.00013, "ay":0.00028, "alpha":0.68643, "fx":[2.62946,-0.19618,-2.63385,0.19195], "fy":[0.14198,2.72265,-0.13287,-2.71357]}, + {"t":1.1742, "x":5.5627, "y":5.50873, "heading":-2.31009, "vx":-0.90328, "vy":-0.42837, "omega":0.23774, "ax":-0.00014, "ay":0.00029, "alpha":0.59016, "fx":[2.23916,-0.18073,-2.24395,0.17646], "fy":[0.14076,2.3608,-0.1312,-2.35126]}, + {"t":1.20895, "x":5.53132, "y":5.49384, "heading":-2.30183, "vx":-0.90329, "vy":-0.42836, "omega":0.25824, "ax":-0.00014, "ay":0.00028, "alpha":0.52341, "fx":[2.00172,-0.18004,-2.0064,0.17588], "fy":[0.14061,2.07556,-0.13129,-2.06625]}, + {"t":1.24369, "x":5.49993, "y":5.47896, "heading":-2.29286, "vx":-0.90329, "vy":-0.42835, "omega":0.27643, "ax":-0.00013, "ay":0.00026, "alpha":0.42037, "fx":[1.58871,-0.15366,-1.59301,0.14975], "fy":[0.1281,1.68431,-0.11944,-1.67566]}, + {"t":1.27843, "x":5.46855, "y":5.46408, "heading":-2.28326, "vx":-0.9033, "vy":-0.42834, "omega":0.29103, "ax":-0.00011, "ay":0.00023, "alpha":0.36371, "fx":[1.38671,-0.14845,-1.39041,0.14489], "fy":[0.12246,1.44282,-0.1148,-1.43517]}, + {"t":1.31318, "x":5.43716, "y":5.44919, "heading":-2.27315, "vx":-0.9033, "vy":-0.42833, "omega":0.30367, "ax":-0.0001, "ay":0.0002, "alpha":0.25764, "fx":[0.96645,-0.10994,-0.96957,0.10672], "fy":[0.09693,1.03686,-0.09024,-1.03017]}, + {"t":1.34792, "x":5.40578, "y":5.43431, "heading":-2.26259, "vx":-0.9033, "vy":-0.42833, "omega":0.31262, "ax":-0.00008, "ay":0.00017, "alpha":0.20498, "fx":[0.7761,-0.09684,-0.77856,0.09396], "fy":[0.0836,0.81621,-0.07798,-0.81058]}, + {"t":1.38267, "x":5.37439, "y":5.41943, "heading":-2.25173, "vx":-0.90331, "vy":-0.42832, "omega":0.31974, "ax":-0.00008, "ay":0.00017, "alpha":0.09954, "fx":[0.36304,-0.04687,-0.36506,0.04375], "fy":[0.04503,0.41086,-0.0396,-0.40543]}, + {"t":1.41741, "x":5.34301, "y":5.40455, "heading":-2.24062, "vx":-0.90331, "vy":-0.42832, "omega":0.3232, "ax":-0.0001, "ay":0.00022, "alpha":0.04548, "fx":[0.16411,-0.02459,-0.16582,0.01949], "fy":[0.02266,0.19135,-0.01548,-0.18416]}, + {"t":1.45216, "x":5.31162, "y":5.38967, "heading":-2.22939, "vx":-0.90331, "vy":-0.42831, "omega":0.32478, "ax":-0.0001, "ay":0.00021, "alpha":-0.05673, "fx":[-0.23078,0.03341,0.23163,-0.04075], "fy":[-0.0289,-0.20294,0.03574,0.20978]}, + {"t":1.4869, "x":5.28024, "y":5.37479, "heading":-2.21811, "vx":-0.90332, "vy":-0.4283, "omega":0.32281, "ax":0.00017, "ay":-0.00023, "alpha":-0.11605, "fx":[-0.44458,0.06676,0.46075,-0.07171], "fy":[-0.07253,-0.4467,0.06487,0.43904]}, + {"t":1.52165, "x":5.24885, "y":5.3599, "heading":-2.20689, "vx":-0.90331, "vy":-0.42831, "omega":0.31878, "ax":0.7229, "ay":0.34342, "alpha":-0.19629, "fx":[11.10803,11.88902,12.5835,11.69139], "fy":[5.51197,4.8325,5.71618,6.3962]}, + {"t":1.55639, "x":5.2179, "y":5.34523, "heading":-2.19582, "vx":-0.87819, "vy":-0.41638, "omega":0.31196, "ax":1.79959, "ay":0.85325, "alpha":-0.19977, "fx":[28.73682,29.71501,30.05725,29.17027], "fy":[13.91918,13.07701,13.98661,14.81313]}, + {"t":1.59113, "x":5.18848, "y":5.33128, "heading":-2.18498, "vx":-0.81567, "vy":-0.38673, "omega":0.30502, "ax":1.80339, "ay":0.85505, "alpha":-0.26824, "fx":[28.57983,29.8311,30.35649,29.16031], "fy":[13.91916,12.80953,14.04375,15.1412]}, + {"t":1.62588, "x":5.16123, "y":5.31836, "heading":-2.17438, "vx":-0.75301, "vy":-0.35702, "omega":0.2957, "ax":1.80465, "ay":0.85564, "alpha":-0.32382, "fx":[28.41849,29.91544,30.56338,29.11299], "fy":[13.9008,12.58004,14.08172,15.39009]}, + {"t":1.66062, "x":5.13615, "y":5.30647, "heading":-2.16411, "vx":-0.69031, "vy":-0.32729, "omega":0.28445, "ax":1.80528, "ay":0.85594, "alpha":-0.39325, "fx":[28.19885,30.01318,30.80307,29.03644], "fy":[13.86712,12.287,14.12585,15.69207]}, + {"t":1.69537, "x":5.11326, "y":5.29561, "heading":-2.15423, "vx":-0.62759, "vy":-0.29756, "omega":0.27078, "ax":1.80566, "ay":0.85612, "alpha":-0.45529, "fx":[28.00022,30.10464,31.01193,28.95949], "fy":[13.83126,12.02454,14.16836,15.95946]}, + {"t":1.73011, "x":5.09254, "y":5.28579, "heading":-2.14482, "vx":-0.56485, "vy":-0.26781, "omega":0.25496, "ax":1.80591, "ay":0.85624, "alpha":-0.52499, "fx":[27.77411,30.20722,31.24227,28.86917], "fy":[13.78673,11.7288,14.21781,16.25795]}, + {"t":1.76486, "x":5.07401, "y":5.277, "heading":-2.13596, "vx":-0.5021, "vy":-0.23806, "omega":0.23672, "ax":1.80609, "ay":0.85632, "alpha":-0.59225, "fx":[27.55561,30.30972,31.46175,28.77747], "fy":[13.73981,11.44352,14.26859,16.54484]}, + {"t":1.7996, "x":5.05765, "y":5.26925, "heading":-2.12773, "vx":-0.43935, "vy":-0.20831, "omega":0.21615, "ax":1.80623, "ay":0.85638, "alpha":-0.6636, "fx":[27.32307,30.4196,31.69252,28.6782], "fy":[13.68744,11.14089,14.32414,16.84835]}, + {"t":1.83435, "x":5.04348, "y":5.26253, "heading":-2.12022, "vx":-0.3766, "vy":-0.17855, "omega":0.19309, "ax":1.80633, "ay":0.85643, "alpha":-0.73539, "fx":[27.08888,30.53228,31.92296,28.57617], "fy":[13.63256,10.83653,14.38183,17.15307]}, + {"t":1.86909, "x":5.03148, "y":5.25684, "heading":-2.11351, "vx":-0.31384, "vy":-0.1488, "omega":0.16754, "ax":1.80642, "ay":0.85647, "alpha":-0.80973, "fx":[26.84593,30.64992,32.16025,28.46969], "fy":[13.57463,10.52141,14.44233,17.46814]}, + {"t":1.90384, "x":5.02167, "y":5.25219, "heading":-2.10769, "vx":-0.25107, "vy":-0.11904, "omega":0.1394, "ax":1.80648, "ay":0.8565, "alpha":-0.88582, "fx":[26.59674,30.771,32.40215,28.3604], "fy":[13.51496,10.19878,14.5044,17.79043]}, + {"t":1.93858, "x":5.01404, "y":5.24857, "heading":-2.10285, "vx":-0.18831, "vy":-0.08928, "omega":0.10863, "ax":1.80654, "ay":0.85653, "alpha":-0.96416, "fx":[26.33942,30.89557,32.65052,28.24854], "fy":[13.45428,9.86639,14.56747,18.12214]}, + {"t":1.97332, "x":5.00858, "y":5.24599, "heading":-2.09908, "vx":-0.12554, "vy":-0.05952, "omega":0.07513, "ax":1.80659, "ay":0.85655, "alpha":-1.04479, "fx":[26.07346,31.02296,32.90586,28.13494], "fy":[13.39373,9.52382,14.63051,18.46367]}, + {"t":2.00807, "x":5.00531, "y":5.24443, "heading":-2.09647, "vx":-0.06277, "vy":-0.02976, "omega":0.03883, "ax":1.80663, "ay":0.85657, "alpha":-1.11751, "fx":[25.79595,31.14215,33.16956,28.03227], "fy":[13.34592,9.25013,14.67893,18.738]}, + {"t":2.04281, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index 340ec034..ce5aeaed 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,62 +30,64 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.24249,1.89266], + "waypoints":[0.0,1.31293,2.15325], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42298, "ay":-3.09524, "alpha":-0.01592, "fx":[55.89073,56.00508,56.02772,55.91319], "fy":[-50.67707,-50.55073,-50.52536,-50.65202]}, - {"t":0.03765, "x":1.55258, "y":7.5212, "heading":-0.93501, "vx":0.12888, "vy":-0.11654, "omega":-0.0006, "ax":3.42275, "ay":-3.0951, "alpha":-0.01626, "fx":[55.88554,56.00232,56.02544,55.90847], "fy":[-50.67633,-50.54731,-50.52139,-50.65074]}, - {"t":0.0753, "x":1.55986, "y":7.51462, "heading":-0.93503, "vx":0.25775, "vy":-0.23307, "omega":-0.00121, "ax":3.4225, "ay":-3.09494, "alpha":-0.01664, "fx":[55.87978,55.99925,56.0229,55.90322], "fy":[-50.6755,-50.54351,-50.51699,-50.64932]}, - {"t":0.11295, "x":1.57199, "y":7.50365, "heading":-0.93507, "vx":0.38661, "vy":-0.3496, "omega":-0.00184, "ax":3.42221, "ay":-3.09476, "alpha":-0.01706, "fx":[55.87333,55.99582,56.02006,55.89736], "fy":[-50.67458,-50.53928,-50.51206,-50.64773]}, - {"t":0.1506, "x":1.58897, "y":7.4883, "heading":-0.93514, "vx":0.51546, "vy":-0.46612, "omega":-0.00248, "ax":3.42189, "ay":-3.09456, "alpha":-0.01753, "fx":[55.86609,55.99196,56.01687,55.89077], "fy":[-50.67355,-50.53451,-50.50652,-50.64595]}, - {"t":0.18826, "x":1.6108, "y":7.46855, "heading":-0.93524, "vx":0.6443, "vy":-0.58264, "omega":-0.00314, "ax":3.42153, "ay":-3.09433, "alpha":-0.01807, "fx":[55.85788,55.98758,56.01326,55.88331], "fy":[-50.67238,-50.52911,-50.50024,-50.64392]}, - {"t":0.22591, "x":1.63748, "y":7.44442, "heading":-0.93535, "vx":0.77312, "vy":-0.69914, "omega":-0.00382, "ax":3.42112, "ay":-3.09407, "alpha":-0.01868, "fx":[55.84849,55.98259,56.00913,55.87478], "fy":[-50.67104,-50.52295,-50.49307,-50.6416]}, - {"t":0.26356, "x":1.66902, "y":7.41591, "heading":-0.9355, "vx":0.90193, "vy":-0.81563, "omega":-0.00452, "ax":3.42064, "ay":-3.09377, "alpha":-0.01939, "fx":[55.83768,55.97683,56.00437,55.86495], "fy":[-50.66951,-50.51585,-50.48479,-50.63893]}, - {"t":0.30121, "x":1.7054, "y":7.383, "heading":-0.93567, "vx":1.03072, "vy":-0.93212, "omega":-0.00525, "ax":3.42008, "ay":-3.09342, "alpha":-0.02021, "fx":[55.82506,55.97011,55.99883,55.85348], "fy":[-50.66771,-50.50756,-50.47514,-50.63581]}, - {"t":0.33886, "x":1.74663, "y":7.34572, "heading":-0.93587, "vx":1.15949, "vy":-1.04859, "omega":-0.00601, "ax":3.41943, "ay":-3.09301, "alpha":-0.02118, "fx":[55.81016,55.96217,55.99228,55.83994], "fy":[-50.6656,-50.49778,-50.46374,-50.63212]}, - {"t":0.37651, "x":1.79271, "y":7.30404, "heading":-0.93609, "vx":1.28824, "vy":-1.16504, "omega":-0.00681, "ax":3.41864, "ay":-3.09251, "alpha":-0.02235, "fx":[55.7923,55.95266,55.98442,55.8237], "fy":[-50.66306,-50.48606,-50.45007,-50.6277]}, - {"t":0.41416, "x":1.84364, "y":7.25799, "heading":-0.93635, "vx":1.41695, "vy":-1.28148, "omega":-0.00765, "ax":3.41768, "ay":-3.09191, "alpha":-0.02378, "fx":[55.7705,55.94105,55.97484,55.80387], "fy":[-50.65997,-50.47176,-50.43337,-50.62229]}, - {"t":0.45181, "x":1.89941, "y":7.20755, "heading":-0.93664, "vx":1.54563, "vy":-1.39789, "omega":-0.00855, "ax":3.41648, "ay":-3.09116, "alpha":-0.02556, "fx":[55.74327,55.92656,55.96287,55.77911], "fy":[-50.6561,-50.4539,-50.41253,-50.61555]}, - {"t":0.48946, "x":1.96003, "y":7.15272, "heading":-0.93696, "vx":1.67427, "vy":-1.51428, "omega":-0.00951, "ax":3.41494, "ay":-3.09019, "alpha":-0.02785, "fx":[55.70834,55.90797,55.94752,55.74732], "fy":[-50.65114,-50.43099,-50.38576,-50.60688]}, - {"t":0.52712, "x":2.02549, "y":7.09352, "heading":-0.93732, "vx":1.80284, "vy":-1.63063, "omega":-0.01056, "ax":3.41289, "ay":-3.0889, "alpha":-0.0309, "fx":[55.66188,55.88326,55.92708,55.70501], "fy":[-50.64455,-50.40053,-50.35013,-50.59535]}, - {"t":0.56477, "x":2.09579, "y":7.02993, "heading":-0.93772, "vx":1.93134, "vy":-1.74693, "omega":-0.01172, "ax":3.41004, "ay":-3.0871, "alpha":-0.03516, "fx":[55.59706,55.8488,55.89856,55.64592], "fy":[-50.63533,-50.35803,-50.30039,-50.57924]}, - {"t":0.60242, "x":2.17092, "y":6.96197, "heading":-0.93816, "vx":2.05973, "vy":-1.86316, "omega":-0.01305, "ax":3.40577, "ay":-3.08442, "alpha":-0.04154, "fx":[55.50032,55.7974,55.85596,55.55763], "fy":[-50.62156,-50.29464,-50.22608,-50.55515]}, - {"t":0.64007, "x":2.25088, "y":6.88964, "heading":-0.93865, "vx":2.18796, "vy":-1.97929, "omega":-0.01461, "ax":3.39871, "ay":-3.07997, "alpha":-0.0521, "fx":[55.34046,55.71254,55.78543,55.41138], "fy":[-50.5987,-50.18994,-50.10305,-50.51518]}, - {"t":0.67772, "x":2.33567, "y":6.81293, "heading":-0.9392, "vx":2.31593, "vy":-2.09526, "omega":-0.01657, "ax":3.3848, "ay":-3.0712, "alpha":-0.07301, "fx":[55.02581,55.54567,55.64606,55.12236], "fy":[-50.5533,-49.98407,-49.8601,-50.43589]}, - {"t":0.71537, "x":2.42527, "y":6.73186, "heading":-0.93982, "vx":2.44337, "vy":-2.21089, "omega":-0.01932, "ax":3.34461, "ay":-3.0458, "alpha":-0.13385, "fx":[54.12159,55.06675,55.2407,54.28269], "fy":[-50.41915,-49.3939,-49.156,-50.20279]}, - {"t":0.75302, "x":2.51963, "y":6.64646, "heading":-0.94055, "vx":2.5693, "vy":-2.32557, "omega":-0.02436, "ax":2.00182, "ay":-2.12377, "alpha":-2.4523, "fx":[28.16582,41.09782,38.75506,22.88519], "fy":[-43.783,-32.95583,-24.2922,-37.84709]}, - {"t":0.79067, "x":2.61779, "y":6.5574, "heading":-0.94147, "vx":2.64467, "vy":-2.40553, "omega":-0.11669, "ax":-3.361, "ay":3.01304, "alpha":-0.13185, "fx":[-55.49683,-54.56436,-54.40098,-55.32147], "fy":[48.62263,49.65633,49.88191,48.86911]}, - {"t":0.82832, "x":2.71498, "y":6.46896, "heading":-0.94586, "vx":2.51812, "vy":-2.29209, "omega":-0.12166, "ax":-3.39492, "ay":3.05633, "alpha":-0.06069, "fx":[-55.75946,-55.32726,-55.2429,-55.67252], "fy":[49.67292,50.15149,50.255,49.78096]}, - {"t":0.86597, "x":2.80739, "y":6.38483, "heading":-0.95044, "vx":2.3903, "vy":-2.17701, "omega":-0.12394, "ax":-3.4059, "ay":3.07038, "alpha":-0.03754, "fx":[-55.84157,-55.57385,-55.51899,-55.78573], "fy":[50.01389,50.31023,50.3751,50.08052]}, - {"t":0.90363, "x":2.89497, "y":6.30504, "heading":-0.95511, "vx":2.26206, "vy":-2.06141, "omega":-0.12536, "ax":-3.41133, "ay":3.07735, "alpha":-0.026, "fx":[-55.88133,-55.69583,-55.65647,-55.8415], "fy":[50.18304,50.38832,50.43405,50.22961]}, - {"t":0.94128, "x":2.97772, "y":6.22961, "heading":-0.95983, "vx":2.13362, "vy":-1.94555, "omega":-0.12634, "ax":-3.41457, "ay":3.0815, "alpha":-0.01907, "fx":[-55.90465,-55.76862,-55.73889,-55.87467], "fy":[50.28424,50.43475,50.46892,50.31886]}, - {"t":0.97893, "x":3.05563, "y":6.15854, "heading":-0.96458, "vx":2.00506, "vy":-1.82952, "omega":-0.12705, "ax":-3.41672, "ay":3.08426, "alpha":-0.01444, "fx":[-55.9199,-55.81697,-55.79387,-55.89666], "fy":[50.35165,50.46553,50.4919,50.37828]}, - {"t":1.01658, "x":3.12871, "y":6.09184, "heading":-0.96937, "vx":1.87642, "vy":-1.7134, "omega":-0.1276, "ax":-3.41825, "ay":3.08623, "alpha":-0.01112, "fx":[-55.93062,-55.85141,-55.83318,-55.91231], "fy":[50.39981,50.48745,50.50815,50.42067]}, - {"t":1.05423, "x":3.19693, "y":6.02952, "heading":-0.97417, "vx":1.74772, "vy":-1.5972, "omega":-0.12802, "ax":-3.4194, "ay":3.08771, "alpha":-0.00862, "fx":[-55.93854,-55.87717,-55.86271,-55.92404], "fy":[50.43596,50.50385,50.52022,50.45242]}, - {"t":1.09188, "x":3.26031, "y":5.97157, "heading":-0.97899, "vx":1.61897, "vy":-1.48094, "omega":-0.12834, "ax":-3.42029, "ay":3.08885, "alpha":-0.00667, "fx":[-55.94462,-55.89716,-55.88572,-55.93315], "fy":[50.46411,50.51661,50.52952,50.47707]}, - {"t":1.12953, "x":3.31884, "y":5.918, "heading":-0.98382, "vx":1.4902, "vy":-1.36464, "omega":-0.12859, "ax":-3.421, "ay":3.08977, "alpha":-0.00511, "fx":[-55.94942,-55.91311,-55.90416,-55.94046], "fy":[50.48666,50.52683,50.5369,50.49676]}, - {"t":1.16718, "x":3.37253, "y":5.86881, "heading":-0.98867, "vx":1.36139, "vy":-1.24831, "omega":-0.12878, "ax":-3.42158, "ay":3.09051, "alpha":-0.00383, "fx":[-55.95329,-55.92612,-55.91928,-55.94644], "fy":[50.50514,50.5352,50.54288,50.51284]}, - {"t":1.20483, "x":3.42136, "y":5.824, "heading":-0.99351, "vx":1.23257, "vy":-1.13195, "omega":-0.12893, "ax":-3.42206, "ay":3.09114, "alpha":-0.00276, "fx":[-55.95648,-55.93693,-55.9319,-55.95145], "fy":[50.52056,50.54219,50.54783,50.52621]}, - {"t":1.24249, "x":3.46534, "y":5.78357, "heading":-0.99837, "vx":1.10372, "vy":-1.01556, "omega":-0.12903, "ax":-1.79347, "ay":-1.82922, "alpha":-0.51189, "fx":[-29.18831,-27.19212,-29.49743,-31.40171], "fy":[-31.68933,-30.93732,-28.0663,-28.92436]}, - {"t":1.27499, "x":3.50027, "y":5.74959, "heading":-1.00256, "vx":1.04542, "vy":-1.07503, "omega":-0.14567, "ax":-0.45558, "ay":-0.43702, "alpha":-0.65351, "fx":[-7.98189,-4.93786,-6.923,-9.94863], "fy":[-9.61063,-7.72567,-4.65626,-6.58501]}, - {"t":1.3075, "x":3.53402, "y":5.71441, "heading":-1.0073, "vx":1.03061, "vy":-1.08924, "omega":-0.16692, "ax":-0.08038, "ay":-0.07587, "alpha":-0.38805, "fx":[-1.66253,0.1674,-0.9661,-2.79498], "fy":[-2.71349,-1.55952,0.23414,-0.92254]}, - {"t":1.34001, "x":3.56748, "y":5.67896, "heading":-1.01273, "vx":1.028, "vy":-1.0917, "omega":-0.17953, "ax":-0.01382, "ay":-0.01301, "alpha":-0.09976, "fx":[-0.31774,0.15433,-0.13418,-0.60624], "fy":[-0.59109,-0.29642,0.16574,-0.12897]}, - {"t":1.37252, "x":3.60089, "y":5.64347, "heading":-1.01856, "vx":1.02755, "vy":-1.09213, "omega":-0.18277, "ax":-0.00237, "ay":-0.00223, "alpha":0.18863, "fx":[0.13897,-0.7569,-0.2165,0.67938], "fy":[0.67806,0.12602,-0.75098,-0.19897]}, - {"t":1.40503, "x":3.63429, "y":5.60796, "heading":-1.0245, "vx":1.02747, "vy":-1.0922, "omega":-0.17664, "ax":-0.00041, "ay":-0.00038, "alpha":0.47768, "fx":[0.45419,-1.82277,-0.46749,1.80948], "fy":[1.80049,0.41604,-1.81298,-0.42856]}, - {"t":1.43754, "x":3.66769, "y":5.57246, "heading":-1.03025, "vx":1.02746, "vy":-1.09221, "omega":-0.16111, "ax":-0.00007, "ay":-0.00007, "alpha":0.7684, "fx":[0.75685,-2.91865,-0.75913,2.91637], "fy":[2.90097,0.69501,-2.90311,-0.69716]}, - {"t":1.47004, "x":3.70109, "y":5.53695, "heading":-1.03548, "vx":1.02745, "vy":-1.09221, "omega":-0.13613, "ax":-0.00001, "ay":-0.00001, "alpha":1.06179, "fx":[1.0682,-4.02656,-1.06859,4.02616], "fy":[4.00435,0.98277,-4.00472,-0.98315]}, - {"t":1.50255, "x":3.7345, "y":5.50144, "heading":-1.03991, "vx":1.02745, "vy":-1.09221, "omega":-0.10162, "ax":-0.00001, "ay":0.0, "alpha":1.35879, "fx":[1.38978,-5.14716,-1.39006,5.14688], "fy":[5.11867,1.28077,-5.11855,-1.28065]}, - {"t":1.53506, "x":3.7679, "y":5.46594, "heading":-1.04321, "vx":1.02745, "vy":-1.09221, "omega":-0.05745, "ax":-0.07403, "ay":0.07869, "alpha":1.65856, "fx":[0.49807,-7.47444,-2.94322,5.07885], "fy":[7.52547,2.87082,-4.9602,-0.29016]}, - {"t":1.56757, "x":3.80126, "y":5.43047, "heading":-1.04508, "vx":1.02505, "vy":-1.08966, "omega":-0.00353, "ax":-3.12096, "ay":3.31767, "alpha":0.0287, "fx":[-50.88787,-51.10815,-51.15581,-50.93489], "fy":[54.36555,54.15988,54.10912,54.31575]}, - {"t":1.60008, "x":3.83293, "y":5.3968, "heading":-1.04519, "vx":0.92359, "vy":-0.9818, "omega":-0.00259, "ax":-3.14452, "ay":3.34272, "alpha":0.01562, "fx":[-51.33317,-51.4538,-51.48069,-51.35987], "fy":[54.71694,54.60387,54.57703,54.69039]}, - {"t":1.63259, "x":3.86129, "y":5.36665, "heading":-1.04528, "vx":0.82137, "vy":-0.87314, "omega":-0.00209, "ax":-3.15179, "ay":3.35045, "alpha":0.0116, "fx":[-51.47083,-51.56058,-51.58077,-51.49092], "fy":[54.82539,54.74117,54.72142,54.8058]}, - {"t":1.6651, "x":3.88633, "y":5.34004, "heading":-1.04535, "vx":0.71891, "vy":-0.76422, "omega":-0.00171, "ax":-3.15532, "ay":3.3542, "alpha":0.00965, "fx":[-51.53775,-51.61247,-51.62936,-51.55457], "fy":[54.87808,54.80792,54.79156,54.86184]}, - {"t":1.6976, "x":3.90803, "y":5.31697, "heading":-1.0454, "vx":0.61633, "vy":-0.65518, "omega":-0.0014, "ax":-3.15741, "ay":3.35642, "alpha":0.0085, "fx":[-51.57731,-51.64314,-51.65807,-51.59218], "fy":[54.90922,54.84738,54.83301,54.89494]}, - {"t":1.73011, "x":3.9264, "y":5.29744, "heading":-1.04545, "vx":0.51369, "vy":-0.54607, "omega":-0.00112, "ax":-3.15879, "ay":3.35789, "alpha":0.00774, "fx":[-51.60344,-51.6634,-51.67701,-51.61701], "fy":[54.92977,54.87344,54.86038,54.91679]}, - {"t":1.76262, "x":3.94143, "y":5.28146, "heading":-1.04548, "vx":0.411, "vy":-0.43691, "omega":-0.00087, "ax":-3.15977, "ay":3.35893, "alpha":0.0072, "fx":[-51.62198,-51.67777,-51.69046,-51.63462], "fy":[54.94436,54.89193,54.8798,54.93229]}, - {"t":1.79513, "x":3.95312, "y":5.26904, "heading":-1.04551, "vx":0.30828, "vy":-0.32771, "omega":-0.00063, "ax":-3.1605, "ay":3.35971, "alpha":0.00679, "fx":[-51.63581,-51.6885,-51.70049,-51.64777], "fy":[54.95525,54.90574,54.89429,54.94386]}, - {"t":1.82764, "x":3.96147, "y":5.26016, "heading":-1.04553, "vx":0.20554, "vy":-0.21849, "omega":-0.00041, "ax":-3.16107, "ay":3.36031, "alpha":0.00648, "fx":[-51.64654,-51.69681,-51.70826,-51.65796], "fy":[54.96369,54.91644,54.90552,54.95282]}, - {"t":1.86015, "x":3.96649, "y":5.25483, "heading":-1.04555, "vx":0.10278, "vy":-0.10925, "omega":-0.0002, "ax":-3.16152, "ay":3.36079, "alpha":0.00623, "fx":[-51.6551,-51.70344,-51.71446,-51.66609], "fy":[54.97042,54.92497,54.91448,54.95997]}, - {"t":1.89266, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42221, "ay":-3.09664, "alpha":-0.00515, "fx":[55.9244,55.96144,55.96874,55.93168], "fy":[-50.64862,-50.60771,-50.59955,-50.6405]}, + {"t":0.03979, "x":1.55286, "y":7.52095, "heading":-0.93501, "vx":0.13616, "vy":-0.1232, "omega":-0.00021, "ax":3.42199, "ay":-3.09648, "alpha":-0.00529, "fx":[55.92031,55.95834,55.96583,55.92778], "fy":[-50.64676,-50.60476,-50.59638,-50.63842]}, + {"t":0.07957, "x":1.56099, "y":7.5136, "heading":-0.93501, "vx":0.2723, "vy":-0.2464, "omega":-0.00042, "ax":3.42175, "ay":-3.09631, "alpha":-0.00544, "fx":[55.91575,55.95487,55.96257,55.92343], "fy":[-50.64467,-50.60147,-50.59285,-50.63609]}, + {"t":0.11936, "x":1.57453, "y":7.50134, "heading":-0.93503, "vx":0.40844, "vy":-0.36959, "omega":-0.00063, "ax":3.42148, "ay":-3.09612, "alpha":-0.00561, "fx":[55.91061,55.95096,55.95891,55.91853], "fy":[-50.64232,-50.59776,-50.58886,-50.63347]}, + {"t":0.15914, "x":1.59349, "y":7.48419, "heading":-0.93506, "vx":0.54457, "vy":-0.49277, "omega":-0.00086, "ax":3.42118, "ay":-3.0959, "alpha":-0.00581, "fx":[55.90478,55.94653,55.95475,55.91298], "fy":[-50.63966,-50.59356,-50.58435,-50.6305]}, + {"t":0.19893, "x":1.61786, "y":7.46213, "heading":-0.93509, "vx":0.68068, "vy":-0.61594, "omega":-0.00109, "ax":3.42083, "ay":-3.09565, "alpha":-0.00603, "fx":[55.89812,55.94147,55.95,55.90663], "fy":[-50.63662,-50.58876,-50.57919,-50.6271]}, + {"t":0.23871, "x":1.64765, "y":7.43518, "heading":-0.93513, "vx":0.81678, "vy":-0.7391, "omega":-0.00133, "ax":3.42043, "ay":-3.09536, "alpha":-0.00629, "fx":[55.89044,55.93563,55.94452,55.8993], "fy":[-50.63311,-50.58321,-50.57323,-50.62318]}, + {"t":0.2785, "x":1.68285, "y":7.40332, "heading":-0.93519, "vx":0.95287, "vy":-0.86226, "omega":-0.00158, "ax":3.41996, "ay":-3.09502, "alpha":-0.00659, "fx":[55.88147,55.92881,55.93812,55.89075], "fy":[-50.62901,-50.57675,-50.56628,-50.61861]}, + {"t":0.31829, "x":1.72347, "y":7.36656, "heading":-0.93525, "vx":1.08893, "vy":-0.98539, "omega":-0.00184, "ax":3.4194, "ay":-3.09462, "alpha":-0.00694, "fx":[55.87087,55.92075,55.93056,55.88063], "fy":[-50.62417,-50.5691,-50.55806,-50.6132]}, + {"t":0.35807, "x":1.7695, "y":7.32491, "heading":-0.93532, "vx":1.22498, "vy":-1.10852, "omega":-0.00211, "ax":3.41873, "ay":-3.09414, "alpha":-0.00737, "fx":[55.85813,55.91108,55.92147,55.86849], "fy":[-50.61836,-50.55992,-50.5482,-50.6067]}, + {"t":0.39786, "x":1.82094, "y":7.27836, "heading":-0.93541, "vx":1.36099, "vy":-1.23162, "omega":-0.00241, "ax":3.41792, "ay":-3.09355, "alpha":-0.00789, "fx":[55.84257,55.89924,55.91037,55.85365], "fy":[-50.61125,-50.5487,-50.53613,-50.59876]}, + {"t":0.43764, "x":1.87779, "y":7.22691, "heading":-0.9355, "vx":1.49698, "vy":-1.3547, "omega":-0.00272, "ax":3.4169, "ay":-3.09282, "alpha":-0.00854, "fx":[55.8231,55.88445,55.89648,55.83507], "fy":[-50.60236,-50.53466,-50.52103,-50.58882]}, + {"t":0.47743, "x":1.94006, "y":7.17056, "heading":-0.93561, "vx":1.63292, "vy":-1.47775, "omega":-0.00306, "ax":3.41558, "ay":-3.09188, "alpha":-0.00938, "fx":[55.79804,55.86541,55.8786,55.81117], "fy":[-50.59092,-50.5166,-50.5016,-50.57603]}, + {"t":0.51722, "x":2.00773, "y":7.10932, "heading":-0.93573, "vx":1.76881, "vy":-1.60076, "omega":-0.00344, "ax":3.41383, "ay":-3.09062, "alpha":-0.01051, "fx":[55.7646,55.84001,55.85474,55.77926], "fy":[-50.57565,-50.49249,-50.47566,-50.55895]}, + {"t":0.557, "x":2.0808, "y":7.04319, "heading":-0.93587, "vx":1.90463, "vy":-1.72372, "omega":-0.00385, "ax":3.41138, "ay":-3.08885, "alpha":-0.01208, "fx":[55.71773,55.80441,55.82129,55.73451], "fy":[-50.55424,-50.45871,-50.43929,-50.53501]}, + {"t":0.59679, "x":2.15928, "y":6.97216, "heading":-0.93602, "vx":2.04036, "vy":-1.84662, "omega":-0.00433, "ax":3.40768, "ay":-3.08619, "alpha":-0.01445, "fx":[55.6473,55.75092,55.77101,55.66725], "fy":[-50.52208,-50.40794,-50.38461,-50.49901]}, + {"t":0.63657, "x":2.24316, "y":6.89625, "heading":-0.93619, "vx":2.17594, "vy":-1.9694, "omega":-0.00491, "ax":3.40151, "ay":-3.08176, "alpha":-0.01843, "fx":[55.52961,55.66156,55.68693,55.55474], "fy":[-50.46829,-50.32311,-50.29316,-50.43876]}, + {"t":0.67636, "x":2.33242, "y":6.81546, "heading":-0.93639, "vx":2.31127, "vy":-2.09201, "omega":-0.00564, "ax":3.3891, "ay":-3.07282, "alpha":-0.02645, "fx":[55.29311,55.48204,55.51773,55.32829], "fy":[-50.36006,-50.15267,-50.10913,-50.31738]}, + {"t":0.71614, "x":2.42706, "y":6.72979, "heading":-0.93661, "vx":2.44611, "vy":-2.21427, "omega":-0.00669, "ax":3.35133, "ay":-3.0456, "alpha":-0.05106, "fx":[54.57503,54.93707,55.00156,54.63764], "fy":[-50.02987,-49.63524,-49.5481,-49.94587]}, + {"t":0.75593, "x":2.52703, "y":6.63929, "heading":-0.93688, "vx":2.57944, "vy":-2.33544, "omega":-0.00873, "ax":-2.00945, "ay":1.57646, "alpha":-1.26235, "fx":[-35.15185,-28.41142,-30.82392,-37.01526], "fy":[20.21366,26.62332,31.04791,25.20342]}, + {"t":0.79572, "x":2.62806, "y":6.54762, "heading":-0.93723, "vx":2.49949, "vy":-2.27272, "omega":-0.05895, "ax":-3.36877, "ay":3.03455, "alpha":-0.04392, "fx":[-55.2569,-54.94547,-54.88982,-55.19988], "fy":[49.39969,49.74314,49.81722,49.4761]}, + {"t":0.8355, "x":2.72484, "y":6.4596, "heading":-0.93957, "vx":2.36547, "vy":-2.15199, "omega":-0.0607, "ax":-3.39688, "ay":3.06631, "alpha":-0.02137, "fx":[-55.62346,-55.47097,-55.44172,-55.59389], "fy":[50.02624,50.1946,50.23005,50.06226]}, + {"t":0.87529, "x":2.81627, "y":6.37641, "heading":-0.94199, "vx":2.23032, "vy":-2.02999, "omega":-0.06155, "ax":-3.40649, "ay":3.07718, "alpha":-0.01363, "fx":[-55.74804,-55.65055,-55.63126,-55.72861], "fy":[50.24089,50.34856,50.37123,50.26378]}, + {"t":0.91507, "x":2.90231, "y":6.29808, "heading":-0.94444, "vx":2.09479, "vy":-1.90756, "omega":-0.06209, "ax":-3.41134, "ay":3.08268, "alpha":-0.00971, "fx":[-55.81069,-55.74122,-55.72717,-55.79658], "fy":[50.34941,50.42615,50.4424,50.36578]}, + {"t":0.95486, "x":2.98295, "y":6.22462, "heading":-0.94691, "vx":1.95906, "vy":-1.78492, "omega":-0.06248, "ax":-3.41427, "ay":3.08599, "alpha":-0.00733, "fx":[-55.84836,-55.79592,-55.78513,-55.83753], "fy":[50.41496,50.47289,50.48526,50.4274]}, + {"t":0.99465, "x":3.05819, "y":6.15605, "heading":-0.94939, "vx":1.82323, "vy":-1.66214, "omega":-0.06277, "ax":-3.41622, "ay":3.08821, "alpha":-0.00572, "fx":[-55.87349,-55.83251,-55.82394,-55.86489], "fy":[50.45885,50.50413,50.51388,50.46865]}, + {"t":1.03443, "x":3.12802, "y":6.09237, "heading":-0.95189, "vx":1.68731, "vy":-1.53927, "omega":-0.06299, "ax":-3.41762, "ay":3.0898, "alpha":-0.00457, "fx":[-55.89143,-55.85871,-55.85177,-55.88447], "fy":[50.49031,50.52647,50.53434,50.49821]}, + {"t":1.07422, "x":3.19245, "y":6.03357, "heading":-0.9544, "vx":1.55134, "vy":-1.41634, "omega":-0.06318, "ax":-3.41867, "ay":3.09099, "alpha":-0.0037, "fx":[-55.90488,-55.87839,-55.87269,-55.89917], "fy":[50.51397,50.54324,50.54968,50.52043]}, + {"t":1.114, "x":3.25146, "y":5.97967, "heading":-0.95691, "vx":1.41532, "vy":-1.29336, "omega":-0.06332, "ax":-3.41949, "ay":3.09192, "alpha":-0.00302, "fx":[-55.91533,-55.89372,-55.889,-55.91061], "fy":[50.53241,50.5563,50.56161,50.53774]}, + {"t":1.15379, "x":3.30507, "y":5.93066, "heading":-0.95943, "vx":1.27927, "vy":-1.17035, "omega":-0.06344, "ax":-3.42015, "ay":3.09266, "alpha":-0.00248, "fx":[-55.92369,-55.90599,-55.90208,-55.91977], "fy":[50.5472,50.56676,50.57115,50.5516]}, + {"t":1.19357, "x":3.35326, "y":5.88654, "heading":-0.96195, "vx":1.1432, "vy":-1.0473, "omega":-0.06354, "ax":-3.42068, "ay":3.09327, "alpha":-0.00203, "fx":[-55.93052,-55.91603,-55.91279,-55.92727], "fy":[50.55931,50.57532,50.57896,50.56296]}, + {"t":1.23336, "x":3.39603, "y":5.84732, "heading":-0.96448, "vx":1.00711, "vy":-0.92424, "omega":-0.06362, "ax":-3.42113, "ay":3.09378, "alpha":-0.00165, "fx":[-55.9362,-55.92441,-55.92173,-55.93353], "fy":[50.56942,50.58246,50.58545,50.57242]}, + {"t":1.27315, "x":3.43339, "y":5.813, "heading":-0.96701, "vx":0.87099, "vy":-0.80115, "omega":-0.06369, "ax":-3.42151, "ay":3.09421, "alpha":-0.00133, "fx":[-55.94101,-55.93149,-55.92931,-55.93882], "fy":[50.57799,50.5885,50.59095,50.58043]}, + {"t":1.31293, "x":3.46534, "y":5.78357, "heading":-0.96955, "vx":0.73487, "vy":-0.67804, "omega":-0.06374, "ax":-1.18481, "ay":-1.19859, "alpha":-0.55591, "fx":[-19.52027,-17.16003,-19.24526,-21.55216], "fy":[-21.64942,-20.27563,-17.49848,-18.95514]}, + {"t":1.35113, "x":3.49254, "y":5.7568, "heading":-0.97198, "vx":0.68961, "vy":-0.72382, "omega":-0.08498, "ax":-0.08328, "ay":-0.079, "alpha":-0.61409, "fx":[-1.83056,0.99955,-0.8941,-3.72098], "fy":[-3.64021,-1.71427,1.06062,-0.87217]}, + {"t":1.38932, "x":3.51882, "y":5.72909, "heading":-0.97523, "vx":0.68643, "vy":-0.72684, "omega":-0.10843, "ax":-0.00494, "ay":-0.00466, "alpha":-0.5049, "fx":[-0.47315,1.85865,0.31173,-2.01994], "fy":[-2.00749,-0.42752,1.85528,0.27503]}, + {"t":1.42752, "x":3.54504, "y":5.70133, "heading":-0.97937, "vx":0.68624, "vy":-0.72702, "omega":-0.12772, "ax":-0.00029, "ay":-0.00028, "alpha":-0.39778, "fx":[-0.32025,1.52192,0.31072,-1.53145], "fy":[-1.52481,-0.28757,1.51582,0.27857]}, + {"t":1.46572, "x":3.57125, "y":5.67356, "heading":-0.98425, "vx":0.68623, "vy":-0.72703, "omega":-0.14291, "ax":-0.00002, "ay":-0.00002, "alpha":-0.29289, "fx":[-0.23803,1.12279,0.23747,-1.12335], "fy":[-1.11853,-0.21417,1.118,0.21364]}, + {"t":1.50391, "x":3.59746, "y":5.64579, "heading":-0.98971, "vx":0.68623, "vy":-0.72703, "omega":-0.1541, "ax":0.0, "ay":0.0, "alpha":-0.18964, "fx":[-0.15791,0.7264,0.15787,-0.72643], "fy":[-0.72323,-0.14249,0.7232,0.14245]}, + {"t":1.54211, "x":3.62367, "y":5.61802, "heading":-0.99559, "vx":0.68623, "vy":-0.72703, "omega":-0.16134, "ax":0.0, "ay":0.0, "alpha":-0.08746, "fx":[-0.07478,0.33464,0.07478,-0.33464], "fy":[-0.33312,-0.06768,0.33312,0.06768]}, + {"t":1.58031, "x":3.64989, "y":5.59025, "heading":-1.00175, "vx":0.68623, "vy":-0.72703, "omega":-0.16468, "ax":0.0, "ay":0.0, "alpha":0.01422, "fx":[0.01249,-0.05434,-0.01249,0.05434], "fy":[0.05408,0.01134,-0.05408,-0.01134]}, + {"t":1.6185, "x":3.6761, "y":5.56248, "heading":-1.00804, "vx":0.68623, "vy":-0.72703, "omega":-0.16414, "ax":0.0, "ay":0.0, "alpha":0.11598, "fx":[0.10466,-0.4426,-0.10466,0.4426], "fy":[0.44047,0.09527,-0.44047,-0.09527]}, + {"t":1.6567, "x":3.70231, "y":5.53471, "heading":-1.01431, "vx":0.68623, "vy":-0.72703, "omega":-0.15971, "ax":0.0, "ay":0.0, "alpha":0.2184, "fx":[0.20228,-0.83229,-0.20228,0.83229], "fy":[0.82817,0.18461,-0.82817,-0.18461]}, + {"t":1.69489, "x":3.72852, "y":5.50694, "heading":-1.02041, "vx":0.68623, "vy":-0.72703, "omega":-0.15137, "ax":0.0, "ay":0.0, "alpha":0.32205, "fx":[0.30571,-1.22558,-0.30571,1.22558], "fy":[1.21936,0.2797,-1.21936,-0.2797]}, + {"t":1.73309, "x":3.75473, "y":5.47917, "heading":-1.0262, "vx":0.68623, "vy":-0.72703, "omega":-0.13907, "ax":0.0, "ay":0.0, "alpha":0.42751, "fx":[0.41517,-1.62474,-0.41517,1.62474], "fy":[1.61627,0.38069,-1.61627,-0.38069]}, + {"t":1.77129, "x":3.78094, "y":5.4514, "heading":-1.03151, "vx":0.68623, "vy":-0.72703, "omega":-0.12274, "ax":0.0, "ay":0.0, "alpha":0.53536, "fx":[0.53066,-2.03208,-0.53066,2.03208], "fy":[2.02125,0.48754,-2.02125,-0.48754]}, + {"t":1.80948, "x":3.80715, "y":5.42363, "heading":-1.0362, "vx":0.68623, "vy":-0.72703, "omega":-0.10229, "ax":0.0, "ay":0.0, "alpha":0.64621, "fx":[0.65197,-2.45005,-0.65197,2.45005], "fy":[2.43673,0.59998,-2.43673,-0.59998]}, + {"t":1.84768, "x":3.83336, "y":5.39586, "heading":-1.0401, "vx":0.68623, "vy":-0.72703, "omega":-0.07761, "ax":0.0, "ay":0.0, "alpha":0.76067, "fx":[0.77864,-2.88121,-0.77865,2.88121], "fy":[2.8653,0.71751,-2.8653,-0.71751]}, + {"t":1.88588, "x":3.85958, "y":5.36809, "heading":-1.04307, "vx":0.68623, "vy":-0.72703, "omega":-0.04855, "ax":-0.00007, "ay":0.00007, "alpha":0.87934, "fx":[0.90877,-3.32942,-0.91111,3.3271], "fy":[3.31085,0.84052,-3.30846,-0.83813]}, + {"t":1.92407, "x":3.88579, "y":5.34032, "heading":-1.04492, "vx":0.68623, "vy":-0.72703, "omega":-0.01496, "ax":-2.15208, "ay":2.28004, "alpha":0.37433, "fx":[-34.1614,-36.44229,-36.23808,-33.88762], "fy":[38.79323,36.99332,35.72035,37.59013]}, + {"t":1.96227, "x":3.91043, "y":5.31421, "heading":-1.04549, "vx":0.60402, "vy":-0.63994, "omega":-0.00067, "ax":-3.15282, "ay":3.34028, "alpha":0.0067, "fx":[-51.51099,-51.56264,-51.57427,-51.52259], "fy":[54.63727,54.58866,54.57712,54.62578]}, + {"t":2.00046, "x":3.9312, "y":5.29221, "heading":-1.04552, "vx":0.4836, "vy":-0.51235, "omega":-0.00041, "ax":-3.16192, "ay":3.34992, "alpha":0.00375, "fx":[-51.67363,-51.70261,-51.70921,-51.68023], "fy":[54.7817,54.7544,54.748,54.77533]}, + {"t":2.03866, "x":3.94737, "y":5.27508, "heading":-1.04553, "vx":0.36283, "vy":-0.3844, "omega":-0.00027, "ax":-3.16499, "ay":3.35317, "alpha":0.00276, "fx":[-51.72848,-51.7498,-51.75468,-51.73335], "fy":[54.83039,54.81028,54.8056,54.82571]}, + {"t":2.07686, "x":3.95892, "y":5.26285, "heading":-1.04554, "vx":0.24193, "vy":-0.25632, "omega":-0.00016, "ax":-3.16653, "ay":3.3548, "alpha":0.00226, "fx":[-51.75601,-51.77349,-51.7775,-51.76001], "fy":[54.85483,54.83834,54.83451,54.851]}, + {"t":2.11505, "x":3.96585, "y":5.2555, "heading":-1.04555, "vx":0.12098, "vy":-0.12818, "omega":-0.00007, "ax":-3.16746, "ay":3.35579, "alpha":0.00196, "fx":[-51.77257,-51.78773,-51.79122,-51.77605], "fy":[54.86952,54.85521,54.85189,54.86621]}, + {"t":2.15325, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index 25bb77af..e1e0be04 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,61 +30,65 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.20207,1.89844], + "waypoints":[0.0,1.27141,2.18043], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0897, "ay":-3.42807, "alpha":-0.0103, "fx":[50.46517,50.5483,50.55626,50.47302], "fy":[-56.08355,-56.00865,-56.00131,-56.07632]}, - {"t":0.03756, "x":1.55233, "y":7.52098, "heading":-0.93501, "vx":0.11606, "vy":-0.12877, "omega":-0.00039, "ax":3.08951, "ay":-3.42789, "alpha":-0.01052, "fx":[50.4611,50.54593,50.55404,50.46911], "fy":[-56.08142,-56.00499,-55.99749,-56.07404]}, - {"t":0.07513, "x":1.55887, "y":7.51372, "heading":-0.93502, "vx":0.23212, "vy":-0.25754, "omega":-0.00078, "ax":3.08929, "ay":-3.42769, "alpha":-0.01075, "fx":[50.45658,50.54329,50.55159,50.46476], "fy":[-56.07905,-56.00093,-55.99326,-56.07151]}, - {"t":0.11269, "x":1.56977, "y":7.50163, "heading":-0.93505, "vx":0.34817, "vy":-0.3863, "omega":-0.00119, "ax":3.08905, "ay":-3.42746, "alpha":-0.01101, "fx":[50.45153,50.54035,50.54884,50.4599], "fy":[-56.0764,-55.99639,-55.98852,-56.06867]}, - {"t":0.15026, "x":1.58503, "y":7.4847, "heading":-0.93509, "vx":0.46421, "vy":-0.51505, "omega":-0.0016, "ax":3.08879, "ay":-3.42721, "alpha":-0.01131, "fx":[50.44584,50.53704,50.54576,50.45443], "fy":[-56.07343,-55.99128,-55.98318,-56.06548]}, - {"t":0.18782, "x":1.60465, "y":7.46294, "heading":-0.93515, "vx":0.58024, "vy":-0.64379, "omega":-0.00202, "ax":3.08848, "ay":-3.42692, "alpha":-0.01164, "fx":[50.4394,50.53328,50.54226,50.44824], "fy":[-56.07006,-55.98548,-55.97714,-56.06187]}, - {"t":0.22539, "x":1.62862, "y":7.43633, "heading":-0.93523, "vx":0.69625, "vy":-0.77253, "omega":-0.00246, "ax":3.08813, "ay":-3.42659, "alpha":-0.01203, "fx":[50.43203,50.52899,50.53826,50.44115], "fy":[-56.06621,-55.97887,-55.97023,-56.05773]}, - {"t":0.26295, "x":1.65695, "y":7.4049, "heading":-0.93532, "vx":0.81226, "vy":-0.90124, "omega":-0.00291, "ax":3.08773, "ay":-3.42621, "alpha":-0.01247, "fx":[50.42353,50.52403,50.53364,50.43298], "fy":[-56.06176,-55.97123,-55.96225,-56.05296]}, - {"t":0.30052, "x":1.68964, "y":7.36862, "heading":-0.93543, "vx":0.92825, "vy":-1.02995, "omega":-0.00338, "ax":3.08726, "ay":-3.42577, "alpha":-0.01298, "fx":[50.4136,50.51825,50.52826,50.42344], "fy":[-56.05657,-55.96232,-55.95294,-56.04739]}, - {"t":0.33808, "x":1.72669, "y":7.32752, "heading":-0.93556, "vx":1.04422, "vy":-1.15864, "omega":-0.00387, "ax":3.08671, "ay":-3.42525, "alpha":-0.01359, "fx":[50.40188,50.51142,50.5219,50.41216], "fy":[-56.05044,-55.95179,-55.94194,-56.04081]}, - {"t":0.37565, "x":1.7681, "y":7.28158, "heading":-0.9357, "vx":1.16017, "vy":-1.2873, "omega":-0.00438, "ax":3.08605, "ay":-3.42462, "alpha":-0.01432, "fx":[50.3878,50.50322,50.51426,50.39863], "fy":[-56.04309,-55.93915,-55.92873,-56.0329]}, - {"t":0.41321, "x":1.81385, "y":7.2308, "heading":-0.93587, "vx":1.2761, "vy":-1.41595, "omega":-0.00492, "ax":3.08523, "ay":-3.42385, "alpha":-0.01522, "fx":[50.37059,50.49319,50.50492,50.38208], "fy":[-56.0341,-55.92369,-55.91258,-56.02324]}, - {"t":0.45078, "x":1.86397, "y":7.1752, "heading":-0.93605, "vx":1.39199, "vy":-1.54456, "omega":-0.00549, "ax":3.08422, "ay":-3.42289, "alpha":-0.01634, "fx":[50.34907,50.48066,50.49325,50.36138], "fy":[-56.02286,-55.90438,-55.89238,-56.01116]}, - {"t":0.48834, "x":1.91843, "y":7.11476, "heading":-0.93626, "vx":1.50785, "vy":-1.67314, "omega":-0.0061, "ax":3.08291, "ay":-3.42165, "alpha":-0.01778, "fx":[50.3214,50.46455,50.47823,50.33476], "fy":[-56.0084,-55.87954,-55.86639,-55.99561]}, - {"t":0.5259, "x":1.97725, "y":7.0495, "heading":-0.93649, "vx":1.62366, "vy":-1.80168, "omega":-0.00677, "ax":3.08117, "ay":-3.42, "alpha":-0.01971, "fx":[50.2845,50.44307,50.4582,50.29923], "fy":[-55.98912,-55.84641,-55.83172,-55.97487]}, - {"t":0.56347, "x":2.04042, "y":6.97941, "heading":-0.93674, "vx":1.7394, "vy":-1.93015, "omega":-0.00751, "ax":3.07873, "ay":-3.41769, "alpha":-0.02241, "fx":[50.23282,50.41299,50.43013,50.24945], "fy":[-55.96212,-55.80002,-55.78314,-55.94581]}, - {"t":0.60103, "x":2.10793, "y":6.90449, "heading":-0.93703, "vx":1.85505, "vy":-2.05853, "omega":-0.00835, "ax":3.07506, "ay":-3.41423, "alpha":-0.02648, "fx":[50.15528,50.36787,50.38798,50.17468], "fy":[-55.9216,-55.73041,-55.71017,-55.90214]}, - {"t":0.6386, "x":2.17978, "y":6.82475, "heading":-0.93734, "vx":1.97057, "vy":-2.18679, "omega":-0.00935, "ax":3.06895, "ay":-3.40844, "alpha":-0.03327, "fx":[50.02599,50.29264,50.31761,50.04983], "fy":[-55.854,-55.61438,-55.58834,-55.8292]}, - {"t":0.67616, "x":2.25597, "y":6.7402, "heading":-0.93769, "vx":2.08585, "vy":-2.31482, "omega":-0.0106, "ax":3.0567, "ay":-3.39684, "alpha":-0.04694, "fx":[49.76732,50.14211,50.17635,49.79934], "fy":[-55.71854,-55.38225,-55.34394,-55.68266]}, - {"t":0.71373, "x":2.33648, "y":6.65085, "heading":-0.93809, "vx":2.20067, "vy":-2.44242, "omega":-0.01236, "ax":3.01979, "ay":-3.36184, "alpha":-0.08848, "fx":[48.99123,49.68975,49.74824,49.04198], "fy":[-55.30984,-54.68591,-54.6052,-55.23757]}, - {"t":0.75129, "x":2.42128, "y":6.55673, "heading":-0.93855, "vx":2.31411, "vy":-2.56871, "omega":-0.01569, "ax":-1.17911, "ay":1.07722, "alpha":-2.80881, "fx":[-22.81284,-8.23637,-16.70856,-29.34676], "fy":[6.19788,17.67164,28.38594,18.18641]}, - {"t":0.78886, "x":2.50738, "y":6.461, "heading":-0.93914, "vx":2.26982, "vy":-2.52824, "omega":-0.1212, "ax":-3.03383, "ay":3.35394, "alpha":-0.07567, "fx":[-49.92303,-49.32122,-49.27443,-49.87063], "fy":[54.52556,55.06809,55.13255,54.59622]}, - {"t":0.82642, "x":2.5905, "y":6.36839, "heading":-0.9437, "vx":2.15585, "vy":-2.40225, "omega":-0.12404, "ax":-3.06321, "ay":3.39219, "alpha":-0.03596, "fx":[-50.2355,-49.94749,-49.92057,-50.20729], "fy":[55.31079,55.5704,55.60006,55.34188]}, - {"t":0.86399, "x":2.66932, "y":6.28054, "heading":-0.94836, "vx":2.04078, "vy":-2.27483, "omega":-0.12539, "ax":-3.07318, "ay":3.40517, "alpha":-0.02243, "fx":[-50.33995,-50.15993,-50.14158,-50.32111], "fy":[55.57744,55.73971,55.75851,55.5968]}, - {"t":0.90155, "x":2.74382, "y":6.19749, "heading":-0.95307, "vx":1.92534, "vy":-2.14691, "omega":-0.12623, "ax":-3.0782, "ay":3.41171, "alpha":-0.01557, "fx":[-50.39201,-50.26694,-50.25338,-50.3782], "fy":[55.7119,55.82462,55.83803,55.72558]}, - {"t":0.93912, "x":2.81397, "y":6.11925, "heading":-0.95781, "vx":1.80971, "vy":-2.01875, "omega":-0.12682, "ax":-3.08121, "ay":3.41565, "alpha":-0.01141, "fx":[-50.42309,-50.33144,-50.32096,-50.41248], "fy":[55.79301,55.87562,55.88576,55.8033]}, - {"t":0.97668, "x":2.87978, "y":6.04583, "heading":-0.96257, "vx":1.69397, "vy":-1.89045, "omega":-0.12725, "ax":-3.08323, "ay":3.41828, "alpha":-0.00861, "fx":[-50.44369,-50.37456,-50.36627,-50.43533], "fy":[55.84732,55.90963,55.91753,55.85531]}, - {"t":1.01424, "x":2.94123, "y":5.97723, "heading":-0.96735, "vx":1.57815, "vy":-1.76204, "omega":-0.12757, "ax":-3.08467, "ay":3.42017, "alpha":-0.00659, "fx":[-50.45831,-50.40541,-50.39879,-50.45165], "fy":[55.88625,55.93393,55.94018,55.89255]}, - {"t":1.05181, "x":2.99834, "y":5.91345, "heading":-0.97214, "vx":1.46227, "vy":-1.63356, "omega":-0.12782, "ax":-3.08576, "ay":3.42158, "alpha":-0.00506, "fx":[-50.46922,-50.42858,-50.42328,-50.46389], "fy":[55.91553,55.95216,55.95713,55.92053]}, - {"t":1.08937, "x":3.05109, "y":5.8545, "heading":-0.97694, "vx":1.34636, "vy":-1.50503, "omega":-0.12801, "ax":-3.0866, "ay":3.42268, "alpha":-0.00387, "fx":[-50.47765,-50.4466,-50.4424,-50.47343], "fy":[55.93837,55.96635,55.97027,55.94232]}, - {"t":1.12694, "x":3.09949, "y":5.80038, "heading":-0.98175, "vx":1.23041, "vy":-1.37646, "omega":-0.12815, "ax":-3.08727, "ay":3.42356, "alpha":-0.00291, "fx":[-50.48435,-50.46103,-50.45775,-50.48106], "fy":[55.9567,55.97772,55.98076,55.95975]}, - {"t":1.1645, "x":3.14353, "y":5.75109, "heading":-0.98657, "vx":1.11444, "vy":-1.24786, "omega":-0.12826, "ax":-3.08783, "ay":3.42428, "alpha":-0.00212, "fx":[-50.4898,-50.47282,-50.47035,-50.48732], "fy":[55.97173,55.98703,55.98931,55.97402]}, - {"t":1.20207, "x":3.18322, "y":5.70663, "heading":-0.99139, "vx":0.99844, "vy":-1.11922, "omega":-0.12834, "ax":-1.13542, "ay":-0.96733, "alpha":-0.60968, "fx":[-18.83966,-16.19997,-18.31127,-20.89663], "fy":[-18.10667,-16.54806,-13.47617,-15.12505]}, - {"t":1.23689, "x":3.21729, "y":5.66707, "heading":-0.99585, "vx":0.95891, "vy":-1.15291, "omega":-0.14957, "ax":-0.18809, "ay":-0.15555, "alpha":-0.5291, "fx":[-3.52348,-1.04926,-2.62913,-5.09802], "fy":[-4.55516,-2.96129,-0.52525,-2.12982]}, - {"t":1.2717, "x":3.25057, "y":5.62684, "heading":-1.00106, "vx":0.95236, "vy":-1.15832, "omega":-0.16799, "ax":-0.02773, "ay":-0.02278, "alpha":-0.31371, "fx":[-0.72809,0.7456,-0.17873,-1.65216], "fy":[-1.56559,-0.62194,0.82102,-0.12318]}, - {"t":1.30652, "x":3.28371, "y":5.58649, "heading":-1.00691, "vx":0.9514, "vy":-1.15911, "omega":-0.17892, "ax":-0.00406, "ay":-0.00334, "alpha":-0.09691, "fx":[-0.15348,0.30345,0.02058,-0.43634], "fy":[-0.42266,-0.13371,0.3136,0.02465]}, - {"t":1.34134, "x":3.31683, "y":5.54613, "heading":-1.01314, "vx":0.95125, "vy":-1.15923, "omega":-0.18229, "ax":-0.0006, "ay":-0.00049, "alpha":0.11946, "fx":[0.10038,-0.46508,-0.11984,0.44562], "fy":[0.44513,0.09246,-0.4611,-0.10843]}, - {"t":1.37616, "x":3.34995, "y":5.50577, "heading":-1.01949, "vx":0.95123, "vy":-1.15925, "omega":-0.17813, "ax":-0.00009, "ay":-0.00007, "alpha":0.3364, "fx":[0.31673,-1.2819,-0.31958,1.27905], "fy":[1.27283,0.28981,-1.27516,-0.29215]}, - {"t":1.41098, "x":3.38308, "y":5.4654, "heading":-1.02569, "vx":0.95123, "vy":-1.15925, "omega":-0.16642, "ax":-0.00001, "ay":-0.00001, "alpha":0.55496, "fx":[0.53768,-2.10959,-0.5381,2.10918], "fy":[2.09825,0.49296,-2.09859,-0.4933]}, - {"t":1.4458, "x":3.4162, "y":5.42504, "heading":-1.03148, "vx":0.95123, "vy":-1.15925, "omega":-0.14709, "ax":0.0, "ay":0.0, "alpha":0.77618, "fx":[0.76926,-2.9462,-0.76933,2.94614], "fy":[2.93044,0.70675,-2.93049,-0.70681]}, - {"t":1.48062, "x":3.44932, "y":5.38467, "heading":-1.03661, "vx":0.95123, "vy":-1.15925, "omega":-0.12007, "ax":0.0, "ay":0.0, "alpha":1.00108, "fx":[1.01153,-3.79513,-1.01156,3.79509], "fy":[3.77443,0.93101,-3.77446,-0.93103]}, - {"t":1.51544, "x":3.48244, "y":5.34431, "heading":-1.04079, "vx":0.95123, "vy":-1.15925, "omega":-0.08521, "ax":-0.00007, "ay":0.00008, "alpha":1.23068, "fx":[1.26171,-4.66192,-1.26417,4.65949], "fy":[4.6362,1.16537,-4.63357,-1.16273]}, - {"t":1.55026, "x":3.51556, "y":5.30395, "heading":-1.04375, "vx":0.95123, "vy":-1.15925, "omega":-0.04236, "ax":-1.03287, "ay":1.25877, "alpha":1.14645, "fx":[-15.23468,-21.0979,-18.71391,-12.49555], "fy":[24.87235,21.16568,16.18847,20.0872]}, - {"t":1.58507, "x":3.54805, "y":5.26435, "heading":-1.04523, "vx":0.91526, "vy":-1.11542, "omega":-0.00244, "ax":-2.89922, "ay":3.53324, "alpha":0.01888, "fx":[-47.30698,-47.46298,-47.48669,-47.33034], "fy":[57.83656,57.70906,57.6868,57.8147]}, - {"t":1.61989, "x":3.57816, "y":5.22765, "heading":-1.04531, "vx":0.81432, "vy":-0.9924, "omega":-0.00179, "ax":-2.91485, "ay":3.55229, "alpha":0.01081, "fx":[-47.60042,-47.69015,-47.70414,-47.6143], "fy":[58.11603,58.04255,58.03026,58.10386]}, - {"t":1.65471, "x":3.60475, "y":5.19525, "heading":-1.04538, "vx":0.71282, "vy":-0.86871, "omega":-0.00141, "ax":-2.92009, "ay":3.55868, "alpha":0.00811, "fx":[-47.69896,-47.76638,-47.77698,-47.7095], "fy":[58.20978,58.15454,58.14543,58.20074]}, - {"t":1.68953, "x":3.6278, "y":5.16716, "heading":-1.04542, "vx":0.61115, "vy":-0.7448, "omega":-0.00113, "ax":-2.92272, "ay":3.56188, "alpha":0.00676, "fx":[-47.74835,-47.80457,-47.81346,-47.75719], "fy":[58.25675,58.21067,58.20312,58.24926]}, - {"t":1.72435, "x":3.64731, "y":5.14338, "heading":-1.04546, "vx":0.50938, "vy":-0.62078, "omega":-0.00089, "ax":-2.9243, "ay":3.5638, "alpha":0.00594, "fx":[-47.77801,-47.82751,-47.83535,-47.78582], "fy":[58.28496,58.24438,58.23777,58.27839]}, - {"t":1.75917, "x":3.66327, "y":5.12393, "heading":-1.0455, "vx":0.40756, "vy":-0.49669, "omega":-0.00068, "ax":-2.92535, "ay":3.56508, "alpha":0.0054, "fx":[-47.7978,-47.84281,-47.84996,-47.80492], "fy":[58.30378,58.26687,58.26087,58.29781]}, - {"t":1.79399, "x":3.67569, "y":5.1088, "heading":-1.04552, "vx":0.30571, "vy":-0.37256, "omega":-0.0005, "ax":-2.9261, "ay":3.566, "alpha":0.00502, "fx":[-47.81195,-47.85375,-47.86039,-47.81857], "fy":[58.31722,58.28294,58.27738,58.31169]}, - {"t":1.82881, "x":3.68456, "y":5.09799, "heading":-1.04554, "vx":0.20382, "vy":-0.24839, "omega":-0.00032, "ax":-2.92666, "ay":3.56669, "alpha":0.00473, "fx":[-47.82256,-47.86195,-47.86822,-47.8288], "fy":[58.32731,58.295,58.28977,58.3221]}, - {"t":1.86363, "x":3.68988, "y":5.0915, "heading":-1.04555, "vx":0.10192, "vy":-0.12421, "omega":-0.00016, "ax":-2.9271, "ay":3.56722, "alpha":0.0045, "fx":[-47.83081,-47.86833,-47.87431,-47.83676], "fy":[58.33516,58.30438,58.2994,58.3302]}, - {"t":1.89844, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.08922, "ay":-3.42876, "alpha":-0.00403, "fx":[50.48511,50.51766,50.52076,50.48819], "fy":[-56.06972,-56.0404,-56.03755,-56.06689]}, + {"t":0.03973, "x":1.55259, "y":7.52069, "heading":-0.93501, "vx":0.12274, "vy":-0.13623, "omega":-0.00016, "ax":3.08902, "ay":-3.42856, "alpha":-0.00415, "fx":[50.48131,50.51479,50.51798,50.48448], "fy":[-56.06692,-56.03677,-56.03383,-56.06401]}, + {"t":0.07946, "x":1.5599, "y":7.51257, "heading":-0.93501, "vx":0.24547, "vy":-0.27245, "omega":-0.00033, "ax":3.0888, "ay":-3.42833, "alpha":-0.00428, "fx":[50.47704,50.51157,50.51485,50.4803], "fy":[-56.06378,-56.03268,-56.02965,-56.06077]}, + {"t":0.11919, "x":1.5721, "y":7.49904, "heading":-0.93503, "vx":0.36819, "vy":-0.40867, "omega":-0.0005, "ax":3.08854, "ay":-3.42808, "alpha":-0.00443, "fx":[50.4722,50.50791,50.51131,50.47557], "fy":[-56.06022,-56.02805,-56.02491,-56.0571]}, + {"t":0.15893, "x":1.58916, "y":7.4801, "heading":-0.93504, "vx":0.49091, "vy":-0.54487, "omega":-0.00067, "ax":3.08825, "ay":-3.42779, "alpha":-0.00459, "fx":[50.46666,50.50374,50.50726,50.47016], "fy":[-56.05615,-56.02276,-56.01949,-56.05291]}, + {"t":0.19866, "x":1.6111, "y":7.45575, "heading":-0.93507, "vx":0.61361, "vy":-0.68106, "omega":-0.00085, "ax":3.08791, "ay":-3.42745, "alpha":-0.00479, "fx":[50.46028,50.49892,50.50259,50.46392], "fy":[-56.05145,-56.01665,-56.01325,-56.04807]}, + {"t":0.23839, "x":1.63792, "y":7.42598, "heading":-0.93511, "vx":0.7363, "vy":-0.81724, "omega":-0.00104, "ax":3.08751, "ay":-3.42706, "alpha":-0.00502, "fx":[50.45284,50.4933,50.49714,50.45665], "fy":[-56.04597,-56.00953,-56.00596,-56.04243]}, + {"t":0.27812, "x":1.66961, "y":7.39081, "heading":-0.93515, "vx":0.85897, "vy":-0.9534, "omega":-0.00124, "ax":3.08705, "ay":-3.4266, "alpha":-0.00528, "fx":[50.44404,50.48666,50.4907,50.44805], "fy":[-56.0395,-56.00113,-55.99735,-56.03577]}, + {"t":0.31785, "x":1.70618, "y":7.35022, "heading":-0.9352, "vx":0.98162, "vy":-1.08955, "omega":-0.00145, "ax":3.08649, "ay":-3.42605, "alpha":-0.0056, "fx":[50.43349,50.4787,50.48298,50.43774], "fy":[-56.03175,-55.99104,-55.98703,-56.02777]}, + {"t":0.35758, "x":1.74761, "y":7.30423, "heading":-0.93525, "vx":1.10425, "vy":-1.22567, "omega":-0.00168, "ax":3.08581, "ay":-3.42537, "alpha":-0.006, "fx":[50.42061,50.46898,50.47355,50.42515], "fy":[-56.02227,-55.97872,-55.97442,-56.01801]}, + {"t":0.39732, "x":1.79392, "y":7.25283, "heading":-0.93532, "vx":1.22686, "vy":-1.36176, "omega":-0.00191, "ax":3.08495, "ay":-3.42453, "alpha":-0.00649, "fx":[50.40452,50.45683,50.46177,50.40941], "fy":[-56.01043,-55.96333,-55.95866,-56.00581]}, + {"t":0.43705, "x":1.8451, "y":7.19602, "heading":-0.9354, "vx":1.34943, "vy":-1.49783, "omega":-0.00217, "ax":3.08386, "ay":-3.42345, "alpha":-0.00712, "fx":[50.38384,50.44123,50.44663,50.3892], "fy":[-55.99522,-55.94356,-55.93842,-55.99013]}, + {"t":0.47678, "x":1.90115, "y":7.1338, "heading":-0.93548, "vx":1.47195, "vy":-1.63384, "omega":-0.00245, "ax":3.0824, "ay":-3.422, "alpha":-0.00796, "fx":[50.35632,50.42045,50.42648,50.36228], "fy":[-55.97497,-55.91724,-55.91146,-55.96926]}, + {"t":0.51651, "x":1.96207, "y":7.06619, "heading":-0.93558, "vx":1.59442, "vy":-1.76981, "omega":-0.00277, "ax":3.08036, "ay":-3.41999, "alpha":-0.00914, "fx":[50.31785,50.39142,50.3983,50.32464], "fy":[-55.94667,-55.88046,-55.87377,-55.94007]}, + {"t":0.55624, "x":2.02785, "y":6.99317, "heading":-0.93569, "vx":1.71681, "vy":-1.90569, "omega":-0.00313, "ax":3.07731, "ay":-3.41697, "alpha":-0.0109, "fx":[50.2603,50.34799,50.35614,50.26832], "fy":[-55.90433,-55.82544,-55.81736,-55.89639]}, + {"t":0.59597, "x":2.09849, "y":6.91476, "heading":-0.93581, "vx":1.83908, "vy":-2.04145, "omega":-0.00357, "ax":3.07225, "ay":-3.41195, "alpha":-0.01384, "fx":[50.1648,50.27593,50.28613,50.1748], "fy":[-55.83405,-55.73413,-55.7237,-55.82384]}, + {"t":0.63571, "x":2.17398, "y":6.83096, "heading":-0.93596, "vx":1.96114, "vy":-2.17701, "omega":-0.00412, "ax":3.0622, "ay":-3.402, "alpha":-0.01967, "fx":[49.9754,50.13298,50.14706,49.98909], "fy":[-55.69456,-55.55302,-55.53771,-55.67968]}, + {"t":0.67544, "x":2.25432, "y":6.74177, "heading":-0.93612, "vx":2.08281, "vy":-2.31218, "omega":-0.0049, "ax":3.03266, "ay":-3.37273, "alpha":-0.03695, "fx":[49.41982,49.71331,49.7374,49.44256], "fy":[-55.28447,-55.02166,-54.9903,-55.25458]}, + {"t":0.71517, "x":2.33947, "y":6.64725, "heading":-0.93631, "vx":2.2033, "vy":-2.44618, "omega":-0.00637, "ax":1.31961, "ay":-1.62049, "alpha":-1.2297, "fx":[19.95674,26.23815,23.45074,16.64649], "fy":[-31.06802,-26.2804,-21.71887,-26.90072]}, + {"t":0.7549, "x":2.42805, "y":6.54878, "heading":-0.93657, "vx":2.25573, "vy":-2.51057, "omega":-0.05522, "ax":-3.03747, "ay":3.36341, "alpha":-0.03544, "fx":[-49.80934,-49.52736,-49.50508,-49.78582], "fy":[54.8433,55.09709,55.12668,54.87425]}, + {"t":0.79463, "x":2.51528, "y":6.45168, "heading":-0.93876, "vx":2.13505, "vy":-2.37693, "omega":-0.05663, "ax":-3.06526, "ay":3.39798, "alpha":-0.01686, "fx":[-50.18484,-50.04971,-50.03748,-50.17232], "fy":[55.48302,55.6047,55.61798,55.49661]}, + {"t":0.83436, "x":2.59768, "y":6.35992, "heading":-0.94101, "vx":2.01326, "vy":-2.24193, "omega":-0.0573, "ax":-3.07443, "ay":3.40942, "alpha":-0.0107, "fx":[-50.30827,-50.22233,-50.2141,-50.29992], "fy":[55.69458,55.77198,55.78036,55.70308]}, + {"t":0.8741, "x":2.67525, "y":6.27354, "heading":-0.94329, "vx":1.89111, "vy":-2.10646, "omega":-0.05773, "ax":-3.07901, "ay":3.41512, "alpha":-0.0076, "fx":[-50.36957,-50.30841,-50.30232,-50.36342], "fy":[55.80011,55.85521,55.86121,55.80618]}, + {"t":0.91383, "x":2.74795, "y":6.19254, "heading":-0.94558, "vx":1.76877, "vy":-1.97078, "omega":-0.05803, "ax":-3.08175, "ay":3.41853, "alpha":-0.00574, "fx":[-50.40619,-50.35999,-50.35525,-50.40141], "fy":[55.86338,55.90499,55.90958,55.868]}, + {"t":0.95356, "x":2.8158, "y":6.11694, "heading":-0.94789, "vx":1.64633, "vy":-1.83495, "omega":-0.05826, "ax":-3.08357, "ay":3.42081, "alpha":-0.00449, "fx":[-50.43052,-50.39437,-50.39055,-50.42667], "fy":[55.90555,55.93811,55.94176,55.90922]}, + {"t":0.99329, "x":2.87878, "y":6.04673, "heading":-0.9502, "vx":1.52381, "vy":-1.69904, "omega":-0.05844, "ax":-3.08487, "ay":3.42243, "alpha":-0.00359, "fx":[-50.44784,-50.41891,-50.41577,-50.44469], "fy":[55.93567,55.96173,55.9647,55.93865]}, + {"t":1.03302, "x":2.93688, "y":5.98193, "heading":-0.95252, "vx":1.40125, "vy":-1.56306, "omega":-0.05858, "ax":-3.08585, "ay":3.42365, "alpha":-0.00292, "fx":[-50.4608,-50.43731,-50.4347,-50.45818], "fy":[55.95827,55.97943,55.98188,55.96073]}, + {"t":1.07275, "x":2.99012, "y":5.92253, "heading":-0.95485, "vx":1.27864, "vy":-1.42703, "omega":-0.05869, "ax":-3.08661, "ay":3.42459, "alpha":-0.00239, "fx":[-50.47085,-50.45162,-50.44944,-50.46866], "fy":[55.97585,55.99318,55.99522,55.9779]}, + {"t":1.11248, "x":3.03849, "y":5.86853, "heading":-0.95718, "vx":1.15601, "vy":-1.29097, "omega":-0.05879, "ax":-3.08721, "ay":3.42535, "alpha":-0.00196, "fx":[-50.47888,-50.46307,-50.46123,-50.47704], "fy":[55.98992,56.00417,56.00588,55.99164]}, + {"t":1.15222, "x":3.08198, "y":5.81994, "heading":-0.95952, "vx":1.03335, "vy":-1.15487, "omega":-0.05887, "ax":-3.08771, "ay":3.42597, "alpha":-0.00162, "fx":[-50.48544,-50.47243,-50.47089,-50.48389], "fy":[56.00144,56.01316,56.01459,56.00287]}, + {"t":1.19195, "x":3.1206, "y":5.77676, "heading":-0.96186, "vx":0.91067, "vy":-1.01876, "omega":-0.05893, "ax":-3.08812, "ay":3.42648, "alpha":-0.00132, "fx":[-50.49089,-50.48023,-50.47894,-50.4896], "fy":[56.01104,56.02064,56.02184,56.01224]}, + {"t":1.23168, "x":3.15435, "y":5.73899, "heading":-0.9642, "vx":0.78797, "vy":-0.88262, "omega":-0.05898, "ax":-3.08847, "ay":3.42692, "alpha":-0.00108, "fx":[-50.4955,-50.48683,-50.48576,-50.49443], "fy":[56.01917,56.02698,56.02796,56.02016]}, + {"t":1.27141, "x":3.18322, "y":5.70663, "heading":-0.96654, "vx":0.66526, "vy":-0.74646, "omega":-0.05903, "ax":-0.75349, "ay":-0.64148, "alpha":-0.55179, "fx":[-12.63721,-10.18557,-12.0132,-14.4368], "fy":[-12.58849,-10.96348,-8.36182,-10.0339]}, + {"t":1.30929, "x":3.20787, "y":5.6779, "heading":-0.96878, "vx":0.63672, "vy":-0.77076, "omega":-0.07993, "ax":-0.04831, "ay":-0.03981, "alpha":-0.5193, "fx":[-1.18051,1.20753,-0.39961,-2.7862], "fy":[-2.63917,-1.00023,1.33885,-0.30265]}, + {"t":1.34716, "x":3.23195, "y":5.64867, "heading":-0.97181, "vx":0.63489, "vy":-0.77226, "omega":-0.0996, "ax":-0.00292, "ay":-0.0024, "alpha":-0.43649, "fx":[-0.38129,1.62987,0.28583,-1.72527], "fy":[-1.71005,-0.33719,1.63166,0.2587]}, + {"t":1.38504, "x":3.256, "y":5.61942, "heading":-0.97558, "vx":0.63478, "vy":-0.77235, "omega":-0.11613, "ax":-0.00018, "ay":-0.00014, "alpha":-0.35594, "fx":[-0.28002,1.36418,0.27426,-1.36993], "fy":[-1.36383,-0.25048,1.3591,0.24575]}, + {"t":1.42291, "x":3.28004, "y":5.59017, "heading":-0.97998, "vx":0.63477, "vy":-0.77236, "omega":-0.12961, "ax":-0.00001, "ay":-0.00001, "alpha":-0.27738, "fx":[-0.22081,1.06429,0.22046,-1.06463], "fy":[-1.06015,-0.19818,1.05986,0.19789]}, + {"t":1.46079, "x":3.30408, "y":5.56092, "heading":-0.98489, "vx":0.63477, "vy":-0.77236, "omega":-0.14012, "ax":0.0, "ay":0.0, "alpha":-0.20037, "fx":[-0.16315,0.76821,0.16313,-0.76823], "fy":[-0.76493,-0.14684,0.76492,0.14682]}, + {"t":1.49867, "x":3.32813, "y":5.53166, "heading":-0.99019, "vx":0.63477, "vy":-0.77236, "omega":-0.1477, "ax":0.0, "ay":0.0, "alpha":-0.12448, "fx":[-0.10388,0.47679,0.10387,-0.47679], "fy":[-0.47468,-0.09375,0.47468,0.09375]}, + {"t":1.53654, "x":3.35217, "y":5.50241, "heading":-0.99579, "vx":0.63477, "vy":-0.77236, "omega":-0.15242, "ax":0.0, "ay":0.0, "alpha":-0.0493, "fx":[-0.04219,0.18861,0.04219,-0.18861], "fy":[-0.18775,-0.03818,0.18775,0.03818]}, + {"t":1.57442, "x":3.37621, "y":5.47315, "heading":-1.00156, "vx":0.63477, "vy":-0.77236, "omega":-0.15429, "ax":0.0, "ay":0.0, "alpha":0.02561, "fx":[0.02248,-0.09787,-0.02248,0.09787], "fy":[0.09741,0.0204,-0.09741,-0.0204]}, + {"t":1.61229, "x":3.40025, "y":5.4439, "heading":-1.0074, "vx":0.63477, "vy":-0.77236, "omega":-0.15332, "ax":0.0, "ay":0.0, "alpha":0.10066, "fx":[0.09059,-0.38419,-0.09059,0.38419], "fy":[0.38235,0.08244,-0.38235,-0.08244]}, + {"t":1.65017, "x":3.4243, "y":5.41465, "heading":-1.01321, "vx":0.63477, "vy":-0.77236, "omega":-0.1495, "ax":0.0, "ay":0.0, "alpha":0.17628, "fx":[0.16253,-0.67194,-0.16253,0.67194], "fy":[0.66863,0.14827,-0.66863,-0.14827]}, + {"t":1.68804, "x":3.44834, "y":5.38539, "heading":-1.01887, "vx":0.63477, "vy":-0.77236, "omega":-0.14283, "ax":0.0, "ay":0.0, "alpha":0.25289, "fx":[0.23859,-0.96273,-0.23859,0.96273], "fy":[0.95787,0.21815,-0.95787,-0.21815]}, + {"t":1.72592, "x":3.47238, "y":5.35614, "heading":-1.02428, "vx":0.63477, "vy":-0.77236, "omega":-0.13325, "ax":0.0, "ay":0.0, "alpha":0.33091, "fx":[0.31897,-1.25819,-0.31897,1.25819], "fy":[1.25169,0.29227,-1.25169,-0.29227]}, + {"t":1.7638, "x":3.49642, "y":5.32689, "heading":-1.02933, "vx":0.63477, "vy":-0.77236, "omega":-0.12072, "ax":0.0, "ay":0.0, "alpha":0.41079, "fx":[0.40381,-1.56005,-0.40381,1.56005], "fy":[1.55181,0.3707,-1.55181,-0.3707]}, + {"t":1.80167, "x":3.52047, "y":5.29763, "heading":-1.0339, "vx":0.63477, "vy":-0.77236, "omega":-0.10516, "ax":0.0, "ay":0.0, "alpha":0.49296, "fx":[0.49309,-1.87007,-0.49309,1.87007], "fy":[1.86,0.45341,-1.86,-0.45341]}, + {"t":1.83955, "x":3.54451, "y":5.26838, "heading":-1.03789, "vx":0.63477, "vy":-0.77236, "omega":-0.08648, "ax":0.0, "ay":0.0, "alpha":0.57789, "fx":[0.58672,-2.19009,-0.58672,2.19009], "fy":[2.1781,0.54025,-2.1781,-0.54025]}, + {"t":1.87742, "x":3.56855, "y":5.23912, "heading":-1.04116, "vx":0.63477, "vy":-0.77236, "omega":-0.0646, "ax":0.0, "ay":0.0, "alpha":0.66603, "fx":[0.68442,-2.52207,-0.68442,2.52207], "fy":[2.50808,0.63091,-2.50808,-0.63091]}, + {"t":1.9153, "x":3.5926, "y":5.20987, "heading":-1.04361, "vx":0.63477, "vy":-0.77236, "omega":-0.03937, "ax":-0.00008, "ay":0.0001, "alpha":0.75786, "fx":[0.78442,-2.86942,-0.78713,2.86672], "fy":[2.85362,0.72655,-2.85037,-0.7233]}, + {"t":1.95318, "x":3.61664, "y":5.18062, "heading":-1.0451, "vx":0.63477, "vy":-0.77236, "omega":-0.01067, "ax":-2.13179, "ay":2.59386, "alpha":0.26686, "fx":[-34.04384,-35.80929,-35.67966,-33.87028], "fy":[43.42887,42.12499,41.36043,42.70409]}, + {"t":1.99105, "x":3.63915, "y":5.15322, "heading":-1.0455, "vx":0.55403, "vy":-0.67411, "omega":-0.00056, "ax":-2.91622, "ay":3.5483, "alpha":0.00572, "fx":[-47.64718,-47.69459,-47.70199,-47.65455], "fy":[58.0306,57.99172,57.98517,58.02409]}, + {"t":2.02893, "x":3.65804, "y":5.13024, "heading":-1.04552, "vx":0.44357, "vy":-0.53972, "omega":-0.00034, "ax":-2.92472, "ay":3.55865, "alpha":0.00318, "fx":[-47.79835,-47.82478,-47.82897,-47.80252], "fy":[58.18975,58.16805,58.16448,58.18619]}, + {"t":2.0668, "x":3.67275, "y":5.11235, "heading":-1.04554, "vx":0.3328, "vy":-0.40493, "omega":-0.00022, "ax":-2.9276, "ay":3.56215, "alpha":0.00232, "fx":[-47.84944,-47.86878,-47.87185,-47.85251], "fy":[58.24353,58.22765,58.22506,58.24094]}, + {"t":2.10468, "x":3.68325, "y":5.09956, "heading":-1.04555, "vx":0.22191, "vy":-0.27001, "omega":-0.00013, "ax":-2.92904, "ay":3.5639, "alpha":0.00189, "fx":[-47.87512,-47.89088,-47.8934,-47.87763], "fy":[58.27056,58.2576,58.2555,58.26845]}, + {"t":2.14255, "x":3.68956, "y":5.09189, "heading":-1.04555, "vx":0.11097, "vy":-0.13503, "omega":-0.00006, "ax":-2.92991, "ay":3.56496, "alpha":0.00164, "fx":[-47.89056,-47.90418,-47.90636,-47.89274], "fy":[58.28681,58.27562,58.27381,58.285]}, + {"t":2.18043, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index f73ce214..588bedf7 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -3,7 +3,15 @@ "version":1, "type":"Swerve", "variables":{ - "expressions":{}, + "expressions":{ + "slow":{ + "dimension":"LinVel", + "var":{ + "exp":"1 m / s", + "val":1.0 + } + } + }, "poses":{ "A":{ "x":{ diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 4457841c..f6cdaa9e 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -147,7 +147,7 @@ public Command LOtoJ() { HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { - "LO", "J", "PLO", "K", "PLO", "L", // "PLM", "A", "PLO" // each stop we are going to, in order + "LO", "J", "PLO", "K", "PLO", "L", "PLM", "A", "PLO" // each stop we are going to, in order }; // i don't love repeating the plos but ??? for (int i = 0; i < stops.length - 1; i++) { String name = stops[i] + "to" + stops[i + 1]; // concatenate the names of the stops @@ -167,19 +167,23 @@ public Command LOtoJ() { runPath(routine, startPos, endPos, nextPos, steps); } - final var groundTraj = routine.trajectory("LtoAGround"); + // final var groundTraj = routine.trajectory("LtoAGround"); - routine - .observe(steps.get("PLOtoL").recentlyDone()) - .onTrue(scoreInAuto(() -> steps.get("PLOtoL").getFinalPose().get())) - .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) - .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) - .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + // routine + // .observe(steps.get("PLOtoL").recentlyDone()) + // .onTrue(scoreInAuto(() -> steps.get("PLOtoL").getFinalPose().get())) + // .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak()) + // .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) + // .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) + // .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + + // routine + // .observe(groundTraj.done()) + // .onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); routine - .observe(groundTraj.done()) - .onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); + .observe(steps.get("LtoPLM").done()) + .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); return routine.cmd().alongWith(Commands.print("auto :(")); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 953538f8..95c1ab80 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -334,7 +334,7 @@ public static enum AlgaeScoreTarget { 12, WristIOReal.getDefaultConfiguration() .withSlot0( - new Slot0Configs().withKP(1000.0).withKD(30.0).withKS(0.3).withKV(3.6)) + new Slot0Configs().withKP(1000.0).withKD(30.0).withKS(0.3).withKV(3.2)) .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() From 91bcb2dea502182f404ecf5978c519504bdedf4c Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 21:48:06 -0700 Subject: [PATCH 062/126] speed up auto a smidge --- src/main/deploy/choreo/LOtoJ.traj | 130 ++++++++++++++--------------- src/main/deploy/choreo/PLOtoK.traj | 120 +++++++++++++------------- src/main/deploy/choreo/PLOtoL.traj | 120 +++++++++++++------------- src/main/deploy/choreo/choreo.chor | 4 +- src/main/java/frc/robot/Robot.java | 2 +- 5 files changed, 185 insertions(+), 191 deletions(-) diff --git a/src/main/deploy/choreo/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 5ad5aebe..d361058e 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -4,14 +4,14 @@ "snapshot":{ "waypoints":[ {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":4.484625190496445, "y":4.0126113295555115, "r":3.25}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 @@ -19,14 +19,14 @@ "params":{ "waypoints":[ {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.484625190496445 m", "val":4.484625190496445}, "y":{"exp":"4.0126113295555115 m", "val":4.0126113295555115}, "r":{"exp":"3.25 m", "val":3.25}}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], "targetDt":{ @@ -36,68 +36,68 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.96573,2.04281], + "waypoints":[0.0,0.90591,1.83227], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17863, "ay":-1.95783, "alpha":0.00583, "fx":[-68.30429,-68.32983,-68.32085,-68.29531], "fy":[-32.02433,-31.96985,-31.9892,-32.04364]}, - {"t":0.03449, "x":7.10883, "y":6.2344, "heading":-2.35619, "vx":-0.14412, "vy":-0.06753, "omega":0.0002, "ax":-4.17832, "ay":-1.95769, "alpha":0.00603, "fx":[-68.29893,-68.32535,-68.31607,-68.28965], "fy":[-32.02264,-31.96629,-31.9863,-32.04261]}, - {"t":0.06898, "x":7.10137, "y":6.23091, "heading":-2.35619, "vx":-0.28824, "vy":-0.13505, "omega":0.00041, "ax":-4.17796, "ay":-1.95753, "alpha":0.00626, "fx":[-68.29286,-68.32028,-68.31065,-68.28323], "fy":[-32.02072,-31.96225,-31.98302,-32.04144]}, - {"t":0.10347, "x":7.08895, "y":6.22508, "heading":-2.35617, "vx":-0.43234, "vy":-0.20256, "omega":0.00063, "ax":-4.17756, "ay":-1.95735, "alpha":0.00652, "fx":[-68.28591,-68.31447,-68.30445,-68.27589], "fy":[-32.01853,-31.95764,-31.97927,-32.04011]}, - {"t":0.13796, "x":7.07155, "y":6.21693, "heading":-2.35615, "vx":-0.57642, "vy":-0.27007, "omega":0.00085, "ax":-4.1771, "ay":-1.95714, "alpha":0.00682, "fx":[-68.2779,-68.30777,-68.29729,-68.26743], "fy":[-32.01599,-31.95231,-31.97495,-32.03857]}, - {"t":0.17245, "x":7.04918, "y":6.20645, "heading":-2.35612, "vx":-0.72049, "vy":-0.33758, "omega":0.00109, "ax":-4.17656, "ay":-1.95689, "alpha":0.00717, "fx":[-68.26854,-68.29994,-68.28894,-68.25754], "fy":[-32.01304,-31.94609,-31.96989,-32.03677]}, - {"t":0.20694, "x":7.02185, "y":6.19365, "heading":-2.35609, "vx":-0.86454, "vy":-0.40507, "omega":0.00133, "ax":-4.17592, "ay":-1.9566, "alpha":0.00759, "fx":[-68.25747,-68.29069,-68.27906,-68.24585], "fy":[-32.00954,-31.93874,-31.96392,-32.03465]}, - {"t":0.24143, "x":6.98955, "y":6.17851, "heading":-2.35604, "vx":-1.00857, "vy":-0.47255, "omega":0.00159, "ax":-4.17515, "ay":-1.95625, "alpha":0.00808, "fx":[-68.24418,-68.27957,-68.26719,-68.2318], "fy":[-32.00534,-31.9299,-31.95674,-32.0321]}, - {"t":0.27592, "x":6.95228, "y":6.16105, "heading":-2.35598, "vx":-1.15258, "vy":-0.54003, "omega":0.00187, "ax":-4.1742, "ay":-1.95583, "alpha":0.00869, "fx":[-68.22791,-68.26598,-68.25268,-68.21462], "fy":[-32.0002,-31.91909,-31.94797,-32.02898]}, - {"t":0.31041, "x":6.91004, "y":6.14126, "heading":-2.35592, "vx":-1.29655, "vy":-0.60748, "omega":0.00217, "ax":-4.17303, "ay":-1.95529, "alpha":0.00946, "fx":[-68.20755,-68.24896,-68.23452,-68.19312], "fy":[-31.99376,-31.90555,-31.93698,-32.02507]}, - {"t":0.3449, "x":6.86284, "y":6.11914, "heading":-2.35584, "vx":-1.44048, "vy":-0.67492, "omega":0.0025, "ax":-4.17151, "ay":-1.95461, "alpha":0.01044, "fx":[-68.18133,-68.22704,-68.21113,-68.16543], "fy":[-31.98547,-31.88812,-31.92284,-32.02005]}, - {"t":0.3794, "x":6.81068, "y":6.0947, "heading":-2.35576, "vx":-1.58435, "vy":-0.74234, "omega":0.00286, "ax":-4.16948, "ay":-1.95369, "alpha":0.01176, "fx":[-68.14629,-68.19775,-68.1799,-68.12844], "fy":[-31.9744,-31.86483,-31.90395,-32.01334]}, - {"t":0.41389, "x":6.75355, "y":6.06794, "heading":-2.35566, "vx":-1.72816, "vy":-0.80972, "omega":0.00327, "ax":-4.16664, "ay":-1.9524, "alpha":0.01361, "fx":[-68.09707,-68.15663,-68.13606,-68.07651], "fy":[-31.95885,-31.83212,-31.87743,-32.00392]}, - {"t":0.44838, "x":6.69147, "y":6.03885, "heading":-2.35555, "vx":-1.87187, "vy":-0.87706, "omega":0.00373, "ax":-4.16235, "ay":-1.95046, "alpha":0.0164, "fx":[-68.02292,-68.09469,-68.07006,-67.99831], "fy":[-31.93542,-31.78283,-31.8375,-31.98975]}, - {"t":0.48287, "x":6.62443, "y":6.00744, "heading":-2.35542, "vx":-2.01543, "vy":-0.94433, "omega":0.0043, "ax":-4.15516, "ay":-1.94721, "alpha":0.0211, "fx":[-67.89843,-67.99075,-67.95942,-67.86714], "fy":[-31.8961,-31.7001,-31.77056,-31.96599]}, - {"t":0.51736, "x":6.55245, "y":5.97371, "heading":-2.35527, "vx":-2.15874, "vy":-1.01149, "omega":0.00503, "ax":-4.1406, "ay":-1.94062, "alpha":0.03068, "fx":[-67.64604,-67.78019,-67.73573,-67.60164], "fy":[-31.81642,-31.53239,-31.63516,-31.918]}, - {"t":0.55185, "x":6.47553, "y":5.93767, "heading":-2.3551, "vx":-2.30156, "vy":-1.07843, "omega":0.00609, "ax":-4.09544, "ay":-1.92019, "alpha":0.06119, "fx":[-66.85996,-67.12714,-67.045,-66.77819], "fy":[-31.56973,-31.0088,-31.21566,-31.77171]}, - {"t":0.58634, "x":6.39371, "y":5.89933, "heading":-2.35489, "vx":-2.44281, "vy":-1.14466, "omega":0.0082, "ax":2.98228, "ay":1.38131, "alpha":1.02202, "fx":[51.39917,47.33681,45.9398,50.3424], "fy":[21.22059,27.65747,24.22716,17.22224]}, - {"t":0.62083, "x":6.31123, "y":5.86067, "heading":-2.3546, "vx":-2.33995, "vy":-1.09701, "omega":0.04345, "ax":4.10601, "ay":1.92246, "alpha":0.04948, "fx":[67.19943,66.98384,67.05127,67.26705], "fy":[31.28581,31.73694,31.5728,31.11855]}, - {"t":0.65532, "x":6.23297, "y":5.82398, "heading":-2.35311, "vx":-2.19833, "vy":-1.03071, "omega":0.04515, "ax":4.14352, "ay":1.94065, "alpha":0.02405, "fx":[67.77357,67.66858,67.70377,67.80879], "fy":[31.65528,31.87706,31.79681,31.57429]}, - {"t":0.68981, "x":6.15961, "y":5.78958, "heading":-2.35155, "vx":-2.05542, "vy":-0.96377, "omega":0.04598, "ax":4.15658, "ay":1.94698, "alpha":0.01535, "fx":[67.97411,67.90708,67.93015,67.9972], "fy":[31.78412,31.92621,31.87473,31.73234]}, - {"t":0.7243, "x":6.09119, "y":5.7575, "heading":-2.34996, "vx":-1.91206, "vy":-0.89662, "omega":0.04651, "ax":4.16322, "ay":1.95019, "alpha":0.01095, "fx":[68.07618,68.02837,68.04509,68.09291], "fy":[31.84968,31.9512,31.91431,31.81264]}, - {"t":0.75879, "x":6.02772, "y":5.72774, "heading":-2.34836, "vx":-1.76847, "vy":-0.82936, "omega":0.04689, "ax":4.16723, "ay":1.95214, "alpha":0.00829, "fx":[68.13801,68.10182,68.11462,68.15081], "fy":[31.88939,31.96631,31.93825,31.86124]}, - {"t":0.79328, "x":5.9692, "y":5.70029, "heading":-2.34674, "vx":-1.62474, "vy":-0.76203, "omega":0.04718, "ax":4.16993, "ay":1.95345, "alpha":0.00651, "fx":[68.17947,68.15108,68.16122,68.18962], "fy":[31.91602,31.97643,31.9543,31.89384]}, - {"t":0.82777, "x":5.91564, "y":5.67517, "heading":-2.34511, "vx":-1.48091, "vy":-0.69465, "omega":0.0474, "ax":4.17186, "ay":1.95438, "alpha":0.00523, "fx":[68.20922,68.1864,68.19461,68.21743], "fy":[31.93512,31.98367,31.96581,31.91722]}, - {"t":0.86226, "x":5.86705, "y":5.65238, "heading":-2.34348, "vx":-1.33702, "vy":-0.62725, "omega":0.04758, "ax":4.17331, "ay":1.95509, "alpha":0.00427, "fx":[68.23159,68.21297,68.21972,68.23834], "fy":[31.94948,31.98911,31.97446,31.93481]}, - {"t":0.89675, "x":5.82342, "y":5.6319, "heading":-2.34184, "vx":-1.19308, "vy":-0.55981, "omega":0.04773, "ax":4.17444, "ay":1.95564, "alpha":0.00352, "fx":[68.24903,68.23369,68.23929,68.25463], "fy":[31.96067,31.99334,31.98121,31.94853]}, - {"t":0.93124, "x":5.78475, "y":5.61376, "heading":-2.34019, "vx":-1.04911, "vy":-0.49236, "omega":0.04785, "ax":4.17535, "ay":1.95608, "alpha":0.00292, "fx":[68.26301,68.2503,68.25497,68.26769], "fy":[31.96964,31.99672,31.98662,31.95952]}, - {"t":0.96573, "x":5.75105, "y":5.59794, "heading":-2.33854, "vx":-0.9051, "vy":-0.4249, "omega":0.04795, "ax":0.04195, "ay":-0.07851, "alpha":1.15701, "fx":[5.11239,0.4484,-3.7255,0.90799], "fy":[-1.16555,3.3266,-1.4048,-5.88993]}, - {"t":1.00048, "x":5.71963, "y":5.58313, "heading":-2.33687, "vx":-0.90364, "vy":-0.42762, "omega":0.08815, "ax":0.00953, "ay":-0.02011, "alpha":1.0362, "fx":[4.13041,-0.05691,-3.8142,0.36374], "fy":[-0.2105,3.78377,-0.4485,-4.43995]}, - {"t":1.03522, "x":5.68824, "y":5.56826, "heading":-2.33381, "vx":-0.90331, "vy":-0.42832, "omega":0.12415, "ax":0.00102, "ay":-0.00215, "alpha":0.95776, "fx":[3.67126,-0.1913,-3.6341,0.22073], "fy":[0.09073,3.78355,-0.16128,-3.85343]}, - {"t":1.06997, "x":5.65685, "y":5.55338, "heading":-2.3295, "vx":-0.90327, "vy":-0.4284, "omega":0.15743, "ax":-0.00008, "ay":0.00016, "alpha":0.856, "fx":[3.28227,-0.20447,-3.28308,0.20019], "fy":[0.13179,3.39585,-0.12649,-3.39042]}, - {"t":1.10471, "x":5.62547, "y":5.53849, "heading":-2.32403, "vx":-0.90327, "vy":-0.42839, "omega":0.18717, "ax":-0.00013, "ay":0.00028, "alpha":0.76889, "fx":[2.92523,-0.19643,-2.92913,0.19161], "fy":[0.13921,3.07233,-0.13001,-3.06313]}, - {"t":1.13946, "x":5.59408, "y":5.52361, "heading":-2.31753, "vx":-0.90328, "vy":-0.42838, "omega":0.21389, "ax":-0.00013, "ay":0.00028, "alpha":0.68643, "fx":[2.62946,-0.19618,-2.63385,0.19195], "fy":[0.14198,2.72265,-0.13287,-2.71357]}, - {"t":1.1742, "x":5.5627, "y":5.50873, "heading":-2.31009, "vx":-0.90328, "vy":-0.42837, "omega":0.23774, "ax":-0.00014, "ay":0.00029, "alpha":0.59016, "fx":[2.23916,-0.18073,-2.24395,0.17646], "fy":[0.14076,2.3608,-0.1312,-2.35126]}, - {"t":1.20895, "x":5.53132, "y":5.49384, "heading":-2.30183, "vx":-0.90329, "vy":-0.42836, "omega":0.25824, "ax":-0.00014, "ay":0.00028, "alpha":0.52341, "fx":[2.00172,-0.18004,-2.0064,0.17588], "fy":[0.14061,2.07556,-0.13129,-2.06625]}, - {"t":1.24369, "x":5.49993, "y":5.47896, "heading":-2.29286, "vx":-0.90329, "vy":-0.42835, "omega":0.27643, "ax":-0.00013, "ay":0.00026, "alpha":0.42037, "fx":[1.58871,-0.15366,-1.59301,0.14975], "fy":[0.1281,1.68431,-0.11944,-1.67566]}, - {"t":1.27843, "x":5.46855, "y":5.46408, "heading":-2.28326, "vx":-0.9033, "vy":-0.42834, "omega":0.29103, "ax":-0.00011, "ay":0.00023, "alpha":0.36371, "fx":[1.38671,-0.14845,-1.39041,0.14489], "fy":[0.12246,1.44282,-0.1148,-1.43517]}, - {"t":1.31318, "x":5.43716, "y":5.44919, "heading":-2.27315, "vx":-0.9033, "vy":-0.42833, "omega":0.30367, "ax":-0.0001, "ay":0.0002, "alpha":0.25764, "fx":[0.96645,-0.10994,-0.96957,0.10672], "fy":[0.09693,1.03686,-0.09024,-1.03017]}, - {"t":1.34792, "x":5.40578, "y":5.43431, "heading":-2.26259, "vx":-0.9033, "vy":-0.42833, "omega":0.31262, "ax":-0.00008, "ay":0.00017, "alpha":0.20498, "fx":[0.7761,-0.09684,-0.77856,0.09396], "fy":[0.0836,0.81621,-0.07798,-0.81058]}, - {"t":1.38267, "x":5.37439, "y":5.41943, "heading":-2.25173, "vx":-0.90331, "vy":-0.42832, "omega":0.31974, "ax":-0.00008, "ay":0.00017, "alpha":0.09954, "fx":[0.36304,-0.04687,-0.36506,0.04375], "fy":[0.04503,0.41086,-0.0396,-0.40543]}, - {"t":1.41741, "x":5.34301, "y":5.40455, "heading":-2.24062, "vx":-0.90331, "vy":-0.42832, "omega":0.3232, "ax":-0.0001, "ay":0.00022, "alpha":0.04548, "fx":[0.16411,-0.02459,-0.16582,0.01949], "fy":[0.02266,0.19135,-0.01548,-0.18416]}, - {"t":1.45216, "x":5.31162, "y":5.38967, "heading":-2.22939, "vx":-0.90331, "vy":-0.42831, "omega":0.32478, "ax":-0.0001, "ay":0.00021, "alpha":-0.05673, "fx":[-0.23078,0.03341,0.23163,-0.04075], "fy":[-0.0289,-0.20294,0.03574,0.20978]}, - {"t":1.4869, "x":5.28024, "y":5.37479, "heading":-2.21811, "vx":-0.90332, "vy":-0.4283, "omega":0.32281, "ax":0.00017, "ay":-0.00023, "alpha":-0.11605, "fx":[-0.44458,0.06676,0.46075,-0.07171], "fy":[-0.07253,-0.4467,0.06487,0.43904]}, - {"t":1.52165, "x":5.24885, "y":5.3599, "heading":-2.20689, "vx":-0.90331, "vy":-0.42831, "omega":0.31878, "ax":0.7229, "ay":0.34342, "alpha":-0.19629, "fx":[11.10803,11.88902,12.5835,11.69139], "fy":[5.51197,4.8325,5.71618,6.3962]}, - {"t":1.55639, "x":5.2179, "y":5.34523, "heading":-2.19582, "vx":-0.87819, "vy":-0.41638, "omega":0.31196, "ax":1.79959, "ay":0.85325, "alpha":-0.19977, "fx":[28.73682,29.71501,30.05725,29.17027], "fy":[13.91918,13.07701,13.98661,14.81313]}, - {"t":1.59113, "x":5.18848, "y":5.33128, "heading":-2.18498, "vx":-0.81567, "vy":-0.38673, "omega":0.30502, "ax":1.80339, "ay":0.85505, "alpha":-0.26824, "fx":[28.57983,29.8311,30.35649,29.16031], "fy":[13.91916,12.80953,14.04375,15.1412]}, - {"t":1.62588, "x":5.16123, "y":5.31836, "heading":-2.17438, "vx":-0.75301, "vy":-0.35702, "omega":0.2957, "ax":1.80465, "ay":0.85564, "alpha":-0.32382, "fx":[28.41849,29.91544,30.56338,29.11299], "fy":[13.9008,12.58004,14.08172,15.39009]}, - {"t":1.66062, "x":5.13615, "y":5.30647, "heading":-2.16411, "vx":-0.69031, "vy":-0.32729, "omega":0.28445, "ax":1.80528, "ay":0.85594, "alpha":-0.39325, "fx":[28.19885,30.01318,30.80307,29.03644], "fy":[13.86712,12.287,14.12585,15.69207]}, - {"t":1.69537, "x":5.11326, "y":5.29561, "heading":-2.15423, "vx":-0.62759, "vy":-0.29756, "omega":0.27078, "ax":1.80566, "ay":0.85612, "alpha":-0.45529, "fx":[28.00022,30.10464,31.01193,28.95949], "fy":[13.83126,12.02454,14.16836,15.95946]}, - {"t":1.73011, "x":5.09254, "y":5.28579, "heading":-2.14482, "vx":-0.56485, "vy":-0.26781, "omega":0.25496, "ax":1.80591, "ay":0.85624, "alpha":-0.52499, "fx":[27.77411,30.20722,31.24227,28.86917], "fy":[13.78673,11.7288,14.21781,16.25795]}, - {"t":1.76486, "x":5.07401, "y":5.277, "heading":-2.13596, "vx":-0.5021, "vy":-0.23806, "omega":0.23672, "ax":1.80609, "ay":0.85632, "alpha":-0.59225, "fx":[27.55561,30.30972,31.46175,28.77747], "fy":[13.73981,11.44352,14.26859,16.54484]}, - {"t":1.7996, "x":5.05765, "y":5.26925, "heading":-2.12773, "vx":-0.43935, "vy":-0.20831, "omega":0.21615, "ax":1.80623, "ay":0.85638, "alpha":-0.6636, "fx":[27.32307,30.4196,31.69252,28.6782], "fy":[13.68744,11.14089,14.32414,16.84835]}, - {"t":1.83435, "x":5.04348, "y":5.26253, "heading":-2.12022, "vx":-0.3766, "vy":-0.17855, "omega":0.19309, "ax":1.80633, "ay":0.85643, "alpha":-0.73539, "fx":[27.08888,30.53228,31.92296,28.57617], "fy":[13.63256,10.83653,14.38183,17.15307]}, - {"t":1.86909, "x":5.03148, "y":5.25684, "heading":-2.11351, "vx":-0.31384, "vy":-0.1488, "omega":0.16754, "ax":1.80642, "ay":0.85647, "alpha":-0.80973, "fx":[26.84593,30.64992,32.16025,28.46969], "fy":[13.57463,10.52141,14.44233,17.46814]}, - {"t":1.90384, "x":5.02167, "y":5.25219, "heading":-2.10769, "vx":-0.25107, "vy":-0.11904, "omega":0.1394, "ax":1.80648, "ay":0.8565, "alpha":-0.88582, "fx":[26.59674,30.771,32.40215,28.3604], "fy":[13.51496,10.19878,14.5044,17.79043]}, - {"t":1.93858, "x":5.01404, "y":5.24857, "heading":-2.10285, "vx":-0.18831, "vy":-0.08928, "omega":0.10863, "ax":1.80654, "ay":0.85653, "alpha":-0.96416, "fx":[26.33942,30.89557,32.65052,28.24854], "fy":[13.45428,9.86639,14.56747,18.12214]}, - {"t":1.97332, "x":5.00858, "y":5.24599, "heading":-2.09908, "vx":-0.12554, "vy":-0.05952, "omega":0.07513, "ax":1.80659, "ay":0.85655, "alpha":-1.04479, "fx":[26.07346,31.02296,32.90586,28.13494], "fy":[13.39373,9.52382,14.63051,18.46367]}, - {"t":2.00807, "x":5.00531, "y":5.24443, "heading":-2.09647, "vx":-0.06277, "vy":-0.02976, "omega":0.03883, "ax":1.80663, "ay":0.85657, "alpha":-1.11751, "fx":[25.79595,31.14215,33.16956,28.03227], "fy":[13.34592,9.25013,14.67893,18.738]}, - {"t":2.04281, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17838, "ay":-1.95744, "alpha":0.01027, "fx":[-68.29393,-68.33891,-68.3231,-68.27813], "fy":[-32.03141,-31.93545,-31.96956,-32.06538]}, + {"t":0.03235, "x":7.10913, "y":6.23454, "heading":-2.35619, "vx":-0.13519, "vy":-0.06333, "omega":0.00033, "ax":-4.17808, "ay":-1.95731, "alpha":0.01057, "fx":[-68.28856,-68.33483,-68.31858,-68.27232], "fy":[-32.0302,-31.9315,-31.96659,-32.06514]}, + {"t":0.06471, "x":7.10257, "y":6.23147, "heading":-2.35618, "vx":-0.27036, "vy":-0.12666, "omega":0.00067, "ax":-4.17774, "ay":-1.95717, "alpha":0.0109, "fx":[-68.28255,-68.33026,-68.31351,-68.26581], "fy":[-32.02885,-31.92708,-31.96327,-32.06488]}, + {"t":0.09706, "x":7.09163, "y":6.22634, "heading":-2.35616, "vx":-0.40553, "vy":-0.18998, "omega":0.00103, "ax":-4.17735, "ay":-1.95701, "alpha":0.01127, "fx":[-68.27577,-68.3251,-68.30779,-68.25846], "fy":[-32.02733,-31.92209,-31.95952,-32.06459]}, + {"t":0.12942, "x":7.07633, "y":6.21917, "heading":-2.35613, "vx":-0.54069, "vy":-0.2533, "omega":0.00139, "ax":-4.17692, "ay":-1.95683, "alpha":0.01169, "fx":[-68.26806,-68.31925,-68.3013,-68.25012], "fy":[-32.0256,-31.91643,-31.95527,-32.06426]}, + {"t":0.16177, "x":7.05665, "y":6.20995, "heading":-2.35608, "vx":-0.67583, "vy":-0.31661, "omega":0.00177, "ax":-4.17642, "ay":-1.95662, "alpha":0.01218, "fx":[-68.25924,-68.31254,-68.29386,-68.24056], "fy":[-32.02362,-31.90994,-31.95039,-32.06388]}, + {"t":0.19412, "x":7.03259, "y":6.19869, "heading":-2.35603, "vx":-0.81095, "vy":-0.37991, "omega":0.00216, "ax":-4.17585, "ay":-1.95637, "alpha":0.01274, "fx":[-68.24902,-68.30478,-68.28525,-68.2295], "fy":[-32.02132,-31.90242,-31.94475,-32.06345]}, + {"t":0.22648, "x":7.00417, "y":6.18537, "heading":-2.35596, "vx":-0.94606, "vy":-0.44321, "omega":0.00258, "ax":-4.17517, "ay":-1.95609, "alpha":0.01339, "fx":[-68.23706,-68.29569,-68.27517,-68.21655], "fy":[-32.01863,-31.89362,-31.93815,-32.06293]}, + {"t":0.25883, "x":6.97138, "y":6.17001, "heading":-2.35587, "vx":-1.08114, "vy":-0.5065, "omega":0.00301, "ax":-4.17437, "ay":-1.95575, "alpha":0.01417, "fx":[-68.22286,-68.2849,-68.26322,-68.20118], "fy":[-32.01544,-31.88317,-31.93032,-32.06233]}, + {"t":0.29119, "x":6.93421, "y":6.1526, "heading":-2.35578, "vx":-1.2162, "vy":-0.56977, "omega":0.00347, "ax":-4.17341, "ay":-1.95534, "alpha":0.01511, "fx":[-68.20573,-68.27189,-68.2488,-68.18265], "fy":[-32.01159,-31.87057,-31.92087,-32.0616]}, + {"t":0.32354, "x":6.89268, "y":6.13314, "heading":-2.35566, "vx":-1.35122, "vy":-0.63304, "omega":0.00396, "ax":-4.17222, "ay":-1.95484, "alpha":0.01627, "fx":[-68.18468,-68.2559,-68.23107,-68.15986], "fy":[-32.00686,-31.85508,-31.90926,-32.06071]}, + {"t":0.35589, "x":6.84678, "y":6.11163, "heading":-2.35554, "vx":-1.48621, "vy":-0.69628, "omega":0.00448, "ax":-4.17073, "ay":-1.95421, "alpha":0.01773, "fx":[-68.15815,-68.23576,-68.20876,-68.13117], "fy":[-32.0009,-31.83556,-31.89465,-32.05958]}, + {"t":0.38825, "x":6.79651, "y":6.08808, "heading":-2.35539, "vx":-1.62115, "vy":-0.75951, "omega":0.00506, "ax":-4.16879, "ay":-1.9534, "alpha":0.01963, "fx":[-68.12371,-68.20961,-68.17981,-68.09393], "fy":[-31.99316,-31.81021,-31.87568,-32.05813]}, + {"t":0.4206, "x":6.74188, "y":6.06249, "heading":-2.35523, "vx":-1.75603, "vy":-0.82271, "omega":0.00569, "ax":-4.16617, "ay":-1.95229, "alpha":0.02219, "fx":[-68.07719,-68.1743,-68.14073,-68.04365], "fy":[-31.98271,-31.77598,-31.85008,-32.05617]}, + {"t":0.45296, "x":6.68288, "y":6.03485, "heading":-2.35504, "vx":-1.89082, "vy":-0.88588, "omega":0.00641, "ax":-4.16244, "ay":-1.95072, "alpha":0.02585, "fx":[-68.01089,-68.12401,-68.08512,-67.97204], "fy":[-31.96781,-31.7272,-31.81363,-32.05338]}, + {"t":0.48531, "x":6.61953, "y":6.00516, "heading":-2.35484, "vx":-2.02549, "vy":-0.94899, "omega":0.00725, "ax":-4.15671, "ay":-1.9483, "alpha":0.0315, "fx":[-67.9088,-68.0466,-67.99963,-67.86189], "fy":[-31.94489,-31.65207,-31.75758,-32.04912]}, + {"t":0.51766, "x":6.55182, "y":5.97344, "heading":-2.3546, "vx":-2.15998, "vy":-1.01202, "omega":0.00827, "ax":-4.14674, "ay":-1.94409, "alpha":0.04135, "fx":[-67.7312,-67.91207,-67.85136,-67.67061], "fy":[-31.90502,-31.5214,-31.66032,-32.04175]}, + {"t":0.55002, "x":6.47977, "y":5.93968, "heading":-2.35433, "vx":-2.29414, "vy":-1.07492, "omega":0.0096, "ax":-4.12514, "ay":-1.93496, "alpha":0.06291, "fx":[-67.34519,-67.62025,-67.53109,-67.25631], "fy":[-31.81851,-31.2374,-31.44995,-32.02603]}, + {"t":0.58237, "x":6.40338, "y":5.90389, "heading":-2.35402, "vx":-2.42761, "vy":-1.13753, "omega":0.01164, "ax":-4.04299, "ay":-1.90022, "alpha":0.14903, "fx":[-65.86064,-66.5122,-66.32823,-65.6796], "fy":[-31.49346,-30.1367,-30.65103,-31.97846]}, + {"t":0.61473, "x":6.32272, "y":5.86609, "heading":-2.35365, "vx":-2.55841, "vy":-1.19901, "omega":0.01646, "ax":3.86962, "ay":1.79787, "alpha":0.37431, "fx":[63.88861,62.30187,62.61506,64.23813], "fy":[28.46701,31.6037,30.4227,27.07384]}, + {"t":0.64708, "x":6.24197, "y":5.82824, "heading":-2.35311, "vx":-2.43322, "vy":-1.14084, "omega":0.02857, "ax":4.10975, "ay":1.92151, "alpha":0.07129, "fx":[67.29256,66.9825,67.08055,67.39098], "fy":[31.20846,31.85763,31.62079,30.96521]}, + {"t":0.67943, "x":6.1654, "y":5.79234, "heading":-2.35219, "vx":-2.30025, "vy":-1.07867, "omega":0.03088, "ax":4.14181, "ay":1.93805, "alpha":0.03787, "fx":[67.7656,67.6005,67.65584,67.82101], "fy":[31.57282,31.92141,31.79489,31.44447]}, + {"t":0.71179, "x":6.09315, "y":5.75845, "heading":-2.35119, "vx":-2.16625, "vy":-1.01597, "omega":0.0321, "ax":4.15449, "ay":1.94459, "alpha":0.02484, "fx":[67.95354,67.84519,67.8824,67.99078], "fy":[31.71745,31.94701,31.8637,31.63336]}, + {"t":0.74414, "x":6.02523, "y":5.7266, "heading":-2.35015, "vx":-2.03183, "vy":-0.95305, "omega":0.03291, "ax":4.16128, "ay":1.9481, "alpha":0.01789, "fx":[68.05443,67.97637,68.00357,68.08164], "fy":[31.79508,31.96075,31.90054,31.73446]}, + {"t":0.7765, "x":5.96167, "y":5.69678, "heading":-2.34909, "vx":-1.8972, "vy":-0.89002, "omega":0.03348, "ax":4.16551, "ay":1.95028, "alpha":0.01357, "fx":[68.11737,68.05817,68.07901,68.13822], "fy":[31.84351,31.96932,31.9235,31.79745]}, + {"t":0.80885, "x":5.90247, "y":5.66901, "heading":-2.348, "vx":-1.76243, "vy":-0.82692, "omega":0.03392, "ax":4.1684, "ay":1.95178, "alpha":0.01063, "fx":[68.1604,68.11405,68.13049,68.17685], "fy":[31.8766,31.97516,31.93917,31.84047]}, + {"t":0.8412, "x":5.84763, "y":5.64327, "heading":-2.34691, "vx":-1.62756, "vy":-0.76377, "omega":0.03427, "ax":4.1705, "ay":1.95286, "alpha":0.00849, "fx":[68.19166,68.15465,68.16787,68.20489], "fy":[31.90064,31.9794,31.95056,31.87171]}, + {"t":0.87356, "x":5.79716, "y":5.61959, "heading":-2.3458, "vx":-1.49263, "vy":-0.70059, "omega":0.03454, "ax":4.1721, "ay":1.95369, "alpha":0.00687, "fx":[68.21541,68.18548,68.19623,68.22617], "fy":[31.9189,31.98261,31.95921,31.89544]}, + {"t":0.90591, "x":5.75105, "y":5.59794, "heading":-2.34468, "vx":-1.35764, "vy":-0.63738, "omega":0.03476, "ax":0.05987, "ay":-0.10164, "alpha":1.71381, "fx":[7.49955,0.63808,-5.5102,1.28778], "fy":[-1.51775,5.21228,-1.8156,-8.52565]}, + {"t":0.9358, "x":5.71051, "y":5.57885, "heading":-2.34364, "vx":-1.35586, "vy":-0.64042, "omega":0.08598, "ax":0.02666, "ay":-0.05634, "alpha":1.57831, "fx":[6.42525,0.15385,-5.55325,0.71751], "fy":[-0.77006,5.40925,-1.07813,-7.24537]}, + {"t":0.96568, "x":5.67, "y":5.55969, "heading":-2.34107, "vx":-1.35506, "vy":-0.6421, "omega":0.13314, "ax":0.00839, "ay":-0.01769, "alpha":1.4456, "fx":[5.61955,-0.14249,-5.34418,0.41552], "fy":[-0.12749,5.50963,-0.45286,-6.086]}, + {"t":0.99556, "x":5.62951, "y":5.54049, "heading":-2.33709, "vx":-1.35481, "vy":-0.64263, "omega":0.17634, "ax":0.00201, "ay":-0.00424, "alpha":1.31612, "fx":[5.02143,-0.2486,-4.95236,0.31096], "fy":[0.10375,5.21164,-0.24286,-5.34959]}, + {"t":1.02544, "x":5.58903, "y":5.52129, "heading":-2.33182, "vx":-1.35475, "vy":-0.64276, "omega":0.21567, "ax":0.00049, "ay":-0.00089, "alpha":1.18973, "fx":[4.5221,-0.29981,-4.48433,0.29436], "fy":[0.16905,4.76071,-0.19838,-4.78982]}, + {"t":1.05532, "x":5.54855, "y":5.50208, "heading":-2.32538, "vx":-1.35473, "vy":-0.64278, "omega":0.25122, "ax":0.22004, "ay":0.11182, "alpha":1.05402, "fx":[7.64739,3.03381,-0.27413,3.9822], "fy":[2.00108,6.07518,1.65927,-2.42325]}, + {"t":1.08521, "x":5.50816, "y":5.48292, "heading":-2.31787, "vx":-1.34816, "vy":-0.63944, "omega":0.28272, "ax":1.7904, "ay":0.84953, "alpha":0.76819, "fx":[31.90966,28.96054,26.50897,29.69939], "fy":[13.80923,17.15687,14.00248,10.58411]}, + {"t":1.11509, "x":5.46867, "y":5.46419, "heading":-2.30942, "vx":-1.29466, "vy":-0.61406, "omega":0.30567, "ax":1.79896, "ay":0.85352, "alpha":0.66808, "fx":[31.71185,29.07092,27.0392,29.81636], "fy":[13.90053,16.80857,14.02872,11.07561]}, + {"t":1.14497, "x":5.43079, "y":5.44622, "heading":-2.30029, "vx":-1.2409, "vy":-0.58855, "omega":0.32564, "ax":1.8017, "ay":0.85477, "alpha":0.5736, "fx":[31.43378,29.13645,27.42873,29.81874], "fy":[13.95032,16.42794,14.0127,11.50457]}, + {"t":1.17485, "x":5.39451, "y":5.42902, "heading":-2.29056, "vx":-1.18706, "vy":-0.56301, "omega":0.34278, "ax":1.80306, "ay":0.85538, "alpha":0.48071, "fx":[31.13755,29.19577,27.78236,29.79051], "fy":[13.98584,16.03999,13.99188,11.9174]}, + {"t":1.20474, "x":5.35985, "y":5.41258, "heading":-2.28032, "vx":-1.13318, "vy":-0.53745, "omega":0.35714, "ax":1.80386, "ay":0.85573, "alpha":0.3885, "fx":[30.83462,29.25632,28.12082,29.74726], "fy":[14.01183,15.64899,13.97373,12.32352]}, + {"t":1.23462, "x":5.32679, "y":5.3969, "heading":-2.26964, "vx":-1.07928, "vy":-0.51188, "omega":0.36875, "ax":1.8044, "ay":0.85595, "alpha":0.29655, "fx":[30.52846,29.32092,28.4511,29.69374], "fy":[14.02953,15.25595,13.96093,12.72647]}, + {"t":1.2645, "x":5.29534, "y":5.38198, "heading":-2.25862, "vx":-1.02536, "vy":-0.4863, "omega":0.37761, "ax":1.80479, "ay":0.85611, "alpha":0.20463, "fx":[30.22036,29.39095,28.77634,29.63175], "fy":[14.03926,14.86115,13.95478,13.12791]}, + {"t":1.29438, "x":5.26551, "y":5.36783, "heading":-2.24734, "vx":-0.97143, "vy":-0.46072, "omega":0.38373, "ax":1.80508, "ay":0.85622, "alpha":0.11253, "fx":[29.91078,29.46728,29.09825,29.56202], "fy":[14.04101,14.46445,13.95603,13.52901]}, + {"t":1.32427, "x":5.23729, "y":5.35445, "heading":-2.23587, "vx":-0.91749, "vy":-0.43513, "omega":0.38709, "ax":1.8053, "ay":0.85631, "alpha":0.02011, "fx":[29.59986,29.55048,29.41788,29.4849], "fy":[14.03472,14.06571,13.9651,13.93052]}, + {"t":1.35415, "x":5.21068, "y":5.34183, "heading":-2.22431, "vx":-0.86354, "vy":-0.40954, "omega":0.38769, "ax":1.80549, "ay":0.85637, "alpha":-0.07278, "fx":[29.28753,29.64093,29.73598,29.40054], "fy":[14.0203,13.66462,13.98229,14.33312]}, + {"t":1.38403, "x":5.18568, "y":5.32997, "heading":-2.21272, "vx":-0.80959, "vy":-0.38395, "omega":0.38552, "ax":1.80563, "ay":0.85643, "alpha":-0.16627, "fx":[28.97366,29.73894,30.05311,29.30903], "fy":[13.99773,13.261,14.00768,14.7373]}, + {"t":1.41391, "x":5.16229, "y":5.31888, "heading":-2.2012, "vx":-0.75563, "vy":-0.35836, "omega":0.38055, "ax":1.80576, "ay":0.85647, "alpha":-0.26047, "fx":[28.65803,29.84465,30.36976,29.21047], "fy":[13.96701,12.85451,14.04129,15.14361]}, + {"t":1.4438, "x":5.14052, "y":5.30856, "heading":-2.18983, "vx":-0.70167, "vy":-0.33277, "omega":0.37276, "ax":1.80587, "ay":0.8565, "alpha":-0.35552, "fx":[28.34037,29.95813,30.68636,29.10498], "fy":[13.92827,12.44485,14.083,15.55252]}, + {"t":1.47368, "x":5.12036, "y":5.29899, "heading":-2.17869, "vx":-0.64771, "vy":-0.30717, "omega":0.36214, "ax":1.80596, "ay":0.85653, "alpha":-0.45152, "fx":[28.02036,30.07936,31.00334,28.99274], "fy":[13.88168,12.03169,14.1326,15.96451]}, + {"t":1.50356, "x":5.10181, "y":5.2902, "heading":-2.16787, "vx":-0.59374, "vy":-0.28158, "omega":0.34865, "ax":1.80604, "ay":0.85655, "alpha":-0.54859, "fx":[27.69766,30.20825,31.32109,28.87399], "fy":[13.82754,11.61468,14.18975,16.38005]}, + {"t":1.53344, "x":5.08487, "y":5.28217, "heading":-2.15745, "vx":-0.53977, "vy":-0.25598, "omega":0.33225, "ax":1.80611, "ay":0.85657, "alpha":-0.64684, "fx":[27.37185,30.3446,31.64007,28.74902], "fy":[13.76624,11.19342,14.25404,16.79965]}, + {"t":1.56332, "x":5.06955, "y":5.2749, "heading":-2.14752, "vx":-0.4858, "vy":-0.23039, "omega":0.31293, "ax":1.80617, "ay":0.85659, "alpha":-0.74639, "fx":[27.04247,30.48815,31.96074,28.61822], "fy":[13.69829,10.76746,14.32492,17.22381]}, + {"t":1.59321, "x":5.05584, "y":5.2684, "heading":-2.13817, "vx":-0.43183, "vy":-0.20479, "omega":0.29062, "ax":1.80622, "ay":0.85661, "alpha":-0.84736, "fx":[26.70898,30.63855,32.28359,28.48204], "fy":[13.62431,10.33631,14.40174,17.65311]}, + {"t":1.62309, "x":5.04374, "y":5.26266, "heading":-2.12949, "vx":-0.37785, "vy":-0.17919, "omega":0.2653, "ax":1.80627, "ay":0.85662, "alpha":-0.94985, "fx":[26.37081,30.79536,32.60916,28.34105], "fy":[13.54507,9.89942,14.48373,18.08812]}, + {"t":1.65297, "x":5.03326, "y":5.25769, "heading":-2.12156, "vx":-0.32388, "vy":-0.15359, "omega":0.23692, "ax":1.80632, "ay":0.85663, "alpha":-1.054, "fx":[26.02727,30.95804,32.93807,28.19589], "fy":[13.46148,9.45616,14.56998,18.52948]}, + {"t":1.68285, "x":5.02438, "y":5.25348, "heading":-2.11448, "vx":-0.2699, "vy":-0.12799, "omega":0.20542, "ax":1.80636, "ay":0.85664, "alpha":-1.15992, "fx":[25.67764,31.12596,33.27097,28.04733], "fy":[13.37462,9.00584,14.65948,18.97785]}, + {"t":1.71274, "x":5.01713, "y":5.25004, "heading":-2.10834, "vx":-0.21592, "vy":-0.1024, "omega":0.17076, "ax":1.80639, "ay":0.85665, "alpha":-1.26774, "fx":[25.32107,31.29838,33.60859,27.89626], "fy":[13.28571,8.54765,14.75106,19.43398]}, + {"t":1.74262, "x":5.01148, "y":5.24736, "heading":-2.10324, "vx":-0.16194, "vy":-0.0768, "omega":0.13288, "ax":1.80643, "ay":0.85666, "alpha":-1.37759, "fx":[24.95664,31.47444,33.9517,27.74369], "fy":[13.1962,8.08075,14.8434,19.89863]}, + {"t":1.7725, "x":5.00745, "y":5.24545, "heading":-2.09927, "vx":-0.10796, "vy":-0.0512, "omega":0.09171, "ax":1.80646, "ay":0.85667, "alpha":-1.4896, "fx":[24.58334,31.65319,34.30118,27.59075], "fy":[13.10774,7.60416,14.93501,20.37258]}, + {"t":1.80238, "x":5.00503, "y":5.2443, "heading":-2.09653, "vx":-0.05398, "vy":-0.0256, "omega":0.0472, "ax":1.80648, "ay":0.85667, "alpha":-1.57946, "fx":[24.19362,31.8159,34.65882,27.46195], "fy":[13.05416,7.30736,14.9856,20.67288]}, + {"t":1.83227, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index ce5aeaed..c0bfd7ab 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,64 +30,62 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.31293,2.15325], + "waypoints":[0.0,1.24249,1.89266], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42221, "ay":-3.09664, "alpha":-0.00515, "fx":[55.9244,55.96144,55.96874,55.93168], "fy":[-50.64862,-50.60771,-50.59955,-50.6405]}, - {"t":0.03979, "x":1.55286, "y":7.52095, "heading":-0.93501, "vx":0.13616, "vy":-0.1232, "omega":-0.00021, "ax":3.42199, "ay":-3.09648, "alpha":-0.00529, "fx":[55.92031,55.95834,55.96583,55.92778], "fy":[-50.64676,-50.60476,-50.59638,-50.63842]}, - {"t":0.07957, "x":1.56099, "y":7.5136, "heading":-0.93501, "vx":0.2723, "vy":-0.2464, "omega":-0.00042, "ax":3.42175, "ay":-3.09631, "alpha":-0.00544, "fx":[55.91575,55.95487,55.96257,55.92343], "fy":[-50.64467,-50.60147,-50.59285,-50.63609]}, - {"t":0.11936, "x":1.57453, "y":7.50134, "heading":-0.93503, "vx":0.40844, "vy":-0.36959, "omega":-0.00063, "ax":3.42148, "ay":-3.09612, "alpha":-0.00561, "fx":[55.91061,55.95096,55.95891,55.91853], "fy":[-50.64232,-50.59776,-50.58886,-50.63347]}, - {"t":0.15914, "x":1.59349, "y":7.48419, "heading":-0.93506, "vx":0.54457, "vy":-0.49277, "omega":-0.00086, "ax":3.42118, "ay":-3.0959, "alpha":-0.00581, "fx":[55.90478,55.94653,55.95475,55.91298], "fy":[-50.63966,-50.59356,-50.58435,-50.6305]}, - {"t":0.19893, "x":1.61786, "y":7.46213, "heading":-0.93509, "vx":0.68068, "vy":-0.61594, "omega":-0.00109, "ax":3.42083, "ay":-3.09565, "alpha":-0.00603, "fx":[55.89812,55.94147,55.95,55.90663], "fy":[-50.63662,-50.58876,-50.57919,-50.6271]}, - {"t":0.23871, "x":1.64765, "y":7.43518, "heading":-0.93513, "vx":0.81678, "vy":-0.7391, "omega":-0.00133, "ax":3.42043, "ay":-3.09536, "alpha":-0.00629, "fx":[55.89044,55.93563,55.94452,55.8993], "fy":[-50.63311,-50.58321,-50.57323,-50.62318]}, - {"t":0.2785, "x":1.68285, "y":7.40332, "heading":-0.93519, "vx":0.95287, "vy":-0.86226, "omega":-0.00158, "ax":3.41996, "ay":-3.09502, "alpha":-0.00659, "fx":[55.88147,55.92881,55.93812,55.89075], "fy":[-50.62901,-50.57675,-50.56628,-50.61861]}, - {"t":0.31829, "x":1.72347, "y":7.36656, "heading":-0.93525, "vx":1.08893, "vy":-0.98539, "omega":-0.00184, "ax":3.4194, "ay":-3.09462, "alpha":-0.00694, "fx":[55.87087,55.92075,55.93056,55.88063], "fy":[-50.62417,-50.5691,-50.55806,-50.6132]}, - {"t":0.35807, "x":1.7695, "y":7.32491, "heading":-0.93532, "vx":1.22498, "vy":-1.10852, "omega":-0.00211, "ax":3.41873, "ay":-3.09414, "alpha":-0.00737, "fx":[55.85813,55.91108,55.92147,55.86849], "fy":[-50.61836,-50.55992,-50.5482,-50.6067]}, - {"t":0.39786, "x":1.82094, "y":7.27836, "heading":-0.93541, "vx":1.36099, "vy":-1.23162, "omega":-0.00241, "ax":3.41792, "ay":-3.09355, "alpha":-0.00789, "fx":[55.84257,55.89924,55.91037,55.85365], "fy":[-50.61125,-50.5487,-50.53613,-50.59876]}, - {"t":0.43764, "x":1.87779, "y":7.22691, "heading":-0.9355, "vx":1.49698, "vy":-1.3547, "omega":-0.00272, "ax":3.4169, "ay":-3.09282, "alpha":-0.00854, "fx":[55.8231,55.88445,55.89648,55.83507], "fy":[-50.60236,-50.53466,-50.52103,-50.58882]}, - {"t":0.47743, "x":1.94006, "y":7.17056, "heading":-0.93561, "vx":1.63292, "vy":-1.47775, "omega":-0.00306, "ax":3.41558, "ay":-3.09188, "alpha":-0.00938, "fx":[55.79804,55.86541,55.8786,55.81117], "fy":[-50.59092,-50.5166,-50.5016,-50.57603]}, - {"t":0.51722, "x":2.00773, "y":7.10932, "heading":-0.93573, "vx":1.76881, "vy":-1.60076, "omega":-0.00344, "ax":3.41383, "ay":-3.09062, "alpha":-0.01051, "fx":[55.7646,55.84001,55.85474,55.77926], "fy":[-50.57565,-50.49249,-50.47566,-50.55895]}, - {"t":0.557, "x":2.0808, "y":7.04319, "heading":-0.93587, "vx":1.90463, "vy":-1.72372, "omega":-0.00385, "ax":3.41138, "ay":-3.08885, "alpha":-0.01208, "fx":[55.71773,55.80441,55.82129,55.73451], "fy":[-50.55424,-50.45871,-50.43929,-50.53501]}, - {"t":0.59679, "x":2.15928, "y":6.97216, "heading":-0.93602, "vx":2.04036, "vy":-1.84662, "omega":-0.00433, "ax":3.40768, "ay":-3.08619, "alpha":-0.01445, "fx":[55.6473,55.75092,55.77101,55.66725], "fy":[-50.52208,-50.40794,-50.38461,-50.49901]}, - {"t":0.63657, "x":2.24316, "y":6.89625, "heading":-0.93619, "vx":2.17594, "vy":-1.9694, "omega":-0.00491, "ax":3.40151, "ay":-3.08176, "alpha":-0.01843, "fx":[55.52961,55.66156,55.68693,55.55474], "fy":[-50.46829,-50.32311,-50.29316,-50.43876]}, - {"t":0.67636, "x":2.33242, "y":6.81546, "heading":-0.93639, "vx":2.31127, "vy":-2.09201, "omega":-0.00564, "ax":3.3891, "ay":-3.07282, "alpha":-0.02645, "fx":[55.29311,55.48204,55.51773,55.32829], "fy":[-50.36006,-50.15267,-50.10913,-50.31738]}, - {"t":0.71614, "x":2.42706, "y":6.72979, "heading":-0.93661, "vx":2.44611, "vy":-2.21427, "omega":-0.00669, "ax":3.35133, "ay":-3.0456, "alpha":-0.05106, "fx":[54.57503,54.93707,55.00156,54.63764], "fy":[-50.02987,-49.63524,-49.5481,-49.94587]}, - {"t":0.75593, "x":2.52703, "y":6.63929, "heading":-0.93688, "vx":2.57944, "vy":-2.33544, "omega":-0.00873, "ax":-2.00945, "ay":1.57646, "alpha":-1.26235, "fx":[-35.15185,-28.41142,-30.82392,-37.01526], "fy":[20.21366,26.62332,31.04791,25.20342]}, - {"t":0.79572, "x":2.62806, "y":6.54762, "heading":-0.93723, "vx":2.49949, "vy":-2.27272, "omega":-0.05895, "ax":-3.36877, "ay":3.03455, "alpha":-0.04392, "fx":[-55.2569,-54.94547,-54.88982,-55.19988], "fy":[49.39969,49.74314,49.81722,49.4761]}, - {"t":0.8355, "x":2.72484, "y":6.4596, "heading":-0.93957, "vx":2.36547, "vy":-2.15199, "omega":-0.0607, "ax":-3.39688, "ay":3.06631, "alpha":-0.02137, "fx":[-55.62346,-55.47097,-55.44172,-55.59389], "fy":[50.02624,50.1946,50.23005,50.06226]}, - {"t":0.87529, "x":2.81627, "y":6.37641, "heading":-0.94199, "vx":2.23032, "vy":-2.02999, "omega":-0.06155, "ax":-3.40649, "ay":3.07718, "alpha":-0.01363, "fx":[-55.74804,-55.65055,-55.63126,-55.72861], "fy":[50.24089,50.34856,50.37123,50.26378]}, - {"t":0.91507, "x":2.90231, "y":6.29808, "heading":-0.94444, "vx":2.09479, "vy":-1.90756, "omega":-0.06209, "ax":-3.41134, "ay":3.08268, "alpha":-0.00971, "fx":[-55.81069,-55.74122,-55.72717,-55.79658], "fy":[50.34941,50.42615,50.4424,50.36578]}, - {"t":0.95486, "x":2.98295, "y":6.22462, "heading":-0.94691, "vx":1.95906, "vy":-1.78492, "omega":-0.06248, "ax":-3.41427, "ay":3.08599, "alpha":-0.00733, "fx":[-55.84836,-55.79592,-55.78513,-55.83753], "fy":[50.41496,50.47289,50.48526,50.4274]}, - {"t":0.99465, "x":3.05819, "y":6.15605, "heading":-0.94939, "vx":1.82323, "vy":-1.66214, "omega":-0.06277, "ax":-3.41622, "ay":3.08821, "alpha":-0.00572, "fx":[-55.87349,-55.83251,-55.82394,-55.86489], "fy":[50.45885,50.50413,50.51388,50.46865]}, - {"t":1.03443, "x":3.12802, "y":6.09237, "heading":-0.95189, "vx":1.68731, "vy":-1.53927, "omega":-0.06299, "ax":-3.41762, "ay":3.0898, "alpha":-0.00457, "fx":[-55.89143,-55.85871,-55.85177,-55.88447], "fy":[50.49031,50.52647,50.53434,50.49821]}, - {"t":1.07422, "x":3.19245, "y":6.03357, "heading":-0.9544, "vx":1.55134, "vy":-1.41634, "omega":-0.06318, "ax":-3.41867, "ay":3.09099, "alpha":-0.0037, "fx":[-55.90488,-55.87839,-55.87269,-55.89917], "fy":[50.51397,50.54324,50.54968,50.52043]}, - {"t":1.114, "x":3.25146, "y":5.97967, "heading":-0.95691, "vx":1.41532, "vy":-1.29336, "omega":-0.06332, "ax":-3.41949, "ay":3.09192, "alpha":-0.00302, "fx":[-55.91533,-55.89372,-55.889,-55.91061], "fy":[50.53241,50.5563,50.56161,50.53774]}, - {"t":1.15379, "x":3.30507, "y":5.93066, "heading":-0.95943, "vx":1.27927, "vy":-1.17035, "omega":-0.06344, "ax":-3.42015, "ay":3.09266, "alpha":-0.00248, "fx":[-55.92369,-55.90599,-55.90208,-55.91977], "fy":[50.5472,50.56676,50.57115,50.5516]}, - {"t":1.19357, "x":3.35326, "y":5.88654, "heading":-0.96195, "vx":1.1432, "vy":-1.0473, "omega":-0.06354, "ax":-3.42068, "ay":3.09327, "alpha":-0.00203, "fx":[-55.93052,-55.91603,-55.91279,-55.92727], "fy":[50.55931,50.57532,50.57896,50.56296]}, - {"t":1.23336, "x":3.39603, "y":5.84732, "heading":-0.96448, "vx":1.00711, "vy":-0.92424, "omega":-0.06362, "ax":-3.42113, "ay":3.09378, "alpha":-0.00165, "fx":[-55.9362,-55.92441,-55.92173,-55.93353], "fy":[50.56942,50.58246,50.58545,50.57242]}, - {"t":1.27315, "x":3.43339, "y":5.813, "heading":-0.96701, "vx":0.87099, "vy":-0.80115, "omega":-0.06369, "ax":-3.42151, "ay":3.09421, "alpha":-0.00133, "fx":[-55.94101,-55.93149,-55.92931,-55.93882], "fy":[50.57799,50.5885,50.59095,50.58043]}, - {"t":1.31293, "x":3.46534, "y":5.78357, "heading":-0.96955, "vx":0.73487, "vy":-0.67804, "omega":-0.06374, "ax":-1.18481, "ay":-1.19859, "alpha":-0.55591, "fx":[-19.52027,-17.16003,-19.24526,-21.55216], "fy":[-21.64942,-20.27563,-17.49848,-18.95514]}, - {"t":1.35113, "x":3.49254, "y":5.7568, "heading":-0.97198, "vx":0.68961, "vy":-0.72382, "omega":-0.08498, "ax":-0.08328, "ay":-0.079, "alpha":-0.61409, "fx":[-1.83056,0.99955,-0.8941,-3.72098], "fy":[-3.64021,-1.71427,1.06062,-0.87217]}, - {"t":1.38932, "x":3.51882, "y":5.72909, "heading":-0.97523, "vx":0.68643, "vy":-0.72684, "omega":-0.10843, "ax":-0.00494, "ay":-0.00466, "alpha":-0.5049, "fx":[-0.47315,1.85865,0.31173,-2.01994], "fy":[-2.00749,-0.42752,1.85528,0.27503]}, - {"t":1.42752, "x":3.54504, "y":5.70133, "heading":-0.97937, "vx":0.68624, "vy":-0.72702, "omega":-0.12772, "ax":-0.00029, "ay":-0.00028, "alpha":-0.39778, "fx":[-0.32025,1.52192,0.31072,-1.53145], "fy":[-1.52481,-0.28757,1.51582,0.27857]}, - {"t":1.46572, "x":3.57125, "y":5.67356, "heading":-0.98425, "vx":0.68623, "vy":-0.72703, "omega":-0.14291, "ax":-0.00002, "ay":-0.00002, "alpha":-0.29289, "fx":[-0.23803,1.12279,0.23747,-1.12335], "fy":[-1.11853,-0.21417,1.118,0.21364]}, - {"t":1.50391, "x":3.59746, "y":5.64579, "heading":-0.98971, "vx":0.68623, "vy":-0.72703, "omega":-0.1541, "ax":0.0, "ay":0.0, "alpha":-0.18964, "fx":[-0.15791,0.7264,0.15787,-0.72643], "fy":[-0.72323,-0.14249,0.7232,0.14245]}, - {"t":1.54211, "x":3.62367, "y":5.61802, "heading":-0.99559, "vx":0.68623, "vy":-0.72703, "omega":-0.16134, "ax":0.0, "ay":0.0, "alpha":-0.08746, "fx":[-0.07478,0.33464,0.07478,-0.33464], "fy":[-0.33312,-0.06768,0.33312,0.06768]}, - {"t":1.58031, "x":3.64989, "y":5.59025, "heading":-1.00175, "vx":0.68623, "vy":-0.72703, "omega":-0.16468, "ax":0.0, "ay":0.0, "alpha":0.01422, "fx":[0.01249,-0.05434,-0.01249,0.05434], "fy":[0.05408,0.01134,-0.05408,-0.01134]}, - {"t":1.6185, "x":3.6761, "y":5.56248, "heading":-1.00804, "vx":0.68623, "vy":-0.72703, "omega":-0.16414, "ax":0.0, "ay":0.0, "alpha":0.11598, "fx":[0.10466,-0.4426,-0.10466,0.4426], "fy":[0.44047,0.09527,-0.44047,-0.09527]}, - {"t":1.6567, "x":3.70231, "y":5.53471, "heading":-1.01431, "vx":0.68623, "vy":-0.72703, "omega":-0.15971, "ax":0.0, "ay":0.0, "alpha":0.2184, "fx":[0.20228,-0.83229,-0.20228,0.83229], "fy":[0.82817,0.18461,-0.82817,-0.18461]}, - {"t":1.69489, "x":3.72852, "y":5.50694, "heading":-1.02041, "vx":0.68623, "vy":-0.72703, "omega":-0.15137, "ax":0.0, "ay":0.0, "alpha":0.32205, "fx":[0.30571,-1.22558,-0.30571,1.22558], "fy":[1.21936,0.2797,-1.21936,-0.2797]}, - {"t":1.73309, "x":3.75473, "y":5.47917, "heading":-1.0262, "vx":0.68623, "vy":-0.72703, "omega":-0.13907, "ax":0.0, "ay":0.0, "alpha":0.42751, "fx":[0.41517,-1.62474,-0.41517,1.62474], "fy":[1.61627,0.38069,-1.61627,-0.38069]}, - {"t":1.77129, "x":3.78094, "y":5.4514, "heading":-1.03151, "vx":0.68623, "vy":-0.72703, "omega":-0.12274, "ax":0.0, "ay":0.0, "alpha":0.53536, "fx":[0.53066,-2.03208,-0.53066,2.03208], "fy":[2.02125,0.48754,-2.02125,-0.48754]}, - {"t":1.80948, "x":3.80715, "y":5.42363, "heading":-1.0362, "vx":0.68623, "vy":-0.72703, "omega":-0.10229, "ax":0.0, "ay":0.0, "alpha":0.64621, "fx":[0.65197,-2.45005,-0.65197,2.45005], "fy":[2.43673,0.59998,-2.43673,-0.59998]}, - {"t":1.84768, "x":3.83336, "y":5.39586, "heading":-1.0401, "vx":0.68623, "vy":-0.72703, "omega":-0.07761, "ax":0.0, "ay":0.0, "alpha":0.76067, "fx":[0.77864,-2.88121,-0.77865,2.88121], "fy":[2.8653,0.71751,-2.8653,-0.71751]}, - {"t":1.88588, "x":3.85958, "y":5.36809, "heading":-1.04307, "vx":0.68623, "vy":-0.72703, "omega":-0.04855, "ax":-0.00007, "ay":0.00007, "alpha":0.87934, "fx":[0.90877,-3.32942,-0.91111,3.3271], "fy":[3.31085,0.84052,-3.30846,-0.83813]}, - {"t":1.92407, "x":3.88579, "y":5.34032, "heading":-1.04492, "vx":0.68623, "vy":-0.72703, "omega":-0.01496, "ax":-2.15208, "ay":2.28004, "alpha":0.37433, "fx":[-34.1614,-36.44229,-36.23808,-33.88762], "fy":[38.79323,36.99332,35.72035,37.59013]}, - {"t":1.96227, "x":3.91043, "y":5.31421, "heading":-1.04549, "vx":0.60402, "vy":-0.63994, "omega":-0.00067, "ax":-3.15282, "ay":3.34028, "alpha":0.0067, "fx":[-51.51099,-51.56264,-51.57427,-51.52259], "fy":[54.63727,54.58866,54.57712,54.62578]}, - {"t":2.00046, "x":3.9312, "y":5.29221, "heading":-1.04552, "vx":0.4836, "vy":-0.51235, "omega":-0.00041, "ax":-3.16192, "ay":3.34992, "alpha":0.00375, "fx":[-51.67363,-51.70261,-51.70921,-51.68023], "fy":[54.7817,54.7544,54.748,54.77533]}, - {"t":2.03866, "x":3.94737, "y":5.27508, "heading":-1.04553, "vx":0.36283, "vy":-0.3844, "omega":-0.00027, "ax":-3.16499, "ay":3.35317, "alpha":0.00276, "fx":[-51.72848,-51.7498,-51.75468,-51.73335], "fy":[54.83039,54.81028,54.8056,54.82571]}, - {"t":2.07686, "x":3.95892, "y":5.26285, "heading":-1.04554, "vx":0.24193, "vy":-0.25632, "omega":-0.00016, "ax":-3.16653, "ay":3.3548, "alpha":0.00226, "fx":[-51.75601,-51.77349,-51.7775,-51.76001], "fy":[54.85483,54.83834,54.83451,54.851]}, - {"t":2.11505, "x":3.96585, "y":5.2555, "heading":-1.04555, "vx":0.12098, "vy":-0.12818, "omega":-0.00007, "ax":-3.16746, "ay":3.35579, "alpha":0.00196, "fx":[-51.77257,-51.78773,-51.79122,-51.77605], "fy":[54.86952,54.85521,54.85189,54.86621]}, - {"t":2.15325, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42298, "ay":-3.09524, "alpha":-0.01592, "fx":[55.89073,56.00508,56.02772,55.91319], "fy":[-50.67707,-50.55073,-50.52536,-50.65202]}, + {"t":0.03765, "x":1.55258, "y":7.5212, "heading":-0.93501, "vx":0.12888, "vy":-0.11654, "omega":-0.0006, "ax":3.42275, "ay":-3.0951, "alpha":-0.01626, "fx":[55.88554,56.00232,56.02544,55.90847], "fy":[-50.67633,-50.54731,-50.52139,-50.65074]}, + {"t":0.0753, "x":1.55986, "y":7.51462, "heading":-0.93503, "vx":0.25775, "vy":-0.23307, "omega":-0.00121, "ax":3.4225, "ay":-3.09494, "alpha":-0.01664, "fx":[55.87978,55.99925,56.0229,55.90322], "fy":[-50.6755,-50.54351,-50.51699,-50.64932]}, + {"t":0.11295, "x":1.57199, "y":7.50365, "heading":-0.93507, "vx":0.38661, "vy":-0.3496, "omega":-0.00184, "ax":3.42221, "ay":-3.09476, "alpha":-0.01706, "fx":[55.87333,55.99582,56.02006,55.89736], "fy":[-50.67458,-50.53928,-50.51206,-50.64773]}, + {"t":0.1506, "x":1.58897, "y":7.4883, "heading":-0.93514, "vx":0.51546, "vy":-0.46612, "omega":-0.00248, "ax":3.42189, "ay":-3.09456, "alpha":-0.01753, "fx":[55.86609,55.99196,56.01687,55.89077], "fy":[-50.67355,-50.53451,-50.50652,-50.64595]}, + {"t":0.18826, "x":1.6108, "y":7.46855, "heading":-0.93524, "vx":0.6443, "vy":-0.58264, "omega":-0.00314, "ax":3.42153, "ay":-3.09433, "alpha":-0.01807, "fx":[55.85788,55.98758,56.01326,55.88331], "fy":[-50.67238,-50.52911,-50.50024,-50.64392]}, + {"t":0.22591, "x":1.63748, "y":7.44442, "heading":-0.93535, "vx":0.77312, "vy":-0.69914, "omega":-0.00382, "ax":3.42112, "ay":-3.09407, "alpha":-0.01868, "fx":[55.84849,55.98259,56.00913,55.87478], "fy":[-50.67104,-50.52295,-50.49307,-50.6416]}, + {"t":0.26356, "x":1.66902, "y":7.41591, "heading":-0.9355, "vx":0.90193, "vy":-0.81563, "omega":-0.00452, "ax":3.42064, "ay":-3.09377, "alpha":-0.01939, "fx":[55.83768,55.97683,56.00437,55.86495], "fy":[-50.66951,-50.51585,-50.48479,-50.63893]}, + {"t":0.30121, "x":1.7054, "y":7.383, "heading":-0.93567, "vx":1.03072, "vy":-0.93212, "omega":-0.00525, "ax":3.42008, "ay":-3.09342, "alpha":-0.02021, "fx":[55.82506,55.97011,55.99883,55.85348], "fy":[-50.66771,-50.50756,-50.47514,-50.63581]}, + {"t":0.33886, "x":1.74663, "y":7.34572, "heading":-0.93587, "vx":1.15949, "vy":-1.04859, "omega":-0.00601, "ax":3.41943, "ay":-3.09301, "alpha":-0.02118, "fx":[55.81016,55.96217,55.99228,55.83994], "fy":[-50.6656,-50.49778,-50.46374,-50.63212]}, + {"t":0.37651, "x":1.79271, "y":7.30404, "heading":-0.93609, "vx":1.28824, "vy":-1.16504, "omega":-0.00681, "ax":3.41864, "ay":-3.09251, "alpha":-0.02235, "fx":[55.7923,55.95266,55.98442,55.8237], "fy":[-50.66306,-50.48606,-50.45007,-50.6277]}, + {"t":0.41416, "x":1.84364, "y":7.25799, "heading":-0.93635, "vx":1.41695, "vy":-1.28148, "omega":-0.00765, "ax":3.41768, "ay":-3.09191, "alpha":-0.02378, "fx":[55.7705,55.94105,55.97484,55.80387], "fy":[-50.65997,-50.47176,-50.43337,-50.62229]}, + {"t":0.45181, "x":1.89941, "y":7.20755, "heading":-0.93664, "vx":1.54563, "vy":-1.39789, "omega":-0.00855, "ax":3.41648, "ay":-3.09116, "alpha":-0.02556, "fx":[55.74327,55.92656,55.96287,55.77911], "fy":[-50.6561,-50.4539,-50.41253,-50.61555]}, + {"t":0.48946, "x":1.96003, "y":7.15272, "heading":-0.93696, "vx":1.67427, "vy":-1.51428, "omega":-0.00951, "ax":3.41494, "ay":-3.09019, "alpha":-0.02785, "fx":[55.70834,55.90797,55.94752,55.74732], "fy":[-50.65114,-50.43099,-50.38576,-50.60688]}, + {"t":0.52712, "x":2.02549, "y":7.09352, "heading":-0.93732, "vx":1.80284, "vy":-1.63063, "omega":-0.01056, "ax":3.41289, "ay":-3.0889, "alpha":-0.0309, "fx":[55.66188,55.88326,55.92708,55.70501], "fy":[-50.64455,-50.40053,-50.35013,-50.59535]}, + {"t":0.56477, "x":2.09579, "y":7.02993, "heading":-0.93772, "vx":1.93134, "vy":-1.74693, "omega":-0.01172, "ax":3.41004, "ay":-3.0871, "alpha":-0.03516, "fx":[55.59706,55.8488,55.89856,55.64592], "fy":[-50.63533,-50.35803,-50.30039,-50.57924]}, + {"t":0.60242, "x":2.17092, "y":6.96197, "heading":-0.93816, "vx":2.05973, "vy":-1.86316, "omega":-0.01305, "ax":3.40577, "ay":-3.08442, "alpha":-0.04154, "fx":[55.50032,55.7974,55.85596,55.55763], "fy":[-50.62156,-50.29464,-50.22608,-50.55515]}, + {"t":0.64007, "x":2.25088, "y":6.88964, "heading":-0.93865, "vx":2.18796, "vy":-1.97929, "omega":-0.01461, "ax":3.39871, "ay":-3.07997, "alpha":-0.0521, "fx":[55.34046,55.71254,55.78543,55.41138], "fy":[-50.5987,-50.18994,-50.10305,-50.51518]}, + {"t":0.67772, "x":2.33567, "y":6.81293, "heading":-0.9392, "vx":2.31593, "vy":-2.09526, "omega":-0.01657, "ax":3.3848, "ay":-3.0712, "alpha":-0.07301, "fx":[55.02581,55.54567,55.64606,55.12236], "fy":[-50.5533,-49.98407,-49.8601,-50.43589]}, + {"t":0.71537, "x":2.42527, "y":6.73186, "heading":-0.93982, "vx":2.44337, "vy":-2.21089, "omega":-0.01932, "ax":3.34461, "ay":-3.0458, "alpha":-0.13385, "fx":[54.12159,55.06675,55.2407,54.28269], "fy":[-50.41915,-49.3939,-49.156,-50.20279]}, + {"t":0.75302, "x":2.51963, "y":6.64646, "heading":-0.94055, "vx":2.5693, "vy":-2.32557, "omega":-0.02436, "ax":2.00182, "ay":-2.12377, "alpha":-2.4523, "fx":[28.16582,41.09782,38.75506,22.88519], "fy":[-43.783,-32.95583,-24.2922,-37.84709]}, + {"t":0.79067, "x":2.61779, "y":6.5574, "heading":-0.94147, "vx":2.64467, "vy":-2.40553, "omega":-0.11669, "ax":-3.361, "ay":3.01304, "alpha":-0.13185, "fx":[-55.49683,-54.56436,-54.40098,-55.32147], "fy":[48.62263,49.65633,49.88191,48.86911]}, + {"t":0.82832, "x":2.71498, "y":6.46896, "heading":-0.94586, "vx":2.51812, "vy":-2.29209, "omega":-0.12166, "ax":-3.39492, "ay":3.05633, "alpha":-0.06069, "fx":[-55.75946,-55.32726,-55.2429,-55.67252], "fy":[49.67292,50.15149,50.255,49.78096]}, + {"t":0.86597, "x":2.80739, "y":6.38483, "heading":-0.95044, "vx":2.3903, "vy":-2.17701, "omega":-0.12394, "ax":-3.4059, "ay":3.07038, "alpha":-0.03754, "fx":[-55.84157,-55.57385,-55.51899,-55.78573], "fy":[50.01389,50.31023,50.3751,50.08052]}, + {"t":0.90363, "x":2.89497, "y":6.30504, "heading":-0.95511, "vx":2.26206, "vy":-2.06141, "omega":-0.12536, "ax":-3.41133, "ay":3.07735, "alpha":-0.026, "fx":[-55.88133,-55.69583,-55.65647,-55.8415], "fy":[50.18304,50.38832,50.43405,50.22961]}, + {"t":0.94128, "x":2.97772, "y":6.22961, "heading":-0.95983, "vx":2.13362, "vy":-1.94555, "omega":-0.12634, "ax":-3.41457, "ay":3.0815, "alpha":-0.01907, "fx":[-55.90465,-55.76862,-55.73889,-55.87467], "fy":[50.28424,50.43475,50.46892,50.31886]}, + {"t":0.97893, "x":3.05563, "y":6.15854, "heading":-0.96458, "vx":2.00506, "vy":-1.82952, "omega":-0.12705, "ax":-3.41672, "ay":3.08426, "alpha":-0.01444, "fx":[-55.9199,-55.81697,-55.79387,-55.89666], "fy":[50.35165,50.46553,50.4919,50.37828]}, + {"t":1.01658, "x":3.12871, "y":6.09184, "heading":-0.96937, "vx":1.87642, "vy":-1.7134, "omega":-0.1276, "ax":-3.41825, "ay":3.08623, "alpha":-0.01112, "fx":[-55.93062,-55.85141,-55.83318,-55.91231], "fy":[50.39981,50.48745,50.50815,50.42067]}, + {"t":1.05423, "x":3.19693, "y":6.02952, "heading":-0.97417, "vx":1.74772, "vy":-1.5972, "omega":-0.12802, "ax":-3.4194, "ay":3.08771, "alpha":-0.00862, "fx":[-55.93854,-55.87717,-55.86271,-55.92404], "fy":[50.43596,50.50385,50.52022,50.45242]}, + {"t":1.09188, "x":3.26031, "y":5.97157, "heading":-0.97899, "vx":1.61897, "vy":-1.48094, "omega":-0.12834, "ax":-3.42029, "ay":3.08885, "alpha":-0.00667, "fx":[-55.94462,-55.89716,-55.88572,-55.93315], "fy":[50.46411,50.51661,50.52952,50.47707]}, + {"t":1.12953, "x":3.31884, "y":5.918, "heading":-0.98382, "vx":1.4902, "vy":-1.36464, "omega":-0.12859, "ax":-3.421, "ay":3.08977, "alpha":-0.00511, "fx":[-55.94942,-55.91311,-55.90416,-55.94046], "fy":[50.48666,50.52683,50.5369,50.49676]}, + {"t":1.16718, "x":3.37253, "y":5.86881, "heading":-0.98867, "vx":1.36139, "vy":-1.24831, "omega":-0.12878, "ax":-3.42158, "ay":3.09051, "alpha":-0.00383, "fx":[-55.95329,-55.92612,-55.91928,-55.94644], "fy":[50.50514,50.5352,50.54288,50.51284]}, + {"t":1.20483, "x":3.42136, "y":5.824, "heading":-0.99351, "vx":1.23257, "vy":-1.13195, "omega":-0.12893, "ax":-3.42206, "ay":3.09114, "alpha":-0.00276, "fx":[-55.95648,-55.93693,-55.9319,-55.95145], "fy":[50.52056,50.54219,50.54783,50.52621]}, + {"t":1.24249, "x":3.46534, "y":5.78357, "heading":-0.99837, "vx":1.10372, "vy":-1.01556, "omega":-0.12903, "ax":-1.79347, "ay":-1.82922, "alpha":-0.51189, "fx":[-29.18831,-27.19212,-29.49743,-31.40171], "fy":[-31.68933,-30.93732,-28.0663,-28.92436]}, + {"t":1.27499, "x":3.50027, "y":5.74959, "heading":-1.00256, "vx":1.04542, "vy":-1.07503, "omega":-0.14567, "ax":-0.45558, "ay":-0.43702, "alpha":-0.65351, "fx":[-7.98189,-4.93786,-6.923,-9.94863], "fy":[-9.61063,-7.72567,-4.65626,-6.58501]}, + {"t":1.3075, "x":3.53402, "y":5.71441, "heading":-1.0073, "vx":1.03061, "vy":-1.08924, "omega":-0.16692, "ax":-0.08038, "ay":-0.07587, "alpha":-0.38805, "fx":[-1.66253,0.1674,-0.9661,-2.79498], "fy":[-2.71349,-1.55952,0.23414,-0.92254]}, + {"t":1.34001, "x":3.56748, "y":5.67896, "heading":-1.01273, "vx":1.028, "vy":-1.0917, "omega":-0.17953, "ax":-0.01382, "ay":-0.01301, "alpha":-0.09976, "fx":[-0.31774,0.15433,-0.13418,-0.60624], "fy":[-0.59109,-0.29642,0.16574,-0.12897]}, + {"t":1.37252, "x":3.60089, "y":5.64347, "heading":-1.01856, "vx":1.02755, "vy":-1.09213, "omega":-0.18277, "ax":-0.00237, "ay":-0.00223, "alpha":0.18863, "fx":[0.13897,-0.7569,-0.2165,0.67938], "fy":[0.67806,0.12602,-0.75098,-0.19897]}, + {"t":1.40503, "x":3.63429, "y":5.60796, "heading":-1.0245, "vx":1.02747, "vy":-1.0922, "omega":-0.17664, "ax":-0.00041, "ay":-0.00038, "alpha":0.47768, "fx":[0.45419,-1.82277,-0.46749,1.80948], "fy":[1.80049,0.41604,-1.81298,-0.42856]}, + {"t":1.43754, "x":3.66769, "y":5.57246, "heading":-1.03025, "vx":1.02746, "vy":-1.09221, "omega":-0.16111, "ax":-0.00007, "ay":-0.00007, "alpha":0.7684, "fx":[0.75685,-2.91865,-0.75913,2.91637], "fy":[2.90097,0.69501,-2.90311,-0.69716]}, + {"t":1.47004, "x":3.70109, "y":5.53695, "heading":-1.03548, "vx":1.02745, "vy":-1.09221, "omega":-0.13613, "ax":-0.00001, "ay":-0.00001, "alpha":1.06179, "fx":[1.0682,-4.02656,-1.06859,4.02616], "fy":[4.00435,0.98277,-4.00472,-0.98315]}, + {"t":1.50255, "x":3.7345, "y":5.50144, "heading":-1.03991, "vx":1.02745, "vy":-1.09221, "omega":-0.10162, "ax":-0.00001, "ay":0.0, "alpha":1.35879, "fx":[1.38978,-5.14716,-1.39006,5.14688], "fy":[5.11867,1.28077,-5.11855,-1.28065]}, + {"t":1.53506, "x":3.7679, "y":5.46594, "heading":-1.04321, "vx":1.02745, "vy":-1.09221, "omega":-0.05745, "ax":-0.07403, "ay":0.07869, "alpha":1.65856, "fx":[0.49807,-7.47444,-2.94322,5.07885], "fy":[7.52547,2.87082,-4.9602,-0.29016]}, + {"t":1.56757, "x":3.80126, "y":5.43047, "heading":-1.04508, "vx":1.02505, "vy":-1.08966, "omega":-0.00353, "ax":-3.12096, "ay":3.31767, "alpha":0.0287, "fx":[-50.88787,-51.10815,-51.15581,-50.93489], "fy":[54.36555,54.15988,54.10912,54.31575]}, + {"t":1.60008, "x":3.83293, "y":5.3968, "heading":-1.04519, "vx":0.92359, "vy":-0.9818, "omega":-0.00259, "ax":-3.14452, "ay":3.34272, "alpha":0.01562, "fx":[-51.33317,-51.4538,-51.48069,-51.35987], "fy":[54.71694,54.60387,54.57703,54.69039]}, + {"t":1.63259, "x":3.86129, "y":5.36665, "heading":-1.04528, "vx":0.82137, "vy":-0.87314, "omega":-0.00209, "ax":-3.15179, "ay":3.35045, "alpha":0.0116, "fx":[-51.47083,-51.56058,-51.58077,-51.49092], "fy":[54.82539,54.74117,54.72142,54.8058]}, + {"t":1.6651, "x":3.88633, "y":5.34004, "heading":-1.04535, "vx":0.71891, "vy":-0.76422, "omega":-0.00171, "ax":-3.15532, "ay":3.3542, "alpha":0.00965, "fx":[-51.53775,-51.61247,-51.62936,-51.55457], "fy":[54.87808,54.80792,54.79156,54.86184]}, + {"t":1.6976, "x":3.90803, "y":5.31697, "heading":-1.0454, "vx":0.61633, "vy":-0.65518, "omega":-0.0014, "ax":-3.15741, "ay":3.35642, "alpha":0.0085, "fx":[-51.57731,-51.64314,-51.65807,-51.59218], "fy":[54.90922,54.84738,54.83301,54.89494]}, + {"t":1.73011, "x":3.9264, "y":5.29744, "heading":-1.04545, "vx":0.51369, "vy":-0.54607, "omega":-0.00112, "ax":-3.15879, "ay":3.35789, "alpha":0.00774, "fx":[-51.60344,-51.6634,-51.67701,-51.61701], "fy":[54.92977,54.87344,54.86038,54.91679]}, + {"t":1.76262, "x":3.94143, "y":5.28146, "heading":-1.04548, "vx":0.411, "vy":-0.43691, "omega":-0.00087, "ax":-3.15977, "ay":3.35893, "alpha":0.0072, "fx":[-51.62198,-51.67777,-51.69046,-51.63462], "fy":[54.94436,54.89193,54.8798,54.93229]}, + {"t":1.79513, "x":3.95312, "y":5.26904, "heading":-1.04551, "vx":0.30828, "vy":-0.32771, "omega":-0.00063, "ax":-3.1605, "ay":3.35971, "alpha":0.00679, "fx":[-51.63581,-51.6885,-51.70049,-51.64777], "fy":[54.95525,54.90574,54.89429,54.94386]}, + {"t":1.82764, "x":3.96147, "y":5.26016, "heading":-1.04553, "vx":0.20554, "vy":-0.21849, "omega":-0.00041, "ax":-3.16107, "ay":3.36031, "alpha":0.00648, "fx":[-51.64654,-51.69681,-51.70826,-51.65796], "fy":[54.96369,54.91644,54.90552,54.95282]}, + {"t":1.86015, "x":3.96649, "y":5.25483, "heading":-1.04555, "vx":0.10278, "vy":-0.10925, "omega":-0.0002, "ax":-3.16152, "ay":3.36079, "alpha":0.00623, "fx":[-51.6551,-51.70344,-51.71446,-51.66609], "fy":[54.97042,54.92497,54.91448,54.95997]}, + {"t":1.89266, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index e1e0be04..325ea3c0 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,65 +30,61 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.27141,2.18043], + "waypoints":[0.0,1.20207,1.89844], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.08922, "ay":-3.42876, "alpha":-0.00403, "fx":[50.48511,50.51766,50.52076,50.48819], "fy":[-56.06972,-56.0404,-56.03755,-56.06689]}, - {"t":0.03973, "x":1.55259, "y":7.52069, "heading":-0.93501, "vx":0.12274, "vy":-0.13623, "omega":-0.00016, "ax":3.08902, "ay":-3.42856, "alpha":-0.00415, "fx":[50.48131,50.51479,50.51798,50.48448], "fy":[-56.06692,-56.03677,-56.03383,-56.06401]}, - {"t":0.07946, "x":1.5599, "y":7.51257, "heading":-0.93501, "vx":0.24547, "vy":-0.27245, "omega":-0.00033, "ax":3.0888, "ay":-3.42833, "alpha":-0.00428, "fx":[50.47704,50.51157,50.51485,50.4803], "fy":[-56.06378,-56.03268,-56.02965,-56.06077]}, - {"t":0.11919, "x":1.5721, "y":7.49904, "heading":-0.93503, "vx":0.36819, "vy":-0.40867, "omega":-0.0005, "ax":3.08854, "ay":-3.42808, "alpha":-0.00443, "fx":[50.4722,50.50791,50.51131,50.47557], "fy":[-56.06022,-56.02805,-56.02491,-56.0571]}, - {"t":0.15893, "x":1.58916, "y":7.4801, "heading":-0.93504, "vx":0.49091, "vy":-0.54487, "omega":-0.00067, "ax":3.08825, "ay":-3.42779, "alpha":-0.00459, "fx":[50.46666,50.50374,50.50726,50.47016], "fy":[-56.05615,-56.02276,-56.01949,-56.05291]}, - {"t":0.19866, "x":1.6111, "y":7.45575, "heading":-0.93507, "vx":0.61361, "vy":-0.68106, "omega":-0.00085, "ax":3.08791, "ay":-3.42745, "alpha":-0.00479, "fx":[50.46028,50.49892,50.50259,50.46392], "fy":[-56.05145,-56.01665,-56.01325,-56.04807]}, - {"t":0.23839, "x":1.63792, "y":7.42598, "heading":-0.93511, "vx":0.7363, "vy":-0.81724, "omega":-0.00104, "ax":3.08751, "ay":-3.42706, "alpha":-0.00502, "fx":[50.45284,50.4933,50.49714,50.45665], "fy":[-56.04597,-56.00953,-56.00596,-56.04243]}, - {"t":0.27812, "x":1.66961, "y":7.39081, "heading":-0.93515, "vx":0.85897, "vy":-0.9534, "omega":-0.00124, "ax":3.08705, "ay":-3.4266, "alpha":-0.00528, "fx":[50.44404,50.48666,50.4907,50.44805], "fy":[-56.0395,-56.00113,-55.99735,-56.03577]}, - {"t":0.31785, "x":1.70618, "y":7.35022, "heading":-0.9352, "vx":0.98162, "vy":-1.08955, "omega":-0.00145, "ax":3.08649, "ay":-3.42605, "alpha":-0.0056, "fx":[50.43349,50.4787,50.48298,50.43774], "fy":[-56.03175,-55.99104,-55.98703,-56.02777]}, - {"t":0.35758, "x":1.74761, "y":7.30423, "heading":-0.93525, "vx":1.10425, "vy":-1.22567, "omega":-0.00168, "ax":3.08581, "ay":-3.42537, "alpha":-0.006, "fx":[50.42061,50.46898,50.47355,50.42515], "fy":[-56.02227,-55.97872,-55.97442,-56.01801]}, - {"t":0.39732, "x":1.79392, "y":7.25283, "heading":-0.93532, "vx":1.22686, "vy":-1.36176, "omega":-0.00191, "ax":3.08495, "ay":-3.42453, "alpha":-0.00649, "fx":[50.40452,50.45683,50.46177,50.40941], "fy":[-56.01043,-55.96333,-55.95866,-56.00581]}, - {"t":0.43705, "x":1.8451, "y":7.19602, "heading":-0.9354, "vx":1.34943, "vy":-1.49783, "omega":-0.00217, "ax":3.08386, "ay":-3.42345, "alpha":-0.00712, "fx":[50.38384,50.44123,50.44663,50.3892], "fy":[-55.99522,-55.94356,-55.93842,-55.99013]}, - {"t":0.47678, "x":1.90115, "y":7.1338, "heading":-0.93548, "vx":1.47195, "vy":-1.63384, "omega":-0.00245, "ax":3.0824, "ay":-3.422, "alpha":-0.00796, "fx":[50.35632,50.42045,50.42648,50.36228], "fy":[-55.97497,-55.91724,-55.91146,-55.96926]}, - {"t":0.51651, "x":1.96207, "y":7.06619, "heading":-0.93558, "vx":1.59442, "vy":-1.76981, "omega":-0.00277, "ax":3.08036, "ay":-3.41999, "alpha":-0.00914, "fx":[50.31785,50.39142,50.3983,50.32464], "fy":[-55.94667,-55.88046,-55.87377,-55.94007]}, - {"t":0.55624, "x":2.02785, "y":6.99317, "heading":-0.93569, "vx":1.71681, "vy":-1.90569, "omega":-0.00313, "ax":3.07731, "ay":-3.41697, "alpha":-0.0109, "fx":[50.2603,50.34799,50.35614,50.26832], "fy":[-55.90433,-55.82544,-55.81736,-55.89639]}, - {"t":0.59597, "x":2.09849, "y":6.91476, "heading":-0.93581, "vx":1.83908, "vy":-2.04145, "omega":-0.00357, "ax":3.07225, "ay":-3.41195, "alpha":-0.01384, "fx":[50.1648,50.27593,50.28613,50.1748], "fy":[-55.83405,-55.73413,-55.7237,-55.82384]}, - {"t":0.63571, "x":2.17398, "y":6.83096, "heading":-0.93596, "vx":1.96114, "vy":-2.17701, "omega":-0.00412, "ax":3.0622, "ay":-3.402, "alpha":-0.01967, "fx":[49.9754,50.13298,50.14706,49.98909], "fy":[-55.69456,-55.55302,-55.53771,-55.67968]}, - {"t":0.67544, "x":2.25432, "y":6.74177, "heading":-0.93612, "vx":2.08281, "vy":-2.31218, "omega":-0.0049, "ax":3.03266, "ay":-3.37273, "alpha":-0.03695, "fx":[49.41982,49.71331,49.7374,49.44256], "fy":[-55.28447,-55.02166,-54.9903,-55.25458]}, - {"t":0.71517, "x":2.33947, "y":6.64725, "heading":-0.93631, "vx":2.2033, "vy":-2.44618, "omega":-0.00637, "ax":1.31961, "ay":-1.62049, "alpha":-1.2297, "fx":[19.95674,26.23815,23.45074,16.64649], "fy":[-31.06802,-26.2804,-21.71887,-26.90072]}, - {"t":0.7549, "x":2.42805, "y":6.54878, "heading":-0.93657, "vx":2.25573, "vy":-2.51057, "omega":-0.05522, "ax":-3.03747, "ay":3.36341, "alpha":-0.03544, "fx":[-49.80934,-49.52736,-49.50508,-49.78582], "fy":[54.8433,55.09709,55.12668,54.87425]}, - {"t":0.79463, "x":2.51528, "y":6.45168, "heading":-0.93876, "vx":2.13505, "vy":-2.37693, "omega":-0.05663, "ax":-3.06526, "ay":3.39798, "alpha":-0.01686, "fx":[-50.18484,-50.04971,-50.03748,-50.17232], "fy":[55.48302,55.6047,55.61798,55.49661]}, - {"t":0.83436, "x":2.59768, "y":6.35992, "heading":-0.94101, "vx":2.01326, "vy":-2.24193, "omega":-0.0573, "ax":-3.07443, "ay":3.40942, "alpha":-0.0107, "fx":[-50.30827,-50.22233,-50.2141,-50.29992], "fy":[55.69458,55.77198,55.78036,55.70308]}, - {"t":0.8741, "x":2.67525, "y":6.27354, "heading":-0.94329, "vx":1.89111, "vy":-2.10646, "omega":-0.05773, "ax":-3.07901, "ay":3.41512, "alpha":-0.0076, "fx":[-50.36957,-50.30841,-50.30232,-50.36342], "fy":[55.80011,55.85521,55.86121,55.80618]}, - {"t":0.91383, "x":2.74795, "y":6.19254, "heading":-0.94558, "vx":1.76877, "vy":-1.97078, "omega":-0.05803, "ax":-3.08175, "ay":3.41853, "alpha":-0.00574, "fx":[-50.40619,-50.35999,-50.35525,-50.40141], "fy":[55.86338,55.90499,55.90958,55.868]}, - {"t":0.95356, "x":2.8158, "y":6.11694, "heading":-0.94789, "vx":1.64633, "vy":-1.83495, "omega":-0.05826, "ax":-3.08357, "ay":3.42081, "alpha":-0.00449, "fx":[-50.43052,-50.39437,-50.39055,-50.42667], "fy":[55.90555,55.93811,55.94176,55.90922]}, - {"t":0.99329, "x":2.87878, "y":6.04673, "heading":-0.9502, "vx":1.52381, "vy":-1.69904, "omega":-0.05844, "ax":-3.08487, "ay":3.42243, "alpha":-0.00359, "fx":[-50.44784,-50.41891,-50.41577,-50.44469], "fy":[55.93567,55.96173,55.9647,55.93865]}, - {"t":1.03302, "x":2.93688, "y":5.98193, "heading":-0.95252, "vx":1.40125, "vy":-1.56306, "omega":-0.05858, "ax":-3.08585, "ay":3.42365, "alpha":-0.00292, "fx":[-50.4608,-50.43731,-50.4347,-50.45818], "fy":[55.95827,55.97943,55.98188,55.96073]}, - {"t":1.07275, "x":2.99012, "y":5.92253, "heading":-0.95485, "vx":1.27864, "vy":-1.42703, "omega":-0.05869, "ax":-3.08661, "ay":3.42459, "alpha":-0.00239, "fx":[-50.47085,-50.45162,-50.44944,-50.46866], "fy":[55.97585,55.99318,55.99522,55.9779]}, - {"t":1.11248, "x":3.03849, "y":5.86853, "heading":-0.95718, "vx":1.15601, "vy":-1.29097, "omega":-0.05879, "ax":-3.08721, "ay":3.42535, "alpha":-0.00196, "fx":[-50.47888,-50.46307,-50.46123,-50.47704], "fy":[55.98992,56.00417,56.00588,55.99164]}, - {"t":1.15222, "x":3.08198, "y":5.81994, "heading":-0.95952, "vx":1.03335, "vy":-1.15487, "omega":-0.05887, "ax":-3.08771, "ay":3.42597, "alpha":-0.00162, "fx":[-50.48544,-50.47243,-50.47089,-50.48389], "fy":[56.00144,56.01316,56.01459,56.00287]}, - {"t":1.19195, "x":3.1206, "y":5.77676, "heading":-0.96186, "vx":0.91067, "vy":-1.01876, "omega":-0.05893, "ax":-3.08812, "ay":3.42648, "alpha":-0.00132, "fx":[-50.49089,-50.48023,-50.47894,-50.4896], "fy":[56.01104,56.02064,56.02184,56.01224]}, - {"t":1.23168, "x":3.15435, "y":5.73899, "heading":-0.9642, "vx":0.78797, "vy":-0.88262, "omega":-0.05898, "ax":-3.08847, "ay":3.42692, "alpha":-0.00108, "fx":[-50.4955,-50.48683,-50.48576,-50.49443], "fy":[56.01917,56.02698,56.02796,56.02016]}, - {"t":1.27141, "x":3.18322, "y":5.70663, "heading":-0.96654, "vx":0.66526, "vy":-0.74646, "omega":-0.05903, "ax":-0.75349, "ay":-0.64148, "alpha":-0.55179, "fx":[-12.63721,-10.18557,-12.0132,-14.4368], "fy":[-12.58849,-10.96348,-8.36182,-10.0339]}, - {"t":1.30929, "x":3.20787, "y":5.6779, "heading":-0.96878, "vx":0.63672, "vy":-0.77076, "omega":-0.07993, "ax":-0.04831, "ay":-0.03981, "alpha":-0.5193, "fx":[-1.18051,1.20753,-0.39961,-2.7862], "fy":[-2.63917,-1.00023,1.33885,-0.30265]}, - {"t":1.34716, "x":3.23195, "y":5.64867, "heading":-0.97181, "vx":0.63489, "vy":-0.77226, "omega":-0.0996, "ax":-0.00292, "ay":-0.0024, "alpha":-0.43649, "fx":[-0.38129,1.62987,0.28583,-1.72527], "fy":[-1.71005,-0.33719,1.63166,0.2587]}, - {"t":1.38504, "x":3.256, "y":5.61942, "heading":-0.97558, "vx":0.63478, "vy":-0.77235, "omega":-0.11613, "ax":-0.00018, "ay":-0.00014, "alpha":-0.35594, "fx":[-0.28002,1.36418,0.27426,-1.36993], "fy":[-1.36383,-0.25048,1.3591,0.24575]}, - {"t":1.42291, "x":3.28004, "y":5.59017, "heading":-0.97998, "vx":0.63477, "vy":-0.77236, "omega":-0.12961, "ax":-0.00001, "ay":-0.00001, "alpha":-0.27738, "fx":[-0.22081,1.06429,0.22046,-1.06463], "fy":[-1.06015,-0.19818,1.05986,0.19789]}, - {"t":1.46079, "x":3.30408, "y":5.56092, "heading":-0.98489, "vx":0.63477, "vy":-0.77236, "omega":-0.14012, "ax":0.0, "ay":0.0, "alpha":-0.20037, "fx":[-0.16315,0.76821,0.16313,-0.76823], "fy":[-0.76493,-0.14684,0.76492,0.14682]}, - {"t":1.49867, "x":3.32813, "y":5.53166, "heading":-0.99019, "vx":0.63477, "vy":-0.77236, "omega":-0.1477, "ax":0.0, "ay":0.0, "alpha":-0.12448, "fx":[-0.10388,0.47679,0.10387,-0.47679], "fy":[-0.47468,-0.09375,0.47468,0.09375]}, - {"t":1.53654, "x":3.35217, "y":5.50241, "heading":-0.99579, "vx":0.63477, "vy":-0.77236, "omega":-0.15242, "ax":0.0, "ay":0.0, "alpha":-0.0493, "fx":[-0.04219,0.18861,0.04219,-0.18861], "fy":[-0.18775,-0.03818,0.18775,0.03818]}, - {"t":1.57442, "x":3.37621, "y":5.47315, "heading":-1.00156, "vx":0.63477, "vy":-0.77236, "omega":-0.15429, "ax":0.0, "ay":0.0, "alpha":0.02561, "fx":[0.02248,-0.09787,-0.02248,0.09787], "fy":[0.09741,0.0204,-0.09741,-0.0204]}, - {"t":1.61229, "x":3.40025, "y":5.4439, "heading":-1.0074, "vx":0.63477, "vy":-0.77236, "omega":-0.15332, "ax":0.0, "ay":0.0, "alpha":0.10066, "fx":[0.09059,-0.38419,-0.09059,0.38419], "fy":[0.38235,0.08244,-0.38235,-0.08244]}, - {"t":1.65017, "x":3.4243, "y":5.41465, "heading":-1.01321, "vx":0.63477, "vy":-0.77236, "omega":-0.1495, "ax":0.0, "ay":0.0, "alpha":0.17628, "fx":[0.16253,-0.67194,-0.16253,0.67194], "fy":[0.66863,0.14827,-0.66863,-0.14827]}, - {"t":1.68804, "x":3.44834, "y":5.38539, "heading":-1.01887, "vx":0.63477, "vy":-0.77236, "omega":-0.14283, "ax":0.0, "ay":0.0, "alpha":0.25289, "fx":[0.23859,-0.96273,-0.23859,0.96273], "fy":[0.95787,0.21815,-0.95787,-0.21815]}, - {"t":1.72592, "x":3.47238, "y":5.35614, "heading":-1.02428, "vx":0.63477, "vy":-0.77236, "omega":-0.13325, "ax":0.0, "ay":0.0, "alpha":0.33091, "fx":[0.31897,-1.25819,-0.31897,1.25819], "fy":[1.25169,0.29227,-1.25169,-0.29227]}, - {"t":1.7638, "x":3.49642, "y":5.32689, "heading":-1.02933, "vx":0.63477, "vy":-0.77236, "omega":-0.12072, "ax":0.0, "ay":0.0, "alpha":0.41079, "fx":[0.40381,-1.56005,-0.40381,1.56005], "fy":[1.55181,0.3707,-1.55181,-0.3707]}, - {"t":1.80167, "x":3.52047, "y":5.29763, "heading":-1.0339, "vx":0.63477, "vy":-0.77236, "omega":-0.10516, "ax":0.0, "ay":0.0, "alpha":0.49296, "fx":[0.49309,-1.87007,-0.49309,1.87007], "fy":[1.86,0.45341,-1.86,-0.45341]}, - {"t":1.83955, "x":3.54451, "y":5.26838, "heading":-1.03789, "vx":0.63477, "vy":-0.77236, "omega":-0.08648, "ax":0.0, "ay":0.0, "alpha":0.57789, "fx":[0.58672,-2.19009,-0.58672,2.19009], "fy":[2.1781,0.54025,-2.1781,-0.54025]}, - {"t":1.87742, "x":3.56855, "y":5.23912, "heading":-1.04116, "vx":0.63477, "vy":-0.77236, "omega":-0.0646, "ax":0.0, "ay":0.0, "alpha":0.66603, "fx":[0.68442,-2.52207,-0.68442,2.52207], "fy":[2.50808,0.63091,-2.50808,-0.63091]}, - {"t":1.9153, "x":3.5926, "y":5.20987, "heading":-1.04361, "vx":0.63477, "vy":-0.77236, "omega":-0.03937, "ax":-0.00008, "ay":0.0001, "alpha":0.75786, "fx":[0.78442,-2.86942,-0.78713,2.86672], "fy":[2.85362,0.72655,-2.85037,-0.7233]}, - {"t":1.95318, "x":3.61664, "y":5.18062, "heading":-1.0451, "vx":0.63477, "vy":-0.77236, "omega":-0.01067, "ax":-2.13179, "ay":2.59386, "alpha":0.26686, "fx":[-34.04384,-35.80929,-35.67966,-33.87028], "fy":[43.42887,42.12499,41.36043,42.70409]}, - {"t":1.99105, "x":3.63915, "y":5.15322, "heading":-1.0455, "vx":0.55403, "vy":-0.67411, "omega":-0.00056, "ax":-2.91622, "ay":3.5483, "alpha":0.00572, "fx":[-47.64718,-47.69459,-47.70199,-47.65455], "fy":[58.0306,57.99172,57.98517,58.02409]}, - {"t":2.02893, "x":3.65804, "y":5.13024, "heading":-1.04552, "vx":0.44357, "vy":-0.53972, "omega":-0.00034, "ax":-2.92472, "ay":3.55865, "alpha":0.00318, "fx":[-47.79835,-47.82478,-47.82897,-47.80252], "fy":[58.18975,58.16805,58.16448,58.18619]}, - {"t":2.0668, "x":3.67275, "y":5.11235, "heading":-1.04554, "vx":0.3328, "vy":-0.40493, "omega":-0.00022, "ax":-2.9276, "ay":3.56215, "alpha":0.00232, "fx":[-47.84944,-47.86878,-47.87185,-47.85251], "fy":[58.24353,58.22765,58.22506,58.24094]}, - {"t":2.10468, "x":3.68325, "y":5.09956, "heading":-1.04555, "vx":0.22191, "vy":-0.27001, "omega":-0.00013, "ax":-2.92904, "ay":3.5639, "alpha":0.00189, "fx":[-47.87512,-47.89088,-47.8934,-47.87763], "fy":[58.27056,58.2576,58.2555,58.26845]}, - {"t":2.14255, "x":3.68956, "y":5.09189, "heading":-1.04555, "vx":0.11097, "vy":-0.13503, "omega":-0.00006, "ax":-2.92991, "ay":3.56496, "alpha":0.00164, "fx":[-47.89056,-47.90418,-47.90636,-47.89274], "fy":[58.28681,58.27562,58.27381,58.285]}, - {"t":2.18043, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0897, "ay":-3.42807, "alpha":-0.0103, "fx":[50.46517,50.5483,50.55626,50.47302], "fy":[-56.08355,-56.00865,-56.00131,-56.07632]}, + {"t":0.03756, "x":1.55233, "y":7.52098, "heading":-0.93501, "vx":0.11606, "vy":-0.12877, "omega":-0.00039, "ax":3.08951, "ay":-3.42789, "alpha":-0.01052, "fx":[50.4611,50.54593,50.55404,50.46911], "fy":[-56.08142,-56.00499,-55.99749,-56.07404]}, + {"t":0.07513, "x":1.55887, "y":7.51372, "heading":-0.93502, "vx":0.23212, "vy":-0.25754, "omega":-0.00078, "ax":3.08929, "ay":-3.42769, "alpha":-0.01075, "fx":[50.45658,50.54329,50.55159,50.46476], "fy":[-56.07905,-56.00093,-55.99326,-56.07151]}, + {"t":0.11269, "x":1.56977, "y":7.50163, "heading":-0.93505, "vx":0.34817, "vy":-0.3863, "omega":-0.00119, "ax":3.08905, "ay":-3.42746, "alpha":-0.01101, "fx":[50.45153,50.54035,50.54884,50.4599], "fy":[-56.0764,-55.99639,-55.98852,-56.06867]}, + {"t":0.15026, "x":1.58503, "y":7.4847, "heading":-0.93509, "vx":0.46421, "vy":-0.51505, "omega":-0.0016, "ax":3.08879, "ay":-3.42721, "alpha":-0.01131, "fx":[50.44584,50.53704,50.54576,50.45443], "fy":[-56.07343,-55.99128,-55.98318,-56.06548]}, + {"t":0.18782, "x":1.60465, "y":7.46294, "heading":-0.93515, "vx":0.58024, "vy":-0.64379, "omega":-0.00202, "ax":3.08848, "ay":-3.42692, "alpha":-0.01164, "fx":[50.4394,50.53328,50.54226,50.44824], "fy":[-56.07006,-55.98548,-55.97714,-56.06187]}, + {"t":0.22539, "x":1.62862, "y":7.43633, "heading":-0.93523, "vx":0.69625, "vy":-0.77253, "omega":-0.00246, "ax":3.08813, "ay":-3.42659, "alpha":-0.01203, "fx":[50.43203,50.52899,50.53826,50.44115], "fy":[-56.06621,-55.97887,-55.97023,-56.05773]}, + {"t":0.26295, "x":1.65695, "y":7.4049, "heading":-0.93532, "vx":0.81226, "vy":-0.90124, "omega":-0.00291, "ax":3.08773, "ay":-3.42621, "alpha":-0.01247, "fx":[50.42353,50.52403,50.53364,50.43298], "fy":[-56.06176,-55.97123,-55.96225,-56.05296]}, + {"t":0.30052, "x":1.68964, "y":7.36862, "heading":-0.93543, "vx":0.92825, "vy":-1.02995, "omega":-0.00338, "ax":3.08726, "ay":-3.42577, "alpha":-0.01298, "fx":[50.4136,50.51825,50.52826,50.42344], "fy":[-56.05657,-55.96232,-55.95294,-56.04739]}, + {"t":0.33808, "x":1.72669, "y":7.32752, "heading":-0.93556, "vx":1.04422, "vy":-1.15864, "omega":-0.00387, "ax":3.08671, "ay":-3.42525, "alpha":-0.01359, "fx":[50.40188,50.51142,50.5219,50.41216], "fy":[-56.05044,-55.95179,-55.94194,-56.04081]}, + {"t":0.37565, "x":1.7681, "y":7.28158, "heading":-0.9357, "vx":1.16017, "vy":-1.2873, "omega":-0.00438, "ax":3.08605, "ay":-3.42462, "alpha":-0.01432, "fx":[50.3878,50.50322,50.51426,50.39863], "fy":[-56.04309,-55.93915,-55.92873,-56.0329]}, + {"t":0.41321, "x":1.81385, "y":7.2308, "heading":-0.93587, "vx":1.2761, "vy":-1.41595, "omega":-0.00492, "ax":3.08523, "ay":-3.42385, "alpha":-0.01522, "fx":[50.37059,50.49319,50.50492,50.38208], "fy":[-56.0341,-55.92369,-55.91258,-56.02324]}, + {"t":0.45078, "x":1.86397, "y":7.1752, "heading":-0.93605, "vx":1.39199, "vy":-1.54456, "omega":-0.00549, "ax":3.08422, "ay":-3.42289, "alpha":-0.01634, "fx":[50.34907,50.48066,50.49325,50.36138], "fy":[-56.02286,-55.90438,-55.89238,-56.01116]}, + {"t":0.48834, "x":1.91843, "y":7.11476, "heading":-0.93626, "vx":1.50785, "vy":-1.67314, "omega":-0.0061, "ax":3.08291, "ay":-3.42165, "alpha":-0.01778, "fx":[50.3214,50.46455,50.47823,50.33476], "fy":[-56.0084,-55.87954,-55.86639,-55.99561]}, + {"t":0.5259, "x":1.97725, "y":7.0495, "heading":-0.93649, "vx":1.62366, "vy":-1.80168, "omega":-0.00677, "ax":3.08117, "ay":-3.42, "alpha":-0.01971, "fx":[50.2845,50.44307,50.4582,50.29923], "fy":[-55.98912,-55.84641,-55.83172,-55.97487]}, + {"t":0.56347, "x":2.04042, "y":6.97941, "heading":-0.93674, "vx":1.7394, "vy":-1.93015, "omega":-0.00751, "ax":3.07873, "ay":-3.41769, "alpha":-0.02241, "fx":[50.23282,50.41299,50.43013,50.24945], "fy":[-55.96212,-55.80002,-55.78314,-55.94581]}, + {"t":0.60103, "x":2.10793, "y":6.90449, "heading":-0.93703, "vx":1.85505, "vy":-2.05853, "omega":-0.00835, "ax":3.07506, "ay":-3.41423, "alpha":-0.02648, "fx":[50.15528,50.36787,50.38798,50.17468], "fy":[-55.9216,-55.73041,-55.71017,-55.90214]}, + {"t":0.6386, "x":2.17978, "y":6.82475, "heading":-0.93734, "vx":1.97057, "vy":-2.18679, "omega":-0.00935, "ax":3.06895, "ay":-3.40844, "alpha":-0.03327, "fx":[50.02599,50.29264,50.31761,50.04983], "fy":[-55.854,-55.61438,-55.58834,-55.8292]}, + {"t":0.67616, "x":2.25597, "y":6.7402, "heading":-0.93769, "vx":2.08585, "vy":-2.31482, "omega":-0.0106, "ax":3.0567, "ay":-3.39684, "alpha":-0.04694, "fx":[49.76732,50.14211,50.17635,49.79934], "fy":[-55.71854,-55.38225,-55.34394,-55.68266]}, + {"t":0.71373, "x":2.33648, "y":6.65085, "heading":-0.93809, "vx":2.20067, "vy":-2.44242, "omega":-0.01236, "ax":3.01979, "ay":-3.36184, "alpha":-0.08848, "fx":[48.99123,49.68975,49.74824,49.04198], "fy":[-55.30984,-54.68591,-54.6052,-55.23757]}, + {"t":0.75129, "x":2.42128, "y":6.55673, "heading":-0.93855, "vx":2.31411, "vy":-2.56871, "omega":-0.01569, "ax":-1.17911, "ay":1.07722, "alpha":-2.80881, "fx":[-22.81284,-8.23637,-16.70856,-29.34676], "fy":[6.19788,17.67164,28.38594,18.18641]}, + {"t":0.78886, "x":2.50738, "y":6.461, "heading":-0.93914, "vx":2.26982, "vy":-2.52824, "omega":-0.1212, "ax":-3.03383, "ay":3.35394, "alpha":-0.07567, "fx":[-49.92303,-49.32122,-49.27443,-49.87063], "fy":[54.52556,55.06809,55.13255,54.59622]}, + {"t":0.82642, "x":2.5905, "y":6.36839, "heading":-0.9437, "vx":2.15585, "vy":-2.40225, "omega":-0.12404, "ax":-3.06321, "ay":3.39219, "alpha":-0.03596, "fx":[-50.2355,-49.94749,-49.92057,-50.20729], "fy":[55.31079,55.5704,55.60006,55.34188]}, + {"t":0.86399, "x":2.66932, "y":6.28054, "heading":-0.94836, "vx":2.04078, "vy":-2.27483, "omega":-0.12539, "ax":-3.07318, "ay":3.40517, "alpha":-0.02243, "fx":[-50.33995,-50.15993,-50.14158,-50.32111], "fy":[55.57744,55.73971,55.75851,55.5968]}, + {"t":0.90155, "x":2.74382, "y":6.19749, "heading":-0.95307, "vx":1.92534, "vy":-2.14691, "omega":-0.12623, "ax":-3.0782, "ay":3.41171, "alpha":-0.01557, "fx":[-50.39201,-50.26694,-50.25338,-50.3782], "fy":[55.7119,55.82462,55.83803,55.72558]}, + {"t":0.93912, "x":2.81397, "y":6.11925, "heading":-0.95781, "vx":1.80971, "vy":-2.01875, "omega":-0.12682, "ax":-3.08121, "ay":3.41565, "alpha":-0.01141, "fx":[-50.42309,-50.33144,-50.32096,-50.41248], "fy":[55.79301,55.87562,55.88576,55.8033]}, + {"t":0.97668, "x":2.87978, "y":6.04583, "heading":-0.96257, "vx":1.69397, "vy":-1.89045, "omega":-0.12725, "ax":-3.08323, "ay":3.41828, "alpha":-0.00861, "fx":[-50.44369,-50.37456,-50.36627,-50.43533], "fy":[55.84732,55.90963,55.91753,55.85531]}, + {"t":1.01424, "x":2.94123, "y":5.97723, "heading":-0.96735, "vx":1.57815, "vy":-1.76204, "omega":-0.12757, "ax":-3.08467, "ay":3.42017, "alpha":-0.00659, "fx":[-50.45831,-50.40541,-50.39879,-50.45165], "fy":[55.88625,55.93393,55.94018,55.89255]}, + {"t":1.05181, "x":2.99834, "y":5.91345, "heading":-0.97214, "vx":1.46227, "vy":-1.63356, "omega":-0.12782, "ax":-3.08576, "ay":3.42158, "alpha":-0.00506, "fx":[-50.46922,-50.42858,-50.42328,-50.46389], "fy":[55.91553,55.95216,55.95713,55.92053]}, + {"t":1.08937, "x":3.05109, "y":5.8545, "heading":-0.97694, "vx":1.34636, "vy":-1.50503, "omega":-0.12801, "ax":-3.0866, "ay":3.42268, "alpha":-0.00387, "fx":[-50.47765,-50.4466,-50.4424,-50.47343], "fy":[55.93837,55.96635,55.97027,55.94232]}, + {"t":1.12694, "x":3.09949, "y":5.80038, "heading":-0.98175, "vx":1.23041, "vy":-1.37646, "omega":-0.12815, "ax":-3.08727, "ay":3.42356, "alpha":-0.00291, "fx":[-50.48435,-50.46103,-50.45775,-50.48106], "fy":[55.9567,55.97772,55.98076,55.95975]}, + {"t":1.1645, "x":3.14353, "y":5.75109, "heading":-0.98657, "vx":1.11444, "vy":-1.24786, "omega":-0.12826, "ax":-3.08783, "ay":3.42428, "alpha":-0.00212, "fx":[-50.4898,-50.47282,-50.47035,-50.48732], "fy":[55.97173,55.98703,55.98931,55.97402]}, + {"t":1.20207, "x":3.18322, "y":5.70663, "heading":-0.99139, "vx":0.99844, "vy":-1.11922, "omega":-0.12834, "ax":-1.13542, "ay":-0.96733, "alpha":-0.60968, "fx":[-18.83966,-16.19997,-18.31127,-20.89663], "fy":[-18.10667,-16.54806,-13.47617,-15.12505]}, + {"t":1.23689, "x":3.21729, "y":5.66707, "heading":-0.99585, "vx":0.95891, "vy":-1.15291, "omega":-0.14957, "ax":-0.18809, "ay":-0.15555, "alpha":-0.5291, "fx":[-3.52348,-1.04926,-2.62913,-5.09802], "fy":[-4.55516,-2.96129,-0.52525,-2.12982]}, + {"t":1.2717, "x":3.25057, "y":5.62684, "heading":-1.00106, "vx":0.95236, "vy":-1.15832, "omega":-0.16799, "ax":-0.02773, "ay":-0.02278, "alpha":-0.31371, "fx":[-0.72809,0.7456,-0.17873,-1.65216], "fy":[-1.56559,-0.62194,0.82102,-0.12318]}, + {"t":1.30652, "x":3.28371, "y":5.58649, "heading":-1.00691, "vx":0.9514, "vy":-1.15911, "omega":-0.17892, "ax":-0.00406, "ay":-0.00334, "alpha":-0.09691, "fx":[-0.15348,0.30345,0.02058,-0.43634], "fy":[-0.42266,-0.13371,0.3136,0.02465]}, + {"t":1.34134, "x":3.31683, "y":5.54613, "heading":-1.01314, "vx":0.95125, "vy":-1.15923, "omega":-0.18229, "ax":-0.0006, "ay":-0.00049, "alpha":0.11946, "fx":[0.10038,-0.46508,-0.11984,0.44562], "fy":[0.44513,0.09246,-0.4611,-0.10843]}, + {"t":1.37616, "x":3.34995, "y":5.50577, "heading":-1.01949, "vx":0.95123, "vy":-1.15925, "omega":-0.17813, "ax":-0.00009, "ay":-0.00007, "alpha":0.3364, "fx":[0.31673,-1.2819,-0.31958,1.27905], "fy":[1.27283,0.28981,-1.27516,-0.29215]}, + {"t":1.41098, "x":3.38308, "y":5.4654, "heading":-1.02569, "vx":0.95123, "vy":-1.15925, "omega":-0.16642, "ax":-0.00001, "ay":-0.00001, "alpha":0.55496, "fx":[0.53768,-2.10959,-0.5381,2.10918], "fy":[2.09825,0.49296,-2.09859,-0.4933]}, + {"t":1.4458, "x":3.4162, "y":5.42504, "heading":-1.03148, "vx":0.95123, "vy":-1.15925, "omega":-0.14709, "ax":0.0, "ay":0.0, "alpha":0.77618, "fx":[0.76926,-2.9462,-0.76933,2.94614], "fy":[2.93044,0.70675,-2.93049,-0.70681]}, + {"t":1.48062, "x":3.44932, "y":5.38467, "heading":-1.03661, "vx":0.95123, "vy":-1.15925, "omega":-0.12007, "ax":0.0, "ay":0.0, "alpha":1.00108, "fx":[1.01153,-3.79513,-1.01156,3.79509], "fy":[3.77443,0.93101,-3.77446,-0.93103]}, + {"t":1.51544, "x":3.48244, "y":5.34431, "heading":-1.04079, "vx":0.95123, "vy":-1.15925, "omega":-0.08521, "ax":-0.00007, "ay":0.00008, "alpha":1.23068, "fx":[1.26171,-4.66192,-1.26417,4.65949], "fy":[4.6362,1.16537,-4.63357,-1.16273]}, + {"t":1.55026, "x":3.51556, "y":5.30395, "heading":-1.04375, "vx":0.95123, "vy":-1.15925, "omega":-0.04236, "ax":-1.03287, "ay":1.25877, "alpha":1.14645, "fx":[-15.23468,-21.0979,-18.71391,-12.49555], "fy":[24.87235,21.16568,16.18847,20.0872]}, + {"t":1.58507, "x":3.54805, "y":5.26435, "heading":-1.04523, "vx":0.91526, "vy":-1.11542, "omega":-0.00244, "ax":-2.89922, "ay":3.53324, "alpha":0.01888, "fx":[-47.30698,-47.46298,-47.48669,-47.33034], "fy":[57.83656,57.70906,57.6868,57.8147]}, + {"t":1.61989, "x":3.57816, "y":5.22765, "heading":-1.04531, "vx":0.81432, "vy":-0.9924, "omega":-0.00179, "ax":-2.91485, "ay":3.55229, "alpha":0.01081, "fx":[-47.60042,-47.69015,-47.70414,-47.6143], "fy":[58.11603,58.04255,58.03026,58.10386]}, + {"t":1.65471, "x":3.60475, "y":5.19525, "heading":-1.04538, "vx":0.71282, "vy":-0.86871, "omega":-0.00141, "ax":-2.92009, "ay":3.55868, "alpha":0.00811, "fx":[-47.69896,-47.76638,-47.77698,-47.7095], "fy":[58.20978,58.15454,58.14543,58.20074]}, + {"t":1.68953, "x":3.6278, "y":5.16716, "heading":-1.04542, "vx":0.61115, "vy":-0.7448, "omega":-0.00113, "ax":-2.92272, "ay":3.56188, "alpha":0.00676, "fx":[-47.74835,-47.80457,-47.81346,-47.75719], "fy":[58.25675,58.21067,58.20312,58.24926]}, + {"t":1.72435, "x":3.64731, "y":5.14338, "heading":-1.04546, "vx":0.50938, "vy":-0.62078, "omega":-0.00089, "ax":-2.9243, "ay":3.5638, "alpha":0.00594, "fx":[-47.77801,-47.82751,-47.83535,-47.78582], "fy":[58.28496,58.24438,58.23777,58.27839]}, + {"t":1.75917, "x":3.66327, "y":5.12393, "heading":-1.0455, "vx":0.40756, "vy":-0.49669, "omega":-0.00068, "ax":-2.92535, "ay":3.56508, "alpha":0.0054, "fx":[-47.7978,-47.84281,-47.84996,-47.80492], "fy":[58.30378,58.26687,58.26087,58.29781]}, + {"t":1.79399, "x":3.67569, "y":5.1088, "heading":-1.04552, "vx":0.30571, "vy":-0.37256, "omega":-0.0005, "ax":-2.9261, "ay":3.566, "alpha":0.00502, "fx":[-47.81195,-47.85375,-47.86039,-47.81857], "fy":[58.31722,58.28294,58.27738,58.31169]}, + {"t":1.82881, "x":3.68456, "y":5.09799, "heading":-1.04554, "vx":0.20382, "vy":-0.24839, "omega":-0.00032, "ax":-2.92666, "ay":3.56669, "alpha":0.00473, "fx":[-47.82256,-47.86195,-47.86822,-47.8288], "fy":[58.32731,58.295,58.28977,58.3221]}, + {"t":1.86363, "x":3.68988, "y":5.0915, "heading":-1.04555, "vx":0.10192, "vy":-0.12421, "omega":-0.00016, "ax":-2.9271, "ay":3.56722, "alpha":0.0045, "fx":[-47.83081,-47.86833,-47.87431,-47.83676], "fy":[58.33516,58.30438,58.2994,58.3302]}, + {"t":1.89844, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index 588bedf7..4539c5c2 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -7,8 +7,8 @@ "slow":{ "dimension":"LinVel", "var":{ - "exp":"1 m / s", - "val":1.0 + "exp":"1.5 m / s", + "val":1.5 } } }, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 95c1ab80..ed65d608 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -471,7 +471,7 @@ public static enum AlgaeScoreTarget { ? AutoAim.BLUE_REEF_CENTER : AutoAim.RED_REEF_CENTER) .getNorm() - < 3.0 + < 3.25 && DriverStation.isAutonomous()), driver.leftTrigger(), driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), From 1e8fe686efe0e8400f2e56df6f296772f6b28e3a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 21:49:17 -0700 Subject: [PATCH 063/126] mirror lhs auto changes to rhs --- src/main/deploy/choreo/PRMtoB.traj | 2 +- src/main/deploy/choreo/PROtoC.traj | 2 +- src/main/deploy/choreo/PROtoD.traj | 2 +- src/main/deploy/choreo/ROtoE.traj | 110 ++++++++++++++--------------- 4 files changed, 57 insertions(+), 59 deletions(-) diff --git a/src/main/deploy/choreo/PRMtoB.traj b/src/main/deploy/choreo/PRMtoB.traj index c425fb10..5bfe7978 100644 --- a/src/main/deploy/choreo/PRMtoB.traj +++ b/src/main/deploy/choreo/PRMtoB.traj @@ -24,7 +24,7 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src/main/deploy/choreo/PROtoC.traj b/src/main/deploy/choreo/PROtoC.traj index fe4a8dcc..6a834004 100644 --- a/src/main/deploy/choreo/PROtoC.traj +++ b/src/main/deploy/choreo/PROtoC.traj @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src/main/deploy/choreo/PROtoD.traj b/src/main/deploy/choreo/PROtoD.traj index 8d42bbb6..e9d81059 100644 --- a/src/main/deploy/choreo/PROtoD.traj +++ b/src/main/deploy/choreo/PROtoD.traj @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src/main/deploy/choreo/ROtoE.traj b/src/main/deploy/choreo/ROtoE.traj index e44794f5..3efd6d7a 100644 --- a/src/main/deploy/choreo/ROtoE.traj +++ b/src/main/deploy/choreo/ROtoE.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":7.046594619750977, "y":1.881866693496704, "heading":2.356194490192345, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.009561061859131, "y":2.7982044219970703, "heading":2.0928880900706415, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"7.046594619750977 m", "val":7.046594619750977}, "y":{"exp":"1.881866693496704 m", "val":1.881866693496704}, "heading":{"exp":"180 deg - 45 deg", "val":2.356194490192345}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"E.x", "val":5.009561061859131}, "y":{"exp":"E.y", "val":2.7982044219970703}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,59 +30,57 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.02144,1.71334], + "waypoints":[0.0,0.95966,1.51128], "samples":[ - {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29489, "ay":1.68764, "alpha":-0.01898, "fx":[-70.16348,-70.23349,-70.26308,-70.19312], "fy":[27.71647,27.53885,27.46267,27.64072]}, - {"t":0.03522, "x":7.04393, "y":1.88291, "heading":2.35619, "vx":-0.15127, "vy":0.05944, "omega":-0.00067, "ax":-4.29454, "ay":1.68761, "alpha":-0.01954, "fx":[-70.15628,-70.22836,-70.25881,-70.18678], "fy":[27.71971,27.53686,27.45842,27.64173]}, - {"t":0.07044, "x":7.03594, "y":1.88605, "heading":2.35617, "vx":-0.30254, "vy":0.11888, "omega":-0.00136, "ax":-4.29414, "ay":1.68757, "alpha":-0.02017, "fx":[-70.14811,-70.22255,-70.25398,-70.1796], "fy":[27.72338,27.53461,27.45361,27.64288]}, - {"t":0.10567, "x":7.02262, "y":1.89129, "heading":2.35612, "vx":-0.45378, "vy":0.17832, "omega":-0.00207, "ax":-4.29369, "ay":1.68753, "alpha":-0.0209, "fx":[-70.13879,-70.21592,-70.24845,-70.17139], "fy":[27.72758,27.53204,27.44811,27.64418]}, - {"t":0.14089, "x":7.00397, "y":1.89862, "heading":2.35605, "vx":-0.60502, "vy":0.23776, "omega":-0.0028, "ax":-4.29317, "ay":1.68749, "alpha":-0.02174, "fx":[-70.12803,-70.20826,-70.24208,-70.16192], "fy":[27.73243,27.52907,27.44176,27.64568]}, - {"t":0.17611, "x":6.98, "y":1.90804, "heading":2.35595, "vx":-0.75623, "vy":0.2972, "omega":-0.00357, "ax":-4.29255, "ay":1.68744, "alpha":-0.02272, "fx":[-70.11547,-70.19933,-70.23465,-70.15087], "fy":[27.73808,27.52562,27.43435,27.64743]}, - {"t":0.21133, "x":6.9507, "y":1.91955, "heading":2.35583, "vx":-0.90742, "vy":0.35663, "omega":-0.00437, "ax":-4.29183, "ay":1.68737, "alpha":-0.02388, "fx":[-70.10064,-70.18878,-70.22587,-70.13781], "fy":[27.74476,27.52154,27.42559,27.64949]}, - {"t":0.24655, "x":6.91608, "y":1.93316, "heading":2.35567, "vx":-1.05859, "vy":0.41606, "omega":-0.00521, "ax":-4.29097, "ay":1.6873, "alpha":-0.02527, "fx":[-70.08284,-70.17612,-70.21534,-70.12215], "fy":[27.75278,27.51666,27.41509,27.65197]}, - {"t":0.28178, "x":6.87613, "y":1.94886, "heading":2.35549, "vx":-1.20973, "vy":0.47549, "omega":-0.0061, "ax":-4.28991, "ay":1.68721, "alpha":-0.02696, "fx":[-70.0611,-70.16066,-70.20246,-70.103], "fy":[27.76256,27.51069,27.40225,27.65498]}, - {"t":0.317, "x":6.83086, "y":1.96666, "heading":2.35527, "vx":-1.36082, "vy":0.53492, "omega":-0.00705, "ax":-4.28858, "ay":1.68709, "alpha":-0.02908, "fx":[-70.03393,-70.14135,-70.18637,-70.07907], "fy":[27.77479,27.50325,27.3862,27.65875]}, - {"t":0.35222, "x":6.78027, "y":1.98654, "heading":2.35503, "vx":-1.51188, "vy":0.59434, "omega":-0.00807, "ax":-4.28688, "ay":1.68694, "alpha":-0.03181, "fx":[-69.99901,-70.11655,-70.1657,-70.04831], "fy":[27.79049,27.49368,27.36557,27.66359]}, - {"t":0.38744, "x":6.72436, "y":2.00852, "heading":2.35474, "vx":-1.66287, "vy":0.65376, "omega":-0.00919, "ax":-4.28462, "ay":1.68675, "alpha":-0.03545, "fx":[-69.95249,-70.08352,-70.13813,-70.00729], "fy":[27.81139,27.48095,27.33808,27.67002]}, - {"t":0.42266, "x":6.66313, "y":2.0326, "heading":2.35442, "vx":-1.81378, "vy":0.71317, "omega":-0.01044, "ax":-4.28145, "ay":1.68647, "alpha":-0.04054, "fx":[-69.88742,-70.03736,-70.09957,-69.94986], "fy":[27.84059,27.46315,27.29962,27.67902]}, - {"t":0.45789, "x":6.59659, "y":2.05876, "heading":2.35405, "vx":-1.96458, "vy":0.77257, "omega":-0.01187, "ax":-4.2767, "ay":1.68606, "alpha":-0.04817, "fx":[-69.78996,-69.9683,-70.04176,-69.86374], "fy":[27.88424,27.43652,27.24199,27.69246]}, - {"t":0.49311, "x":6.52474, "y":2.08702, "heading":2.35363, "vx":-2.11522, "vy":0.83196, "omega":-0.01357, "ax":-4.26881, "ay":1.68536, "alpha":-0.06088, "fx":[-69.62791,-69.85369,-69.94553,-69.72025], "fy":[27.95658,27.39233,27.14613,27.71477]}, - {"t":0.52833, "x":6.44759, "y":2.11737, "heading":2.35315, "vx":-2.26557, "vy":0.89132, "omega":-0.01571, "ax":-4.25307, "ay":1.68396, "alpha":-0.08626, "fx":[-69.3053,-69.6263,-69.75345,-69.43341], "fy":[28.09969,27.30464,26.95514,27.75894]}, - {"t":0.56355, "x":6.36516, "y":2.14981, "heading":2.3526, "vx":-2.41537, "vy":0.95063, "omega":-0.01875, "ax":-4.20634, "ay":1.6797, "alpha":-0.16201, "fx":[-68.34944,-68.95836,-69.18038,-68.5743], "fy":[28.51629,27.04698,26.38831,27.8879]}, - {"t":0.59877, "x":6.27747, "y":2.18433, "heading":2.35194, "vx":-2.56353, "vy":1.0098, "omega":-0.02446, "ax":-0.45392, "ay":0.74781, "alpha":-6.28297, "fx":[-7.03614,-31.37611,-9.50868,18.23821], "fy":[35.19987,12.48229,-13.24336,14.46201]}, - {"t":0.634, "x":6.1869, "y":2.22036, "heading":2.35108, "vx":-2.57952, "vy":1.03613, "omega":-0.24576, "ax":4.22467, "ay":-1.62933, "alpha":-0.14629, "fx":[69.43044,68.90177,68.69875,69.23052], "fy":[-25.6603,-27.01123,-27.60039,-26.27363]}, - {"t":0.66922, "x":6.09866, "y":2.25585, "heading":2.34242, "vx":-2.43072, "vy":0.97875, "omega":-0.25091, "ax":4.26267, "ay":-1.65863, "alpha":-0.06865, "fx":[69.86321,69.61391,69.50949,69.75957], "fy":[-26.65471,-27.29057,-27.5736,-26.94315]}, - {"t":0.70444, "x":6.01569, "y":2.28929, "heading":2.33358, "vx":-2.28058, "vy":0.92033, "omega":-0.25333, "ax":4.27531, "ay":-1.66843, "alpha":-0.04282, "fx":[70.00469,69.84932,69.78136,69.93706], "fy":[-26.98705,-27.38305,-27.56333,-27.16942]}, - {"t":0.73966, "x":5.93802, "y":2.32067, "heading":2.32466, "vx":-2.12999, "vy":0.86156, "omega":-0.25483, "ax":4.28162, "ay":-1.67334, "alpha":-0.02989, "fx":[70.0748,69.9666,69.91769,70.02607], "fy":[-27.15363,-27.4293,-27.55769,-27.28303]}, - {"t":0.77488, "x":5.86565, "y":2.34998, "heading":2.31569, "vx":-1.97918, "vy":0.80262, "omega":-0.25589, "ax":4.2854, "ay":-1.67628, "alpha":-0.02212, "fx":[70.11662,70.0368,69.99966,70.07959], "fy":[-27.25384,-27.45715,-27.55402,-27.35126]}, - {"t":0.8101, "x":5.7986, "y":2.37721, "heading":2.30667, "vx":-1.82824, "vy":0.74358, "omega":-0.25667, "ax":4.28792, "ay":-1.67825, "alpha":-0.01693, "fx":[70.14436,70.08349,70.05441,70.11534], "fy":[-27.32083,-27.47584,-27.55138,-27.39669]}, - {"t":0.84533, "x":5.73686, "y":2.40236, "heading":2.29763, "vx":-1.67722, "vy":0.68447, "omega":-0.25726, "ax":4.28973, "ay":-1.67965, "alpha":-0.01321, "fx":[70.16409,70.11677,70.09358,70.14095], "fy":[-27.36882,-27.48931,-27.54936,-27.42906]}, - {"t":0.88055, "x":5.68045, "y":2.42543, "heading":2.28857, "vx":-1.52612, "vy":0.62531, "omega":-0.25773, "ax":4.29108, "ay":-1.68071, "alpha":-0.01041, "fx":[70.17882,70.14166,70.12301,70.1602], "fy":[-27.40492,-27.49952,-27.54773,-27.45324]}, - {"t":0.91577, "x":5.62936, "y":2.44641, "heading":2.27949, "vx":-1.37498, "vy":0.56611, "omega":-0.25809, "ax":4.29213, "ay":-1.68153, "alpha":-0.00824, "fx":[70.19024,70.16098,70.14594,70.17522], "fy":[-27.43308,-27.50758,-27.54638,-27.47196]}, - {"t":0.95099, "x":5.58359, "y":2.4653, "heading":2.2704, "vx":-1.22381, "vy":0.50688, "omega":-0.25838, "ax":4.29297, "ay":-1.68218, "alpha":-0.00649, "fx":[70.19934,70.17638,70.16431,70.18728], "fy":[-27.45568,-27.51412,-27.54523,-27.48683]}, - {"t":0.98621, "x":5.54315, "y":2.48211, "heading":2.2613, "vx":-1.0726, "vy":0.44763, "omega":-0.25861, "ax":4.29365, "ay":-1.68272, "alpha":-0.00506, "fx":[70.20676,70.18894,70.17936,70.19718], "fy":[-27.47422,-27.51957,-27.54424,-27.49892]}, - {"t":1.02144, "x":5.50803, "y":2.49684, "heading":2.25219, "vx":-0.92137, "vy":0.38837, "omega":-0.25879, "ax":1.6242, "ay":3.27316, "alpha":-0.49769, "fx":[25.93353,23.72883,27.25891,29.28902], "fy":[54.60101,54.60261,52.40239,52.4338]}, - {"t":1.05438, "x":5.47856, "y":2.51141, "heading":2.24367, "vx":-0.86785, "vy":0.49621, "omega":-0.27519, "ax":0.42819, "ay":0.72538, "alpha":-1.65936, "fx":[7.65701,0.39714,6.41552,13.53059], "fy":[18.07444,12.78763,5.45811,11.11394]}, - {"t":1.08733, "x":5.4502, "y":2.52815, "heading":2.2346, "vx":-0.85375, "vy":0.52011, "omega":-0.32986, "ax":0.03999, "ay":0.06546, "alpha":-1.38973, "fx":[1.36978,-4.74032,-0.05823,6.04408], "fy":[6.4406,1.67831,-4.31251,0.47445]}, - {"t":1.12028, "x":5.42209, "y":2.54532, "heading":2.22373, "vx":-0.85243, "vy":0.52227, "omega":-0.37565, "ax":0.00352, "ay":0.00574, "alpha":-1.01018, "fx":[0.61991,-3.85657,-0.50477,3.97137], "fy":[3.99639,0.57328,-3.80937,-0.38511]}, - {"t":1.15323, "x":5.39401, "y":2.56253, "heading":2.21136, "vx":-0.85231, "vy":0.52246, "omega":-0.40893, "ax":0.00031, "ay":0.0005, "alpha":-0.63189, "fx":[0.38697,-2.43934,-0.37692,2.44937], "fy":[2.44496,0.33822,-2.4286,-0.32182]}, - {"t":1.18618, "x":5.36592, "y":2.57975, "heading":2.19788, "vx":-0.8523, "vy":0.52247, "omega":-0.42975, "ax":0.00003, "ay":0.00004, "alpha":-0.25488, "fx":[0.16773,-0.98363,-0.16685,0.9845], "fy":[0.98144,0.1471,-0.98001,-0.14567]}, - {"t":1.21912, "x":5.33784, "y":2.59696, "heading":2.18372, "vx":-0.8523, "vy":0.52247, "omega":-0.43815, "ax":0.0, "ay":0.0, "alpha":0.12161, "fx":[-0.0864,0.46853,0.08647,-0.46845], "fy":[-0.4667,-0.07642,0.46682,0.07655]}, - {"t":1.25207, "x":5.30976, "y":2.61418, "heading":2.16929, "vx":-0.8523, "vy":0.52247, "omega":-0.43414, "ax":0.0, "ay":0.0, "alpha":0.49835, "fx":[-0.38178,1.91512,0.38179,-1.91511], "fy":[-1.90742,-0.34111,1.90743,0.34112]}, - {"t":1.28502, "x":5.28168, "y":2.63139, "heading":2.15498, "vx":-0.8523, "vy":0.52247, "omega":-0.41772, "ax":0.0, "ay":0.0, "alpha":0.8761, "fx":[-0.71908,3.35789,0.71908,-3.35789], "fy":[-3.34335,-0.64779,3.34335,0.64779]}, - {"t":1.31797, "x":5.2536, "y":2.64861, "heading":2.14122, "vx":-0.8523, "vy":0.52247, "omega":-0.38886, "ax":0.0, "ay":0.0, "alpha":1.25562, "fx":[-1.09643,4.79926,1.09643,-4.79926], "fy":[-4.77702,-0.99455,4.77702,0.99455]}, - {"t":1.35092, "x":5.22552, "y":2.66582, "heading":2.12841, "vx":-0.8523, "vy":0.52247, "omega":-0.34749, "ax":0.0, "ay":0.0, "alpha":1.63759, "fx":[-1.50967,6.24209,1.50967,-6.24209], "fy":[-6.21138,-1.37718,6.21138,1.37718]}, - {"t":1.38386, "x":5.19744, "y":2.68303, "heading":2.11696, "vx":-0.8523, "vy":0.52247, "omega":-0.29353, "ax":0.0, "ay":0.0, "alpha":2.02262, "fx":[-1.95233,7.68977,1.95234,-7.68976], "fy":[-7.64995,-1.78914,7.64997,1.78915]}, - {"t":1.41681, "x":5.16935, "y":2.70025, "heading":2.10729, "vx":-0.8523, "vy":0.52247, "omega":-0.22689, "ax":0.0, "ay":0.00001, "alpha":2.41124, "fx":[-2.41549,9.14625,2.41559,-9.14615], "fy":[-9.09679,-2.22138,9.09696,2.22155]}, - {"t":1.44976, "x":5.14127, "y":2.71746, "heading":2.09981, "vx":-0.8523, "vy":0.52247, "omega":-0.14745, "ax":0.00015, "ay":-0.00001, "alpha":2.80387, "fx":[-2.88535,10.61831,2.89038,-10.61346], "fy":[-10.55704,-2.66279,10.55663,2.66246]}, - {"t":1.48271, "x":5.11319, "y":2.73468, "heading":2.09495, "vx":-0.8523, "vy":0.52247, "omega":-0.05507, "ax":2.31114, "ay":-1.41689, "alpha":1.58625, "fx":[34.72184,42.23227,41.13315,33.04396], "fy":[-30.36074,-23.24701,-15.63473,-23.41128]}, - {"t":1.51565, "x":5.08636, "y":2.75112, "heading":2.09314, "vx":-0.77615, "vy":0.47579, "omega":-0.0028, "ax":3.90802, "ay":-2.39566, "alpha":0.02955, "fx":[63.77526,63.92479,64.002,63.85256], "fy":[-39.35008,-39.109,-38.97834,-39.22034]}, - {"t":1.5486, "x":5.06291, "y":2.7655, "heading":2.09305, "vx":-0.64739, "vy":0.39686, "omega":-0.00183, "ax":3.92328, "ay":-2.40502, "alpha":0.0166, "fx":[64.07416,64.15817,64.20225,64.11827], "fy":[-39.4222,-39.28598,-39.21272,-39.34924]}, - {"t":1.58155, "x":5.04371, "y":2.77727, "heading":2.09299, "vx":-0.51812, "vy":0.31762, "omega":-0.00128, "ax":3.92843, "ay":-2.40818, "alpha":0.01224, "fx":[64.17498,64.23696,64.26964,64.20769], "fy":[-39.44643,-39.34575,-39.29175,-39.39259]}, - {"t":1.6145, "x":5.02877, "y":2.78643, "heading":2.09294, "vx":-0.38869, "vy":0.23827, "omega":-0.00088, "ax":3.93101, "ay":-2.40976, "alpha":0.01006, "fx":[64.22561,64.27653,64.30346,64.25255], "fy":[-39.45858,-39.37578,-39.33142,-39.41433]}, - {"t":1.64745, "x":5.0181, "y":2.79297, "heading":2.09291, "vx":-0.25917, "vy":0.15888, "omega":-0.00055, "ax":3.93256, "ay":-2.41072, "alpha":0.00875, "fx":[64.25605,64.30032,64.32378,64.27951], "fy":[-39.46588,-39.39384,-39.35528,-39.4274]}, - {"t":1.68039, "x":5.0117, "y":2.7969, "heading":2.0929, "vx":-0.1296, "vy":0.07945, "omega":-0.00026, "ax":3.9336, "ay":-2.41135, "alpha":0.00787, "fx":[64.27637,64.31621,64.33734,64.29751], "fy":[-39.47075,-39.4059,-39.3712,-39.43612]}, - {"t":1.71334, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.299, "ay":1.67617, "alpha":-0.06921, "fx":[-70.09973,-70.35334,-70.46085,-70.20797], "fy":[27.86452,27.21888,26.937,27.58836]}, + {"t":0.03309, "x":7.04424, "y":1.88278, "heading":2.35619, "vx":-0.14226, "vy":0.05547, "omega":-0.00229, "ax":-4.29861, "ay":1.6763, "alpha":-0.07063, "fx":[-70.0896,-70.34846,-70.45812,-70.20001], "fy":[27.87604,27.21728,26.9296,27.59432]}, + {"t":0.06618, "x":7.03718, "y":1.88554, "heading":2.35612, "vx":-0.28451, "vy":0.11094, "omega":-0.00463, "ax":-4.29817, "ay":1.67644, "alpha":-0.07221, "fx":[-70.07826,-70.34299,-70.45506,-70.19112], "fy":[27.88892,27.21552,26.92132,27.60095]}, + {"t":0.09928, "x":7.02541, "y":1.89013, "heading":2.35597, "vx":-0.42675, "vy":0.16642, "omega":-0.00702, "ax":-4.29767, "ay":1.67661, "alpha":-0.074, "fx":[-70.0655,-70.33682,-70.45163,-70.18113], "fy":[27.90343,27.21357,26.91198,27.60838]}, + {"t":0.13237, "x":7.00894, "y":1.89655, "heading":2.35573, "vx":-0.56896, "vy":0.2219, "omega":-0.00947, "ax":-4.29711, "ay":1.67679, "alpha":-0.07602, "fx":[-70.05103,-70.32982,-70.44774,-70.16982], "fy":[27.91988,27.21141,26.90138,27.61676]}, + {"t":0.16546, "x":6.98776, "y":1.90481, "heading":2.35542, "vx":-0.71116, "vy":0.27739, "omega":-0.01198, "ax":-4.29647, "ay":1.677, "alpha":-0.07833, "fx":[-70.03448,-70.32181,-70.4433,-70.15689], "fy":[27.93869,27.20896,26.88926,27.62631]}, + {"t":0.19855, "x":6.96187, "y":1.91491, "heading":2.35502, "vx":-0.85334, "vy":0.33288, "omega":-0.01457, "ax":-4.29573, "ay":1.67725, "alpha":-0.081, "fx":[-70.01537,-70.31256,-70.43818,-70.14198], "fy":[27.96041,27.20617,26.87525,27.63731]}, + {"t":0.23164, "x":6.93128, "y":1.92684, "heading":2.35454, "vx":-0.9955, "vy":0.38839, "omega":-0.01725, "ax":-4.29487, "ay":1.67753, "alpha":-0.08412, "fx":[-69.99306,-70.30176,-70.43221,-70.12457], "fy":[27.98574,27.20294,26.85888,27.65011]}, + {"t":0.26474, "x":6.89598, "y":1.94062, "heading":2.35397, "vx":-1.13762, "vy":0.4439, "omega":-0.02004, "ax":-4.29385, "ay":1.67787, "alpha":-0.0878, "fx":[-69.96668,-70.289,-70.42515,-70.10399], "fy":[28.01568,27.19916,26.83953,27.66523]}, + {"t":0.29783, "x":6.85599, "y":1.95622, "heading":2.35331, "vx":-1.27971, "vy":0.49942, "omega":-0.02294, "ax":-4.29262, "ay":1.67827, "alpha":-0.09222, "fx":[-69.935,-70.27368,-70.41667,-70.07928], "fy":[28.0516,27.19464,26.81628,27.68336]}, + {"t":0.33092, "x":6.81129, "y":1.97367, "heading":2.35255, "vx":-1.42176, "vy":0.55496, "omega":-0.02599, "ax":-4.29112, "ay":1.67876, "alpha":-0.09763, "fx":[-69.89625,-70.25497,-70.4063,-70.04903], "fy":[28.09549,27.18913,26.78784,27.70552]}, + {"t":0.36401, "x":6.76189, "y":1.99295, "heading":2.35169, "vx":-1.56376, "vy":0.61051, "omega":-0.02923, "ax":-4.28924, "ay":1.67937, "alpha":-0.10439, "fx":[-69.84778,-70.2316,-70.39333,-70.01116], "fy":[28.15032,27.18227,26.75226,27.73322]}, + {"t":0.3971, "x":6.70779, "y":2.01408, "heading":2.35072, "vx":-1.7057, "vy":0.66609, "omega":-0.03268, "ax":-4.28683, "ay":1.68016, "alpha":-0.1131, "fx":[-69.78538,-70.20158,-70.37662,-69.96237], "fy":[28.22075,27.17347,26.70646,27.76888]}, + {"t":0.43019, "x":6.649, "y":2.03704, "heading":2.34964, "vx":-1.84756, "vy":0.72169, "omega":-0.03642, "ax":-4.2836, "ay":1.68121, "alpha":-0.12472, "fx":[-69.70209,-70.16162,-70.35429,-69.89712], "fy":[28.31453,27.16174,26.64533,27.81649]}, + {"t":0.46329, "x":6.58552, "y":2.06184, "heading":2.34843, "vx":-1.98932, "vy":0.77732, "omega":-0.04055, "ax":-4.27908, "ay":1.68267, "alpha":-0.14101, "fx":[-69.58527,-70.1058,-70.32293,-69.8054], "fy":[28.44557,27.14533,26.55962,27.88328]}, + {"t":0.49638, "x":6.51734, "y":2.08848, "heading":2.34709, "vx":-2.13092, "vy":0.833, "omega":-0.04522, "ax":-4.27228, "ay":1.68486, "alpha":-0.1655, "fx":[-69.40963,-70.02236,-70.27566,-69.667], "fy":[28.64156,27.12075,26.4308,27.98377]}, + {"t":0.52947, "x":6.44449, "y":2.11697, "heading":2.3456, "vx":-2.2723, "vy":0.88876, "omega":-0.05069, "ax":-4.2609, "ay":1.68848, "alpha":-0.20644, "fx":[-69.11582,-69.88402,-70.19625,-69.43419], "fy":[28.96657,27.07989,26.21546,28.15197]}, + {"t":0.56256, "x":6.36696, "y":2.14731, "heading":2.34392, "vx":-2.4133, "vy":0.94463, "omega":-0.05752, "ax":-4.23795, "ay":1.69565, "alpha":-0.28875, "fx":[-68.5244,-69.61003,-70.03505,-68.9604], "fy":[29.61028,26.99872,25.78253,28.49092]}, + {"t":0.59565, "x":6.28478, "y":2.1795, "heading":2.34201, "vx":-2.55354, "vy":1.00074, "omega":-0.06708, "ax":-4.16769, "ay":1.71643, "alpha":-0.539, "fx":[-66.72291,-68.8087,-69.53152,-67.47181], "fy":[31.48802,26.76041,24.46682,29.52635]}, + {"t":0.62875, "x":6.198, "y":2.21355, "heading":2.33979, "vx":-2.69146, "vy":1.05754, "omega":-0.08492, "ax":1.06358, "ay":0.49866, "alpha":-11.13879, "fx":[21.37219,-31.08762,24.22399,55.04135], "fy":[48.90824,14.63109,-38.95345,8.02235]}, + {"t":0.66184, "x":6.10951, "y":2.24882, "heading":2.33698, "vx":-2.65626, "vy":1.07405, "omega":-0.45352, "ax":4.23659, "ay":-1.55405, "alpha":-0.43663, "fx":[70.30043,68.82078,68.19989,69.7195], "fy":[-22.41695,-26.47229,-28.29296,-24.44051]}, + {"t":0.69493, "x":6.02393, "y":2.28351, "heading":2.32198, "vx":-2.51606, "vy":1.02262, "omega":-0.46797, "ax":4.27176, "ay":-1.61302, "alpha":-0.20061, "fx":[70.34075,69.64282,69.32496,70.03204], "fy":[-24.99763,-26.84844,-27.72002,-25.91287]}, + {"t":0.72802, "x":5.94301, "y":2.31647, "heading":2.30649, "vx":-2.3747, "vy":0.96924, "omega":-0.47461, "ax":4.28331, "ay":-1.63306, "alpha":-0.12069, "fx":[70.33489,69.91333,69.71109,70.13632], "fy":[-25.86934,-26.97468,-27.51775,-26.42803]}, + {"t":0.76111, "x":5.86677, "y":2.34765, "heading":2.29079, "vx":-2.23296, "vy":0.9152, "omega":-0.4786, "ax":4.28904, "ay":-1.64316, "alpha":-0.08045, "fx":[70.32786,70.04738,69.90667,70.18895], "fy":[-26.3082,-27.03939,-27.41342,-26.68903]}, + {"t":0.79421, "x":5.79523, "y":2.37704, "heading":2.27495, "vx":-2.09103, "vy":0.86083, "omega":-0.48126, "ax":4.29248, "ay":-1.64924, "alpha":-0.0562, "fx":[70.32209,70.12703,70.02503,70.22105], "fy":[-26.57302,-27.07971,-27.34924,-26.84578]}, + {"t":0.8273, "x":5.72838, "y":2.40462, "heading":2.25902, "vx":-1.94898, "vy":0.80625, "omega":-0.48312, "ax":4.29476, "ay":-1.65331, "alpha":-0.03997, "fx":[70.31745,70.17954,70.10449,70.24294], "fy":[-26.75054,-27.10794,-27.30545,-26.94963]}, + {"t":0.86039, "x":5.66624, "y":2.43039, "heading":2.24303, "vx":-1.80686, "vy":0.75154, "omega":-0.48445, "ax":4.29638, "ay":-1.65621, "alpha":-0.02835, "fx":[70.31367,70.21653,70.16161,70.25904], "fy":[-26.87802,-27.12931,-27.27347,-27.02295]}, + {"t":0.89348, "x":5.6088, "y":2.45436, "heading":2.227, "vx":-1.66468, "vy":0.69673, "omega":-0.48538, "ax":4.2976, "ay":-1.6584, "alpha":-0.01961, "fx":[70.31052,70.24383,70.20469,70.27153], "fy":[-26.97415,-27.14644,-27.24896,-27.07703]}, + {"t":0.92657, "x":5.55606, "y":2.47651, "heading":2.21094, "vx":-1.52247, "vy":0.64185, "omega":-0.48603, "ax":4.29855, "ay":-1.6601, "alpha":-0.0128, "fx":[70.30784,70.26466,70.23838,70.28162], "fy":[-27.04933,-27.16075,-27.22949,-27.11822]}, + {"t":0.95966, "x":5.50803, "y":2.49684, "heading":2.19486, "vx":-1.38022, "vy":0.58692, "omega":-0.48646, "ax":1.71789, "ay":3.61284, "alpha":-0.41395, "fx":[27.59243,25.53559,28.6411,30.56803], "fy":[59.7487,60.12639,58.38095,57.9959]}, + {"t":0.9887, "x":5.46869, "y":2.5154, "heading":2.18073, "vx":-1.33035, "vy":0.69181, "omega":-0.49848, "ax":1.36561, "ay":2.46021, "alpha":-0.71883, "fx":[22.11984,18.80876,22.62071,25.75148], "fy":[42.25465,41.49602,38.12459,39.00386]}, + {"t":1.01773, "x":5.43064, "y":2.53652, "heading":2.16626, "vx":-1.2907, "vy":0.76323, "omega":-0.51934, "ax":0.49119, "ay":0.81352, "alpha":-0.2123, "fx":[8.16851,7.19212,7.89246,8.86708], "fy":[14.08808,13.47666,12.50734,13.12565]}, + {"t":1.04676, "x":5.39337, "y":2.55902, "heading":2.15118, "vx":-1.27644, "vy":0.78685, "omega":-0.52551, "ax":0.11215, "ay":0.18109, "alpha":1.10792, "fx":[0.91684,6.08063,2.75335,-2.41719], "fy":[-1.26966,2.12916,7.16823,3.81443]}, + {"t":1.07579, "x":5.35636, "y":2.58194, "heading":2.13593, "vx":-1.27318, "vy":0.79211, "omega":-0.49334, "ax":0.02421, "ay":0.03887, "alpha":2.42888, "fx":[-1.77197,9.66831,2.56619,-8.87959], "fy":[-8.60504,-1.32653,9.85252,2.62087]}, + {"t":1.10483, "x":5.31941, "y":2.60496, "heading":2.1216, "vx":-1.27248, "vy":0.79324, "omega":-0.42283, "ax":0.00531, "ay":0.00852, "alpha":3.69033, "fx":[-3.40974,14.1316,3.58456,-13.95891], "fy":[-13.84056,-3.0539,14.10732,3.34446]}, + {"t":1.13386, "x":5.28247, "y":2.62799, "heading":2.10933, "vx":-1.27233, "vy":0.79348, "omega":-0.31569, "ax":0.00158, "ay":0.00223, "alpha":4.87503, "fx":[-4.81993,18.52638,4.87238,-18.47533], "fy":[-18.36821,-4.41423,18.43562,4.49293]}, + {"t":1.16289, "x":5.24553, "y":2.65103, "heading":2.10016, "vx":-1.27228, "vy":0.79355, "omega":-0.17415, "ax":0.88821, "ay":-0.55438, "alpha":5.43488, "fx":[9.38795,33.33562,22.13789,-6.7796], "fy":[-29.88732,-13.53176,12.09076,-4.92407]}, + {"t":1.19192, "x":5.20897, "y":2.67383, "heading":2.09511, "vx":-1.24649, "vy":0.77745, "omega":-0.01636, "ax":3.85861, "ay":-2.40658, "alpha":0.12768, "fx":[62.59185,63.24888,63.56951,62.9136], "fy":[-40.13078,-39.1061,-38.54679,-39.58858]}, + {"t":1.22096, "x":5.17441, "y":2.69539, "heading":2.09463, "vx":-1.13447, "vy":0.70758, "omega":-0.01266, "ax":3.88928, "ay":-2.42576, "alpha":0.07495, "fx":[63.29184,63.67774,63.87247,63.48702], "fy":[-40.12299,-39.51335,-39.18722,-39.80287]}, + {"t":1.24999, "x":5.14311, "y":2.71491, "heading":2.09426, "vx":-1.02155, "vy":0.63716, "omega":-0.01048, "ax":3.89952, "ay":-2.43217, "alpha":0.0574, "fx":[63.52637,63.82192,63.9728,63.67752], "fy":[-40.11954,-39.6506,-39.40132,-39.8738]}, + {"t":1.27902, "x":5.11509, "y":2.73238, "heading":2.09396, "vx":-0.90834, "vy":0.56655, "omega":-0.00882, "ax":3.90463, "ay":-2.43537, "alpha":0.04864, "fx":[63.64378,63.89419,64.02279,63.77259], "fy":[-40.11764,-39.71947,-39.50841,-39.90914]}, + {"t":1.30805, "x":5.09037, "y":2.7478, "heading":2.0937, "vx":-0.79498, "vy":0.49584, "omega":-0.0074, "ax":3.90771, "ay":-2.43729, "alpha":0.04338, "fx":[63.71426,63.9376,64.05274,63.82957], "fy":[-40.11645,-39.76086,-39.57267,-39.93029]}, + {"t":1.33709, "x":5.06893, "y":2.76117, "heading":2.09349, "vx":-0.68153, "vy":0.42508, "omega":-0.00614, "ax":3.90975, "ay":-2.43857, "alpha":0.03988, "fx":[63.76126,63.96655,64.07267,63.86752], "fy":[-40.11564,-39.78848,-39.6155,-39.94438]}, + {"t":1.36612, "x":5.0508, "y":2.77249, "heading":2.09331, "vx":-0.56802, "vy":0.35428, "omega":-0.00499, "ax":3.91121, "ay":-2.43949, "alpha":0.03738, "fx":[63.79483,63.98724,64.0869,63.89461], "fy":[-40.11504,-39.80823,-39.64609,-39.95443]}, + {"t":1.39515, "x":5.03595, "y":2.78174, "heading":2.09317, "vx":-0.45447, "vy":0.28346, "omega":-0.0039, "ax":3.91231, "ay":-2.44017, "alpha":0.0355, "fx":[63.82001,64.00277,64.09756,63.91492], "fy":[-40.11459,-39.82304,-39.66904,-39.96196]}, + {"t":1.42418, "x":5.02441, "y":2.78894, "heading":2.09305, "vx":-0.34088, "vy":0.21261, "omega":-0.00287, "ax":3.91316, "ay":-2.4407, "alpha":0.03405, "fx":[63.8396,64.01484,64.10584,63.9307], "fy":[-40.11424,-39.83456,-39.68688,-39.96782]}, + {"t":1.45322, "x":5.01616, "y":2.79409, "heading":2.09297, "vx":-0.22727, "vy":0.14175, "omega":-0.00188, "ax":3.91384, "ay":-2.44113, "alpha":0.03288, "fx":[63.85527,64.02451,64.11247,63.94333], "fy":[-40.11395,-39.84378,-39.70116,-39.9725]}, + {"t":1.48225, "x":5.01121, "y":2.79718, "heading":2.09292, "vx":-0.11364, "vy":0.07088, "omega":-0.00093, "ax":3.9144, "ay":-2.44148, "alpha":0.03193, "fx":[63.86809,64.03242,64.11789,63.95365], "fy":[-40.11371,-39.85131,-39.71283,-39.97634]}, + {"t":1.51128, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] From d6678fe5fe1d3d19b0e5e439c1ec6002083faece Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Thu, 3 Apr 2025 21:53:48 -0700 Subject: [PATCH 064/126] adjust slow speed again --- src/main/deploy/choreo/LOtoJ.traj | 126 ++++++++++++------------- src/main/deploy/choreo/PLOtoK.traj | 118 ++++++++++++------------ src/main/deploy/choreo/PLOtoL.traj | 118 ++++++++++++------------ src/main/deploy/choreo/PRMtoB.traj | 142 ++++++++++++++--------------- src/main/deploy/choreo/PROtoC.traj | 109 +++++++++++----------- src/main/deploy/choreo/PROtoD.traj | 110 +++++++++++----------- src/main/deploy/choreo/ROtoE.traj | 109 +++++++++++----------- src/main/deploy/choreo/choreo.chor | 4 +- 8 files changed, 420 insertions(+), 416 deletions(-) diff --git a/src/main/deploy/choreo/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index d361058e..8c038d3c 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -11,7 +11,7 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":4.484625190496445, "y":4.0126113295555115, "r":3.25}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 @@ -26,7 +26,7 @@ {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.484625190496445 m", "val":4.484625190496445}, "y":{"exp":"4.0126113295555115 m", "val":4.0126113295555115}, "r":{"exp":"3.25 m", "val":3.25}}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], "targetDt":{ @@ -36,68 +36,68 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.90591,1.83227], + "waypoints":[0.0,0.9336,1.90766], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.17838, "ay":-1.95744, "alpha":0.01027, "fx":[-68.29393,-68.33891,-68.3231,-68.27813], "fy":[-32.03141,-31.93545,-31.96956,-32.06538]}, - {"t":0.03235, "x":7.10913, "y":6.23454, "heading":-2.35619, "vx":-0.13519, "vy":-0.06333, "omega":0.00033, "ax":-4.17808, "ay":-1.95731, "alpha":0.01057, "fx":[-68.28856,-68.33483,-68.31858,-68.27232], "fy":[-32.0302,-31.9315,-31.96659,-32.06514]}, - {"t":0.06471, "x":7.10257, "y":6.23147, "heading":-2.35618, "vx":-0.27036, "vy":-0.12666, "omega":0.00067, "ax":-4.17774, "ay":-1.95717, "alpha":0.0109, "fx":[-68.28255,-68.33026,-68.31351,-68.26581], "fy":[-32.02885,-31.92708,-31.96327,-32.06488]}, - {"t":0.09706, "x":7.09163, "y":6.22634, "heading":-2.35616, "vx":-0.40553, "vy":-0.18998, "omega":0.00103, "ax":-4.17735, "ay":-1.95701, "alpha":0.01127, "fx":[-68.27577,-68.3251,-68.30779,-68.25846], "fy":[-32.02733,-31.92209,-31.95952,-32.06459]}, - {"t":0.12942, "x":7.07633, "y":6.21917, "heading":-2.35613, "vx":-0.54069, "vy":-0.2533, "omega":0.00139, "ax":-4.17692, "ay":-1.95683, "alpha":0.01169, "fx":[-68.26806,-68.31925,-68.3013,-68.25012], "fy":[-32.0256,-31.91643,-31.95527,-32.06426]}, - {"t":0.16177, "x":7.05665, "y":6.20995, "heading":-2.35608, "vx":-0.67583, "vy":-0.31661, "omega":0.00177, "ax":-4.17642, "ay":-1.95662, "alpha":0.01218, "fx":[-68.25924,-68.31254,-68.29386,-68.24056], "fy":[-32.02362,-31.90994,-31.95039,-32.06388]}, - {"t":0.19412, "x":7.03259, "y":6.19869, "heading":-2.35603, "vx":-0.81095, "vy":-0.37991, "omega":0.00216, "ax":-4.17585, "ay":-1.95637, "alpha":0.01274, "fx":[-68.24902,-68.30478,-68.28525,-68.2295], "fy":[-32.02132,-31.90242,-31.94475,-32.06345]}, - {"t":0.22648, "x":7.00417, "y":6.18537, "heading":-2.35596, "vx":-0.94606, "vy":-0.44321, "omega":0.00258, "ax":-4.17517, "ay":-1.95609, "alpha":0.01339, "fx":[-68.23706,-68.29569,-68.27517,-68.21655], "fy":[-32.01863,-31.89362,-31.93815,-32.06293]}, - {"t":0.25883, "x":6.97138, "y":6.17001, "heading":-2.35587, "vx":-1.08114, "vy":-0.5065, "omega":0.00301, "ax":-4.17437, "ay":-1.95575, "alpha":0.01417, "fx":[-68.22286,-68.2849,-68.26322,-68.20118], "fy":[-32.01544,-31.88317,-31.93032,-32.06233]}, - {"t":0.29119, "x":6.93421, "y":6.1526, "heading":-2.35578, "vx":-1.2162, "vy":-0.56977, "omega":0.00347, "ax":-4.17341, "ay":-1.95534, "alpha":0.01511, "fx":[-68.20573,-68.27189,-68.2488,-68.18265], "fy":[-32.01159,-31.87057,-31.92087,-32.0616]}, - {"t":0.32354, "x":6.89268, "y":6.13314, "heading":-2.35566, "vx":-1.35122, "vy":-0.63304, "omega":0.00396, "ax":-4.17222, "ay":-1.95484, "alpha":0.01627, "fx":[-68.18468,-68.2559,-68.23107,-68.15986], "fy":[-32.00686,-31.85508,-31.90926,-32.06071]}, - {"t":0.35589, "x":6.84678, "y":6.11163, "heading":-2.35554, "vx":-1.48621, "vy":-0.69628, "omega":0.00448, "ax":-4.17073, "ay":-1.95421, "alpha":0.01773, "fx":[-68.15815,-68.23576,-68.20876,-68.13117], "fy":[-32.0009,-31.83556,-31.89465,-32.05958]}, - {"t":0.38825, "x":6.79651, "y":6.08808, "heading":-2.35539, "vx":-1.62115, "vy":-0.75951, "omega":0.00506, "ax":-4.16879, "ay":-1.9534, "alpha":0.01963, "fx":[-68.12371,-68.20961,-68.17981,-68.09393], "fy":[-31.99316,-31.81021,-31.87568,-32.05813]}, - {"t":0.4206, "x":6.74188, "y":6.06249, "heading":-2.35523, "vx":-1.75603, "vy":-0.82271, "omega":0.00569, "ax":-4.16617, "ay":-1.95229, "alpha":0.02219, "fx":[-68.07719,-68.1743,-68.14073,-68.04365], "fy":[-31.98271,-31.77598,-31.85008,-32.05617]}, - {"t":0.45296, "x":6.68288, "y":6.03485, "heading":-2.35504, "vx":-1.89082, "vy":-0.88588, "omega":0.00641, "ax":-4.16244, "ay":-1.95072, "alpha":0.02585, "fx":[-68.01089,-68.12401,-68.08512,-67.97204], "fy":[-31.96781,-31.7272,-31.81363,-32.05338]}, - {"t":0.48531, "x":6.61953, "y":6.00516, "heading":-2.35484, "vx":-2.02549, "vy":-0.94899, "omega":0.00725, "ax":-4.15671, "ay":-1.9483, "alpha":0.0315, "fx":[-67.9088,-68.0466,-67.99963,-67.86189], "fy":[-31.94489,-31.65207,-31.75758,-32.04912]}, - {"t":0.51766, "x":6.55182, "y":5.97344, "heading":-2.3546, "vx":-2.15998, "vy":-1.01202, "omega":0.00827, "ax":-4.14674, "ay":-1.94409, "alpha":0.04135, "fx":[-67.7312,-67.91207,-67.85136,-67.67061], "fy":[-31.90502,-31.5214,-31.66032,-32.04175]}, - {"t":0.55002, "x":6.47977, "y":5.93968, "heading":-2.35433, "vx":-2.29414, "vy":-1.07492, "omega":0.0096, "ax":-4.12514, "ay":-1.93496, "alpha":0.06291, "fx":[-67.34519,-67.62025,-67.53109,-67.25631], "fy":[-31.81851,-31.2374,-31.44995,-32.02603]}, - {"t":0.58237, "x":6.40338, "y":5.90389, "heading":-2.35402, "vx":-2.42761, "vy":-1.13753, "omega":0.01164, "ax":-4.04299, "ay":-1.90022, "alpha":0.14903, "fx":[-65.86064,-66.5122,-66.32823,-65.6796], "fy":[-31.49346,-30.1367,-30.65103,-31.97846]}, - {"t":0.61473, "x":6.32272, "y":5.86609, "heading":-2.35365, "vx":-2.55841, "vy":-1.19901, "omega":0.01646, "ax":3.86962, "ay":1.79787, "alpha":0.37431, "fx":[63.88861,62.30187,62.61506,64.23813], "fy":[28.46701,31.6037,30.4227,27.07384]}, - {"t":0.64708, "x":6.24197, "y":5.82824, "heading":-2.35311, "vx":-2.43322, "vy":-1.14084, "omega":0.02857, "ax":4.10975, "ay":1.92151, "alpha":0.07129, "fx":[67.29256,66.9825,67.08055,67.39098], "fy":[31.20846,31.85763,31.62079,30.96521]}, - {"t":0.67943, "x":6.1654, "y":5.79234, "heading":-2.35219, "vx":-2.30025, "vy":-1.07867, "omega":0.03088, "ax":4.14181, "ay":1.93805, "alpha":0.03787, "fx":[67.7656,67.6005,67.65584,67.82101], "fy":[31.57282,31.92141,31.79489,31.44447]}, - {"t":0.71179, "x":6.09315, "y":5.75845, "heading":-2.35119, "vx":-2.16625, "vy":-1.01597, "omega":0.0321, "ax":4.15449, "ay":1.94459, "alpha":0.02484, "fx":[67.95354,67.84519,67.8824,67.99078], "fy":[31.71745,31.94701,31.8637,31.63336]}, - {"t":0.74414, "x":6.02523, "y":5.7266, "heading":-2.35015, "vx":-2.03183, "vy":-0.95305, "omega":0.03291, "ax":4.16128, "ay":1.9481, "alpha":0.01789, "fx":[68.05443,67.97637,68.00357,68.08164], "fy":[31.79508,31.96075,31.90054,31.73446]}, - {"t":0.7765, "x":5.96167, "y":5.69678, "heading":-2.34909, "vx":-1.8972, "vy":-0.89002, "omega":0.03348, "ax":4.16551, "ay":1.95028, "alpha":0.01357, "fx":[68.11737,68.05817,68.07901,68.13822], "fy":[31.84351,31.96932,31.9235,31.79745]}, - {"t":0.80885, "x":5.90247, "y":5.66901, "heading":-2.348, "vx":-1.76243, "vy":-0.82692, "omega":0.03392, "ax":4.1684, "ay":1.95178, "alpha":0.01063, "fx":[68.1604,68.11405,68.13049,68.17685], "fy":[31.8766,31.97516,31.93917,31.84047]}, - {"t":0.8412, "x":5.84763, "y":5.64327, "heading":-2.34691, "vx":-1.62756, "vy":-0.76377, "omega":0.03427, "ax":4.1705, "ay":1.95286, "alpha":0.00849, "fx":[68.19166,68.15465,68.16787,68.20489], "fy":[31.90064,31.9794,31.95056,31.87171]}, - {"t":0.87356, "x":5.79716, "y":5.61959, "heading":-2.3458, "vx":-1.49263, "vy":-0.70059, "omega":0.03454, "ax":4.1721, "ay":1.95369, "alpha":0.00687, "fx":[68.21541,68.18548,68.19623,68.22617], "fy":[31.9189,31.98261,31.95921,31.89544]}, - {"t":0.90591, "x":5.75105, "y":5.59794, "heading":-2.34468, "vx":-1.35764, "vy":-0.63738, "omega":0.03476, "ax":0.05987, "ay":-0.10164, "alpha":1.71381, "fx":[7.49955,0.63808,-5.5102,1.28778], "fy":[-1.51775,5.21228,-1.8156,-8.52565]}, - {"t":0.9358, "x":5.71051, "y":5.57885, "heading":-2.34364, "vx":-1.35586, "vy":-0.64042, "omega":0.08598, "ax":0.02666, "ay":-0.05634, "alpha":1.57831, "fx":[6.42525,0.15385,-5.55325,0.71751], "fy":[-0.77006,5.40925,-1.07813,-7.24537]}, - {"t":0.96568, "x":5.67, "y":5.55969, "heading":-2.34107, "vx":-1.35506, "vy":-0.6421, "omega":0.13314, "ax":0.00839, "ay":-0.01769, "alpha":1.4456, "fx":[5.61955,-0.14249,-5.34418,0.41552], "fy":[-0.12749,5.50963,-0.45286,-6.086]}, - {"t":0.99556, "x":5.62951, "y":5.54049, "heading":-2.33709, "vx":-1.35481, "vy":-0.64263, "omega":0.17634, "ax":0.00201, "ay":-0.00424, "alpha":1.31612, "fx":[5.02143,-0.2486,-4.95236,0.31096], "fy":[0.10375,5.21164,-0.24286,-5.34959]}, - {"t":1.02544, "x":5.58903, "y":5.52129, "heading":-2.33182, "vx":-1.35475, "vy":-0.64276, "omega":0.21567, "ax":0.00049, "ay":-0.00089, "alpha":1.18973, "fx":[4.5221,-0.29981,-4.48433,0.29436], "fy":[0.16905,4.76071,-0.19838,-4.78982]}, - {"t":1.05532, "x":5.54855, "y":5.50208, "heading":-2.32538, "vx":-1.35473, "vy":-0.64278, "omega":0.25122, "ax":0.22004, "ay":0.11182, "alpha":1.05402, "fx":[7.64739,3.03381,-0.27413,3.9822], "fy":[2.00108,6.07518,1.65927,-2.42325]}, - {"t":1.08521, "x":5.50816, "y":5.48292, "heading":-2.31787, "vx":-1.34816, "vy":-0.63944, "omega":0.28272, "ax":1.7904, "ay":0.84953, "alpha":0.76819, "fx":[31.90966,28.96054,26.50897,29.69939], "fy":[13.80923,17.15687,14.00248,10.58411]}, - {"t":1.11509, "x":5.46867, "y":5.46419, "heading":-2.30942, "vx":-1.29466, "vy":-0.61406, "omega":0.30567, "ax":1.79896, "ay":0.85352, "alpha":0.66808, "fx":[31.71185,29.07092,27.0392,29.81636], "fy":[13.90053,16.80857,14.02872,11.07561]}, - {"t":1.14497, "x":5.43079, "y":5.44622, "heading":-2.30029, "vx":-1.2409, "vy":-0.58855, "omega":0.32564, "ax":1.8017, "ay":0.85477, "alpha":0.5736, "fx":[31.43378,29.13645,27.42873,29.81874], "fy":[13.95032,16.42794,14.0127,11.50457]}, - {"t":1.17485, "x":5.39451, "y":5.42902, "heading":-2.29056, "vx":-1.18706, "vy":-0.56301, "omega":0.34278, "ax":1.80306, "ay":0.85538, "alpha":0.48071, "fx":[31.13755,29.19577,27.78236,29.79051], "fy":[13.98584,16.03999,13.99188,11.9174]}, - {"t":1.20474, "x":5.35985, "y":5.41258, "heading":-2.28032, "vx":-1.13318, "vy":-0.53745, "omega":0.35714, "ax":1.80386, "ay":0.85573, "alpha":0.3885, "fx":[30.83462,29.25632,28.12082,29.74726], "fy":[14.01183,15.64899,13.97373,12.32352]}, - {"t":1.23462, "x":5.32679, "y":5.3969, "heading":-2.26964, "vx":-1.07928, "vy":-0.51188, "omega":0.36875, "ax":1.8044, "ay":0.85595, "alpha":0.29655, "fx":[30.52846,29.32092,28.4511,29.69374], "fy":[14.02953,15.25595,13.96093,12.72647]}, - {"t":1.2645, "x":5.29534, "y":5.38198, "heading":-2.25862, "vx":-1.02536, "vy":-0.4863, "omega":0.37761, "ax":1.80479, "ay":0.85611, "alpha":0.20463, "fx":[30.22036,29.39095,28.77634,29.63175], "fy":[14.03926,14.86115,13.95478,13.12791]}, - {"t":1.29438, "x":5.26551, "y":5.36783, "heading":-2.24734, "vx":-0.97143, "vy":-0.46072, "omega":0.38373, "ax":1.80508, "ay":0.85622, "alpha":0.11253, "fx":[29.91078,29.46728,29.09825,29.56202], "fy":[14.04101,14.46445,13.95603,13.52901]}, - {"t":1.32427, "x":5.23729, "y":5.35445, "heading":-2.23587, "vx":-0.91749, "vy":-0.43513, "omega":0.38709, "ax":1.8053, "ay":0.85631, "alpha":0.02011, "fx":[29.59986,29.55048,29.41788,29.4849], "fy":[14.03472,14.06571,13.9651,13.93052]}, - {"t":1.35415, "x":5.21068, "y":5.34183, "heading":-2.22431, "vx":-0.86354, "vy":-0.40954, "omega":0.38769, "ax":1.80549, "ay":0.85637, "alpha":-0.07278, "fx":[29.28753,29.64093,29.73598,29.40054], "fy":[14.0203,13.66462,13.98229,14.33312]}, - {"t":1.38403, "x":5.18568, "y":5.32997, "heading":-2.21272, "vx":-0.80959, "vy":-0.38395, "omega":0.38552, "ax":1.80563, "ay":0.85643, "alpha":-0.16627, "fx":[28.97366,29.73894,30.05311,29.30903], "fy":[13.99773,13.261,14.00768,14.7373]}, - {"t":1.41391, "x":5.16229, "y":5.31888, "heading":-2.2012, "vx":-0.75563, "vy":-0.35836, "omega":0.38055, "ax":1.80576, "ay":0.85647, "alpha":-0.26047, "fx":[28.65803,29.84465,30.36976,29.21047], "fy":[13.96701,12.85451,14.04129,15.14361]}, - {"t":1.4438, "x":5.14052, "y":5.30856, "heading":-2.18983, "vx":-0.70167, "vy":-0.33277, "omega":0.37276, "ax":1.80587, "ay":0.8565, "alpha":-0.35552, "fx":[28.34037,29.95813,30.68636,29.10498], "fy":[13.92827,12.44485,14.083,15.55252]}, - {"t":1.47368, "x":5.12036, "y":5.29899, "heading":-2.17869, "vx":-0.64771, "vy":-0.30717, "omega":0.36214, "ax":1.80596, "ay":0.85653, "alpha":-0.45152, "fx":[28.02036,30.07936,31.00334,28.99274], "fy":[13.88168,12.03169,14.1326,15.96451]}, - {"t":1.50356, "x":5.10181, "y":5.2902, "heading":-2.16787, "vx":-0.59374, "vy":-0.28158, "omega":0.34865, "ax":1.80604, "ay":0.85655, "alpha":-0.54859, "fx":[27.69766,30.20825,31.32109,28.87399], "fy":[13.82754,11.61468,14.18975,16.38005]}, - {"t":1.53344, "x":5.08487, "y":5.28217, "heading":-2.15745, "vx":-0.53977, "vy":-0.25598, "omega":0.33225, "ax":1.80611, "ay":0.85657, "alpha":-0.64684, "fx":[27.37185,30.3446,31.64007,28.74902], "fy":[13.76624,11.19342,14.25404,16.79965]}, - {"t":1.56332, "x":5.06955, "y":5.2749, "heading":-2.14752, "vx":-0.4858, "vy":-0.23039, "omega":0.31293, "ax":1.80617, "ay":0.85659, "alpha":-0.74639, "fx":[27.04247,30.48815,31.96074,28.61822], "fy":[13.69829,10.76746,14.32492,17.22381]}, - {"t":1.59321, "x":5.05584, "y":5.2684, "heading":-2.13817, "vx":-0.43183, "vy":-0.20479, "omega":0.29062, "ax":1.80622, "ay":0.85661, "alpha":-0.84736, "fx":[26.70898,30.63855,32.28359,28.48204], "fy":[13.62431,10.33631,14.40174,17.65311]}, - {"t":1.62309, "x":5.04374, "y":5.26266, "heading":-2.12949, "vx":-0.37785, "vy":-0.17919, "omega":0.2653, "ax":1.80627, "ay":0.85662, "alpha":-0.94985, "fx":[26.37081,30.79536,32.60916,28.34105], "fy":[13.54507,9.89942,14.48373,18.08812]}, - {"t":1.65297, "x":5.03326, "y":5.25769, "heading":-2.12156, "vx":-0.32388, "vy":-0.15359, "omega":0.23692, "ax":1.80632, "ay":0.85663, "alpha":-1.054, "fx":[26.02727,30.95804,32.93807,28.19589], "fy":[13.46148,9.45616,14.56998,18.52948]}, - {"t":1.68285, "x":5.02438, "y":5.25348, "heading":-2.11448, "vx":-0.2699, "vy":-0.12799, "omega":0.20542, "ax":1.80636, "ay":0.85664, "alpha":-1.15992, "fx":[25.67764,31.12596,33.27097,28.04733], "fy":[13.37462,9.00584,14.65948,18.97785]}, - {"t":1.71274, "x":5.01713, "y":5.25004, "heading":-2.10834, "vx":-0.21592, "vy":-0.1024, "omega":0.17076, "ax":1.80639, "ay":0.85665, "alpha":-1.26774, "fx":[25.32107,31.29838,33.60859,27.89626], "fy":[13.28571,8.54765,14.75106,19.43398]}, - {"t":1.74262, "x":5.01148, "y":5.24736, "heading":-2.10324, "vx":-0.16194, "vy":-0.0768, "omega":0.13288, "ax":1.80643, "ay":0.85666, "alpha":-1.37759, "fx":[24.95664,31.47444,33.9517,27.74369], "fy":[13.1962,8.08075,14.8434,19.89863]}, - {"t":1.7725, "x":5.00745, "y":5.24545, "heading":-2.09927, "vx":-0.10796, "vy":-0.0512, "omega":0.09171, "ax":1.80646, "ay":0.85667, "alpha":-1.4896, "fx":[24.58334,31.65319,34.30118,27.59075], "fy":[13.10774,7.60416,14.93501,20.37258]}, - {"t":1.80238, "x":5.00503, "y":5.2443, "heading":-2.09653, "vx":-0.05398, "vy":-0.0256, "omega":0.0472, "ax":1.80648, "ay":0.85667, "alpha":-1.57946, "fx":[24.19362,31.8159,34.65882,27.46195], "fy":[13.05416,7.30736,14.9856,20.67288]}, - {"t":1.83227, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.1785, "ay":-1.95764, "alpha":0.00825, "fx":[-68.29881,-68.33496,-68.32225,-68.28611], "fy":[-32.02858,-31.95148,-31.97888,-32.05588]}, + {"t":0.03334, "x":7.10899, "y":6.23448, "heading":-2.35619, "vx":-0.13932, "vy":-0.06527, "omega":0.00028, "ax":-4.1782, "ay":-1.95751, "alpha":0.00851, "fx":[-68.29346,-68.33072,-68.31763,-68.28037], "fy":[-32.02716,-31.94768,-31.97592,-32.05531]}, + {"t":0.06669, "x":7.10202, "y":6.23121, "heading":-2.35619, "vx":-0.27864, "vy":-0.13054, "omega":0.00056, "ax":-4.17785, "ay":-1.95736, "alpha":0.0088, "fx":[-68.28742,-68.32595,-68.31242,-68.2739], "fy":[-32.02556,-31.94339,-31.97259,-32.05466]}, + {"t":0.10003, "x":7.09041, "y":6.22577, "heading":-2.35617, "vx":-0.41794, "vy":-0.19581, "omega":0.00085, "ax":-4.17746, "ay":-1.95719, "alpha":0.00913, "fx":[-68.28058,-68.32053,-68.30651,-68.26656], "fy":[-32.02374,-31.93852,-31.96882,-32.05393]}, + {"t":0.13337, "x":7.07415, "y":6.21815, "heading":-2.35614, "vx":-0.55723, "vy":-0.26106, "omega":0.00116, "ax":-4.17702, "ay":-1.95699, "alpha":0.0095, "fx":[-68.27274,-68.31434,-68.29975,-68.25816], "fy":[-32.02166,-31.93295,-31.9645,-32.05309]}, + {"t":0.16671, "x":7.05325, "y":6.20836, "heading":-2.3561, "vx":-0.6965, "vy":-0.32632, "omega":0.00147, "ax":-4.1765, "ay":-1.95676, "alpha":0.00993, "fx":[-68.26369,-68.30718,-68.29194,-68.24845], "fy":[-32.01926,-31.92652,-31.95951,-32.05212]}, + {"t":0.20006, "x":7.02771, "y":6.19639, "heading":-2.35605, "vx":-0.83575, "vy":-0.39156, "omega":0.0018, "ax":-4.1759, "ay":-1.9565, "alpha":0.01044, "fx":[-68.25311,-68.29882,-68.28281,-68.23711], "fy":[-32.01645,-31.91899,-31.95368,-32.05099]}, + {"t":0.2334, "x":6.99752, "y":6.18225, "heading":-2.35599, "vx":-0.97499, "vy":-0.45679, "omega":0.00215, "ax":-4.17518, "ay":-1.95619, "alpha":0.01104, "fx":[-68.24058,-68.28891,-68.27201,-68.22368], "fy":[-32.01312,-31.91009,-31.94677,-32.04965]}, + {"t":0.26674, "x":6.96269, "y":6.16593, "heading":-2.35592, "vx":-1.1142, "vy":-0.52202, "omega":0.00252, "ax":-4.17432, "ay":-1.95581, "alpha":0.01176, "fx":[-68.22551,-68.277,-68.25901,-68.20752], "fy":[-32.00912,-31.89937,-31.93847,-32.04804]}, + {"t":0.30009, "x":6.92322, "y":6.14744, "heading":-2.35583, "vx":-1.25339, "vy":-0.58723, "omega":0.00291, "ax":-4.17327, "ay":-1.95535, "alpha":0.01265, "fx":[-68.20704,-68.2624,-68.24308,-68.18773], "fy":[-32.00422,-31.88624,-31.9283,-32.04607]}, + {"t":0.33343, "x":6.87911, "y":6.12677, "heading":-2.35574, "vx":-1.39254, "vy":-0.65243, "omega":0.00333, "ax":-4.17195, "ay":-1.95477, "alpha":0.01376, "fx":[-68.18386,-68.24408,-68.2231,-68.16289], "fy":[-31.99806,-31.86976,-31.91554,-32.04359]}, + {"t":0.36677, "x":6.83036, "y":6.10393, "heading":-2.35563, "vx":-1.53164, "vy":-0.71761, "omega":0.00379, "ax":-4.17025, "ay":-1.95402, "alpha":0.01519, "fx":[-68.15392,-68.22042,-68.19731,-68.13082], "fy":[-31.99011,-31.84848,-31.89907,-32.0404]}, + {"t":0.40011, "x":6.77697, "y":6.07892, "heading":-2.3555, "vx":-1.67069, "vy":-0.78276, "omega":0.0043, "ax":-4.16796, "ay":-1.95302, "alpha":0.01712, "fx":[-68.11375,-68.18868,-68.16273,-68.08782], "fy":[-31.97943,-31.81993,-31.87698,-32.03611]}, + {"t":0.43346, "x":6.71895, "y":6.05173, "heading":-2.35536, "vx":-1.80966, "vy":-0.84788, "omega":0.00487, "ax":-4.16474, "ay":-1.9516, "alpha":0.01984, "fx":[-68.05705,-68.14388,-68.11395,-68.02714], "fy":[-31.96437,-31.77963,-31.84583,-32.03006]}, + {"t":0.4668, "x":6.65629, "y":6.02238, "heading":-2.35519, "vx":-1.94852, "vy":-0.91295, "omega":0.00553, "ax":-4.15984, "ay":-1.94945, "alpha":0.02398, "fx":[-67.97094,-68.07586,-68.03996,-67.93508], "fy":[-31.94148,-31.71845,-31.79858,-32.02087]}, + {"t":0.50014, "x":6.58901, "y":5.99085, "heading":-2.35501, "vx":-2.08722, "vy":-0.97795, "omega":0.00633, "ax":-4.15152, "ay":-1.9458, "alpha":0.03103, "fx":[-67.82455,-67.96026,-67.91443,-67.77878], "fy":[-31.90255,-31.61452,-31.71843,-32.00523]}, + {"t":0.53348, "x":6.51711, "y":5.95717, "heading":-2.3548, "vx":-2.22565, "vy":-1.04283, "omega":0.00737, "ax":-4.13428, "ay":-1.93824, "alpha":0.0457, "fx":[-67.5205,-67.72031,-67.65469,-67.45502], "fy":[-31.82161,-31.39891,-31.55262,-31.97267]}, + {"t":0.56683, "x":6.4406, "y":5.92132, "heading":-2.35455, "vx":-2.3635, "vy":-1.10745, "omega":0.00889, "ax":-4.07711, "ay":-1.91312, "alpha":0.09507, "fx":[-66.507,-66.9221,-66.79863,-66.38426], "fy":[-31.55122,-30.68244,-31.00625,-31.86384]}, + {"t":0.60017, "x":6.35953, "y":5.88333, "heading":-2.35426, "vx":-2.49944, "vy":-1.17124, "omega":0.01206, "ax":3.61219, "ay":1.67604, "alpha":0.59454, "fx":[60.18933,57.70457,57.87735,60.43836], "fy":[26.12223,30.74728,28.89646,23.83423]}, + {"t":0.63351, "x":6.2782, "y":5.84521, "heading":-2.35385, "vx":-2.379, "vy":-1.11536, "omega":0.03188, "ax":4.10597, "ay":1.92116, "alpha":0.06392, "fx":[67.22021,66.94197,67.02907,67.30759], "fy":[31.22369,31.80561,31.5935,31.00648]}, + {"t":0.66686, "x":6.20116, "y":5.80909, "heading":-2.35279, "vx":-2.24209, "vy":-1.0513, "omega":0.03402, "ax":4.14198, "ay":1.93911, "alpha":0.03232, "fx":[67.76045,67.61947,67.66663,67.80767], "fy":[31.60625,31.90391,31.79608,31.4971]}, + {"t":0.7002, "x":6.12871, "y":5.77511, "heading":-2.35166, "vx":-2.10399, "vy":-0.98665, "omega":0.03509, "ax":4.15518, "ay":1.9457, "alpha":0.02083, "fx":[67.95919,67.86828,67.89949,67.99042], "fy":[31.74716,31.93979,31.87001,31.67683]}, + {"t":0.73354, "x":6.06086, "y":5.74329, "heading":-2.35049, "vx":-1.96544, "vy":-0.92177, "omega":0.03579, "ax":4.16203, "ay":1.94911, "alpha":0.01489, "fx":[68.06248,67.9975,68.02014,68.08513], "fy":[31.82043,31.95835,31.90829,31.77009]}, + {"t":0.76688, "x":5.99764, "y":5.71364, "heading":-2.34929, "vx":-1.82667, "vy":-0.85678, "omega":0.03628, "ax":4.16623, "ay":1.9512, "alpha":0.01125, "fx":[68.12576,68.07665,68.09394,68.14305], "fy":[31.86533,31.96968,31.93171,31.8272]}, + {"t":0.80023, "x":5.93905, "y":5.68616, "heading":-2.34808, "vx":-1.68776, "vy":-0.79172, "omega":0.03666, "ax":4.16906, "ay":1.95262, "alpha":0.0088, "fx":[68.16851,68.13011,68.14374,68.18214], "fy":[31.89566,31.97731,31.94752,31.86576]}, + {"t":0.83357, "x":5.8851, "y":5.66085, "heading":-2.34686, "vx":-1.54875, "vy":-0.72662, "omega":0.03695, "ax":4.1711, "ay":1.95363, "alpha":0.00703, "fx":[68.19932,68.16864,68.17961,68.21029], "fy":[31.91752,31.9828,31.9589,31.89356]}, + {"t":0.86691, "x":5.83577, "y":5.63771, "heading":-2.34563, "vx":-1.40967, "vy":-0.66148, "omega":0.03719, "ax":4.17263, "ay":1.9544, "alpha":0.0057, "fx":[68.22259,68.19774,68.20668,68.23153], "fy":[31.93403,31.98693,31.9675,31.91455]}, + {"t":0.90026, "x":5.79109, "y":5.61674, "heading":-2.34439, "vx":-1.27054, "vy":-0.59631, "omega":0.03738, "ax":4.17384, "ay":1.955, "alpha":0.00466, "fx":[68.24078,68.22048,68.22782,68.24813], "fy":[31.94693,31.99015,31.97422,31.93097]}, + {"t":0.9336, "x":5.75105, "y":5.59794, "heading":-2.34314, "vx":-1.13138, "vy":-0.53113, "omega":0.03753, "ax":0.05238, "ay":-0.09348, "alpha":1.49741, "fx":[6.57224,0.57467,-4.83962,1.1178], "fy":[-1.40773,4.45521,-1.65746,-7.50296]}, + {"t":0.96502, "x":5.71552, "y":5.58121, "heading":-2.34196, "vx":-1.12973, "vy":-0.53407, "omega":0.08458, "ax":0.01787, "ay":-0.03775, "alpha":1.37278, "fx":[5.52189,0.04856,-4.93597,0.53432], "fy":[-0.49098,4.86812,-0.7473,-6.09862]}, + {"t":0.99644, "x":5.68004, "y":5.56441, "heading":-2.33931, "vx":-1.12917, "vy":-0.53525, "omega":0.12772, "ax":0.00401, "ay":-0.00847, "alpha":1.25114, "fx":[4.82917,-0.17508,-4.69552,0.30395], "fy":[-0.00375,4.86175,-0.27407,-5.13758]}, + {"t":1.02786, "x":5.64456, "y":5.54758, "heading":-2.33529, "vx":-1.12904, "vy":-0.53552, "omega":0.16703, "ax":0.00059, "ay":-0.00124, "alpha":1.13272, "fx":[4.31864,-0.22756,-4.29777,0.24515], "fy":[0.12336,4.5082,-0.1641,-4.54854]}, + {"t":1.05928, "x":5.60908, "y":5.53076, "heading":-2.33004, "vx":-1.12902, "vy":-0.53556, "omega":0.20262, "ax":-0.00015, "ay":0.00032, "alpha":1.01731, "fx":[3.86339,-0.23564,-3.86766,0.23007], "fy":[0.15717,4.07396,-0.1468,-4.06359]}, + {"t":1.09071, "x":5.57361, "y":5.51393, "heading":-2.32368, "vx":-1.12903, "vy":-0.53555, "omega":0.23459, "ax":-0.00031, "ay":0.00066, "alpha":0.90457, "fx":[3.42794,-0.23321,-3.43816,0.223], "fy":[0.16923,3.63045,-0.14766,-3.60895]}, + {"t":1.12213, "x":5.53813, "y":5.4971, "heading":-2.31631, "vx":-1.12904, "vy":-0.53553, "omega":0.26301, "ax":-0.00039, "ay":0.00083, "alpha":0.79418, "fx":[3.00298,-0.22702,-3.01615,0.21457], "fy":[0.17535,3.19342,-0.14831,-3.16644]}, + {"t":1.15355, "x":5.50265, "y":5.48027, "heading":-2.30804, "vx":-1.12905, "vy":-0.5355, "omega":0.28796, "ax":-0.00041, "ay":0.00086, "alpha":0.6858, "fx":[2.58689,-0.21619,-2.60083,0.20333], "fy":[0.17516,2.76239,-0.1469,-2.73416]}, + {"t":1.18497, "x":5.46718, "y":5.46345, "heading":-2.29899, "vx":-1.12906, "vy":-0.53547, "omega":0.30951, "ax":-0.00008, "ay":0.00016, "alpha":0.57911, "fx":[2.1833,-0.19517,-2.18627,0.19313], "fy":[0.15774,2.32621,-0.15247,-2.32092]}, + {"t":1.21639, "x":5.4317, "y":5.44662, "heading":-2.28927, "vx":-1.12907, "vy":-0.53547, "omega":0.32771, "ax":0.00078, "ay":-0.00165, "alpha":0.47381, "fx":[1.79501,-0.16358,-1.76737,0.18714], "fy":[0.11628,1.87773,-0.17021,-1.93165]}, + {"t":1.24781, "x":5.39622, "y":5.4298, "heading":-2.27897, "vx":-1.12904, "vy":-0.53552, "omega":0.3426, "ax":-0.00109, "ay":0.0046, "alpha":0.37056, "fx":[1.41513,-0.3206,-1.34619,0.18036], "fy":[0.19979,1.56608,-0.04929,-1.41557]}, + {"t":1.27923, "x":5.36075, "y":5.41297, "heading":-2.26821, "vx":-1.12908, "vy":-0.53538, "omega":0.35424, "ax":1.63703, "ay":0.77583, "alpha":0.2267, "fx":[27.53843,26.66109,25.95436,26.89534], "fy":[12.72101,13.65064,12.65271,11.70923]}, + {"t":1.31066, "x":5.32608, "y":5.39653, "heading":-2.25708, "vx":-1.07764, "vy":-0.511, "omega":0.36136, "ax":1.79641, "ay":0.8519, "alpha":0.14028, "fx":[29.83813,29.31506,28.86792,29.45041], "fy":[13.95483,14.53003,13.90458,13.31815]}, + {"t":1.34208, "x":5.2931, "y":5.3809, "heading":-2.24572, "vx":-1.02119, "vy":-0.48423, "omega":0.36577, "ax":1.8016, "ay":0.85434, "alpha":0.06247, "fx":[29.66372,29.44902,29.22378,29.47448], "fy":[13.99472,14.23186,13.94242,13.69856]}, + {"t":1.3735, "x":5.26191, "y":5.3661, "heading":-2.23423, "vx":-0.96458, "vy":-0.45738, "omega":0.36773, "ax":1.80341, "ay":0.85519, "alpha":-0.0157, "fx":[29.43169,29.54313,29.51962,29.43451], "fy":[14.0026,13.90469,13.96141,14.05401]}, + {"t":1.40492, "x":5.23249, "y":5.35215, "heading":-2.22267, "vx":-0.90792, "vy":-0.43051, "omega":0.36724, "ax":1.80432, "ay":0.85561, "alpha":-0.09445, "fx":[29.18347,29.63266,29.79954,29.37326], "fy":[13.99707,13.56787,13.981,14.40455]}, + {"t":1.43634, "x":5.20485, "y":5.33905, "heading":-2.21113, "vx":-0.85122, "vy":-0.40363, "omega":0.36427, "ax":1.80488, "ay":0.85587, "alpha":-0.17389, "fx":[28.92765,29.72458,30.07323,29.29981], "fy":[13.98195,13.22527,14.00523,14.75469]}, + {"t":1.46776, "x":5.17899, "y":5.32679, "heading":-2.19969, "vx":-0.79451, "vy":-0.37674, "omega":0.35881, "ax":1.80525, "ay":0.85604, "alpha":-0.25415, "fx":[28.66697,29.82122,30.34421,29.21725], "fy":[13.95852,12.87801,14.03541,15.10624]}, + {"t":1.49918, "x":5.15492, "y":5.31537, "heading":-2.18841, "vx":-0.73779, "vy":-0.34984, "omega":0.35082, "ax":1.80552, "ay":0.85616, "alpha":-0.33534, "fx":[28.40242,29.92354,30.61423,29.12698], "fy":[13.9274,12.52633,14.07203,15.46025]}, + {"t":1.53061, "x":5.13263, "y":5.30481, "heading":-2.17739, "vx":-0.68106, "vy":-0.32294, "omega":0.34029, "ax":1.80572, "ay":0.85624, "alpha":-0.41757, "fx":[28.13428,30.03206,30.88432,29.02971], "fy":[13.88904,12.17025,14.11513,15.81742]}, + {"t":1.56203, "x":5.11212, "y":5.29508, "heading":-2.1667, "vx":-0.62432, "vy":-0.29603, "omega":0.32717, "ax":1.80588, "ay":0.85631, "alpha":-0.50095, "fx":[27.86255,30.1469,31.15523,28.92598], "fy":[13.8438,11.80952,14.16464,16.17839]}, + {"t":1.59345, "x":5.0934, "y":5.2862, "heading":-2.15642, "vx":-0.56757, "vy":-0.26913, "omega":0.31143, "ax":1.806, "ay":0.85637, "alpha":-0.58559, "fx":[27.587,30.2681,31.42761,28.81623], "fy":[13.79208,11.44382,14.22026,16.54375]}, + {"t":1.62487, "x":5.07645, "y":5.27817, "heading":-2.14663, "vx":-0.51083, "vy":-0.24222, "omega":0.29303, "ax":1.80611, "ay":0.85641, "alpha":-0.67163, "fx":[27.3073,30.39553,31.70202,28.70087], "fy":[13.73437,11.07275,14.28163,16.91406]}, + {"t":1.65629, "x":5.06129, "y":5.27098, "heading":-2.13743, "vx":-0.45408, "vy":-0.21531, "omega":0.27192, "ax":1.8062, "ay":0.85645, "alpha":-0.75916, "fx":[27.02301,30.52901,31.97904,28.58033], "fy":[13.67123,10.69588,14.34819,17.2899]}, + {"t":1.68771, "x":5.04792, "y":5.26464, "heading":-2.12888, "vx":-0.39732, "vy":-0.1884, "omega":0.24807, "ax":1.80627, "ay":0.85648, "alpha":-0.84832, "fx":[26.73359,30.66819,32.25928,28.45514], "fy":[13.60332,10.31264,14.41936,17.6719]}, + {"t":1.71913, "x":5.03633, "y":5.25914, "heading":-2.12109, "vx":-0.34057, "vy":-0.16149, "omega":0.22141, "ax":1.80633, "ay":0.85651, "alpha":-0.93923, "fx":[26.43843,30.8127,32.54334,28.32585], "fy":[13.53144,9.92242,14.49438,18.06071]}, + {"t":1.75056, "x":5.02652, "y":5.25449, "heading":-2.11413, "vx":-0.28381, "vy":-0.13457, "omega":0.1919, "ax":1.80639, "ay":0.85653, "alpha":-1.03201, "fx":[26.13683,30.96207,32.83191,28.1931], "fy":[13.45654,9.52455,14.57236,18.45699]}, + {"t":1.78198, "x":5.01849, "y":5.25068, "heading":-2.1081, "vx":-0.22705, "vy":-0.10766, "omega":0.15947, "ax":1.80643, "ay":0.85655, "alpha":-1.12681, "fx":[25.82798,31.1157,33.12571,28.05765], "fy":[13.37969,9.11826,14.6523,18.86148]}, + {"t":1.8134, "x":5.01225, "y":5.24772, "heading":-2.10309, "vx":-0.17029, "vy":-0.08075, "omega":0.12407, "ax":1.80648, "ay":0.85657, "alpha":-1.22375, "fx":[25.51101,31.2729,33.42555,27.92036], "fy":[13.30214,8.70268,14.73307,19.27497]}, + {"t":1.84482, "x":5.00779, "y":5.24561, "heading":-2.09919, "vx":-0.11353, "vy":-0.05383, "omega":0.08562, "ax":1.80651, "ay":0.85658, "alpha":-1.32299, "fx":[25.18491,31.43287,33.73227,27.78223], "fy":[13.22537,8.27686,14.81337,19.69828]}, + {"t":1.87624, "x":5.00511, "y":5.24434, "heading":-2.0965, "vx":-0.05676, "vy":-0.02692, "omega":0.04405, "ax":1.80655, "ay":0.8566, "alpha":-1.40176, "fx":[24.83872,31.57646,34.04764,27.67166], "fy":[13.20488,8.00309,14.85831,19.94854]}, + {"t":1.90766, "x":5.00422, "y":5.24392, "heading":-2.09512, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index c0bfd7ab..ec584baa 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,62 +30,62 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.24249,1.89266], + "waypoints":[0.0,1.27566,1.99673], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42298, "ay":-3.09524, "alpha":-0.01592, "fx":[55.89073,56.00508,56.02772,55.91319], "fy":[-50.67707,-50.55073,-50.52536,-50.65202]}, - {"t":0.03765, "x":1.55258, "y":7.5212, "heading":-0.93501, "vx":0.12888, "vy":-0.11654, "omega":-0.0006, "ax":3.42275, "ay":-3.0951, "alpha":-0.01626, "fx":[55.88554,56.00232,56.02544,55.90847], "fy":[-50.67633,-50.54731,-50.52139,-50.65074]}, - {"t":0.0753, "x":1.55986, "y":7.51462, "heading":-0.93503, "vx":0.25775, "vy":-0.23307, "omega":-0.00121, "ax":3.4225, "ay":-3.09494, "alpha":-0.01664, "fx":[55.87978,55.99925,56.0229,55.90322], "fy":[-50.6755,-50.54351,-50.51699,-50.64932]}, - {"t":0.11295, "x":1.57199, "y":7.50365, "heading":-0.93507, "vx":0.38661, "vy":-0.3496, "omega":-0.00184, "ax":3.42221, "ay":-3.09476, "alpha":-0.01706, "fx":[55.87333,55.99582,56.02006,55.89736], "fy":[-50.67458,-50.53928,-50.51206,-50.64773]}, - {"t":0.1506, "x":1.58897, "y":7.4883, "heading":-0.93514, "vx":0.51546, "vy":-0.46612, "omega":-0.00248, "ax":3.42189, "ay":-3.09456, "alpha":-0.01753, "fx":[55.86609,55.99196,56.01687,55.89077], "fy":[-50.67355,-50.53451,-50.50652,-50.64595]}, - {"t":0.18826, "x":1.6108, "y":7.46855, "heading":-0.93524, "vx":0.6443, "vy":-0.58264, "omega":-0.00314, "ax":3.42153, "ay":-3.09433, "alpha":-0.01807, "fx":[55.85788,55.98758,56.01326,55.88331], "fy":[-50.67238,-50.52911,-50.50024,-50.64392]}, - {"t":0.22591, "x":1.63748, "y":7.44442, "heading":-0.93535, "vx":0.77312, "vy":-0.69914, "omega":-0.00382, "ax":3.42112, "ay":-3.09407, "alpha":-0.01868, "fx":[55.84849,55.98259,56.00913,55.87478], "fy":[-50.67104,-50.52295,-50.49307,-50.6416]}, - {"t":0.26356, "x":1.66902, "y":7.41591, "heading":-0.9355, "vx":0.90193, "vy":-0.81563, "omega":-0.00452, "ax":3.42064, "ay":-3.09377, "alpha":-0.01939, "fx":[55.83768,55.97683,56.00437,55.86495], "fy":[-50.66951,-50.51585,-50.48479,-50.63893]}, - {"t":0.30121, "x":1.7054, "y":7.383, "heading":-0.93567, "vx":1.03072, "vy":-0.93212, "omega":-0.00525, "ax":3.42008, "ay":-3.09342, "alpha":-0.02021, "fx":[55.82506,55.97011,55.99883,55.85348], "fy":[-50.66771,-50.50756,-50.47514,-50.63581]}, - {"t":0.33886, "x":1.74663, "y":7.34572, "heading":-0.93587, "vx":1.15949, "vy":-1.04859, "omega":-0.00601, "ax":3.41943, "ay":-3.09301, "alpha":-0.02118, "fx":[55.81016,55.96217,55.99228,55.83994], "fy":[-50.6656,-50.49778,-50.46374,-50.63212]}, - {"t":0.37651, "x":1.79271, "y":7.30404, "heading":-0.93609, "vx":1.28824, "vy":-1.16504, "omega":-0.00681, "ax":3.41864, "ay":-3.09251, "alpha":-0.02235, "fx":[55.7923,55.95266,55.98442,55.8237], "fy":[-50.66306,-50.48606,-50.45007,-50.6277]}, - {"t":0.41416, "x":1.84364, "y":7.25799, "heading":-0.93635, "vx":1.41695, "vy":-1.28148, "omega":-0.00765, "ax":3.41768, "ay":-3.09191, "alpha":-0.02378, "fx":[55.7705,55.94105,55.97484,55.80387], "fy":[-50.65997,-50.47176,-50.43337,-50.62229]}, - {"t":0.45181, "x":1.89941, "y":7.20755, "heading":-0.93664, "vx":1.54563, "vy":-1.39789, "omega":-0.00855, "ax":3.41648, "ay":-3.09116, "alpha":-0.02556, "fx":[55.74327,55.92656,55.96287,55.77911], "fy":[-50.6561,-50.4539,-50.41253,-50.61555]}, - {"t":0.48946, "x":1.96003, "y":7.15272, "heading":-0.93696, "vx":1.67427, "vy":-1.51428, "omega":-0.00951, "ax":3.41494, "ay":-3.09019, "alpha":-0.02785, "fx":[55.70834,55.90797,55.94752,55.74732], "fy":[-50.65114,-50.43099,-50.38576,-50.60688]}, - {"t":0.52712, "x":2.02549, "y":7.09352, "heading":-0.93732, "vx":1.80284, "vy":-1.63063, "omega":-0.01056, "ax":3.41289, "ay":-3.0889, "alpha":-0.0309, "fx":[55.66188,55.88326,55.92708,55.70501], "fy":[-50.64455,-50.40053,-50.35013,-50.59535]}, - {"t":0.56477, "x":2.09579, "y":7.02993, "heading":-0.93772, "vx":1.93134, "vy":-1.74693, "omega":-0.01172, "ax":3.41004, "ay":-3.0871, "alpha":-0.03516, "fx":[55.59706,55.8488,55.89856,55.64592], "fy":[-50.63533,-50.35803,-50.30039,-50.57924]}, - {"t":0.60242, "x":2.17092, "y":6.96197, "heading":-0.93816, "vx":2.05973, "vy":-1.86316, "omega":-0.01305, "ax":3.40577, "ay":-3.08442, "alpha":-0.04154, "fx":[55.50032,55.7974,55.85596,55.55763], "fy":[-50.62156,-50.29464,-50.22608,-50.55515]}, - {"t":0.64007, "x":2.25088, "y":6.88964, "heading":-0.93865, "vx":2.18796, "vy":-1.97929, "omega":-0.01461, "ax":3.39871, "ay":-3.07997, "alpha":-0.0521, "fx":[55.34046,55.71254,55.78543,55.41138], "fy":[-50.5987,-50.18994,-50.10305,-50.51518]}, - {"t":0.67772, "x":2.33567, "y":6.81293, "heading":-0.9392, "vx":2.31593, "vy":-2.09526, "omega":-0.01657, "ax":3.3848, "ay":-3.0712, "alpha":-0.07301, "fx":[55.02581,55.54567,55.64606,55.12236], "fy":[-50.5533,-49.98407,-49.8601,-50.43589]}, - {"t":0.71537, "x":2.42527, "y":6.73186, "heading":-0.93982, "vx":2.44337, "vy":-2.21089, "omega":-0.01932, "ax":3.34461, "ay":-3.0458, "alpha":-0.13385, "fx":[54.12159,55.06675,55.2407,54.28269], "fy":[-50.41915,-49.3939,-49.156,-50.20279]}, - {"t":0.75302, "x":2.51963, "y":6.64646, "heading":-0.94055, "vx":2.5693, "vy":-2.32557, "omega":-0.02436, "ax":2.00182, "ay":-2.12377, "alpha":-2.4523, "fx":[28.16582,41.09782,38.75506,22.88519], "fy":[-43.783,-32.95583,-24.2922,-37.84709]}, - {"t":0.79067, "x":2.61779, "y":6.5574, "heading":-0.94147, "vx":2.64467, "vy":-2.40553, "omega":-0.11669, "ax":-3.361, "ay":3.01304, "alpha":-0.13185, "fx":[-55.49683,-54.56436,-54.40098,-55.32147], "fy":[48.62263,49.65633,49.88191,48.86911]}, - {"t":0.82832, "x":2.71498, "y":6.46896, "heading":-0.94586, "vx":2.51812, "vy":-2.29209, "omega":-0.12166, "ax":-3.39492, "ay":3.05633, "alpha":-0.06069, "fx":[-55.75946,-55.32726,-55.2429,-55.67252], "fy":[49.67292,50.15149,50.255,49.78096]}, - {"t":0.86597, "x":2.80739, "y":6.38483, "heading":-0.95044, "vx":2.3903, "vy":-2.17701, "omega":-0.12394, "ax":-3.4059, "ay":3.07038, "alpha":-0.03754, "fx":[-55.84157,-55.57385,-55.51899,-55.78573], "fy":[50.01389,50.31023,50.3751,50.08052]}, - {"t":0.90363, "x":2.89497, "y":6.30504, "heading":-0.95511, "vx":2.26206, "vy":-2.06141, "omega":-0.12536, "ax":-3.41133, "ay":3.07735, "alpha":-0.026, "fx":[-55.88133,-55.69583,-55.65647,-55.8415], "fy":[50.18304,50.38832,50.43405,50.22961]}, - {"t":0.94128, "x":2.97772, "y":6.22961, "heading":-0.95983, "vx":2.13362, "vy":-1.94555, "omega":-0.12634, "ax":-3.41457, "ay":3.0815, "alpha":-0.01907, "fx":[-55.90465,-55.76862,-55.73889,-55.87467], "fy":[50.28424,50.43475,50.46892,50.31886]}, - {"t":0.97893, "x":3.05563, "y":6.15854, "heading":-0.96458, "vx":2.00506, "vy":-1.82952, "omega":-0.12705, "ax":-3.41672, "ay":3.08426, "alpha":-0.01444, "fx":[-55.9199,-55.81697,-55.79387,-55.89666], "fy":[50.35165,50.46553,50.4919,50.37828]}, - {"t":1.01658, "x":3.12871, "y":6.09184, "heading":-0.96937, "vx":1.87642, "vy":-1.7134, "omega":-0.1276, "ax":-3.41825, "ay":3.08623, "alpha":-0.01112, "fx":[-55.93062,-55.85141,-55.83318,-55.91231], "fy":[50.39981,50.48745,50.50815,50.42067]}, - {"t":1.05423, "x":3.19693, "y":6.02952, "heading":-0.97417, "vx":1.74772, "vy":-1.5972, "omega":-0.12802, "ax":-3.4194, "ay":3.08771, "alpha":-0.00862, "fx":[-55.93854,-55.87717,-55.86271,-55.92404], "fy":[50.43596,50.50385,50.52022,50.45242]}, - {"t":1.09188, "x":3.26031, "y":5.97157, "heading":-0.97899, "vx":1.61897, "vy":-1.48094, "omega":-0.12834, "ax":-3.42029, "ay":3.08885, "alpha":-0.00667, "fx":[-55.94462,-55.89716,-55.88572,-55.93315], "fy":[50.46411,50.51661,50.52952,50.47707]}, - {"t":1.12953, "x":3.31884, "y":5.918, "heading":-0.98382, "vx":1.4902, "vy":-1.36464, "omega":-0.12859, "ax":-3.421, "ay":3.08977, "alpha":-0.00511, "fx":[-55.94942,-55.91311,-55.90416,-55.94046], "fy":[50.48666,50.52683,50.5369,50.49676]}, - {"t":1.16718, "x":3.37253, "y":5.86881, "heading":-0.98867, "vx":1.36139, "vy":-1.24831, "omega":-0.12878, "ax":-3.42158, "ay":3.09051, "alpha":-0.00383, "fx":[-55.95329,-55.92612,-55.91928,-55.94644], "fy":[50.50514,50.5352,50.54288,50.51284]}, - {"t":1.20483, "x":3.42136, "y":5.824, "heading":-0.99351, "vx":1.23257, "vy":-1.13195, "omega":-0.12893, "ax":-3.42206, "ay":3.09114, "alpha":-0.00276, "fx":[-55.95648,-55.93693,-55.9319,-55.95145], "fy":[50.52056,50.54219,50.54783,50.52621]}, - {"t":1.24249, "x":3.46534, "y":5.78357, "heading":-0.99837, "vx":1.10372, "vy":-1.01556, "omega":-0.12903, "ax":-1.79347, "ay":-1.82922, "alpha":-0.51189, "fx":[-29.18831,-27.19212,-29.49743,-31.40171], "fy":[-31.68933,-30.93732,-28.0663,-28.92436]}, - {"t":1.27499, "x":3.50027, "y":5.74959, "heading":-1.00256, "vx":1.04542, "vy":-1.07503, "omega":-0.14567, "ax":-0.45558, "ay":-0.43702, "alpha":-0.65351, "fx":[-7.98189,-4.93786,-6.923,-9.94863], "fy":[-9.61063,-7.72567,-4.65626,-6.58501]}, - {"t":1.3075, "x":3.53402, "y":5.71441, "heading":-1.0073, "vx":1.03061, "vy":-1.08924, "omega":-0.16692, "ax":-0.08038, "ay":-0.07587, "alpha":-0.38805, "fx":[-1.66253,0.1674,-0.9661,-2.79498], "fy":[-2.71349,-1.55952,0.23414,-0.92254]}, - {"t":1.34001, "x":3.56748, "y":5.67896, "heading":-1.01273, "vx":1.028, "vy":-1.0917, "omega":-0.17953, "ax":-0.01382, "ay":-0.01301, "alpha":-0.09976, "fx":[-0.31774,0.15433,-0.13418,-0.60624], "fy":[-0.59109,-0.29642,0.16574,-0.12897]}, - {"t":1.37252, "x":3.60089, "y":5.64347, "heading":-1.01856, "vx":1.02755, "vy":-1.09213, "omega":-0.18277, "ax":-0.00237, "ay":-0.00223, "alpha":0.18863, "fx":[0.13897,-0.7569,-0.2165,0.67938], "fy":[0.67806,0.12602,-0.75098,-0.19897]}, - {"t":1.40503, "x":3.63429, "y":5.60796, "heading":-1.0245, "vx":1.02747, "vy":-1.0922, "omega":-0.17664, "ax":-0.00041, "ay":-0.00038, "alpha":0.47768, "fx":[0.45419,-1.82277,-0.46749,1.80948], "fy":[1.80049,0.41604,-1.81298,-0.42856]}, - {"t":1.43754, "x":3.66769, "y":5.57246, "heading":-1.03025, "vx":1.02746, "vy":-1.09221, "omega":-0.16111, "ax":-0.00007, "ay":-0.00007, "alpha":0.7684, "fx":[0.75685,-2.91865,-0.75913,2.91637], "fy":[2.90097,0.69501,-2.90311,-0.69716]}, - {"t":1.47004, "x":3.70109, "y":5.53695, "heading":-1.03548, "vx":1.02745, "vy":-1.09221, "omega":-0.13613, "ax":-0.00001, "ay":-0.00001, "alpha":1.06179, "fx":[1.0682,-4.02656,-1.06859,4.02616], "fy":[4.00435,0.98277,-4.00472,-0.98315]}, - {"t":1.50255, "x":3.7345, "y":5.50144, "heading":-1.03991, "vx":1.02745, "vy":-1.09221, "omega":-0.10162, "ax":-0.00001, "ay":0.0, "alpha":1.35879, "fx":[1.38978,-5.14716,-1.39006,5.14688], "fy":[5.11867,1.28077,-5.11855,-1.28065]}, - {"t":1.53506, "x":3.7679, "y":5.46594, "heading":-1.04321, "vx":1.02745, "vy":-1.09221, "omega":-0.05745, "ax":-0.07403, "ay":0.07869, "alpha":1.65856, "fx":[0.49807,-7.47444,-2.94322,5.07885], "fy":[7.52547,2.87082,-4.9602,-0.29016]}, - {"t":1.56757, "x":3.80126, "y":5.43047, "heading":-1.04508, "vx":1.02505, "vy":-1.08966, "omega":-0.00353, "ax":-3.12096, "ay":3.31767, "alpha":0.0287, "fx":[-50.88787,-51.10815,-51.15581,-50.93489], "fy":[54.36555,54.15988,54.10912,54.31575]}, - {"t":1.60008, "x":3.83293, "y":5.3968, "heading":-1.04519, "vx":0.92359, "vy":-0.9818, "omega":-0.00259, "ax":-3.14452, "ay":3.34272, "alpha":0.01562, "fx":[-51.33317,-51.4538,-51.48069,-51.35987], "fy":[54.71694,54.60387,54.57703,54.69039]}, - {"t":1.63259, "x":3.86129, "y":5.36665, "heading":-1.04528, "vx":0.82137, "vy":-0.87314, "omega":-0.00209, "ax":-3.15179, "ay":3.35045, "alpha":0.0116, "fx":[-51.47083,-51.56058,-51.58077,-51.49092], "fy":[54.82539,54.74117,54.72142,54.8058]}, - {"t":1.6651, "x":3.88633, "y":5.34004, "heading":-1.04535, "vx":0.71891, "vy":-0.76422, "omega":-0.00171, "ax":-3.15532, "ay":3.3542, "alpha":0.00965, "fx":[-51.53775,-51.61247,-51.62936,-51.55457], "fy":[54.87808,54.80792,54.79156,54.86184]}, - {"t":1.6976, "x":3.90803, "y":5.31697, "heading":-1.0454, "vx":0.61633, "vy":-0.65518, "omega":-0.0014, "ax":-3.15741, "ay":3.35642, "alpha":0.0085, "fx":[-51.57731,-51.64314,-51.65807,-51.59218], "fy":[54.90922,54.84738,54.83301,54.89494]}, - {"t":1.73011, "x":3.9264, "y":5.29744, "heading":-1.04545, "vx":0.51369, "vy":-0.54607, "omega":-0.00112, "ax":-3.15879, "ay":3.35789, "alpha":0.00774, "fx":[-51.60344,-51.6634,-51.67701,-51.61701], "fy":[54.92977,54.87344,54.86038,54.91679]}, - {"t":1.76262, "x":3.94143, "y":5.28146, "heading":-1.04548, "vx":0.411, "vy":-0.43691, "omega":-0.00087, "ax":-3.15977, "ay":3.35893, "alpha":0.0072, "fx":[-51.62198,-51.67777,-51.69046,-51.63462], "fy":[54.94436,54.89193,54.8798,54.93229]}, - {"t":1.79513, "x":3.95312, "y":5.26904, "heading":-1.04551, "vx":0.30828, "vy":-0.32771, "omega":-0.00063, "ax":-3.1605, "ay":3.35971, "alpha":0.00679, "fx":[-51.63581,-51.6885,-51.70049,-51.64777], "fy":[54.95525,54.90574,54.89429,54.94386]}, - {"t":1.82764, "x":3.96147, "y":5.26016, "heading":-1.04553, "vx":0.20554, "vy":-0.21849, "omega":-0.00041, "ax":-3.16107, "ay":3.36031, "alpha":0.00648, "fx":[-51.64654,-51.69681,-51.70826,-51.65796], "fy":[54.96369,54.91644,54.90552,54.95282]}, - {"t":1.86015, "x":3.96649, "y":5.25483, "heading":-1.04555, "vx":0.10278, "vy":-0.10925, "omega":-0.0002, "ax":-3.16152, "ay":3.36079, "alpha":0.00623, "fx":[-51.6551,-51.70344,-51.71446,-51.66609], "fy":[54.97042,54.92497,54.91448,54.95997]}, - {"t":1.89266, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42195, "ay":-3.09664, "alpha":-0.01591, "fx":[55.87399,55.98829,56.01089,55.8964], "fy":[-50.69989,-50.57369,-50.54841,-50.67492]}, + {"t":0.03866, "x":1.55271, "y":7.52108, "heading":-0.93501, "vx":0.13228, "vy":-0.1197, "omega":-0.00061, "ax":3.4217, "ay":-3.09653, "alpha":-0.01631, "fx":[55.86814,55.98534,56.0085,55.89111], "fy":[-50.69993,-50.57055,-50.54461,-50.67433]}, + {"t":0.07731, "x":1.56038, "y":7.51414, "heading":-0.93503, "vx":0.26455, "vy":-0.2394, "omega":-0.00125, "ax":3.42142, "ay":-3.0964, "alpha":-0.01676, "fx":[55.86162,55.98205,56.00584,55.88521], "fy":[-50.69998,-50.56704,-50.54038,-50.67367]}, + {"t":0.11597, "x":1.57316, "y":7.50258, "heading":-0.93508, "vx":0.39681, "vy":-0.3591, "omega":-0.00189, "ax":3.4211, "ay":-3.09626, "alpha":-0.01727, "fx":[55.85431,55.97836,56.00286,55.8786], "fy":[-50.70004,-50.56311,-50.53563,-50.67293]}, + {"t":0.15462, "x":1.59106, "y":7.48638, "heading":-0.93515, "vx":0.52906, "vy":-0.47879, "omega":-0.00256, "ax":3.42075, "ay":-3.0961, "alpha":-0.01784, "fx":[55.84605,55.97419,55.9995,55.87113], "fy":[-50.70011,-50.55868,-50.53027,-50.6721]}, + {"t":0.19328, "x":1.61406, "y":7.46556, "heading":-0.93525, "vx":0.66129, "vy":-0.59847, "omega":-0.00325, "ax":3.42034, "ay":-3.09591, "alpha":-0.01848, "fx":[55.83666,55.96945,55.99568,55.86264], "fy":[-50.70018,-50.55364,-50.52418,-50.67114]}, + {"t":0.23194, "x":1.64218, "y":7.44011, "heading":-0.93538, "vx":0.79351, "vy":-0.71815, "omega":-0.00396, "ax":3.41988, "ay":-3.0957, "alpha":-0.01923, "fx":[55.82588,55.964,55.99129,55.8529], "fy":[-50.70027,-50.54786,-50.51717,-50.67004]}, + {"t":0.27059, "x":1.67541, "y":7.41004, "heading":-0.93553, "vx":0.92571, "vy":-0.83782, "omega":-0.00471, "ax":3.41934, "ay":-3.09546, "alpha":-0.02009, "fx":[55.81338,55.95769,55.98621,55.8416], "fy":[-50.70037,-50.54116,-50.50906,-50.66877]}, + {"t":0.30925, "x":1.71375, "y":7.37534, "heading":-0.93571, "vx":1.05789, "vy":-0.95748, "omega":-0.00548, "ax":3.41871, "ay":-3.09517, "alpha":-0.02111, "fx":[55.79872,55.95028,55.98024,55.82835], "fy":[-50.70049,-50.53331,-50.49953,-50.66727]}, + {"t":0.34791, "x":1.7572, "y":7.33601, "heading":-0.93592, "vx":1.19004, "vy":-1.07712, "omega":-0.0063, "ax":3.41796, "ay":-3.09483, "alpha":-0.02231, "fx":[55.78127,55.94147,55.97314,55.81257], "fy":[-50.70064,-50.52396,-50.48819,-50.66549]}, + {"t":0.38656, "x":1.80575, "y":7.29206, "heading":-0.93617, "vx":1.32216, "vy":-1.19676, "omega":-0.00716, "ax":3.41705, "ay":-3.09442, "alpha":-0.02377, "fx":[55.76016,55.93082,55.96456,55.79349], "fy":[-50.70081,-50.51266,-50.47446,-50.66333]}, + {"t":0.42522, "x":1.85942, "y":7.24349, "heading":-0.93644, "vx":1.45426, "vy":-1.31638, "omega":-0.00808, "ax":3.41593, "ay":-3.09391, "alpha":-0.02558, "fx":[55.73411,55.91767,55.95397,55.76993], "fy":[-50.70103,-50.49871,-50.45752,-50.66066]}, + {"t":0.46387, "x":1.91819, "y":7.19029, "heading":-0.93676, "vx":1.5863, "vy":-1.43598, "omega":-0.00907, "ax":3.41451, "ay":-3.09326, "alpha":-0.02786, "fx":[55.70115,55.90104,55.94056,55.74011], "fy":[-50.7013,-50.48107,-50.43608,-50.65727]}, + {"t":0.50253, "x":1.98206, "y":7.13247, "heading":-0.93711, "vx":1.71829, "vy":-1.55555, "omega":-0.01015, "ax":3.41266, "ay":-3.09242, "alpha":-0.03084, "fx":[55.65813,55.87934,55.92306,55.70116], "fy":[-50.70165,-50.45806,-50.40806,-50.65285]}, + {"t":0.54119, "x":2.05103, "y":7.07003, "heading":-0.9375, "vx":1.85021, "vy":-1.67509, "omega":-0.01134, "ax":3.41013, "ay":-3.09127, "alpha":-0.03491, "fx":[55.59958,55.84983,55.89925,55.64811], "fy":[-50.70211,-50.42675,-50.36992,-50.64681]}, + {"t":0.57984, "x":2.1251, "y":7.00297, "heading":-0.93794, "vx":1.98204, "vy":-1.79459, "omega":-0.01269, "ax":3.4065, "ay":-3.08962, "alpha":-0.04077, "fx":[55.5153,55.80737,55.86494,55.57166], "fy":[-50.70274,-50.38169,-50.31495,-50.63808]}, + {"t":0.6185, "x":2.20426, "y":6.93129, "heading":-0.93843, "vx":2.11372, "vy":-1.91402, "omega":-0.01427, "ax":3.40082, "ay":-3.08703, "alpha":-0.04995, "fx":[55.38354,55.74106,55.81124,55.4519], "fy":[-50.70365,-50.31129,-50.22888,-50.62435]}, + {"t":0.65716, "x":2.28851, "y":6.85499, "heading":-0.93898, "vx":2.24518, "vy":-2.03335, "omega":-0.0162, "ax":3.39067, "ay":-3.0824, "alpha":-0.0664, "fx":[55.14851,55.62294,55.71521,55.23757], "fy":[-50.70499,-50.18582,-50.07494,-50.59957]}, + {"t":0.69581, "x":2.37784, "y":6.77409, "heading":-0.93961, "vx":2.37625, "vy":-2.15251, "omega":-0.01876, "ax":3.3674, "ay":-3.07173, "alpha":-0.1043, "fx":[54.61085,55.35329,55.49416,54.74381], "fy":[-50.70645,-49.89932,-49.72087,-50.54129]}, + {"t":0.73447, "x":2.47221, "y":6.68858, "heading":-0.94033, "vx":2.50642, "vy":-2.27125, "omega":-0.02279, "ax":3.25888, "ay":-3.02124, "alpha":-0.28362, "fx":[52.13558,54.11858,54.4465,52.40523], "fy":[-50.68105,-48.58911,-48.05582,-50.2401]}, + {"t":0.77312, "x":2.57153, "y":6.59853, "heading":-0.94121, "vx":2.6324, "vy":-2.38804, "omega":-0.03376, "ax":-3.31806, "ay":2.91526, "alpha":-0.31165, "fx":[-55.50223,-53.34113,-53.01702,-55.11565], "fy":[46.13397,48.56598,49.12806,46.80776]}, + {"t":0.81178, "x":2.67081, "y":6.50839, "heading":-0.94252, "vx":2.50414, "vy":-2.27535, "omega":-0.04581, "ax":-3.39222, "ay":3.03976, "alpha":-0.09365, "fx":[-55.85326,-55.1882,-55.06256,-55.72154], "fy":[49.24229,49.98196,50.14076,49.41186]}, + {"t":0.85044, "x":2.76508, "y":6.42271, "heading":-0.94429, "vx":2.37301, "vy":-2.15784, "omega":-0.04943, "ax":-3.4062, "ay":3.06361, "alpha":-0.05249, "fx":[-55.90952,-55.53524,-55.4612,-55.83355], "fy":[49.83143,50.24669,50.33524,49.92339]}, + {"t":0.88909, "x":2.85426, "y":6.34158, "heading":-0.9462, "vx":2.24133, "vy":-2.03941, "omega":-0.05145, "ax":-3.41211, "ay":3.07373, "alpha":-0.03505, "fx":[-55.93219,-55.68186,-55.6311,-55.88057], "fy":[50.08086,50.35831,50.41763,50.14171]}, + {"t":0.92775, "x":2.93836, "y":6.26504, "heading":-0.94819, "vx":2.10944, "vy":-1.92059, "omega":-0.05281, "ax":-3.41537, "ay":3.07933, "alpha":-0.02541, "fx":[-55.94435,-55.76279,-55.7253,-55.90641], "fy":[50.2187,50.41983,50.46309,50.26277]}, + {"t":0.96641, "x":3.01735, "y":6.1931, "heading":-0.95023, "vx":1.97741, "vy":-1.80156, "omega":-0.05379, "ax":-3.41743, "ay":3.08288, "alpha":-0.01928, "fx":[-55.95191,-55.81408,-55.78519,-55.92276], "fy":[50.30619,50.4588,50.49187,50.33973]}, + {"t":1.00506, "x":3.09123, "y":6.12576, "heading":-0.95231, "vx":1.84531, "vy":-1.68239, "omega":-0.05454, "ax":-3.41886, "ay":3.08533, "alpha":-0.01504, "fx":[-55.95703,-55.8495,-55.82665,-55.93403], "fy":[50.36666,50.4857,50.51172,50.39296]}, + {"t":1.04372, "x":3.16001, "y":6.06303, "heading":-0.95442, "vx":1.71315, "vy":-1.56312, "omega":-0.05512, "ax":-3.4199, "ay":3.08713, "alpha":-0.01194, "fx":[-55.96073,-55.87541,-55.85706,-55.94227], "fy":[50.41097,50.50539,50.52621,50.43196]}, + {"t":1.08237, "x":3.22368, "y":6.00492, "heading":-0.95655, "vx":1.58095, "vy":-1.44378, "omega":-0.05558, "ax":-3.4207, "ay":3.0885, "alpha":-0.00956, "fx":[-55.96351,-55.89519,-55.88032,-55.94857], "fy":[50.44483,50.52044,50.53726,50.46177]}, + {"t":1.12103, "x":3.28224, "y":5.95141, "heading":-0.9587, "vx":1.44871, "vy":-1.32439, "omega":-0.05595, "ax":-3.42133, "ay":3.08958, "alpha":-0.00768, "fx":[-55.96568,-55.91078,-55.89869,-55.95354], "fy":[50.47157,50.53231,50.54595,50.48528]}, + {"t":1.15969, "x":3.33568, "y":5.90253, "heading":-0.96086, "vx":1.31646, "vy":-1.20496, "omega":-0.05625, "ax":-3.42183, "ay":3.09046, "alpha":-0.00616, "fx":[-55.96741,-55.92338,-55.91357,-55.95757], "fy":[50.49321,50.54192,50.55296,50.5043]}, + {"t":1.19834, "x":3.38402, "y":5.85826, "heading":-0.96303, "vx":1.18418, "vy":-1.0855, "omega":-0.05648, "ax":-3.42225, "ay":3.09118, "alpha":-0.00491, "fx":[-55.96881,-55.93377,-55.92587,-55.9609], "fy":[50.51109,50.54987,50.55874,50.51999]}, + {"t":1.237, "x":3.42724, "y":5.8186, "heading":-0.96522, "vx":1.05189, "vy":-0.966, "omega":-0.05667, "ax":-3.42261, "ay":3.09179, "alpha":-0.00385, "fx":[-55.96998,-55.94248,-55.93621,-55.96371], "fy":[50.52612,50.55655,50.56358,50.53316]}, + {"t":1.27566, "x":3.46534, "y":5.78357, "heading":-0.96741, "vx":0.91959, "vy":-0.84649, "omega":-0.05682, "ax":-1.5087, "ay":-1.53167, "alpha":-1.13942, "fx":[-24.65478,-20.00114,-24.84409,-29.15736], "fy":[-29.11667,-26.86294,-20.74079,-23.43943]}, + {"t":1.31171, "x":3.49751, "y":5.75206, "heading":-0.96946, "vx":0.86519, "vy":-0.90171, "omega":-0.0979, "ax":-0.20145, "ay":-0.19175, "alpha":-1.43419, "fx":[-4.36719,2.23451,-2.2412,-8.79968], "fy":[-8.60231,-4.14368,2.37787,-2.17111]}, + {"t":1.34776, "x":3.52858, "y":5.71942, "heading":-0.97299, "vx":0.85793, "vy":-0.90862, "omega":-0.14961, "ax":-0.02066, "ay":-0.01949, "alpha":-1.12338, "fx":[-1.20182,3.9795,0.52494,-4.65371], "fy":[-4.61642,-1.09214,3.98192,0.45197]}, + {"t":1.38382, "x":3.5595, "y":5.68665, "heading":-0.97838, "vx":0.85718, "vy":-0.90932, "omega":-0.19012, "ax":-0.0021, "ay":-0.00198, "alpha":-0.80656, "fx":[-0.67094,3.06192,0.60235,-3.13037], "fy":[-3.11552,-0.60327,3.05108,0.53854]}, + {"t":1.41987, "x":3.5904, "y":5.65386, "heading":-0.98523, "vx":0.85711, "vy":-0.9094, "omega":-0.21919, "ax":-0.00021, "ay":-0.0002, "alpha":-0.49278, "fx":[-0.40534,1.88575,0.39841,-1.89268], "fy":[-1.88435,-0.36503,1.87783,0.35849]}, + {"t":1.45593, "x":3.6213, "y":5.62108, "heading":-0.99314, "vx":0.8571, "vy":-0.9094, "omega":-0.23696, "ax":-0.00002, "ay":-0.00002, "alpha":-0.18094, "fx":[-0.15337,0.69228,0.15267,-0.69297], "fy":[-0.68986,-0.13864,0.6892,0.13798]}, + {"t":1.49198, "x":3.6522, "y":5.58829, "heading":-1.00168, "vx":0.8571, "vy":-0.9094, "omega":-0.24349, "ax":0.0, "ay":0.0, "alpha":0.13018, "fx":[0.11428,-0.49747,-0.11436,0.4974], "fy":[0.49508,0.10373,-0.49515,-0.10379]}, + {"t":1.52803, "x":3.6831, "y":5.5555, "heading":-1.01046, "vx":0.8571, "vy":-0.9094, "omega":-0.23879, "ax":0.0, "ay":0.0, "alpha":0.44181, "fx":[0.40273,-1.6851,-0.40273,1.68509], "fy":[1.67691,0.36696,-1.67691,-0.36697]}, + {"t":1.56409, "x":3.71401, "y":5.52271, "heading":-1.01907, "vx":0.8571, "vy":-0.9094, "omega":-0.22286, "ax":0.0, "ay":0.0, "alpha":0.75519, "fx":[0.71304,-2.87483,-0.71304,2.87482], "fy":[2.8603,0.65202,-2.8603,-0.65203]}, + {"t":1.60014, "x":3.74491, "y":5.48993, "heading":-1.0271, "vx":0.8571, "vy":-0.9094, "omega":-0.19563, "ax":0.0, "ay":0.0, "alpha":1.07151, "fx":[1.04427,-4.07141,-1.04428,4.07141], "fy":[4.05011,0.95788,-4.05011,-0.95788]}, + {"t":1.6362, "x":3.77581, "y":5.45714, "heading":-1.03416, "vx":0.8571, "vy":-0.9094, "omega":-0.157, "ax":0.0, "ay":0.0, "alpha":1.39194, "fx":[1.39361,-5.28005,-1.39365,5.28001], "fy":[5.25155,1.28158,-5.25158,-1.28161]}, + {"t":1.67225, "x":3.80671, "y":5.42435, "heading":-1.03982, "vx":0.8571, "vy":-0.9094, "omega":-0.10682, "ax":-0.00009, "ay":0.00007, "alpha":1.71758, "fx":[1.7548,-6.50772,-1.75785,6.50472], "fy":[6.47153,1.6195,-6.46911,-1.61706]}, + {"t":1.7083, "x":3.83761, "y":5.39156, "heading":-1.04367, "vx":0.8571, "vy":-0.9094, "omega":-0.04489, "ax":-1.6595, "ay":1.76081, "alpha":1.17929, "fx":[-24.82136,-31.20566,-29.70566,-22.78626], "fy":[33.35883,28.76988,24.01327,29.00162]}, + {"t":1.74436, "x":3.86744, "y":5.35992, "heading":-1.04529, "vx":0.79726, "vy":-0.84592, "omega":-0.00237, "ax":-3.1427, "ay":3.33448, "alpha":0.02031, "fx":[-51.28155,-51.43802,-51.47288,-51.31608], "fy":[54.60327,54.45646,54.42113,54.56843]}, + {"t":1.78041, "x":3.89414, "y":5.33159, "heading":-1.04537, "vx":0.68396, "vy":-0.7257, "omega":-0.00164, "ax":-3.15561, "ay":3.34818, "alpha":0.01168, "fx":[-51.5329,-51.62314,-51.64356,-51.55321], "fy":[54.78867,54.70382,54.68385,54.76886]}, + {"t":1.81646, "x":3.91675, "y":5.3076, "heading":-1.04543, "vx":0.57019, "vy":-0.60498, "omega":-0.00122, "ax":-3.15995, "ay":3.35279, "alpha":0.00877, "fx":[-51.61756,-51.68546,-51.70091,-51.63295], "fy":[54.85105,54.78716,54.77223,54.83622]}, + {"t":1.85252, "x":3.93525, "y":5.28797, "heading":-1.04548, "vx":0.45626, "vy":-0.4841, "omega":-0.0009, "ax":-3.16213, "ay":3.3551, "alpha":0.00732, "fx":[-51.66005,-51.71673,-51.72966,-51.67294], "fy":[54.88234,54.82898,54.81656,54.86999]}, + {"t":1.88857, "x":3.94965, "y":5.27269, "heading":-1.04551, "vx":0.34225, "vy":-0.36313, "omega":-0.00064, "ax":-3.16344, "ay":3.35649, "alpha":0.00645, "fx":[-51.68559,-51.73553,-51.74694,-51.69697], "fy":[54.90115,54.85413,54.84321,54.89028]}, + {"t":1.92463, "x":3.95993, "y":5.26178, "heading":-1.04553, "vx":0.22819, "vy":-0.24212, "omega":-0.00041, "ax":-3.16432, "ay":3.35742, "alpha":0.00587, "fx":[-51.70264,-51.74808,-51.75847,-51.71301], "fy":[54.9137,54.87091,54.86099,54.90382]}, + {"t":1.96068, "x":3.9661, "y":5.25524, "heading":-1.04555, "vx":0.11411, "vy":-0.12107, "omega":-0.0002, "ax":-3.16494, "ay":3.35808, "alpha":0.00545, "fx":[-51.71482,-51.75704,-51.76671,-51.72447], "fy":[54.92267,54.88291,54.8737,54.91349]}, + {"t":1.99673, "x":3.96816, "y":5.25305, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index 325ea3c0..ee67414c 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,61 +30,63 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.20207,1.89844], + "waypoints":[0.0,1.235,2.011], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0897, "ay":-3.42807, "alpha":-0.0103, "fx":[50.46517,50.5483,50.55626,50.47302], "fy":[-56.08355,-56.00865,-56.00131,-56.07632]}, - {"t":0.03756, "x":1.55233, "y":7.52098, "heading":-0.93501, "vx":0.11606, "vy":-0.12877, "omega":-0.00039, "ax":3.08951, "ay":-3.42789, "alpha":-0.01052, "fx":[50.4611,50.54593,50.55404,50.46911], "fy":[-56.08142,-56.00499,-55.99749,-56.07404]}, - {"t":0.07513, "x":1.55887, "y":7.51372, "heading":-0.93502, "vx":0.23212, "vy":-0.25754, "omega":-0.00078, "ax":3.08929, "ay":-3.42769, "alpha":-0.01075, "fx":[50.45658,50.54329,50.55159,50.46476], "fy":[-56.07905,-56.00093,-55.99326,-56.07151]}, - {"t":0.11269, "x":1.56977, "y":7.50163, "heading":-0.93505, "vx":0.34817, "vy":-0.3863, "omega":-0.00119, "ax":3.08905, "ay":-3.42746, "alpha":-0.01101, "fx":[50.45153,50.54035,50.54884,50.4599], "fy":[-56.0764,-55.99639,-55.98852,-56.06867]}, - {"t":0.15026, "x":1.58503, "y":7.4847, "heading":-0.93509, "vx":0.46421, "vy":-0.51505, "omega":-0.0016, "ax":3.08879, "ay":-3.42721, "alpha":-0.01131, "fx":[50.44584,50.53704,50.54576,50.45443], "fy":[-56.07343,-55.99128,-55.98318,-56.06548]}, - {"t":0.18782, "x":1.60465, "y":7.46294, "heading":-0.93515, "vx":0.58024, "vy":-0.64379, "omega":-0.00202, "ax":3.08848, "ay":-3.42692, "alpha":-0.01164, "fx":[50.4394,50.53328,50.54226,50.44824], "fy":[-56.07006,-55.98548,-55.97714,-56.06187]}, - {"t":0.22539, "x":1.62862, "y":7.43633, "heading":-0.93523, "vx":0.69625, "vy":-0.77253, "omega":-0.00246, "ax":3.08813, "ay":-3.42659, "alpha":-0.01203, "fx":[50.43203,50.52899,50.53826,50.44115], "fy":[-56.06621,-55.97887,-55.97023,-56.05773]}, - {"t":0.26295, "x":1.65695, "y":7.4049, "heading":-0.93532, "vx":0.81226, "vy":-0.90124, "omega":-0.00291, "ax":3.08773, "ay":-3.42621, "alpha":-0.01247, "fx":[50.42353,50.52403,50.53364,50.43298], "fy":[-56.06176,-55.97123,-55.96225,-56.05296]}, - {"t":0.30052, "x":1.68964, "y":7.36862, "heading":-0.93543, "vx":0.92825, "vy":-1.02995, "omega":-0.00338, "ax":3.08726, "ay":-3.42577, "alpha":-0.01298, "fx":[50.4136,50.51825,50.52826,50.42344], "fy":[-56.05657,-55.96232,-55.95294,-56.04739]}, - {"t":0.33808, "x":1.72669, "y":7.32752, "heading":-0.93556, "vx":1.04422, "vy":-1.15864, "omega":-0.00387, "ax":3.08671, "ay":-3.42525, "alpha":-0.01359, "fx":[50.40188,50.51142,50.5219,50.41216], "fy":[-56.05044,-55.95179,-55.94194,-56.04081]}, - {"t":0.37565, "x":1.7681, "y":7.28158, "heading":-0.9357, "vx":1.16017, "vy":-1.2873, "omega":-0.00438, "ax":3.08605, "ay":-3.42462, "alpha":-0.01432, "fx":[50.3878,50.50322,50.51426,50.39863], "fy":[-56.04309,-55.93915,-55.92873,-56.0329]}, - {"t":0.41321, "x":1.81385, "y":7.2308, "heading":-0.93587, "vx":1.2761, "vy":-1.41595, "omega":-0.00492, "ax":3.08523, "ay":-3.42385, "alpha":-0.01522, "fx":[50.37059,50.49319,50.50492,50.38208], "fy":[-56.0341,-55.92369,-55.91258,-56.02324]}, - {"t":0.45078, "x":1.86397, "y":7.1752, "heading":-0.93605, "vx":1.39199, "vy":-1.54456, "omega":-0.00549, "ax":3.08422, "ay":-3.42289, "alpha":-0.01634, "fx":[50.34907,50.48066,50.49325,50.36138], "fy":[-56.02286,-55.90438,-55.89238,-56.01116]}, - {"t":0.48834, "x":1.91843, "y":7.11476, "heading":-0.93626, "vx":1.50785, "vy":-1.67314, "omega":-0.0061, "ax":3.08291, "ay":-3.42165, "alpha":-0.01778, "fx":[50.3214,50.46455,50.47823,50.33476], "fy":[-56.0084,-55.87954,-55.86639,-55.99561]}, - {"t":0.5259, "x":1.97725, "y":7.0495, "heading":-0.93649, "vx":1.62366, "vy":-1.80168, "omega":-0.00677, "ax":3.08117, "ay":-3.42, "alpha":-0.01971, "fx":[50.2845,50.44307,50.4582,50.29923], "fy":[-55.98912,-55.84641,-55.83172,-55.97487]}, - {"t":0.56347, "x":2.04042, "y":6.97941, "heading":-0.93674, "vx":1.7394, "vy":-1.93015, "omega":-0.00751, "ax":3.07873, "ay":-3.41769, "alpha":-0.02241, "fx":[50.23282,50.41299,50.43013,50.24945], "fy":[-55.96212,-55.80002,-55.78314,-55.94581]}, - {"t":0.60103, "x":2.10793, "y":6.90449, "heading":-0.93703, "vx":1.85505, "vy":-2.05853, "omega":-0.00835, "ax":3.07506, "ay":-3.41423, "alpha":-0.02648, "fx":[50.15528,50.36787,50.38798,50.17468], "fy":[-55.9216,-55.73041,-55.71017,-55.90214]}, - {"t":0.6386, "x":2.17978, "y":6.82475, "heading":-0.93734, "vx":1.97057, "vy":-2.18679, "omega":-0.00935, "ax":3.06895, "ay":-3.40844, "alpha":-0.03327, "fx":[50.02599,50.29264,50.31761,50.04983], "fy":[-55.854,-55.61438,-55.58834,-55.8292]}, - {"t":0.67616, "x":2.25597, "y":6.7402, "heading":-0.93769, "vx":2.08585, "vy":-2.31482, "omega":-0.0106, "ax":3.0567, "ay":-3.39684, "alpha":-0.04694, "fx":[49.76732,50.14211,50.17635,49.79934], "fy":[-55.71854,-55.38225,-55.34394,-55.68266]}, - {"t":0.71373, "x":2.33648, "y":6.65085, "heading":-0.93809, "vx":2.20067, "vy":-2.44242, "omega":-0.01236, "ax":3.01979, "ay":-3.36184, "alpha":-0.08848, "fx":[48.99123,49.68975,49.74824,49.04198], "fy":[-55.30984,-54.68591,-54.6052,-55.23757]}, - {"t":0.75129, "x":2.42128, "y":6.55673, "heading":-0.93855, "vx":2.31411, "vy":-2.56871, "omega":-0.01569, "ax":-1.17911, "ay":1.07722, "alpha":-2.80881, "fx":[-22.81284,-8.23637,-16.70856,-29.34676], "fy":[6.19788,17.67164,28.38594,18.18641]}, - {"t":0.78886, "x":2.50738, "y":6.461, "heading":-0.93914, "vx":2.26982, "vy":-2.52824, "omega":-0.1212, "ax":-3.03383, "ay":3.35394, "alpha":-0.07567, "fx":[-49.92303,-49.32122,-49.27443,-49.87063], "fy":[54.52556,55.06809,55.13255,54.59622]}, - {"t":0.82642, "x":2.5905, "y":6.36839, "heading":-0.9437, "vx":2.15585, "vy":-2.40225, "omega":-0.12404, "ax":-3.06321, "ay":3.39219, "alpha":-0.03596, "fx":[-50.2355,-49.94749,-49.92057,-50.20729], "fy":[55.31079,55.5704,55.60006,55.34188]}, - {"t":0.86399, "x":2.66932, "y":6.28054, "heading":-0.94836, "vx":2.04078, "vy":-2.27483, "omega":-0.12539, "ax":-3.07318, "ay":3.40517, "alpha":-0.02243, "fx":[-50.33995,-50.15993,-50.14158,-50.32111], "fy":[55.57744,55.73971,55.75851,55.5968]}, - {"t":0.90155, "x":2.74382, "y":6.19749, "heading":-0.95307, "vx":1.92534, "vy":-2.14691, "omega":-0.12623, "ax":-3.0782, "ay":3.41171, "alpha":-0.01557, "fx":[-50.39201,-50.26694,-50.25338,-50.3782], "fy":[55.7119,55.82462,55.83803,55.72558]}, - {"t":0.93912, "x":2.81397, "y":6.11925, "heading":-0.95781, "vx":1.80971, "vy":-2.01875, "omega":-0.12682, "ax":-3.08121, "ay":3.41565, "alpha":-0.01141, "fx":[-50.42309,-50.33144,-50.32096,-50.41248], "fy":[55.79301,55.87562,55.88576,55.8033]}, - {"t":0.97668, "x":2.87978, "y":6.04583, "heading":-0.96257, "vx":1.69397, "vy":-1.89045, "omega":-0.12725, "ax":-3.08323, "ay":3.41828, "alpha":-0.00861, "fx":[-50.44369,-50.37456,-50.36627,-50.43533], "fy":[55.84732,55.90963,55.91753,55.85531]}, - {"t":1.01424, "x":2.94123, "y":5.97723, "heading":-0.96735, "vx":1.57815, "vy":-1.76204, "omega":-0.12757, "ax":-3.08467, "ay":3.42017, "alpha":-0.00659, "fx":[-50.45831,-50.40541,-50.39879,-50.45165], "fy":[55.88625,55.93393,55.94018,55.89255]}, - {"t":1.05181, "x":2.99834, "y":5.91345, "heading":-0.97214, "vx":1.46227, "vy":-1.63356, "omega":-0.12782, "ax":-3.08576, "ay":3.42158, "alpha":-0.00506, "fx":[-50.46922,-50.42858,-50.42328,-50.46389], "fy":[55.91553,55.95216,55.95713,55.92053]}, - {"t":1.08937, "x":3.05109, "y":5.8545, "heading":-0.97694, "vx":1.34636, "vy":-1.50503, "omega":-0.12801, "ax":-3.0866, "ay":3.42268, "alpha":-0.00387, "fx":[-50.47765,-50.4466,-50.4424,-50.47343], "fy":[55.93837,55.96635,55.97027,55.94232]}, - {"t":1.12694, "x":3.09949, "y":5.80038, "heading":-0.98175, "vx":1.23041, "vy":-1.37646, "omega":-0.12815, "ax":-3.08727, "ay":3.42356, "alpha":-0.00291, "fx":[-50.48435,-50.46103,-50.45775,-50.48106], "fy":[55.9567,55.97772,55.98076,55.95975]}, - {"t":1.1645, "x":3.14353, "y":5.75109, "heading":-0.98657, "vx":1.11444, "vy":-1.24786, "omega":-0.12826, "ax":-3.08783, "ay":3.42428, "alpha":-0.00212, "fx":[-50.4898,-50.47282,-50.47035,-50.48732], "fy":[55.97173,55.98703,55.98931,55.97402]}, - {"t":1.20207, "x":3.18322, "y":5.70663, "heading":-0.99139, "vx":0.99844, "vy":-1.11922, "omega":-0.12834, "ax":-1.13542, "ay":-0.96733, "alpha":-0.60968, "fx":[-18.83966,-16.19997,-18.31127,-20.89663], "fy":[-18.10667,-16.54806,-13.47617,-15.12505]}, - {"t":1.23689, "x":3.21729, "y":5.66707, "heading":-0.99585, "vx":0.95891, "vy":-1.15291, "omega":-0.14957, "ax":-0.18809, "ay":-0.15555, "alpha":-0.5291, "fx":[-3.52348,-1.04926,-2.62913,-5.09802], "fy":[-4.55516,-2.96129,-0.52525,-2.12982]}, - {"t":1.2717, "x":3.25057, "y":5.62684, "heading":-1.00106, "vx":0.95236, "vy":-1.15832, "omega":-0.16799, "ax":-0.02773, "ay":-0.02278, "alpha":-0.31371, "fx":[-0.72809,0.7456,-0.17873,-1.65216], "fy":[-1.56559,-0.62194,0.82102,-0.12318]}, - {"t":1.30652, "x":3.28371, "y":5.58649, "heading":-1.00691, "vx":0.9514, "vy":-1.15911, "omega":-0.17892, "ax":-0.00406, "ay":-0.00334, "alpha":-0.09691, "fx":[-0.15348,0.30345,0.02058,-0.43634], "fy":[-0.42266,-0.13371,0.3136,0.02465]}, - {"t":1.34134, "x":3.31683, "y":5.54613, "heading":-1.01314, "vx":0.95125, "vy":-1.15923, "omega":-0.18229, "ax":-0.0006, "ay":-0.00049, "alpha":0.11946, "fx":[0.10038,-0.46508,-0.11984,0.44562], "fy":[0.44513,0.09246,-0.4611,-0.10843]}, - {"t":1.37616, "x":3.34995, "y":5.50577, "heading":-1.01949, "vx":0.95123, "vy":-1.15925, "omega":-0.17813, "ax":-0.00009, "ay":-0.00007, "alpha":0.3364, "fx":[0.31673,-1.2819,-0.31958,1.27905], "fy":[1.27283,0.28981,-1.27516,-0.29215]}, - {"t":1.41098, "x":3.38308, "y":5.4654, "heading":-1.02569, "vx":0.95123, "vy":-1.15925, "omega":-0.16642, "ax":-0.00001, "ay":-0.00001, "alpha":0.55496, "fx":[0.53768,-2.10959,-0.5381,2.10918], "fy":[2.09825,0.49296,-2.09859,-0.4933]}, - {"t":1.4458, "x":3.4162, "y":5.42504, "heading":-1.03148, "vx":0.95123, "vy":-1.15925, "omega":-0.14709, "ax":0.0, "ay":0.0, "alpha":0.77618, "fx":[0.76926,-2.9462,-0.76933,2.94614], "fy":[2.93044,0.70675,-2.93049,-0.70681]}, - {"t":1.48062, "x":3.44932, "y":5.38467, "heading":-1.03661, "vx":0.95123, "vy":-1.15925, "omega":-0.12007, "ax":0.0, "ay":0.0, "alpha":1.00108, "fx":[1.01153,-3.79513,-1.01156,3.79509], "fy":[3.77443,0.93101,-3.77446,-0.93103]}, - {"t":1.51544, "x":3.48244, "y":5.34431, "heading":-1.04079, "vx":0.95123, "vy":-1.15925, "omega":-0.08521, "ax":-0.00007, "ay":0.00008, "alpha":1.23068, "fx":[1.26171,-4.66192,-1.26417,4.65949], "fy":[4.6362,1.16537,-4.63357,-1.16273]}, - {"t":1.55026, "x":3.51556, "y":5.30395, "heading":-1.04375, "vx":0.95123, "vy":-1.15925, "omega":-0.04236, "ax":-1.03287, "ay":1.25877, "alpha":1.14645, "fx":[-15.23468,-21.0979,-18.71391,-12.49555], "fy":[24.87235,21.16568,16.18847,20.0872]}, - {"t":1.58507, "x":3.54805, "y":5.26435, "heading":-1.04523, "vx":0.91526, "vy":-1.11542, "omega":-0.00244, "ax":-2.89922, "ay":3.53324, "alpha":0.01888, "fx":[-47.30698,-47.46298,-47.48669,-47.33034], "fy":[57.83656,57.70906,57.6868,57.8147]}, - {"t":1.61989, "x":3.57816, "y":5.22765, "heading":-1.04531, "vx":0.81432, "vy":-0.9924, "omega":-0.00179, "ax":-2.91485, "ay":3.55229, "alpha":0.01081, "fx":[-47.60042,-47.69015,-47.70414,-47.6143], "fy":[58.11603,58.04255,58.03026,58.10386]}, - {"t":1.65471, "x":3.60475, "y":5.19525, "heading":-1.04538, "vx":0.71282, "vy":-0.86871, "omega":-0.00141, "ax":-2.92009, "ay":3.55868, "alpha":0.00811, "fx":[-47.69896,-47.76638,-47.77698,-47.7095], "fy":[58.20978,58.15454,58.14543,58.20074]}, - {"t":1.68953, "x":3.6278, "y":5.16716, "heading":-1.04542, "vx":0.61115, "vy":-0.7448, "omega":-0.00113, "ax":-2.92272, "ay":3.56188, "alpha":0.00676, "fx":[-47.74835,-47.80457,-47.81346,-47.75719], "fy":[58.25675,58.21067,58.20312,58.24926]}, - {"t":1.72435, "x":3.64731, "y":5.14338, "heading":-1.04546, "vx":0.50938, "vy":-0.62078, "omega":-0.00089, "ax":-2.9243, "ay":3.5638, "alpha":0.00594, "fx":[-47.77801,-47.82751,-47.83535,-47.78582], "fy":[58.28496,58.24438,58.23777,58.27839]}, - {"t":1.75917, "x":3.66327, "y":5.12393, "heading":-1.0455, "vx":0.40756, "vy":-0.49669, "omega":-0.00068, "ax":-2.92535, "ay":3.56508, "alpha":0.0054, "fx":[-47.7978,-47.84281,-47.84996,-47.80492], "fy":[58.30378,58.26687,58.26087,58.29781]}, - {"t":1.79399, "x":3.67569, "y":5.1088, "heading":-1.04552, "vx":0.30571, "vy":-0.37256, "omega":-0.0005, "ax":-2.9261, "ay":3.566, "alpha":0.00502, "fx":[-47.81195,-47.85375,-47.86039,-47.81857], "fy":[58.31722,58.28294,58.27738,58.31169]}, - {"t":1.82881, "x":3.68456, "y":5.09799, "heading":-1.04554, "vx":0.20382, "vy":-0.24839, "omega":-0.00032, "ax":-2.92666, "ay":3.56669, "alpha":0.00473, "fx":[-47.82256,-47.86195,-47.86822,-47.8288], "fy":[58.32731,58.295,58.28977,58.3221]}, - {"t":1.86363, "x":3.68988, "y":5.0915, "heading":-1.04555, "vx":0.10192, "vy":-0.12421, "omega":-0.00016, "ax":-2.9271, "ay":3.56722, "alpha":0.0045, "fx":[-47.83081,-47.86833,-47.87431,-47.83676], "fy":[58.33516,58.30438,58.2994,58.3302]}, - {"t":1.89844, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.08941, "ay":-3.42846, "alpha":-0.00579, "fx":[50.48043,50.52718,50.53164,50.48485], "fy":[-56.07195,-56.02984,-56.02573,-56.06788]}, + {"t":0.03859, "x":1.55245, "y":7.52085, "heading":-0.93501, "vx":0.11923, "vy":-0.13232, "omega":-0.00022, "ax":3.08922, "ay":-3.42827, "alpha":-0.00593, "fx":[50.47666,50.52451,50.52907,50.48118], "fy":[-56.06936,-56.02626,-56.02205,-56.06519]}, + {"t":0.07719, "x":1.55935, "y":7.51319, "heading":-0.93501, "vx":0.23846, "vy":-0.26463, "omega":-0.00045, "ax":3.089, "ay":-3.42806, "alpha":-0.00608, "fx":[50.47244,50.52152,50.52619,50.47708], "fy":[-56.06646,-56.02226,-56.01794,-56.06218]}, + {"t":0.11578, "x":1.57086, "y":7.50042, "heading":-0.93503, "vx":0.35767, "vy":-0.39693, "omega":-0.00069, "ax":3.08876, "ay":-3.42782, "alpha":-0.00625, "fx":[50.4677,50.51815,50.52296,50.47246], "fy":[-56.0632,-56.01776,-56.01331,-56.0588]}, + {"t":0.15437, "x":1.58696, "y":7.48255, "heading":-0.93506, "vx":0.47688, "vy":-0.52922, "omega":-0.00093, "ax":3.08848, "ay":-3.42754, "alpha":-0.00645, "fx":[50.46233,50.51434,50.51929,50.46723], "fy":[-56.05951,-56.01266,-56.00807,-56.05497]}, + {"t":0.19297, "x":1.60767, "y":7.45957, "heading":-0.93509, "vx":0.59608, "vy":-0.6615, "omega":-0.00118, "ax":3.08817, "ay":-3.42723, "alpha":-0.00667, "fx":[50.45618,50.50998,50.5151,50.46126], "fy":[-56.05529,-56.00683,-56.00207,-56.05058]}, + {"t":0.23156, "x":1.63297, "y":7.43149, "heading":-0.93514, "vx":0.71526, "vy":-0.79377, "omega":-0.00143, "ax":3.0878, "ay":-3.42687, "alpha":-0.00693, "fx":[50.4491,50.50496,50.51027,50.45436], "fy":[-56.05042,-56.0001,-55.99516,-56.04553]}, + {"t":0.27016, "x":1.66288, "y":7.3983, "heading":-0.9352, "vx":0.83443, "vy":-0.92603, "omega":-0.0017, "ax":3.08738, "ay":-3.42645, "alpha":-0.00722, "fx":[50.44083,50.49909,50.50463,50.44631], "fy":[-56.04474,-55.99226,-55.98709,-56.03963]}, + {"t":0.30875, "x":1.69738, "y":7.36001, "heading":-0.93526, "vx":0.95358, "vy":-1.05827, "omega":-0.00198, "ax":3.08687, "ay":-3.42595, "alpha":-0.00758, "fx":[50.43106,50.49216,50.49797,50.4368], "fy":[-56.03803,-55.98299,-55.97756,-56.03266]}, + {"t":0.34734, "x":1.73648, "y":7.31662, "heading":-0.93534, "vx":1.07272, "vy":-1.19049, "omega":-0.00227, "ax":3.08627, "ay":-3.42536, "alpha":-0.008, "fx":[50.41933,50.48385,50.48997,50.42539], "fy":[-56.02997,-55.97187,-55.96612,-56.02429]}, + {"t":0.38594, "x":1.78018, "y":7.26812, "heading":-0.93542, "vx":1.19183, "vy":-1.32268, "omega":-0.00258, "ax":3.08553, "ay":-3.42463, "alpha":-0.00852, "fx":[50.40501,50.47369,50.4802,50.41144], "fy":[-56.02013,-55.95828,-55.95213,-56.01406]}, + {"t":0.42453, "x":1.82847, "y":7.21452, "heading":-0.93552, "vx":1.31091, "vy":-1.45485, "omega":-0.00291, "ax":3.08461, "ay":-3.42372, "alpha":-0.00917, "fx":[50.3871,50.46099,50.46799,50.39401], "fy":[-56.00783,-55.9413,-55.93466,-56.00128]}, + {"t":0.46312, "x":1.88136, "y":7.15582, "heading":-0.93564, "vx":1.42996, "vy":-1.58699, "omega":-0.00326, "ax":3.08343, "ay":-3.42255, "alpha":-0.01, "fx":[50.36409,50.44468,50.4523,50.3716], "fy":[-55.99203,-55.91948,-55.91219,-55.98486]}, + {"t":0.50172, "x":1.93885, "y":7.09203, "heading":-0.93576, "vx":1.54896, "vy":-1.71908, "omega":-0.00365, "ax":3.08185, "ay":-3.42099, "alpha":-0.01112, "fx":[50.33342,50.42293,50.43137,50.34173], "fy":[-55.97096,-55.89039,-55.88224,-55.96295]}, + {"t":0.54031, "x":2.00092, "y":7.02313, "heading":-0.9359, "vx":1.6679, "vy":-1.85111, "omega":-0.00408, "ax":3.07964, "ay":-3.41881, "alpha":-0.01268, "fx":[50.29051,50.39252,50.40209,50.29992], "fy":[-55.94149,-55.84969,-55.84032,-55.9323]}, + {"t":0.57891, "x":2.06759, "y":6.94915, "heading":-0.93606, "vx":1.78675, "vy":-1.98305, "omega":-0.00457, "ax":3.07633, "ay":-3.41553, "alpha":-0.01502, "fx":[50.22621,50.34694,50.3582,50.23724], "fy":[-55.89732,-55.78872,-55.77747,-55.88633]}, + {"t":0.6175, "x":2.13883, "y":6.87007, "heading":-0.93624, "vx":1.90548, "vy":-2.11487, "omega":-0.00515, "ax":3.07082, "ay":-3.41009, "alpha":-0.01892, "fx":[50.11924,50.27112,50.2851,50.13286], "fy":[-55.82382,-55.68727,-55.67283,-55.80977]}, + {"t":0.65609, "x":2.21466, "y":6.78591, "heading":-0.93644, "vx":2.02399, "vy":-2.24648, "omega":-0.00588, "ax":3.05983, "ay":-3.39922, "alpha":-0.02674, "fx":[49.90607,50.11998,50.13916,49.92452], "fy":[-55.67721,-55.48511,-55.46394,-55.65684]}, + {"t":0.69469, "x":2.29505, "y":6.69668, "heading":-0.93666, "vx":2.14208, "vy":-2.37766, "omega":-0.00691, "ax":3.02711, "ay":-3.36684, "alpha":-0.05019, "fx":[49.27296,49.67066,49.70323,49.30303], "fy":[-55.24051,-54.8846,-54.84084,-55.19947]}, + {"t":0.73328, "x":2.37998, "y":6.60241, "heading":-0.93693, "vx":2.25891, "vy":-2.5076, "omega":-0.00885, "ax":-0.03913, "ay":-0.15143, "alpha":-2.2462, "fx":[-2.05033,8.04867,0.7743,-9.33143], "fy":[-11.08514,-3.74474,6.20253,-1.27508]}, + {"t":0.77187, "x":2.46713, "y":6.50552, "heading":-0.93727, "vx":2.2574, "vy":-2.51345, "omega":-0.09554, "ax":-3.03493, "ay":3.3596, "alpha":-0.04548, "fx":[-49.81085,-49.44917,-49.42086,-49.78051], "fy":[54.74055,55.06615,55.10447,54.78112]}, + {"t":0.81047, "x":2.55199, "y":6.41102, "heading":-0.94096, "vx":2.14027, "vy":-2.38379, "omega":-0.09729, "ax":-3.06381, "ay":3.39559, "alpha":-0.02173, "fx":[-50.18269,-50.0086,-49.9926,-50.16622], "fy":[55.42409,55.58088,55.59839,55.44212]}, + {"t":0.84906, "x":2.63231, "y":6.32155, "heading":-0.94471, "vx":2.02203, "vy":-2.25274, "omega":-0.09813, "ax":-3.07349, "ay":3.40766, "alpha":-0.01372, "fx":[-50.30635,-50.1962,-50.1853,-50.29526], "fy":[55.6535,55.75272,55.76385,55.66484]}, + {"t":0.88766, "x":2.70806, "y":6.23714, "heading":-0.9485, "vx":1.90341, "vy":-2.12123, "omega":-0.09866, "ax":-3.07834, "ay":3.41371, "alpha":-0.00966, "fx":[-50.36797,-50.2903,-50.28219,-50.35977], "fy":[55.76862,55.83859,55.84659,55.77672]}, + {"t":0.92625, "x":2.77923, "y":6.15782, "heading":-0.95231, "vx":1.7846, "vy":-1.98948, "omega":-0.09903, "ax":-3.08126, "ay":3.41734, "alpha":-0.00721, "fx":[-50.40481,-50.34688,-50.34055,-50.39843], "fy":[55.83788,55.89007,55.89619,55.84405]}, + {"t":0.96484, "x":2.84581, "y":6.08358, "heading":-0.95613, "vx":1.66569, "vy":-1.85759, "omega":-0.09931, "ax":-3.0832, "ay":3.41977, "alpha":-0.00555, "fx":[-50.42928,-50.38466,-50.37958,-50.42418], "fy":[55.88416,55.92436,55.9292,55.88903]}, + {"t":1.00344, "x":2.90779, "y":6.01444, "heading":-0.95996, "vx":1.5467, "vy":-1.72561, "omega":-0.09953, "ax":-3.08459, "ay":3.4215, "alpha":-0.00436, "fx":[-50.44669,-50.41167,-50.40754,-50.44254], "fy":[55.91728,55.94884,55.95274,55.92121]}, + {"t":1.04203, "x":2.96519, "y":5.95039, "heading":-0.9638, "vx":1.42765, "vy":-1.59356, "omega":-0.09969, "ax":-3.08563, "ay":3.4228, "alpha":-0.00345, "fx":[-50.45971,-50.43195,-50.42856,-50.45631], "fy":[55.94217,55.96718,55.97036,55.94536]}, + {"t":1.08062, "x":3.01799, "y":5.89144, "heading":-0.96765, "vx":1.30856, "vy":-1.46146, "omega":-0.09983, "ax":-3.08644, "ay":3.42381, "alpha":-0.00275, "fx":[-50.46981,-50.44773,-50.44494,-50.46701], "fy":[55.96156,55.98145,55.98405,55.96417]}, + {"t":1.11922, "x":3.06619, "y":5.83758, "heading":-0.9715, "vx":1.18945, "vy":-1.32932, "omega":-0.09993, "ax":-3.08709, "ay":3.42462, "alpha":-0.00218, "fx":[-50.47785,-50.46036,-50.45808,-50.47557], "fy":[55.97709,55.99286,55.99497,55.97921]}, + {"t":1.15781, "x":3.1098, "y":5.78883, "heading":-0.97536, "vx":1.0703, "vy":-1.19715, "omega":-0.10002, "ax":-3.08762, "ay":3.42528, "alpha":-0.00171, "fx":[-50.48442,-50.47069,-50.46884,-50.48257], "fy":[55.98982,56.00219,56.0039,55.99153]}, + {"t":1.19641, "x":3.14881, "y":5.74518, "heading":-0.97922, "vx":0.95114, "vy":-1.06496, "omega":-0.10008, "ax":-3.08806, "ay":3.42584, "alpha":-0.00132, "fx":[-50.48987,-50.47929,-50.47783,-50.48841], "fy":[56.00044,56.00997,56.01132,56.0018]}, + {"t":1.235, "x":3.18322, "y":5.70663, "heading":-0.98308, "vx":0.83196, "vy":-0.93274, "omega":-0.10013, "ax":-0.9744, "ay":-0.82976, "alpha":-0.497, "fx":[-16.18986,-14.0089,-15.68417,-17.83521], "fy":[-15.44631,-14.08815,-11.65841,-13.06725]}, + {"t":1.27027, "x":3.21196, "y":5.67321, "heading":-0.98662, "vx":0.79759, "vy":-0.96201, "omega":-0.11766, "ax":-0.11311, "ay":-0.09339, "alpha":-0.4615, "fx":[-2.22683,-0.07972,-1.47285,-3.61746], "fy":[-3.28669,-1.87101,0.23565,-1.18487]}, + {"t":1.30554, "x":3.24002, "y":5.63922, "heading":-0.99077, "vx":0.7936, "vy":-0.96531, "omega":-0.13394, "ax":-0.01201, "ay":-0.00987, "alpha":-0.34675, "fx":[-0.48638,1.13169,0.09378,-1.52415], "fy":[-1.48326,-0.42327,1.16084,0.10057]}, + {"t":1.34082, "x":3.268, "y":5.60517, "heading":-0.99549, "vx":0.79318, "vy":-0.96565, "omega":-0.14617, "ax":-0.00127, "ay":-0.00104, "alpha":-0.23282, "fx":[-0.21974,0.87001,0.1782,-0.91154], "fy":[-0.90379,-0.19712,0.86969,0.163]}, + {"t":1.37609, "x":3.29598, "y":5.5711, "heading":-1.00065, "vx":0.79313, "vy":-0.96569, "omega":-0.15439, "ax":-0.00013, "ay":-0.00011, "alpha":-0.12006, "fx":[-0.10716,0.45668,0.10277,-0.46107], "fy":[-0.45855,-0.09703,0.45494,0.09342]}, + {"t":1.41136, "x":3.32396, "y":5.53704, "heading":-1.00609, "vx":0.79313, "vy":-0.9657, "omega":-0.15862, "ax":-0.00001, "ay":-0.00001, "alpha":-0.00791, "fx":[-0.00731,0.02998,0.00685,-0.03044], "fy":[-0.03026,-0.00663,0.02987,0.00625]}, + {"t":1.44664, "x":3.35193, "y":5.50298, "heading":-1.01169, "vx":0.79313, "vy":-0.9657, "omega":-0.1589, "ax":0.0, "ay":0.0, "alpha":0.10419, "fx":[0.09544,-0.39732,-0.09549,0.39727], "fy":[0.39534,0.08701,-0.39538,-0.08705]}, + {"t":1.48191, "x":3.37991, "y":5.46892, "heading":-1.01729, "vx":0.79313, "vy":-0.9657, "omega":-0.15522, "ax":0.0, "ay":0.0, "alpha":0.21683, "fx":[0.20327,-0.82576,-0.20327,0.82575], "fy":[0.82161,0.18574,-0.82162,-0.18575]}, + {"t":1.51718, "x":3.40788, "y":5.43485, "heading":-1.02277, "vx":0.79313, "vy":-0.9657, "omega":-0.14758, "ax":0.0, "ay":0.0, "alpha":0.33057, "fx":[0.31674,-1.25732,-0.31674,1.25732], "fy":[1.25086,0.29006,-1.25086,-0.29006]}, + {"t":1.55245, "x":3.43586, "y":5.40079, "heading":-1.02797, "vx":0.79313, "vy":-0.9657, "omega":-0.13592, "ax":0.0, "ay":0.0, "alpha":0.44597, "fx":[0.4361,-1.69421,-0.4361,1.69421], "fy":[1.68532,0.40015,-1.68532,-0.40015]}, + {"t":1.58773, "x":3.46384, "y":5.36673, "heading":-1.03277, "vx":0.79313, "vy":-0.9657, "omega":-0.12019, "ax":0.0, "ay":0.0, "alpha":0.56363, "fx":[0.56136,-2.13873,-0.56136,2.13873], "fy":[2.12727,0.51598,-2.12727,-0.51598]}, + {"t":1.623, "x":3.49181, "y":5.33266, "heading":-1.03701, "vx":0.79313, "vy":-0.9657, "omega":-0.10031, "ax":0.0, "ay":0.0, "alpha":0.68412, "fx":[0.69231,-2.59328,-0.69231,2.59327], "fy":[2.57913,0.63728,-2.57913,-0.63729]}, + {"t":1.65827, "x":3.51979, "y":5.2986, "heading":-1.04054, "vx":0.79313, "vy":-0.9657, "omega":-0.07617, "ax":0.0, "ay":0.0, "alpha":0.80805, "fx":[0.82849,-3.06034,-0.82849,3.06033], "fy":[3.0434,0.76355,-3.0434,-0.76356]}, + {"t":1.69355, "x":3.54776, "y":5.26454, "heading":-1.04323, "vx":0.79313, "vy":-0.9657, "omega":-0.04767, "ax":-0.00015, "ay":0.00018, "alpha":0.936, "fx":[0.96671,-3.54498,-0.97159,3.54012], "fy":[3.52564,0.8969,-3.51982,-0.89107]}, + {"t":1.72882, "x":3.57574, "y":5.23048, "heading":-1.04491, "vx":0.79312, "vy":-0.96569, "omega":-0.01466, "ax":-2.02463, "ay":2.46515, "alpha":0.38307, "fx":[-32.021,-34.47573,-34.21918,-31.67969], "fy":[41.76193,39.97843,38.80229,40.65941]}, + {"t":1.76409, "x":3.60246, "y":5.19795, "heading":-1.04543, "vx":0.72171, "vy":-0.87874, "omega":-0.00115, "ax":-2.90753, "ay":3.54014, "alpha":0.01041, "fx":[-47.48284,-47.56901,-47.58228,-47.496], "fy":[57.91577,57.84522,57.83311,57.90379]}, + {"t":1.79936, "x":3.6261, "y":5.16915, "heading":-1.04547, "vx":0.61915, "vy":-0.75387, "omega":-0.00078, "ax":-2.91971, "ay":3.55497, "alpha":0.00585, "fx":[-47.70365,-47.75227,-47.75991,-47.71126], "fy":[58.1403,58.10043,58.09381,58.13371]}, + {"t":1.83464, "x":3.64613, "y":5.14477, "heading":-1.0455, "vx":0.51617, "vy":-0.62847, "omega":-0.00057, "ax":-2.92385, "ay":3.56001, "alpha":0.00431, "fx":[-47.77856,-47.81441,-47.82008,-47.78422], "fy":[58.21642,58.18701,58.18218,58.21161]}, + {"t":1.86991, "x":3.66251, "y":5.12482, "heading":-1.04552, "vx":0.41303, "vy":-0.5029, "omega":-0.00042, "ax":-2.92592, "ay":3.56254, "alpha":0.00353, "fx":[-47.81626,-47.84568,-47.85035,-47.82092], "fy":[58.25472,58.23058,58.22664,58.2508]}, + {"t":1.90518, "x":3.67526, "y":5.1093, "heading":-1.04553, "vx":0.30983, "vy":-0.37724, "omega":-0.00029, "ax":-2.92718, "ay":3.56406, "alpha":0.00307, "fx":[-47.83895,-47.8645,-47.86857,-47.84301], "fy":[58.27778,58.25681,58.2534,58.27438]}, + {"t":1.94045, "x":3.68437, "y":5.09821, "heading":-1.04554, "vx":0.20658, "vy":-0.25153, "omega":-0.00019, "ax":-2.92801, "ay":3.56508, "alpha":0.00276, "fx":[-47.85411,-47.87707,-47.88073,-47.85776], "fy":[58.29318,58.27433,58.27127,58.29013]}, + {"t":1.97573, "x":3.68984, "y":5.09156, "heading":-1.04555, "vx":0.1033, "vy":-0.12578, "omega":-0.00009, "ax":-2.92861, "ay":3.56581, "alpha":0.00253, "fx":[-47.86495,-47.88607,-47.88943,-47.86832], "fy":[58.3042,58.28686,58.28405,58.30139]}, + {"t":2.011, "x":3.69166, "y":5.08934, "heading":-1.04555, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PRMtoB.traj b/src/main/deploy/choreo/PRMtoB.traj index 5bfe7978..a863e89e 100644 --- a/src/main/deploy/choreo/PRMtoB.traj +++ b/src/main/deploy/choreo/PRMtoB.traj @@ -4,27 +4,27 @@ "snapshot":{ "waypoints":[ {"x":1.1213834285736084, "y":0.9940196871757508, "heading":0.9420001549844138, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.9325637817382812, "y":3.2750864028930664, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.9325637817382812, "y":3.2750864028930664, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PRM.x", "val":1.1213834285736084}, "y":{"exp":"PRM.y", "val":0.9940196871757508}, "heading":{"exp":"0.9420001549844138 rad", "val":0.9420001549844138}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.9325637817382812 m", "val":2.9325637817382812}, "y":{"exp":"3.2750864028930664 m", "val":3.2750864028930664}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.9325637817382812 m", "val":2.9325637817382812}, "y":{"exp":"3.2750864028930664 m", "val":3.2750864028930664}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,74 +32,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.33142,1.91233], + "waypoints":[0.0,1.36526,2.00258], "samples":[ - {"t":0.0, "x":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.95851, "ay":3.53982, "alpha":-0.25899, "fx":[49.35922,47.233,47.33583,49.53626], "fy":[57.04458,58.81688,58.7297,56.88634]}, - {"t":0.03247, "x":1.12294, "y":0.99589, "heading":0.942, "vx":0.09607, "vy":0.11495, "omega":-0.00841, "ax":2.95764, "ay":3.5402, "alpha":-0.26319, "fx":[49.36116,47.2008,47.30415,49.54116], "fy":[57.03781,58.83778,58.75,56.8767]}, - {"t":0.06495, "x":1.12762, "y":1.00149, "heading":0.94173, "vx":0.19212, "vy":0.22992, "omega":-0.01696, "ax":2.95669, "ay":3.54061, "alpha":-0.26776, "fx":[49.36358,47.16609,47.26935,49.54623], "fy":[57.03019,58.86024,58.77234,56.86645]}, - {"t":0.09742, "x":1.13542, "y":1.01082, "heading":0.94118, "vx":0.28813, "vy":0.34489, "omega":-0.02565, "ax":2.95566, "ay":3.54106, "alpha":-0.27275, "fx":[49.36653,47.12851,47.23102,49.5515], "fy":[57.02161,58.88449,58.79696,56.85548]}, - {"t":0.1299, "x":1.14634, "y":1.02389, "heading":0.94034, "vx":0.38412, "vy":0.45988, "omega":-0.03451, "ax":2.95452, "ay":3.54155, "alpha":-0.27821, "fx":[49.37006,47.08762,47.18866,49.55701], "fy":[57.01196,58.91079,58.82419,56.84368]}, - {"t":0.16237, "x":1.16037, "y":1.04069, "heading":0.93922, "vx":0.48006, "vy":0.57489, "omega":-0.04354, "ax":2.95327, "ay":3.54209, "alpha":-0.28423, "fx":[49.37424,47.04292,47.14169,49.56283], "fy":[57.00109,58.93946,58.85438,56.83091]}, - {"t":0.19484, "x":1.17751, "y":1.06122, "heading":0.93781, "vx":0.57596, "vy":0.68992, "omega":-0.05277, "ax":2.95189, "ay":3.54269, "alpha":-0.29088, "fx":[49.37914,46.99378,47.0894,49.56902], "fy":[56.98884,58.97089,58.88799,56.81699]}, - {"t":0.22732, "x":1.19777, "y":1.0855, "heading":0.93609, "vx":0.67182, "vy":0.80496, "omega":-0.06222, "ax":2.95036, "ay":3.54335, "alpha":-0.29828, "fx":[49.38485,46.93944,47.03091,49.57566], "fy":[56.97499,59.00554,58.92555,56.80172]}, - {"t":0.25979, "x":1.22115, "y":1.1135, "heading":0.93407, "vx":0.76763, "vy":0.92003, "omega":-0.07191, "ax":2.94864, "ay":3.54408, "alpha":-0.30655, "fx":[49.3915,46.87898,46.96513,49.58287], "fy":[56.9593,59.04399,58.96775,56.78481]}, - {"t":0.29226, "x":1.24763, "y":1.14525, "heading":0.93174, "vx":0.86339, "vy":1.03512, "omega":-0.08186, "ax":2.9467, "ay":3.5449, "alpha":-0.31587, "fx":[49.39921,46.81122,46.89071,49.59076], "fy":[56.94144,59.08694,59.01542,56.76596]}, - {"t":0.32474, "x":1.27722, "y":1.18073, "heading":0.92908, "vx":0.95908, "vy":1.15023, "omega":-0.09212, "ax":2.94451, "ay":3.54584, "alpha":-0.32643, "fx":[49.40818,46.73469,46.80591,49.59951], "fy":[56.92101,59.13531,59.06963,56.74474]}, - {"t":0.35721, "x":1.30992, "y":1.21996, "heading":0.92609, "vx":1.0547, "vy":1.26538, "omega":-0.10272, "ax":2.94199, "ay":3.5469, "alpha":-0.33851, "fx":[49.41863,46.64748,46.70851,49.60933], "fy":[56.8975,59.19024,59.13175,56.72061]}, - {"t":0.38969, "x":1.34572, "y":1.26292, "heading":0.92275, "vx":1.15023, "vy":1.38056, "omega":-0.11371, "ax":2.93909, "ay":3.54812, "alpha":-0.35246, "fx":[49.43087,46.54711,46.59556,49.62049], "fy":[56.87023,59.25322,59.20355,56.69285]}, - {"t":0.42216, "x":1.38462, "y":1.30962, "heading":0.91906, "vx":1.24568, "vy":1.49578, "omega":-0.12516, "ax":2.9357, "ay":3.54953, "alpha":-0.36876, "fx":[49.44531,46.4303,46.46312,49.63337], "fy":[56.8383,59.32622,59.28742,56.66052]}, - {"t":0.45463, "x":1.42662, "y":1.36006, "heading":0.915, "vx":1.34101, "vy":1.61105, "omega":-0.13713, "ax":2.93168, "ay":3.5512, "alpha":-0.38804, "fx":[49.46248,46.29253,46.30578,49.64849], "fy":[56.80049,59.41192,59.38659,56.62232]}, - {"t":0.48711, "x":1.47171, "y":1.41425, "heading":0.91054, "vx":1.43621, "vy":1.72637, "omega":-0.14973, "ax":2.92684, "ay":3.55318, "alpha":-0.41121, "fx":[49.48315,46.12755,46.1159,49.66658], "fy":[56.75509,59.514,59.50558,56.57639]}, - {"t":0.51958, "x":1.5199, "y":1.47219, "heading":0.90568, "vx":1.53126, "vy":1.84175, "omega":-0.16309, "ax":2.92092, "ay":3.55559, "alpha":-0.43958, "fx":[49.50843,45.92632,45.88234,49.68868], "fy":[56.69966,59.63771,59.6509,56.52005]}, - {"t":0.55205, "x":1.57116, "y":1.53387, "heading":0.90039, "vx":1.62611, "vy":1.95722, "omega":-0.17736, "ax":2.91349, "ay":3.55856, "alpha":-0.47511, "fx":[49.53995,45.67535,45.5882,49.71641], "fy":[56.63052,59.79081,59.83228,56.44924]}, - {"t":0.58453, "x":1.6255, "y":1.59931, "heading":0.89463, "vx":1.72072, "vy":2.07278, "omega":-0.19279, "ax":2.9039, "ay":3.56233, "alpha":-0.52092, "fx":[49.58029,45.35355,45.20652,49.75228], "fy":[56.54193,59.9852,60.06492,56.35749]}, - {"t":0.617, "x":1.68291, "y":1.6685, "heading":0.88836, "vx":1.81502, "vy":2.18846, "omega":-0.20971, "ax":2.89104, "ay":3.56727, "alpha":-0.58218, "fx":[49.63369,44.926,44.69155,49.8006], "fy":[56.4244,60.24016,60.37397,56.23384]}, - {"t":0.64948, "x":1.74338, "y":1.74144, "heading":0.88155, "vx":1.90891, "vy":2.3043, "omega":-0.22861, "ax":2.87291, "ay":3.574, "alpha":-0.66833, "fx":[49.70769,44.33052,43.95905,49.86917], "fy":[56.26103,60.58908,60.80418,56.05816]}, - {"t":0.68195, "x":1.80688, "y":1.81816, "heading":0.87413, "vx":2.0022, "vy":2.42036, "omega":-0.25032, "ax":2.84545, "ay":3.58369, "alpha":-0.79834, "fx":[49.81706,43.44457,42.8351,49.97398], "fy":[56.01852,61.09504,61.44326,55.78901]}, - {"t":0.71442, "x":1.8734, "y":1.89865, "heading":0.866, "vx":2.0946, "vy":2.53674, "omega":-0.27624, "ax":2.79899, "ay":3.5987, "alpha":-1.01715, "fx":[49.99506,41.98936,40.89501,50.15329], "fy":[55.62105,61.89272,62.48831,55.32547]}, - {"t":0.7469, "x":1.9429, "y":1.98292, "heading":0.85703, "vx":2.1855, "vy":2.6536, "omega":-0.30927, "ax":2.70368, "ay":3.62423, "alpha":-1.46284, "fx":[50.33491,39.16834,36.76957,50.52693], "fy":[54.85139,63.32528,64.47893,54.3412]}, - {"t":0.77937, "x":2.01529, "y":2.071, "heading":0.84699, "vx":2.2733, "vy":2.77129, "omega":-0.35678, "ax":2.40321, "ay":3.65954, "alpha":-2.86568, "fx":[51.22878,31.50188,22.6753,51.7458], "fy":[52.73972,66.51794,69.17468,50.87347]}, - {"t":0.81184, "x":2.09038, "y":2.16293, "heading":0.8354, "vx":2.35134, "vy":2.89013, "omega":-0.44984, "ax":-0.94116, "ay":0.62812, "alpha":-16.05331, "fx":[57.30852,-23.84208,-68.65453,-26.35664], "fy":[26.38333,64.69567,9.81523,-59.82018]}, - {"t":0.84432, "x":2.16624, "y":2.25711, "heading":0.82079, "vx":2.32077, "vy":2.91053, "omega":-0.97115, "ax":-3.21338, "ay":-3.012, "alpha":-2.52922, "fx":[-42.24707,-62.78254,-60.13013,-44.97114], "fy":[-59.27025,-37.00095,-42.45774,-58.23304]}, - {"t":0.87679, "x":2.23991, "y":2.35004, "heading":0.78926, "vx":2.21642, "vy":2.81272, "omega":-1.05328, "ax":-3.12287, "ay":-3.2835, "alpha":-1.19236, "fx":[-45.85938,-55.86328,-55.4771,-47.01186], "fy":[-58.40994,-48.92135,-49.6505,-57.73434]}, - {"t":0.90927, "x":2.31024, "y":2.43965, "heading":0.75505, "vx":2.11501, "vy":2.70609, "omega":-1.092, "ax":-3.08069, "ay":-3.3691, "alpha":-0.75118, "fx":[-46.96409,-53.25645,-53.43235,-47.80061], "fy":[-58.09763,-52.37943,-52.31907,-57.51705]}, - {"t":0.94174, "x":2.3773, "y":2.52575, "heading":0.71959, "vx":2.01497, "vy":2.59668, "omega":-1.11639, "ax":-3.0575, "ay":-3.41074, "alpha":-0.52944, "fx":[-47.51267,-51.92147,-52.28533,-48.21772], "fy":[-57.92649,-54.00176,-53.71125,-57.3971]}, - {"t":0.97421, "x":2.44112, "y":2.60828, "heading":0.68334, "vx":1.91568, "vy":2.48593, "omega":-1.13359, "ax":-3.04292, "ay":-3.43533, "alpha":-0.39547, "fx":[-47.84784,-51.11643,-51.54695,-48.47288], "fy":[-57.81303,-54.93781,-54.57032,-57.32355]}, - {"t":1.00669, "x":2.50173, "y":2.68719, "heading":0.64653, "vx":1.81687, "vy":2.37437, "omega":-1.14643, "ax":-3.03293, "ay":-3.45156, "alpha":-0.30557, "fx":[-48.07824,-50.58188,-51.02842,-48.64215], "fy":[-57.72911,-55.54349,-55.15652,-57.27632]}, - {"t":1.03916, "x":2.55913, "y":2.76248, "heading":0.6093, "vx":1.71838, "vy":2.26228, "omega":-1.15635, "ax":-3.02566, "ay":-3.46305, "alpha":-0.24098, "fx":[-48.24916,-50.20415,-50.64179,-48.75996], "fy":[-57.66257,-55.96471,-55.58432,-57.24577]}, - {"t":1.07163, "x":2.61334, "y":2.83412, "heading":0.57175, "vx":1.62012, "vy":2.14982, "omega":-1.16418, "ax":-3.02013, "ay":-3.47163, "alpha":-0.1923, "fx":[-48.38288,-49.92562,-50.34067,-48.84428], "fy":[-57.60728,-56.27233,-55.91186,-57.22663]}, - {"t":1.10411, "x":2.66436, "y":2.9021, "heading":0.53394, "vx":1.52205, "vy":2.03709, "omega":-1.17042, "ax":-3.01578, "ay":-3.47827, "alpha":-0.15429, "fx":[-48.49161,-49.71397,-50.0983,-48.90541], "fy":[-57.55983,-56.50489,-56.1718,-57.21576]}, - {"t":1.13658, "x":2.71219, "y":2.96642, "heading":0.49593, "vx":1.42411, "vy":1.92413, "omega":-1.17543, "ax":-3.01228, "ay":-3.48356, "alpha":-0.12377, "fx":[-48.5826,-49.54964,-49.89814,-48.94972], "fy":[-57.51817,-56.68518,-56.38389,-57.21113]}, - {"t":1.16906, "x":2.75685, "y":3.02706, "heading":0.45776, "vx":1.32629, "vy":1.81101, "omega":-1.17945, "ax":-3.00939, "ay":-3.48788, "alpha":-0.09875, "fx":[-48.66043,-49.42008,-49.72947,-48.98135], "fy":[-57.48099,-56.82757,-56.56076,-57.2114]}, - {"t":1.20153, "x":2.79833, "y":3.08403, "heading":0.41946, "vx":1.22857, "vy":1.69775, "omega":-1.18266, "ax":-3.00697, "ay":-3.49147, "alpha":-0.07785, "fx":[-48.72812,-49.3169,-49.58499,-49.00316], "fy":[-57.44744,-56.94152,-56.71086,-57.2156]}, - {"t":1.234, "x":2.83664, "y":3.13733, "heading":0.38106, "vx":1.13092, "vy":1.58436, "omega":-1.18519, "ax":-3.00492, "ay":-3.4945, "alpha":-0.06015, "fx":[-48.78776,-49.23421,-49.45962,-49.01715], "fy":[-57.41692,-57.03358,-56.84006,-57.22303]}, - {"t":1.26648, "x":2.87178, "y":3.18693, "heading":0.34257, "vx":1.03334, "vy":1.47088, "omega":-1.18714, "ax":-3.00315, "ay":-3.49709, "alpha":-0.04496, "fx":[-48.84082,-49.16778,-49.34965,-49.0248], "fy":[-57.38901,-57.10841,-56.95257,-57.23316]}, - {"t":1.29895, "x":2.90376, "y":3.23285, "heading":0.30402, "vx":0.93581, "vy":1.35732, "omega":-1.1886, "ax":-3.00161, "ay":-3.49934, "alpha":-0.0318, "fx":[-48.88837,-49.11451,-49.25237,-49.02722], "fy":[-57.36342,-57.1694,-57.05148,-57.24558]}, - {"t":1.33142, "x":2.93256, "y":3.27509, "heading":0.26542, "vx":0.83834, "vy":1.24368, "omega":-1.18963, "ax":-3.81652, "ay":2.35093, "alpha":-0.25802, "fx":[-62.33,-61.35979,-62.47434,-63.40749], "fy":[38.4459,40.09266,38.4359,36.75865]}, - {"t":1.35563, "x":2.95174, "y":3.30588, "heading":0.23663, "vx":0.74596, "vy":1.30059, "omega":-1.19588, "ax":-3.86263, "ay":2.03926, "alpha":-0.33379, "fx":[-63.12796,-61.97421,-63.20101,-64.2831], "fy":[33.11735,35.50522,33.56629,31.16339]}, - {"t":1.37983, "x":2.96866, "y":3.33796, "heading":0.20768, "vx":0.65247, "vy":1.34995, "omega":-1.20396, "ax":-3.57446, "ay":1.59123, "alpha":-0.55012, "fx":[-58.18054,-56.84115,-58.7622,-59.95848], "fy":[25.18977,29.30728,26.80886,22.74815]}, - {"t":1.40404, "x":2.98341, "y":3.3711, "heading":0.17854, "vx":0.56595, "vy":1.38846, "omega":-1.21727, "ax":-1.26851, "ay":0.50096, "alpha":-0.60045, "fx":[-19.0206,-19.40083,-22.4355,-22.09378], "fy":[6.84586,10.34373,9.50364,6.06559]}, - {"t":1.42824, "x":2.99673, "y":3.40485, "heading":0.14908, "vx":0.53525, "vy":1.40059, "omega":-1.23181, "ax":0.00389, "ay":-0.00149, "alpha":1.89466, "fx":[-5.838,-4.26735,5.96427,4.39523], "fy":[4.43397,-6.01954,-4.48171,5.96975]}, - {"t":1.45245, "x":3.00969, "y":3.43875, "heading":0.11926, "vx":0.53534, "vy":1.40055, "omega":-1.18595, "ax":0.19909, "ay":-0.0765, "alpha":4.30276, "fx":[-9.90291,-6.885,16.21779,13.58918], "fy":[9.41072,-14.7391,-11.62899,11.95501]}, - {"t":1.47665, "x":3.02271, "y":3.47263, "heading":0.09055, "vx":0.54016, "vy":1.3987, "omega":-1.0818, "ax":0.18562, "ay":-0.07203, "alpha":6.38358, "fx":[-16.07992,-12.52356,21.77988,18.96166], "fy":[15.28752,-20.82647,-17.04101,17.8698]}, - {"t":1.50086, "x":3.03584, "y":3.50646, "heading":0.06437, "vx":0.54465, "vy":1.39696, "omega":-0.92729, "ax":0.14823, "ay":-0.05801, "alpha":8.10461, "fx":[-21.30461,-17.97742,25.71246,23.26245], "fy":[20.59751,-25.35945,-21.69734,22.66572]}, - {"t":1.52506, "x":3.04906, "y":3.54026, "heading":0.04193, "vx":0.54824, "vy":1.39555, "omega":-0.73112, "ax":0.11835, "ay":-0.04664, "alpha":9.50081, "fx":[-25.29715,-22.64066,28.72142,26.95549], "fy":[25.08686,-28.79521,-25.71787,26.37664]}, - {"t":1.54927, "x":3.06237, "y":3.57402, "heading":0.02423, "vx":0.55111, "vy":1.39442, "omega":-0.50116, "ax":0.09914, "ay":-0.04059, "alpha":10.63316, "fx":[-28.33279,-26.45145,31.12452,30.14247], "fy":[28.77125,-31.50944,-29.14668,29.23052]}, - {"t":1.57347, "x":3.07574, "y":3.60776, "heading":0.0121, "vx":0.55351, "vy":1.39344, "omega":-0.24379, "ax":-0.67302, "ay":-2.11174, "alpha":8.05559, "fx":[-45.34622,-29.85395,11.01706,20.17251], "fy":[-14.03968,-50.03736,-54.96752,-19.04704]}, - {"t":1.59767, "x":3.08894, "y":3.64087, "heading":0.0062, "vx":0.53722, "vy":1.34233, "omega":-0.0488, "ax":-1.66456, "ay":-4.2015, "alpha":0.44006, "fx":[-30.14719,-28.40757,-24.36183,-25.93285], "fy":[-67.46216,-68.31453,-69.82248,-69.14671]}, - {"t":1.62188, "x":3.10145, "y":3.67213, "heading":0.00502, "vx":0.49693, "vy":1.24063, "omega":-0.03815, "ax":-1.6891, "ay":-4.24512, "alpha":0.25384, "fx":[-29.31792,-28.3119,-25.93755,-26.88663], "fy":[-68.70204,-69.15327,-70.06685,-69.67655]}, - {"t":1.64608, "x":3.11298, "y":3.70092, "heading":0.00409, "vx":0.45604, "vy":1.13788, "omega":-0.03201, "ax":-1.69864, "ay":-4.25949, "alpha":0.19016, "fx":[-29.04899,-28.29541,-26.50617,-27.22762], "fy":[-69.11396,-69.44137,-70.13791,-69.84513]}, - {"t":1.67029, "x":3.12352, "y":3.72721, "heading":0.00332, "vx":0.41493, "vy":1.03478, "omega":-0.02741, "ax":-1.70416, "ay":-4.26644, "alpha":0.15803, "fx":[-28.92401,-28.29809,-26.80674,-27.41041], "fy":[-69.31626,-69.58385,-70.16815,-69.92462]}, - {"t":1.69449, "x":3.13307, "y":3.75101, "heading":0.00265, "vx":0.37368, "vy":0.93151, "omega":-0.02358, "ax":-1.70794, "ay":-4.27045, "alpha":0.13866, "fx":[-28.85582,-28.30694,-26.99603,-27.52775], "fy":[-69.43492,-69.66734,-70.18308,-69.96926]}, - {"t":1.7187, "x":3.14161, "y":3.7723, "heading":0.00208, "vx":0.33234, "vy":0.82815, "omega":-0.02022, "ax":-1.71077, "ay":-4.273, "alpha":0.12572, "fx":[-28.815,-28.31765,-27.12774,-27.61096], "fy":[-69.51215,-69.72143,-70.19097,-69.99701]}, - {"t":1.7429, "x":3.14916, "y":3.7911, "heading":0.00159, "vx":0.29093, "vy":0.72472, "omega":-0.01718, "ax":-1.71299, "ay":-4.27475, "alpha":0.11645, "fx":[-28.78895,-28.3285,-27.22536,-27.67367], "fy":[-69.56606,-69.75896,-70.19524,-70.01552]}, - {"t":1.76711, "x":3.1557, "y":3.80739, "heading":0.00118, "vx":0.24947, "vy":0.62126, "omega":-0.01436, "ax":-1.71478, "ay":-4.276, "alpha":0.10949, "fx":[-28.77145,-28.33872,-27.30084,-27.72284], "fy":[-69.60566,-69.78637,-70.19757,-70.02853]}, - {"t":1.79131, "x":3.16123, "y":3.82117, "heading":0.00083, "vx":0.20796, "vy":0.51776, "omega":-0.01171, "ax":-1.71626, "ay":-4.27695, "alpha":0.10407, "fx":[-28.75915,-28.34798,-27.36098,-27.76245], "fy":[-69.63594,-69.80721,-70.19882,-70.0381]}, - {"t":1.81552, "x":3.16576, "y":3.83245, "heading":0.00055, "vx":0.16642, "vy":0.41424, "omega":-0.00919, "ax":-1.7175, "ay":-4.27769, "alpha":0.09973, "fx":[-28.7501,-28.35618,-27.40996,-27.79496], "fy":[-69.65984,-69.8236,-70.19949,-70.04542]}, - {"t":1.83972, "x":3.16929, "y":3.84122, "heading":0.00033, "vx":0.12485, "vy":0.3107, "omega":-0.00678, "ax":-1.71853, "ay":-4.27828, "alpha":0.09618, "fx":[-28.74314,-28.36332,-27.4505,-27.82203], "fy":[-69.67922,-69.83687,-70.19984,-70.05121]}, - {"t":1.86392, "x":3.17181, "y":3.84749, "heading":0.00016, "vx":0.08325, "vy":0.20714, "omega":-0.00445, "ax":-1.71941, "ay":-4.27877, "alpha":0.09322, "fx":[-28.73754,-28.36945,-27.48452,-27.84482], "fy":[-69.69528,-69.84786,-70.20004,-70.05595]}, - {"t":1.88813, "x":3.17332, "y":3.85125, "heading":0.00005, "vx":0.04164, "vy":0.10358, "omega":-0.0022, "ax":-1.72016, "ay":-4.27918, "alpha":0.09072, "fx":[-28.73289,-28.3747,-27.5134,-27.8642], "fy":[-69.70885,-69.85715,-70.20017,-70.0599]}, - {"t":1.91233, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.91939, "ay":3.57297, "alpha":-0.16326, "fx":[48.37039,47.00801,47.06734,48.45991], "fy":[57.88742,58.99896,58.94891,57.80972]}, + {"t":0.0333, "x":1.123, "y":0.996, "heading":0.942, "vx":0.09721, "vy":0.11898, "omega":-0.00544, "ax":2.91887, "ay":3.57306, "alpha":-0.16632, "fx":[48.37405,46.98637,47.04632,48.46533], "fy":[57.87922,59.01114,58.96045,57.79985]}, + {"t":0.0666, "x":1.12786, "y":1.00194, "heading":0.94182, "vx":0.19441, "vy":0.23796, "omega":-0.01097, "ax":2.91831, "ay":3.57315, "alpha":-0.16966, "fx":[48.37817,46.9629,47.02325,48.47114], "fy":[57.87017,59.02432,58.97315,57.78918]}, + {"t":0.0999, "x":1.13595, "y":1.01185, "heading":0.94145, "vx":0.29159, "vy":0.35694, "omega":-0.01662, "ax":2.9177, "ay":3.57325, "alpha":-0.17332, "fx":[48.38281,46.93731,46.99783,48.47738], "fy":[57.86016,59.03864,58.98717,57.77758]}, + {"t":0.1332, "x":1.14728, "y":1.02571, "heading":0.9409, "vx":0.38874, "vy":0.47592, "omega":-0.0224, "ax":2.91702, "ay":3.57337, "alpha":-0.17734, "fx":[48.38804,46.90929,46.96971,48.48413], "fy":[57.84904,59.05429,59.00269,57.76491]}, + {"t":0.1665, "x":1.16184, "y":1.04354, "heading":0.94015, "vx":0.48588, "vy":0.59491, "omega":-0.0283, "ax":2.91628, "ay":3.57349, "alpha":-0.18179, "fx":[48.39394,46.87846,46.93848,48.49149], "fy":[57.83665,59.07146,59.01995,57.75099]}, + {"t":0.19979, "x":1.17963, "y":1.06533, "heading":0.93921, "vx":0.58299, "vy":0.71391, "omega":-0.03435, "ax":2.91545, "ay":3.57363, "alpha":-0.18674, "fx":[48.40062,46.84433,46.90361,48.49955], "fy":[57.82278,59.09042,59.03922,57.7356]}, + {"t":0.23309, "x":1.20066, "y":1.09109, "heading":0.93807, "vx":0.68007, "vy":0.83291, "omega":-0.04057, "ax":2.91452, "ay":3.57378, "alpha":-0.19226, "fx":[48.40821,46.80632,46.86449,48.50846], "fy":[57.8072,59.11149,59.06085,57.71848]}, + {"t":0.26639, "x":1.22492, "y":1.12081, "heading":0.93672, "vx":0.77712, "vy":0.95191, "omega":-0.04698, "ax":2.91348, "ay":3.57395, "alpha":-0.19848, "fx":[48.41685,46.76369,46.82031,48.51838], "fy":[57.78958,59.13506,59.08528,57.6993]}, + {"t":0.29969, "x":1.25242, "y":1.15448, "heading":0.93515, "vx":0.87413, "vy":1.07092, "omega":-0.05358, "ax":2.91229, "ay":3.57414, "alpha":-0.20553, "fx":[48.42674,46.71553,46.77006,48.52952], "fy":[57.76953,59.16162,59.11304,57.67763]}, + {"t":0.33299, "x":1.28314, "y":1.19213, "heading":0.93337, "vx":0.97111, "vy":1.18993, "omega":-0.06043, "ax":2.91094, "ay":3.57436, "alpha":-0.21358, "fx":[48.43815,46.66063,46.71244,48.54216], "fy":[57.74655,59.19182,59.14484,57.65291]}, + {"t":0.36629, "x":1.31709, "y":1.23373, "heading":0.93136, "vx":1.06804, "vy":1.30896, "omega":-0.06754, "ax":2.90938, "ay":3.57461, "alpha":-0.22288, "fx":[48.45139,46.59746,46.64574,48.55666], "fy":[57.71997,59.22647,59.18161,57.62445]}, + {"t":0.39959, "x":1.35427, "y":1.2793, "heading":0.92911, "vx":1.16492, "vy":1.42799, "omega":-0.07496, "ax":2.90756, "ay":3.5749, "alpha":-0.23372, "fx":[48.46692,46.52394,46.5677,48.57349], "fy":[57.68893,59.26669,59.22454,57.59127]}, + {"t":0.43289, "x":1.39467, "y":1.32883, "heading":0.92661, "vx":1.26174, "vy":1.54703, "omega":-0.08274, "ax":2.9054, "ay":3.57524, "alpha":-0.24654, "fx":[48.48532,46.43728,46.47519,48.59331], "fy":[57.65222,59.31394,59.27532,57.55207]}, + {"t":0.46619, "x":1.4383, "y":1.38233, "heading":0.92386, "vx":1.35849, "vy":1.66608, "omega":-0.09095, "ax":2.90281, "ay":3.57564, "alpha":-0.26192, "fx":[48.50742,46.33356,46.36383,48.61703], "fy":[57.60818,59.3703,59.33625,57.50501]}, + {"t":0.49949, "x":1.48514, "y":1.43979, "heading":0.92083, "vx":1.45515, "vy":1.78515, "omega":-0.09968, "ax":2.89965, "ay":3.57612, "alpha":-0.28073, "fx":[48.53443,46.20715,46.22726,48.64598], "fy":[57.55442,59.43869,59.41068,57.44743]}, + {"t":0.53278, "x":1.5352, "y":1.50122, "heading":0.91751, "vx":1.5517, "vy":1.90423, "omega":-0.10902, "ax":2.89569, "ay":3.57671, "alpha":-0.30423, "fx":[48.56813,46.04964,46.05591,48.68215], "fy":[57.48737,59.52348,59.50359,57.3753]}, + {"t":0.56608, "x":1.58848, "y":1.56661, "heading":0.91388, "vx":1.64813, "vy":2.02333, "omega":-0.11915, "ax":2.89059, "ay":3.57745, "alpha":-0.33445, "fx":[48.61127,45.84793,45.83462,48.72867], "fy":[57.40145,59.63138,59.62279,57.2823]}, + {"t":0.59938, "x":1.64496, "y":1.63597, "heading":0.90991, "vx":1.74438, "vy":2.14245, "omega":-0.13029, "ax":2.88378, "ay":3.5784, "alpha":-0.37474, "fx":[48.66843,45.58035,45.53794,48.79077], "fy":[57.28746,59.77335,59.78119,57.15777]}, + {"t":0.63268, "x":1.70465, "y":1.70929, "heading":0.90557, "vx":1.84041, "vy":2.26161, "omega":-0.14277, "ax":2.87424, "ay":3.57965, "alpha":-0.43113, "fx":[48.74767,45.20835,45.1196,48.87788], "fy":[57.12903,59.96852,60.00179,56.98242]}, + {"t":0.66598, "x":1.76753, "y":1.78659, "heading":0.90082, "vx":1.93612, "vy":2.38081, "omega":-0.15713, "ax":2.8599, "ay":3.58138, "alpha":-0.51565, "fx":[48.86471,44.65614,44.48577,49.00888], "fy":[56.89399,60.25358,60.32991,56.71718]}, + {"t":0.69928, "x":1.83358, "y":1.86785, "heading":0.89558, "vx":2.03135, "vy":2.50007, "omega":-0.1743, "ax":2.83592, "ay":3.58384, "alpha":-0.6563, "fx":[49.05481,43.75159,43.41327,49.22789], "fy":[56.50937,60.70859,60.86854,56.26934]}, + {"t":0.73258, "x":1.9028, "y":1.95309, "heading":0.88978, "vx":2.12578, "vy":2.61941, "omega":-0.19615, "ax":2.78774, "ay":3.58722, "alpha":-0.93678, "fx":[49.41616,42.00279,41.21131,49.667], "fy":[55.76691,61.547,61.91043,55.35276]}, + {"t":0.76588, "x":1.97513, "y":2.0423, "heading":0.88325, "vx":2.21861, "vy":2.73886, "omega":-0.22734, "ax":2.64217, "ay":3.58506, "alpha":-1.77334, "fx":[50.36004,37.23191,34.20914,50.9767], "fy":[53.74271,63.56893,64.68414,52.43971]}, + {"t":0.79918, "x":2.05047, "y":2.13549, "heading":0.87568, "vx":2.30659, "vy":2.85824, "omega":-0.2864, "ax":-0.19101, "ay":0.85926, "alpha":-15.65567, "fx":[57.11884,-8.79183,-62.54868,1.73121], "fy":[29.27025,66.40467,17.93266,-57.41844]}, + {"t":0.83248, "x":2.12717, "y":2.23114, "heading":0.86614, "vx":2.30023, "vy":2.88685, "omega":-0.80771, "ax":-3.00913, "ay":-3.26547, "alpha":-1.80257, "fx":[-41.43195,-56.89172,-55.36033,-43.09038], "fy":[-60.00068,-45.66056,-48.3497,-59.52585]}, + {"t":0.86578, "x":2.2021, "y":2.32546, "heading":0.83925, "vx":2.20003, "vy":2.77811, "omega":-0.86774, "ax":-2.9793, "ay":-3.42861, "alpha":-0.83721, "fx":[-45.0095,-52.18838,-52.0042,-45.62139], "fy":[-59.15017,-52.92053,-53.28736,-58.84676]}, + {"t":0.89907, "x":2.27371, "y":2.41607, "heading":0.81035, "vx":2.10082, "vy":2.66394, "omega":-0.89562, "ax":-2.96513, "ay":-3.47831, "alpha":-0.52854, "fx":[-46.08497,-50.59106,-50.69646,-46.52426], "fy":[-58.85298,-55.02293,-55.00215,-58.57711]}, + {"t":0.93237, "x":2.34202, "y":2.50285, "heading":0.78053, "vx":2.00209, "vy":2.54812, "omega":-0.91322, "ax":-2.95735, "ay":-3.50231, "alpha":-0.3753, "fx":[-46.61042,-49.79256,-49.99694,-46.98811], "fy":[-58.69666,-56.01868,-55.87638,-58.43248]}, + {"t":0.96567, "x":2.40705, "y":2.58576, "heading":0.75012, "vx":1.90361, "vy":2.43149, "omega":-0.92571, "ax":-2.95247, "ay":-3.51643, "alpha":-0.28334, "fx":[-46.92589,-49.31514,-49.55815,-47.26959], "fy":[-58.59725,-56.59804,-56.40927,-58.34311]}, + {"t":0.99897, "x":2.4688, "y":2.66477, "heading":0.71929, "vx":1.8053, "vy":2.3144, "omega":-0.93515, "ax":-2.94912, "ay":-3.52573, "alpha":-0.22191, "fx":[-47.13875,-48.99893,-49.25506,-47.45739], "fy":[-58.52664,-56.97582,-56.76998,-58.28338]}, + {"t":1.03227, "x":2.52728, "y":2.73989, "heading":0.68815, "vx":1.70709, "vy":2.197, "omega":-0.94254, "ax":-2.94669, "ay":-3.53232, "alpha":-0.17791, "fx":[-47.29367,-48.7753,-49.03165,-47.59046], "fy":[-58.47273,-57.24057,-57.03164,-58.24157]}, + {"t":1.06557, "x":2.58249, "y":2.81108, "heading":0.65677, "vx":1.60897, "vy":2.07937, "omega":-0.94846, "ax":-2.94484, "ay":-3.53722, "alpha":-0.14481, "fx":[-47.41258,-48.60988,-48.8591,-47.68866], "fy":[-58.42945,-57.43549,-57.23104,-58.21153]}, + {"t":1.09887, "x":2.63443, "y":2.87836, "heading":0.62519, "vx":1.51091, "vy":1.96159, "omega":-0.95328, "ax":-2.94339, "ay":-3.54102, "alpha":-0.11901, "fx":[-47.50751,-48.48351,-48.72107,-47.76317], "fy":[-58.3934,-57.58418,-57.38868,-58.18969]}, + {"t":1.13217, "x":2.68311, "y":2.94172, "heading":0.59344, "vx":1.4129, "vy":1.84368, "omega":-0.95725, "ax":-2.94222, "ay":-3.54405, "alpha":-0.09831, "fx":[-47.58562,-48.38468,-48.6076,-47.8208], "fy":[-58.36255,-57.70064,-57.51691,-58.17385]}, + {"t":1.16547, "x":2.72853, "y":3.00115, "heading":0.56157, "vx":1.31493, "vy":1.72566, "omega":-0.96052, "ax":-2.94125, "ay":-3.54652, "alpha":-0.08135, "fx":[-47.65142,-48.30604,-48.51226,-47.86594], "fy":[-58.33558,-57.79369,-57.62362,-58.16256]}, + {"t":1.19877, "x":2.77069, "y":3.05664, "heading":0.52958, "vx":1.21699, "vy":1.60757, "omega":-0.96323, "ax":-2.94045, "ay":-3.54857, "alpha":-0.06718, "fx":[-47.70789,-48.24266,-48.43074,-47.90154], "fy":[-58.31164,-57.86917,-57.71405,-58.15481]}, + {"t":1.23206, "x":2.80958, "y":3.10821, "heading":0.49751, "vx":1.11907, "vy":1.4894, "omega":-0.96547, "ax":-2.93976, "ay":-3.55031, "alpha":-0.05519, "fx":[-47.7571,-48.19112,-48.36002,-47.92968], "fy":[-58.29011,-57.93113,-57.79185,-58.1499]}, + {"t":1.26536, "x":2.84521, "y":3.15584, "heading":0.46536, "vx":1.02118, "vy":1.37118, "omega":-0.9673, "ax":-2.93917, "ay":-3.55179, "alpha":-0.04489, "fx":[-47.80051,-48.14899,-48.29795,-47.95185], "fy":[-58.27058,-57.98242,-57.85962,-58.14731]}, + {"t":1.29866, "x":2.87759, "y":3.19953, "heading":0.43315, "vx":0.92331, "vy":1.25291, "omega":-0.9688, "ax":-2.93866, "ay":-3.55307, "alpha":-0.03597, "fx":[-47.83919,-48.11443,-48.24292,-47.96916], "fy":[-58.25274,-58.02516,-57.91928,-58.14665]}, + {"t":1.33196, "x":2.90671, "y":3.23928, "heading":0.40089, "vx":0.82546, "vy":1.1346, "omega":-0.97, "ax":-2.9382, "ay":-3.55419, "alpha":-0.02816, "fx":[-47.87393,-48.0861,-48.19374,-47.98244], "fy":[-58.23634,-58.06092,-57.97226,-58.1476]}, + {"t":1.36526, "x":2.93256, "y":3.27509, "heading":0.36859, "vx":0.72762, "vy":1.01624, "omega":-0.97093, "ax":-3.70783, "ay":2.38395, "alpha":-0.46714, "fx":[-60.22353,-58.75353,-61.04054,-62.44625], "fy":[39.34989,41.85646,38.67566,36.01005]}, + {"t":1.39182, "x":2.95058, "y":3.30291, "heading":0.3428, "vx":0.62915, "vy":1.07955, "omega":-0.98334, "ax":-3.64564, "ay":1.9166, "alpha":-0.92419, "fx":[-58.93726,-56.53613,-60.4259,-62.49761], "fy":[31.23966,37.01893,31.56461,25.508]}, + {"t":1.41837, "x":2.966, "y":3.33226, "heading":0.31669, "vx":0.53234, "vy":1.13045, "omega":-1.00788, "ax":-2.64566, "ay":1.14839, "alpha":-2.67439, "fx":[-37.78713,-37.21506,-48.73048,-49.27361], "fy":[15.09043,32.01479,22.13747,5.85301]}, + {"t":1.44493, "x":2.9792, "y":3.36268, "heading":0.28993, "vx":0.46209, "vy":1.16094, "omega":-1.0789, "ax":-0.54339, "ay":0.21241, "alpha":-4.2358, "fx":[5.70356,-1.48195,-22.76198,-16.99302], "fy":[-4.75829,18.58393,11.18526,-11.12092]}, + {"t":1.47148, "x":2.99128, "y":3.39358, "heading":0.26128, "vx":0.44766, "vy":1.16658, "omega":-1.19138, "ax":0.07099, "ay":-0.02731, "alpha":-2.71268, "fx":[10.2494,6.38144,-7.96615,-4.0223], "fy":[-5.82701,8.76402,4.96478,-9.68773]}, + {"t":1.49804, "x":3.0032, "y":3.42455, "heading":0.22964, "vx":0.44954, "vy":1.16586, "omega":-1.26342, "ax":0.13728, "ay":-0.05318, "alpha":-0.94102, "fx":[5.33597,4.15372,-0.85591,0.34306], "fy":[-2.83596,2.27121,1.10482,-4.01772]}, + {"t":1.52459, "x":3.01518, "y":3.45549, "heading":0.19609, "vx":0.45319, "vy":1.16444, "omega":-1.28841, "ax":0.11671, "ay":-0.0456, "alpha":0.87217, "fx":[-0.90229,0.04857,4.71259,3.77323], "fy":[1.17926,-3.60115,-2.66432,2.10409]}, + {"t":1.55115, "x":3.02726, "y":3.4864, "heading":0.16188, "vx":0.45629, "vy":1.16323, "omega":-1.26525, "ax":0.092, "ay":-0.0362, "alpha":2.64552, "fx":[-6.82958,-4.41718,9.79886,7.46405], "fy":[5.54942,-9.06648,-6.68681,7.83664]}, + {"t":1.5777, "x":3.03941, "y":3.51728, "heading":0.12828, "vx":0.45873, "vy":1.16227, "omega":-1.19499, "ax":0.07318, "ay":-0.02896, "alpha":4.30333, "fx":[-12.02652,-8.88468,14.34405,11.35276], "fy":[9.9795,-13.93708,-10.8238,12.88791]}, + {"t":1.60426, "x":3.05161, "y":3.54813, "heading":0.09654, "vx":0.46067, "vy":1.1615, "omega":-1.08072, "ax":0.05927, "ay":-0.02355, "alpha":5.79995, "fx":[-16.40121,-13.18429,18.23763,15.22342], "fy":[14.27013,-18.09674,-14.883,17.16943]}, + {"t":1.63081, "x":3.06387, "y":3.57897, "heading":0.06785, "vx":0.46225, "vy":1.16088, "omega":-0.9267, "ax":0.04871, "ay":-0.01943, "alpha":7.1206, "fx":[-20.00344,-17.19814,21.48148,18.90534], "fy":[18.28186,-21.55554,-18.71567,20.71894]}, + {"t":1.65737, "x":3.07616, "y":3.60979, "heading":0.04324, "vx":0.46354, "vy":1.16036, "omega":-0.73761, "ax":0.04085, "ay":-0.01634, "alpha":8.27167, "fx":[-22.95211,-20.84037,24.1691,22.29472], "fy":[21.92538,-24.41363,-22.22593,23.64557]}, + {"t":1.68392, "x":3.08848, "y":3.64059, "heading":0.02365, "vx":0.46463, "vy":1.15993, "omega":-0.51796, "ax":0.03643, "ay":-0.01486, "alpha":9.27005, "fx":[-25.38219,-24.03554,26.44848,25.35141], "fy":[25.14297,-26.82686,-25.36231,26.07468]}, + {"t":1.71048, "x":3.10084, "y":3.67139, "heading":0.0099, "vx":0.46559, "vy":1.15953, "omega":-0.27179, "ax":-0.3495, "ay":-1.00775, "alpha":9.30609, "fx":[-36.79367,-28.04746,17.75166,24.23462], "fy":[7.25963,-39.86105,-42.4654,9.16752]}, + {"t":1.73703, "x":3.11308, "y":3.70183, "heading":0.00268, "vx":0.45631, "vy":1.13277, "omega":-0.02467, "ax":-1.69457, "ay":-4.22029, "alpha":0.24467, "fx":[-29.3347,-28.37449,-26.09739,-27.00538], "fy":[-68.31646,-68.76259,-69.64284,-69.25266]}, + {"t":1.76359, "x":3.1246, "y":3.73042, "heading":0.00202, "vx":0.41131, "vy":1.0207, "omega":-0.01817, "ax":-1.71015, "ay":-4.25293, "alpha":0.13735, "fx":[-28.87903,-28.33795,-27.04476,-27.56918], "fy":[-69.15032,-69.38571,-69.89531,-69.67801]}, + {"t":1.79014, "x":3.13492, "y":3.75603, "heading":0.00154, "vx":0.3659, "vy":0.90777, "omega":-0.01452, "ax":-1.7159, "ay":-4.2636, "alpha":0.10155, "fx":[-28.73412,-28.33373,-27.37383,-27.76507], "fy":[-69.42361,-69.59388,-69.97472,-69.81443]}, + {"t":1.8167, "x":3.14403, "y":3.77863, "heading":0.00115, "vx":0.32033, "vy":0.79454, "omega":-0.01182, "ax":-1.71909, "ay":-4.2688, "alpha":0.08367, "fx":[-28.6665,-28.33658,-27.54422,-27.86792], "fy":[-69.55789,-69.69659,-70.0121,-69.88019]}, + {"t":1.84325, "x":3.15193, "y":3.79822, "heading":0.00084, "vx":0.27468, "vy":0.68119, "omega":-0.0096, "ax":-1.72119, "ay":-4.27184, "alpha":0.07294, "fx":[-28.6289,-28.34129,-27.64973,-27.93261], "fy":[-69.6371,-69.7572,-70.0332,-69.91828]}, + {"t":1.86981, "x":3.15861, "y":3.81481, "heading":0.00059, "vx":0.22898, "vy":0.56775, "omega":-0.00767, "ax":-1.7227, "ay":-4.27382, "alpha":0.06579, "fx":[-28.60562,-28.34623,-27.72198,-27.97752], "fy":[-69.68913,-69.79696,-70.04649,-69.94288]}, + {"t":1.89636, "x":3.16409, "y":3.82838, "heading":0.00038, "vx":0.18323, "vy":0.45426, "omega":-0.00592, "ax":-1.72384, "ay":-4.27521, "alpha":0.06068, "fx":[-28.59003,-28.35078,-27.77466,-28.01063], "fy":[-69.72584,-69.82498,-70.05554,-69.96]}, + {"t":1.92292, "x":3.16835, "y":3.83893, "heading":0.00023, "vx":0.13745, "vy":0.34073, "omega":-0.00431, "ax":-1.72473, "ay":-4.27624, "alpha":0.05685, "fx":[-28.57888,-28.35474,-27.81474,-28.036], "fy":[-69.75313,-69.84579,-70.06209,-69.97259]}, + {"t":1.94947, "x":3.17139, "y":3.84647, "heading":0.00011, "vx":0.09165, "vy":0.22717, "omega":-0.0028, "ax":-1.72544, "ay":-4.27703, "alpha":0.05388, "fx":[-28.57046,-28.35806,-27.84619,-28.056], "fy":[-69.77424,-69.86189,-70.06708,-69.98227]}, + {"t":1.97603, "x":3.17321, "y":3.851, "heading":0.00004, "vx":0.04583, "vy":0.11359, "omega":-0.00137, "ax":-1.72601, "ay":-4.27767, "alpha":0.05149, "fx":[-28.56382,-28.3608,-27.87143,-28.07209], "fy":[-69.79109,-69.87474,-70.07103,-69.98997]}, + {"t":2.00258, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PROtoC.traj b/src/main/deploy/choreo/PROtoC.traj index 6a834004..b079e800 100644 --- a/src/main/deploy/choreo/PROtoC.traj +++ b/src/main/deploy/choreo/PROtoC.traj @@ -10,7 +10,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,59 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.21555,1.79446], + "waypoints":[0.0,1.2487,1.88374], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.04231, "ay":3.47028, "alpha":0.01308, "fx":[49.68719,49.79422,49.78468,49.67782], "fy":[56.77512,56.68129,56.68987,56.78351]}, - {"t":0.03799, "x":1.63553, "y":0.62718, "heading":0.942, "vx":0.11556, "vy":0.13182, "omega":0.0005, "ax":3.04214, "ay":3.47009, "alpha":0.01333, "fx":[49.6834,49.79246,49.78274,49.67386], "fy":[56.77277,56.67716,56.68591,56.78133]}, - {"t":0.07597, "x":1.64211, "y":0.63469, "heading":0.94202, "vx":0.23112, "vy":0.26364, "omega":0.001, "ax":3.04194, "ay":3.46987, "alpha":0.01361, "fx":[49.67918,49.79051,49.78059,49.66946], "fy":[56.77017,56.67257,56.68152,56.7789]}, - {"t":0.11396, "x":1.65309, "y":0.64721, "heading":0.94206, "vx":0.34667, "vy":0.39544, "omega":0.00152, "ax":3.04172, "ay":3.46963, "alpha":0.01391, "fx":[49.67448,49.78833,49.77819,49.66454], "fy":[56.76725,56.66744,56.6766,56.77619]}, - {"t":0.15194, "x":1.66845, "y":0.66473, "heading":0.94211, "vx":0.46222, "vy":0.52724, "omega":0.00205, "ax":3.04148, "ay":3.46936, "alpha":0.01426, "fx":[49.66918,49.78587,49.77548,49.659], "fy":[56.76397,56.66167,56.67108,56.77315]}, - {"t":0.18993, "x":1.6882, "y":0.68726, "heading":0.94219, "vx":0.57775, "vy":0.65903, "omega":0.00259, "ax":3.0412, "ay":3.46906, "alpha":0.01466, "fx":[49.66318,49.78309,49.77241,49.65272], "fy":[56.76024,56.65513,56.66481,56.76969]}, - {"t":0.22792, "x":1.71234, "y":0.7148, "heading":0.94229, "vx":0.69327, "vy":0.7908, "omega":0.00315, "ax":3.04088, "ay":3.46871, "alpha":0.01511, "fx":[49.65632,49.77991,49.76889,49.64554], "fy":[56.75599,56.64765,56.65765,56.76574]}, - {"t":0.2659, "x":1.74087, "y":0.74734, "heading":0.94241, "vx":0.80878, "vy":0.92256, "omega":0.00372, "ax":3.04052, "ay":3.4683, "alpha":0.01563, "fx":[49.6484,49.77624,49.76484,49.63725], "fy":[56.75108,56.63901,56.64939,56.76119]}, - {"t":0.30389, "x":1.77379, "y":0.78489, "heading":0.94255, "vx":0.92428, "vy":1.05431, "omega":0.00431, "ax":3.04009, "ay":3.46783, "alpha":0.01624, "fx":[49.63916,49.77197,49.76011,49.62758], "fy":[56.74534,56.62893,56.63975,56.75588]}, - {"t":0.34187, "x":1.81109, "y":0.82744, "heading":0.94272, "vx":1.03976, "vy":1.18604, "omega":0.00493, "ax":3.03959, "ay":3.46728, "alpha":0.01696, "fx":[49.62823,49.76691,49.75452,49.61615], "fy":[56.73856,56.61701,56.62836,56.7496]}, - {"t":0.37986, "x":1.85278, "y":0.87499, "heading":0.9429, "vx":1.15522, "vy":1.31774, "omega":0.00558, "ax":3.03898, "ay":3.46661, "alpha":0.01782, "fx":[49.61511,49.76083,49.74781,49.60242], "fy":[56.73042,56.6027,56.61469,56.74206]}, - {"t":0.41784, "x":1.89885, "y":0.92755, "heading":0.94311, "vx":1.27066, "vy":1.44943, "omega":0.00625, "ax":3.03824, "ay":3.46579, "alpha":0.01888, "fx":[49.59907,49.75341,49.73961,49.58564], "fy":[56.72047,56.5852,56.59797,56.73285]}, - {"t":0.45583, "x":1.94931, "y":0.98511, "heading":0.94335, "vx":1.38607, "vy":1.58108, "omega":0.00697, "ax":3.03731, "ay":3.46477, "alpha":0.0202, "fx":[49.57901,49.74413,49.72935,49.56466], "fy":[56.70802,56.56331,56.57706,56.72133]}, - {"t":0.49382, "x":2.00416, "y":1.04767, "heading":0.94362, "vx":1.50144, "vy":1.71269, "omega":0.00774, "ax":3.03612, "ay":3.46346, "alpha":0.0219, "fx":[49.55318,49.73218,49.71616,49.53767], "fy":[56.692,56.53514,56.55017,56.70651]}, - {"t":0.5318, "x":2.06338, "y":1.11522, "heading":0.94391, "vx":1.61677, "vy":1.84425, "omega":0.00857, "ax":3.03453, "ay":3.46171, "alpha":0.02418, "fx":[49.51872,49.71624,49.69857,49.50166], "fy":[56.67062,56.49754,56.5143,56.68675]}, - {"t":0.56979, "x":2.12698, "y":1.18778, "heading":0.94424, "vx":1.73204, "vy":1.97575, "omega":0.00949, "ax":3.0323, "ay":3.45925, "alpha":0.02737, "fx":[49.47039,49.69389,49.67393,49.45121], "fy":[56.64065,56.44482,56.46406,56.65906]}, - {"t":0.60777, "x":2.19496, "y":1.26532, "heading":0.9446, "vx":1.84723, "vy":2.10715, "omega":0.01053, "ax":3.02895, "ay":3.45556, "alpha":0.03217, "fx":[49.39776,49.66031,49.63694,49.37546], "fy":[56.5956,56.3656,56.38862,56.61749]}, - {"t":0.64576, "x":2.26732, "y":1.34786, "heading":0.945, "vx":1.96228, "vy":2.23841, "omega":0.01175, "ax":3.02336, "ay":3.4494, "alpha":0.04021, "fx":[49.27633,49.60416,49.57523,49.24907], "fy":[56.52028,56.23318,56.26276,56.5481]}, - {"t":0.68375, "x":2.34404, "y":1.43537, "heading":0.94544, "vx":2.07713, "vy":2.36944, "omega":0.01328, "ax":3.01213, "ay":3.43702, "alpha":0.05642, "fx":[49.03217,49.49115,49.45148,48.99578], "fy":[56.36868,55.96697,56.01061,56.40886]}, - {"t":0.72173, "x":2.42511, "y":1.52786, "heading":0.94595, "vx":2.19155, "vy":2.5, "omega":0.01542, "ax":2.97811, "ay":3.39952, "alpha":0.10592, "fx":[48.28977,49.14591,49.07746,48.23264], "fy":[55.90589,55.158,55.25129,55.98727]}, - {"t":0.75972, "x":2.51051, "y":1.62528, "heading":0.94653, "vx":2.30467, "vy":2.62913, "omega":0.01944, "ax":-1.36098, "ay":-1.50717, "alpha":2.87407, "fx":[-32.5474,-18.85456,-10.60506,-26.99077], "fy":[-24.62425,-35.2734,-25.65908,-13.00052]}, - {"t":0.7977, "x":2.59707, "y":1.72406, "heading":0.94727, "vx":2.25298, "vy":2.57188, "omega":0.12862, "ax":-2.98382, "ay":-3.40096, "alpha":0.08748, "fx":[-49.10221,-48.40352,-48.45361,-49.16], "fy":[-55.33068,-55.94047,-55.87179,-55.25386]}, - {"t":0.83569, "x":2.6805, "y":1.8193, "heading":0.95216, "vx":2.13963, "vy":2.44269, "omega":0.13194, "ax":-3.01422, "ay":-3.43684, "alpha":0.04114, "fx":[-49.42791,-49.09554,-49.12479,-49.45889], "fy":[-56.05668,-56.34738,-56.31579,-56.02324]}, - {"t":0.87367, "x":2.7596, "y":1.90961, "heading":0.95717, "vx":2.02514, "vy":2.31214, "omega":0.1335, "ax":-3.02458, "ay":-3.44907, "alpha":0.02529, "fx":[-49.53856,-49.33352,-49.35338,-49.55908], "fy":[-56.30611,-56.48557,-56.4657,-56.28553]}, - {"t":0.91166, "x":2.83435, "y":1.99495, "heading":0.96224, "vx":1.91024, "vy":2.18113, "omega":0.13446, "ax":-3.0298, "ay":-3.45524, "alpha":0.01724, "fx":[-49.59417,-49.45417,-49.46871,-49.60901], "fy":[-56.43234,-56.55492,-56.54092,-56.41801]}, - {"t":0.94965, "x":2.90472, "y":2.07531, "heading":0.96735, "vx":1.79515, "vy":2.04988, "omega":0.13512, "ax":-3.03295, "ay":-3.45895, "alpha":0.01235, "fx":[-49.6276,-49.5272,-49.53826,-49.6388], "fy":[-56.5086,-56.59652,-56.5861,-56.49801]}, - {"t":0.98763, "x":2.97073, "y":2.15068, "heading":0.97248, "vx":1.67995, "vy":1.91849, "omega":0.13559, "ax":-3.03505, "ay":-3.46144, "alpha":0.00906, "fx":[-49.6499,-49.57622,-49.58477,-49.65853], "fy":[-56.55965,-56.62419,-56.61625,-56.55162]}, - {"t":1.02562, "x":3.03235, "y":2.22106, "heading":0.97763, "vx":1.56466, "vy":1.787, "omega":0.13593, "ax":-3.03656, "ay":-3.46322, "alpha":0.00669, "fx":[-49.66586,-49.61143,-49.61805,-49.67252], "fy":[-56.59621,-56.64389,-56.6378,-56.59007]}, - {"t":1.0636, "x":3.08959, "y":2.28644, "heading":0.98279, "vx":1.44931, "vy":1.65545, "omega":0.13619, "ax":-3.03769, "ay":-3.46455, "alpha":0.00491, "fx":[-49.67784,-49.63796,-49.64304,-49.68294], "fy":[-56.62367,-56.65861,-56.65398,-56.61902]}, - {"t":1.10159, "x":3.14246, "y":2.34682, "heading":0.98797, "vx":1.33392, "vy":1.52384, "omega":0.13637, "ax":-3.03857, "ay":-3.46559, "alpha":0.00351, "fx":[-49.68718,-49.6587,-49.66248,-49.69098], "fy":[-56.64505,-56.67001,-56.66658,-56.64161]}, - {"t":1.13958, "x":3.19093, "y":2.40221, "heading":0.99315, "vx":1.2185, "vy":1.3922, "omega":0.13651, "ax":-3.03927, "ay":-3.46642, "alpha":0.00238, "fx":[-49.69468,-49.67535,-49.67802,-49.69736], "fy":[-56.66216,-56.6791,-56.67669,-56.65974]}, - {"t":1.17756, "x":3.23503, "y":2.45259, "heading":0.99833, "vx":1.10305, "vy":1.26052, "omega":0.1366, "ax":-3.03985, "ay":-3.4671, "alpha":0.00145, "fx":[-49.70084,-49.68903,-49.69072,-49.70253], "fy":[-56.67615,-56.6865,-56.68497,-56.67462]}, - {"t":1.21555, "x":3.27473, "y":2.49797, "heading":1.00352, "vx":0.98758, "vy":1.12882, "omega":0.13665, "ax":-0.26089, "ay":0.21318, "alpha":0.3632, "fx":[-5.65174,-3.95191,-2.87666,-4.57991], "fy":[3.18727,2.10265,3.78654,4.86415]}, - {"t":1.24771, "x":3.30636, "y":2.53439, "heading":1.00792, "vx":0.97919, "vy":1.13568, "omega":0.14833, "ax":-0.04507, "ay":0.03881, "alpha":0.1137, "fx":[-1.17068,-0.63434,-0.30289,-0.83928], "fy":[0.54106,0.20261,0.72787,1.0662]}, - {"t":1.27987, "x":3.33783, "y":2.57093, "heading":1.01269, "vx":0.97774, "vy":1.13693, "omega":0.15199, "ax":-0.00792, "ay":0.00681, "alpha":-0.13923, "fx":[0.40133,-0.25754,-0.66021,-0.00136], "fy":[0.22813,0.63945,-0.00552,-0.41688]}, - {"t":1.31203, "x":3.36927, "y":2.6075, "heading":1.01758, "vx":0.97748, "vy":1.13715, "omega":0.14751, "ax":-0.00139, "ay":0.00119, "alpha":-0.39272, "fx":[1.47276,-0.3913,-1.5182,0.34585], "fy":[0.35638,1.5075,-0.31729,-1.46846]}, - {"t":1.34419, "x":3.40071, "y":2.64407, "heading":1.02232, "vx":0.97744, "vy":1.13719, "omega":0.13488, "ax":-0.00024, "ay":0.00021, "alpha":-0.64766, "fx":[2.45966,-0.62348,-2.46764,0.61549], "fy":[0.57063,2.45445,-0.56376,-2.4476]}, - {"t":1.37636, "x":3.43214, "y":2.68065, "heading":1.02666, "vx":0.97743, "vy":1.13719, "omega":0.11405, "ax":-0.00004, "ay":0.00004, "alpha":-0.90499, "fx":[3.43834,-0.88117,-3.43975,0.87976], "fy":[0.8081,3.42169,-0.80687,-3.42048]}, - {"t":1.40852, "x":3.46358, "y":2.71722, "heading":1.03033, "vx":0.97743, "vy":1.13719, "omega":0.08494, "ax":-0.00004, "ay":-0.00003, "alpha":-1.16562, "fx":[4.42491,-1.15089,-4.42631,1.14948], "fy":[1.05579,4.40165,-1.05676,-4.40262]}, - {"t":1.44068, "x":3.49502, "y":2.7538, "heading":1.03306, "vx":0.97743, "vy":1.13719, "omega":0.04746, "ax":-0.39078, "ay":-0.45465, "alpha":-1.38378, "fx":[-1.1002,-7.90515,-11.58302,-4.96534], "fy":[-6.2748,-2.18391,-8.62876,-12.64344]}, - {"t":1.47284, "x":3.52625, "y":2.79013, "heading":1.03458, "vx":0.96486, "vy":1.12257, "omega":0.00295, "ax":-2.97232, "ay":-3.45815, "alpha":-0.02282, "fx":[-48.51414,-48.69889,-48.66903,-48.48475], "fy":[-56.59853,-56.44038,-56.47023,-56.6278]}, - {"t":1.505, "x":3.55574, "y":2.82445, "heading":1.03468, "vx":0.86926, "vy":1.01135, "omega":0.00222, "ax":-2.99198, "ay":-3.48103, "alpha":-0.01313, "fx":[-48.8686,-48.97538,-48.95755,-48.85093], "fy":[-56.94578,-56.85418,-56.87071,-56.96211]}, - {"t":1.53716, "x":3.58215, "y":2.85518, "heading":1.03475, "vx":0.77304, "vy":0.89939, "omega":0.0018, "ax":-2.99849, "ay":-3.4886, "alpha":-0.00993, "fx":[-48.98589,-49.06678,-49.05313,-48.97233], "fy":[-57.06058,-56.99114,-57.00348,-57.07281]}, - {"t":1.56933, "x":3.60546, "y":2.8823, "heading":1.03481, "vx":0.6766, "vy":0.78719, "omega":0.00148, "ax":-3.00173, "ay":-3.49238, "alpha":-0.00834, "fx":[-49.04433,-49.11231,-49.10077,-49.03286], "fy":[-57.11777,-57.0594,-57.06969,-57.12798]}, - {"t":1.60149, "x":3.62567, "y":2.90581, "heading":1.03486, "vx":0.58006, "vy":0.67487, "omega":0.00121, "ax":-3.00367, "ay":-3.49464, "alpha":-0.00738, "fx":[-49.07933,-49.13956,-49.12931,-49.06913], "fy":[-57.152,-57.10027,-57.10936,-57.16103]}, - {"t":1.63365, "x":3.64277, "y":2.92571, "heading":1.03489, "vx":0.48346, "vy":0.56248, "omega":0.00097, "ax":-3.00497, "ay":-3.49614, "alpha":-0.00675, "fx":[-49.10263,-49.15771,-49.14831,-49.09327], "fy":[-57.1748,-57.12748,-57.13577,-57.18303]}, - {"t":1.66581, "x":3.65677, "y":2.94199, "heading":1.03493, "vx":0.38681, "vy":0.45004, "omega":0.00075, "ax":-3.00589, "ay":-3.49722, "alpha":-0.0063, "fx":[-49.11926,-49.17066,-49.16187,-49.11051], "fy":[-57.19106,-57.1469,-57.15462,-57.19873]}, - {"t":1.69797, "x":3.66766, "y":2.95465, "heading":1.03495, "vx":0.29014, "vy":0.33756, "omega":0.00055, "ax":-3.00658, "ay":-3.49802, "alpha":-0.00596, "fx":[-49.13172,-49.18036,-49.17203,-49.12343], "fy":[-57.20325,-57.16146,-57.16875,-57.2105]}, - {"t":1.73013, "x":3.67543, "y":2.9637, "heading":1.03497, "vx":0.19344, "vy":0.22506, "omega":0.00036, "ax":-3.00712, "ay":-3.49865, "alpha":-0.0057, "fx":[-49.1414,-49.1879,-49.17994,-49.13347], "fy":[-57.21272,-57.17277,-57.17974,-57.21965]}, - {"t":1.7623, "x":3.6801, "y":2.96913, "heading":1.03498, "vx":0.09673, "vy":0.11254, "omega":0.00018, "ax":-3.00755, "ay":-3.49915, "alpha":-0.00548, "fx":[-49.14915,-49.19393,-49.18625,-49.1415], "fy":[-57.2203,-57.18182,-57.18852,-57.22696]}, - {"t":1.79446, "x":3.68165, "y":2.97094, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0423, "ay":3.47042, "alpha":0.0068, "fx":[49.7105,49.76616,49.76122,49.70561], "fy":[56.75687,56.70808,56.71252,56.76125]}, + {"t":0.03902, "x":1.63565, "y":0.62732, "heading":0.942, "vx":0.11872, "vy":0.13542, "omega":0.00027, "ax":3.04212, "ay":3.47021, "alpha":0.00695, "fx":[49.70701,49.76388,49.75883,49.70201], "fy":[56.75407,56.70422,56.70876,56.75856]}, + {"t":0.07804, "x":1.6426, "y":0.63525, "heading":0.94201, "vx":0.23743, "vy":0.27084, "omega":0.00054, "ax":3.04192, "ay":3.46999, "alpha":0.00712, "fx":[49.70311,49.76133,49.75617,49.698], "fy":[56.75095,56.69991,56.70457,56.75555]}, + {"t":0.11707, "x":1.65418, "y":0.64846, "heading":0.94203, "vx":0.35613, "vy":0.40624, "omega":0.00081, "ax":3.0417, "ay":3.46974, "alpha":0.00731, "fx":[49.69872,49.75847,49.75317,49.69347], "fy":[56.74744,56.69507,56.69984,56.75216]}, + {"t":0.15609, "x":1.67039, "y":0.66695, "heading":0.94206, "vx":0.47482, "vy":0.54164, "omega":0.0011, "ax":3.04144, "ay":3.46945, "alpha":0.00752, "fx":[49.69374,49.75522,49.74977,49.68835], "fy":[56.74346,56.68957,56.6945,56.74832]}, + {"t":0.19511, "x":1.69124, "y":0.69073, "heading":0.94211, "vx":0.5935, "vy":0.67702, "omega":0.00139, "ax":3.04115, "ay":3.46913, "alpha":0.00776, "fx":[49.68805,49.75151,49.74588,49.68249], "fy":[56.73891,56.68329,56.68838,56.74393]}, + {"t":0.23413, "x":1.71671, "y":0.71979, "heading":0.94216, "vx":0.71218, "vy":0.8124, "omega":0.0017, "ax":3.04081, "ay":3.46875, "alpha":0.00804, "fx":[49.68149,49.74722,49.7414,49.67574], "fy":[56.73365,56.67604,56.68133,56.73887]}, + {"t":0.27315, "x":1.74682, "y":0.75413, "heading":0.94223, "vx":0.83083, "vy":0.94775, "omega":0.00201, "ax":3.04042, "ay":3.46831, "alpha":0.00836, "fx":[49.67384,49.74223,49.73617,49.66785], "fy":[56.72753,56.66759,56.6731,56.73296]}, + {"t":0.31218, "x":1.78155, "y":0.79375, "heading":0.94231, "vx":0.94948, "vy":1.08309, "omega":0.00234, "ax":3.03995, "ay":3.46779, "alpha":0.00875, "fx":[49.66479,49.73632,49.72999,49.65854], "fy":[56.72029,56.65759,56.66338,56.72599]}, + {"t":0.3512, "x":1.82092, "y":0.83866, "heading":0.9424, "vx":1.0681, "vy":1.21841, "omega":0.00268, "ax":3.03939, "ay":3.46717, "alpha":0.00921, "fx":[49.65393,49.72924,49.72257,49.64736], "fy":[56.7116,56.6456,56.65171,56.71761]}, + {"t":0.39022, "x":1.86491, "y":0.88884, "heading":0.9425, "vx":1.18671, "vy":1.35371, "omega":0.00304, "ax":3.03871, "ay":3.46641, "alpha":0.00978, "fx":[49.64066,49.72058,49.71351,49.63369], "fy":[56.70098,56.63094,56.63745,56.70738]}, + {"t":0.42924, "x":1.91353, "y":0.94431, "heading":0.94262, "vx":1.30528, "vy":1.48898, "omega":0.00342, "ax":3.03786, "ay":3.46546, "alpha":0.01049, "fx":[49.62407,49.70975,49.70218,49.61662], "fy":[56.6877,56.61261,56.61963,56.69459]}, + {"t":0.46826, "x":1.96678, "y":1.00505, "heading":0.94275, "vx":1.42382, "vy":1.62421, "omega":0.00383, "ax":3.03676, "ay":3.46423, "alpha":0.0114, "fx":[49.60274,49.69584,49.68762,49.59466], "fy":[56.67063,56.58905,56.59672,56.67816]}, + {"t":0.50729, "x":2.02465, "y":1.07107, "heading":0.9429, "vx":1.54233, "vy":1.75939, "omega":0.00427, "ax":3.0353, "ay":3.4626, "alpha":0.01261, "fx":[49.5743,49.67729,49.66822,49.5654], "fy":[56.64787,56.55764,56.56619,56.65625]}, + {"t":0.54631, "x":2.08715, "y":1.14236, "heading":0.94307, "vx":1.66077, "vy":1.8945, "omega":0.00476, "ax":3.03325, "ay":3.46032, "alpha":0.01431, "fx":[49.53449,49.65132,49.64107,49.52446], "fy":[56.61602,56.51367,56.52347,56.6256]}, + {"t":0.58533, "x":2.15427, "y":1.21892, "heading":0.94325, "vx":1.77913, "vy":2.02953, "omega":0.00532, "ax":3.03018, "ay":3.4569, "alpha":0.01687, "fx":[49.47478,49.61237,49.60038,49.46309], "fy":[56.56824,56.44772,56.45944,56.57965]}, + {"t":0.62435, "x":2.226, "y":1.30075, "heading":0.94346, "vx":1.89738, "vy":2.16443, "omega":0.00598, "ax":3.02507, "ay":3.4512, "alpha":0.02113, "fx":[49.37531,49.54749,49.53265,49.36094], "fy":[56.48862,56.33785,56.35287,56.50316]}, + {"t":0.66337, "x":2.30234, "y":1.38783, "heading":0.9437, "vx":2.01542, "vy":2.2991, "omega":0.00681, "ax":3.01486, "ay":3.43983, "alpha":0.02968, "fx":[49.17655,49.41778,49.39753,49.15721], "fy":[56.32947,56.11836,56.14035,56.35051]}, + {"t":0.7024, "x":2.38328, "y":1.48017, "heading":0.94396, "vx":2.13307, "vy":2.43333, "omega":0.00796, "ax":2.9844, "ay":3.40587, "alpha":0.05538, "fx":[48.58215,49.02911,48.99472,48.55088], "fy":[55.85258,55.46212,55.50793,55.89512]}, + {"t":0.74142, "x":2.46879, "y":1.57772, "heading":0.94427, "vx":2.24952, "vy":2.56623, "omega":0.01012, "ax":-0.19624, "ay":-0.18478, "alpha":2.46195, "fx":[-12.64153,-1.61938,6.35428,-4.92602], "fy":[-4.43821,-12.46876,-1.66449,6.48822]}, + {"t":0.78044, "x":2.55642, "y":1.67771, "heading":0.94467, "vx":2.24186, "vy":2.55902, "omega":0.10619, "ax":-2.98627, "ay":-3.40482, "alpha":0.04907, "fx":[-49.00166,-48.60835,-48.6367,-49.03245], "fy":[-55.51037,-55.85381,-55.81557,-55.46956]}, + {"t":0.81946, "x":2.64163, "y":1.77498, "heading":0.94881, "vx":2.12533, "vy":2.42616, "omega":0.10811, "ax":-3.01574, "ay":-3.43922, "alpha":0.02327, "fx":[-49.38747,-49.19912,-49.21535,-49.40425], "fy":[-56.15114,-56.31592,-56.29848,-56.13311]}, + {"t":0.85848, "x":2.72227, "y":1.86704, "heading":0.95303, "vx":2.00765, "vy":2.29195, "omega":0.10902, "ax":-3.02564, "ay":-3.45078, "alpha":0.01453, "fx":[-49.51683,-49.3988,-49.40986,-49.5281], "fy":[-56.36753,-56.47086,-56.45982,-56.35626]}, + {"t":0.89751, "x":2.79831, "y":1.95384, "heading":0.95728, "vx":1.88959, "vy":2.1573, "omega":0.10958, "ax":-3.0306, "ay":-3.45657, "alpha":0.01011, "fx":[-49.58154,-49.49933,-49.50752,-49.58983], "fy":[-56.47634,-56.54834,-56.54046,-56.46836]}, + {"t":0.93653, "x":2.86974, "y":2.03539, "heading":0.96156, "vx":1.77133, "vy":2.02242, "omega":0.10998, "ax":-3.03358, "ay":-3.46006, "alpha":0.00742, "fx":[-49.62034,-49.55994,-49.56627,-49.62672], "fy":[-56.54186,-56.59476,-56.5888,-56.53584]}, + {"t":0.97555, "x":2.93655, "y":2.11168, "heading":0.96585, "vx":1.65295, "vy":1.8874, "omega":0.11027, "ax":-3.03557, "ay":-3.46238, "alpha":0.00561, "fx":[-49.64618,-49.6005,-49.60552,-49.65122], "fy":[-56.58564,-56.62565,-56.62099,-56.58095]}, + {"t":1.01457, "x":2.99874, "y":2.18269, "heading":0.97015, "vx":1.5345, "vy":1.75229, "omega":0.11049, "ax":-3.037, "ay":-3.46404, "alpha":0.00431, "fx":[-49.66462,-49.62957,-49.63359,-49.66865], "fy":[-56.61696,-56.64766,-56.64397,-56.61325]}, + {"t":1.05359, "x":3.0563, "y":2.24843, "heading":0.97447, "vx":1.41599, "vy":1.61712, "omega":0.11066, "ax":-3.03806, "ay":-3.46529, "alpha":0.00332, "fx":[-49.67845,-49.65144,-49.65466,-49.68167], "fy":[-56.64048,-56.66414,-56.6612,-56.63753]}, + {"t":1.09262, "x":3.10925, "y":2.3089, "heading":0.97878, "vx":1.29744, "vy":1.48189, "omega":0.11079, "ax":-3.03889, "ay":-3.46626, "alpha":0.00254, "fx":[-49.6892,-49.6685,-49.67106,-49.69176], "fy":[-56.65878,-56.67692,-56.67459,-56.65645]}, + {"t":1.13164, "x":3.15756, "y":2.36409, "heading":0.98311, "vx":1.17885, "vy":1.34663, "omega":0.11088, "ax":-3.03956, "ay":-3.46703, "alpha":0.00192, "fx":[-49.6978,-49.68217,-49.68418,-49.69981], "fy":[-56.67343,-56.68712,-56.68531,-56.67162]}, + {"t":1.17066, "x":3.20125, "y":2.41399, "heading":0.98743, "vx":1.06024, "vy":1.21134, "omega":0.11096, "ax":-3.0401, "ay":-3.46767, "alpha":0.00141, "fx":[-49.70484,-49.69339,-49.69491,-49.70636], "fy":[-56.68542,-56.69545,-56.69408,-56.68405]}, + {"t":1.20968, "x":3.24031, "y":2.45862, "heading":0.99176, "vx":0.94161, "vy":1.07603, "omega":0.11101, "ax":-3.04055, "ay":-3.4682, "alpha":0.00098, "fx":[-49.71071,-49.70276,-49.70385,-49.71181], "fy":[-56.6954,-56.70237,-56.70139,-56.69442]}, + {"t":1.2487, "x":3.27473, "y":2.49797, "heading":0.9961, "vx":0.82296, "vy":0.94069, "omega":0.11105, "ax":-0.21883, "ay":0.18009, "alpha":0.35826, "fx":[-4.94745,-3.27644,-2.2061,-3.87997], "fy":[2.66244,1.5785,3.22883,4.30706]}, + {"t":1.28213, "x":3.30212, "y":2.52951, "heading":0.99981, "vx":0.81565, "vy":0.94671, "omega":0.12303, "ax":-0.0265, "ay":0.02281, "alpha":0.22906, "fx":[-1.30883,-0.23381,0.4424,-0.63275], "fy":[0.192,-0.49872,0.55397,1.2444]}, + {"t":1.31555, "x":3.32936, "y":2.56117, "heading":1.00392, "vx":0.81476, "vy":0.94747, "omega":0.13068, "ax":-0.00326, "ay":0.0028, "alpha":0.0982, "fx":[-0.42832,0.0338,0.32177,-0.14034], "fy":[-0.0333,-0.32747,0.12492,0.41908]}, + {"t":1.34897, "x":3.35659, "y":2.59284, "heading":1.00829, "vx":0.81466, "vy":0.94757, "omega":0.13396, "ax":-0.0004, "ay":0.00034, "alpha":-0.03225, "fx":[0.11653,-0.03568,-0.12962,0.02259], "fy":[0.03215,0.12811,-0.02089,-0.11685]}, + {"t":1.38239, "x":3.38382, "y":2.62451, "heading":1.01276, "vx":0.81464, "vy":0.94758, "omega":0.13289, "ax":-0.00005, "ay":0.00004, "alpha":-0.16285, "fx":[0.61999,-0.15067,-0.62159,0.14906], "fy":[0.13738,0.61843,-0.136,-0.61705]}, + {"t":1.41582, "x":3.41105, "y":2.65618, "heading":1.01721, "vx":0.81464, "vy":0.94758, "omega":0.12744, "ax":-0.00001, "ay":0.00001, "alpha":-0.29417, "fx":[1.12019,-0.27577,-1.12038,0.27557], "fy":[0.25198,1.11476,-0.25181,-1.11459]}, + {"t":1.44924, "x":3.43828, "y":2.68785, "heading":1.02147, "vx":0.81464, "vy":0.94758, "omega":0.11761, "ax":0.0, "ay":0.0, "alpha":-0.42679, "fx":[1.6238,-0.40686,-1.62382,0.40683], "fy":[0.37239,1.61553,-0.37237,-1.61551]}, + {"t":1.48266, "x":3.4655, "y":2.71952, "heading":1.0254, "vx":0.81464, "vy":0.94758, "omega":0.10335, "ax":0.0, "ay":0.0, "alpha":-0.56132, "fx":[2.13368,-0.54343,-2.13369,0.54343], "fy":[0.49815,2.12261,-0.49815,-2.1226]}, + {"t":1.51609, "x":3.49273, "y":2.75119, "heading":1.02885, "vx":0.81464, "vy":0.94758, "omega":0.08459, "ax":0.0, "ay":0.0, "alpha":-0.69832, "fx":[2.65232,-0.68519,-2.65232,0.68518], "fy":[0.6289,2.63834,-0.6289,-2.63834]}, + {"t":1.54951, "x":3.51996, "y":2.78286, "heading":1.03168, "vx":0.81464, "vy":0.94758, "omega":0.06125, "ax":-0.00002, "ay":-0.00002, "alpha":-0.83841, "fx":[3.18193,-0.83189,-3.18254,0.83128], "fy":[0.76373,3.16493,-0.76439,-3.16559]}, + {"t":1.58293, "x":3.54719, "y":2.81453, "heading":1.03372, "vx":0.81464, "vy":0.94758, "omega":0.03322, "ax":-0.36075, "ay":-0.41962, "alpha":-0.95474, "fx":[-2.26053,-6.92545,-9.49346,-4.91087], "fy":[-6.0444,-3.24466,-7.69221,-10.45887]}, + {"t":1.61635, "x":3.57421, "y":2.84597, "heading":1.03484, "vx":0.80258, "vy":0.93355, "omega":0.00131, "ax":-2.9817, "ay":-3.46828, "alpha":-0.01161, "fx":[-48.70576,-48.79994,-48.78449,-48.69044], "fy":[-56.73271,-56.65201,-56.66692,-56.74747]}, + {"t":1.64978, "x":3.59937, "y":2.87523, "heading":1.03488, "vx":0.70293, "vy":0.81764, "omega":0.00093, "ax":-2.99684, "ay":-3.48588, "alpha":-0.00652, "fx":[-48.97048,-49.02356,-49.01463,-48.96159], "fy":[-57.00629,-56.96073,-56.96886,-57.01438]}, + {"t":1.6832, "x":3.62119, "y":2.90061, "heading":1.03491, "vx":0.60276, "vy":0.70113, "omega":0.00071, "ax":-3.00185, "ay":-3.49171, "alpha":-0.00484, "fx":[-49.05813,-49.09757,-49.09088,-49.05146], "fy":[-57.09683,-57.06296,-57.06894,-57.10278]}, + {"t":1.71662, "x":3.63966, "y":2.9221, "heading":1.03493, "vx":0.50243, "vy":0.58442, "omega":0.00055, "ax":-3.00435, "ay":-3.49462, "alpha":-0.004, "fx":[-49.10182,-49.13445,-49.12889,-49.09627], "fy":[-57.14195,-57.11392,-57.11884,-57.14686]}, + {"t":1.75004, "x":3.65478, "y":2.93968, "heading":1.03495, "vx":0.40202, "vy":0.46762, "omega":0.00041, "ax":-3.00584, "ay":-3.49636, "alpha":-0.0035, "fx":[-49.12798,-49.15654,-49.15166,-49.12312], "fy":[-57.16898,-57.14445,-57.14874,-57.17326]}, + {"t":1.78347, "x":3.66653, "y":2.95335, "heading":1.03497, "vx":0.30156, "vy":0.35077, "omega":0.0003, "ax":-3.00684, "ay":-3.49752, "alpha":-0.00317, "fx":[-49.14541,-49.17125,-49.16683,-49.141], "fy":[-57.18698,-57.16477,-57.16865,-57.19084]}, + {"t":1.81689, "x":3.67493, "y":2.96312, "heading":1.03498, "vx":0.20106, "vy":0.23387, "omega":0.00019, "ax":-3.00755, "ay":-3.49835, "alpha":-0.00293, "fx":[-49.15785,-49.18174,-49.17765,-49.15376], "fy":[-57.19982,-57.17928,-57.18286,-57.20339]}, + {"t":1.85031, "x":3.67997, "y":2.96899, "heading":1.03498, "vx":0.10054, "vy":0.11695, "omega":0.00009, "ax":-3.00809, "ay":-3.49897, "alpha":-0.00275, "fx":[-49.16717,-49.18961,-49.18576,-49.16333], "fy":[-57.20944,-57.19016,-57.19351,-57.21279]}, + {"t":1.88374, "x":3.68165, "y":2.97094, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/PROtoD.traj b/src/main/deploy/choreo/PROtoD.traj index e9d81059..67573fa4 100644 --- a/src/main/deploy/choreo/PROtoD.traj +++ b/src/main/deploy/choreo/PROtoD.traj @@ -10,7 +10,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,60 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.26596,1.77624], + "waypoints":[0.0,1.29973,1.85258], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37088, "ay":3.15193, "alpha":0.01343, "fx":[55.06742,55.16592,55.14742,55.04906], "fy":[51.57075,51.46541,51.48546,51.59058]}, - {"t":0.03723, "x":1.63567, "y":0.62686, "heading":0.942, "vx":0.12551, "vy":0.11736, "omega":0.0005, "ax":3.3707, "ay":3.15176, "alpha":0.01364, "fx":[55.06377,55.16386,55.14507,55.04512], "fy":[51.56859,51.46156,51.48193,51.58874]}, - {"t":0.07447, "x":1.64268, "y":0.63342, "heading":0.94202, "vx":0.25102, "vy":0.23471, "omega":0.00101, "ax":3.37049, "ay":3.15156, "alpha":0.01388, "fx":[55.05975,55.16159,55.14247,55.04078], "fy":[51.5662,51.4573,51.47804,51.58671]}, - {"t":0.1117, "x":1.65436, "y":0.64434, "heading":0.94206, "vx":0.37651, "vy":0.35206, "omega":0.00152, "ax":3.37027, "ay":3.15135, "alpha":0.01415, "fx":[55.05527,55.15906,55.13958,55.03595], "fy":[51.56354,51.45256,51.47371,51.58445]}, - {"t":0.14894, "x":1.67072, "y":0.65963, "heading":0.94211, "vx":0.502, "vy":0.4694, "omega":0.00205, "ax":3.37002, "ay":3.15111, "alpha":0.01444, "fx":[55.05027,55.15624,55.13635,55.03055], "fy":[51.56057,51.44727,51.46888,51.58193]}, - {"t":0.18617, "x":1.69175, "y":0.6793, "heading":0.94219, "vx":0.62748, "vy":0.58672, "omega":0.00259, "ax":3.36973, "ay":3.15084, "alpha":0.01478, "fx":[55.04465,55.15306,55.13271,55.02447], "fy":[51.55723,51.44131,51.46344,51.57909]}, - {"t":0.2234, "x":1.71745, "y":0.70333, "heading":0.94229, "vx":0.75295, "vy":0.70404, "omega":0.00314, "ax":3.36941, "ay":3.15054, "alpha":0.01516, "fx":[55.03827,55.14946,55.12859,55.01758], "fy":[51.55344,51.43455,51.45728,51.57588]}, - {"t":0.26064, "x":1.74782, "y":0.73172, "heading":0.9424, "vx":0.87841, "vy":0.82135, "omega":0.0037, "ax":3.36905, "ay":3.15019, "alpha":0.01559, "fx":[55.03099,55.14534,55.12388,55.00971], "fy":[51.54911,51.42683,51.45023,51.57221]}, - {"t":0.29787, "x":1.78286, "y":0.76449, "heading":0.94254, "vx":1.00385, "vy":0.93865, "omega":0.00428, "ax":3.36862, "ay":3.14979, "alpha":0.01609, "fx":[55.02258,55.1406,55.11844,55.00062], "fy":[51.5441,51.41792,51.44211,51.56797]}, - {"t":0.33511, "x":1.82257, "y":0.80162, "heading":0.9427, "vx":1.12928, "vy":1.05592, "omega":0.00488, "ax":3.36813, "ay":3.14932, "alpha":0.01668, "fx":[55.01277,55.13506,55.11209,54.99002], "fy":[51.53827,51.40752,51.43263,51.56303]}, - {"t":0.37234, "x":1.86696, "y":0.84312, "heading":0.94288, "vx":1.25469, "vy":1.17319, "omega":0.0055, "ax":3.36754, "ay":3.14876, "alpha":0.01737, "fx":[55.00117,55.12851,55.1046,54.97748], "fy":[51.53137,51.39522,51.42142,51.55719]}, - {"t":0.40958, "x":1.91601, "y":0.88899, "heading":0.94309, "vx":1.38008, "vy":1.29043, "omega":0.00615, "ax":3.36684, "ay":3.1481, "alpha":0.0182, "fx":[54.98724,55.12065,55.0956,54.96244], "fy":[51.52309,51.38046,51.40797,51.55018]}, - {"t":0.44681, "x":1.96973, "y":0.93922, "heading":0.94332, "vx":1.50544, "vy":1.40765, "omega":0.00683, "ax":3.36599, "ay":3.14729, "alpha":0.01921, "fx":[54.97022,55.11105,55.0846,54.94405], "fy":[51.51296,51.36241,51.39153,51.54162]}, - {"t":0.48404, "x":2.02811, "y":0.99381, "heading":0.94357, "vx":1.63077, "vy":1.52483, "omega":0.00754, "ax":3.36492, "ay":3.14627, "alpha":0.02049, "fx":[54.94893,55.09904,55.07085,54.92106], "fy":[51.5003,51.33984,51.37099,51.53092]}, - {"t":0.52128, "x":2.09117, "y":1.05277, "heading":0.94385, "vx":1.75606, "vy":1.64198, "omega":0.00831, "ax":3.36354, "ay":3.14496, "alpha":0.02212, "fx":[54.92154,55.0836,55.05318,54.89149], "fy":[51.48401,51.31081,51.34456,51.51716]}, - {"t":0.55851, "x":2.15888, "y":1.11609, "heading":0.94416, "vx":1.8813, "vy":1.75908, "omega":0.00913, "ax":3.3617, "ay":3.14322, "alpha":0.02431, "fx":[54.88499,55.06299,55.02962,54.85206], "fy":[51.46229,51.27207,51.30933,51.49881]}, - {"t":0.59575, "x":2.23126, "y":1.18376, "heading":0.9445, "vx":2.00647, "vy":1.87611, "omega":0.01004, "ax":3.35913, "ay":3.14078, "alpha":0.02737, "fx":[54.83378,55.03414,54.99664,54.79685], "fy":[51.43186,51.2178,51.26,51.47313]}, - {"t":0.63298, "x":2.3083, "y":1.2558, "heading":0.94487, "vx":2.13154, "vy":1.99306, "omega":0.01106, "ax":3.35527, "ay":3.13711, "alpha":0.03198, "fx":[54.75687,54.99081,54.94719,54.71402], "fy":[51.38617,51.13631,51.186,51.43461]}, - {"t":0.67021, "x":2.38999, "y":1.33218, "heading":0.94529, "vx":2.25647, "vy":2.10987, "omega":0.01225, "ax":3.34883, "ay":3.131, "alpha":0.0397, "fx":[54.62846,54.9185,54.8648,54.57596], "fy":[51.3099,51.0003,51.06273,51.3704]}, - {"t":0.70745, "x":2.47633, "y":1.41291, "heading":0.94574, "vx":2.38116, "vy":2.22645, "omega":0.01372, "ax":3.33593, "ay":3.11876, "alpha":0.05521, "fx":[54.37094,54.77342,54.70014,54.29996], "fy":[51.15689,50.72771,50.81646,51.24193]}, - {"t":0.74468, "x":2.5673, "y":1.49797, "heading":0.94625, "vx":2.50537, "vy":2.34257, "omega":0.01578, "ax":3.29715, "ay":3.08195, "alpha":0.10228, "fx":[53.59342,54.33423,54.20686,53.4738], "fy":[50.69367,49.90629,50.08061,50.85558]}, - {"t":0.78192, "x":2.66287, "y":1.58733, "heading":0.94684, "vx":2.62814, "vy":2.45732, "omega":0.01959, "ax":-0.89184, "ay":-0.8555, "alpha":3.30422, "fx":[-26.63892,-11.99065,-1.46793,-18.22182], "fy":[-15.29979,-26.53795,-13.28319,-0.82216]}, - {"t":0.81915, "x":2.76011, "y":1.67823, "heading":0.94757, "vx":2.59493, "vy":2.42547, "omega":0.14262, "ax":-3.29826, "ay":-3.08515, "alpha":0.08608, "fx":[-54.17675,-53.55883,-53.66117,-54.28457], "fy":[-50.1806,-50.8338,-50.69643,-50.03443]}, - {"t":0.85638, "x":2.85445, "y":1.76641, "heading":0.95288, "vx":2.47212, "vy":2.3106, "omega":0.14582, "ax":-3.33595, "ay":-3.11987, "alpha":0.04029, "fx":[-54.65507,-54.36309,-54.41719,-54.71038], "fy":[-50.88141,-51.19182,-51.12732,-50.81493]}, - {"t":0.89362, "x":2.94418, "y":1.85028, "heading":0.95831, "vx":2.34791, "vy":2.19443, "omega":0.14732, "ax":-3.34872, "ay":-3.13163, "alpha":0.02463, "fx":[-54.81701,-54.63803,-54.67305,-54.85248], "fy":[-51.12107,-51.31171,-51.27153,-51.08015]}, - {"t":0.93085, "x":3.02928, "y":1.92981, "heading":0.96379, "vx":2.22323, "vy":2.07783, "omega":0.14824, "ax":-3.35514, "ay":-3.13754, "alpha":0.01666, "fx":[-54.89831,-54.77717,-54.80184,-54.92318], "fy":[-51.2422,-51.37136,-51.34358,-51.21408]}, - {"t":0.96809, "x":3.10974, "y":2.005, "heading":0.96931, "vx":2.0983, "vy":1.96101, "omega":0.14886, "ax":-3.359, "ay":-3.1411, "alpha":0.0118, "fx":[-54.94715,-54.86132,-54.87941,-54.96534], "fy":[-51.31533,-51.40689,-51.38676,-51.29501]}, - {"t":1.00532, "x":3.18554, "y":2.07584, "heading":0.97486, "vx":1.97323, "vy":1.84405, "omega":0.1493, "ax":-3.36158, "ay":-3.14348, "alpha":0.00853, "fx":[-54.97974,-54.91777,-54.93125,-54.99327], "fy":[-51.36426,-51.4304,-51.41551,-51.34928]}, - {"t":1.04256, "x":3.25668, "y":2.14233, "heading":0.98042, "vx":1.84806, "vy":1.727, "omega":0.14962, "ax":-3.36343, "ay":-3.14518, "alpha":0.00616, "fx":[-55.00304,-54.95831,-54.96832,-55.01308], "fy":[-51.39929,-51.44705,-51.43605,-51.38824]}, - {"t":1.07979, "x":3.32316, "y":2.20445, "heading":0.98599, "vx":1.72283, "vy":1.6099, "omega":0.14985, "ax":-3.36482, "ay":-3.14645, "alpha":0.00437, "fx":[-55.02054,-54.98884,-54.99614,-55.02785], "fy":[-51.4256,-51.45944,-51.45146,-51.41759]}, - {"t":1.11702, "x":3.38497, "y":2.26221, "heading":0.99157, "vx":1.59754, "vy":1.49274, "omega":0.15001, "ax":-3.3659, "ay":-3.14745, "alpha":0.00296, "fx":[-55.03417,-55.0127,-55.01777,-55.03926], "fy":[-51.44606,-51.469,-51.46346,-51.44051]}, - {"t":1.15426, "x":3.44212, "y":2.31561, "heading":0.99715, "vx":1.47222, "vy":1.37555, "omega":0.15012, "ax":-3.36676, "ay":-3.14824, "alpha":0.00183, "fx":[-55.04511,-55.03185,-55.03506,-55.04833], "fy":[-51.46243,-51.47659,-51.47309,-51.45892]}, - {"t":1.19149, "x":3.49461, "y":2.36465, "heading":1.00274, "vx":1.34686, "vy":1.25833, "omega":0.15019, "ax":-3.36746, "ay":-3.14889, "alpha":0.0009, "fx":[-55.05408,-55.04758,-55.04919,-55.0557], "fy":[-51.4758,-51.48274,-51.48099,-51.47404]}, - {"t":1.22873, "x":3.54242, "y":2.40932, "heading":1.00833, "vx":1.22148, "vy":1.14108, "omega":0.15022, "ax":-3.36805, "ay":-3.14943, "alpha":0.00012, "fx":[-55.06159,-55.06073,-55.06095,-55.0618], "fy":[-51.48692,-51.48783,-51.4876,-51.48668]}, - {"t":1.26596, "x":3.58557, "y":2.44962, "heading":1.01393, "vx":1.09607, "vy":1.02381, "omega":0.15023, "ax":0.1146, "ay":-0.14004, "alpha":-0.18325, "fx":[2.57237,1.70516,1.17465,2.04209], "fy":[-2.13401,-1.59463,-2.44529,-2.98364]}, - {"t":1.29598, "x":3.61852, "y":2.48029, "heading":1.01844, "vx":1.09951, "vy":1.01961, "omega":0.14472, "ax":0.02428, "ay":-0.0262, "alpha":-0.41718, "fx":[1.98518,0.00416,-1.19155,0.78977], "fy":[-0.06931,1.15224,-0.78787,-2.00837]}, - {"t":1.32599, "x":3.65153, "y":2.51088, "heading":1.02278, "vx":1.10024, "vy":1.01882, "omega":0.1322, "ax":0.00479, "ay":-0.00518, "alpha":-0.652, "fx":[2.55824,-0.54636,-2.40157,0.70317], "fy":[0.48738,2.38265,-0.65692,-2.55169]}, - {"t":1.35601, "x":3.68456, "y":2.54146, "heading":1.02675, "vx":1.10038, "vy":1.01867, "omega":0.11263, "ax":0.00095, "ay":-0.00102, "alpha":-0.88889, "fx":[3.39325,-0.8496,-3.36229,0.8806], "fy":[0.77664,3.34344,-0.8102,-3.37681]}, - {"t":1.38603, "x":3.71759, "y":2.57204, "heading":1.03013, "vx":1.10041, "vy":1.01864, "omega":0.08595, "ax":0.00019, "ay":-0.0002, "alpha":-1.12861, "fx":[4.28836,-1.10977,-4.28225,1.11589], "fy":[1.01857,4.25931,-1.02522,-4.26589]}, - {"t":1.41604, "x":3.75062, "y":2.60261, "heading":1.03271, "vx":1.10042, "vy":1.01863, "omega":0.05207, "ax":-0.00113, "ay":-0.00112, "alpha":-1.37191, "fx":[5.18748,-1.38473,-5.22426,1.34745], "fy":[1.23727,5.1597,-1.27397,-5.19625]}, - {"t":1.44606, "x":3.78365, "y":2.63319, "heading":1.03427, "vx":1.10038, "vy":1.0186, "omega":0.01089, "ax":-2.89025, "ay":-2.67546, "alpha":-0.26115, "fx":[-46.49023,-48.20702,-47.99268,-46.31051], "fy":[-44.21123,-42.50995,-43.29525,-44.93834]}, - {"t":1.47607, "x":3.81538, "y":2.66256, "heading":1.0346, "vx":1.01363, "vy":0.93829, "omega":0.00306, "ax":-3.34632, "ay":-3.09761, "alpha":-0.02506, "fx":[-54.64151,-54.81916,-54.77021,-54.59292], "fy":[-50.70657,-50.51595,-50.57392,-50.76377]}, - {"t":1.50609, "x":3.84429, "y":2.68932, "heading":1.03469, "vx":0.91318, "vy":0.84531, "omega":0.0023, "ax":-3.36736, "ay":-3.11709, "alpha":-0.01481, "fx":[-55.01211,-55.11745,-55.0877,-54.98248], "fy":[-50.99838,-50.88497,-50.9187,-51.03184]}, - {"t":1.53611, "x":3.87019, "y":2.71329, "heading":1.03476, "vx":0.81211, "vy":0.75175, "omega":0.00186, "ax":-3.3748, "ay":-3.12398, "alpha":-0.0112, "fx":[-55.14306,-55.22281,-55.20008,-55.12041], "fy":[-51.10142,-51.01547,-51.04083,-51.12662]}, - {"t":1.56612, "x":3.89304, "y":2.73445, "heading":1.03482, "vx":0.71081, "vy":0.65798, "omega":0.00152, "ax":-3.37861, "ay":-3.1275, "alpha":-0.00935, "fx":[-55.21001,-55.27665,-55.25758,-55.19098], "fy":[-51.15407,-51.0822,-51.10333,-51.17509]}, - {"t":1.59614, "x":3.91286, "y":2.75279, "heading":1.03486, "vx":0.60939, "vy":0.5641, "omega":0.00124, "ax":-3.38092, "ay":-3.12964, "alpha":-0.00823, "fx":[-55.25066,-55.30934,-55.2925,-55.23385], "fy":[-51.18604,-51.12273,-51.14129,-51.20453]}, - {"t":1.62616, "x":3.92963, "y":2.76831, "heading":1.0349, "vx":0.50791, "vy":0.47016, "omega":0.00099, "ax":-3.38247, "ay":-3.13108, "alpha":-0.00748, "fx":[-55.27795,-55.3313,-55.31595,-55.26264], "fy":[-51.20751,-51.14995,-51.1668,-51.22429]}, - {"t":1.65617, "x":3.94335, "y":2.78102, "heading":1.03493, "vx":0.40638, "vy":0.37618, "omega":0.00077, "ax":-3.38359, "ay":-3.13211, "alpha":-0.00694, "fx":[-55.29755,-55.34706,-55.3328,-55.28332], "fy":[-51.22292,-51.16949,-51.18512,-51.23849]}, - {"t":1.68619, "x":3.95402, "y":2.7909, "heading":1.03495, "vx":0.30482, "vy":0.28216, "omega":0.00056, "ax":-3.38442, "ay":-3.13288, "alpha":-0.00653, "fx":[-55.3123,-55.35892,-55.34548,-55.29889], "fy":[-51.23452,-51.1842,-51.1989,-51.24917]}, - {"t":1.7162, "x":3.96165, "y":2.79796, "heading":1.03497, "vx":0.20323, "vy":0.18813, "omega":0.00037, "ax":-3.38508, "ay":-3.13349, "alpha":-0.00622, "fx":[-55.32381,-55.36817,-55.35537,-55.31103], "fy":[-51.24356,-51.19567,-51.20966,-51.2575]}, - {"t":1.74622, "x":3.96622, "y":2.80219, "heading":1.03498, "vx":0.10162, "vy":0.09407, "omega":0.00018, "ax":-3.3856, "ay":-3.13398, "alpha":-0.00596, "fx":[-55.33303,-55.37559,-55.3633,-55.32077], "fy":[-51.25082,-51.20488,-51.21829,-51.26418]}, - {"t":1.77624, "x":3.96775, "y":2.8036, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37099, "ay":3.15195, "alpha":0.00824, "fx":[55.08474,55.14519,55.13385,55.07345], "fy":[51.55463,51.49,51.50228,51.56683]}, + {"t":0.03823, "x":1.6358, "y":0.62698, "heading":0.942, "vx":0.12886, "vy":0.12049, "omega":0.00032, "ax":3.3708, "ay":3.15177, "alpha":0.0084, "fx":[55.08117,55.14279,55.13123,55.06966], "fy":[51.55221,51.48632,51.49884,51.56465]}, + {"t":0.07645, "x":1.64319, "y":0.63389, "heading":0.94201, "vx":0.25772, "vy":0.24097, "omega":0.00064, "ax":3.37059, "ay":3.15158, "alpha":0.00858, "fx":[55.0772,55.14013,55.12833,55.06545], "fy":[51.54952,51.48223,51.49503,51.56223]}, + {"t":0.11468, "x":1.6555, "y":0.6454, "heading":0.94204, "vx":0.38657, "vy":0.36145, "omega":0.00096, "ax":3.37036, "ay":3.15135, "alpha":0.00878, "fx":[55.07276,55.13715,55.12508,55.06075], "fy":[51.54652,51.47767,51.49077,51.55952]}, + {"t":0.15291, "x":1.67274, "y":0.66152, "heading":0.94207, "vx":0.51541, "vy":0.48192, "omega":0.0013, "ax":3.37009, "ay":3.1511, "alpha":0.009, "fx":[55.06777,55.1338,55.12142,55.05545], "fy":[51.54314,51.47253,51.48597,51.55648]}, + {"t":0.19114, "x":1.69491, "y":0.68225, "heading":0.94212, "vx":0.64424, "vy":0.60238, "omega":0.00164, "ax":3.36979, "ay":3.15082, "alpha":0.00926, "fx":[55.06211,55.13001,55.11728,55.04945], "fy":[51.5393,51.46671,51.48054,51.55303]}, + {"t":0.22936, "x":1.722, "y":0.70758, "heading":0.94219, "vx":0.77306, "vy":0.72282, "omega":0.002, "ax":3.36945, "ay":3.1505, "alpha":0.00955, "fx":[55.05564,55.12567,55.11255,55.04259], "fy":[51.53492,51.46006,51.47433,51.54908]}, + {"t":0.26759, "x":1.75401, "y":0.73751, "heading":0.94226, "vx":0.90186, "vy":0.84326, "omega":0.00236, "ax":3.36906, "ay":3.15012, "alpha":0.00988, "fx":[55.04819,55.12066,55.10709,55.03468], "fy":[51.52986,51.45238,51.46717,51.54454]}, + {"t":0.30582, "x":1.79095, "y":0.77205, "heading":0.94235, "vx":1.03065, "vy":0.96368, "omega":0.00274, "ax":3.3686, "ay":3.14969, "alpha":0.01028, "fx":[55.03948,55.11483,55.10071,55.02545], "fy":[51.52397,51.44342,51.45882,51.53923]}, + {"t":0.34405, "x":1.83281, "y":0.81119, "heading":0.94246, "vx":1.15942, "vy":1.08408, "omega":0.00313, "ax":3.36805, "ay":3.14917, "alpha":0.01074, "fx":[55.0292,55.10793,55.09318,55.01454], "fy":[51.51699,51.43283,51.44895,51.53296]}, + {"t":0.38227, "x":1.87959, "y":0.85493, "heading":0.94258, "vx":1.28817, "vy":1.20447, "omega":0.00354, "ax":3.3674, "ay":3.14855, "alpha":0.0113, "fx":[55.01686,55.09965,55.08415,55.00146], "fy":[51.50863,51.42013,51.4371,51.52545]}, + {"t":0.4205, "x":1.93129, "y":0.90328, "heading":0.94271, "vx":1.4169, "vy":1.32483, "omega":0.00398, "ax":3.3666, "ay":3.1478, "alpha":0.01198, "fx":[55.00177,55.08954,55.07312,54.98546], "fy":[51.49841,51.4046,51.42263,51.51626]}, + {"t":0.45873, "x":1.98792, "y":0.95622, "heading":0.94286, "vx":1.5456, "vy":1.44516, "omega":0.00443, "ax":3.3656, "ay":3.14685, "alpha":0.01283, "fx":[54.98292,55.0769,55.05933,54.96548], "fy":[51.48563,51.3852,51.40455,51.50478]}, + {"t":0.49695, "x":2.04946, "y":1.01376, "heading":0.94303, "vx":1.67425, "vy":1.56545, "omega":0.00492, "ax":3.36432, "ay":3.14564, "alpha":0.01392, "fx":[54.95869,55.06065,55.04161,54.93979], "fy":[51.46921,51.36025,51.38131,51.49003]}, + {"t":0.53518, "x":2.11592, "y":1.0759, "heading":0.94322, "vx":1.80286, "vy":1.6857, "omega":0.00546, "ax":3.36261, "ay":3.14402, "alpha":0.01538, "fx":[54.92638,55.03899,55.01799,54.90556], "fy":[51.44732,51.32699,51.35035,51.47038]}, + {"t":0.57341, "x":2.1873, "y":1.14264, "heading":0.94343, "vx":1.93141, "vy":1.80589, "omega":0.00604, "ax":3.36022, "ay":3.14176, "alpha":0.01742, "fx":[54.88117,55.00869,54.98497,54.85768], "fy":[51.41668,51.28045,51.30703,51.44289]}, + {"t":0.61164, "x":2.26359, "y":1.21397, "heading":0.94366, "vx":2.05986, "vy":1.92599, "omega":0.00671, "ax":3.35663, "ay":3.13837, "alpha":0.02049, "fx":[54.81339,54.96327,54.93551,54.78595], "fy":[51.37076,51.2107,51.24216,51.4017]}, + {"t":0.64986, "x":2.34478, "y":1.28989, "heading":0.94392, "vx":2.18817, "vy":2.04596, "omega":0.00749, "ax":3.35067, "ay":3.13273, "alpha":0.02561, "fx":[54.70054,54.88766,54.85328,54.66665], "fy":[51.2943,51.09458,51.1343,51.33321]}, + {"t":0.68809, "x":2.43088, "y":1.37039, "heading":0.9442, "vx":2.31626, "vy":2.16572, "omega":0.00847, "ax":3.33878, "ay":3.12148, "alpha":0.03586, "fx":[54.47535,54.73675,54.68954,54.42911], "fy":[51.14169,50.86299,50.91962,51.19675]}, + {"t":0.72632, "x":2.52186, "y":1.45546, "heading":0.94453, "vx":2.44389, "vy":2.28504, "omega":0.00984, "ax":3.30345, "ay":3.08806, "alpha":0.06656, "fx":[53.80435,54.2864,54.20408,53.72533], "fy":[50.68611,50.17385,50.2843,50.79129]}, + {"t":0.76455, "x":2.6177, "y":1.54507, "heading":0.94491, "vx":2.57017, "vy":2.40309, "omega":0.01239, "ax":0.29815, "ay":0.26122, "alpha":2.73059, "fx":[-5.76398,6.87286,15.27546,3.1122], "fy":[2.8179,-6.30561,5.82634,14.74326]}, + {"t":0.80277, "x":2.71617, "y":1.63712, "heading":0.94538, "vx":2.58157, "vy":2.41308, "omega":0.11677, "ax":-3.30212, "ay":-3.08838, "alpha":0.06006, "fx":[-54.16293,-53.73074,-53.80239,-54.23727], "fy":[-50.30918,-50.76667,-50.67122,-50.20944]}, + {"t":0.841, "x":2.81244, "y":1.72711, "heading":0.94984, "vx":2.45534, "vy":2.29502, "omega":0.11907, "ax":-3.33825, "ay":-3.12177, "alpha":0.02849, "fx":[-54.65826,-54.45149,-54.48942,-54.6968], "fy":[-50.94784,-51.16783,-51.12277,-50.90179]}, + {"t":0.87923, "x":2.90386, "y":1.81256, "heading":0.95439, "vx":2.32773, "vy":2.17568, "omega":0.12016, "ax":-3.35034, "ay":-3.13295, "alpha":0.0178, "fx":[-54.82396,-54.69444,-54.71936,-54.84912], "fy":[-51.16315,-51.30118,-51.27264,-51.13422]}, + {"t":0.91745, "x":2.9904, "y":1.89344, "heading":0.95899, "vx":2.19965, "vy":2.05592, "omega":0.12084, "ax":-3.3564, "ay":-3.13855, "alpha":0.01237, "fx":[-54.90677,-54.81666,-54.83463,-54.92485], "fy":[-51.27138,-51.36749,-51.3473,-51.251]}, + {"t":0.95568, "x":3.07203, "y":1.96974, "heading":0.96361, "vx":2.07135, "vy":1.93594, "omega":0.12131, "ax":-3.36004, "ay":-3.14191, "alpha":0.00907, "fx":[-54.95641,-54.89034,-54.90391,-54.97004], "fy":[-51.33654,-51.40706,-51.39197,-51.32135]}, + {"t":0.99391, "x":3.14876, "y":2.04145, "heading":0.96824, "vx":1.9429, "vy":1.81583, "omega":0.12166, "ax":-3.36246, "ay":-3.14415, "alpha":0.00685, "fx":[-54.98947,-54.93964,-54.95015,-55.00002], "fy":[-51.38009,-51.43329,-51.42168,-51.36842]}, + {"t":1.03214, "x":3.22057, "y":2.10857, "heading":0.97289, "vx":1.81437, "vy":1.69564, "omega":0.12192, "ax":-3.36419, "ay":-3.14575, "alpha":0.00524, "fx":[-55.01307,-54.97497,-54.98321,-55.02133], "fy":[-51.41124,-51.45192,-51.44286,-51.40214]}, + {"t":1.07036, "x":3.28747, "y":2.17109, "heading":0.97756, "vx":1.68576, "vy":1.57539, "omega":0.12212, "ax":-3.36549, "ay":-3.14695, "alpha":0.00402, "fx":[-55.03076,-55.00154,-55.00802,-55.03725], "fy":[-51.43462,-51.46582,-51.45874,-51.42751]}, + {"t":1.10859, "x":3.34946, "y":2.22902, "heading":0.98222, "vx":1.55711, "vy":1.45509, "omega":0.12227, "ax":-3.3665, "ay":-3.14789, "alpha":0.00306, "fx":[-55.04452,-55.02226,-55.02731,-55.04957], "fy":[-51.45282,-51.47659,-51.47108,-51.4473]}, + {"t":1.14682, "x":3.40652, "y":2.28234, "heading":0.9869, "vx":1.42842, "vy":1.33475, "omega":0.12239, "ax":-3.36731, "ay":-3.14863, "alpha":0.00229, "fx":[-55.05553,-55.03888,-55.04274,-55.05939], "fy":[-51.46737,-51.48515,-51.48095,-51.46316]}, + {"t":1.18505, "x":3.45866, "y":2.33106, "heading":0.99158, "vx":1.29969, "vy":1.21439, "omega":0.12248, "ax":-3.36797, "ay":-3.14924, "alpha":0.00166, "fx":[-55.06454,-55.05251,-55.05536,-55.0674], "fy":[-51.47928,-51.49213,-51.48903,-51.47618]}, + {"t":1.22327, "x":3.50589, "y":2.37518, "heading":0.99626, "vx":1.17094, "vy":1.094, "omega":0.12254, "ax":-3.36853, "ay":-3.14975, "alpha":0.00113, "fx":[-55.07206,-55.06389,-55.06587,-55.07404], "fy":[-51.48919,-51.49792,-51.49578,-51.48705]}, + {"t":1.2615, "x":3.54819, "y":2.4147, "heading":1.00094, "vx":1.04217, "vy":0.97359, "omega":0.12258, "ax":-3.36899, "ay":-3.15019, "alpha":0.00068, "fx":[-55.07844,-55.07355,-55.07476,-55.07965], "fy":[-51.49757,-51.5028,-51.50149,-51.49626]}, + {"t":1.29973, "x":3.58557, "y":2.44962, "heading":1.00563, "vx":0.91339, "vy":0.85317, "omega":0.12261, "ax":0.09524, "ay":-0.11403, "alpha":0.18612, "fx":[0.84597,1.72255,2.26779,1.39144], "fy":[-2.01624,-2.5709,-1.71253,-1.157]}, + {"t":1.33225, "x":3.61532, "y":2.47731, "heading":1.00962, "vx":0.91648, "vy":0.84946, "omega":0.12866, "ax":0.01311, "ay":-0.01415, "alpha":0.03201, "fx":[0.09221,0.2434,0.33644,0.18525], "fy":[-0.25785,-0.35288,-0.20488,-0.10984]}, + {"t":1.36477, "x":3.64513, "y":2.50492, "heading":1.0138, "vx":0.91691, "vy":0.849, "omega":0.1297, "ax":0.0017, "ay":-0.00184, "alpha":-0.12236, "fx":[0.49415,-0.08526,-0.4385,0.14091], "fy":[0.07313,0.43397,-0.13324,-0.49408]}, + {"t":1.39729, "x":3.67495, "y":2.53253, "heading":1.01802, "vx":0.91697, "vy":0.84894, "omega":0.12572, "ax":0.00022, "ay":-0.00024, "alpha":-0.27722, "fx":[1.05919,-0.25703,-1.05196,0.26426], "fy":[0.23434,1.04636,-0.24215,-1.05417]}, + {"t":1.42981, "x":3.70477, "y":2.56014, "heading":1.02211, "vx":0.91697, "vy":0.84894, "omega":0.11671, "ax":0.00003, "ay":-0.00003, "alpha":-0.43323, "fx":[1.64851,-0.41356,-1.64757,0.4145], "fy":[0.37855,1.6391,-0.37956,-1.64011]}, + {"t":1.46233, "x":3.73459, "y":2.58775, "heading":1.0259, "vx":0.91697, "vy":0.84893, "omega":0.10262, "ax":0.0, "ay":0.0, "alpha":-0.591, "fx":[2.2463,-0.57323,-2.24618,0.57335], "fy":[0.52556,2.23449,-0.52569,-2.23462]}, + {"t":1.49485, "x":3.76441, "y":2.61536, "heading":1.02924, "vx":0.91697, "vy":0.84893, "omega":0.0834, "ax":0.0, "ay":0.0, "alpha":-0.75117, "fx":[2.85277,-0.73814,-2.85277,0.73814], "fy":[0.6776,2.83771,-0.6776,-2.83771]}, + {"t":1.52737, "x":3.79423, "y":2.64296, "heading":1.03195, "vx":0.91697, "vy":0.84893, "omega":0.05897, "ax":-0.00007, "ay":-0.00006, "alpha":-0.91438, "fx":[3.46913,-0.90911,-3.47157,0.90665], "fy":[0.83322,3.4508,-0.83527,-3.45284]}, + {"t":1.55989, "x":3.82406, "y":2.67057, "heading":1.03387, "vx":0.91697, "vy":0.84893, "omega":0.02923, "ax":-1.16985, "ay":-1.08306, "alpha":-0.85354, "fx":[-15.96955,-20.39596,-22.18884,-17.94513], "fy":[-17.32205,-14.3404,-18.13458,-21.02687]}, + {"t":1.59242, "x":3.85326, "y":2.69761, "heading":1.03482, "vx":0.87893, "vy":0.81371, "omega":0.00148, "ax":-3.35541, "ay":-3.10643, "alpha":-0.01318, "fx":[-54.82079,-54.9143,-54.88826,-54.79485], "fy":[-50.81945,-50.71901,-50.74923,-50.84946]}, + {"t":1.62494, "x":3.88007, "y":2.72243, "heading":1.03487, "vx":0.76981, "vy":0.71269, "omega":0.00105, "ax":-3.37272, "ay":-3.12246, "alpha":-0.0075, "fx":[-55.11842,-55.17182,-55.15664,-55.10327], "fy":[-51.06657,-51.00905,-51.02605,-51.0835]}, + {"t":1.65746, "x":3.90332, "y":2.74395, "heading":1.0349, "vx":0.66012, "vy":0.61114, "omega":0.0008, "ax":-3.37852, "ay":-3.12783, "alpha":-0.0056, "fx":[-55.21816,-55.25809,-55.24666,-55.20674], "fy":[-51.14934,-51.10628,-51.11893,-51.16195]}, + {"t":1.68998, "x":3.923, "y":2.76217, "heading":1.03493, "vx":0.55025, "vy":0.50942, "omega":0.00062, "ax":-3.38143, "ay":-3.13053, "alpha":-0.00466, "fx":[-55.2681,-55.30129,-55.29176,-55.25858], "fy":[-51.19079,-51.15499,-51.16547,-51.20125]}, + {"t":1.7225, "x":3.93911, "y":2.77709, "heading":1.03495, "vx":0.44028, "vy":0.40761, "omega":0.00047, "ax":-3.38318, "ay":-3.13214, "alpha":-0.00409, "fx":[-55.29809,-55.32723,-55.31884,-55.28971], "fy":[-51.21567,-51.18423,-51.19342,-51.22484]}, + {"t":1.75502, "x":3.95164, "y":2.78869, "heading":1.03496, "vx":0.33026, "vy":0.30575, "omega":0.00034, "ax":-3.38434, "ay":-3.13322, "alpha":-0.00371, "fx":[-55.31809,-55.34453,-55.33691,-55.31048], "fy":[-51.23227,-51.20374,-51.21207,-51.24058]}, + {"t":1.78754, "x":3.96059, "y":2.79697, "heading":1.03497, "vx":0.2202, "vy":0.20386, "omega":0.00022, "ax":-3.38517, "ay":-3.13399, "alpha":-0.00343, "fx":[-55.33238,-55.35689,-55.34982,-55.32532], "fy":[-51.24412,-51.21767,-51.22539,-51.25183]}, + {"t":1.82006, "x":3.96596, "y":2.80194, "heading":1.03498, "vx":0.11011, "vy":0.10194, "omega":0.00011, "ax":-3.3858, "ay":-3.13457, "alpha":-0.00323, "fx":[-55.3431,-55.36616,-55.3595,-55.33645], "fy":[-51.25302,-51.22813,-51.23539,-51.26026]}, + {"t":1.85258, "x":3.96775, "y":2.8036, "heading":1.03499, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/ROtoE.traj b/src/main/deploy/choreo/ROtoE.traj index 3efd6d7a..b30213f4 100644 --- a/src/main/deploy/choreo/ROtoE.traj +++ b/src/main/deploy/choreo/ROtoE.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":7.046594619750977, "y":1.881866693496704, "heading":2.356194490192345, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.009561061859131, "y":2.7982044219970703, "heading":2.0928880900706415, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"7.046594619750977 m", "val":7.046594619750977}, "y":{"exp":"1.881866693496704 m", "val":1.881866693496704}, "heading":{"exp":"180 deg - 45 deg", "val":2.356194490192345}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"E.x", "val":5.009561061859131}, "y":{"exp":"E.y", "val":2.7982044219970703}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,57 +30,58 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.95966,1.51128], + "waypoints":[0.0,0.98844,1.5904], "samples":[ - {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.299, "ay":1.67617, "alpha":-0.06921, "fx":[-70.09973,-70.35334,-70.46085,-70.20797], "fy":[27.86452,27.21888,26.937,27.58836]}, - {"t":0.03309, "x":7.04424, "y":1.88278, "heading":2.35619, "vx":-0.14226, "vy":0.05547, "omega":-0.00229, "ax":-4.29861, "ay":1.6763, "alpha":-0.07063, "fx":[-70.0896,-70.34846,-70.45812,-70.20001], "fy":[27.87604,27.21728,26.9296,27.59432]}, - {"t":0.06618, "x":7.03718, "y":1.88554, "heading":2.35612, "vx":-0.28451, "vy":0.11094, "omega":-0.00463, "ax":-4.29817, "ay":1.67644, "alpha":-0.07221, "fx":[-70.07826,-70.34299,-70.45506,-70.19112], "fy":[27.88892,27.21552,26.92132,27.60095]}, - {"t":0.09928, "x":7.02541, "y":1.89013, "heading":2.35597, "vx":-0.42675, "vy":0.16642, "omega":-0.00702, "ax":-4.29767, "ay":1.67661, "alpha":-0.074, "fx":[-70.0655,-70.33682,-70.45163,-70.18113], "fy":[27.90343,27.21357,26.91198,27.60838]}, - {"t":0.13237, "x":7.00894, "y":1.89655, "heading":2.35573, "vx":-0.56896, "vy":0.2219, "omega":-0.00947, "ax":-4.29711, "ay":1.67679, "alpha":-0.07602, "fx":[-70.05103,-70.32982,-70.44774,-70.16982], "fy":[27.91988,27.21141,26.90138,27.61676]}, - {"t":0.16546, "x":6.98776, "y":1.90481, "heading":2.35542, "vx":-0.71116, "vy":0.27739, "omega":-0.01198, "ax":-4.29647, "ay":1.677, "alpha":-0.07833, "fx":[-70.03448,-70.32181,-70.4433,-70.15689], "fy":[27.93869,27.20896,26.88926,27.62631]}, - {"t":0.19855, "x":6.96187, "y":1.91491, "heading":2.35502, "vx":-0.85334, "vy":0.33288, "omega":-0.01457, "ax":-4.29573, "ay":1.67725, "alpha":-0.081, "fx":[-70.01537,-70.31256,-70.43818,-70.14198], "fy":[27.96041,27.20617,26.87525,27.63731]}, - {"t":0.23164, "x":6.93128, "y":1.92684, "heading":2.35454, "vx":-0.9955, "vy":0.38839, "omega":-0.01725, "ax":-4.29487, "ay":1.67753, "alpha":-0.08412, "fx":[-69.99306,-70.30176,-70.43221,-70.12457], "fy":[27.98574,27.20294,26.85888,27.65011]}, - {"t":0.26474, "x":6.89598, "y":1.94062, "heading":2.35397, "vx":-1.13762, "vy":0.4439, "omega":-0.02004, "ax":-4.29385, "ay":1.67787, "alpha":-0.0878, "fx":[-69.96668,-70.289,-70.42515,-70.10399], "fy":[28.01568,27.19916,26.83953,27.66523]}, - {"t":0.29783, "x":6.85599, "y":1.95622, "heading":2.35331, "vx":-1.27971, "vy":0.49942, "omega":-0.02294, "ax":-4.29262, "ay":1.67827, "alpha":-0.09222, "fx":[-69.935,-70.27368,-70.41667,-70.07928], "fy":[28.0516,27.19464,26.81628,27.68336]}, - {"t":0.33092, "x":6.81129, "y":1.97367, "heading":2.35255, "vx":-1.42176, "vy":0.55496, "omega":-0.02599, "ax":-4.29112, "ay":1.67876, "alpha":-0.09763, "fx":[-69.89625,-70.25497,-70.4063,-70.04903], "fy":[28.09549,27.18913,26.78784,27.70552]}, - {"t":0.36401, "x":6.76189, "y":1.99295, "heading":2.35169, "vx":-1.56376, "vy":0.61051, "omega":-0.02923, "ax":-4.28924, "ay":1.67937, "alpha":-0.10439, "fx":[-69.84778,-70.2316,-70.39333,-70.01116], "fy":[28.15032,27.18227,26.75226,27.73322]}, - {"t":0.3971, "x":6.70779, "y":2.01408, "heading":2.35072, "vx":-1.7057, "vy":0.66609, "omega":-0.03268, "ax":-4.28683, "ay":1.68016, "alpha":-0.1131, "fx":[-69.78538,-70.20158,-70.37662,-69.96237], "fy":[28.22075,27.17347,26.70646,27.76888]}, - {"t":0.43019, "x":6.649, "y":2.03704, "heading":2.34964, "vx":-1.84756, "vy":0.72169, "omega":-0.03642, "ax":-4.2836, "ay":1.68121, "alpha":-0.12472, "fx":[-69.70209,-70.16162,-70.35429,-69.89712], "fy":[28.31453,27.16174,26.64533,27.81649]}, - {"t":0.46329, "x":6.58552, "y":2.06184, "heading":2.34843, "vx":-1.98932, "vy":0.77732, "omega":-0.04055, "ax":-4.27908, "ay":1.68267, "alpha":-0.14101, "fx":[-69.58527,-70.1058,-70.32293,-69.8054], "fy":[28.44557,27.14533,26.55962,27.88328]}, - {"t":0.49638, "x":6.51734, "y":2.08848, "heading":2.34709, "vx":-2.13092, "vy":0.833, "omega":-0.04522, "ax":-4.27228, "ay":1.68486, "alpha":-0.1655, "fx":[-69.40963,-70.02236,-70.27566,-69.667], "fy":[28.64156,27.12075,26.4308,27.98377]}, - {"t":0.52947, "x":6.44449, "y":2.11697, "heading":2.3456, "vx":-2.2723, "vy":0.88876, "omega":-0.05069, "ax":-4.2609, "ay":1.68848, "alpha":-0.20644, "fx":[-69.11582,-69.88402,-70.19625,-69.43419], "fy":[28.96657,27.07989,26.21546,28.15197]}, - {"t":0.56256, "x":6.36696, "y":2.14731, "heading":2.34392, "vx":-2.4133, "vy":0.94463, "omega":-0.05752, "ax":-4.23795, "ay":1.69565, "alpha":-0.28875, "fx":[-68.5244,-69.61003,-70.03505,-68.9604], "fy":[29.61028,26.99872,25.78253,28.49092]}, - {"t":0.59565, "x":6.28478, "y":2.1795, "heading":2.34201, "vx":-2.55354, "vy":1.00074, "omega":-0.06708, "ax":-4.16769, "ay":1.71643, "alpha":-0.539, "fx":[-66.72291,-68.8087,-69.53152,-67.47181], "fy":[31.48802,26.76041,24.46682,29.52635]}, - {"t":0.62875, "x":6.198, "y":2.21355, "heading":2.33979, "vx":-2.69146, "vy":1.05754, "omega":-0.08492, "ax":1.06358, "ay":0.49866, "alpha":-11.13879, "fx":[21.37219,-31.08762,24.22399,55.04135], "fy":[48.90824,14.63109,-38.95345,8.02235]}, - {"t":0.66184, "x":6.10951, "y":2.24882, "heading":2.33698, "vx":-2.65626, "vy":1.07405, "omega":-0.45352, "ax":4.23659, "ay":-1.55405, "alpha":-0.43663, "fx":[70.30043,68.82078,68.19989,69.7195], "fy":[-22.41695,-26.47229,-28.29296,-24.44051]}, - {"t":0.69493, "x":6.02393, "y":2.28351, "heading":2.32198, "vx":-2.51606, "vy":1.02262, "omega":-0.46797, "ax":4.27176, "ay":-1.61302, "alpha":-0.20061, "fx":[70.34075,69.64282,69.32496,70.03204], "fy":[-24.99763,-26.84844,-27.72002,-25.91287]}, - {"t":0.72802, "x":5.94301, "y":2.31647, "heading":2.30649, "vx":-2.3747, "vy":0.96924, "omega":-0.47461, "ax":4.28331, "ay":-1.63306, "alpha":-0.12069, "fx":[70.33489,69.91333,69.71109,70.13632], "fy":[-25.86934,-26.97468,-27.51775,-26.42803]}, - {"t":0.76111, "x":5.86677, "y":2.34765, "heading":2.29079, "vx":-2.23296, "vy":0.9152, "omega":-0.4786, "ax":4.28904, "ay":-1.64316, "alpha":-0.08045, "fx":[70.32786,70.04738,69.90667,70.18895], "fy":[-26.3082,-27.03939,-27.41342,-26.68903]}, - {"t":0.79421, "x":5.79523, "y":2.37704, "heading":2.27495, "vx":-2.09103, "vy":0.86083, "omega":-0.48126, "ax":4.29248, "ay":-1.64924, "alpha":-0.0562, "fx":[70.32209,70.12703,70.02503,70.22105], "fy":[-26.57302,-27.07971,-27.34924,-26.84578]}, - {"t":0.8273, "x":5.72838, "y":2.40462, "heading":2.25902, "vx":-1.94898, "vy":0.80625, "omega":-0.48312, "ax":4.29476, "ay":-1.65331, "alpha":-0.03997, "fx":[70.31745,70.17954,70.10449,70.24294], "fy":[-26.75054,-27.10794,-27.30545,-26.94963]}, - {"t":0.86039, "x":5.66624, "y":2.43039, "heading":2.24303, "vx":-1.80686, "vy":0.75154, "omega":-0.48445, "ax":4.29638, "ay":-1.65621, "alpha":-0.02835, "fx":[70.31367,70.21653,70.16161,70.25904], "fy":[-26.87802,-27.12931,-27.27347,-27.02295]}, - {"t":0.89348, "x":5.6088, "y":2.45436, "heading":2.227, "vx":-1.66468, "vy":0.69673, "omega":-0.48538, "ax":4.2976, "ay":-1.6584, "alpha":-0.01961, "fx":[70.31052,70.24383,70.20469,70.27153], "fy":[-26.97415,-27.14644,-27.24896,-27.07703]}, - {"t":0.92657, "x":5.55606, "y":2.47651, "heading":2.21094, "vx":-1.52247, "vy":0.64185, "omega":-0.48603, "ax":4.29855, "ay":-1.6601, "alpha":-0.0128, "fx":[70.30784,70.26466,70.23838,70.28162], "fy":[-27.04933,-27.16075,-27.22949,-27.11822]}, - {"t":0.95966, "x":5.50803, "y":2.49684, "heading":2.19486, "vx":-1.38022, "vy":0.58692, "omega":-0.48646, "ax":1.71789, "ay":3.61284, "alpha":-0.41395, "fx":[27.59243,25.53559,28.6411,30.56803], "fy":[59.7487,60.12639,58.38095,57.9959]}, - {"t":0.9887, "x":5.46869, "y":2.5154, "heading":2.18073, "vx":-1.33035, "vy":0.69181, "omega":-0.49848, "ax":1.36561, "ay":2.46021, "alpha":-0.71883, "fx":[22.11984,18.80876,22.62071,25.75148], "fy":[42.25465,41.49602,38.12459,39.00386]}, - {"t":1.01773, "x":5.43064, "y":2.53652, "heading":2.16626, "vx":-1.2907, "vy":0.76323, "omega":-0.51934, "ax":0.49119, "ay":0.81352, "alpha":-0.2123, "fx":[8.16851,7.19212,7.89246,8.86708], "fy":[14.08808,13.47666,12.50734,13.12565]}, - {"t":1.04676, "x":5.39337, "y":2.55902, "heading":2.15118, "vx":-1.27644, "vy":0.78685, "omega":-0.52551, "ax":0.11215, "ay":0.18109, "alpha":1.10792, "fx":[0.91684,6.08063,2.75335,-2.41719], "fy":[-1.26966,2.12916,7.16823,3.81443]}, - {"t":1.07579, "x":5.35636, "y":2.58194, "heading":2.13593, "vx":-1.27318, "vy":0.79211, "omega":-0.49334, "ax":0.02421, "ay":0.03887, "alpha":2.42888, "fx":[-1.77197,9.66831,2.56619,-8.87959], "fy":[-8.60504,-1.32653,9.85252,2.62087]}, - {"t":1.10483, "x":5.31941, "y":2.60496, "heading":2.1216, "vx":-1.27248, "vy":0.79324, "omega":-0.42283, "ax":0.00531, "ay":0.00852, "alpha":3.69033, "fx":[-3.40974,14.1316,3.58456,-13.95891], "fy":[-13.84056,-3.0539,14.10732,3.34446]}, - {"t":1.13386, "x":5.28247, "y":2.62799, "heading":2.10933, "vx":-1.27233, "vy":0.79348, "omega":-0.31569, "ax":0.00158, "ay":0.00223, "alpha":4.87503, "fx":[-4.81993,18.52638,4.87238,-18.47533], "fy":[-18.36821,-4.41423,18.43562,4.49293]}, - {"t":1.16289, "x":5.24553, "y":2.65103, "heading":2.10016, "vx":-1.27228, "vy":0.79355, "omega":-0.17415, "ax":0.88821, "ay":-0.55438, "alpha":5.43488, "fx":[9.38795,33.33562,22.13789,-6.7796], "fy":[-29.88732,-13.53176,12.09076,-4.92407]}, - {"t":1.19192, "x":5.20897, "y":2.67383, "heading":2.09511, "vx":-1.24649, "vy":0.77745, "omega":-0.01636, "ax":3.85861, "ay":-2.40658, "alpha":0.12768, "fx":[62.59185,63.24888,63.56951,62.9136], "fy":[-40.13078,-39.1061,-38.54679,-39.58858]}, - {"t":1.22096, "x":5.17441, "y":2.69539, "heading":2.09463, "vx":-1.13447, "vy":0.70758, "omega":-0.01266, "ax":3.88928, "ay":-2.42576, "alpha":0.07495, "fx":[63.29184,63.67774,63.87247,63.48702], "fy":[-40.12299,-39.51335,-39.18722,-39.80287]}, - {"t":1.24999, "x":5.14311, "y":2.71491, "heading":2.09426, "vx":-1.02155, "vy":0.63716, "omega":-0.01048, "ax":3.89952, "ay":-2.43217, "alpha":0.0574, "fx":[63.52637,63.82192,63.9728,63.67752], "fy":[-40.11954,-39.6506,-39.40132,-39.8738]}, - {"t":1.27902, "x":5.11509, "y":2.73238, "heading":2.09396, "vx":-0.90834, "vy":0.56655, "omega":-0.00882, "ax":3.90463, "ay":-2.43537, "alpha":0.04864, "fx":[63.64378,63.89419,64.02279,63.77259], "fy":[-40.11764,-39.71947,-39.50841,-39.90914]}, - {"t":1.30805, "x":5.09037, "y":2.7478, "heading":2.0937, "vx":-0.79498, "vy":0.49584, "omega":-0.0074, "ax":3.90771, "ay":-2.43729, "alpha":0.04338, "fx":[63.71426,63.9376,64.05274,63.82957], "fy":[-40.11645,-39.76086,-39.57267,-39.93029]}, - {"t":1.33709, "x":5.06893, "y":2.76117, "heading":2.09349, "vx":-0.68153, "vy":0.42508, "omega":-0.00614, "ax":3.90975, "ay":-2.43857, "alpha":0.03988, "fx":[63.76126,63.96655,64.07267,63.86752], "fy":[-40.11564,-39.78848,-39.6155,-39.94438]}, - {"t":1.36612, "x":5.0508, "y":2.77249, "heading":2.09331, "vx":-0.56802, "vy":0.35428, "omega":-0.00499, "ax":3.91121, "ay":-2.43949, "alpha":0.03738, "fx":[63.79483,63.98724,64.0869,63.89461], "fy":[-40.11504,-39.80823,-39.64609,-39.95443]}, - {"t":1.39515, "x":5.03595, "y":2.78174, "heading":2.09317, "vx":-0.45447, "vy":0.28346, "omega":-0.0039, "ax":3.91231, "ay":-2.44017, "alpha":0.0355, "fx":[63.82001,64.00277,64.09756,63.91492], "fy":[-40.11459,-39.82304,-39.66904,-39.96196]}, - {"t":1.42418, "x":5.02441, "y":2.78894, "heading":2.09305, "vx":-0.34088, "vy":0.21261, "omega":-0.00287, "ax":3.91316, "ay":-2.4407, "alpha":0.03405, "fx":[63.8396,64.01484,64.10584,63.9307], "fy":[-40.11424,-39.83456,-39.68688,-39.96782]}, - {"t":1.45322, "x":5.01616, "y":2.79409, "heading":2.09297, "vx":-0.22727, "vy":0.14175, "omega":-0.00188, "ax":3.91384, "ay":-2.44113, "alpha":0.03288, "fx":[63.85527,64.02451,64.11247,63.94333], "fy":[-40.11395,-39.84378,-39.70116,-39.9725]}, - {"t":1.48225, "x":5.01121, "y":2.79718, "heading":2.09292, "vx":-0.11364, "vy":0.07088, "omega":-0.00093, "ax":3.9144, "ay":-2.44148, "alpha":0.03193, "fx":[63.86809,64.03242,64.11789,63.95365], "fy":[-40.11371,-39.85131,-39.71283,-39.97634]}, - {"t":1.51128, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29685, "ay":1.68221, "alpha":-0.03642, "fx":[-70.14995,-70.28385,-70.34056,-70.20685], "fy":[27.74423,27.40376,27.25669,27.59874]}, + {"t":0.03408, "x":7.0441, "y":1.88284, "heading":2.35619, "vx":-0.14645, "vy":0.05734, "omega":-0.00124, "ax":-4.29649, "ay":1.68224, "alpha":-0.03731, "fx":[-70.14166,-70.27889,-70.33696,-70.19994], "fy":[27.75069,27.40188,27.25118,27.60166]}, + {"t":0.06817, "x":7.03661, "y":1.88578, "heading":2.35615, "vx":-0.2929, "vy":0.11467, "omega":-0.00251, "ax":-4.29608, "ay":1.68227, "alpha":-0.03832, "fx":[-70.13234,-70.2733,-70.33291,-70.19217], "fy":[27.75796,27.39978,27.24498,27.60492]}, + {"t":0.10225, "x":7.02413, "y":1.89066, "heading":2.35607, "vx":-0.43933, "vy":0.17201, "omega":-0.00382, "ax":-4.29561, "ay":1.68231, "alpha":-0.03947, "fx":[-70.12178,-70.26696,-70.32833,-70.18337], "fy":[27.76621,27.3974,27.23794,27.60861]}, + {"t":0.13634, "x":7.00666, "y":1.8975, "heading":2.35594, "vx":-0.58574, "vy":0.22935, "omega":-0.00516, "ax":-4.29508, "ay":1.68235, "alpha":-0.04078, "fx":[-70.1097,-70.25972,-70.32309,-70.17331], "fy":[27.77563,27.3947,27.2299,27.61282]}, + {"t":0.17042, "x":6.9842, "y":1.9063, "heading":2.35576, "vx":-0.73213, "vy":0.2867, "omega":-0.00655, "ax":-4.29447, "ay":1.6824, "alpha":-0.04229, "fx":[-70.09576,-70.25136,-70.31704,-70.16171], "fy":[27.78651,27.39159,27.22061,27.61767]}, + {"t":0.20451, "x":6.95676, "y":1.91704, "heading":2.35554, "vx":-0.87851, "vy":0.34404, "omega":-0.008, "ax":-4.29376, "ay":1.68246, "alpha":-0.04405, "fx":[-70.0795,-70.2416,-70.30999,-70.14817], "fy":[27.7992,27.38798,27.20977,27.62331]}, + {"t":0.23859, "x":6.92432, "y":1.92975, "heading":2.35526, "vx":-1.02486, "vy":0.40138, "omega":-0.0095, "ax":-4.29292, "ay":1.68253, "alpha":-0.04613, "fx":[-70.06028,-70.23008,-70.30166,-70.13217], "fy":[27.8142,27.38372,27.19695,27.62997]}, + {"t":0.27267, "x":6.88689, "y":1.94441, "heading":2.35494, "vx":-1.17118, "vy":0.45873, "omega":-0.01107, "ax":-4.29191, "ay":1.68262, "alpha":-0.04863, "fx":[-70.03721,-70.21625,-70.29166,-70.11296], "fy":[27.83219,27.37862,27.18157,27.63796]}, + {"t":0.30676, "x":6.84448, "y":1.96102, "heading":2.35456, "vx":-1.31746, "vy":0.51608, "omega":-0.01273, "ax":-4.29067, "ay":1.68272, "alpha":-0.05169, "fx":[-70.00901,-70.19936,-70.27944,-70.08948], "fy":[27.85416,27.37239,27.16276,27.64772]}, + {"t":0.34084, "x":6.79708, "y":1.97959, "heading":2.35413, "vx":-1.46371, "vy":0.57344, "omega":-0.01449, "ax":-4.28913, "ay":1.68285, "alpha":-0.05551, "fx":[-69.97376,-70.17826,-70.26415,-70.06011], "fy":[27.88162,27.36462,27.13924,27.65991]}, + {"t":0.37493, "x":6.7447, "y":2.00011, "heading":2.35364, "vx":-1.6099, "vy":0.6308, "omega":-0.01638, "ax":-4.28714, "ay":1.68301, "alpha":-0.06042, "fx":[-69.92844,-70.15116,-70.2445,-70.02231], "fy":[27.91688,27.35465,27.10899,27.67557]}, + {"t":0.40901, "x":6.68734, "y":2.02259, "heading":2.35308, "vx":-1.75602, "vy":0.68816, "omega":-0.01844, "ax":-4.28449, "ay":1.68323, "alpha":-0.06698, "fx":[-69.86801,-70.11507,-70.21828,-69.97187], "fy":[27.96384,27.34137,27.06865,27.69645]}, + {"t":0.4431, "x":6.625, "y":2.04702, "heading":2.35245, "vx":-1.90206, "vy":0.74553, "omega":-0.02072, "ax":-4.28079, "ay":1.68353, "alpha":-0.07616, "fx":[-69.78341,-70.06464,-70.18155,-69.90115], "fy":[28.02945,27.32281,27.01217,27.72568]}, + {"t":0.47718, "x":6.55768, "y":2.07341, "heading":2.35174, "vx":-2.04797, "vy":0.80291, "omega":-0.02332, "ax":-4.27522, "ay":1.68398, "alpha":-0.08995, "fx":[-69.65652,-69.98921,-70.12639,-69.79486], "fy":[28.12757,27.29505,26.92743,27.76952]}, + {"t":0.51126, "x":6.4854, "y":2.10175, "heading":2.35095, "vx":-2.19368, "vy":0.86031, "omega":-0.02638, "ax":-4.26595, "ay":1.68472, "alpha":-0.11294, "fx":[-69.4451,-69.86403,-70.0343,-69.61714], "fy":[28.29029,27.24895,26.78619,27.84255]}, + {"t":0.54535, "x":6.40815, "y":2.13206, "heading":2.35005, "vx":-2.33908, "vy":0.91773, "omega":-0.03023, "ax":-4.2474, "ay":1.68616, "alpha":-0.15898, "fx":[-69.02254,-69.61574,-69.84949,-69.25958], "fy":[28.61252,27.15746,26.50378,27.98841]}, + {"t":0.57943, "x":6.32595, "y":2.16432, "heading":2.34902, "vx":-2.48385, "vy":0.9752, "omega":-0.03565, "ax":-4.1917, "ay":1.69016, "alpha":-0.29744, "fx":[-67.7581,-68.88695,-69.28989,-68.17009], "fy":[29.55318,26.8887,25.65784,28.4239]}, + {"t":0.61352, "x":6.23886, "y":2.19854, "heading":2.3478, "vx":-2.62672, "vy":1.03281, "omega":-0.04579, "ax":0.48096, "ay":0.5564, "alpha":-9.13365, "fx":[9.21905,-29.62251,10.04063,41.81374], "fy":[42.62723,12.16863,-28.58004,10.16854]}, + {"t":0.6476, "x":6.14961, "y":2.23406, "heading":2.34624, "vx":-2.61033, "vy":1.05178, "omega":-0.3571, "ax":4.22778, "ay":-1.60063, "alpha":-0.25597, "fx":[69.74402,68.83973,68.48275,69.39804], "fy":[-24.44306,-26.81238,-27.85534,-25.55844]}, + {"t":0.68168, "x":6.06309, "y":2.26898, "heading":2.33407, "vx":-2.46623, "vy":0.99722, "omega":-0.36583, "ax":4.26547, "ay":-1.6408, "alpha":-0.11893, "fx":[70.03598,69.61101,69.42723,69.85489], "fy":[-26.01974,-27.11939,-27.62037,-26.53659]}, + {"t":0.71577, "x":5.98151, "y":2.30202, "heading":2.3216, "vx":-2.32085, "vy":0.94129, "omega":-0.36988, "ax":4.27801, "ay":-1.65434, "alpha":-0.073, "fx":[70.1268,69.86591,69.74724,70.00926], "fy":[-26.54946,-27.2216,-27.53813,-26.87191]}, + {"t":0.74985, "x":5.90489, "y":2.33314, "heading":2.30899, "vx":-2.17503, "vy":0.88491, "omega":-0.37237, "ax":4.28428, "ay":-1.66113, "alpha":-0.04995, "fx":[70.17077,69.99276,69.90842,70.08702], "fy":[-26.81558,-27.27316,-27.49575,-27.0409]}, + {"t":0.78394, "x":5.83325, "y":2.36234, "heading":2.2963, "vx":-2.02901, "vy":0.82829, "omega":-0.37407, "ax":4.28803, "ay":-1.66522, "alpha":-0.03608, "fx":[70.19658,70.06854,70.00561,70.13398], "fy":[-26.97594,-27.3046,-27.46964,-27.14238]}, + {"t":0.81802, "x":5.76658, "y":2.3896, "heading":2.28355, "vx":-1.88285, "vy":0.77153, "omega":-0.3753, "ax":4.29054, "ay":-1.66795, "alpha":-0.0268, "fx":[70.21347,70.11882,70.07067,70.16552], "fy":[-27.0833,-27.32603,-27.45179,-27.20981]}, + {"t":0.85211, "x":5.7049, "y":2.41493, "heading":2.27076, "vx":-1.73661, "vy":0.71468, "omega":-0.37622, "ax":4.29233, "ay":-1.6699, "alpha":-0.02016, "fx":[70.22534,70.15454,70.11731,70.18824], "fy":[-27.16031,-27.34176,-27.43871,-27.25768]}, + {"t":0.88619, "x":5.6482, "y":2.43832, "heading":2.25794, "vx":-1.59031, "vy":0.65776, "omega":-0.3769, "ax":4.29367, "ay":-1.67136, "alpha":-0.01517, "fx":[70.23411,70.18116,70.15241,70.20544], "fy":[-27.21832,-27.35396,-27.42866,-27.29325]}, + {"t":0.92027, "x":5.59649, "y":2.45977, "heading":2.24509, "vx":-1.44397, "vy":0.6008, "omega":-0.37742, "ax":4.29471, "ay":-1.6725, "alpha":-0.01128, "fx":[70.24083,70.20171,70.1798,70.21897], "fy":[-27.26364,-27.3638,-27.42064,-27.3206]}, + {"t":0.95436, "x":5.54977, "y":2.47927, "heading":2.23223, "vx":-1.29758, "vy":0.54379, "omega":-0.3778, "ax":4.29554, "ay":-1.67341, "alpha":-0.00816, "fx":[70.24613,70.21801,70.20178,70.22992], "fy":[-27.30005,-27.37202,-27.41407,-27.34217]}, + {"t":0.98844, "x":5.50803, "y":2.49684, "heading":2.21935, "vx":-1.15117, "vy":0.48675, "omega":-0.37808, "ax":1.69706, "ay":3.5182, "alpha":-0.42607, "fx":[27.20457,25.18554,28.35205,30.23272], "fy":[58.29849,58.56483,56.73115,56.46929]}, + {"t":1.01854, "x":5.47416, "y":2.51308, "heading":2.20797, "vx":-1.1001, "vy":0.59264, "omega":-0.39091, "ax":0.99104, "ay":1.73812, "alpha":-1.28091, "fx":[16.3411,10.58259,16.20995,21.67312], "fy":[32.66817,29.93993,23.95521,27.09652]}, + {"t":1.04864, "x":5.44149, "y":2.5317, "heading":2.1962, "vx":-1.07027, "vy":0.64496, "omega":-0.42946, "ax":0.20087, "ay":0.32987, "alpha":-1.17008, "fx":[4.03949,-1.25698,2.53927,7.81385], "fy":[9.85294,6.12057,0.88796,4.70939]}, + {"t":1.07874, "x":5.40937, "y":2.55127, "heading":2.18328, "vx":-1.06422, "vy":0.65489, "omega":-0.46468, "ax":0.03202, "ay":0.05195, "alpha":-0.49616, "fx":[0.87684,-1.38813,0.17038,2.43481], "fy":[2.75261,1.1631,-1.05534,0.53673]}, + {"t":1.10883, "x":5.37736, "y":2.571, "heading":2.16929, "vx":-1.06326, "vy":0.65645, "omega":-0.47961, "ax":0.005, "ay":0.0081, "alpha":0.19584, "fx":[-0.06822,0.83439,0.23183,-0.6708], "fy":[-0.61713,-0.00157,0.88201,0.26652]}, + {"t":1.13893, "x":5.34536, "y":2.59076, "heading":2.15486, "vx":-1.06311, "vy":0.65669, "omega":-0.47371, "ax":0.00078, "ay":0.00126, "alpha":0.88726, "fx":[-0.71591,3.41331,0.7414,-3.38785], "fy":[-3.36528,-0.63579,3.40642,0.67713]}, + {"t":1.16903, "x":5.31336, "y":2.61053, "heading":2.1406, "vx":-1.06308, "vy":0.65673, "omega":-0.44701, "ax":0.00012, "ay":0.0002, "alpha":1.57533, "fx":[-1.37731,6.02246,1.38129,-6.01849], "fy":[-5.9893,-1.24827,5.99568,1.25475]}, + {"t":1.19913, "x":5.28136, "y":2.63029, "heading":2.12715, "vx":-1.06308, "vy":0.65674, "omega":-0.3996, "ax":0.00002, "ay":0.00003, "alpha":2.25748, "fx":[-2.09161,8.6029,2.09224,-8.60227], "fy":[-8.55952,-1.90883,8.56052,1.90986]}, + {"t":1.22922, "x":5.24937, "y":2.65006, "heading":2.11512, "vx":-1.06308, "vy":0.65674, "omega":-0.33165, "ax":0.0, "ay":0.00001, "alpha":2.93129, "fx":[-2.84973,11.1397,2.84987,-11.13957], "fy":[-11.08141,-2.6133,11.08162,2.61352]}, + {"t":1.25932, "x":5.21737, "y":2.66983, "heading":2.10514, "vx":-1.06308, "vy":0.65674, "omega":-0.24343, "ax":0.00001, "ay":0.00001, "alpha":3.59446, "fx":[-3.62976,13.62744,3.63025,-13.62697], "fy":[-13.55294,-3.34074,13.55315,3.34097]}, + {"t":1.28942, "x":5.18538, "y":2.68959, "heading":2.09781, "vx":-1.06308, "vy":0.65674, "omega":-0.13524, "ax":0.09125, "ay":-0.05639, "alpha":4.24059, "fx":[-2.83029,17.45034,5.98188,-14.63487], "fy":[-16.8811,-4.97731,15.04351,3.12734]}, + {"t":1.31952, "x":5.15342, "y":2.70933, "heading":2.09374, "vx":-1.06033, "vy":0.65504, "omega":-0.00761, "ax":3.87672, "ay":-2.39492, "alpha":0.07248, "fx":[63.09996,63.46942,63.6538,63.28479], "fy":[-39.60327,-39.0181,-38.69873,-39.28939]}, + {"t":1.34962, "x":5.12326, "y":2.72796, "heading":2.09351, "vx":-0.94365, "vy":0.58296, "omega":-0.00543, "ax":3.9051, "ay":-2.41245, "alpha":0.03901, "fx":[63.6903,63.88924,63.99157,63.79278], "fy":[-39.68345,-39.36484,-39.19373,-39.51397]}, + {"t":1.37971, "x":5.09663, "y":2.74442, "heading":2.09335, "vx":-0.82612, "vy":0.51035, "omega":-0.00425, "ax":3.91387, "ay":-2.41787, "alpha":0.02871, "fx":[63.87309,64.0195,64.09552,63.94919], "fy":[-39.70785,-39.47257,-39.34682,-39.58298]}, + {"t":1.40981, "x":5.07354, "y":2.75868, "heading":2.09322, "vx":-0.70832, "vy":0.43758, "omega":-0.00339, "ax":3.91813, "ay":-2.4205, "alpha":0.0237, "fx":[63.96201,64.0829,64.14597,64.02513], "fy":[-39.71965,-39.52504,-39.42125,-39.61647]}, + {"t":1.43991, "x":5.05399, "y":2.77075, "heading":2.09312, "vx":-0.59039, "vy":0.36473, "omega":-0.00268, "ax":3.92065, "ay":-2.42206, "alpha":0.02075, "fx":[64.01458,64.1204,64.17577,64.06998], "fy":[-39.7266,-39.55608,-39.46525,-39.63624]}, + {"t":1.47001, "x":5.038, "y":2.78064, "heading":2.09304, "vx":-0.47239, "vy":0.29183, "omega":-0.00205, "ax":3.92231, "ay":-2.42309, "alpha":0.0188, "fx":[64.0493,64.14518,64.19543,64.09959], "fy":[-39.73118,-39.57659,-39.49432,-39.64929]}, + {"t":1.5001, "x":5.02556, "y":2.78832, "heading":2.09298, "vx":-0.35434, "vy":0.2189, "omega":-0.00149, "ax":3.9235, "ay":-2.42382, "alpha":0.01742, "fx":[64.07395,64.16277,64.20939,64.1206], "fy":[-39.73443,-39.59115,-39.51494,-39.65855]}, + {"t":1.5302, "x":5.01667, "y":2.79381, "heading":2.09293, "vx":-0.23625, "vy":0.14595, "omega":-0.00096, "ax":3.92438, "ay":-2.42436, "alpha":0.01638, "fx":[64.09235,64.1759,64.2198,64.13628], "fy":[-39.73685,-39.60202,-39.53034,-39.66546]}, + {"t":1.5603, "x":5.01134, "y":2.79711, "heading":2.0929, "vx":-0.11814, "vy":0.07298, "omega":-0.00047, "ax":3.92506, "ay":-2.42478, "alpha":0.01558, "fx":[64.10661,64.18608,64.22787,64.14842], "fy":[-39.73872,-39.61045,-39.54227,-39.67081]}, + {"t":1.5904, "x":5.00956, "y":2.7982, "heading":2.09289, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/choreo.chor b/src/main/deploy/choreo/choreo.chor index 4539c5c2..6f163365 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -7,8 +7,8 @@ "slow":{ "dimension":"LinVel", "var":{ - "exp":"1.5 m / s", - "val":1.5 + "exp":"1.25 m / s", + "val":1.25 } } }, From 1693fffd497d585c08f234bdabb9446a167c6859 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 10:20:22 -0700 Subject: [PATCH 065/126] make shoulder manual zero ignore disasble --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ed65d608..e205e31c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -615,7 +615,7 @@ public Robot() { () -> { elevator.resetExtension(0.0); wrist.resetPosition(Rotation2d.k180deg); - })); + }).ignoringDisable(true)); // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() From a0af06862615ac1c6ae801a8a7746f5602375375 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 10:20:22 -0700 Subject: [PATCH 066/126] make shoulder manual zero ignore disasble --- src/main/java/frc/robot/Robot.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ed65d608..9a19cbd7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -612,10 +612,11 @@ public Robot() { SmartDashboard.putData( "Manual Zero Extension", Commands.runOnce( - () -> { - elevator.resetExtension(0.0); - wrist.resetPosition(Rotation2d.k180deg); - })); + () -> { + elevator.resetExtension(0.0); + wrist.resetPosition(Rotation2d.k180deg); + }) + .ignoringDisable(true)); // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() From 650c6f8f33b98ef11b4654e2edf94e3a6cba0f90 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 14:02:54 -0700 Subject: [PATCH 067/126] make leds orange when not zeroed --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9a19cbd7..1c96e939 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -716,7 +716,7 @@ public Robot() { DriverStation.getAlliance() .map((a) -> a == Alliance.Blue ? Color.kBlue : Color.kRed) .orElse(Color.kWhite), - () -> LEDSubsystem.PURPLE, + () -> wrist.hasZeroed ? LEDSubsystem.PURPLE : Color.kOrange, 4, 1.0) .until(() -> DriverStation.isEnabled()), From b092e4d461e51a135a1a22f4d1f4986c3b909e44 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 15:02:00 -0700 Subject: [PATCH 068/126] make anti jam hold position and spit coral --- src/main/java/frc/robot/subsystems/Superstructure.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 688d6335..95be17ed 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -582,8 +582,10 @@ private void configureStateTransitionCommands() { // ANTI_JAM logic stateTriggers .get(SuperState.ANTI_JAM) - .whileTrue(elevator.setExtension(ElevatorSubsystem.L2_EXTENSION_METERS)) - .whileTrue(manipulator.setVelocity(10)) + .whileTrue(elevator.hold()) + .whileTrue(wrist.hold()) + .whileTrue(shoulder.hold()) + .whileTrue(manipulator.setVelocity(-10)) .whileTrue(funnel.setVoltage(-10.0)); stateTriggers From d53cb0c53d426f7b4cdef8ec172b7a501017f1f2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 17:20:38 -0700 Subject: [PATCH 069/126] make wrist assume starting pose if not alr zeroed --- src/main/java/frc/robot/Robot.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1c96e939..880a315d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -642,6 +642,14 @@ public Robot() { new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoGroundIntake = false)); + new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) + .onTrue( + Commands.runOnce( + () -> { + wrist.resetPosition(Rotation2d.fromRadians(3.059)); + elevator.resetExtension(0.0); + })); + new Trigger( () -> { var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); From fa2e20d5ab2b01c98ec6927117000ed37ad497f4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 17:43:41 -0700 Subject: [PATCH 070/126] adjsut l4 pose --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 54eae9f4..4cd89a77 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.37, 2.0), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 0add3648386989290244929feb354eb1b5a092ef Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Fri, 4 Apr 2025 18:12:13 -0700 Subject: [PATCH 071/126] adjust hp pose --- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index f74e9def..0ffacf51 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(165.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); From 84b024af66ac54609e304552021729a2ca7beb3f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 08:23:17 -0700 Subject: [PATCH 072/126] Update at 'Sat Apr 05 08:23:17 PDT 2025' --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 96dfc3dd..472226fa 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -81,7 +81,7 @@ public void periodic() { if (!firstBBInputs.get && secondBBInputs.get) { // Number calculated from coral length, may need tuning - Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.63)); + Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); zeroTimer.reset(); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 0ffacf51..f74e9def 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(170.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(165.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); From 1e1c16cbaa851277dff9967491761ad34c6f7a32 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 08:26:26 -0700 Subject: [PATCH 073/126] Update at 'Sat Apr 05 08:26:26 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 95be17ed..d85322a9 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -408,7 +408,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L1_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) @@ -429,7 +429,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L2_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -447,7 +447,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L3_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -465,7 +465,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L4_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.5 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From a73f7ae6de416a2acc0ecbdd4b415979ddcacf01 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 08:35:03 -0700 Subject: [PATCH 074/126] Update at 'Sat Apr 05 08:35:03 PDT 2025' --- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 1 + src/main/java/frc/robot/subsystems/Superstructure.java | 8 ++++---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 472226fa..d889b3b8 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -25,6 +25,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; public static final double CORAL_INTAKE_VELOCITY = -18.0; + public static final double JOG_POS = 0.6; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; public static final double ALGAE_CURRENT_THRESHOLD = 6.0; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d85322a9..9f1adb1d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -408,7 +408,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L1_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) @@ -429,7 +429,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L2_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -447,7 +447,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L3_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -465,7 +465,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L4_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue(manipulator.jog(() -> 0.7 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); From e2771afae39c2efe3a4c87d6ebfa4c4af437d19d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 08:38:02 -0700 Subject: [PATCH 075/126] Update at 'Sat Apr 05 08:38:02 PDT 2025' --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 880a315d..bf565e7b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -314,7 +314,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.39))) + .withSlot1(new Slot1Configs().withKP(20).withKD(0.5).withKS(0.39))) : new RollerIOSim( 0.01, 5.8677, From 115d9cf380fef5ebd82b26d9cbab083ffaf96bec Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 08:41:38 -0700 Subject: [PATCH 076/126] Update at 'Sat Apr 05 08:41:38 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9f1adb1d..a75d7051 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -225,6 +225,7 @@ private void configureStateTransitionCommands() { ? 5.0 : 0.0))) .and(manipulator::getFirstBeambreak) + .debounce(0.75) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers From 1fde8292b1d6bea3fd9461575372d0a971068c2f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 09:05:42 -0700 Subject: [PATCH 077/126] Update at 'Sat Apr 05 09:05:42 PDT 2025' --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index d889b3b8..6b01e1e0 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -25,7 +25,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; public static final double CORAL_INTAKE_VELOCITY = -18.0; - public static final double JOG_POS = 0.6; + public static final double JOG_POS = 0.7; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; public static final double ALGAE_CURRENT_THRESHOLD = 6.0; @@ -117,7 +117,7 @@ public Command jog(DoubleSupplier rotations) { } public Command hold() { - return this.jog(inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); + return this.jog(() -> inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); } public void resetPosition(final double rotations) { From 4f43c35b8e90c9c8b19f6fd82773f28f4686ed34 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 09:12:29 -0700 Subject: [PATCH 078/126] Update at 'Sat Apr 05 09:12:29 PDT 2025' --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 6b01e1e0..535f91b7 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -25,7 +25,7 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; public static final double CORAL_INTAKE_VELOCITY = -18.0; - public static final double JOG_POS = 0.7; + public static final double JOG_POS = 0.75; public static final double ALGAE_INTAKE_VOLTAGE = 10.0; public static final double ALGAE_HOLDING_VOLTAGE = 1.0; public static final double ALGAE_CURRENT_THRESHOLD = 6.0; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a75d7051..316f4892 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -225,7 +225,7 @@ private void configureStateTransitionCommands() { ? 5.0 : 0.0))) .and(manipulator::getFirstBeambreak) - .debounce(0.75) + .debounce(0.25) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers From 3dc30421fd6152b696ef24583ddecf1d0157841f Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 09:12:58 -0700 Subject: [PATCH 079/126] Update at 'Sat Apr 05 09:12:58 PDT 2025' --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bf565e7b..8113756c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -314,7 +314,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(20).withKD(0.5).withKS(0.39))) + .withSlot1(new Slot1Configs().withKP(10).withKD(0.5).withKS(0.39))) : new RollerIOSim( 0.01, 5.8677, From 4d51253c4ff440912a9eba6857b96b433d70311a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 10:24:57 -0700 Subject: [PATCH 080/126] make ground intake position reset happen sooner --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 316f4892..5fd6fe41 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -314,8 +314,8 @@ private void configureStateTransitionCommands() { .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly()) .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) - .and(intakeCoralReq.negate()) .debounce(0.060) + .and(intakeCoralReq.negate()) .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.5))) .onTrue(this.forceState(SuperState.READY_CORAL)); From de0def12e6c2958c79c8f914095e21bdd3fe90eb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:06:05 -0700 Subject: [PATCH 081/126] increase climb pos --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5fd6fe41..cc6098bf 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -808,7 +808,7 @@ private void configureStateTransitionCommands() { wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS))) .whileTrue( climber - .setPositionSlow(1.3) + .setPositionSlow(1.35) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); // May need more checks to see if canceling is safe From 6b0d6045446a6cf02d3f3fd720659e17c44cdd60 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:08:53 -0700 Subject: [PATCH 082/126] Update at 'Sat Apr 05 11:08:53 PDT 2025' --- .../java/frc/robot/subsystems/Superstructure.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index cc6098bf..0f488c45 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -790,10 +790,8 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.PRE_CLIMB) .whileTrue( - Commands.parallel( - elevator.setExtension(0), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_HP_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS))) + extendWithClearance(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) + .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) .onTrue(funnel.unlatch()) // !! .and(climbConfReq) @@ -802,10 +800,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CLIMB) .whileTrue( - Commands.parallel( - elevator.setExtension(0), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_HP_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_HP_POS))) + extendWithClearance(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( climber .setPositionSlow(1.35) From af12c89ce46de3a8ed7c425bf90755a55f6e3ecb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:15:14 -0700 Subject: [PATCH 083/126] fmt --- .../java/frc/robot/subsystems/Superstructure.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0f488c45..0bc830cc 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -790,8 +790,10 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.PRE_CLIMB) .whileTrue( - extendWithClearance(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) .onTrue(funnel.unlatch()) // !! .and(climbConfReq) @@ -800,7 +802,10 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CLIMB) .whileTrue( - extendWithClearance(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( climber .setPositionSlow(1.35) From aff0805652ed42279dd19b11b451d2a3fba37a84 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:15:37 -0700 Subject: [PATCH 084/126] Update at 'Sat Apr 05 11:15:37 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 4cd89a77..f44e86da 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(80.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From f4f27958df828df181d2cac5d637bd05f1d801de Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:35:25 -0700 Subject: [PATCH 085/126] Update at 'Sat Apr 05 11:35:25 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index f44e86da..4cd89a77 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(80.0)); + new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(90.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From cdcc9b0809c9a0d27ab6fd7954ff9fd26e09b20d Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:36:50 -0700 Subject: [PATCH 086/126] Update at 'Sat Apr 05 11:36:50 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 4cd89a77..ad12f1d4 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(90.0)); + new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(100.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 29640130cb3e77993bcd7e530a6af5d54dbd62b8 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:38:49 -0700 Subject: [PATCH 087/126] Update at 'Sat Apr 05 11:38:49 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index ad12f1d4..c0558395 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(100.0)); + new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From b4100e296835b1b9849e8ec412b54861a3580039 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:40:53 -0700 Subject: [PATCH 088/126] Update at 'Sat Apr 05 11:40:53 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index c0558395..7dde7e17 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.35, 2.0), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.32, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 3489b3d61aec1797f9e2dcb3b4ab9ec2ca93cdc6 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:42:24 -0700 Subject: [PATCH 089/126] Update at 'Sat Apr 05 11:42:24 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 7dde7e17..0ab7faa2 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.32, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.29, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 5ba975a8c26998f95f476ff273b0996439a6e564 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 11:45:58 -0700 Subject: [PATCH 090/126] Update at 'Sat Apr 05 11:45:58 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 0bc830cc..38188433 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -314,8 +314,8 @@ private void configureStateTransitionCommands() { .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly()) .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) - .debounce(0.060) .and(intakeCoralReq.negate()) + .debounce(0.060) .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.5))) .onTrue(this.forceState(SuperState.READY_CORAL)); From edfe782801c5d5d479cbeb0bb59c757aa47b0d06 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:15:42 -0700 Subject: [PATCH 091/126] make manipulator hold reset if moved too far --- .../subsystems/ManipulatorSubsystem.java | 33 ++++++++++++++++--- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 535f91b7..749d2107 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -48,6 +49,8 @@ public class ManipulatorSubsystem extends RollerSubsystem { private Timer zeroTimer = new Timer(); + private double positionSetpoint = 0.0; + /** Creates a new Manipulator. */ public ManipulatorSubsystem(RollerIO rollerIO, BeambreakIO firstBBIO, BeambreakIO secondBBIO) { super(rollerIO, NAME); @@ -107,17 +110,29 @@ public Command index() { public Command jog(double rotations) { return Commands.sequence( // this.runOnce(() -> io.resetEncoder(0.0)), - this.run(() -> io.setPosition(Rotation2d.fromRotations(rotations)))); + this.run( + () -> { + io.setPosition(Rotation2d.fromRotations(rotations)); + positionSetpoint = rotations; + })); } public Command jog(DoubleSupplier rotations) { return Commands.sequence( // this.runOnce(() -> io.resetEncoder(0.0)), - this.run(() -> io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())))); + this.run( + () -> { + io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())); + positionSetpoint = rotations.getAsDouble(); + })); } public Command hold() { - return this.jog(() -> inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); + return this.jog(() -> inputs.positionRotations) + .until(() -> true) + .andThen(this.run(() -> {})) + .until(() -> !MathUtil.isNear(positionSetpoint, inputs.positionRotations, 2.0)) + .repeatedly(); } public void resetPosition(final double rotations) { @@ -132,7 +147,11 @@ public Command intakeCoralAir(double vel) { return Commands.sequence( setVelocity(vel) .until(() -> secondBBInputs.get) - .finallyDo(() -> io.setPosition(Rotation2d.fromRotations(0.63))), + .finallyDo( + () -> { + io.setPosition(Rotation2d.fromRotations(0.63)); + positionSetpoint = 0.63; + }), setVoltage(0.25).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } @@ -140,7 +159,11 @@ public Command intakeCoralAir(double vel) { public Command intakeCoral(double vel) { return Commands.sequence( setVelocity(vel).until(new Trigger(() -> secondBBInputs.get).debounce(0.5)), - Commands.runOnce(() -> io.setPosition(Rotation2d.fromRotations(0.5))), + Commands.runOnce( + () -> { + io.setPosition(Rotation2d.fromRotations(0.5)); + positionSetpoint = 0.5; + }), setVelocity(1.0).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } From b44bc93d5ba9bcf74a6e5ea52e3096447b0fb7f2 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:17:03 -0700 Subject: [PATCH 092/126] adjust l4 --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 0ab7faa2..fb4190ea 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.29, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 8103438f97b1454499195f94b23325f129873535 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:45:41 -0700 Subject: [PATCH 093/126] Update at 'Sat Apr 05 12:45:41 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 38188433..13a324d5 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -316,7 +316,7 @@ private void configureStateTransitionCommands() { .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) - .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.5))) + .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.792))) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers From 5d36229361716cf0507187a482ddbb893ee23d97 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:49:50 -0700 Subject: [PATCH 094/126] Update at 'Sat Apr 05 12:49:50 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 13a324d5..d0ecfb77 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -312,8 +312,10 @@ private void configureStateTransitionCommands() { .whileTrue( Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) - .whileTrue(manipulator.intakeCoral().repeatedly()) - .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak()) + .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); + + stateTriggers.get(SuperState.INTAKE_CORAL_GROUND) + .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.792))) From 00b1450e2764e74c793a559930b2711cce85cadb Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:50:13 -0700 Subject: [PATCH 095/126] Update at 'Sat Apr 05 12:50:13 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d0ecfb77..8886425a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -313,8 +313,9 @@ private void configureStateTransitionCommands() { Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) .andThen(Commands.runOnce(() -> shoulder.rezero()))) .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); - - stateTriggers.get(SuperState.INTAKE_CORAL_GROUND) + + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) @@ -324,7 +325,7 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.INTAKE_CORAL_GROUND) .and(intakeCoralReq.negate()) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) + .and(() -> !manipulator.getFirstBeambreak() || !manipulator.getSecondBeambreak()) .onTrue(this.forceState(SuperState.IDLE)); // READY_CORAL logic From 35f9ab2bb03cf51bee8333af6fb81ad5f0b711a7 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:53:47 -0700 Subject: [PATCH 096/126] Update at 'Sat Apr 05 12:53:47 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 8886425a..f96e04c3 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -336,7 +336,14 @@ private void configureStateTransitionCommands() { ElevatorSubsystem.HP_EXTENSION_METERS, ShoulderSubsystem.SHOULDER_HP_POS, WristSubsystem.WRIST_HP_POS)) - .whileTrue(manipulator.hold()); + .whileTrue( + manipulator + .hold() + .until( + () -> + shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) + && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) + .andThen(manipulator.jog(ManipulatorSubsystem.JOG_POS))); // keep indexing to make sure its chilling stateTriggers From d8926d7dcd79f247c74cd0c8486c1d3ea94aa36e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:55:39 -0700 Subject: [PATCH 097/126] Update at 'Sat Apr 05 12:55:39 PDT 2025' --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8113756c..865bb436 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -314,7 +314,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(10).withKD(0.5).withKS(0.39))) + .withSlot1(new Slot1Configs().withKP(5.0).withKD(0.5).withKS(0.39))) : new RollerIOSim( 0.01, 5.8677, From 76cff1d5b1b7f4332825975cd190b86efdfc2199 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 12:57:23 -0700 Subject: [PATCH 098/126] Update at 'Sat Apr 05 12:57:23 PDT 2025' --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 865bb436..71dbc246 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -314,7 +314,7 @@ public static enum AlgaeScoreTarget { new FeedbackConfigs() .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(5.0).withKD(0.5).withKS(0.39))) + .withSlot1(new Slot1Configs().withKP(7.5).withKD(0.5).withKS(0.39))) : new RollerIOSim( 0.01, 5.8677, From fa850bd5d6d70df13fde0ab48e10cbb3bf75af4e Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:04:25 -0700 Subject: [PATCH 099/126] Update at 'Sat Apr 05 13:04:25 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f96e04c3..368e776a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -222,7 +222,7 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 5.0 + ? 3.0 : 0.0))) .and(manipulator::getFirstBeambreak) .debounce(0.25) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 4fb543a5..6891d6e5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(57.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); From 7ef65817c25cea30b7f49de6acd52db4d3b140e1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:06:34 -0700 Subject: [PATCH 100/126] Update at 'Sat Apr 05 13:06:34 PDT 2025' --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index fb4190ea..006ceb5d 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.22, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From 9dd2ba13b2e3ea6660b4e1ff06c9f150a5679165 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:12:37 -0700 Subject: [PATCH 101/126] Update at 'Sat Apr 05 13:12:36 PDT 2025' --- src/main/java/frc/robot/subsystems/FunnelSubsystem.java | 4 ++++ src/main/java/frc/robot/subsystems/Superstructure.java | 6 +++--- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 07dbe1cd..bc71c37d 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -44,6 +44,10 @@ public double getFilteredCurrent() { return currentFilterValue; } + public double getVoltage() { + return inputs.appliedVolts; + } + /** DO NOT RUN IF YOU WANT TO SCORE MORE CORAL */ public Command unlatch() { return this.runOnce( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 368e776a..9a991145 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -201,9 +201,9 @@ private void configureStateTransitionCommands() { // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) // .andThen( extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS) + () -> ElevatorSubsystem.HP_EXTENSION_METERS, + () -> ShoulderSubsystem.SHOULDER_HP_POS.plus(funnel.getVoltage() > 1.0 ? Rotation2d.fromDegrees(2.0) : Rotation2d.kZero), + () -> WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 6891d6e5..4fb543a5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(57.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); From d77f7948d4b292e15f60fdd9929444340b6b96d5 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:12:49 -0700 Subject: [PATCH 102/126] Update at 'Sat Apr 05 13:12:49 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9a991145..ab0d492b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -202,7 +202,11 @@ private void configureStateTransitionCommands() { // .andThen( extendWithClearance( () -> ElevatorSubsystem.HP_EXTENSION_METERS, - () -> ShoulderSubsystem.SHOULDER_HP_POS.plus(funnel.getVoltage() > 1.0 ? Rotation2d.fromDegrees(2.0) : Rotation2d.kZero), + () -> + ShoulderSubsystem.SHOULDER_HP_POS.plus( + funnel.getVoltage() > 1.0 + ? Rotation2d.fromDegrees(2.0) + : Rotation2d.kZero), () -> WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) From 21711eabb813b8d47eac2a2724c7338075914ff4 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:28:17 -0700 Subject: [PATCH 103/126] Update at 'Sat Apr 05 13:28:17 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ab0d492b..11c3d011 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -898,8 +898,8 @@ private Command extendWithClearance( // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + (shoulder.isNearTarget() && (wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() && wrist.isNearTarget()) From 94120b9323d43995945d792f00be72f5cd96a099 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 13:33:08 -0700 Subject: [PATCH 104/126] Update at 'Sat Apr 05 13:33:08 PDT 2025' --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 11c3d011..f193bcff 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -898,11 +898,13 @@ private Command extendWithClearance( // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - (shoulder.isNearTarget() && (wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) + ((shoulder.isNearTarget() + && (wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle( + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) + && wrist.isNearTarget()) && elevatorExtension.getAsDouble() != ElevatorSubsystem.L4_EXTENSION_METERS) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // extend elevator Commands.parallel( From 4ed1c238c8e7ddd3f4c08725835635495d24f1f9 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:12:13 -0700 Subject: [PATCH 105/126] Revert "Update at 'Sat Apr 05 13:33:08 PDT 2025'" This reverts commit 94120b9323d43995945d792f00be72f5cd96a099. --- src/main/java/frc/robot/subsystems/Superstructure.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f193bcff..11c3d011 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -898,13 +898,11 @@ private Command extendWithClearance( // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - ((shoulder.isNearTarget() - && (wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle( - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) + (shoulder.isNearTarget() && (wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) && elevatorExtension.getAsDouble() != ElevatorSubsystem.L4_EXTENSION_METERS) + && wrist.isNearTarget()) .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // extend elevator Commands.parallel( From b927795a68e164ea0c443128a28d3249b2b80bea Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:12:18 -0700 Subject: [PATCH 106/126] Revert "Update at 'Sat Apr 05 13:28:17 PDT 2025'" This reverts commit 21711eabb813b8d47eac2a2724c7338075914ff4. --- src/main/java/frc/robot/subsystems/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 11c3d011..ab0d492b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -898,8 +898,8 @@ private Command extendWithClearance( // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - (shoulder.isNearTarget() && (wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))) + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() && wrist.isNearTarget()) From d36308fd0b0396f5fc8f3e7e33d8161118e6399b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:12:20 -0700 Subject: [PATCH 107/126] Revert "Update at 'Sat Apr 05 13:12:49 PDT 2025'" This reverts commit d77f7948d4b292e15f60fdd9929444340b6b96d5. --- src/main/java/frc/robot/subsystems/Superstructure.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ab0d492b..9a991145 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -202,11 +202,7 @@ private void configureStateTransitionCommands() { // .andThen( extendWithClearance( () -> ElevatorSubsystem.HP_EXTENSION_METERS, - () -> - ShoulderSubsystem.SHOULDER_HP_POS.plus( - funnel.getVoltage() > 1.0 - ? Rotation2d.fromDegrees(2.0) - : Rotation2d.kZero), + () -> ShoulderSubsystem.SHOULDER_HP_POS.plus(funnel.getVoltage() > 1.0 ? Rotation2d.fromDegrees(2.0) : Rotation2d.kZero), () -> WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) From 81c6cb19322f3b1bf740431b0271783296dfcdf3 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:12:21 -0700 Subject: [PATCH 108/126] Revert "Update at 'Sat Apr 05 13:12:36 PDT 2025'" This reverts commit 9dd2ba13b2e3ea6660b4e1ff06c9f150a5679165. --- src/main/java/frc/robot/subsystems/FunnelSubsystem.java | 4 ---- src/main/java/frc/robot/subsystems/Superstructure.java | 6 +++--- .../frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index bc71c37d..07dbe1cd 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -44,10 +44,6 @@ public double getFilteredCurrent() { return currentFilterValue; } - public double getVoltage() { - return inputs.appliedVolts; - } - /** DO NOT RUN IF YOU WANT TO SCORE MORE CORAL */ public Command unlatch() { return this.runOnce( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9a991145..368e776a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -201,9 +201,9 @@ private void configureStateTransitionCommands() { // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) // .andThen( extendWithClearance( - () -> ElevatorSubsystem.HP_EXTENSION_METERS, - () -> ShoulderSubsystem.SHOULDER_HP_POS.plus(funnel.getVoltage() > 1.0 ? Rotation2d.fromDegrees(2.0) : Rotation2d.kZero), - () -> WristSubsystem.WRIST_HP_POS) + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS) .repeatedly()) // ) .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 4fb543a5..6891d6e5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(57.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); From 13b34210c17f658de782be05f7c09cb8f6522cad Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:12:23 -0700 Subject: [PATCH 109/126] Revert "Update at 'Sat Apr 05 13:06:34 PDT 2025'" This reverts commit 7ef65817c25cea30b7f49de6acd52db4d3b140e1. --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 006ceb5d..fb4190ea 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.22, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = From c75a9195971aabd1ba04f433277f3f4f793b12b1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:13:46 -0700 Subject: [PATCH 110/126] Revert "Update at 'Sat Apr 05 13:04:25 PDT 2025'" This reverts commit fa850bd5d6d70df13fde0ab48e10cbb3bf75af4e. --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 368e776a..f96e04c3 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -222,7 +222,7 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 3.0 + ? 5.0 : 0.0))) .and(manipulator::getFirstBeambreak) .debounce(0.25) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 6891d6e5..4fb543a5 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(57.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); From b515a23e30b4821dcde18a1c699c603be6968727 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 14:59:35 -0700 Subject: [PATCH 111/126] adjust hp pose again --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 4fb543a5..9a56e9e7 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -29,7 +29,7 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(55.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index f74e9def..65dd63ad 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -23,7 +23,7 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(165.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); From f9473592fc83bbb8961299d2dc9b272f5f96918b Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 15:01:10 -0700 Subject: [PATCH 112/126] decrease funnel vel --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index f96e04c3..368e776a 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -222,7 +222,7 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 5.0 + ? 3.0 : 0.0))) .and(manipulator::getFirstBeambreak) .debounce(0.25) From 3ebe8a44c200a90236d0ff82befffe62cbaf2553 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 15:04:27 -0700 Subject: [PATCH 113/126] slow funnel more --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 368e776a..b0b1d48c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -222,7 +222,7 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 3.0 + ? 1.0 : 0.0))) .and(manipulator::getFirstBeambreak) .debounce(0.25) From dc69d5eb76336244a22b2211e7cd86b55607a7e1 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 15:09:12 -0700 Subject: [PATCH 114/126] slow down reverse indexing --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 749d2107..f54275c3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -152,7 +152,7 @@ public Command intakeCoralAir(double vel) { io.setPosition(Rotation2d.fromRotations(0.63)); positionSetpoint = 0.63; }), - setVoltage(0.25).until(() -> !firstBBInputs.get), + setVoltage(0.1).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } From 517a6e4bdb0a337b8805087e0b352c960cb47a8a Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 15:15:46 -0700 Subject: [PATCH 115/126] make IDLE wait for zero to go to READY_CORAL --- .../java/frc/robot/subsystems/ManipulatorSubsystem.java | 6 +++++- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index f54275c3..e2b1cf8c 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -152,7 +152,7 @@ public Command intakeCoralAir(double vel) { io.setPosition(Rotation2d.fromRotations(0.63)); positionSetpoint = 0.63; }), - setVoltage(0.1).until(() -> !firstBBInputs.get), + setVoltage(0.5).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } @@ -180,6 +180,10 @@ public double getStatorCurrentAmps() { return currentFilterValue; } + public double getTimeSinceZero() { + return zeroTimer.get(); + } + public boolean getFirstBeambreak() { return firstBBInputs.get || bb1; } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index b0b1d48c..72ebc1ef 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -225,7 +225,7 @@ private void configureStateTransitionCommands() { ? 1.0 : 0.0))) .and(manipulator::getFirstBeambreak) - .debounce(0.25) + .and(() -> manipulator.getTimeSinceZero() < 1.0) .onTrue(this.forceState(SuperState.READY_CORAL)); stateTriggers From 9b16223d242b7a52bf35e57f449672ac65e788df Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 16:27:22 -0700 Subject: [PATCH 116/126] increase rev indexing speed again --- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index e2b1cf8c..664b90d3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -152,7 +152,7 @@ public Command intakeCoralAir(double vel) { io.setPosition(Rotation2d.fromRotations(0.63)); positionSetpoint = 0.63; }), - setVoltage(0.5).until(() -> !firstBBInputs.get), + setVoltage(2.0).until(() -> !firstBBInputs.get), jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } From 8f057e3fedca42a3517138cc7d96d4e2b363fd65 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sat, 5 Apr 2025 18:11:10 -0700 Subject: [PATCH 117/126] make manipulator never stop in SCORE_CORAL --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 72ebc1ef..1a5a5c55 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -540,7 +540,10 @@ private void configureStateTransitionCommands() { // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) ) .and(() -> elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) - .whileTrue(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed)) + .onTrue( + manipulator + .setVelocity(() -> reefTarget.get().outtakeSpeed) + .until(() -> this.state != SuperState.READY_CORAL)) // .and(() -> reefTarget.get() == ReefTarget.L1) // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) From c6d3d570bd0324dd0d2920becf56cbfa1325d773 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 08:31:44 -0700 Subject: [PATCH 118/126] adjust how we wait for hitting target to outtake for SCORE_CORAL --- src/main/java/frc/robot/subsystems/Superstructure.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 1a5a5c55..89b10b09 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -539,11 +539,13 @@ private void configureStateTransitionCommands() { // MathUtil.isNear( // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) ) - .and(() -> elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) - .onTrue( + .whileTrue( manipulator - .setVelocity(() -> reefTarget.get().outtakeSpeed) - .until(() -> this.state != SuperState.READY_CORAL)) + .hold() + .until( + () -> + elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) + .andThen(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed))) // .and(() -> reefTarget.get() == ReefTarget.L1) // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) From 8adc89f7b6afd95ca5173640cd6b78ad5f563166 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 08:50:48 -0700 Subject: [PATCH 119/126] timeout shoulder hold in final stage of clearance extend --- src/main/java/frc/robot/subsystems/Superstructure.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 89b10b09..5551b756 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -933,6 +933,7 @@ private Command extendWithClearance( wrist.isNearTarget() && (wrist.getAngle().getDegrees() < 120.0 || !elevator.isNearExtension(1.4, 0.25))) + .withTimeout(1.0) .andThen(shoulder.setTargetAngle(shoulderAngle)), wrist .hold() From 7453743a3e908fad08e486ac4d2b10b3b0cac781 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 09:40:24 -0700 Subject: [PATCH 120/126] tune operator jog --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 71dbc246..8ee2599c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -492,7 +492,7 @@ public static enum AlgaeScoreTarget { new Trigger(() -> killVisionIK) .or(() -> currentTarget == ReefTarget.L1) .or(() -> DriverStation.isAutonomous()), - () -> MathUtil.clamp(operator.getLeftY(), -1.0, 1.0)); + () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); From c14703c481c6a9e19ab88c763c5e15861c295314 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 09:50:38 -0700 Subject: [PATCH 121/126] adjust processor height to slam less --- .../java/frc/robot/subsystems/elevator/ElevatorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index b412b20d..d8ac1320 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -57,7 +57,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); - public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; + public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); From b751f0192f4a9bb691d2c0a261c9d63f2e6b20ef Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 14:07:09 -0700 Subject: [PATCH 122/126] use proper slow barge shot --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 9a56e9e7..978d3eae 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -122,7 +122,7 @@ public Command setTargetAngleSlow(final Supplier target) { } public Command setTargetAngleSlow(final Rotation2d target) { - return setTargetAngle(() -> target); + return setTargetAngleSlow(() -> target); } public Command setVoltage(final double volts) { From 57d957350baa93cb9a7faeb46f170d7de89be912 Mon Sep 17 00:00:00 2001 From: Lewis-Seiden Date: Sun, 6 Apr 2025 14:44:47 -0700 Subject: [PATCH 123/126] reduce net shoulder vel a bit --- .../java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 978d3eae..2d3a3db2 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -54,7 +54,7 @@ public class ShoulderSubsystem extends SubsystemBase { private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); private static final MotionMagicConfigs TOSS_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.6).withMotionMagicAcceleration(4.0); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.5).withMotionMagicAcceleration(4.0); private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); From 463147ada0dd662bbbfb718a59bf0fbab054142e Mon Sep 17 00:00:00 2001 From: SCool62 Date: Mon, 7 Apr 2025 21:45:45 -0700 Subject: [PATCH 124/126] Start tuning --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 +++-- .../frc/robot/subsystems/elevator/ElevatorSubsystem.java | 4 ++-- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 4 +++- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5551b756..31cc4a2f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -773,10 +773,11 @@ private void configureStateTransitionCommands() { .whileTrue( Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), + // Make it initially extend to the full 90 degrees + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS), wrist.setSlowTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS))) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) + .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .and(scoreReq) .onTrue(forceState(SuperState.SCORE_ALGAE_NET)); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d8ac1320..4e555b5e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -55,8 +55,8 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); - - public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); + // 1 inch high (maybe not needed?) + public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 65dd63ad..e2213b2f 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,7 +39,9 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); + // Hard to tune this bc my vis doesn't have the new effector, but I think that another 10 degrees + // would be better + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(115); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = From b8c9368092c1ac02db2e54c1d7eeb7cd304a7080 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 11:56:34 -0700 Subject: [PATCH 125/126] Tune at loom --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java | 5 ++--- .../java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java | 4 +++- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8ee2599c..034250c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -806,7 +806,7 @@ public Robot() { AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), - 0.75)), + 1)), Commands.waitUntil( new Trigger( () -> diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index e2213b2f..5c0d21dc 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -39,9 +39,8 @@ public class WristSubsystem extends SubsystemBase { public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(-20.0); - // Hard to tune this bc my vis doesn't have the new effector, but I think that another 10 degrees - // would be better - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(115); + + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = diff --git a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java index bfb2d116..105e35a4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java +++ b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; import frc.robot.Robot; import frc.robot.Robot.AlgaeIntakeTarget; import java.util.Arrays; @@ -55,7 +56,8 @@ public static Pose2d getRobotTargetLocation(Pose2d original) { } public static Pose2d getOffsetLocation(Pose2d original) { - return original.transformBy(new Transform2d((-0.3), 0, Rotation2d.kZero)); + return original.transformBy( + new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); } /** Gets the closest offset target to the given pose. */ diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index a249680a..078fa79b 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -32,7 +32,7 @@ public class AutoAim { new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); public static final Translation2d RED_REEF_CENTER = ChoreoAllianceFlipUtil.flip(BLUE_REEF_CENTER); - public static double BLUE_NET_X = 8.08; + public static double BLUE_NET_X = 8.08 + Units.inchesToMeters(4); public static double RED_NET_X = ChoreoAllianceFlipUtil.flipX(BLUE_NET_X); public static Pose2d BLUE_PROCESSOR_POS = new Pose2d(5.973, 0, Rotation2d.fromDegrees(270)); From 13995fe685db5425cf57ebe1c28dc563ead891ba Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 14:46:12 -0700 Subject: [PATCH 126/126] make it build --- src/main/java/frc/robot/subsystems/Superstructure.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 69c063ee..48eed6b7 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds;