From c6c583f3920db0a705ac5602080a50ff5f7c20d5 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 18:59:24 -0400 Subject: [PATCH 01/20] Added auto aim shoot mode --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../frc/robot/commands/lightStrip/SetLed.java | 3 +++ .../frc/robot/constants/enums/ShootingState.java | 3 ++- .../frc/robot/subsystems/ControllerSubsystem.java | 15 ++++++++++++--- 4 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86ab1c78..b997c8fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,6 +336,10 @@ private void configureBindings() { } public void putShuffleboardCommands() { + SmartDashboard.putData( + "Shooting State: Auto aim", + new SetShootingState(shootState, ShootState.AUTO_AIM)); + if (Constants.DEBUG) { SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0); SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED); diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4158e718..87dc24e5 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -57,6 +57,9 @@ public void execute() { case SHOOTING_HUB: lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); break; + case AUTO_AIM: + lightStrip.setPattern(BlinkinPattern.RAINBOW_LAVA_PALETTE); + break; case SHUTTLING: lightStrip.setPattern(BlinkinPattern.GREEN); break; diff --git a/src/main/java/frc/robot/constants/enums/ShootingState.java b/src/main/java/frc/robot/constants/enums/ShootingState.java index 18e1141c..b01aec87 100644 --- a/src/main/java/frc/robot/constants/enums/ShootingState.java +++ b/src/main/java/frc/robot/constants/enums/ShootingState.java @@ -8,7 +8,8 @@ public enum ShootState { FIXED, // Shoot from a known location FIXED_2, // Shoot from another known location SHOOTING_HUB, // Auto-aim into the hub - SHUTTLING // Auto-aim into alliance zone + SHUTTLING, // Auto-aim into alliance zone + AUTO_AIM //auto aim without hopper and feeder } diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 4f4f7366..2934cc3c 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -186,6 +186,15 @@ private void updateTargets(ShootState state, Pose2d robotPose) { useShotTargets(FIXED_TARGETS); } } + case AUTO_AIM ->{ if (Robot.allianceColor().isEmpty()) { + useShotTargets(FIXED_TARGETS); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { + useShotTargets(calculateTargetsFromPose(state, BLUE_HUB_PROFILE, robotPosePredictionCalculation(BLUE_HUB_PROFILE.targetPose,robotPose))); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { + useShotTargets(calculateTargetsFromPose(state, RED_HUB_PROFILE, robotPosePredictionCalculation(RED_HUB_PROFILE.targetPose,robotPose))); + } else { + useShotTargets(FIXED_TARGETS); + }} } } @@ -235,7 +244,7 @@ private void useShotTargets(ShotTargets shotTargets) { false, activeTargets.intakeDeploy); - } + } } @@ -249,8 +258,8 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile double anglerAngleDegrees = calculateAnglerAngleDegrees(state, computedDistanceMeters, profile); double shooterVelocity = calculateShooterVelocity(state, computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(state, robotPose, profile); - return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, - true, true); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, state != ShootState.AUTO_AIM, + state != ShootState.AUTO_AIM, true); } private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) { From 3c877557628e5d14bd7e36199c9c3d2d18bf22e0 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 31 Mar 2026 19:09:00 -0400 Subject: [PATCH 02/20] added fast depot sequence, updated fast depot path, autochooser and auto action --- src/main/deploy/choreo/Depot_Fast.traj | 103 ++++++++++++++++++ .../frc/robot/autochooser/AutoAction.java | 1 + .../frc/robot/autochooser/AutoChooser.java | 5 + .../commands/auto/newauto/FastDepot.java | 30 +++++ 4 files changed, 139 insertions(+) create mode 100644 src/main/deploy/choreo/Depot_Fast.traj create mode 100644 src/main/java/frc/robot/commands/auto/newauto/FastDepot.java diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj new file mode 100644 index 00000000..ca631ef0 --- /dev/null +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -0,0 +1,103 @@ +{ + "name":"Depot_Fast", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.596719741821289, "y":5.060794830322266, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.263749837875366, "y":5.565431118011475, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.11590838432312, "y":5.966093063354492, "heading":3.141592653589793, "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":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.596719741821289 m", "val":3.596719741821289}, "y":{"exp":"5.060794830322266 m", "val":5.060794830322266}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.263749837875366 m", "val":2.263749837875366}, "y":{"exp":"5.565431118011475 m", "val":5.565431118011475}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.1159083843231201 m", "val":1.11590838432312}, "y":{"exp":"5.966093063354492 m", "val":5.966093063354492}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":45.359237, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.43, + "side":0.43, + "back":0.43 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.00817,1.87662], + "samples":[ + {"t":0.0, "x":3.59672, "y":5.06079, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.6482, "ay":4.79312, "alpha":0.0, "fx":[-143.42823,-143.42823,-143.42823,-143.42823], "fy":[54.35308,54.35308,54.35308,54.35308]}, + {"t":0.04583, "x":3.58344, "y":5.06583, "heading":3.14159, "vx":-0.57962, "vy":0.21965, "omega":0.0, "ax":-12.6349, "ay":4.78808, "alpha":0.0, "fx":[-143.27734,-143.27734,-143.27734,-143.27734], "fy":[54.2959,54.2959,54.2959,54.2959]}, + {"t":0.09165, "x":3.54361, "y":5.08092, "heading":3.14159, "vx":-1.15863, "vy":0.43907, "omega":0.0, "ax":-5.31855, "ay":2.0155, "alpha":0.0, "fx":[-60.31135,-60.31135,-60.31135,-60.31135], "fy":[22.85539,22.85539,22.85539,22.85539]}, + {"t":0.13748, "x":3.48493, "y":5.10316, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00016,-0.00016,-0.00016,-0.00016], "fy":[0.00006,0.00006,0.00006,0.00006]}, + {"t":0.1833, "x":3.42067, "y":5.12751, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.22913, "x":3.3564, "y":5.15186, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.27496, "x":3.29214, "y":5.17622, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.32078, "x":3.22787, "y":5.20057, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.36661, "x":3.16361, "y":5.22493, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.41243, "x":3.09934, "y":5.24928, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.45826, "x":3.03508, "y":5.27363, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.50409, "x":2.97082, "y":5.29799, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.54991, "x":2.90655, "y":5.32234, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.59574, "x":2.84229, "y":5.34669, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.64157, "x":2.77802, "y":5.37105, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.68739, "x":2.71376, "y":5.3954, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.73322, "x":2.64949, "y":5.41975, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.77904, "x":2.58523, "y":5.44411, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.82487, "x":2.52096, "y":5.46846, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":0.8707, "x":2.4567, "y":5.49281, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00013,-0.00013,-0.00013,-0.00013], "fy":[-0.00034,-0.00034,-0.00034,-0.00034]}, + {"t":0.91652, "x":2.39243, "y":5.51717, "heading":3.14159, "vx":-1.40236, "vy":0.53143, "omega":0.0, "ax":-0.00154, "ay":-0.00407, "alpha":0.0, "fx":[-0.01748,-0.01748,-0.01748,-0.01748], "fy":[-0.04614,-0.04614,-0.04614,-0.04614]}, + {"t":0.96235, "x":2.32817, "y":5.54152, "heading":3.14159, "vx":-1.40243, "vy":0.53124, "omega":0.0, "ax":-0.14414, "ay":-0.40882, "alpha":0.0, "fx":[-1.63456,-1.63456,-1.63456,-1.63456], "fy":[-4.63588,-4.63588,-4.63588,-4.63588]}, + {"t":1.00817, "x":2.26375, "y":5.56543, "heading":3.14159, "vx":-1.40903, "vy":0.51251, "omega":0.0, "ax":-0.15284, "ay":-0.40772, "alpha":0.0, "fx":[-1.73318,-1.73318,-1.73318,-1.73318], "fy":[-4.62349,-4.62349,-4.62349,-4.62349]}, + {"t":1.05388, "x":2.19919, "y":5.58843, "heading":3.14159, "vx":-1.41602, "vy":0.49387, "omega":0.0, "ax":-0.00145, "ay":-0.00416, "alpha":0.0, "fx":[-0.01644,-0.01644,-0.01644,-0.01644], "fy":[-0.04714,-0.04714,-0.04714,-0.04714]}, + {"t":1.09959, "x":2.13446, "y":5.611, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00012,-0.00012,-0.00012,-0.00012], "fy":[-0.00035,-0.00035,-0.00035,-0.00035]}, + {"t":1.1453, "x":2.06974, "y":5.63357, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, + {"t":1.19101, "x":2.00501, "y":5.65613, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.23671, "x":1.94028, "y":5.6787, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.28242, "x":1.87556, "y":5.70126, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.32813, "x":1.81083, "y":5.72383, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.37384, "x":1.7461, "y":5.74639, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.41955, "x":1.68138, "y":5.76896, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.46525, "x":1.61665, "y":5.79152, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.51096, "x":1.55192, "y":5.81409, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.55667, "x":1.4872, "y":5.83665, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.60238, "x":1.42247, "y":5.85922, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.64809, "x":1.35775, "y":5.88178, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.69379, "x":1.29302, "y":5.90435, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.00001, "ay":-0.00001, "alpha":0.0, "fx":[0.00017,0.00017,0.00017,0.00017], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, + {"t":1.7395, "x":1.22829, "y":5.92691, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":5.45068, "ay":-1.90024, "alpha":0.0, "fx":[61.80972,61.80972,61.80972,61.80972], "fy":[-21.54838,-21.54838,-21.54838,-21.54838]}, + {"t":1.78521, "x":1.16926, "y":5.94749, "heading":3.14159, "vx":-1.16694, "vy":0.40683, "omega":0.0, "ax":12.75848, "ay":-4.44792, "alpha":0.0, "fx":[144.67873,144.67873,144.67873,144.67873], "fy":[-50.43854,-50.43854,-50.43854,-50.43854]}, + {"t":1.83092, "x":1.12925, "y":5.96144, "heading":3.14159, "vx":-0.58378, "vy":0.20352, "omega":0.0, "ax":12.77198, "ay":-4.45262, "alpha":0.0, "fx":[144.8318,144.8318,144.8318,144.8318], "fy":[-50.4919,-50.4919,-50.4919,-50.4919]}, + {"t":1.87662, "x":1.11591, "y":5.96609, "heading":3.14159, "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/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index ce806027..142ace20 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -8,6 +8,7 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), SHOOT("Shoot"), + FAST_SHOOT("Fast Shoot"), //SHOOT_PICKUP("Shoot and Pickup"), //DISTURBANCE("Disturbance"), //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index cec590a7..45b6fcbb 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -11,6 +11,7 @@ import frc.robot.Robot; import frc.robot.commands.auto.neutral.DepotNeutral; import frc.robot.commands.auto.neutral.OutpostNeutral; +import frc.robot.commands.auto.newauto.FastDepot; import frc.robot.commands.auto.shoot.ShootBlue; import frc.robot.commands.auto.shoot.ShootRed; import frc.robot.commands.auto.disturbance.DepotDisturbance; @@ -125,6 +126,10 @@ private void populateCommandMap() { new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), + new FastDepot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + /* //shoot-pickup diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java new file mode 100644 index 00000000..0241966f --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -0,0 +1,30 @@ +package frc.robot.commands.auto.newauto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.auto.AutoReset; +import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.*; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class FastDepot extends LoggableSequentialCommandGroup { + public FastDepot( + SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intake) { + super( + new LoggableParallelCommandGroup( + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast").withTimeout(10)), + new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM) + ), + new AutoShoot(hopper, feeder, 0) + ); + } +} From cd0ad99f6f03f016a3fc40796ed74f6dce109a3f Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Tue, 31 Mar 2026 20:01:25 -0400 Subject: [PATCH 03/20] having issues with choreo not going to where we tell it to go --- src/main/deploy/choreo/Depot_Fast.traj | 82 +++++++------------ .../frc/robot/apriltags/SimApriltagIO.java | 4 +- .../frc/robot/autochooser/AutoChooser.java | 2 +- .../frc/robot/autochooser/FieldLocation.java | 3 +- .../commands/auto/newauto/FastDepot.java | 4 +- .../frc/robot/constants/GameConstants.java | 2 +- vendordeps/ChoreoLib2026.json | 6 +- 7 files changed, 41 insertions(+), 62 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index ca631ef0..9ef8528d 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,26 +3,22 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596719741821289, "y":5.060794830322266, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.263749837875366, "y":5.565431118011475, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.11590838432312, "y":5.966093063354492, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.3662222921848297, "y":5.994427680969238, "heading":3.141592653589793, "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":16.541, "h":8.0692}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"3.596719741821289 m", "val":3.596719741821289}, "y":{"exp":"5.060794830322266 m", "val":5.060794830322266}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.263749837875366 m", "val":2.263749837875366}, "y":{"exp":"5.565431118011475 m", "val":5.565431118011475}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.1159083843231201 m", "val":1.11590838432312}, "y":{"exp":"5.966093063354492 m", "val":5.966093063354492}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.3662222921848297 m", "val":0.3662222921848297}, "y":{"exp":"5.994427680969238 m", "val":5.994427680969238}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -53,50 +49,30 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.00817,1.87662], + "waypoints":[0.0,1.04949], "samples":[ - {"t":0.0, "x":3.59672, "y":5.06079, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.6482, "ay":4.79312, "alpha":0.0, "fx":[-143.42823,-143.42823,-143.42823,-143.42823], "fy":[54.35308,54.35308,54.35308,54.35308]}, - {"t":0.04583, "x":3.58344, "y":5.06583, "heading":3.14159, "vx":-0.57962, "vy":0.21965, "omega":0.0, "ax":-12.6349, "ay":4.78808, "alpha":0.0, "fx":[-143.27734,-143.27734,-143.27734,-143.27734], "fy":[54.2959,54.2959,54.2959,54.2959]}, - {"t":0.09165, "x":3.54361, "y":5.08092, "heading":3.14159, "vx":-1.15863, "vy":0.43907, "omega":0.0, "ax":-5.31855, "ay":2.0155, "alpha":0.0, "fx":[-60.31135,-60.31135,-60.31135,-60.31135], "fy":[22.85539,22.85539,22.85539,22.85539]}, - {"t":0.13748, "x":3.48493, "y":5.10316, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":0.00001, "alpha":0.0, "fx":[-0.00016,-0.00016,-0.00016,-0.00016], "fy":[0.00006,0.00006,0.00006,0.00006]}, - {"t":0.1833, "x":3.42067, "y":5.12751, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.22913, "x":3.3564, "y":5.15186, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.27496, "x":3.29214, "y":5.17622, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.32078, "x":3.22787, "y":5.20057, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.36661, "x":3.16361, "y":5.22493, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.41243, "x":3.09934, "y":5.24928, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.45826, "x":3.03508, "y":5.27363, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.50409, "x":2.97082, "y":5.29799, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.54991, "x":2.90655, "y":5.32234, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.59574, "x":2.84229, "y":5.34669, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.64157, "x":2.77802, "y":5.37105, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.68739, "x":2.71376, "y":5.3954, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.73322, "x":2.64949, "y":5.41975, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.77904, "x":2.58523, "y":5.44411, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "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.82487, "x":2.52096, "y":5.46846, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, - {"t":0.8707, "x":2.4567, "y":5.49281, "heading":3.14159, "vx":-1.40235, "vy":0.53143, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00013,-0.00013,-0.00013,-0.00013], "fy":[-0.00034,-0.00034,-0.00034,-0.00034]}, - {"t":0.91652, "x":2.39243, "y":5.51717, "heading":3.14159, "vx":-1.40236, "vy":0.53143, "omega":0.0, "ax":-0.00154, "ay":-0.00407, "alpha":0.0, "fx":[-0.01748,-0.01748,-0.01748,-0.01748], "fy":[-0.04614,-0.04614,-0.04614,-0.04614]}, - {"t":0.96235, "x":2.32817, "y":5.54152, "heading":3.14159, "vx":-1.40243, "vy":0.53124, "omega":0.0, "ax":-0.14414, "ay":-0.40882, "alpha":0.0, "fx":[-1.63456,-1.63456,-1.63456,-1.63456], "fy":[-4.63588,-4.63588,-4.63588,-4.63588]}, - {"t":1.00817, "x":2.26375, "y":5.56543, "heading":3.14159, "vx":-1.40903, "vy":0.51251, "omega":0.0, "ax":-0.15284, "ay":-0.40772, "alpha":0.0, "fx":[-1.73318,-1.73318,-1.73318,-1.73318], "fy":[-4.62349,-4.62349,-4.62349,-4.62349]}, - {"t":1.05388, "x":2.19919, "y":5.58843, "heading":3.14159, "vx":-1.41602, "vy":0.49387, "omega":0.0, "ax":-0.00145, "ay":-0.00416, "alpha":0.0, "fx":[-0.01644,-0.01644,-0.01644,-0.01644], "fy":[-0.04714,-0.04714,-0.04714,-0.04714]}, - {"t":1.09959, "x":2.13446, "y":5.611, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00012,-0.00012,-0.00012,-0.00012], "fy":[-0.00035,-0.00035,-0.00035,-0.00035]}, - {"t":1.1453, "x":2.06974, "y":5.63357, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]}, - {"t":1.19101, "x":2.00501, "y":5.65613, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.23671, "x":1.94028, "y":5.6787, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.28242, "x":1.87556, "y":5.70126, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.32813, "x":1.81083, "y":5.72383, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.37384, "x":1.7461, "y":5.74639, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.41955, "x":1.68138, "y":5.76896, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.46525, "x":1.61665, "y":5.79152, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.51096, "x":1.55192, "y":5.81409, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.55667, "x":1.4872, "y":5.83665, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.60238, "x":1.42247, "y":5.85922, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.64809, "x":1.35775, "y":5.88178, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "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":1.69379, "x":1.29302, "y":5.90435, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":0.00001, "ay":-0.00001, "alpha":0.0, "fx":[0.00017,0.00017,0.00017,0.00017], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, - {"t":1.7395, "x":1.22829, "y":5.92691, "heading":3.14159, "vx":-1.41608, "vy":0.49368, "omega":0.0, "ax":5.45068, "ay":-1.90024, "alpha":0.0, "fx":[61.80972,61.80972,61.80972,61.80972], "fy":[-21.54838,-21.54838,-21.54838,-21.54838]}, - {"t":1.78521, "x":1.16926, "y":5.94749, "heading":3.14159, "vx":-1.16694, "vy":0.40683, "omega":0.0, "ax":12.75848, "ay":-4.44792, "alpha":0.0, "fx":[144.67873,144.67873,144.67873,144.67873], "fy":[-50.43854,-50.43854,-50.43854,-50.43854]}, - {"t":1.83092, "x":1.12925, "y":5.96144, "heading":3.14159, "vx":-0.58378, "vy":0.20352, "omega":0.0, "ax":12.77198, "ay":-4.45262, "alpha":0.0, "fx":[144.8318,144.8318,144.8318,144.8318], "fy":[-50.4919,-50.4919,-50.4919,-50.4919]}, - {"t":1.87662, "x":1.11591, "y":5.96609, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.99576, "ay":3.75989, "alpha":0.0, "fx":[-147.36948,-147.36948,-147.36948,-147.36948], "fy":[42.63641,42.63641,42.63641,42.63641]}, + {"t":0.04998, "x":3.57977, "y":5.0647, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":-12.99392, "ay":3.75935, "alpha":0.0, "fx":[-147.34857,-147.34857,-147.34857,-147.34857], "fy":[42.63036,42.63036,42.63036,42.63036]}, + {"t":0.09995, "x":3.53109, "y":5.07878, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":-12.99134, "ay":3.75861, "alpha":0.0, "fx":[-147.31929,-147.31929,-147.31929,-147.31929], "fy":[42.62189,42.62189,42.62189,42.62189]}, + {"t":0.14993, "x":3.44995, "y":5.10225, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":-12.98746, "ay":3.75748, "alpha":0.0, "fx":[-147.27529,-147.27529,-147.27529,-147.27529], "fy":[42.60916,42.60916,42.60916,42.60916]}, + {"t":0.1999, "x":3.33637, "y":5.13511, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":-12.98098, "ay":3.75561, "alpha":0.0, "fx":[-147.20183,-147.20183,-147.20183,-147.20183], "fy":[42.5879,42.5879,42.5879,42.5879]}, + {"t":0.24988, "x":3.19037, "y":5.17736, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":-12.96798, "ay":3.75185, "alpha":0.0, "fx":[-147.05443,-147.05443,-147.05443,-147.05443], "fy":[42.54526,42.54526,42.54526,42.54526]}, + {"t":0.29986, "x":3.01196, "y":5.22897, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":-12.92876, "ay":3.7405, "alpha":0.0, "fx":[-146.60968,-146.60968,-146.60968,-146.60968], "fy":[42.41658,42.41658,42.41658,42.41658]}, + {"t":0.34983, "x":2.80121, "y":5.28995, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":-3.46426, "ay":1.00227, "alpha":0.0, "fx":[-39.28405,-39.28405,-39.28405,-39.28405], "fy":[11.36552,11.36552,11.36552,11.36552]}, + {"t":0.39981, "x":2.56998, "y":5.35684, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":-0.00026, "ay":0.00008, "alpha":0.0, "fx":[-0.00296,-0.00296,-0.00296,-0.00296], "fy":[0.00086,0.00086,0.00086,0.00086]}, + {"t":0.44978, "x":2.33443, "y":5.42499, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.49976, "x":2.09889, "y":5.49314, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.54973, "x":1.86334, "y":5.56129, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.59971, "x":1.62779, "y":5.62944, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.00026, "ay":-0.00008, "alpha":0.0, "fx":[0.00296,0.00296,0.00296,0.00296], "fy":[-0.00086,-0.00086,-0.00086,-0.00086]}, + {"t":0.64969, "x":1.39224, "y":5.69758, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":3.46426, "ay":-1.00227, "alpha":0.0, "fx":[39.28405,39.28405,39.28405,39.28405], "fy":[-11.36552,-11.36552,-11.36552,-11.36552]}, + {"t":0.69966, "x":1.16102, "y":5.76448, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":12.92876, "ay":-3.7405, "alpha":0.0, "fx":[146.60968,146.60968,146.60968,146.60968], "fy":[-42.41658,-42.41658,-42.41658,-42.41658]}, + {"t":0.74964, "x":0.95027, "y":5.82545, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":12.96798, "ay":-3.75185, "alpha":0.0, "fx":[147.05443,147.05443,147.05443,147.05443], "fy":[-42.54526,-42.54526,-42.54526,-42.54526]}, + {"t":0.79961, "x":0.77185, "y":5.87707, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":12.98098, "ay":-3.75561, "alpha":0.0, "fx":[147.20183,147.20183,147.20183,147.20183], "fy":[-42.5879,-42.5879,-42.5879,-42.5879]}, + {"t":0.84959, "x":0.62585, "y":5.91931, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":12.98746, "ay":-3.75748, "alpha":0.0, "fx":[147.27529,147.27529,147.27529,147.27529], "fy":[-42.60916,-42.60916,-42.60916,-42.60916]}, + {"t":0.89957, "x":0.51227, "y":5.95217, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":12.99134, "ay":-3.75861, "alpha":0.0, "fx":[147.31929,147.31929,147.31929,147.31929], "fy":[-42.62189,-42.62189,-42.62189,-42.62189]}, + {"t":0.94954, "x":0.43114, "y":5.97565, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":12.99392, "ay":-3.75935, "alpha":0.0, "fx":[147.34857,147.34857,147.34857,147.34857], "fy":[-42.63036,-42.63036,-42.63036,-42.63036]}, + {"t":0.99952, "x":0.38245, "y":5.98973, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":12.99576, "ay":-3.75989, "alpha":0.0, "fx":[147.36948,147.36948,147.36948,147.36948], "fy":[-42.63641,-42.63641,-42.63641,-42.63641]}, + {"t":1.04949, "x":0.36622, "y":5.99443, "heading":3.14159, "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/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 80cc9d06..d8ad47a1 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -44,8 +44,8 @@ public void simReadings() { if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0); Vector stdDevs = truster.calculateTrust(measurement); - double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); - double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); + double readingX = pose.getX(); //+ random.nextGaussian() * stdDevs.get(0); + double readingY = pose.getY(); //+ random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 45b6fcbb..99470516 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -127,7 +127,7 @@ private void populateCommandMap() { commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); - commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.DEPOT_SIDE, Alliance.Blue), + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT), new FastDepot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 3bdae5c9..18e718a0 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -17,7 +17,8 @@ public enum FieldLocation { INVALID(-1, -1, -1, "INVALID"), DEPOT_SIDE(3.599, 7.33, 180, "Depot side - Driver Station LEFT"), //robot is 24 cm away from the wall MID(3.599, 4.029, 180, "Middle"), - OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"); //robot is 24 cm away from the wall + OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"), //robot is 24 cm away from the wall + FAST_DEPOT(3.596, 5.06, 180, "Depot side - for the fast auto"); //robot's back left corner is next to hopper corner private static final double RED_X_POS = 9.338; // meters public static final double HEIGHT_OF_FIELD = 8.043; diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 0241966f..7bed88a6 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -11,6 +11,7 @@ import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; public class FastDepot extends LoggableSequentialCommandGroup { public FastDepot( @@ -20,10 +21,11 @@ public FastDepot( super( new LoggableParallelCommandGroup( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast").withTimeout(10)), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")), new AutoReset(shootstate, turret, angler), new SetShootingState(shootstate, ShootState.AUTO_AIM) ), + new LoggableWaitCommand(3), new AutoShoot(hopper, feeder, 0) ); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 78c19113..fd15231e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json index 322c9e20..48751078 100644 --- a/vendordeps/ChoreoLib2026.json +++ b/vendordeps/ChoreoLib2026.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2026.json", "name": "ChoreoLib", - "version": "2026.0.1", + "version": "2026.0.2", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2026.0.1" + "version": "2026.0.2" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, From ba0eec9b6ff53c51017b30c9a1f84c1f5081814e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 20:31:16 -0400 Subject: [PATCH 04/20] Changed P of drive command WIP --- src/main/java/frc/robot/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java index e75b9fee..c3b31b0a 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,8 +9,8 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(.01, 0.0, 0.0); - private final PIDController yController = new PIDController(.01, 0.0, 0.0); + private final PIDController xController = new PIDController(10, 0.0, 0.0); + private final PIDController yController = new PIDController(10, 0.0, 0.0); private final PIDController headingController = new PIDController(.01, 0.0, 0.0); private SwerveSubsystem subsystem; From 6d0c3854819688088e80be0f684a498d1ed517cc Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 31 Mar 2026 21:02:24 -0400 Subject: [PATCH 05/20] Chnaged angler turret and angler reset speeds --- src/main/java/frc/robot/constants/GameConstants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index fd15231e..37c34e21 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -101,12 +101,12 @@ public enum Mode { public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler - public static final double ANGLER_ENCODER_HIGH = 272; //Highest encoder position of Angler - public static final double ANGLER_ANGLE_LOW = 16; //Lowest angle position of Angler - public static final double ANGLER_ANGLE_HIGH = 37; //Highest angle position of Angler + public static final double ANGLER_ENCODER_HIGH = 265; //Highest encoder position of Angler + public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler + public static final double ANGLER_ANGLE_HIGH = 38; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState - public static final double ANGLER_LIMIT_SPEED = 0.3; + public static final double ANGLER_LIMIT_SPEED = 0.5; // turret (pan angle) PID @@ -121,8 +121,8 @@ public enum Mode { public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward - public static final double TURRET_MIN_ANGLE = -92; - public static final double TURRET_MAX_ANGLE = 92; + public static final double TURRET_MIN_ANGLE = -96; + public static final double TURRET_MAX_ANGLE = 96; public static final double TURRET_LIMIT_SPEED = 0.2; public static final double TURRET_OUT_OF_RANGE_FLOP_RPM = -1500.0; public static final double TURRET_PID_DISTANCE_THRESHOLD = 10; From fa6bd26f8cf59229793fa27324cca6be55b177df Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Wed, 1 Apr 2026 18:59:00 -0400 Subject: [PATCH 06/20] Updated shuttling target locations --- src/main/java/frc/robot/utils/math/TurretCalculations.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index ce98ced9..67887034 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -62,7 +62,7 @@ public static double calculateTurretAngle(ShootState state, double robotPosX, do }else{ //Sets the hubPosX to be either on the blue or red side //Sets the hubPoseY to be lower or upper depending on the Y position of the robot - hubPosX = isBlueAlliance ? Constants.BLUE_HUB_X_POSITION : Constants.RED_HUB_X_POSITION; + hubPosX = isBlueAlliance ? Constants.BLUE_SHUTTLING_TARGET_X_POSITION : Constants.RED_SHUTTLING_TARGET_X_POSITION; hubPosY = robotPosY > (Constants.FIELD_WIDTH / 2) ? Constants.SHUTTLING_TARGET_HIGHER_Y_POSITION : Constants.SHUTTLING_TARGET_LOWER_Y_POSITION; } /* From da05caa0c379dd6dccc18d0b61a900a3b68b25b6 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Wed, 1 Apr 2026 20:47:33 -0400 Subject: [PATCH 07/20] Added correct shooter formulas for into the hub shooting --- src/main/java/frc/robot/Drive.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 12 ++++++------ .../java/frc/robot/constants/GameConstants.java | 2 +- .../frc/robot/subsystems/ControllerSubsystem.java | 15 ++++++++------- 4 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java index c3b31b0a..02db4086 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,9 +9,9 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(10, 0.0, 0.0); - private final PIDController yController = new PIDController(10, 0.0, 0.0); - private final PIDController headingController = new PIDController(.01, 0.0, 0.0); + private final PIDController xController = new PIDController(0.01, 0.0, 0.0); + private final PIDController yController = new PIDController(0.01, 0.0, 0.0); + private final PIDController headingController = new PIDController(.001, 0.0, 0.0); private SwerveSubsystem subsystem; public Drive(SwerveSubsystem subsystem) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b997c8fb..2d544b7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,6 +336,12 @@ private void configureBindings() { } public void putShuffleboardCommands() { + SmartDashboard.putData( + "intakedeployer/Deployment State: UP", + new SetDeploymentState(intakeDeployer, DeploymentState.UP)); + SmartDashboard.putData( + "intakedeployer/Deployment State: DOWN", + new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); SmartDashboard.putData( "Shooting State: Auto aim", new SetShootingState(shootState, ShootState.AUTO_AIM)); @@ -512,12 +518,6 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Shooting State: Shuttling", new SetShootingState(shootState, ShootState.SHUTTLING)); - SmartDashboard.putData( - "intakedeployer/Deployment State: UP", - new SetDeploymentState(intakeDeployer, DeploymentState.UP)); - SmartDashboard.putData( - "intakedeployer/Deployment State: DOWN", - new SetDeploymentState(intakeDeployer, DeploymentState.DOWN)); SmartDashboard.putData( "intakedeployer/Deployment State: STOPPED", new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 37c34e21..60cea9ec 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -47,7 +47,7 @@ public enum Mode { //Speeds public static final double INTAKE_SPEED = -0.7; public static final double INTAKE_REVERSE_SPEED = 0.5; - public static final double HOPPER_SPEED = 0.75; + public static final double HOPPER_SPEED = 0.6; public static final double CLIMBER_SPEED_UP = 0.1; public static final double CLIMBER_SPEED_DOWN = -0.1; public static final double FEEDER_SPEED = 1; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 2934cc3c..3d3d9273 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -265,7 +265,7 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) { double distance = robotPose.getTranslation() .getDistance(targetPose.getTranslation()); - if(state == ShootState.SHOOTING_HUB){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM){ if (distance > Constants.MAX_HUB_DISTANCE) { return Constants.MAX_HUB_DISTANCE; } else if (distance < Constants.MIN_HUB_DISTANCE) { @@ -298,7 +298,7 @@ private double calculateFlightTime(double computedDistanceMeters) { } private double calculateAnglerAngleDegrees(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { - if (state == ShootState.SHOOTING_HUB) { + if (state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; return 0.169 * distance * distance - 1.73 * distance @@ -309,10 +309,11 @@ private double calculateAnglerAngleDegrees(ShootState state, double computedDist private double calculateShooterVelocity(ShootState state, double computedDistanceMeters, PoseControlProfile profile) { double distance = (UnitConversion.METER_TO_FOOT * computedDistanceMeters) - Constants.COMPUTATED_DISTANCE_OFFSET; - if (state == ShootState.SHOOTING_HUB) { - return (8.46 * distance * distance - - 237 * distance - - 1380); + if (state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { + return (-0.583675*distance*distance*distance + +20.18291*distance*distance + -261.37309*distance + -1324.19278); }else if(state == ShootState.SHUTTLING){ return (((-distance*distance) - 5 * distance) - 2800); } @@ -320,7 +321,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc } private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { - if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING || state == ShootState.AUTO_AIM){ return Math.floor( Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), From c65b21b911d95d6a32d1bcf44f5971734faa5fa3 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 09:12:22 -0400 Subject: [PATCH 08/20] changed the path, changed the pid for the choreo drive --- src/main/deploy/choreo/Depot_Fast.traj | 114 ++++++++++++++++++------- src/main/java/frc/robot/Drive.java | 6 +- 2 files changed, 88 insertions(+), 32 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index 9ef8528d..39ec463f 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,22 +3,26 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":0.3662222921848297, "y":5.994427680969238, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.118183970451355, "y":5.943911552429199, "heading":3.141592653589793, "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":16.541, "h":8.0692}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"0.3662222921848297 m", "val":0.3662222921848297}, "y":{"exp":"5.994427680969238 m", "val":5.994427680969238}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.118183970451355 m", "val":1.118183970451355}, "y":{"exp":"5.943911552429199 m", "val":5.943911552429199}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -49,30 +53,82 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.04949], + "waypoints":[0.0,3.63172], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.99576, "ay":3.75989, "alpha":0.0, "fx":[-147.36948,-147.36948,-147.36948,-147.36948], "fy":[42.63641,42.63641,42.63641,42.63641]}, - {"t":0.04998, "x":3.57977, "y":5.0647, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":-12.99392, "ay":3.75935, "alpha":0.0, "fx":[-147.34857,-147.34857,-147.34857,-147.34857], "fy":[42.63036,42.63036,42.63036,42.63036]}, - {"t":0.09995, "x":3.53109, "y":5.07878, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":-12.99134, "ay":3.75861, "alpha":0.0, "fx":[-147.31929,-147.31929,-147.31929,-147.31929], "fy":[42.62189,42.62189,42.62189,42.62189]}, - {"t":0.14993, "x":3.44995, "y":5.10225, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":-12.98746, "ay":3.75748, "alpha":0.0, "fx":[-147.27529,-147.27529,-147.27529,-147.27529], "fy":[42.60916,42.60916,42.60916,42.60916]}, - {"t":0.1999, "x":3.33637, "y":5.13511, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":-12.98098, "ay":3.75561, "alpha":0.0, "fx":[-147.20183,-147.20183,-147.20183,-147.20183], "fy":[42.5879,42.5879,42.5879,42.5879]}, - {"t":0.24988, "x":3.19037, "y":5.17736, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":-12.96798, "ay":3.75185, "alpha":0.0, "fx":[-147.05443,-147.05443,-147.05443,-147.05443], "fy":[42.54526,42.54526,42.54526,42.54526]}, - {"t":0.29986, "x":3.01196, "y":5.22897, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":-12.92876, "ay":3.7405, "alpha":0.0, "fx":[-146.60968,-146.60968,-146.60968,-146.60968], "fy":[42.41658,42.41658,42.41658,42.41658]}, - {"t":0.34983, "x":2.80121, "y":5.28995, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":-3.46426, "ay":1.00227, "alpha":0.0, "fx":[-39.28405,-39.28405,-39.28405,-39.28405], "fy":[11.36552,11.36552,11.36552,11.36552]}, - {"t":0.39981, "x":2.56998, "y":5.35684, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":-0.00026, "ay":0.00008, "alpha":0.0, "fx":[-0.00296,-0.00296,-0.00296,-0.00296], "fy":[0.00086,0.00086,0.00086,0.00086]}, - {"t":0.44978, "x":2.33443, "y":5.42499, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.49976, "x":2.09889, "y":5.49314, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.54973, "x":1.86334, "y":5.56129, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "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.59971, "x":1.62779, "y":5.62944, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.00026, "ay":-0.00008, "alpha":0.0, "fx":[0.00296,0.00296,0.00296,0.00296], "fy":[-0.00086,-0.00086,-0.00086,-0.00086]}, - {"t":0.64969, "x":1.39224, "y":5.69758, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":3.46426, "ay":-1.00227, "alpha":0.0, "fx":[39.28405,39.28405,39.28405,39.28405], "fy":[-11.36552,-11.36552,-11.36552,-11.36552]}, - {"t":0.69966, "x":1.16102, "y":5.76448, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":12.92876, "ay":-3.7405, "alpha":0.0, "fx":[146.60968,146.60968,146.60968,146.60968], "fy":[-42.41658,-42.41658,-42.41658,-42.41658]}, - {"t":0.74964, "x":0.95027, "y":5.82545, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":12.96798, "ay":-3.75185, "alpha":0.0, "fx":[147.05443,147.05443,147.05443,147.05443], "fy":[-42.54526,-42.54526,-42.54526,-42.54526]}, - {"t":0.79961, "x":0.77185, "y":5.87707, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":12.98098, "ay":-3.75561, "alpha":0.0, "fx":[147.20183,147.20183,147.20183,147.20183], "fy":[-42.5879,-42.5879,-42.5879,-42.5879]}, - {"t":0.84959, "x":0.62585, "y":5.91931, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":12.98746, "ay":-3.75748, "alpha":0.0, "fx":[147.27529,147.27529,147.27529,147.27529], "fy":[-42.60916,-42.60916,-42.60916,-42.60916]}, - {"t":0.89957, "x":0.51227, "y":5.95217, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":12.99134, "ay":-3.75861, "alpha":0.0, "fx":[147.31929,147.31929,147.31929,147.31929], "fy":[-42.62189,-42.62189,-42.62189,-42.62189]}, - {"t":0.94954, "x":0.43114, "y":5.97565, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":12.99392, "ay":-3.75935, "alpha":0.0, "fx":[147.34857,147.34857,147.34857,147.34857], "fy":[-42.63036,-42.63036,-42.63036,-42.63036]}, - {"t":0.99952, "x":0.38245, "y":5.98973, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":12.99576, "ay":-3.75989, "alpha":0.0, "fx":[147.36948,147.36948,147.36948,147.36948], "fy":[-42.63641,-42.63641,-42.63641,-42.63641]}, - {"t":1.04949, "x":0.36622, "y":5.99443, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.94147, "ay":0.33588, "alpha":0.00228, "fx":[-10.65214,-10.65219,-10.70005,-10.70007], "fy":[3.82049,3.74265,3.85147,3.82047]}, + {"t":0.04975, "x":3.59483, "y":5.06042, "heading":3.14159, "vx":-0.04684, "vy":0.01671, "omega":0.00011, "ax":-0.94165, "ay":0.33594, "alpha":0.00442, "fx":[-10.65487,-10.65489,-10.70146,-10.70148], "fy":[3.80911,3.78065,3.83924,3.80909]}, + {"t":0.0995, "x":3.59134, "y":5.06166, "heading":-3.14159, "vx":-0.09368, "vy":0.03342, "omega":0.00033, "ax":-0.94164, "ay":0.33594, "alpha":0.00038, "fx":[-10.70459,-10.70455,-10.73425,-10.56877], "fy":[3.75341,3.83807,3.83805,3.80843]}, + {"t":0.14925, "x":3.58551, "y":5.06374, "heading":-3.14157, "vx":-0.14053, "vy":0.05014, "omega":0.00035, "ax":-0.94163, "ay":0.33594, "alpha":0.00374, "fx":[-10.65552,-10.65555,-10.70025,-10.70027], "fy":[3.81173,3.77375,3.84065,3.81171]}, + {"t":0.199, "x":3.57736, "y":5.06665, "heading":-3.14155, "vx":-0.18738, "vy":0.06685, "omega":0.00054, "ax":-0.94161, "ay":0.33593, "alpha":-0.00054, "fx":[-10.7103,-10.71025,-10.73887,-10.55149], "fy":[3.74896,3.83909,3.83908,3.81055]}, + {"t":0.24875, "x":3.56687, "y":5.07039, "heading":-3.14153, "vx":-0.23422, "vy":0.08356, "omega":0.00051, "ax":-0.9416, "ay":0.33593, "alpha":0.0031, "fx":[-10.65592,-10.65596,-10.69912,-10.69915], "fy":[3.81432,3.76663,3.84224,3.8143]}, + {"t":0.2985, "x":3.55405, "y":5.07497, "heading":-3.1415, "vx":-0.28107, "vy":0.10027, "omega":0.00067, "ax":-0.94158, "ay":0.33593, "alpha":-0.00129, "fx":[-10.71689,-10.71684,-10.74455,-10.531], "fy":[3.73912,3.84195,3.84193,3.81432]}, + {"t":0.34825, "x":3.5389, "y":5.08037, "heading":-3.14147, "vx":-0.32791, "vy":0.11699, "omega":0.0006, "ax":-0.94156, "ay":0.33592, "alpha":0.00242, "fx":[-10.65611,-10.65615,-10.698,-10.69803], "fy":[3.8172,3.75841,3.84427,3.81718]}, + {"t":0.398, "x":3.52143, "y":5.08661, "heading":-3.14144, "vx":-0.37475, "vy":0.1337, "omega":0.00072, "ax":-0.94153, "ay":0.33591, "alpha":-0.00228, "fx":[-10.72591,-10.72585,-10.75277,-10.50258], "fy":[3.72538,3.84608,3.84607,3.81927]}, + {"t":0.44775, "x":3.50162, "y":5.09367, "heading":-3.1414, "vx":-0.42159, "vy":0.15041, "omega":0.00061, "ax":-0.9415, "ay":0.33591, "alpha":0.00132, "fx":[-10.65604,-10.6561,-10.69678,-10.6968], "fy":[3.82241,3.74301,3.84871,3.82239]}, + {"t":0.4975, "x":3.47948, "y":5.10157, "heading":-3.14137, "vx":-0.46843, "vy":0.16712, "omega":0.00067, "ax":-0.94146, "ay":0.3359, "alpha":-0.00418, "fx":[-10.73895,-10.73889,-10.76509,-10.46111], "fy":[3.71375,3.84949,3.84948,3.82341]}, + {"t":0.54725, "x":3.45501, "y":5.1103, "heading":-3.14134, "vx":-0.51527, "vy":0.18383, "omega":0.00047, "ax":-0.94142, "ay":0.33589, "alpha":0.00007, "fx":[-10.65574,-10.6558,-10.69523,-10.69525], "fy":[3.8283,3.7253,3.85377,3.82828]}, + {"t":0.59699, "x":3.42821, "y":5.11986, "heading":-3.14131, "vx":-0.5621, "vy":0.20054, "omega":0.00047, "ax":-0.94136, "ay":0.33588, "alpha":-0.00779, "fx":[-10.75888,-10.75882,-10.78421,-10.39756], "fy":[3.70754,3.85092,3.85091,3.82569]}, + {"t":0.64674, "x":3.39908, "y":5.13025, "heading":-3.14129, "vx":-0.60894, "vy":0.21725, "omega":0.00008, "ax":-0.94129, "ay":0.33586, "alpha":-0.00138, "fx":[-10.6551,-10.65518,-10.69294,-10.69296], "fy":[3.83489,3.70533,3.85928,3.83487]}, + {"t":0.69649, "x":3.36762, "y":5.14148, "heading":-3.14129, "vx":-0.65576, "vy":0.23396, "omega":0.00001, "ax":-0.94119, "ay":0.33584, "alpha":-0.00952, "fx":[-10.7771,-10.77702,-10.80111,-10.33656], "fy":[3.67338,3.86131,3.8613,3.8374]}, + {"t":0.74624, "x":3.33383, "y":5.15353, "heading":-3.14129, "vx":-0.70259, "vy":0.25067, "omega":-0.00046, "ax":-0.94106, "ay":0.33581, "alpha":-0.00243, "fx":[-10.65368,-10.65375,-10.68909,-10.6891], "fy":[3.83876,3.69301,3.86156,3.83874]}, + {"t":0.79599, "x":3.29771, "y":5.16642, "heading":-3.14131, "vx":-0.74941, "vy":0.26737, "omega":-0.00058, "ax":-0.94085, "ay":0.33576, "alpha":-0.01052, "fx":[-10.796,-10.79591,-10.8182,-10.26636], "fy":[3.61845,3.8778,3.87779,3.85569]}, + {"t":0.84574, "x":3.25927, "y":5.18014, "heading":-3.14134, "vx":-0.79621, "vy":0.28408, "omega":-0.0011, "ax":-0.94051, "ay":0.33569, "alpha":-0.00444, "fx":[-10.64911,-10.64917,-10.68128,-10.68129], "fy":[3.84655,3.66627,3.86728,3.84654]}, + {"t":0.89549, "x":3.21849, "y":5.19468, "heading":-3.14139, "vx":-0.843, "vy":0.30078, "omega":-0.00132, "ax":-0.9398, "ay":0.3356, "alpha":-0.01869, "fx":[-10.82951,-10.82945,-10.84977,-10.11982], "fy":[3.60984,3.87768,3.87767,3.85751]}, + {"t":0.94524, "x":3.17539, "y":5.21006, "heading":-3.14146, "vx":-0.88976, "vy":0.31747, "omega":-0.00225, "ax":-0.93775, "ay":0.33499, "alpha":-0.01284, "fx":[-10.61976,-10.61983,-10.64795,-10.64795], "fy":[3.88169,3.53087,3.90061,3.88169]}, + {"t":0.99499, "x":3.12996, "y":5.22627, "heading":-3.14157, "vx":-0.93641, "vy":0.33414, "omega":-0.00289, "ax":-0.10609, "ay":0.03507, "alpha":0.02429, "fx":[-1.00877,-1.00876,-1.02059,-1.77391], "fy":[0.60092,0.33385,0.33385,0.32203]}, + {"t":1.04474, "x":3.08325, "y":5.24294, "heading":3.14147, "vx":-0.94169, "vy":0.33588, "omega":-0.00168, "ax":-0.00197, "ay":-0.00553, "alpha":0.00521, "fx":[-0.01204,-0.01204,-0.03271,-0.03271], "fy":[-0.08029,-0.02335,-0.06668,-0.08029]}, + {"t":1.09449, "x":3.0364, "y":5.25964, "heading":3.14139, "vx":-0.94179, "vy":0.33561, "omega":-0.00142, "ax":0.00167, "ay":0.00469, "alpha":0.00735, "fx":[0.0606,0.06059,0.04875,-0.09409], "fy":[0.07157,0.05102,0.05102,0.03917]}, + {"t":1.14424, "x":2.98954, "y":5.27634, "heading":3.14132, "vx":-0.9417, "vy":0.33584, "omega":-0.00106, "ax":0.00027, "ay":0.00077, "alpha":0.00678, "fx":[0.01064,0.01064,-0.00444,-0.00444], "fy":[-0.02019,0.08545,-0.01029,-0.02019]}, + {"t":1.19399, "x":2.9427, "y":5.29305, "heading":3.14126, "vx":-0.94169, "vy":0.33588, "omega":-0.00072, "ax":0.00023, "ay":0.00065, "alpha":0.00981, "fx":[0.07413,0.07413,0.06562,-0.20342], "fy":[0.07237,-0.01151,-0.01151,-0.02001]}, + {"t":1.24374, "x":2.89585, "y":5.30976, "heading":3.14123, "vx":-0.94168, "vy":0.33591, "omega":-0.00023, "ax":0.00002, "ay":0.00007, "alpha":0.0036, "fx":[0.00593,0.00593,-0.00538,-0.00538], "fy":[-0.01291,0.03421,-0.00533,-0.01291]}, + {"t":1.29349, "x":2.849, "y":5.32648, "heading":3.14122, "vx":-0.94168, "vy":0.33592, "omega":-0.00005, "ax":0.00007, "ay":0.00019, "alpha":0.00421, "fx":[0.02872,0.02872,0.02214,-0.07651], "fy":[0.02479,-0.0032,-0.0032,-0.00978]}, + {"t":1.34324, "x":2.80215, "y":5.34319, "heading":3.14121, "vx":-0.94167, "vy":0.33593, "omega":0.00015, "ax":0.00002, "ay":0.00005, "alpha":0.00209, "fx":[0.00446,0.00446,-0.00406,-0.00406], "fy":[-0.00639,0.01571,-0.00064,-0.00639]}, + {"t":1.39299, "x":2.7553, "y":5.3599, "heading":3.14122, "vx":-0.94167, "vy":0.33593, "omega":0.00026, "ax":0.00001, "ay":0.00002, "alpha":0.00226, "fx":[0.01351,0.01351,0.00855,-0.03518], "fy":[0.00913,-0.00103,-0.00103,-0.00599]}, + {"t":1.44274, "x":2.70846, "y":5.37661, "heading":3.14123, "vx":-0.94167, "vy":0.33593, "omega":0.00037, "ax":0.0, "ay":0.00001, "alpha":0.00127, "fx":[0.00313,0.00313,-0.00309,-0.00309], "fy":[-0.00362,0.00687,0.00063,-0.00362]}, + {"t":1.49249, "x":2.66161, "y":5.39332, "heading":3.14125, "vx":-0.94167, "vy":0.33593, "omega":0.00043, "ax":0.0, "ay":-0.00001, "alpha":0.00126, "fx":[0.00653,0.00653,0.00296,-0.01623], "fy":[0.0029,0.00002,0.00002,-0.00355]}, + {"t":1.54224, "x":2.61476, "y":5.41004, "heading":3.14127, "vx":-0.94167, "vy":0.33593, "omega":0.0005, "ax":0.0, "ay":-0.00001, "alpha":0.00079, "fx":[0.00203,0.00203,-0.00213,-0.00213], "fy":[-0.00228,0.00334,0.00066,-0.00228]}, + {"t":1.59199, "x":2.56791, "y":5.42675, "heading":3.1413, "vx":-0.94167, "vy":0.33593, "omega":0.00054, "ax":-0.00001, "ay":-0.00001, "alpha":0.00073, "fx":[0.00347,0.00347,0.00116,-0.00834], "fy":[0.00098,0.00022,0.00022,-0.00209]}, + {"t":1.64174, "x":2.52106, "y":5.44346, "heading":3.14132, "vx":-0.94167, "vy":0.33593, "omega":0.00057, "ax":0.0, "ay":-0.00001, "alpha":0.00043, "fx":[0.00108,0.00108,-0.00118,-0.00118], "fy":[-0.0013,0.00165,0.00042,-0.0013]}, + {"t":1.69149, "x":2.47422, "y":5.46017, "heading":3.14135, "vx":-0.94167, "vy":0.33593, "omega":0.00059, "ax":0.0, "ay":-0.00001, "alpha":0.00035, "fx":[0.0016,0.0016,0.00048,-0.00379], "fy":[0.00037,0.00016,0.00016,-0.00097]}, + {"t":1.74124, "x":2.42737, "y":5.47689, "heading":3.14138, "vx":-0.94167, "vy":0.33593, "omega":0.00061, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00021,0.00021,-0.00023,-0.00023], "fy":[-0.0004,0.00053,0.00016,-0.0004]}, + {"t":1.79098, "x":2.38052, "y":5.4936, "heading":3.14141, "vx":-0.94167, "vy":0.33593, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00006,0.00006,0.00009,-0.00015], "fy":[0.00009,0.00002,0.00002,0.00005]}, + {"t":1.84073, "x":2.33367, "y":5.51031, "heading":3.14144, "vx":-0.94167, "vy":0.33593, "omega":0.00062, "ax":0.0, "ay":0.00001, "alpha":-0.0002, "fx":[-0.00064,-0.00064,0.00073,0.00073], "fy":[0.00052,-0.00047,-0.00009,0.00052]}, + {"t":1.89048, "x":2.28683, "y":5.52702, "heading":3.14147, "vx":-0.94167, "vy":0.33593, "omega":0.00061, "ax":0.00001, "ay":0.00002, "alpha":-0.00034, "fx":[-0.00147,-0.00147,-0.00027,0.00348], "fy":[-0.00017,-0.00008,-0.00008,0.00111]}, + {"t":1.94023, "x":2.23998, "y":5.54374, "heading":3.1415, "vx":-0.94167, "vy":0.33593, "omega":0.00059, "ax":0.00001, "ay":0.00002, "alpha":-0.00053, "fx":[-0.00152,-0.00152,0.00172,0.00172], "fy":[0.0015,-0.0016,-0.00029,0.0015]}, + {"t":1.98998, "x":2.19313, "y":5.56045, "heading":3.14153, "vx":-0.94167, "vy":0.33593, "omega":0.00056, "ax":0.00001, "ay":0.00003, "alpha":-0.00072, "fx":[-0.0033,-0.0033,-0.00089,0.00799], "fy":[-0.00078,-0.00008,-0.00008,0.00233]}, + {"t":2.03973, "x":2.14628, "y":5.57716, "heading":3.14156, "vx":-0.94167, "vy":0.33593, "omega":0.00053, "ax":0.00001, "ay":0.00004, "alpha":-0.0009, "fx":[-0.00245,-0.00245,0.00274,0.00274], "fy":[0.00265,-0.0033,-0.00039,0.00265]}, + {"t":2.08948, "x":2.09943, "y":5.59387, "heading":3.14159, "vx":-0.94167, "vy":0.33593, "omega":0.00048, "ax":0.00001, "ay":0.00004, "alpha":-0.00123, "fx":[-0.00626,-0.00626,-0.00254,0.01572], "fy":[-0.00268,0.00026,0.00026,0.00398]}, + {"t":2.13923, "x":2.05259, "y":5.61059, "heading":-3.14157, "vx":-0.94167, "vy":0.33594, "omega":0.00042, "ax":0.00001, "ay":0.00004, "alpha":-0.00141, "fx":[-0.00351,-0.00351,0.00382,0.00382], "fy":[0.00431,-0.0068,-0.00011,0.00431]}, + {"t":2.18898, "x":2.00574, "y":5.6273, "heading":-3.14155, "vx":-0.94167, "vy":0.33594, "omega":0.00035, "ax":0.00001, "ay":0.00002, "alpha":-0.0022, "fx":[-0.013,-0.013,-0.00782,0.03412], "fy":[-0.00882,0.00149,0.00149,0.00668]}, + {"t":2.23873, "x":1.95889, "y":5.64401, "heading":-3.14153, "vx":-0.94167, "vy":0.33594, "omega":0.00024, "ax":0.00001, "ay":0.00002, "alpha":-0.00226, "fx":[-0.00478,-0.00478,0.00498,0.00498], "fy":[0.00753,-0.01551,0.00154,0.00753]}, + {"t":2.28848, "x":1.91204, "y":5.66072, "heading":-3.14152, "vx":-0.94167, "vy":0.33594, "omega":0.00013, "ax":-0.00005, "ay":-0.00014, "alpha":-0.00408, "fx":[-0.02774,-0.02774,-0.02084,0.07402], "fy":[-0.02427,0.00366,0.00366,0.01056]}, + {"t":2.33823, "x":1.8652, "y":5.67744, "heading":-3.14152, "vx":-0.94167, "vy":0.33593, "omega":-0.00007, "ax":0.00001, "ay":0.00003, "alpha":-0.00379, "fx":[-0.00624,-0.00624,0.00645,0.00645], "fy":[0.01431,-0.03385,0.00641,0.01431]}, + {"t":2.38798, "x":1.81835, "y":5.69415, "heading":-3.14152, "vx":-0.94167, "vy":0.33593, "omega":-0.00026, "ax":-0.00027, "ay":-0.00075, "alpha":-0.00936, "fx":[-0.07147,-0.07147,-0.06252,0.19325], "fy":[-0.07198,0.0096,0.0096,0.01855]}, + {"t":2.43773, "x":1.7715, "y":5.71086, "heading":-3.14153, "vx":-0.94168, "vy":0.3359, "omega":-0.00073, "ax":-0.00031, "ay":-0.00086, "alpha":-0.00689, "fx":[-0.01178,-0.01178,0.00483,0.00483], "fy":[0.01894,-0.08548,0.00861,0.01894]}, + {"t":2.48748, "x":1.72465, "y":5.72757, "heading":-3.14157, "vx":-0.9417, "vy":0.33585, "omega":-0.00107, "ax":-0.0015, "ay":-0.00421, "alpha":-0.0071, "fx":[-0.06211,-0.06211,-0.05004,0.10625], "fy":[-0.08074,-0.04071,-0.04071,-0.02863]}, + {"t":2.53723, "x":1.6778, "y":5.74427, "heading":3.14156, "vx":-0.94177, "vy":0.33564, "omega":-0.00142, "ax":0.00178, "ay":0.00498, "alpha":-0.00548, "fx":[0.00908,0.00908,0.03131,0.0313], "fy":[0.07481,0.01564,0.06081,0.07482]}, + {"t":2.58698, "x":1.63095, "y":5.76098, "heading":3.14149, "vx":-0.94168, "vy":0.33589, "omega":-0.0017, "ax":0.10542, "ay":-0.03632, "alpha":-0.02363, "fx":[1.00714,1.00714,1.01962,1.7481], "fy":[-0.60892,-0.35031,-0.35031,-0.33784]}, + {"t":2.63673, "x":1.58423, "y":5.77764, "heading":3.14141, "vx":-0.93644, "vy":0.33408, "omega":-0.00287, "ax":0.93803, "ay":-0.3342, "alpha":0.01235, "fx":[10.62223,10.62229,10.65186,10.65187], "fy":[-3.87089,-3.52701,-3.89019,-3.87089]}, + {"t":2.68648, "x":1.53881, "y":5.79385, "heading":3.14127, "vx":-0.88977, "vy":0.31746, "omega":-0.00226, "ax":0.93984, "ay":-0.33549, "alpha":0.01823, "fx":[10.82887,10.82882,10.84937,10.12333], "fy":[-3.60446,-3.87782,-3.87782,-3.85742]}, + {"t":2.73623, "x":1.4957, "y":5.80923, "heading":3.14115, "vx":-0.84302, "vy":0.30077, "omega":-0.00135, "ax":0.94053, "ay":-0.33564, "alpha":0.0065, "fx":[10.64851,10.64861,10.68226,10.68227], "fy":[-3.85786,-3.62967,-3.87901,-3.85784]}, + {"t":2.78598, "x":1.45493, "y":5.82378, "heading":3.14109, "vx":-0.79623, "vy":0.28407, "omega":-0.00103, "ax":0.94086, "ay":-0.33574, "alpha":0.01398, "fx":[10.80984,10.80976,10.83249,10.22468], "fy":[-3.63302,-3.8728,-3.87279,-3.85025]}, + {"t":2.83573, "x":1.41648, "y":5.83749, "heading":3.14103, "vx":-0.74942, "vy":0.26737, "omega":-0.00033, "ax":0.94107, "ay":-0.33579, "alpha":0.00432, "fx":[10.65318,10.65329,10.68976,10.68977], "fy":[-3.84928,-3.6602,-3.87228,-3.84926]}, + {"t":2.88548, "x":1.38036, "y":5.85038, "heading":3.14102, "vx":-0.7026, "vy":0.25066, "omega":-0.00012, "ax":0.9412, "ay":-0.33582, "alpha":0.00904, "fx":[10.7762,10.77612,10.80009,10.33975], "fy":[-3.66801,-3.86272,-3.86271,-3.83891]}, + {"t":2.93522, "x":1.34657, "y":5.86243, "heading":3.14101, "vx":-0.65578, "vy":0.23396, "omega":0.00033, "ax":0.9413, "ay":-0.33584, "alpha":0.00128, "fx":[10.65501,10.65509,10.69318,10.69319], "fy":[-3.83437,-3.7064,-3.85843,-3.83435]}, + {"t":2.98497, "x":1.31511, "y":5.87366, "heading":3.14103, "vx":-0.60895, "vy":0.21725, "omega":0.0004, "ax":0.94137, "ay":-0.33586, "alpha":0.00755, "fx":[10.75845,10.75839,10.78324,10.39967], "fy":[-3.70425,-3.8516,-3.85158,-3.82689]}, + {"t":3.03472, "x":1.28598, "y":5.88405, "heading":3.14105, "vx":-0.56211, "vy":0.20054, "omega":0.00077, "ax":0.94142, "ay":-0.33588, "alpha":-0.00005, "fx":[10.65594,10.65601,10.69513,10.69515], "fy":[-3.82811,-3.72598,-3.85286,-3.82809]}, + {"t":3.08447, "x":1.25918, "y":5.89361, "heading":3.14109, "vx":-0.51528, "vy":0.18383, "omega":0.00077, "ax":0.94147, "ay":-0.33589, "alpha":0.00488, "fx":[10.74096,10.7409,10.76621,10.4562], "fy":[-3.718,-3.84758,-3.84756,-3.82239]}, + {"t":3.13422, "x":1.23471, "y":5.90234, "heading":3.14113, "vx":-0.46844, "vy":0.16712, "omega":0.00101, "ax":0.9415, "ay":-0.33589, "alpha":-0.00116, "fx":[10.65655,10.65661,10.69637,10.69639], "fy":[-3.82266,-3.74278,-3.84783,-3.82264]}, + {"t":3.18397, "x":1.21257, "y":5.91024, "heading":3.14118, "vx":-0.4216, "vy":0.15041, "omega":0.00096, "ax":0.94153, "ay":-0.3359, "alpha":0.00291, "fx":[10.72782,10.72777,10.75339,10.4983], "fy":[-3.72844,-3.84446,-3.84445,-3.81895]}, + {"t":3.23372, "x":1.19276, "y":5.91731, "heading":3.14122, "vx":-0.37476, "vy":0.1337, "omega":0.0011, "ax":0.94156, "ay":-0.33591, "alpha":-0.00221, "fx":[10.65693,10.65698,10.69726,10.69728], "fy":[-3.81746,-3.7587,-3.84299,-3.81744]}, + {"t":3.28347, "x":1.17528, "y":5.92354, "heading":3.14128, "vx":-0.32792, "vy":0.11699, "omega":0.00099, "ax":0.94158, "ay":-0.33592, "alpha":0.00181, "fx":[10.71829,10.71824,10.74421,10.5287], "fy":[-3.74156,-3.8404,-3.84038,-3.81452]}, + {"t":3.33322, "x":1.16013, "y":5.92895, "heading":3.14133, "vx":-0.28108, "vy":0.10027, "omega":0.00108, "ax":0.9416, "ay":-0.33592, "alpha":-0.00274, "fx":[10.65709,10.65713,10.69803,10.69805], "fy":[-3.81502,-3.7661,-3.84096,-3.815]}, + {"t":3.38297, "x":1.14732, "y":5.93352, "heading":3.14138, "vx":-0.23423, "vy":0.08356, "omega":0.00094, "ax":0.94162, "ay":-0.33593, "alpha":0.00104, "fx":[10.7116,10.71156,10.73798,10.54989], "fy":[-3.75097,-3.83756,-3.83755,-3.81122]}, + {"t":3.43272, "x":1.13683, "y":5.93726, "heading":3.14143, "vx":-0.18739, "vy":0.06685, "omega":0.001, "ax":0.94163, "ay":-0.33593, "alpha":-0.00331, "fx":[10.65704,10.65707,10.69878,10.6988], "fy":[-3.8125,-3.77352,-3.83898,-3.81248]}, + {"t":3.48247, "x":1.12867, "y":5.94017, "heading":3.14148, "vx":-0.14054, "vy":0.05014, "omega":0.00083, "ax":0.94164, "ay":-0.33593, "alpha":0.00005, "fx":[10.70567,10.70563,10.73265,10.56834], "fy":[-3.75439,-3.83674,-3.83672,-3.80978]}, + {"t":3.53222, "x":1.12285, "y":5.94225, "heading":3.14152, "vx":-0.09369, "vy":0.03343, "omega":0.00083, "ax":0.94166, "ay":-0.33594, "alpha":-0.00369, "fx":[10.65677,10.6568,10.69962,10.69964], "fy":[-3.81108,-3.77736,-3.83829,-3.81105]}, + {"t":3.58197, "x":1.11935, "y":5.9435, "heading":3.14156, "vx":-0.04685, "vy":0.01671, "omega":0.00065, "ax":0.94167, "ay":-0.33594, "alpha":-0.01307, "fx":[10.69967,10.55976,10.72693,10.72695], "fy":[-3.78794,-3.83108,-3.83095,-3.78792]}, + {"t":3.63172, "x":1.11818, "y":5.94391, "heading":3.14159, "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/Drive.java b/src/main/java/frc/robot/Drive.java index 02db4086..2845a541 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,9 +9,9 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(0.01, 0.0, 0.0); - private final PIDController yController = new PIDController(0.01, 0.0, 0.0); - private final PIDController headingController = new PIDController(.001, 0.0, 0.0); + private final PIDController xController = new PIDController(0.08, 0.0, 0.0); + private final PIDController yController = new PIDController(0.08, 0.0, 0.0); + private final PIDController headingController = new PIDController(0.8, 0.0, 0.0); private SwerveSubsystem subsystem; public Drive(SwerveSubsystem subsystem) { From 7bf9c34789b5d92a755b92c408dd11cbe3415d6d Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 18:25:12 -0400 Subject: [PATCH 09/20] changed max velocity to 2 --- src/main/deploy/choreo/Depot_Fast.traj | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index 39ec463f..32792931 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -21,7 +21,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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", From 0f93f57944262a036764dc0eaade8b6f158ddf02 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 18:30:32 -0400 Subject: [PATCH 10/20] oops i didn't generate the path --- src/main/deploy/choreo/Depot_Fast.traj | 148 ++++++++++++------------- 1 file changed, 70 insertions(+), 78 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index 32792931..b516c35d 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,19 +3,19 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.118183970451355, "y":5.943911552429199, "heading":3.141592653589793, "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":16.541, "h":8.0692}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.118183970451355 m", "val":1.118183970451355}, "y":{"exp":"5.943911552429199 m", "val":5.943911552429199}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -53,82 +53,74 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,3.63172], + "waypoints":[0.0,3.24495], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.94147, "ay":0.33588, "alpha":0.00228, "fx":[-10.65214,-10.65219,-10.70005,-10.70007], "fy":[3.82049,3.74265,3.85147,3.82047]}, - {"t":0.04975, "x":3.59483, "y":5.06042, "heading":3.14159, "vx":-0.04684, "vy":0.01671, "omega":0.00011, "ax":-0.94165, "ay":0.33594, "alpha":0.00442, "fx":[-10.65487,-10.65489,-10.70146,-10.70148], "fy":[3.80911,3.78065,3.83924,3.80909]}, - {"t":0.0995, "x":3.59134, "y":5.06166, "heading":-3.14159, "vx":-0.09368, "vy":0.03342, "omega":0.00033, "ax":-0.94164, "ay":0.33594, "alpha":0.00038, "fx":[-10.70459,-10.70455,-10.73425,-10.56877], "fy":[3.75341,3.83807,3.83805,3.80843]}, - {"t":0.14925, "x":3.58551, "y":5.06374, "heading":-3.14157, "vx":-0.14053, "vy":0.05014, "omega":0.00035, "ax":-0.94163, "ay":0.33594, "alpha":0.00374, "fx":[-10.65552,-10.65555,-10.70025,-10.70027], "fy":[3.81173,3.77375,3.84065,3.81171]}, - {"t":0.199, "x":3.57736, "y":5.06665, "heading":-3.14155, "vx":-0.18738, "vy":0.06685, "omega":0.00054, "ax":-0.94161, "ay":0.33593, "alpha":-0.00054, "fx":[-10.7103,-10.71025,-10.73887,-10.55149], "fy":[3.74896,3.83909,3.83908,3.81055]}, - {"t":0.24875, "x":3.56687, "y":5.07039, "heading":-3.14153, "vx":-0.23422, "vy":0.08356, "omega":0.00051, "ax":-0.9416, "ay":0.33593, "alpha":0.0031, "fx":[-10.65592,-10.65596,-10.69912,-10.69915], "fy":[3.81432,3.76663,3.84224,3.8143]}, - {"t":0.2985, "x":3.55405, "y":5.07497, "heading":-3.1415, "vx":-0.28107, "vy":0.10027, "omega":0.00067, "ax":-0.94158, "ay":0.33593, "alpha":-0.00129, "fx":[-10.71689,-10.71684,-10.74455,-10.531], "fy":[3.73912,3.84195,3.84193,3.81432]}, - {"t":0.34825, "x":3.5389, "y":5.08037, "heading":-3.14147, "vx":-0.32791, "vy":0.11699, "omega":0.0006, "ax":-0.94156, "ay":0.33592, "alpha":0.00242, "fx":[-10.65611,-10.65615,-10.698,-10.69803], "fy":[3.8172,3.75841,3.84427,3.81718]}, - {"t":0.398, "x":3.52143, "y":5.08661, "heading":-3.14144, "vx":-0.37475, "vy":0.1337, "omega":0.00072, "ax":-0.94153, "ay":0.33591, "alpha":-0.00228, "fx":[-10.72591,-10.72585,-10.75277,-10.50258], "fy":[3.72538,3.84608,3.84607,3.81927]}, - {"t":0.44775, "x":3.50162, "y":5.09367, "heading":-3.1414, "vx":-0.42159, "vy":0.15041, "omega":0.00061, "ax":-0.9415, "ay":0.33591, "alpha":0.00132, "fx":[-10.65604,-10.6561,-10.69678,-10.6968], "fy":[3.82241,3.74301,3.84871,3.82239]}, - {"t":0.4975, "x":3.47948, "y":5.10157, "heading":-3.14137, "vx":-0.46843, "vy":0.16712, "omega":0.00067, "ax":-0.94146, "ay":0.3359, "alpha":-0.00418, "fx":[-10.73895,-10.73889,-10.76509,-10.46111], "fy":[3.71375,3.84949,3.84948,3.82341]}, - {"t":0.54725, "x":3.45501, "y":5.1103, "heading":-3.14134, "vx":-0.51527, "vy":0.18383, "omega":0.00047, "ax":-0.94142, "ay":0.33589, "alpha":0.00007, "fx":[-10.65574,-10.6558,-10.69523,-10.69525], "fy":[3.8283,3.7253,3.85377,3.82828]}, - {"t":0.59699, "x":3.42821, "y":5.11986, "heading":-3.14131, "vx":-0.5621, "vy":0.20054, "omega":0.00047, "ax":-0.94136, "ay":0.33588, "alpha":-0.00779, "fx":[-10.75888,-10.75882,-10.78421,-10.39756], "fy":[3.70754,3.85092,3.85091,3.82569]}, - {"t":0.64674, "x":3.39908, "y":5.13025, "heading":-3.14129, "vx":-0.60894, "vy":0.21725, "omega":0.00008, "ax":-0.94129, "ay":0.33586, "alpha":-0.00138, "fx":[-10.6551,-10.65518,-10.69294,-10.69296], "fy":[3.83489,3.70533,3.85928,3.83487]}, - {"t":0.69649, "x":3.36762, "y":5.14148, "heading":-3.14129, "vx":-0.65576, "vy":0.23396, "omega":0.00001, "ax":-0.94119, "ay":0.33584, "alpha":-0.00952, "fx":[-10.7771,-10.77702,-10.80111,-10.33656], "fy":[3.67338,3.86131,3.8613,3.8374]}, - {"t":0.74624, "x":3.33383, "y":5.15353, "heading":-3.14129, "vx":-0.70259, "vy":0.25067, "omega":-0.00046, "ax":-0.94106, "ay":0.33581, "alpha":-0.00243, "fx":[-10.65368,-10.65375,-10.68909,-10.6891], "fy":[3.83876,3.69301,3.86156,3.83874]}, - {"t":0.79599, "x":3.29771, "y":5.16642, "heading":-3.14131, "vx":-0.74941, "vy":0.26737, "omega":-0.00058, "ax":-0.94085, "ay":0.33576, "alpha":-0.01052, "fx":[-10.796,-10.79591,-10.8182,-10.26636], "fy":[3.61845,3.8778,3.87779,3.85569]}, - {"t":0.84574, "x":3.25927, "y":5.18014, "heading":-3.14134, "vx":-0.79621, "vy":0.28408, "omega":-0.0011, "ax":-0.94051, "ay":0.33569, "alpha":-0.00444, "fx":[-10.64911,-10.64917,-10.68128,-10.68129], "fy":[3.84655,3.66627,3.86728,3.84654]}, - {"t":0.89549, "x":3.21849, "y":5.19468, "heading":-3.14139, "vx":-0.843, "vy":0.30078, "omega":-0.00132, "ax":-0.9398, "ay":0.3356, "alpha":-0.01869, "fx":[-10.82951,-10.82945,-10.84977,-10.11982], "fy":[3.60984,3.87768,3.87767,3.85751]}, - {"t":0.94524, "x":3.17539, "y":5.21006, "heading":-3.14146, "vx":-0.88976, "vy":0.31747, "omega":-0.00225, "ax":-0.93775, "ay":0.33499, "alpha":-0.01284, "fx":[-10.61976,-10.61983,-10.64795,-10.64795], "fy":[3.88169,3.53087,3.90061,3.88169]}, - {"t":0.99499, "x":3.12996, "y":5.22627, "heading":-3.14157, "vx":-0.93641, "vy":0.33414, "omega":-0.00289, "ax":-0.10609, "ay":0.03507, "alpha":0.02429, "fx":[-1.00877,-1.00876,-1.02059,-1.77391], "fy":[0.60092,0.33385,0.33385,0.32203]}, - {"t":1.04474, "x":3.08325, "y":5.24294, "heading":3.14147, "vx":-0.94169, "vy":0.33588, "omega":-0.00168, "ax":-0.00197, "ay":-0.00553, "alpha":0.00521, "fx":[-0.01204,-0.01204,-0.03271,-0.03271], "fy":[-0.08029,-0.02335,-0.06668,-0.08029]}, - {"t":1.09449, "x":3.0364, "y":5.25964, "heading":3.14139, "vx":-0.94179, "vy":0.33561, "omega":-0.00142, "ax":0.00167, "ay":0.00469, "alpha":0.00735, "fx":[0.0606,0.06059,0.04875,-0.09409], "fy":[0.07157,0.05102,0.05102,0.03917]}, - {"t":1.14424, "x":2.98954, "y":5.27634, "heading":3.14132, "vx":-0.9417, "vy":0.33584, "omega":-0.00106, "ax":0.00027, "ay":0.00077, "alpha":0.00678, "fx":[0.01064,0.01064,-0.00444,-0.00444], "fy":[-0.02019,0.08545,-0.01029,-0.02019]}, - {"t":1.19399, "x":2.9427, "y":5.29305, "heading":3.14126, "vx":-0.94169, "vy":0.33588, "omega":-0.00072, "ax":0.00023, "ay":0.00065, "alpha":0.00981, "fx":[0.07413,0.07413,0.06562,-0.20342], "fy":[0.07237,-0.01151,-0.01151,-0.02001]}, - {"t":1.24374, "x":2.89585, "y":5.30976, "heading":3.14123, "vx":-0.94168, "vy":0.33591, "omega":-0.00023, "ax":0.00002, "ay":0.00007, "alpha":0.0036, "fx":[0.00593,0.00593,-0.00538,-0.00538], "fy":[-0.01291,0.03421,-0.00533,-0.01291]}, - {"t":1.29349, "x":2.849, "y":5.32648, "heading":3.14122, "vx":-0.94168, "vy":0.33592, "omega":-0.00005, "ax":0.00007, "ay":0.00019, "alpha":0.00421, "fx":[0.02872,0.02872,0.02214,-0.07651], "fy":[0.02479,-0.0032,-0.0032,-0.00978]}, - {"t":1.34324, "x":2.80215, "y":5.34319, "heading":3.14121, "vx":-0.94167, "vy":0.33593, "omega":0.00015, "ax":0.00002, "ay":0.00005, "alpha":0.00209, "fx":[0.00446,0.00446,-0.00406,-0.00406], "fy":[-0.00639,0.01571,-0.00064,-0.00639]}, - {"t":1.39299, "x":2.7553, "y":5.3599, "heading":3.14122, "vx":-0.94167, "vy":0.33593, "omega":0.00026, "ax":0.00001, "ay":0.00002, "alpha":0.00226, "fx":[0.01351,0.01351,0.00855,-0.03518], "fy":[0.00913,-0.00103,-0.00103,-0.00599]}, - {"t":1.44274, "x":2.70846, "y":5.37661, "heading":3.14123, "vx":-0.94167, "vy":0.33593, "omega":0.00037, "ax":0.0, "ay":0.00001, "alpha":0.00127, "fx":[0.00313,0.00313,-0.00309,-0.00309], "fy":[-0.00362,0.00687,0.00063,-0.00362]}, - {"t":1.49249, "x":2.66161, "y":5.39332, "heading":3.14125, "vx":-0.94167, "vy":0.33593, "omega":0.00043, "ax":0.0, "ay":-0.00001, "alpha":0.00126, "fx":[0.00653,0.00653,0.00296,-0.01623], "fy":[0.0029,0.00002,0.00002,-0.00355]}, - {"t":1.54224, "x":2.61476, "y":5.41004, "heading":3.14127, "vx":-0.94167, "vy":0.33593, "omega":0.0005, "ax":0.0, "ay":-0.00001, "alpha":0.00079, "fx":[0.00203,0.00203,-0.00213,-0.00213], "fy":[-0.00228,0.00334,0.00066,-0.00228]}, - {"t":1.59199, "x":2.56791, "y":5.42675, "heading":3.1413, "vx":-0.94167, "vy":0.33593, "omega":0.00054, "ax":-0.00001, "ay":-0.00001, "alpha":0.00073, "fx":[0.00347,0.00347,0.00116,-0.00834], "fy":[0.00098,0.00022,0.00022,-0.00209]}, - {"t":1.64174, "x":2.52106, "y":5.44346, "heading":3.14132, "vx":-0.94167, "vy":0.33593, "omega":0.00057, "ax":0.0, "ay":-0.00001, "alpha":0.00043, "fx":[0.00108,0.00108,-0.00118,-0.00118], "fy":[-0.0013,0.00165,0.00042,-0.0013]}, - {"t":1.69149, "x":2.47422, "y":5.46017, "heading":3.14135, "vx":-0.94167, "vy":0.33593, "omega":0.00059, "ax":0.0, "ay":-0.00001, "alpha":0.00035, "fx":[0.0016,0.0016,0.00048,-0.00379], "fy":[0.00037,0.00016,0.00016,-0.00097]}, - {"t":1.74124, "x":2.42737, "y":5.47689, "heading":3.14138, "vx":-0.94167, "vy":0.33593, "omega":0.00061, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00021,0.00021,-0.00023,-0.00023], "fy":[-0.0004,0.00053,0.00016,-0.0004]}, - {"t":1.79098, "x":2.38052, "y":5.4936, "heading":3.14141, "vx":-0.94167, "vy":0.33593, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00006,0.00006,0.00009,-0.00015], "fy":[0.00009,0.00002,0.00002,0.00005]}, - {"t":1.84073, "x":2.33367, "y":5.51031, "heading":3.14144, "vx":-0.94167, "vy":0.33593, "omega":0.00062, "ax":0.0, "ay":0.00001, "alpha":-0.0002, "fx":[-0.00064,-0.00064,0.00073,0.00073], "fy":[0.00052,-0.00047,-0.00009,0.00052]}, - {"t":1.89048, "x":2.28683, "y":5.52702, "heading":3.14147, "vx":-0.94167, "vy":0.33593, "omega":0.00061, "ax":0.00001, "ay":0.00002, "alpha":-0.00034, "fx":[-0.00147,-0.00147,-0.00027,0.00348], "fy":[-0.00017,-0.00008,-0.00008,0.00111]}, - {"t":1.94023, "x":2.23998, "y":5.54374, "heading":3.1415, "vx":-0.94167, "vy":0.33593, "omega":0.00059, "ax":0.00001, "ay":0.00002, "alpha":-0.00053, "fx":[-0.00152,-0.00152,0.00172,0.00172], "fy":[0.0015,-0.0016,-0.00029,0.0015]}, - {"t":1.98998, "x":2.19313, "y":5.56045, "heading":3.14153, "vx":-0.94167, "vy":0.33593, "omega":0.00056, "ax":0.00001, "ay":0.00003, "alpha":-0.00072, "fx":[-0.0033,-0.0033,-0.00089,0.00799], "fy":[-0.00078,-0.00008,-0.00008,0.00233]}, - {"t":2.03973, "x":2.14628, "y":5.57716, "heading":3.14156, "vx":-0.94167, "vy":0.33593, "omega":0.00053, "ax":0.00001, "ay":0.00004, "alpha":-0.0009, "fx":[-0.00245,-0.00245,0.00274,0.00274], "fy":[0.00265,-0.0033,-0.00039,0.00265]}, - {"t":2.08948, "x":2.09943, "y":5.59387, "heading":3.14159, "vx":-0.94167, "vy":0.33593, "omega":0.00048, "ax":0.00001, "ay":0.00004, "alpha":-0.00123, "fx":[-0.00626,-0.00626,-0.00254,0.01572], "fy":[-0.00268,0.00026,0.00026,0.00398]}, - {"t":2.13923, "x":2.05259, "y":5.61059, "heading":-3.14157, "vx":-0.94167, "vy":0.33594, "omega":0.00042, "ax":0.00001, "ay":0.00004, "alpha":-0.00141, "fx":[-0.00351,-0.00351,0.00382,0.00382], "fy":[0.00431,-0.0068,-0.00011,0.00431]}, - {"t":2.18898, "x":2.00574, "y":5.6273, "heading":-3.14155, "vx":-0.94167, "vy":0.33594, "omega":0.00035, "ax":0.00001, "ay":0.00002, "alpha":-0.0022, "fx":[-0.013,-0.013,-0.00782,0.03412], "fy":[-0.00882,0.00149,0.00149,0.00668]}, - {"t":2.23873, "x":1.95889, "y":5.64401, "heading":-3.14153, "vx":-0.94167, "vy":0.33594, "omega":0.00024, "ax":0.00001, "ay":0.00002, "alpha":-0.00226, "fx":[-0.00478,-0.00478,0.00498,0.00498], "fy":[0.00753,-0.01551,0.00154,0.00753]}, - {"t":2.28848, "x":1.91204, "y":5.66072, "heading":-3.14152, "vx":-0.94167, "vy":0.33594, "omega":0.00013, "ax":-0.00005, "ay":-0.00014, "alpha":-0.00408, "fx":[-0.02774,-0.02774,-0.02084,0.07402], "fy":[-0.02427,0.00366,0.00366,0.01056]}, - {"t":2.33823, "x":1.8652, "y":5.67744, "heading":-3.14152, "vx":-0.94167, "vy":0.33593, "omega":-0.00007, "ax":0.00001, "ay":0.00003, "alpha":-0.00379, "fx":[-0.00624,-0.00624,0.00645,0.00645], "fy":[0.01431,-0.03385,0.00641,0.01431]}, - {"t":2.38798, "x":1.81835, "y":5.69415, "heading":-3.14152, "vx":-0.94167, "vy":0.33593, "omega":-0.00026, "ax":-0.00027, "ay":-0.00075, "alpha":-0.00936, "fx":[-0.07147,-0.07147,-0.06252,0.19325], "fy":[-0.07198,0.0096,0.0096,0.01855]}, - {"t":2.43773, "x":1.7715, "y":5.71086, "heading":-3.14153, "vx":-0.94168, "vy":0.3359, "omega":-0.00073, "ax":-0.00031, "ay":-0.00086, "alpha":-0.00689, "fx":[-0.01178,-0.01178,0.00483,0.00483], "fy":[0.01894,-0.08548,0.00861,0.01894]}, - {"t":2.48748, "x":1.72465, "y":5.72757, "heading":-3.14157, "vx":-0.9417, "vy":0.33585, "omega":-0.00107, "ax":-0.0015, "ay":-0.00421, "alpha":-0.0071, "fx":[-0.06211,-0.06211,-0.05004,0.10625], "fy":[-0.08074,-0.04071,-0.04071,-0.02863]}, - {"t":2.53723, "x":1.6778, "y":5.74427, "heading":3.14156, "vx":-0.94177, "vy":0.33564, "omega":-0.00142, "ax":0.00178, "ay":0.00498, "alpha":-0.00548, "fx":[0.00908,0.00908,0.03131,0.0313], "fy":[0.07481,0.01564,0.06081,0.07482]}, - {"t":2.58698, "x":1.63095, "y":5.76098, "heading":3.14149, "vx":-0.94168, "vy":0.33589, "omega":-0.0017, "ax":0.10542, "ay":-0.03632, "alpha":-0.02363, "fx":[1.00714,1.00714,1.01962,1.7481], "fy":[-0.60892,-0.35031,-0.35031,-0.33784]}, - {"t":2.63673, "x":1.58423, "y":5.77764, "heading":3.14141, "vx":-0.93644, "vy":0.33408, "omega":-0.00287, "ax":0.93803, "ay":-0.3342, "alpha":0.01235, "fx":[10.62223,10.62229,10.65186,10.65187], "fy":[-3.87089,-3.52701,-3.89019,-3.87089]}, - {"t":2.68648, "x":1.53881, "y":5.79385, "heading":3.14127, "vx":-0.88977, "vy":0.31746, "omega":-0.00226, "ax":0.93984, "ay":-0.33549, "alpha":0.01823, "fx":[10.82887,10.82882,10.84937,10.12333], "fy":[-3.60446,-3.87782,-3.87782,-3.85742]}, - {"t":2.73623, "x":1.4957, "y":5.80923, "heading":3.14115, "vx":-0.84302, "vy":0.30077, "omega":-0.00135, "ax":0.94053, "ay":-0.33564, "alpha":0.0065, "fx":[10.64851,10.64861,10.68226,10.68227], "fy":[-3.85786,-3.62967,-3.87901,-3.85784]}, - {"t":2.78598, "x":1.45493, "y":5.82378, "heading":3.14109, "vx":-0.79623, "vy":0.28407, "omega":-0.00103, "ax":0.94086, "ay":-0.33574, "alpha":0.01398, "fx":[10.80984,10.80976,10.83249,10.22468], "fy":[-3.63302,-3.8728,-3.87279,-3.85025]}, - {"t":2.83573, "x":1.41648, "y":5.83749, "heading":3.14103, "vx":-0.74942, "vy":0.26737, "omega":-0.00033, "ax":0.94107, "ay":-0.33579, "alpha":0.00432, "fx":[10.65318,10.65329,10.68976,10.68977], "fy":[-3.84928,-3.6602,-3.87228,-3.84926]}, - {"t":2.88548, "x":1.38036, "y":5.85038, "heading":3.14102, "vx":-0.7026, "vy":0.25066, "omega":-0.00012, "ax":0.9412, "ay":-0.33582, "alpha":0.00904, "fx":[10.7762,10.77612,10.80009,10.33975], "fy":[-3.66801,-3.86272,-3.86271,-3.83891]}, - {"t":2.93522, "x":1.34657, "y":5.86243, "heading":3.14101, "vx":-0.65578, "vy":0.23396, "omega":0.00033, "ax":0.9413, "ay":-0.33584, "alpha":0.00128, "fx":[10.65501,10.65509,10.69318,10.69319], "fy":[-3.83437,-3.7064,-3.85843,-3.83435]}, - {"t":2.98497, "x":1.31511, "y":5.87366, "heading":3.14103, "vx":-0.60895, "vy":0.21725, "omega":0.0004, "ax":0.94137, "ay":-0.33586, "alpha":0.00755, "fx":[10.75845,10.75839,10.78324,10.39967], "fy":[-3.70425,-3.8516,-3.85158,-3.82689]}, - {"t":3.03472, "x":1.28598, "y":5.88405, "heading":3.14105, "vx":-0.56211, "vy":0.20054, "omega":0.00077, "ax":0.94142, "ay":-0.33588, "alpha":-0.00005, "fx":[10.65594,10.65601,10.69513,10.69515], "fy":[-3.82811,-3.72598,-3.85286,-3.82809]}, - {"t":3.08447, "x":1.25918, "y":5.89361, "heading":3.14109, "vx":-0.51528, "vy":0.18383, "omega":0.00077, "ax":0.94147, "ay":-0.33589, "alpha":0.00488, "fx":[10.74096,10.7409,10.76621,10.4562], "fy":[-3.718,-3.84758,-3.84756,-3.82239]}, - {"t":3.13422, "x":1.23471, "y":5.90234, "heading":3.14113, "vx":-0.46844, "vy":0.16712, "omega":0.00101, "ax":0.9415, "ay":-0.33589, "alpha":-0.00116, "fx":[10.65655,10.65661,10.69637,10.69639], "fy":[-3.82266,-3.74278,-3.84783,-3.82264]}, - {"t":3.18397, "x":1.21257, "y":5.91024, "heading":3.14118, "vx":-0.4216, "vy":0.15041, "omega":0.00096, "ax":0.94153, "ay":-0.3359, "alpha":0.00291, "fx":[10.72782,10.72777,10.75339,10.4983], "fy":[-3.72844,-3.84446,-3.84445,-3.81895]}, - {"t":3.23372, "x":1.19276, "y":5.91731, "heading":3.14122, "vx":-0.37476, "vy":0.1337, "omega":0.0011, "ax":0.94156, "ay":-0.33591, "alpha":-0.00221, "fx":[10.65693,10.65698,10.69726,10.69728], "fy":[-3.81746,-3.7587,-3.84299,-3.81744]}, - {"t":3.28347, "x":1.17528, "y":5.92354, "heading":3.14128, "vx":-0.32792, "vy":0.11699, "omega":0.00099, "ax":0.94158, "ay":-0.33592, "alpha":0.00181, "fx":[10.71829,10.71824,10.74421,10.5287], "fy":[-3.74156,-3.8404,-3.84038,-3.81452]}, - {"t":3.33322, "x":1.16013, "y":5.92895, "heading":3.14133, "vx":-0.28108, "vy":0.10027, "omega":0.00108, "ax":0.9416, "ay":-0.33592, "alpha":-0.00274, "fx":[10.65709,10.65713,10.69803,10.69805], "fy":[-3.81502,-3.7661,-3.84096,-3.815]}, - {"t":3.38297, "x":1.14732, "y":5.93352, "heading":3.14138, "vx":-0.23423, "vy":0.08356, "omega":0.00094, "ax":0.94162, "ay":-0.33593, "alpha":0.00104, "fx":[10.7116,10.71156,10.73798,10.54989], "fy":[-3.75097,-3.83756,-3.83755,-3.81122]}, - {"t":3.43272, "x":1.13683, "y":5.93726, "heading":3.14143, "vx":-0.18739, "vy":0.06685, "omega":0.001, "ax":0.94163, "ay":-0.33593, "alpha":-0.00331, "fx":[10.65704,10.65707,10.69878,10.6988], "fy":[-3.8125,-3.77352,-3.83898,-3.81248]}, - {"t":3.48247, "x":1.12867, "y":5.94017, "heading":3.14148, "vx":-0.14054, "vy":0.05014, "omega":0.00083, "ax":0.94164, "ay":-0.33593, "alpha":0.00005, "fx":[10.70567,10.70563,10.73265,10.56834], "fy":[-3.75439,-3.83674,-3.83672,-3.80978]}, - {"t":3.53222, "x":1.12285, "y":5.94225, "heading":3.14152, "vx":-0.09369, "vy":0.03343, "omega":0.00083, "ax":0.94166, "ay":-0.33594, "alpha":-0.00369, "fx":[10.65677,10.6568,10.69962,10.69964], "fy":[-3.81108,-3.77736,-3.83829,-3.81105]}, - {"t":3.58197, "x":1.11935, "y":5.9435, "heading":3.14156, "vx":-0.04685, "vy":0.01671, "omega":0.00065, "ax":0.94167, "ay":-0.33594, "alpha":-0.01307, "fx":[10.69967,10.55976,10.72693,10.72695], "fy":[-3.78794,-3.83108,-3.83095,-3.78792]}, - {"t":3.63172, "x":1.11818, "y":5.94391, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.94148, "ay":0.33585, "alpha":0.00289, "fx":[-10.65382,-10.65386,-10.69861,-10.69863], "fy":[3.81539,3.75401,3.8493,3.81537]}, + {"t":0.04992, "x":3.59483, "y":5.06042, "heading":3.14159, "vx":-0.047, "vy":0.01677, "omega":0.00014, "ax":-0.94167, "ay":0.33592, "alpha":0.00471, "fx":[-10.65657,-10.65658,-10.70008,-10.70011], "fy":[3.80577,3.78681,3.83873,3.80574]}, + {"t":0.09984, "x":3.59131, "y":5.06167, "heading":-3.14159, "vx":-0.09401, "vy":0.03354, "omega":0.00038, "ax":-0.94166, "ay":0.33592, "alpha":0.00112, "fx":[-10.69924,-10.69919,-10.73167,-10.58293], "fy":[3.76076,3.83621,3.83619,3.80379]}, + {"t":0.14977, "x":3.58544, "y":5.06377, "heading":-3.14157, "vx":-0.14102, "vy":0.05031, "omega":0.00044, "ax":-0.94165, "ay":0.33591, "alpha":0.00419, "fx":[-10.65731,-10.65732,-10.69903,-10.69905], "fy":[3.80757,3.78253,3.8392,3.80754]}, + {"t":0.19969, "x":3.57723, "y":5.0667, "heading":-3.14154, "vx":-0.18803, "vy":0.06708, "omega":0.00064, "ax":-0.94165, "ay":0.33591, "alpha":0.00049, "fx":[-10.70288,-10.70283,-10.73415,-10.5725], "fy":[3.75829,3.83656,3.83655,3.80532]}, + {"t":0.24961, "x":3.56667, "y":5.07046, "heading":-3.14151, "vx":-0.23504, "vy":0.08385, "omega":0.00067, "ax":-0.94164, "ay":0.33591, "alpha":0.00377, "fx":[-10.65782,-10.65784,-10.69816,-10.69818], "fy":[3.80909,3.77872,3.8397,3.80907]}, + {"t":0.29953, "x":3.55376, "y":5.07507, "heading":-3.14148, "vx":-0.28205, "vy":0.10061, "omega":0.00086, "ax":-0.94163, "ay":0.33591, "alpha":-0.00012, "fx":[-10.7064,-10.70635,-10.7368,-10.56206], "fy":[3.756,3.83694,3.83692,3.80657]}, + {"t":0.34946, "x":3.53851, "y":5.08051, "heading":-3.14144, "vx":-0.32906, "vy":0.11738, "omega":0.00085, "ax":-0.94162, "ay":0.3359, "alpha":0.00338, "fx":[-10.65814,-10.65817,-10.69742,-10.69745], "fy":[3.81058,3.77474,3.84042,3.81055]}, + {"t":0.39938, "x":3.5209, "y":5.08679, "heading":-3.14139, "vx":-0.37607, "vy":0.13415, "omega":0.00102, "ax":-0.94161, "ay":0.3359, "alpha":-0.00043, "fx":[-10.70983,-10.70978,-10.73956,-10.55154], "fy":[3.74933,3.83884,3.83882,3.80914]}, + {"t":0.4493, "x":3.50096, "y":5.0939, "heading":-3.14134, "vx":-0.42307, "vy":0.15092, "omega":0.001, "ax":-0.9416, "ay":0.33589, "alpha":0.00289, "fx":[-10.65829,-10.65833,-10.69679,-10.69681], "fy":[3.81273,3.76848,3.84201,3.81271]}, + {"t":0.49922, "x":3.47866, "y":5.10186, "heading":-3.14129, "vx":-0.47008, "vy":0.16769, "omega":0.00114, "ax":-0.94159, "ay":0.33589, "alpha":-0.00145, "fx":[-10.71517,-10.71512,-10.74445,-10.53494], "fy":[3.74829,3.8389,3.83888,3.80966]}, + {"t":0.54915, "x":3.45402, "y":5.11065, "heading":-3.14124, "vx":-0.51709, "vy":0.18446, "omega":0.00107, "ax":-0.94157, "ay":0.33589, "alpha":0.00255, "fx":[-10.65832,-10.65836,-10.69618,-10.6962], "fy":[3.81414,3.76426,3.84299,3.81412]}, + {"t":0.59907, "x":3.42703, "y":5.12027, "heading":-3.14118, "vx":-0.56409, "vy":0.20123, "omega":0.0012, "ax":-0.94156, "ay":0.33588, "alpha":-0.00197, "fx":[-10.71951,-10.71945,-10.74839,-10.52106], "fy":[3.7423,3.84061,3.84059,3.81178]}, + {"t":0.64899, "x":3.3977, "y":5.13074, "heading":-3.14112, "vx":-0.6111, "vy":0.218, "omega":0.0011, "ax":-0.94154, "ay":0.33587, "alpha":0.00215, "fx":[-10.65825,-10.65829,-10.69556,-10.69558], "fy":[3.81587,3.75897,3.84433,3.81585]}, + {"t":0.69891, "x":3.36602, "y":5.14204, "heading":-3.14107, "vx":-0.6581, "vy":0.23476, "omega":0.00121, "ax":-0.94153, "ay":0.33587, "alpha":-0.00299, "fx":[-10.72551,-10.72545,-10.75403,-10.50188], "fy":[3.73907,3.84138,3.84136,3.81291]}, + {"t":0.74884, "x":3.33199, "y":5.15418, "heading":-3.14101, "vx":-0.7051, "vy":0.25153, "omega":0.00106, "ax":-0.94151, "ay":0.33586, "alpha":0.00153, "fx":[-10.6581,-10.65815,-10.69484,-10.69486], "fy":[3.81875,3.75008,3.84682,3.81873]}, + {"t":0.79876, "x":3.29562, "y":5.16715, "heading":-3.14096, "vx":-0.75211, "vy":0.2683, "omega":0.00113, "ax":-0.94148, "ay":0.33585, "alpha":-0.00376, "fx":[-10.73366,-10.73359,-10.76174,-10.47595], "fy":[3.72493,3.84571,3.84569,3.81769]}, + {"t":0.84868, "x":3.2569, "y":5.18097, "heading":-3.1409, "vx":-0.79911, "vy":0.28506, "omega":0.00095, "ax":-0.94146, "ay":0.33584, "alpha":0.00102, "fx":[-10.65793,-10.65798,-10.69393,-10.69394], "fy":[3.82088,3.74343,3.84842,3.82086]}, + {"t":0.8986, "x":3.21583, "y":5.19562, "heading":-3.14085, "vx":-0.84611, "vy":0.30183, "omega":0.001, "ax":-0.94143, "ay":0.33583, "alpha":-0.00527, "fx":[-10.74567,-10.74559,-10.77317,-10.43803], "fy":[3.7106,3.84998,3.84997,3.82257]}, + {"t":0.94853, "x":3.17242, "y":5.2111, "heading":-3.1408, "vx":-0.8931, "vy":0.3186, "omega":0.00073, "ax":-0.94139, "ay":0.33582, "alpha":-0.0004, "fx":[-10.65771,-10.65778,-10.69272,-10.69273], "fy":[3.82775,3.72253,3.85455,3.82773]}, + {"t":0.99845, "x":3.12666, "y":5.22743, "heading":-3.14077, "vx":-0.9401, "vy":0.33536, "omega":0.00071, "ax":-0.94136, "ay":0.33581, "alpha":-0.00617, "fx":[-10.75423,-10.75415,-10.78075,-10.41003], "fy":[3.69579,3.85419,3.85418,3.82776]}, + {"t":1.04837, "x":3.07856, "y":5.24459, "heading":-3.14073, "vx":-0.9871, "vy":0.35212, "omega":0.00041, "ax":-0.94131, "ay":0.33579, "alpha":-0.00126, "fx":[-10.65752,-10.6576,-10.69095,-10.69097], "fy":[3.83122,3.71193,3.85681,3.83121]}, + {"t":1.09829, "x":3.0281, "y":5.26258, "heading":-3.14071, "vx":-1.03409, "vy":0.36889, "omega":0.00034, "ax":-0.94125, "ay":0.33577, "alpha":-0.00777, "fx":[-10.76344,-10.76336,-10.78851,-10.37921], "fy":[3.68855,3.85556,3.85554,3.83059]}, + {"t":1.14821, "x":2.97531, "y":5.28142, "heading":-3.14069, "vx":-1.08108, "vy":0.38565, "omega":-0.00005, "ax":-0.94119, "ay":0.33574, "alpha":-0.00273, "fx":[-10.65727,-10.65735,-10.68841,-10.68842], "fy":[3.83744,3.693,3.86125,3.83743]}, + {"t":1.19814, "x":2.92016, "y":5.30109, "heading":-3.14069, "vx":-1.12806, "vy":0.40241, "omega":-0.00018, "ax":-0.9411, "ay":0.33571, "alpha":-0.00918, "fx":[-10.77651,-10.77642,-10.79943,-10.33525], "fy":[3.66414,3.86214,3.86213,3.83931]}, + {"t":1.24806, "x":2.86268, "y":5.3216, "heading":-3.1407, "vx":-1.17505, "vy":0.41917, "omega":-0.00064, "ax":-0.94099, "ay":0.33567, "alpha":-0.00376, "fx":[-10.65674,-10.65683,-10.68453,-10.68454], "fy":[3.84052,3.6831,3.86178,3.84051]}, + {"t":1.29798, "x":2.80284, "y":5.34294, "heading":-3.14074, "vx":-1.22202, "vy":0.43593, "omega":-0.00083, "ax":-0.94085, "ay":0.33562, "alpha":-0.01304, "fx":[-10.80121,-10.80112,-10.82126,-10.25245], "fy":[3.63956,3.86796,3.86796,3.84804]}, + {"t":1.3479, "x":2.74066, "y":5.36512, "heading":-3.14078, "vx":-1.26899, "vy":0.45268, "omega":-0.00148, "ax":-0.94064, "ay":0.33555, "alpha":-0.00861, "fx":[-10.65502,-10.65514,-10.67831,-10.67831], "fy":[3.86283,3.61389,3.88067,3.86282]}, + {"t":1.39783, "x":2.67614, "y":5.38814, "heading":-3.14085, "vx":-1.31595, "vy":0.46944, "omega":-0.00191, "ax":-0.94034, "ay":0.33544, "alpha":-0.02271, "fx":[-10.85736,-10.85727,-10.87342,-10.0649], "fy":[3.60348,3.87583,3.87583,3.85995]}, + {"t":1.44775, "x":2.60927, "y":5.41199, "heading":-3.14095, "vx":-1.3629, "vy":0.48618, "omega":-0.00304, "ax":-0.93983, "ay":0.33525, "alpha":-0.01177, "fx":[-10.64943,-10.64954,-10.66544,-10.66545], "fy":[3.8728,3.57594,3.88512,3.8728]}, + {"t":1.49767, "x":2.54006, "y":5.43668, "heading":-3.1411, "vx":-1.40981, "vy":0.50292, "omega":-0.00363, "ax":-0.93881, "ay":0.33488, "alpha":-0.01451, "fx":[-10.77798,-10.77793,-10.78654,-10.24122], "fy":[3.64365,3.85161,3.85161,3.84313]}, + {"t":1.54759, "x":2.46851, "y":5.46221, "heading":-3.14128, "vx":-1.45668, "vy":0.51964, "omega":-0.00435, "ax":-0.93571, "ay":0.33391, "alpha":-0.03063, "fx":[-10.60724,-10.60733,-10.61434,-10.61434], "fy":[3.95445,3.27297,3.96417,3.95445]}, + {"t":1.59752, "x":2.39463, "y":5.48856, "heading":-3.1415, "vx":-1.50339, "vy":0.53631, "omega":-0.00588, "ax":-0.00009, "ay":0.00035, "alpha":-0.0004, "fx":[-0.00839,-0.00839,-0.00716,0.01978], "fy":[-0.01289,0.00915,0.00915,0.01038]}, + {"t":1.64744, "x":2.31957, "y":5.51534, "heading":3.1414, "vx":-1.5034, "vy":0.53632, "omega":-0.0059, "ax":0.93569, "ay":-0.33398, "alpha":0.02917, "fx":[10.60516,10.60525,10.61585,10.61585], "fy":[-3.94929,-3.28912,-3.96153,-3.94929]}, + {"t":1.69736, "x":2.24568, "y":5.5417, "heading":3.1411, "vx":-1.45669, "vy":0.51965, "omega":-0.00445, "ax":0.9388, "ay":-0.33492, "alpha":0.02657, "fx":[10.86874,10.86869,10.88061,9.9651], "fy":[-3.56846,-3.87826,-3.87826,-3.86652]}, + {"t":1.74728, "x":2.17413, "y":5.56722, "heading":3.14088, "vx":-1.40982, "vy":0.50293, "omega":-0.00312, "ax":0.93982, "ay":-0.33528, "alpha":0.01055, "fx":[10.64774,10.64785,10.66692,10.66693], "fy":[-3.86824,-3.58868,-3.88273,-3.86823]}, + {"t":1.79721, "x":2.10492, "y":5.59191, "heading":3.14072, "vx":-1.3629, "vy":0.48619, "omega":-0.00259, "ax":0.94033, "ay":-0.33545, "alpha":0.02128, "fx":[10.84705,10.84696,10.86494,10.09368], "fy":[-3.61252,-3.87372,-3.87372,-3.856]}, + {"t":1.84713, "x":2.03806, "y":5.61576, "heading":3.14059, "vx":-1.31596, "vy":0.46945, "omega":-0.00153, "ax":0.94064, "ay":-0.33556, "alpha":0.00792, "fx":[10.65385,10.65397,10.67938,10.67939], "fy":[-3.86052,-3.61979,-3.87989,-3.86052]}, + {"t":1.89705, "x":1.97353, "y":5.63878, "heading":3.14052, "vx":-1.269, "vy":0.45269, "omega":-0.00114, "ax":0.94084, "ay":-0.33563, "alpha":0.01195, "fx":[10.79601,10.79592,10.81724,10.26668], "fy":[-3.63839,-3.86893,-3.86892,-3.84781]}, + {"t":1.94697, "x":1.91135, "y":5.66096, "heading":3.14046, "vx":-1.22203, "vy":0.43594, "omega":-0.00054, "ax":0.94099, "ay":-0.33568, "alpha":0.00339, "fx":[10.65597,10.65605,10.68523,10.68524], "fy":[-3.83945,-3.68585,-3.86163,-3.83944]}, + {"t":1.99689, "x":1.85152, "y":5.68231, "heading":3.14043, "vx":-1.17505, "vy":0.41918, "omega":-0.00037, "ax":0.9411, "ay":-0.33572, "alpha":0.00843, "fx":[10.77351,10.77342,10.79707,10.34346], "fy":[-3.66118,-3.86347,-3.86346,-3.84001]}, + {"t":2.04682, "x":1.79403, "y":5.70282, "heading":3.14042, "vx":-1.12807, "vy":0.40242, "omega":0.00005, "ax":0.94118, "ay":-0.33575, "alpha":0.00255, "fx":[10.65684,10.65693,10.68877,10.68878], "fy":[-3.83703,-3.69416,-3.86125,-3.83702]}, + {"t":2.09674, "x":1.73889, "y":5.72249, "heading":3.14042, "vx":-1.08109, "vy":0.38566, "omega":0.00018, "ax":0.94125, "ay":-0.33578, "alpha":0.0072, "fx":[10.76092,10.76084,10.78613,10.38654], "fy":[-3.68667,-3.85634,-3.85632,-3.83121]}, + {"t":2.14666, "x":1.68609, "y":5.74132, "heading":3.14043, "vx":-1.0341, "vy":0.3689, "omega":0.00054, "ax":0.94131, "ay":-0.3358, "alpha":0.00117, "fx":[10.65741,10.65749,10.69102,10.69104], "fy":[-3.83098,-3.71299,-3.85651,-3.83096]}, + {"t":2.19658, "x":1.63564, "y":5.75932, "heading":3.14045, "vx":-0.9871, "vy":0.35213, "omega":0.0006, "ax":0.94135, "ay":-0.33581, "alpha":0.00578, "fx":[10.75242,10.75234,10.77866,10.41564], "fy":[-3.69427,-3.85469,-3.85467,-3.82854]}, + {"t":2.24651, "x":1.58753, "y":5.77648, "heading":3.14048, "vx":-0.94011, "vy":0.33537, "omega":0.00088, "ax":0.94139, "ay":-0.33583, "alpha":0.00041, "fx":[10.65786,10.65794,10.69252,10.69253], "fy":[-3.82776,-3.72319,-3.8541,-3.82774]}, + {"t":2.29643, "x":1.54177, "y":5.7928, "heading":3.14053, "vx":-0.89311, "vy":0.3186, "omega":0.0009, "ax":0.94143, "ay":-0.33584, "alpha":0.0051, "fx":[10.74513,10.74506,10.77197,10.44022], "fy":[-3.7086,-3.8505,-3.85049,-3.82375]}, + {"t":2.34635, "x":1.49836, "y":5.80829, "heading":3.14057, "vx":-0.84612, "vy":0.30184, "omega":0.00116, "ax":0.94146, "ay":-0.33585, "alpha":-0.00087, "fx":[10.65833,10.65839,10.69348,10.6935], "fy":[-3.82136,-3.74303,-3.84806,-3.82134]}, + {"t":2.39627, "x":1.45729, "y":5.82294, "heading":3.14063, "vx":-0.79912, "vy":0.28507, "omega":0.00112, "ax":0.94148, "ay":-0.33586, "alpha":0.00368, "fx":[10.73356,10.73349,10.76061,10.47721], "fy":[-3.72279,-3.84614,-3.84612,-3.81915]}, + {"t":2.4462, "x":1.41857, "y":5.83675, "heading":3.14069, "vx":-0.75211, "vy":0.2683, "omega":0.0013, "ax":0.9415, "ay":-0.33586, "alpha":-0.0013, "fx":[10.65875,10.65881,10.69416,10.69417], "fy":[-3.8194,-3.74953,-3.84626,-3.81938]}, + {"t":2.49612, "x":1.3822, "y":5.84973, "heading":3.14075, "vx":-0.70511, "vy":0.25154, "omega":0.00124, "ax":0.94152, "ay":-0.33587, "alpha":0.00251, "fx":[10.72521,10.72515,10.75231,10.50414], "fy":[-3.73064,-3.84377,-3.84375,-3.81672]}, + {"t":2.54604, "x":1.34817, "y":5.86187, "heading":3.14081, "vx":-0.65811, "vy":0.23477, "omega":0.00136, "ax":0.94154, "ay":-0.33588, "alpha":-0.00187, "fx":[10.65914,10.65918,10.69464,10.69466], "fy":[-3.81652,-3.75871,-3.84343,-3.8165]}, + {"t":2.59596, "x":1.31649, "y":5.87317, "heading":3.14088, "vx":-0.61111, "vy":0.218, "omega":0.00127, "ax":0.94156, "ay":-0.33588, "alpha":0.0021, "fx":[10.71996,10.71991,10.7471,10.52138], "fy":[-3.74116,-3.84046,-3.84044,-3.81337]}, + {"t":2.64589, "x":1.28715, "y":5.88363, "heading":3.14094, "vx":-0.5641, "vy":0.20123, "omega":0.00137, "ax":0.94157, "ay":-0.33589, "alpha":-0.00222, "fx":[10.65943,10.65947,10.69504,10.69506], "fy":[-3.81484,-3.76423,-3.84178,-3.81482]}, + {"t":2.69581, "x":1.26017, "y":5.89326, "heading":3.14101, "vx":-0.51709, "vy":0.18446, "omega":0.00126, "ax":0.94159, "ay":-0.33589, "alpha":0.00159, "fx":[10.71546,10.7154,10.74264,10.53612], "fy":[-3.74755,-3.8385,-3.83848,-3.81135]}, + {"t":2.74573, "x":1.23553, "y":5.90205, "heading":3.14108, "vx":-0.47009, "vy":0.1677, "omega":0.00134, "ax":0.9416, "ay":-0.3359, "alpha":-0.00243, "fx":[10.65964,10.65968,10.69542,10.69544], "fy":[-3.81387,-3.76746,-3.84089,-3.81384]}, + {"t":2.79565, "x":1.21323, "y":5.91001, "heading":3.14114, "vx":-0.42308, "vy":0.15093, "omega":0.00122, "ax":0.94161, "ay":-0.3359, "alpha":0.00103, "fx":[10.71136,10.71131,10.73866,10.54934], "fy":[-3.75191,-3.8372,-3.83718,-3.80994]}, + {"t":2.84558, "x":1.19328, "y":5.91712, "heading":3.1412, "vx":-0.37607, "vy":0.13416, "omega":0.00127, "ax":0.94162, "ay":-0.33591, "alpha":-0.00286, "fx":[10.65974,10.65977,10.6958,10.69582], "fy":[-3.81181,-3.77379,-3.83901,-3.81179]}, + {"t":2.8955, "x":1.17568, "y":5.9234, "heading":3.14127, "vx":-0.32907, "vy":0.11739, "omega":0.00113, "ax":0.94163, "ay":-0.33591, "alpha":0.00047, "fx":[10.70755,10.7075,10.73509,10.56142], "fy":[-3.7552,-3.83629,-3.83627,-3.80879]}, + {"t":2.94542, "x":1.16043, "y":5.92884, "heading":3.14132, "vx":-0.28206, "vy":0.10062, "omega":0.00115, "ax":0.94164, "ay":-0.33591, "alpha":-0.00316, "fx":[10.6597,10.65973,10.69626,10.69628], "fy":[-3.81051,-3.77765,-3.83805,-3.81049]}, + {"t":2.99534, "x":1.14752, "y":5.93345, "heading":3.14138, "vx":-0.23505, "vy":0.08385, "omega":0.00099, "ax":0.94165, "ay":-0.33591, "alpha":0.0002, "fx":[10.70475,10.70471,10.73273,10.57014], "fy":[-3.76044,-3.83478,-3.83477,-3.80683]}, + {"t":3.04526, "x":1.13696, "y":5.93721, "heading":3.14143, "vx":-0.18804, "vy":0.06708, "omega":0.001, "ax":0.94165, "ay":-0.33592, "alpha":-0.00346, "fx":[10.6595,10.65952,10.69682,10.69684], "fy":[-3.80933,-3.78089,-3.83741,-3.80931]}, + {"t":3.09519, "x":1.12874, "y":5.94014, "heading":3.14148, "vx":-0.14103, "vy":0.05031, "omega":0.00083, "ax":0.94166, "ay":-0.33592, "alpha":-0.0004, "fx":[10.70144,10.70139,10.73006,10.58011], "fy":[-3.76197,-3.83457,-3.83455,-3.80596]}, + {"t":3.14511, "x":1.12288, "y":5.94224, "heading":3.14152, "vx":-0.09402, "vy":0.03354, "omega":0.00081, "ax":0.94167, "ay":-0.33592, "alpha":-0.00389, "fx":[10.65911,10.65912,10.69752,10.69755], "fy":[-3.80765,-3.78535,-3.83653,-3.80763]}, + {"t":3.19503, "x":1.11936, "y":5.94349, "heading":3.14156, "vx":-0.04701, "vy":0.01677, "omega":0.00062, "ax":0.94167, "ay":-0.33592, "alpha":-0.01236, "fx":[10.69642,10.56633,10.7254,10.72542], "fy":[-3.78997,-3.82874,-3.82861,-3.78994]}, + {"t":3.24495, "x":1.11818, "y":5.94391, "heading":3.14159, "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 16c98a5e40ea51a5db4f39e49255523690d9204a Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 2 Apr 2026 18:31:35 -0400 Subject: [PATCH 11/20] Changed drive Command P and made chero great again --- src/main/java/frc/robot/Drive.java | 8 ++++---- .../java/frc/robot/commands/auto/newauto/FastDepot.java | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java index 2845a541..f2d6523f 100644 --- a/src/main/java/frc/robot/Drive.java +++ b/src/main/java/frc/robot/Drive.java @@ -9,9 +9,9 @@ public class Drive extends SubsystemBase{ - private final PIDController xController = new PIDController(0.08, 0.0, 0.0); - private final PIDController yController = new PIDController(0.08, 0.0, 0.0); - private final PIDController headingController = new PIDController(0.8, 0.0, 0.0); + private final PIDController xController = new PIDController(10, 0.0, 0.0); + private final PIDController yController = new PIDController(10, 0.0, 0.0); + private final PIDController headingController = new PIDController(5, 0.0, 0.0); private SwerveSubsystem subsystem; public Drive(SwerveSubsystem subsystem) { @@ -29,7 +29,7 @@ public void followTrajectory(SwerveSample sample) { ); ChassisSpeeds robotRelativeSpeeds = - ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()); + ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()); subsystem.setChassisSpeeds(robotRelativeSpeeds); } diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 7bed88a6..0dd6915a 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -19,12 +19,12 @@ public FastDepot( HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, ControllerSubsystem controller, IntakeDeployerSubsystem intake) { super( + new AutoReset(shootstate, turret, angler), new LoggableParallelCommandGroup( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")), - new AutoReset(shootstate, turret, angler), - new SetShootingState(shootstate, ShootState.AUTO_AIM) + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), + new SetShootingState(shootstate, ShootState.AUTO_AIM), new LoggableWaitCommand(3), new AutoShoot(hopper, feeder, 0) ); From 341f1bfc0efe324ffa4de54a97db4a6b33705775 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 18:56:49 -0400 Subject: [PATCH 12/20] changed path, changed sequence --- src/main/deploy/choreo/Depot_Fast.traj | 133 ++++++++---------- .../commands/auto/newauto/FastDepot.java | 2 +- 2 files changed, 63 insertions(+), 72 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index b516c35d..b56ae4d5 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.118183970451355, "y":5.943911552429199, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8895899057388303, "y":5.941661834716797, "heading":3.141592653589793, "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}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":65, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.118183970451355 m", "val":1.118183970451355}, "y":{"exp":"5.943911552429199 m", "val":5.943911552429199}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8895899057388306 m", "val":1.8895899057388303}, "y":{"exp":"5.941661834716797 m", "val":5.941661834716797}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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}, @@ -53,74 +53,65 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,3.24495], + "waypoints":[0.0,2.77237], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.94148, "ay":0.33585, "alpha":0.00289, "fx":[-10.65382,-10.65386,-10.69861,-10.69863], "fy":[3.81539,3.75401,3.8493,3.81537]}, - {"t":0.04992, "x":3.59483, "y":5.06042, "heading":3.14159, "vx":-0.047, "vy":0.01677, "omega":0.00014, "ax":-0.94167, "ay":0.33592, "alpha":0.00471, "fx":[-10.65657,-10.65658,-10.70008,-10.70011], "fy":[3.80577,3.78681,3.83873,3.80574]}, - {"t":0.09984, "x":3.59131, "y":5.06167, "heading":-3.14159, "vx":-0.09401, "vy":0.03354, "omega":0.00038, "ax":-0.94166, "ay":0.33592, "alpha":0.00112, "fx":[-10.69924,-10.69919,-10.73167,-10.58293], "fy":[3.76076,3.83621,3.83619,3.80379]}, - {"t":0.14977, "x":3.58544, "y":5.06377, "heading":-3.14157, "vx":-0.14102, "vy":0.05031, "omega":0.00044, "ax":-0.94165, "ay":0.33591, "alpha":0.00419, "fx":[-10.65731,-10.65732,-10.69903,-10.69905], "fy":[3.80757,3.78253,3.8392,3.80754]}, - {"t":0.19969, "x":3.57723, "y":5.0667, "heading":-3.14154, "vx":-0.18803, "vy":0.06708, "omega":0.00064, "ax":-0.94165, "ay":0.33591, "alpha":0.00049, "fx":[-10.70288,-10.70283,-10.73415,-10.5725], "fy":[3.75829,3.83656,3.83655,3.80532]}, - {"t":0.24961, "x":3.56667, "y":5.07046, "heading":-3.14151, "vx":-0.23504, "vy":0.08385, "omega":0.00067, "ax":-0.94164, "ay":0.33591, "alpha":0.00377, "fx":[-10.65782,-10.65784,-10.69816,-10.69818], "fy":[3.80909,3.77872,3.8397,3.80907]}, - {"t":0.29953, "x":3.55376, "y":5.07507, "heading":-3.14148, "vx":-0.28205, "vy":0.10061, "omega":0.00086, "ax":-0.94163, "ay":0.33591, "alpha":-0.00012, "fx":[-10.7064,-10.70635,-10.7368,-10.56206], "fy":[3.756,3.83694,3.83692,3.80657]}, - {"t":0.34946, "x":3.53851, "y":5.08051, "heading":-3.14144, "vx":-0.32906, "vy":0.11738, "omega":0.00085, "ax":-0.94162, "ay":0.3359, "alpha":0.00338, "fx":[-10.65814,-10.65817,-10.69742,-10.69745], "fy":[3.81058,3.77474,3.84042,3.81055]}, - {"t":0.39938, "x":3.5209, "y":5.08679, "heading":-3.14139, "vx":-0.37607, "vy":0.13415, "omega":0.00102, "ax":-0.94161, "ay":0.3359, "alpha":-0.00043, "fx":[-10.70983,-10.70978,-10.73956,-10.55154], "fy":[3.74933,3.83884,3.83882,3.80914]}, - {"t":0.4493, "x":3.50096, "y":5.0939, "heading":-3.14134, "vx":-0.42307, "vy":0.15092, "omega":0.001, "ax":-0.9416, "ay":0.33589, "alpha":0.00289, "fx":[-10.65829,-10.65833,-10.69679,-10.69681], "fy":[3.81273,3.76848,3.84201,3.81271]}, - {"t":0.49922, "x":3.47866, "y":5.10186, "heading":-3.14129, "vx":-0.47008, "vy":0.16769, "omega":0.00114, "ax":-0.94159, "ay":0.33589, "alpha":-0.00145, "fx":[-10.71517,-10.71512,-10.74445,-10.53494], "fy":[3.74829,3.8389,3.83888,3.80966]}, - {"t":0.54915, "x":3.45402, "y":5.11065, "heading":-3.14124, "vx":-0.51709, "vy":0.18446, "omega":0.00107, "ax":-0.94157, "ay":0.33589, "alpha":0.00255, "fx":[-10.65832,-10.65836,-10.69618,-10.6962], "fy":[3.81414,3.76426,3.84299,3.81412]}, - {"t":0.59907, "x":3.42703, "y":5.12027, "heading":-3.14118, "vx":-0.56409, "vy":0.20123, "omega":0.0012, "ax":-0.94156, "ay":0.33588, "alpha":-0.00197, "fx":[-10.71951,-10.71945,-10.74839,-10.52106], "fy":[3.7423,3.84061,3.84059,3.81178]}, - {"t":0.64899, "x":3.3977, "y":5.13074, "heading":-3.14112, "vx":-0.6111, "vy":0.218, "omega":0.0011, "ax":-0.94154, "ay":0.33587, "alpha":0.00215, "fx":[-10.65825,-10.65829,-10.69556,-10.69558], "fy":[3.81587,3.75897,3.84433,3.81585]}, - {"t":0.69891, "x":3.36602, "y":5.14204, "heading":-3.14107, "vx":-0.6581, "vy":0.23476, "omega":0.00121, "ax":-0.94153, "ay":0.33587, "alpha":-0.00299, "fx":[-10.72551,-10.72545,-10.75403,-10.50188], "fy":[3.73907,3.84138,3.84136,3.81291]}, - {"t":0.74884, "x":3.33199, "y":5.15418, "heading":-3.14101, "vx":-0.7051, "vy":0.25153, "omega":0.00106, "ax":-0.94151, "ay":0.33586, "alpha":0.00153, "fx":[-10.6581,-10.65815,-10.69484,-10.69486], "fy":[3.81875,3.75008,3.84682,3.81873]}, - {"t":0.79876, "x":3.29562, "y":5.16715, "heading":-3.14096, "vx":-0.75211, "vy":0.2683, "omega":0.00113, "ax":-0.94148, "ay":0.33585, "alpha":-0.00376, "fx":[-10.73366,-10.73359,-10.76174,-10.47595], "fy":[3.72493,3.84571,3.84569,3.81769]}, - {"t":0.84868, "x":3.2569, "y":5.18097, "heading":-3.1409, "vx":-0.79911, "vy":0.28506, "omega":0.00095, "ax":-0.94146, "ay":0.33584, "alpha":0.00102, "fx":[-10.65793,-10.65798,-10.69393,-10.69394], "fy":[3.82088,3.74343,3.84842,3.82086]}, - {"t":0.8986, "x":3.21583, "y":5.19562, "heading":-3.14085, "vx":-0.84611, "vy":0.30183, "omega":0.001, "ax":-0.94143, "ay":0.33583, "alpha":-0.00527, "fx":[-10.74567,-10.74559,-10.77317,-10.43803], "fy":[3.7106,3.84998,3.84997,3.82257]}, - {"t":0.94853, "x":3.17242, "y":5.2111, "heading":-3.1408, "vx":-0.8931, "vy":0.3186, "omega":0.00073, "ax":-0.94139, "ay":0.33582, "alpha":-0.0004, "fx":[-10.65771,-10.65778,-10.69272,-10.69273], "fy":[3.82775,3.72253,3.85455,3.82773]}, - {"t":0.99845, "x":3.12666, "y":5.22743, "heading":-3.14077, "vx":-0.9401, "vy":0.33536, "omega":0.00071, "ax":-0.94136, "ay":0.33581, "alpha":-0.00617, "fx":[-10.75423,-10.75415,-10.78075,-10.41003], "fy":[3.69579,3.85419,3.85418,3.82776]}, - {"t":1.04837, "x":3.07856, "y":5.24459, "heading":-3.14073, "vx":-0.9871, "vy":0.35212, "omega":0.00041, "ax":-0.94131, "ay":0.33579, "alpha":-0.00126, "fx":[-10.65752,-10.6576,-10.69095,-10.69097], "fy":[3.83122,3.71193,3.85681,3.83121]}, - {"t":1.09829, "x":3.0281, "y":5.26258, "heading":-3.14071, "vx":-1.03409, "vy":0.36889, "omega":0.00034, "ax":-0.94125, "ay":0.33577, "alpha":-0.00777, "fx":[-10.76344,-10.76336,-10.78851,-10.37921], "fy":[3.68855,3.85556,3.85554,3.83059]}, - {"t":1.14821, "x":2.97531, "y":5.28142, "heading":-3.14069, "vx":-1.08108, "vy":0.38565, "omega":-0.00005, "ax":-0.94119, "ay":0.33574, "alpha":-0.00273, "fx":[-10.65727,-10.65735,-10.68841,-10.68842], "fy":[3.83744,3.693,3.86125,3.83743]}, - {"t":1.19814, "x":2.92016, "y":5.30109, "heading":-3.14069, "vx":-1.12806, "vy":0.40241, "omega":-0.00018, "ax":-0.9411, "ay":0.33571, "alpha":-0.00918, "fx":[-10.77651,-10.77642,-10.79943,-10.33525], "fy":[3.66414,3.86214,3.86213,3.83931]}, - {"t":1.24806, "x":2.86268, "y":5.3216, "heading":-3.1407, "vx":-1.17505, "vy":0.41917, "omega":-0.00064, "ax":-0.94099, "ay":0.33567, "alpha":-0.00376, "fx":[-10.65674,-10.65683,-10.68453,-10.68454], "fy":[3.84052,3.6831,3.86178,3.84051]}, - {"t":1.29798, "x":2.80284, "y":5.34294, "heading":-3.14074, "vx":-1.22202, "vy":0.43593, "omega":-0.00083, "ax":-0.94085, "ay":0.33562, "alpha":-0.01304, "fx":[-10.80121,-10.80112,-10.82126,-10.25245], "fy":[3.63956,3.86796,3.86796,3.84804]}, - {"t":1.3479, "x":2.74066, "y":5.36512, "heading":-3.14078, "vx":-1.26899, "vy":0.45268, "omega":-0.00148, "ax":-0.94064, "ay":0.33555, "alpha":-0.00861, "fx":[-10.65502,-10.65514,-10.67831,-10.67831], "fy":[3.86283,3.61389,3.88067,3.86282]}, - {"t":1.39783, "x":2.67614, "y":5.38814, "heading":-3.14085, "vx":-1.31595, "vy":0.46944, "omega":-0.00191, "ax":-0.94034, "ay":0.33544, "alpha":-0.02271, "fx":[-10.85736,-10.85727,-10.87342,-10.0649], "fy":[3.60348,3.87583,3.87583,3.85995]}, - {"t":1.44775, "x":2.60927, "y":5.41199, "heading":-3.14095, "vx":-1.3629, "vy":0.48618, "omega":-0.00304, "ax":-0.93983, "ay":0.33525, "alpha":-0.01177, "fx":[-10.64943,-10.64954,-10.66544,-10.66545], "fy":[3.8728,3.57594,3.88512,3.8728]}, - {"t":1.49767, "x":2.54006, "y":5.43668, "heading":-3.1411, "vx":-1.40981, "vy":0.50292, "omega":-0.00363, "ax":-0.93881, "ay":0.33488, "alpha":-0.01451, "fx":[-10.77798,-10.77793,-10.78654,-10.24122], "fy":[3.64365,3.85161,3.85161,3.84313]}, - {"t":1.54759, "x":2.46851, "y":5.46221, "heading":-3.14128, "vx":-1.45668, "vy":0.51964, "omega":-0.00435, "ax":-0.93571, "ay":0.33391, "alpha":-0.03063, "fx":[-10.60724,-10.60733,-10.61434,-10.61434], "fy":[3.95445,3.27297,3.96417,3.95445]}, - {"t":1.59752, "x":2.39463, "y":5.48856, "heading":-3.1415, "vx":-1.50339, "vy":0.53631, "omega":-0.00588, "ax":-0.00009, "ay":0.00035, "alpha":-0.0004, "fx":[-0.00839,-0.00839,-0.00716,0.01978], "fy":[-0.01289,0.00915,0.00915,0.01038]}, - {"t":1.64744, "x":2.31957, "y":5.51534, "heading":3.1414, "vx":-1.5034, "vy":0.53632, "omega":-0.0059, "ax":0.93569, "ay":-0.33398, "alpha":0.02917, "fx":[10.60516,10.60525,10.61585,10.61585], "fy":[-3.94929,-3.28912,-3.96153,-3.94929]}, - {"t":1.69736, "x":2.24568, "y":5.5417, "heading":3.1411, "vx":-1.45669, "vy":0.51965, "omega":-0.00445, "ax":0.9388, "ay":-0.33492, "alpha":0.02657, "fx":[10.86874,10.86869,10.88061,9.9651], "fy":[-3.56846,-3.87826,-3.87826,-3.86652]}, - {"t":1.74728, "x":2.17413, "y":5.56722, "heading":3.14088, "vx":-1.40982, "vy":0.50293, "omega":-0.00312, "ax":0.93982, "ay":-0.33528, "alpha":0.01055, "fx":[10.64774,10.64785,10.66692,10.66693], "fy":[-3.86824,-3.58868,-3.88273,-3.86823]}, - {"t":1.79721, "x":2.10492, "y":5.59191, "heading":3.14072, "vx":-1.3629, "vy":0.48619, "omega":-0.00259, "ax":0.94033, "ay":-0.33545, "alpha":0.02128, "fx":[10.84705,10.84696,10.86494,10.09368], "fy":[-3.61252,-3.87372,-3.87372,-3.856]}, - {"t":1.84713, "x":2.03806, "y":5.61576, "heading":3.14059, "vx":-1.31596, "vy":0.46945, "omega":-0.00153, "ax":0.94064, "ay":-0.33556, "alpha":0.00792, "fx":[10.65385,10.65397,10.67938,10.67939], "fy":[-3.86052,-3.61979,-3.87989,-3.86052]}, - {"t":1.89705, "x":1.97353, "y":5.63878, "heading":3.14052, "vx":-1.269, "vy":0.45269, "omega":-0.00114, "ax":0.94084, "ay":-0.33563, "alpha":0.01195, "fx":[10.79601,10.79592,10.81724,10.26668], "fy":[-3.63839,-3.86893,-3.86892,-3.84781]}, - {"t":1.94697, "x":1.91135, "y":5.66096, "heading":3.14046, "vx":-1.22203, "vy":0.43594, "omega":-0.00054, "ax":0.94099, "ay":-0.33568, "alpha":0.00339, "fx":[10.65597,10.65605,10.68523,10.68524], "fy":[-3.83945,-3.68585,-3.86163,-3.83944]}, - {"t":1.99689, "x":1.85152, "y":5.68231, "heading":3.14043, "vx":-1.17505, "vy":0.41918, "omega":-0.00037, "ax":0.9411, "ay":-0.33572, "alpha":0.00843, "fx":[10.77351,10.77342,10.79707,10.34346], "fy":[-3.66118,-3.86347,-3.86346,-3.84001]}, - {"t":2.04682, "x":1.79403, "y":5.70282, "heading":3.14042, "vx":-1.12807, "vy":0.40242, "omega":0.00005, "ax":0.94118, "ay":-0.33575, "alpha":0.00255, "fx":[10.65684,10.65693,10.68877,10.68878], "fy":[-3.83703,-3.69416,-3.86125,-3.83702]}, - {"t":2.09674, "x":1.73889, "y":5.72249, "heading":3.14042, "vx":-1.08109, "vy":0.38566, "omega":0.00018, "ax":0.94125, "ay":-0.33578, "alpha":0.0072, "fx":[10.76092,10.76084,10.78613,10.38654], "fy":[-3.68667,-3.85634,-3.85632,-3.83121]}, - {"t":2.14666, "x":1.68609, "y":5.74132, "heading":3.14043, "vx":-1.0341, "vy":0.3689, "omega":0.00054, "ax":0.94131, "ay":-0.3358, "alpha":0.00117, "fx":[10.65741,10.65749,10.69102,10.69104], "fy":[-3.83098,-3.71299,-3.85651,-3.83096]}, - {"t":2.19658, "x":1.63564, "y":5.75932, "heading":3.14045, "vx":-0.9871, "vy":0.35213, "omega":0.0006, "ax":0.94135, "ay":-0.33581, "alpha":0.00578, "fx":[10.75242,10.75234,10.77866,10.41564], "fy":[-3.69427,-3.85469,-3.85467,-3.82854]}, - {"t":2.24651, "x":1.58753, "y":5.77648, "heading":3.14048, "vx":-0.94011, "vy":0.33537, "omega":0.00088, "ax":0.94139, "ay":-0.33583, "alpha":0.00041, "fx":[10.65786,10.65794,10.69252,10.69253], "fy":[-3.82776,-3.72319,-3.8541,-3.82774]}, - {"t":2.29643, "x":1.54177, "y":5.7928, "heading":3.14053, "vx":-0.89311, "vy":0.3186, "omega":0.0009, "ax":0.94143, "ay":-0.33584, "alpha":0.0051, "fx":[10.74513,10.74506,10.77197,10.44022], "fy":[-3.7086,-3.8505,-3.85049,-3.82375]}, - {"t":2.34635, "x":1.49836, "y":5.80829, "heading":3.14057, "vx":-0.84612, "vy":0.30184, "omega":0.00116, "ax":0.94146, "ay":-0.33585, "alpha":-0.00087, "fx":[10.65833,10.65839,10.69348,10.6935], "fy":[-3.82136,-3.74303,-3.84806,-3.82134]}, - {"t":2.39627, "x":1.45729, "y":5.82294, "heading":3.14063, "vx":-0.79912, "vy":0.28507, "omega":0.00112, "ax":0.94148, "ay":-0.33586, "alpha":0.00368, "fx":[10.73356,10.73349,10.76061,10.47721], "fy":[-3.72279,-3.84614,-3.84612,-3.81915]}, - {"t":2.4462, "x":1.41857, "y":5.83675, "heading":3.14069, "vx":-0.75211, "vy":0.2683, "omega":0.0013, "ax":0.9415, "ay":-0.33586, "alpha":-0.0013, "fx":[10.65875,10.65881,10.69416,10.69417], "fy":[-3.8194,-3.74953,-3.84626,-3.81938]}, - {"t":2.49612, "x":1.3822, "y":5.84973, "heading":3.14075, "vx":-0.70511, "vy":0.25154, "omega":0.00124, "ax":0.94152, "ay":-0.33587, "alpha":0.00251, "fx":[10.72521,10.72515,10.75231,10.50414], "fy":[-3.73064,-3.84377,-3.84375,-3.81672]}, - {"t":2.54604, "x":1.34817, "y":5.86187, "heading":3.14081, "vx":-0.65811, "vy":0.23477, "omega":0.00136, "ax":0.94154, "ay":-0.33588, "alpha":-0.00187, "fx":[10.65914,10.65918,10.69464,10.69466], "fy":[-3.81652,-3.75871,-3.84343,-3.8165]}, - {"t":2.59596, "x":1.31649, "y":5.87317, "heading":3.14088, "vx":-0.61111, "vy":0.218, "omega":0.00127, "ax":0.94156, "ay":-0.33588, "alpha":0.0021, "fx":[10.71996,10.71991,10.7471,10.52138], "fy":[-3.74116,-3.84046,-3.84044,-3.81337]}, - {"t":2.64589, "x":1.28715, "y":5.88363, "heading":3.14094, "vx":-0.5641, "vy":0.20123, "omega":0.00137, "ax":0.94157, "ay":-0.33589, "alpha":-0.00222, "fx":[10.65943,10.65947,10.69504,10.69506], "fy":[-3.81484,-3.76423,-3.84178,-3.81482]}, - {"t":2.69581, "x":1.26017, "y":5.89326, "heading":3.14101, "vx":-0.51709, "vy":0.18446, "omega":0.00126, "ax":0.94159, "ay":-0.33589, "alpha":0.00159, "fx":[10.71546,10.7154,10.74264,10.53612], "fy":[-3.74755,-3.8385,-3.83848,-3.81135]}, - {"t":2.74573, "x":1.23553, "y":5.90205, "heading":3.14108, "vx":-0.47009, "vy":0.1677, "omega":0.00134, "ax":0.9416, "ay":-0.3359, "alpha":-0.00243, "fx":[10.65964,10.65968,10.69542,10.69544], "fy":[-3.81387,-3.76746,-3.84089,-3.81384]}, - {"t":2.79565, "x":1.21323, "y":5.91001, "heading":3.14114, "vx":-0.42308, "vy":0.15093, "omega":0.00122, "ax":0.94161, "ay":-0.3359, "alpha":0.00103, "fx":[10.71136,10.71131,10.73866,10.54934], "fy":[-3.75191,-3.8372,-3.83718,-3.80994]}, - {"t":2.84558, "x":1.19328, "y":5.91712, "heading":3.1412, "vx":-0.37607, "vy":0.13416, "omega":0.00127, "ax":0.94162, "ay":-0.33591, "alpha":-0.00286, "fx":[10.65974,10.65977,10.6958,10.69582], "fy":[-3.81181,-3.77379,-3.83901,-3.81179]}, - {"t":2.8955, "x":1.17568, "y":5.9234, "heading":3.14127, "vx":-0.32907, "vy":0.11739, "omega":0.00113, "ax":0.94163, "ay":-0.33591, "alpha":0.00047, "fx":[10.70755,10.7075,10.73509,10.56142], "fy":[-3.7552,-3.83629,-3.83627,-3.80879]}, - {"t":2.94542, "x":1.16043, "y":5.92884, "heading":3.14132, "vx":-0.28206, "vy":0.10062, "omega":0.00115, "ax":0.94164, "ay":-0.33591, "alpha":-0.00316, "fx":[10.6597,10.65973,10.69626,10.69628], "fy":[-3.81051,-3.77765,-3.83805,-3.81049]}, - {"t":2.99534, "x":1.14752, "y":5.93345, "heading":3.14138, "vx":-0.23505, "vy":0.08385, "omega":0.00099, "ax":0.94165, "ay":-0.33591, "alpha":0.0002, "fx":[10.70475,10.70471,10.73273,10.57014], "fy":[-3.76044,-3.83478,-3.83477,-3.80683]}, - {"t":3.04526, "x":1.13696, "y":5.93721, "heading":3.14143, "vx":-0.18804, "vy":0.06708, "omega":0.001, "ax":0.94165, "ay":-0.33592, "alpha":-0.00346, "fx":[10.6595,10.65952,10.69682,10.69684], "fy":[-3.80933,-3.78089,-3.83741,-3.80931]}, - {"t":3.09519, "x":1.12874, "y":5.94014, "heading":3.14148, "vx":-0.14103, "vy":0.05031, "omega":0.00083, "ax":0.94166, "ay":-0.33592, "alpha":-0.0004, "fx":[10.70144,10.70139,10.73006,10.58011], "fy":[-3.76197,-3.83457,-3.83455,-3.80596]}, - {"t":3.14511, "x":1.12288, "y":5.94224, "heading":3.14152, "vx":-0.09402, "vy":0.03354, "omega":0.00081, "ax":0.94167, "ay":-0.33592, "alpha":-0.00389, "fx":[10.65911,10.65912,10.69752,10.69755], "fy":[-3.80765,-3.78535,-3.83653,-3.80763]}, - {"t":3.19503, "x":1.11936, "y":5.94349, "heading":3.14156, "vx":-0.04701, "vy":0.01677, "omega":0.00062, "ax":0.94167, "ay":-0.33592, "alpha":-0.01236, "fx":[10.69642,10.56633,10.7254,10.72542], "fy":[-3.78997,-3.82874,-3.82861,-3.78994]}, - {"t":3.24495, "x":1.11818, "y":5.94391, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.88807, "ay":0.45882, "alpha":0.00564, "fx":[-10.06584,-10.06573,-10.07521,-10.07518], "fy":[5.17738,5.26281,5.19422,5.17741]}, + {"t":0.04951, "x":3.59491, "y":5.06056, "heading":3.14159, "vx":-0.04397, "vy":0.02271, "omega":0.00028, "ax":-0.88824, "ay":0.45891, "alpha":0.00396, "fx":[-10.06797,-10.06793,-10.07704,-10.07701], "fy":[5.18724,5.23792,5.20348,5.18727]}, + {"t":0.09901, "x":3.59165, "y":5.06225, "heading":-3.14158, "vx":-0.08794, "vy":0.04543, "omega":0.00048, "ax":-0.88823, "ay":0.45891, "alpha":0.00274, "fx":[-10.05205,-10.05215,-10.06779,-10.11765], "fy":[5.2365,5.1983,5.19832,5.18259]}, + {"t":0.14852, "x":3.5862, "y":5.06506, "heading":-3.14156, "vx":-0.13191, "vy":0.06815, "omega":0.00061, "ax":-0.88823, "ay":0.4589, "alpha":0.00387, "fx":[-10.06836,-10.06831,-10.07632,-10.0763], "fy":[5.18705,5.23962,5.20173,5.18708]}, + {"t":0.19803, "x":3.57859, "y":5.069, "heading":-3.14153, "vx":-0.17589, "vy":0.09087, "omega":0.0008, "ax":-0.88822, "ay":0.4589, "alpha":0.00266, "fx":[-10.05015,-10.05025,-10.06416,-10.12436], "fy":[5.24099,5.19607,5.19609,5.18207]}, + {"t":0.24753, "x":3.56879, "y":5.07406, "heading":-3.14149, "vx":-0.21986, "vy":0.11359, "omega":0.00093, "ax":-0.88821, "ay":0.45889, "alpha":0.00405, "fx":[-10.06893,-10.06886,-10.07537,-10.07535], "fy":[5.18521,5.24651,5.198,5.18524]}, + {"t":0.29704, "x":3.55682, "y":5.08024, "heading":-3.14144, "vx":-0.26383, "vy":0.13631, "omega":0.00113, "ax":-0.8882, "ay":0.45888, "alpha":0.00313, "fx":[-10.04657,-10.04667,-10.05856,-10.13627], "fy":[5.24141,5.19508,5.1951,5.18307]}, + {"t":0.34655, "x":3.54267, "y":5.08755, "heading":-3.14138, "vx":-0.3078, "vy":0.15903, "omega":0.00129, "ax":-0.88819, "ay":0.45888, "alpha":0.00385, "fx":[-10.06956,-10.06948,-10.07428,-10.07426], "fy":[5.18525,5.2478,5.196,5.18529]}, + {"t":0.39605, "x":3.52634, "y":5.09599, "heading":-3.14132, "vx":-0.35177, "vy":0.18174, "omega":0.00148, "ax":-0.88818, "ay":0.45887, "alpha":0.00227, "fx":[-10.04545,-10.04557,-10.05541,-10.1406], "fy":[5.25568,5.18941,5.18944,5.17945]}, + {"t":0.44556, "x":3.50784, "y":5.10555, "heading":-3.14125, "vx":-0.39574, "vy":0.20446, "omega":0.00159, "ax":-0.88816, "ay":0.45886, "alpha":0.00433, "fx":[-10.07025,-10.07014,-10.07304,-10.07302], "fy":[5.18158,5.26024,5.19015,5.18162]}, + {"t":0.49507, "x":3.48716, "y":5.11623, "heading":-3.14117, "vx":-0.43971, "vy":0.22718, "omega":0.00181, "ax":-0.88815, "ay":0.45885, "alpha":0.00271, "fx":[-10.04036,-10.04048,-10.04813,-10.15681], "fy":[5.26052,5.1868,5.18683,5.179]}, + {"t":0.54457, "x":3.4643, "y":5.12804, "heading":-3.14108, "vx":-0.48368, "vy":0.24989, "omega":0.00194, "ax":-0.88813, "ay":0.45884, "alpha":0.00382, "fx":[-10.07084,-10.07073,-10.07174,-10.07173], "fy":[5.18309,5.25688,5.18955,5.18314]}, + {"t":0.59408, "x":3.43927, "y":5.14098, "heading":-3.14098, "vx":-0.52765, "vy":0.27261, "omega":0.00213, "ax":-0.88811, "ay":0.45883, "alpha":0.00348, "fx":[-10.034,-10.03412,-10.03962,-10.17646], "fy":[5.26361,5.18471,5.18475,5.17901]}, + {"t":0.64359, "x":3.41205, "y":5.15504, "heading":-3.14088, "vx":-0.57162, "vy":0.29532, "omega":0.0023, "ax":-0.88809, "ay":0.45881, "alpha":0.00447, "fx":[-10.0713,-10.07116,-10.07039,-10.07038], "fy":[5.17845,5.27162,5.18289,5.1785]}, + {"t":0.69309, "x":3.38267, "y":5.17022, "heading":-3.14076, "vx":-0.61559, "vy":0.31804, "omega":0.00252, "ax":-0.88807, "ay":0.4588, "alpha":0.00289, "fx":[-10.02773,-10.02789,-10.03164,-10.19489], "fy":[5.28821,5.17548,5.17552,5.1715]}, + {"t":0.7426, "x":3.3511, "y":5.18653, "heading":-3.14064, "vx":-0.65955, "vy":0.34075, "omega":0.00267, "ax":-0.88804, "ay":0.45878, "alpha":0.00539, "fx":[-10.07145,-10.07126,-10.06908,-10.06907], "fy":[5.17242,5.28979,5.17521,5.17247]}, + {"t":0.79211, "x":3.31736, "y":5.20396, "heading":-3.1405, "vx":-0.70352, "vy":0.36346, "omega":0.00293, "ax":-0.88801, "ay":0.45876, "alpha":0.00353, "fx":[-10.01548,-10.01568,-10.01805,-10.23018], "fy":[5.3108,5.1669,5.16695,5.16422]}, + {"t":0.84161, "x":3.28145, "y":5.22251, "heading":-3.14036, "vx":-0.74748, "vy":0.38618, "omega":0.00311, "ax":-0.88797, "ay":0.45873, "alpha":0.00666, "fx":[-10.07105,-10.07081,-10.06787,-10.06787], "fy":[5.16465,5.31196,5.16637,5.16471]}, + {"t":0.89112, "x":3.24335, "y":5.24219, "heading":-3.14021, "vx":-0.79144, "vy":0.40889, "omega":0.00344, "ax":-0.88792, "ay":0.4587, "alpha":0.0026, "fx":[-10.01155,-10.01177,-10.01367,-10.23852], "fy":[5.33358,5.15825,5.1583,5.15603]}, + {"t":0.94063, "x":3.20308, "y":5.263, "heading":-3.14004, "vx":-0.8354, "vy":0.43159, "omega":0.00357, "ax":-0.88786, "ay":0.45866, "alpha":0.00822, "fx":[-10.06978,-10.06949,-10.0668,-10.06679], "fy":[5.15557,5.33601,5.15717,5.15562]}, + {"t":0.99013, "x":3.16064, "y":5.28493, "heading":-3.13986, "vx":-0.87935, "vy":0.4543, "omega":0.00397, "ax":-0.88779, "ay":0.45861, "alpha":0.00821, "fx":[-9.97406,-9.9743,-9.97644,-10.34468], "fy":[5.35038,5.15146,5.15151,5.14881]}, + {"t":1.03964, "x":3.11602, "y":5.30798, "heading":-3.13966, "vx":-0.9233, "vy":0.47701, "omega":0.00438, "ax":-0.88769, "ay":0.45854, "alpha":0.01028, "fx":[-10.06685,-10.0665,-10.06591,-10.06591], "fy":[5.14429,5.363,5.14743,5.14435]}, + {"t":1.08915, "x":3.06922, "y":5.33216, "heading":-3.13945, "vx":-0.96725, "vy":0.49971, "omega":0.00489, "ax":-0.88757, "ay":0.45845, "alpha":0.00401, "fx":[-9.97013,-9.97047,-9.97583,-10.34285], "fy":[5.42343,5.12574,5.12579,5.1199]}, + {"t":1.13865, "x":3.02025, "y":5.35746, "heading":-3.1392, "vx":-1.01119, "vy":0.5224, "omega":0.00509, "ax":-0.88738, "ay":0.45832, "alpha":0.00881, "fx":[-10.06074,-10.06045,-10.06474,-10.06476], "fy":[5.15211,5.32521,5.15946,5.15216]}, + {"t":1.18816, "x":2.9691, "y":5.38388, "heading":-3.13895, "vx":-1.05512, "vy":0.54509, "omega":0.00552, "ax":-0.88708, "ay":0.45812, "alpha":-0.00565, "fx":[-10.0271,-10.02734,-10.03883,-10.14384], "fy":[5.39422,5.13246,5.13251,5.12069]}, + {"t":1.23767, "x":2.91578, "y":5.41143, "heading":-3.13868, "vx":-1.09904, "vy":0.56777, "omega":0.00524, "ax":-0.88655, "ay":0.45774, "alpha":-0.00585, "fx":[-10.04698,-10.04689,-10.05957,-10.0596], "fy":[5.2283,5.06265,5.2435,5.22836]}, + {"t":1.28717, "x":2.86028, "y":5.4401, "heading":-3.13842, "vx":-1.14293, "vy":0.59043, "omega":0.00495, "ax":-0.88532, "ay":0.45686, "alpha":-0.03564, "fx":[-10.33128,-10.33114,-10.35297,-9.14189], "fy":[4.90267,5.28048,5.28054,5.25895]}, + {"t":1.33668, "x":2.80261, "y":5.46989, "heading":-3.13817, "vx":-1.18676, "vy":0.61305, "omega":0.00319, "ax":-0.87872, "ay":0.45345, "alpha":-0.17234, "fx":[-9.94637,-9.947,-9.98242,-9.98249], "fy":[6.08182,2.2838,6.12063,6.08191]}, + {"t":1.38619, "x":2.74278, "y":5.50079, "heading":-3.13801, "vx":-1.23026, "vy":0.6355, "omega":-0.00534, "ax":0.88074, "ay":-0.44918, "alpha":0.03098, "fx":[10.80828,10.80781,10.78742,7.54622], "fy":[-3.11123,-5.74739,-5.7473,-5.76848]}, + {"t":1.43569, "x":2.68296, "y":5.5317, "heading":-3.13828, "vx":-1.18666, "vy":0.61326, "omega":-0.00381, "ax":0.88469, "ay":-0.45802, "alpha":0.02592, "fx":[10.04487,10.04501,10.01959,10.01955], "fy":[-5.32001,-4.8397,-5.29589,-5.31996]}, + {"t":1.4852, "x":2.62529, "y":5.5615, "heading":-3.13847, "vx":-1.14286, "vy":0.59059, "omega":-0.00252, "ax":0.88627, "ay":-0.45827, "alpha":-0.00553, "fx":[10.08031,10.08039,10.051,9.9887], "fy":[-4.98702,-5.25684,-5.25678,-5.28602]}, + {"t":1.5347, "x":2.5698, "y":5.59018, "heading":-3.13859, "vx":-1.09898, "vy":0.5679, "omega":-0.0028, "ax":0.8869, "ay":-0.45846, "alpha":0.00158, "fx":[10.07428,10.07406,10.04034,10.0403], "fy":[-5.19048,-5.25583,-5.15852,-5.19045]}, + {"t":1.58421, "x":2.51648, "y":5.61773, "heading":-3.13873, "vx":-1.05507, "vy":0.5452, "omega":-0.00272, "ax":0.88723, "ay":-0.45859, "alpha":-0.00706, "fx":[9.93879,9.93926,9.90456,10.46165], "fy":[-5.41814,-5.11636,-5.11634,-5.15036]}, + {"t":1.63372, "x":2.46533, "y":5.64416, "heading":-3.13887, "vx":-1.01115, "vy":0.5225, "omega":-0.00307, "ax":0.88745, "ay":-0.45866, "alpha":-0.00232, "fx":[10.08283,10.0825,10.04445,10.04441], "fy":[-5.16969,-5.33142,-5.13379,-5.16968]}, + {"t":1.68322, "x":2.41636, "y":5.66947, "heading":-3.13902, "vx":-0.96722, "vy":0.49979, "omega":-0.00318, "ax":0.8876, "ay":-0.45872, "alpha":-0.00566, "fx":[9.97168,9.97203,9.93496,10.38219], "fy":[-5.35382,-5.13901,-5.139,-5.17548]}, + {"t":1.73273, "x":2.36957, "y":5.69365, "heading":-3.13918, "vx":-0.92327, "vy":0.47708, "omega":-0.00346, "ax":0.88771, "ay":-0.45876, "alpha":0.00103, "fx":[10.08632,10.08608,10.04681,10.04676], "fy":[-5.18812,-5.28146,-5.15117,-5.18812]}, + {"t":1.78224, "x":2.32495, "y":5.71671, "heading":-3.13935, "vx":-0.87933, "vy":0.45437, "omega":-0.00341, "ax":0.8878, "ay":-0.45879, "alpha":-0.0003, "fx":[10.01258,10.01289,9.9757,10.26857], "fy":[-5.32489,-5.14956,-5.14956,-5.18634]}, + {"t":1.83174, "x":2.2825, "y":5.73864, "heading":-3.13952, "vx":-0.83537, "vy":0.43166, "omega":-0.00343, "ax":0.88786, "ay":-0.45881, "alpha":0.00216, "fx":[10.08787,10.08767,10.04864,10.04859], "fy":[-5.19495,-5.26342,-5.15808,-5.19496]}, + {"t":1.88125, "x":2.24223, "y":5.75944, "heading":-3.13969, "vx":-0.79142, "vy":0.40894, "omega":-0.00332, "ax":0.88792, "ay":-0.45883, "alpha":0.00025, "fx":[10.02451,10.02477,9.98798,10.23792], "fy":[-5.30288,-5.15768,-5.15769,-5.19412]}, + {"t":1.93076, "x":2.20414, "y":5.77913, "heading":-3.13985, "vx":-0.74746, "vy":0.38623, "omega":-0.00331, "ax":0.88796, "ay":-0.45885, "alpha":0.00349, "fx":[10.08844,10.08831,10.05024,10.05019], "fy":[-5.20294,-5.24045,-5.16677,-5.20296]}, + {"t":1.98026, "x":2.16823, "y":5.79769, "heading":-3.14001, "vx":-0.7035, "vy":0.36351, "omega":-0.00314, "ax":0.888, "ay":-0.45887, "alpha":0.00046, "fx":[10.03713,10.03734,10.00133,10.20304], "fy":[-5.27243,-5.16854,-5.16855,-5.20427]}, + {"t":2.02977, "x":2.13449, "y":5.81512, "heading":-3.14017, "vx":-0.65954, "vy":0.3408, "omega":-0.00311, "ax":0.88803, "ay":-0.45888, "alpha":0.00319, "fx":[10.08859,10.08846,10.05165,10.0516], "fy":[-5.20229,-5.24271,-5.16699,-5.2023]}, + {"t":2.07928, "x":2.10292, "y":5.83143, "heading":-3.14032, "vx":-0.61558, "vy":0.31808, "omega":-0.00296, "ax":0.88806, "ay":-0.45889, "alpha":0.0023, "fx":[10.04888,10.04907,10.01418,10.1694], "fy":[-5.27023,-5.16995,-5.16996,-5.20463]}, + {"t":2.12878, "x":2.07354, "y":5.84661, "heading":-3.14047, "vx":-0.57161, "vy":0.29536, "omega":-0.00284, "ax":0.88808, "ay":-0.4589, "alpha":0.00409, "fx":[10.08842,10.08833,10.05294,10.0529], "fy":[-5.20801,-5.22538,-5.17376,-5.20803]}, + {"t":2.17829, "x":2.04633, "y":5.86067, "heading":-3.14061, "vx":-0.52765, "vy":0.27264, "omega":-0.00264, "ax":0.8881, "ay":-0.4589, "alpha":0.00242, "fx":[10.05324,10.05341,10.01947,10.15742], "fy":[-5.26175,-5.17334,-5.17336,-5.2071]}, + {"t":2.2278, "x":2.02129, "y":5.87361, "heading":-3.14074, "vx":-0.48368, "vy":0.24992, "omega":-0.00252, "ax":0.88812, "ay":-0.45891, "alpha":0.0041, "fx":[10.08824,10.08816,10.05401,10.05397], "fy":[-5.20886,-5.22267,-5.17545,-5.20888]}, + {"t":2.2773, "x":1.99844, "y":5.88542, "heading":-3.14087, "vx":-0.43971, "vy":0.2272, "omega":-0.00232, "ax":0.88813, "ay":-0.45892, "alpha":0.00233, "fx":[10.05771,10.05784,10.02465,10.14492], "fy":[-5.24898,-5.17803,-5.17805,-5.21108]}, + {"t":2.32681, "x":1.97775, "y":5.89611, "heading":-3.14098, "vx":-0.39574, "vy":0.20449, "omega":-0.0022, "ax":0.88815, "ay":-0.45892, "alpha":0.00414, "fx":[10.08806,10.08799,10.05489,10.05484], "fy":[-5.20974,-5.21991,-5.17699,-5.20977]}, + {"t":2.37632, "x":1.95925, "y":5.90567, "heading":-3.14109, "vx":-0.35177, "vy":0.18177, "omega":-0.002, "ax":0.88816, "ay":-0.45893, "alpha":0.00343, "fx":[10.06351,10.06364,10.03104,10.12819], "fy":[-5.25104,-5.17769,-5.17772,-5.21019]}, + {"t":2.42582, "x":1.94292, "y":5.9141, "heading":-3.14119, "vx":-0.3078, "vy":0.15905, "omega":-0.00183, "ax":0.88817, "ay":-0.45893, "alpha":0.00458, "fx":[10.08797,10.08792,10.05555,10.0555], "fy":[-5.21259,-5.21141,-5.18022,-5.21262]}, + {"t":2.47533, "x":1.92877, "y":5.92141, "heading":-3.14128, "vx":-0.26383, "vy":0.13633, "omega":-0.0016, "ax":0.88819, "ay":-0.45894, "alpha":0.00376, "fx":[10.06729,10.06741,10.03497,10.11776], "fy":[-5.24595,-5.17956,-5.17959,-5.21193]}, + {"t":2.52484, "x":1.9168, "y":5.9276, "heading":-3.14136, "vx":-0.21986, "vy":0.1136, "omega":-0.00141, "ax":0.8882, "ay":-0.45894, "alpha":0.00476, "fx":[10.08808,10.08804,10.0559,10.05586], "fy":[-5.21374,-5.20841,-5.18128,-5.21378]}, + {"t":2.57434, "x":1.90701, "y":5.93266, "heading":-3.14143, "vx":-0.17589, "vy":0.09088, "omega":-0.00118, "ax":0.88821, "ay":-0.45894, "alpha":0.00426, "fx":[10.07048,10.07059,10.03783,10.1094], "fy":[-5.24489,-5.17992,-5.17995,-5.21263]}, + {"t":2.62385, "x":1.89939, "y":5.9366, "heading":-3.14149, "vx":-0.13192, "vy":0.06816, "omega":-0.00097, "ax":0.88821, "ay":-0.45895, "alpha":0.00497, "fx":[10.08844,10.08841,10.05594,10.0559], "fy":[-5.21481,-5.20614,-5.18173,-5.21485]}, + {"t":2.67336, "x":1.89394, "y":5.93941, "heading":-3.14153, "vx":-0.08795, "vy":0.04544, "omega":-0.00072, "ax":0.88822, "ay":-0.45895, "alpha":0.00517, "fx":[10.08874,10.08871,10.05582,10.05578], "fy":[-5.21571,-5.2041,-5.1821,-5.21575]}, + {"t":2.72286, "x":1.89068, "y":5.9411, "heading":-3.14157, "vx":-0.04397, "vy":0.02272, "omega":-0.00046, "ax":0.88823, "ay":-0.45895, "alpha":0.00938, "fx":[10.07292,10.13892,10.03879,10.03874], "fy":[-5.2212,-5.18758,-5.18777,-5.22124]}, + {"t":2.77237, "x":1.88959, "y":5.94166, "heading":3.14159, "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/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 0dd6915a..3b050908 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -20,11 +20,11 @@ public FastDepot( ControllerSubsystem controller, IntakeDeployerSubsystem intake) { super( new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM), new LoggableParallelCommandGroup( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), - new SetShootingState(shootstate, ShootState.AUTO_AIM), new LoggableWaitCommand(3), new AutoShoot(hopper, feeder, 0) ); From 1c80db9ecd1c4011b9e844bd4809c463f42edd73 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 19:09:45 -0400 Subject: [PATCH 13/20] made robot go closer to depot in path --- src/main/deploy/choreo/Depot_Fast.traj | 129 +++++++++++++------------ 1 file changed, 67 insertions(+), 62 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index b56ae4d5..e6025d32 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.8895899057388303, "y":5.941661834716797, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.4981616735458374, "y":5.960968494415283, "heading":3.141592653589793, "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}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":56, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.8895899057388306 m", "val":1.8895899057388303}, "y":{"exp":"5.941661834716797 m", "val":5.941661834716797}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.4981616735458374 m", "val":1.4981616735458374}, "y":{"exp":"5.960968494415283 m", "val":5.960968494415283}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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}, @@ -53,65 +53,70 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,2.77237], + "waypoints":[0.0,3.02302], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.88807, "ay":0.45882, "alpha":0.00564, "fx":[-10.06584,-10.06573,-10.07521,-10.07518], "fy":[5.17738,5.26281,5.19422,5.17741]}, - {"t":0.04951, "x":3.59491, "y":5.06056, "heading":3.14159, "vx":-0.04397, "vy":0.02271, "omega":0.00028, "ax":-0.88824, "ay":0.45891, "alpha":0.00396, "fx":[-10.06797,-10.06793,-10.07704,-10.07701], "fy":[5.18724,5.23792,5.20348,5.18727]}, - {"t":0.09901, "x":3.59165, "y":5.06225, "heading":-3.14158, "vx":-0.08794, "vy":0.04543, "omega":0.00048, "ax":-0.88823, "ay":0.45891, "alpha":0.00274, "fx":[-10.05205,-10.05215,-10.06779,-10.11765], "fy":[5.2365,5.1983,5.19832,5.18259]}, - {"t":0.14852, "x":3.5862, "y":5.06506, "heading":-3.14156, "vx":-0.13191, "vy":0.06815, "omega":0.00061, "ax":-0.88823, "ay":0.4589, "alpha":0.00387, "fx":[-10.06836,-10.06831,-10.07632,-10.0763], "fy":[5.18705,5.23962,5.20173,5.18708]}, - {"t":0.19803, "x":3.57859, "y":5.069, "heading":-3.14153, "vx":-0.17589, "vy":0.09087, "omega":0.0008, "ax":-0.88822, "ay":0.4589, "alpha":0.00266, "fx":[-10.05015,-10.05025,-10.06416,-10.12436], "fy":[5.24099,5.19607,5.19609,5.18207]}, - {"t":0.24753, "x":3.56879, "y":5.07406, "heading":-3.14149, "vx":-0.21986, "vy":0.11359, "omega":0.00093, "ax":-0.88821, "ay":0.45889, "alpha":0.00405, "fx":[-10.06893,-10.06886,-10.07537,-10.07535], "fy":[5.18521,5.24651,5.198,5.18524]}, - {"t":0.29704, "x":3.55682, "y":5.08024, "heading":-3.14144, "vx":-0.26383, "vy":0.13631, "omega":0.00113, "ax":-0.8882, "ay":0.45888, "alpha":0.00313, "fx":[-10.04657,-10.04667,-10.05856,-10.13627], "fy":[5.24141,5.19508,5.1951,5.18307]}, - {"t":0.34655, "x":3.54267, "y":5.08755, "heading":-3.14138, "vx":-0.3078, "vy":0.15903, "omega":0.00129, "ax":-0.88819, "ay":0.45888, "alpha":0.00385, "fx":[-10.06956,-10.06948,-10.07428,-10.07426], "fy":[5.18525,5.2478,5.196,5.18529]}, - {"t":0.39605, "x":3.52634, "y":5.09599, "heading":-3.14132, "vx":-0.35177, "vy":0.18174, "omega":0.00148, "ax":-0.88818, "ay":0.45887, "alpha":0.00227, "fx":[-10.04545,-10.04557,-10.05541,-10.1406], "fy":[5.25568,5.18941,5.18944,5.17945]}, - {"t":0.44556, "x":3.50784, "y":5.10555, "heading":-3.14125, "vx":-0.39574, "vy":0.20446, "omega":0.00159, "ax":-0.88816, "ay":0.45886, "alpha":0.00433, "fx":[-10.07025,-10.07014,-10.07304,-10.07302], "fy":[5.18158,5.26024,5.19015,5.18162]}, - {"t":0.49507, "x":3.48716, "y":5.11623, "heading":-3.14117, "vx":-0.43971, "vy":0.22718, "omega":0.00181, "ax":-0.88815, "ay":0.45885, "alpha":0.00271, "fx":[-10.04036,-10.04048,-10.04813,-10.15681], "fy":[5.26052,5.1868,5.18683,5.179]}, - {"t":0.54457, "x":3.4643, "y":5.12804, "heading":-3.14108, "vx":-0.48368, "vy":0.24989, "omega":0.00194, "ax":-0.88813, "ay":0.45884, "alpha":0.00382, "fx":[-10.07084,-10.07073,-10.07174,-10.07173], "fy":[5.18309,5.25688,5.18955,5.18314]}, - {"t":0.59408, "x":3.43927, "y":5.14098, "heading":-3.14098, "vx":-0.52765, "vy":0.27261, "omega":0.00213, "ax":-0.88811, "ay":0.45883, "alpha":0.00348, "fx":[-10.034,-10.03412,-10.03962,-10.17646], "fy":[5.26361,5.18471,5.18475,5.17901]}, - {"t":0.64359, "x":3.41205, "y":5.15504, "heading":-3.14088, "vx":-0.57162, "vy":0.29532, "omega":0.0023, "ax":-0.88809, "ay":0.45881, "alpha":0.00447, "fx":[-10.0713,-10.07116,-10.07039,-10.07038], "fy":[5.17845,5.27162,5.18289,5.1785]}, - {"t":0.69309, "x":3.38267, "y":5.17022, "heading":-3.14076, "vx":-0.61559, "vy":0.31804, "omega":0.00252, "ax":-0.88807, "ay":0.4588, "alpha":0.00289, "fx":[-10.02773,-10.02789,-10.03164,-10.19489], "fy":[5.28821,5.17548,5.17552,5.1715]}, - {"t":0.7426, "x":3.3511, "y":5.18653, "heading":-3.14064, "vx":-0.65955, "vy":0.34075, "omega":0.00267, "ax":-0.88804, "ay":0.45878, "alpha":0.00539, "fx":[-10.07145,-10.07126,-10.06908,-10.06907], "fy":[5.17242,5.28979,5.17521,5.17247]}, - {"t":0.79211, "x":3.31736, "y":5.20396, "heading":-3.1405, "vx":-0.70352, "vy":0.36346, "omega":0.00293, "ax":-0.88801, "ay":0.45876, "alpha":0.00353, "fx":[-10.01548,-10.01568,-10.01805,-10.23018], "fy":[5.3108,5.1669,5.16695,5.16422]}, - {"t":0.84161, "x":3.28145, "y":5.22251, "heading":-3.14036, "vx":-0.74748, "vy":0.38618, "omega":0.00311, "ax":-0.88797, "ay":0.45873, "alpha":0.00666, "fx":[-10.07105,-10.07081,-10.06787,-10.06787], "fy":[5.16465,5.31196,5.16637,5.16471]}, - {"t":0.89112, "x":3.24335, "y":5.24219, "heading":-3.14021, "vx":-0.79144, "vy":0.40889, "omega":0.00344, "ax":-0.88792, "ay":0.4587, "alpha":0.0026, "fx":[-10.01155,-10.01177,-10.01367,-10.23852], "fy":[5.33358,5.15825,5.1583,5.15603]}, - {"t":0.94063, "x":3.20308, "y":5.263, "heading":-3.14004, "vx":-0.8354, "vy":0.43159, "omega":0.00357, "ax":-0.88786, "ay":0.45866, "alpha":0.00822, "fx":[-10.06978,-10.06949,-10.0668,-10.06679], "fy":[5.15557,5.33601,5.15717,5.15562]}, - {"t":0.99013, "x":3.16064, "y":5.28493, "heading":-3.13986, "vx":-0.87935, "vy":0.4543, "omega":0.00397, "ax":-0.88779, "ay":0.45861, "alpha":0.00821, "fx":[-9.97406,-9.9743,-9.97644,-10.34468], "fy":[5.35038,5.15146,5.15151,5.14881]}, - {"t":1.03964, "x":3.11602, "y":5.30798, "heading":-3.13966, "vx":-0.9233, "vy":0.47701, "omega":0.00438, "ax":-0.88769, "ay":0.45854, "alpha":0.01028, "fx":[-10.06685,-10.0665,-10.06591,-10.06591], "fy":[5.14429,5.363,5.14743,5.14435]}, - {"t":1.08915, "x":3.06922, "y":5.33216, "heading":-3.13945, "vx":-0.96725, "vy":0.49971, "omega":0.00489, "ax":-0.88757, "ay":0.45845, "alpha":0.00401, "fx":[-9.97013,-9.97047,-9.97583,-10.34285], "fy":[5.42343,5.12574,5.12579,5.1199]}, - {"t":1.13865, "x":3.02025, "y":5.35746, "heading":-3.1392, "vx":-1.01119, "vy":0.5224, "omega":0.00509, "ax":-0.88738, "ay":0.45832, "alpha":0.00881, "fx":[-10.06074,-10.06045,-10.06474,-10.06476], "fy":[5.15211,5.32521,5.15946,5.15216]}, - {"t":1.18816, "x":2.9691, "y":5.38388, "heading":-3.13895, "vx":-1.05512, "vy":0.54509, "omega":0.00552, "ax":-0.88708, "ay":0.45812, "alpha":-0.00565, "fx":[-10.0271,-10.02734,-10.03883,-10.14384], "fy":[5.39422,5.13246,5.13251,5.12069]}, - {"t":1.23767, "x":2.91578, "y":5.41143, "heading":-3.13868, "vx":-1.09904, "vy":0.56777, "omega":0.00524, "ax":-0.88655, "ay":0.45774, "alpha":-0.00585, "fx":[-10.04698,-10.04689,-10.05957,-10.0596], "fy":[5.2283,5.06265,5.2435,5.22836]}, - {"t":1.28717, "x":2.86028, "y":5.4401, "heading":-3.13842, "vx":-1.14293, "vy":0.59043, "omega":0.00495, "ax":-0.88532, "ay":0.45686, "alpha":-0.03564, "fx":[-10.33128,-10.33114,-10.35297,-9.14189], "fy":[4.90267,5.28048,5.28054,5.25895]}, - {"t":1.33668, "x":2.80261, "y":5.46989, "heading":-3.13817, "vx":-1.18676, "vy":0.61305, "omega":0.00319, "ax":-0.87872, "ay":0.45345, "alpha":-0.17234, "fx":[-9.94637,-9.947,-9.98242,-9.98249], "fy":[6.08182,2.2838,6.12063,6.08191]}, - {"t":1.38619, "x":2.74278, "y":5.50079, "heading":-3.13801, "vx":-1.23026, "vy":0.6355, "omega":-0.00534, "ax":0.88074, "ay":-0.44918, "alpha":0.03098, "fx":[10.80828,10.80781,10.78742,7.54622], "fy":[-3.11123,-5.74739,-5.7473,-5.76848]}, - {"t":1.43569, "x":2.68296, "y":5.5317, "heading":-3.13828, "vx":-1.18666, "vy":0.61326, "omega":-0.00381, "ax":0.88469, "ay":-0.45802, "alpha":0.02592, "fx":[10.04487,10.04501,10.01959,10.01955], "fy":[-5.32001,-4.8397,-5.29589,-5.31996]}, - {"t":1.4852, "x":2.62529, "y":5.5615, "heading":-3.13847, "vx":-1.14286, "vy":0.59059, "omega":-0.00252, "ax":0.88627, "ay":-0.45827, "alpha":-0.00553, "fx":[10.08031,10.08039,10.051,9.9887], "fy":[-4.98702,-5.25684,-5.25678,-5.28602]}, - {"t":1.5347, "x":2.5698, "y":5.59018, "heading":-3.13859, "vx":-1.09898, "vy":0.5679, "omega":-0.0028, "ax":0.8869, "ay":-0.45846, "alpha":0.00158, "fx":[10.07428,10.07406,10.04034,10.0403], "fy":[-5.19048,-5.25583,-5.15852,-5.19045]}, - {"t":1.58421, "x":2.51648, "y":5.61773, "heading":-3.13873, "vx":-1.05507, "vy":0.5452, "omega":-0.00272, "ax":0.88723, "ay":-0.45859, "alpha":-0.00706, "fx":[9.93879,9.93926,9.90456,10.46165], "fy":[-5.41814,-5.11636,-5.11634,-5.15036]}, - {"t":1.63372, "x":2.46533, "y":5.64416, "heading":-3.13887, "vx":-1.01115, "vy":0.5225, "omega":-0.00307, "ax":0.88745, "ay":-0.45866, "alpha":-0.00232, "fx":[10.08283,10.0825,10.04445,10.04441], "fy":[-5.16969,-5.33142,-5.13379,-5.16968]}, - {"t":1.68322, "x":2.41636, "y":5.66947, "heading":-3.13902, "vx":-0.96722, "vy":0.49979, "omega":-0.00318, "ax":0.8876, "ay":-0.45872, "alpha":-0.00566, "fx":[9.97168,9.97203,9.93496,10.38219], "fy":[-5.35382,-5.13901,-5.139,-5.17548]}, - {"t":1.73273, "x":2.36957, "y":5.69365, "heading":-3.13918, "vx":-0.92327, "vy":0.47708, "omega":-0.00346, "ax":0.88771, "ay":-0.45876, "alpha":0.00103, "fx":[10.08632,10.08608,10.04681,10.04676], "fy":[-5.18812,-5.28146,-5.15117,-5.18812]}, - {"t":1.78224, "x":2.32495, "y":5.71671, "heading":-3.13935, "vx":-0.87933, "vy":0.45437, "omega":-0.00341, "ax":0.8878, "ay":-0.45879, "alpha":-0.0003, "fx":[10.01258,10.01289,9.9757,10.26857], "fy":[-5.32489,-5.14956,-5.14956,-5.18634]}, - {"t":1.83174, "x":2.2825, "y":5.73864, "heading":-3.13952, "vx":-0.83537, "vy":0.43166, "omega":-0.00343, "ax":0.88786, "ay":-0.45881, "alpha":0.00216, "fx":[10.08787,10.08767,10.04864,10.04859], "fy":[-5.19495,-5.26342,-5.15808,-5.19496]}, - {"t":1.88125, "x":2.24223, "y":5.75944, "heading":-3.13969, "vx":-0.79142, "vy":0.40894, "omega":-0.00332, "ax":0.88792, "ay":-0.45883, "alpha":0.00025, "fx":[10.02451,10.02477,9.98798,10.23792], "fy":[-5.30288,-5.15768,-5.15769,-5.19412]}, - {"t":1.93076, "x":2.20414, "y":5.77913, "heading":-3.13985, "vx":-0.74746, "vy":0.38623, "omega":-0.00331, "ax":0.88796, "ay":-0.45885, "alpha":0.00349, "fx":[10.08844,10.08831,10.05024,10.05019], "fy":[-5.20294,-5.24045,-5.16677,-5.20296]}, - {"t":1.98026, "x":2.16823, "y":5.79769, "heading":-3.14001, "vx":-0.7035, "vy":0.36351, "omega":-0.00314, "ax":0.888, "ay":-0.45887, "alpha":0.00046, "fx":[10.03713,10.03734,10.00133,10.20304], "fy":[-5.27243,-5.16854,-5.16855,-5.20427]}, - {"t":2.02977, "x":2.13449, "y":5.81512, "heading":-3.14017, "vx":-0.65954, "vy":0.3408, "omega":-0.00311, "ax":0.88803, "ay":-0.45888, "alpha":0.00319, "fx":[10.08859,10.08846,10.05165,10.0516], "fy":[-5.20229,-5.24271,-5.16699,-5.2023]}, - {"t":2.07928, "x":2.10292, "y":5.83143, "heading":-3.14032, "vx":-0.61558, "vy":0.31808, "omega":-0.00296, "ax":0.88806, "ay":-0.45889, "alpha":0.0023, "fx":[10.04888,10.04907,10.01418,10.1694], "fy":[-5.27023,-5.16995,-5.16996,-5.20463]}, - {"t":2.12878, "x":2.07354, "y":5.84661, "heading":-3.14047, "vx":-0.57161, "vy":0.29536, "omega":-0.00284, "ax":0.88808, "ay":-0.4589, "alpha":0.00409, "fx":[10.08842,10.08833,10.05294,10.0529], "fy":[-5.20801,-5.22538,-5.17376,-5.20803]}, - {"t":2.17829, "x":2.04633, "y":5.86067, "heading":-3.14061, "vx":-0.52765, "vy":0.27264, "omega":-0.00264, "ax":0.8881, "ay":-0.4589, "alpha":0.00242, "fx":[10.05324,10.05341,10.01947,10.15742], "fy":[-5.26175,-5.17334,-5.17336,-5.2071]}, - {"t":2.2278, "x":2.02129, "y":5.87361, "heading":-3.14074, "vx":-0.48368, "vy":0.24992, "omega":-0.00252, "ax":0.88812, "ay":-0.45891, "alpha":0.0041, "fx":[10.08824,10.08816,10.05401,10.05397], "fy":[-5.20886,-5.22267,-5.17545,-5.20888]}, - {"t":2.2773, "x":1.99844, "y":5.88542, "heading":-3.14087, "vx":-0.43971, "vy":0.2272, "omega":-0.00232, "ax":0.88813, "ay":-0.45892, "alpha":0.00233, "fx":[10.05771,10.05784,10.02465,10.14492], "fy":[-5.24898,-5.17803,-5.17805,-5.21108]}, - {"t":2.32681, "x":1.97775, "y":5.89611, "heading":-3.14098, "vx":-0.39574, "vy":0.20449, "omega":-0.0022, "ax":0.88815, "ay":-0.45892, "alpha":0.00414, "fx":[10.08806,10.08799,10.05489,10.05484], "fy":[-5.20974,-5.21991,-5.17699,-5.20977]}, - {"t":2.37632, "x":1.95925, "y":5.90567, "heading":-3.14109, "vx":-0.35177, "vy":0.18177, "omega":-0.002, "ax":0.88816, "ay":-0.45893, "alpha":0.00343, "fx":[10.06351,10.06364,10.03104,10.12819], "fy":[-5.25104,-5.17769,-5.17772,-5.21019]}, - {"t":2.42582, "x":1.94292, "y":5.9141, "heading":-3.14119, "vx":-0.3078, "vy":0.15905, "omega":-0.00183, "ax":0.88817, "ay":-0.45893, "alpha":0.00458, "fx":[10.08797,10.08792,10.05555,10.0555], "fy":[-5.21259,-5.21141,-5.18022,-5.21262]}, - {"t":2.47533, "x":1.92877, "y":5.92141, "heading":-3.14128, "vx":-0.26383, "vy":0.13633, "omega":-0.0016, "ax":0.88819, "ay":-0.45894, "alpha":0.00376, "fx":[10.06729,10.06741,10.03497,10.11776], "fy":[-5.24595,-5.17956,-5.17959,-5.21193]}, - {"t":2.52484, "x":1.9168, "y":5.9276, "heading":-3.14136, "vx":-0.21986, "vy":0.1136, "omega":-0.00141, "ax":0.8882, "ay":-0.45894, "alpha":0.00476, "fx":[10.08808,10.08804,10.0559,10.05586], "fy":[-5.21374,-5.20841,-5.18128,-5.21378]}, - {"t":2.57434, "x":1.90701, "y":5.93266, "heading":-3.14143, "vx":-0.17589, "vy":0.09088, "omega":-0.00118, "ax":0.88821, "ay":-0.45894, "alpha":0.00426, "fx":[10.07048,10.07059,10.03783,10.1094], "fy":[-5.24489,-5.17992,-5.17995,-5.21263]}, - {"t":2.62385, "x":1.89939, "y":5.9366, "heading":-3.14149, "vx":-0.13192, "vy":0.06816, "omega":-0.00097, "ax":0.88821, "ay":-0.45895, "alpha":0.00497, "fx":[10.08844,10.08841,10.05594,10.0559], "fy":[-5.21481,-5.20614,-5.18173,-5.21485]}, - {"t":2.67336, "x":1.89394, "y":5.93941, "heading":-3.14153, "vx":-0.08795, "vy":0.04544, "omega":-0.00072, "ax":0.88822, "ay":-0.45895, "alpha":0.00517, "fx":[10.08874,10.08871,10.05582,10.05578], "fy":[-5.21571,-5.2041,-5.1821,-5.21575]}, - {"t":2.72286, "x":1.89068, "y":5.9411, "heading":-3.14157, "vx":-0.04397, "vy":0.02272, "omega":-0.00046, "ax":0.88823, "ay":-0.45895, "alpha":0.00938, "fx":[10.07292,10.13892,10.03879,10.03874], "fy":[-5.2212,-5.18758,-5.18777,-5.22124]}, - {"t":2.77237, "x":1.88959, "y":5.94166, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.91847, "ay":0.39445, "alpha":0.00104, "fx":[-10.41911,-10.41903,-10.41152,-10.41149], "fy":[4.46361,4.50911,4.45558,4.46363]}, + {"t":0.04956, "x":3.59487, "y":5.06048, "heading":3.14159, "vx":-0.04552, "vy":0.01955, "omega":0.00005, "ax":-0.91865, "ay":0.39453, "alpha":-0.00042, "fx":[-10.42104,-10.42103,-10.41369,-10.41366], "fy":[4.47243,4.48613,4.46444,4.47246]}, + {"t":0.09912, "x":3.59149, "y":5.06194, "heading":-3.14159, "vx":-0.09104, "vy":0.0391, "omega":0.00003, "ax":-0.91865, "ay":0.39452, "alpha":-0.0007, "fx":[-10.40881,-10.40889,-10.40084,-10.45057], "fy":[4.50241,4.46162,4.46164,4.46964]}, + {"t":0.14867, "x":3.58585, "y":5.06436, "heading":-3.14159, "vx":-0.13657, "vy":0.05865, "omega":0.0, "ax":-0.91864, "ay":0.39452, "alpha":-0.00041, "fx":[-10.42093,-10.42091,-10.41348,-10.41345], "fy":[4.47222,4.48684,4.46384,4.47225]}, + {"t":0.19823, "x":3.57795, "y":5.06775, "heading":-3.14159, "vx":-0.1821, "vy":0.0782, "omega":-0.00002, "ax":-0.91863, "ay":0.39452, "alpha":-0.00036, "fx":[-10.40673,-10.4068,-10.39812,-10.45677], "fy":[4.50193,4.46146,4.46148,4.4701]}, + {"t":0.24779, "x":3.5678, "y":5.07211, "heading":-3.14159, "vx":-0.22762, "vy":0.09775, "omega":-0.00004, "ax":-0.91862, "ay":0.39451, "alpha":-0.00036, "fx":[-10.42103,-10.421,-10.41301,-10.41299], "fy":[4.47158,4.4892,4.46239,4.47161]}, + {"t":0.29735, "x":3.55539, "y":5.07744, "heading":-3.14159, "vx":-0.27315, "vy":0.11731, "omega":-0.00006, "ax":-0.91861, "ay":0.39451, "alpha":-0.00069, "fx":[-10.40563,-10.40571,-10.39607,-10.4602], "fy":[4.50887,4.4587,4.45872,4.46829]}, + {"t":0.3469, "x":3.54073, "y":5.08374, "heading":3.14159, "vx":-0.31867, "vy":0.13686, "omega":-0.00009, "ax":-0.9186, "ay":0.3945, "alpha":-0.00047, "fx":[-10.4213,-10.42126,-10.41231,-10.41229], "fy":[4.4716,4.48987,4.46126,4.47163]}, + {"t":0.39646, "x":3.52381, "y":5.091, "heading":3.14159, "vx":-0.36419, "vy":0.15641, "omega":-0.00012, "ax":-0.91859, "ay":0.3945, "alpha":-0.00014, "fx":[-10.40203,-10.40211,-10.3911,-10.47144], "fy":[4.50858,4.45819,4.45821,4.46913]}, + {"t":0.44602, "x":3.50463, "y":5.09924, "heading":3.14158, "vx":-0.40972, "vy":0.17596, "omega":-0.00012, "ax":-0.91858, "ay":0.39449, "alpha":-0.00022, "fx":[-10.42171,-10.42166,-10.41139,-10.41136], "fy":[4.46945,4.49727,4.45763,4.46948]}, + {"t":0.49558, "x":3.4832, "y":5.10844, "heading":3.14157, "vx":-0.45524, "vy":0.19551, "omega":-0.00013, "ax":-0.91857, "ay":0.39449, "alpha":0.00028, "fx":[-10.3978,-10.39789,-10.38529,-10.48455], "fy":[4.51203,4.45633,4.45636,4.46883]}, + {"t":0.54513, "x":3.45951, "y":5.11862, "heading":3.14157, "vx":-0.50076, "vy":0.21506, "omega":-0.00012, "ax":-0.91855, "ay":0.39448, "alpha":-0.00008, "fx":[-10.42217,-10.42211,-10.41031,-10.41028], "fy":[4.46783,4.50319,4.45436,4.46785]}, + {"t":0.59469, "x":3.43356, "y":5.12976, "heading":3.14156, "vx":-0.54628, "vy":0.23461, "omega":-0.00012, "ax":-0.91854, "ay":0.39447, "alpha":0.00042, "fx":[-10.39339,-10.39349,-10.3792,-10.49805], "fy":[4.52004,4.45288,4.4529,4.46704]}, + {"t":0.64425, "x":3.40536, "y":5.14187, "heading":3.14155, "vx":-0.5918, "vy":0.25416, "omega":-0.0001, "ax":-0.91852, "ay":0.39446, "alpha":-0.00043, "fx":[-10.42258,-10.42252,-10.40912,-10.40909], "fy":[4.46872,4.50143,4.45357,4.46874]}, + {"t":0.69381, "x":3.37491, "y":5.15495, "heading":3.14155, "vx":-0.63732, "vy":0.2737, "omega":-0.00013, "ax":-0.9185, "ay":0.39445, "alpha":0.00009, "fx":[-10.38911,-10.38924,-10.37325,-10.51078], "fy":[4.53492,4.44708,4.4471,4.46291]}, + {"t":0.74337, "x":3.34219, "y":5.169, "heading":3.14154, "vx":-0.68284, "vy":0.29325, "omega":-0.00012, "ax":-0.91848, "ay":0.39444, "alpha":0.00031, "fx":[-10.42295,-10.42286,-10.40778,-10.40775], "fy":[4.46362,4.51753,4.44669,4.46365]}, + {"t":0.79292, "x":3.30723, "y":5.18402, "heading":3.14154, "vx":-0.72836, "vy":0.3128, "omega":-0.00011, "ax":-0.91845, "ay":0.39443, "alpha":0.00175, "fx":[-10.37774,-10.37787,-10.36007,-10.54447], "fy":[4.53868,4.44487,4.44489,4.46246]}, + {"t":0.84248, "x":3.27, "y":5.2, "heading":3.14153, "vx":-0.77388, "vy":0.33235, "omega":-0.00002, "ax":-0.91842, "ay":0.39441, "alpha":0.00045, "fx":[-10.42304,-10.42294,-10.40641,-10.40638], "fy":[4.46181,4.52326,4.44332,4.46184]}, + {"t":0.89204, "x":3.23052, "y":5.21696, "heading":3.14153, "vx":-0.81939, "vy":0.35189, "omega":0.0, "ax":-0.91838, "ay":0.39439, "alpha":0.00312, "fx":[-10.36444,-10.3646,-10.34537,-10.58276], "fy":[4.55259,4.4393,4.43932,4.45824]}, + {"t":0.9416, "x":3.18879, "y":5.23488, "heading":3.14153, "vx":-0.8649, "vy":0.37144, "omega":0.00016, "ax":-0.91834, "ay":0.39437, "alpha":0.00133, "fx":[-10.42264,-10.4225,-10.40508,-10.40504], "fy":[4.4562,4.53946,4.43663,4.45622]}, + {"t":0.99115, "x":3.1448, "y":5.25377, "heading":3.14154, "vx":-0.91041, "vy":0.39098, "omega":0.00022, "ax":-0.91829, "ay":0.39435, "alpha":0.00549, "fx":[-10.3426,-10.34279,-10.32279,-10.6448], "fy":[4.57545,4.43076,4.43078,4.45038]}, + {"t":1.04071, "x":3.09855, "y":5.27363, "heading":3.14155, "vx":-0.95592, "vy":0.41053, "omega":0.0005, "ax":-0.91823, "ay":0.39432, "alpha":0.00299, "fx":[-10.4213,-10.42111,-10.40392,-10.40388], "fy":[4.4468,4.56526,4.42712,4.44682]}, + {"t":1.09027, "x":3.05005, "y":5.29446, "heading":3.14158, "vx":-1.00143, "vy":0.43007, "omega":0.00064, "ax":-0.91815, "ay":0.39428, "alpha":0.00444, "fx":[-10.32598,-10.32626,-10.30712,-10.68737], "fy":[4.63759,4.40933,4.40935,4.42801]}, + {"t":1.13983, "x":2.9993, "y":5.31626, "heading":-3.14158, "vx":-1.04693, "vy":0.44961, "omega":0.00086, "ax":-0.91805, "ay":0.39423, "alpha":0.00316, "fx":[-10.41831,-10.41813,-10.40292,-10.40289], "fy":[4.44588,4.56261,4.42771,4.44589]}, + {"t":1.18938, "x":2.94629, "y":5.33902, "heading":-3.14154, "vx":-1.09243, "vy":0.46914, "omega":0.00102, "ax":-0.91792, "ay":0.39417, "alpha":0.00604, "fx":[-10.31316,-10.31344,-10.29657,-10.71315], "fy":[4.64349,4.40639,4.4064,4.42278]}, + {"t":1.23894, "x":2.89102, "y":5.36276, "heading":-3.14148, "vx":-1.13792, "vy":0.48868, "omega":0.00132, "ax":-0.91774, "ay":0.39408, "alpha":0.00228, "fx":[-10.41262,-10.41246,-10.40145,-10.40142], "fy":[4.45096,4.53675,4.4363,4.45097]}, + {"t":1.2885, "x":2.8335, "y":5.38746, "heading":-3.14142, "vx":-1.1834, "vy":0.50821, "omega":0.00143, "ax":-0.91746, "ay":0.39394, "alpha":0.0013, "fx":[-10.34485,-10.34507,-10.33296,-10.59257], "fy":[4.61113,4.4153,4.41531,4.42707]}, + {"t":1.33806, "x":2.77373, "y":5.41313, "heading":-3.14135, "vx":-1.22887, "vy":0.52773, "omega":0.0015, "ax":-0.917, "ay":0.39371, "alpha":-0.00245, "fx":[-10.40077,-10.40069,-10.39656,-10.39655], "fy":[4.47571,4.44016,4.46695,4.47572]}, + {"t":1.38762, "x":2.7117, "y":5.43977, "heading":-3.14127, "vx":-1.27431, "vy":0.54724, "omega":0.00138, "ax":-0.91608, "ay":0.39327, "alpha":-0.01456, "fx":[-10.52649,-10.52647,-10.52403,-9.97578], "fy":[4.27676,4.51965,4.51966,4.52215]}, + {"t":1.43717, "x":2.64742, "y":5.46737, "heading":-3.14121, "vx":-1.31971, "vy":0.56673, "omega":0.00065, "ax":-0.91331, "ay":0.39196, "alpha":-0.063, "fx":[-10.35199,-10.35221,-10.36142,-10.3614], "fy":[4.78746,3.40604,4.79785,4.78745]}, + {"t":1.48673, "x":2.5809, "y":5.49594, "heading":-3.14117, "vx":-1.36497, "vy":0.58616, "omega":-0.00247, "ax":0.00064, "ay":0.00202, "alpha":-0.00829, "fx":[-0.05766,-0.05764,-0.05508,0.19953], "fy":[-0.04097,0.04327,0.04329,0.04584]}, + {"t":1.53629, "x":2.51326, "y":5.52499, "heading":-3.1413, "vx":-1.36494, "vy":0.58626, "omega":-0.00288, "ax":0.91322, "ay":-0.39217, "alpha":0.06468, "fx":[10.35857,10.35877,10.35284,10.35274], "fy":[-4.79133,-3.40326,-4.80247,-4.79126]}, + {"t":1.58585, "x":2.44674, "y":5.55356, "heading":-3.14144, "vx":-1.31968, "vy":0.56682, "omega":0.00033, "ax":0.91596, "ay":-0.39355, "alpha":0.01625, "fx":[10.53576,10.53577,10.5325,9.94326], "fy":[-4.2743,-4.52447,-4.52444,-4.52779]}, + {"t":1.6354, "x":2.38246, "y":5.58117, "heading":-3.14142, "vx":-1.27429, "vy":0.54732, "omega":0.00113, "ax":0.91692, "ay":-0.39391, "alpha":0.00439, "fx":[10.40748,10.40742,10.38794,10.38791], "fy":[-4.48066,-4.43553,-4.47059,-4.48065]}, + {"t":1.68496, "x":2.32044, "y":5.60781, "heading":-3.14137, "vx":-1.22885, "vy":0.5278, "omega":0.00135, "ax":0.9174, "ay":-0.39409, "alpha":-0.0002, "fx":[10.34911,10.34935,10.336,10.57808], "fy":[-4.6143,-4.41608,-4.41607,-4.42909]}, + {"t":1.73452, "x":2.26066, "y":5.63348, "heading":-3.1413, "vx":-1.18338, "vy":0.50827, "omega":0.00134, "ax":0.91769, "ay":-0.3942, "alpha":-0.00042, "fx":[10.42,10.41984,10.39291,10.39288], "fy":[-4.45436,-4.53383,-4.43785,-4.45437]}, + {"t":1.78408, "x":2.20314, "y":5.65818, "heading":-3.14123, "vx":-1.13791, "vy":0.48873, "omega":0.00132, "ax":0.91788, "ay":-0.39426, "alpha":-0.01629, "fx":[10.26469,10.26494,10.24518,10.85959], "fy":[-4.62063,-4.41459,-4.41461,-4.43371]}, + {"t":1.83364, "x":2.14788, "y":5.68192, "heading":-3.14117, "vx":-1.09242, "vy":0.46919, "omega":0.00051, "ax":0.91802, "ay":-0.39432, "alpha":-0.0016, "fx":[10.42593,10.42574,10.3945,10.39447], "fy":[-4.44722,-4.56434,-4.42702,-4.44724]}, + {"t":1.88319, "x":2.09487, "y":5.70469, "heading":-3.14114, "vx":-1.04692, "vy":0.44965, "omega":0.00043, "ax":0.91812, "ay":-0.39435, "alpha":-0.01067, "fx":[10.30158,10.30182,10.28037,10.76154], "fy":[-4.60799,-4.41955,-4.41958,-4.44046]}, + {"t":1.93275, "x":2.04411, "y":5.72649, "heading":-3.14112, "vx":-1.00142, "vy":0.43011, "omega":-0.0001, "ax":0.9182, "ay":-0.39438, "alpha":-0.0015, "fx":[10.42857,10.42837,10.39602,10.39599], "fy":[-4.44794,-4.56614,-4.42686,-4.44797]}, + {"t":1.98231, "x":1.99561, "y":5.74732, "heading":-3.14113, "vx":-0.95592, "vy":0.41056, "omega":-0.00017, "ax":0.91827, "ay":-0.39441, "alpha":-0.00468, "fx":[10.34502,10.34521,10.32422,10.63741], "fy":[-4.58005,-4.42976,-4.42979,-4.45039]}, + {"t":2.03187, "x":1.94937, "y":5.76718, "heading":-3.14113, "vx":-0.91041, "vy":0.39102, "omega":-0.0004, "ax":0.91832, "ay":-0.39443, "alpha":0.00026, "fx":[10.42943,10.42929,10.39778,10.39775], "fy":[-4.45833,-4.53604,-4.43815,-4.45836]}, + {"t":2.08142, "x":1.90538, "y":5.78607, "heading":-3.14115, "vx":-0.8649, "vy":0.37147, "omega":-0.00039, "ax":0.91836, "ay":-0.39444, "alpha":-0.0028, "fx":[10.35851,10.35871,10.33911,10.59991], "fy":[-4.57482,-4.43249,-4.43252,-4.45178]}, + {"t":2.13098, "x":1.86364, "y":5.804, "heading":-3.14117, "vx":-0.81939, "vy":0.35192, "omega":-0.00053, "ax":0.9184, "ay":-0.39446, "alpha":0.00071, "fx":[10.42949,10.42937,10.39954,10.39951], "fy":[-4.46193,-4.52496,-4.44336,-4.46196]}, + {"t":2.18054, "x":1.82416, "y":5.82095, "heading":-3.1412, "vx":-0.77388, "vy":0.33237, "omega":-0.00049, "ax":0.91843, "ay":-0.39447, "alpha":-0.00253, "fx":[10.37266,10.37281,10.35504,10.55885], "fy":[-4.54114,-4.44468,-4.44471,-4.46222]}, + {"t":2.2301, "x":1.78694, "y":5.83694, "heading":-3.14122, "vx":-0.72836, "vy":0.31282, "omega":-0.00062, "ax":0.91846, "ay":-0.39448, "alpha":0.00099, "fx":[10.42907,10.42897,10.4013,10.40127], "fy":[-4.46475,-4.5155,-4.44819,-4.46477]}, + {"t":2.27965, "x":1.75197, "y":5.85196, "heading":-3.14125, "vx":-0.68284, "vy":0.29328, "omega":-0.00057, "ax":0.91848, "ay":-0.39449, "alpha":-0.00093, "fx":[10.38666,10.38679,10.37126,10.51698], "fy":[-4.52914,-4.4497,-4.44972,-4.46506]}, + {"t":2.32921, "x":1.71926, "y":5.86601, "heading":-3.14128, "vx":-0.63733, "vy":0.27373, "omega":-0.00061, "ax":0.9185, "ay":-0.39449, "alpha":0.00085, "fx":[10.4284,10.42832,10.40298,10.40295], "fy":[-4.46537,-4.51222,-4.45099,-4.46539]}, + {"t":2.37877, "x":1.6888, "y":5.87909, "heading":-3.14131, "vx":-0.59181, "vy":0.25418, "omega":-0.00057, "ax":0.91852, "ay":-0.3945, "alpha":-0.00047, "fx":[10.39307,10.39318,10.37988,10.4974], "fy":[-4.52106,-4.45334,-4.45336,-4.46652]}, + {"t":2.42833, "x":1.6606, "y":5.8912, "heading":-3.14134, "vx":-0.54629, "vy":0.23462, "omega":-0.0006, "ax":0.91854, "ay":-0.39451, "alpha":0.00097, "fx":[10.42764,10.42758,10.40455,10.40451], "fy":[-4.46731,-4.50483,-4.45511,-4.46733]}, + {"t":2.47789, "x":1.63466, "y":5.90235, "heading":-3.14137, "vx":-0.50077, "vy":0.21507, "omega":-0.00055, "ax":0.91856, "ay":-0.39451, "alpha":0.00018, "fx":[10.39825,10.39836,10.3872,10.48117], "fy":[-4.51944,-4.45477,-4.45479,-4.46583]}, + {"t":2.52744, "x":1.61097, "y":5.91252, "heading":-3.1414, "vx":-0.45524, "vy":0.19552, "omega":-0.00054, "ax":0.91857, "ay":-0.39452, "alpha":0.00108, "fx":[10.42691,10.42686,10.40593,10.40589], "fy":[-4.46905,-4.49808,-4.45885,-4.46908]}, + {"t":2.577, "x":1.58953, "y":5.92172, "heading":-3.14142, "vx":-0.40972, "vy":0.17597, "omega":-0.00049, "ax":0.91858, "ay":-0.39452, "alpha":0.0001, "fx":[10.4018,10.4019,10.39259,10.46987], "fy":[-4.51023,-4.45859,-4.45861,-4.46783]}, + {"t":2.62656, "x":1.57036, "y":5.92996, "heading":-3.14145, "vx":-0.3642, "vy":0.15642, "omega":-0.00048, "ax":0.91859, "ay":-0.39453, "alpha":0.00096, "fx":[10.4263,10.42625,10.40708,10.40705], "fy":[-4.46941,-4.4957,-4.46091,-4.46943]}, + {"t":2.67612, "x":1.55344, "y":5.93723, "heading":-3.14147, "vx":-0.31868, "vy":0.13687, "omega":-0.00043, "ax":0.9186, "ay":-0.39453, "alpha":0.00015, "fx":[10.40379,10.40387,10.39614,10.46336], "fy":[-4.5075,-4.46014,-4.46016,-4.46782]}, + {"t":2.72567, "x":1.53877, "y":5.94353, "heading":-3.14149, "vx":-0.27315, "vy":0.11732, "omega":-0.00043, "ax":0.91861, "ay":-0.39453, "alpha":0.00111, "fx":[10.42581,10.42578,10.40801,10.40799], "fy":[-4.47099,-4.4899,-4.4639,-4.47101]}, + {"t":2.77523, "x":1.52636, "y":5.94886, "heading":-3.14152, "vx":-0.22763, "vy":0.09776, "omega":-0.00037, "ax":0.91862, "ay":-0.39454, "alpha":0.00006, "fx":[10.40588,10.40595,10.39942,10.45674], "fy":[-4.50164,-4.46259,-4.46261,-4.46909]}, + {"t":2.82479, "x":1.51621, "y":5.95322, "heading":-3.14153, "vx":-0.1821, "vy":0.07821, "omega":-0.00037, "ax":0.91863, "ay":-0.39454, "alpha":0.00119, "fx":[10.42552,10.4255,10.40869,10.40866], "fy":[-4.47196,-4.48625,-4.46589,-4.47199]}, + {"t":2.87435, "x":1.50831, "y":5.95661, "heading":-3.14155, "vx":-0.13658, "vy":0.05866, "omega":-0.00031, "ax":0.91864, "ay":-0.39454, "alpha":0.00041, "fx":[10.40802,10.40809,10.40237,10.45024], "fy":[-4.50231,-4.46272,-4.46275,-4.46843]}, + {"t":2.9239, "x":1.50267, "y":5.95903, "heading":-3.14157, "vx":-0.09105, "vy":0.03911, "omega":-0.00029, "ax":0.91865, "ay":-0.39455, "alpha":0.00118, "fx":[10.42547,10.42545,10.40907,10.40905], "fy":[-4.4722,-4.48522,-4.46667,-4.47223]}, + {"t":2.97346, "x":1.49929, "y":5.96048, "heading":-3.14158, "vx":-0.04553, "vy":0.01955, "omega":-0.00023, "ax":0.91865, "ay":-0.39455, "alpha":0.00465, "fx":[10.40592,10.46221,10.40062,10.40059], "fy":[-4.48231,-4.46582,-4.46597,-4.48234]}, + {"t":3.02302, "x":1.49816, "y":5.96097, "heading":3.14159, "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 fe2a910dec706547e047aba3d1cee3094d853af8 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 2 Apr 2026 19:16:15 -0400 Subject: [PATCH 14/20] Fixed depot shoot intake --- .../frc/robot/commands/auto/AutoShoot.java | 3 +- .../commands/auto/newauto/FastDepot.java | 2 +- .../robot/commands/hopper/AutoSpinHopper.java | 47 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 1 + .../robot/subsystems/ControllerSubsystem.java | 2 +- .../robot/utils/math/TurretCalculations.java | 2 +- 6 files changed, 53 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java diff --git a/src/main/java/frc/robot/commands/auto/AutoShoot.java b/src/main/java/frc/robot/commands/auto/AutoShoot.java index b80f8880..ec0ebfe4 100644 --- a/src/main/java/frc/robot/commands/auto/AutoShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutoShoot.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.hopper.AutoSpinHopper; import frc.robot.commands.hopper.SpinHopper; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -10,7 +11,7 @@ public class AutoShoot extends LoggableRaceCommandGroup{ public AutoShoot(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem, double time){ super( - new SpinHopper(hopperSubsystem), + new AutoSpinHopper(hopperSubsystem), new SpinFeeder(feederSubsystem), new LoggableWaitCommand(time) ); diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 3b050908..d7cd54c1 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -26,7 +26,7 @@ public FastDepot( LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), new LoggableWaitCommand(3), - new AutoShoot(hopper, feeder, 0) + new AutoShoot(hopper, feeder, 5) ); } } diff --git a/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java b/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java new file mode 100644 index 00000000..ed325892 --- /dev/null +++ b/src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java @@ -0,0 +1,47 @@ +package frc.robot.commands.hopper; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class AutoSpinHopper extends LoggableCommand { + + public final HopperSubsystem subsystem; + public final Timer timer; + + + public AutoSpinHopper(HopperSubsystem subsystem){ + timer = new Timer(); + this.subsystem = subsystem; + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + subsystem.setSpeed(Constants.HOPPER_AUTO_SPEED); + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)){ + return true; + } + else{ + return false; + } + + } + + +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 60cea9ec..138ae2ee 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -48,6 +48,7 @@ public enum Mode { public static final double INTAKE_SPEED = -0.7; public static final double INTAKE_REVERSE_SPEED = 0.5; public static final double HOPPER_SPEED = 0.6; + public static final double HOPPER_AUTO_SPEED = 0.35; public static final double CLIMBER_SPEED_UP = 0.1; public static final double CLIMBER_SPEED_DOWN = -0.1; public static final double FEEDER_SPEED = 1; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 3d3d9273..424a758b 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -321,7 +321,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc } private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { - if(state == ShootState.SHOOTING_HUB || state == ShootState.SHUTTLING || state == ShootState.AUTO_AIM){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM){ return Math.floor( Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), diff --git a/src/main/java/frc/robot/utils/math/TurretCalculations.java b/src/main/java/frc/robot/utils/math/TurretCalculations.java index 67887034..77885f93 100644 --- a/src/main/java/frc/robot/utils/math/TurretCalculations.java +++ b/src/main/java/frc/robot/utils/math/TurretCalculations.java @@ -50,7 +50,7 @@ public static double calculateTurretAngle(ShootState state, double robotPosX, do double hubPosX; double hubPosY; - if(state == ShootState.SHOOTING_HUB) { + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM) { if (isBlueAlliance) { // hub position determined by which alliance robot is on hubPosX = Constants.BLUE_HUB_X_POSITION; From 8cd636d33eff49cfd1205746c6a52210c95901c2 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 2 Apr 2026 19:19:45 -0400 Subject: [PATCH 15/20] Removed loggable wait --- src/main/java/frc/robot/commands/auto/newauto/FastDepot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index d7cd54c1..8a42631b 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -25,7 +25,6 @@ public FastDepot( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), - new LoggableWaitCommand(3), new AutoShoot(hopper, feeder, 5) ); } From 9c67dd258000839fbfaa4e83f7ce6ed806d6f2d1 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 2 Apr 2026 19:52:51 -0400 Subject: [PATCH 16/20] Fixed fast depot sequence --- .../commands/auto/newauto/FastDepot.java | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java index 8a42631b..a4d2d794 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java @@ -1,9 +1,14 @@ package frc.robot.commands.auto.newauto; import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.auto.AutoReset; import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.drive.DriveSwerve; +import frc.robot.commands.intakeDeployment.ToggleDeployment; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.DriveDirection; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.*; @@ -25,7 +30,19 @@ public FastDepot( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), - new AutoShoot(hopper, feeder, 5) + new AutoShoot(hopper, feeder, 2), + new ToggleDeployment(intake, controller), + new LoggableWaitCommand(0.5), + new LoggableParallelCommandGroup( + new AutoShoot(hopper, feeder, 20), + new LoggableSequentialCommandGroup( + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 5, 0.2), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 0.5, 0.5), + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 6, 0.2), + new ToggleDeployment(intake, controller) + ) + ) + ); } } From f5bf11763ea9d74da07a3460154d5d8ec9b1539e Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 19:54:48 -0400 Subject: [PATCH 17/20] changed path to stop further away from the intake --- src/main/deploy/choreo/Depot_Fast.traj | 130 ++++++++++++------------- 1 file changed, 63 insertions(+), 67 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index e6025d32..105fa39b 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4981616735458374, "y":5.960968494415283, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.794022798538208, "y":5.955489635467529, "heading":3.141592653589793, "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}, @@ -15,8 +15,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4981616735458374 m", "val":1.4981616735458374}, "y":{"exp":"5.960968494415283 m", "val":5.960968494415283}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.794022798538208 m", "val":1.794022798538208}, "y":{"exp":"5.955489635467529 m", "val":5.955489635467529}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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}, @@ -53,70 +53,66 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,3.02302], + "waypoints":[0.0,2.83806], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.91847, "ay":0.39445, "alpha":0.00104, "fx":[-10.41911,-10.41903,-10.41152,-10.41149], "fy":[4.46361,4.50911,4.45558,4.46363]}, - {"t":0.04956, "x":3.59487, "y":5.06048, "heading":3.14159, "vx":-0.04552, "vy":0.01955, "omega":0.00005, "ax":-0.91865, "ay":0.39453, "alpha":-0.00042, "fx":[-10.42104,-10.42103,-10.41369,-10.41366], "fy":[4.47243,4.48613,4.46444,4.47246]}, - {"t":0.09912, "x":3.59149, "y":5.06194, "heading":-3.14159, "vx":-0.09104, "vy":0.0391, "omega":0.00003, "ax":-0.91865, "ay":0.39452, "alpha":-0.0007, "fx":[-10.40881,-10.40889,-10.40084,-10.45057], "fy":[4.50241,4.46162,4.46164,4.46964]}, - {"t":0.14867, "x":3.58585, "y":5.06436, "heading":-3.14159, "vx":-0.13657, "vy":0.05865, "omega":0.0, "ax":-0.91864, "ay":0.39452, "alpha":-0.00041, "fx":[-10.42093,-10.42091,-10.41348,-10.41345], "fy":[4.47222,4.48684,4.46384,4.47225]}, - {"t":0.19823, "x":3.57795, "y":5.06775, "heading":-3.14159, "vx":-0.1821, "vy":0.0782, "omega":-0.00002, "ax":-0.91863, "ay":0.39452, "alpha":-0.00036, "fx":[-10.40673,-10.4068,-10.39812,-10.45677], "fy":[4.50193,4.46146,4.46148,4.4701]}, - {"t":0.24779, "x":3.5678, "y":5.07211, "heading":-3.14159, "vx":-0.22762, "vy":0.09775, "omega":-0.00004, "ax":-0.91862, "ay":0.39451, "alpha":-0.00036, "fx":[-10.42103,-10.421,-10.41301,-10.41299], "fy":[4.47158,4.4892,4.46239,4.47161]}, - {"t":0.29735, "x":3.55539, "y":5.07744, "heading":-3.14159, "vx":-0.27315, "vy":0.11731, "omega":-0.00006, "ax":-0.91861, "ay":0.39451, "alpha":-0.00069, "fx":[-10.40563,-10.40571,-10.39607,-10.4602], "fy":[4.50887,4.4587,4.45872,4.46829]}, - {"t":0.3469, "x":3.54073, "y":5.08374, "heading":3.14159, "vx":-0.31867, "vy":0.13686, "omega":-0.00009, "ax":-0.9186, "ay":0.3945, "alpha":-0.00047, "fx":[-10.4213,-10.42126,-10.41231,-10.41229], "fy":[4.4716,4.48987,4.46126,4.47163]}, - {"t":0.39646, "x":3.52381, "y":5.091, "heading":3.14159, "vx":-0.36419, "vy":0.15641, "omega":-0.00012, "ax":-0.91859, "ay":0.3945, "alpha":-0.00014, "fx":[-10.40203,-10.40211,-10.3911,-10.47144], "fy":[4.50858,4.45819,4.45821,4.46913]}, - {"t":0.44602, "x":3.50463, "y":5.09924, "heading":3.14158, "vx":-0.40972, "vy":0.17596, "omega":-0.00012, "ax":-0.91858, "ay":0.39449, "alpha":-0.00022, "fx":[-10.42171,-10.42166,-10.41139,-10.41136], "fy":[4.46945,4.49727,4.45763,4.46948]}, - {"t":0.49558, "x":3.4832, "y":5.10844, "heading":3.14157, "vx":-0.45524, "vy":0.19551, "omega":-0.00013, "ax":-0.91857, "ay":0.39449, "alpha":0.00028, "fx":[-10.3978,-10.39789,-10.38529,-10.48455], "fy":[4.51203,4.45633,4.45636,4.46883]}, - {"t":0.54513, "x":3.45951, "y":5.11862, "heading":3.14157, "vx":-0.50076, "vy":0.21506, "omega":-0.00012, "ax":-0.91855, "ay":0.39448, "alpha":-0.00008, "fx":[-10.42217,-10.42211,-10.41031,-10.41028], "fy":[4.46783,4.50319,4.45436,4.46785]}, - {"t":0.59469, "x":3.43356, "y":5.12976, "heading":3.14156, "vx":-0.54628, "vy":0.23461, "omega":-0.00012, "ax":-0.91854, "ay":0.39447, "alpha":0.00042, "fx":[-10.39339,-10.39349,-10.3792,-10.49805], "fy":[4.52004,4.45288,4.4529,4.46704]}, - {"t":0.64425, "x":3.40536, "y":5.14187, "heading":3.14155, "vx":-0.5918, "vy":0.25416, "omega":-0.0001, "ax":-0.91852, "ay":0.39446, "alpha":-0.00043, "fx":[-10.42258,-10.42252,-10.40912,-10.40909], "fy":[4.46872,4.50143,4.45357,4.46874]}, - {"t":0.69381, "x":3.37491, "y":5.15495, "heading":3.14155, "vx":-0.63732, "vy":0.2737, "omega":-0.00013, "ax":-0.9185, "ay":0.39445, "alpha":0.00009, "fx":[-10.38911,-10.38924,-10.37325,-10.51078], "fy":[4.53492,4.44708,4.4471,4.46291]}, - {"t":0.74337, "x":3.34219, "y":5.169, "heading":3.14154, "vx":-0.68284, "vy":0.29325, "omega":-0.00012, "ax":-0.91848, "ay":0.39444, "alpha":0.00031, "fx":[-10.42295,-10.42286,-10.40778,-10.40775], "fy":[4.46362,4.51753,4.44669,4.46365]}, - {"t":0.79292, "x":3.30723, "y":5.18402, "heading":3.14154, "vx":-0.72836, "vy":0.3128, "omega":-0.00011, "ax":-0.91845, "ay":0.39443, "alpha":0.00175, "fx":[-10.37774,-10.37787,-10.36007,-10.54447], "fy":[4.53868,4.44487,4.44489,4.46246]}, - {"t":0.84248, "x":3.27, "y":5.2, "heading":3.14153, "vx":-0.77388, "vy":0.33235, "omega":-0.00002, "ax":-0.91842, "ay":0.39441, "alpha":0.00045, "fx":[-10.42304,-10.42294,-10.40641,-10.40638], "fy":[4.46181,4.52326,4.44332,4.46184]}, - {"t":0.89204, "x":3.23052, "y":5.21696, "heading":3.14153, "vx":-0.81939, "vy":0.35189, "omega":0.0, "ax":-0.91838, "ay":0.39439, "alpha":0.00312, "fx":[-10.36444,-10.3646,-10.34537,-10.58276], "fy":[4.55259,4.4393,4.43932,4.45824]}, - {"t":0.9416, "x":3.18879, "y":5.23488, "heading":3.14153, "vx":-0.8649, "vy":0.37144, "omega":0.00016, "ax":-0.91834, "ay":0.39437, "alpha":0.00133, "fx":[-10.42264,-10.4225,-10.40508,-10.40504], "fy":[4.4562,4.53946,4.43663,4.45622]}, - {"t":0.99115, "x":3.1448, "y":5.25377, "heading":3.14154, "vx":-0.91041, "vy":0.39098, "omega":0.00022, "ax":-0.91829, "ay":0.39435, "alpha":0.00549, "fx":[-10.3426,-10.34279,-10.32279,-10.6448], "fy":[4.57545,4.43076,4.43078,4.45038]}, - {"t":1.04071, "x":3.09855, "y":5.27363, "heading":3.14155, "vx":-0.95592, "vy":0.41053, "omega":0.0005, "ax":-0.91823, "ay":0.39432, "alpha":0.00299, "fx":[-10.4213,-10.42111,-10.40392,-10.40388], "fy":[4.4468,4.56526,4.42712,4.44682]}, - {"t":1.09027, "x":3.05005, "y":5.29446, "heading":3.14158, "vx":-1.00143, "vy":0.43007, "omega":0.00064, "ax":-0.91815, "ay":0.39428, "alpha":0.00444, "fx":[-10.32598,-10.32626,-10.30712,-10.68737], "fy":[4.63759,4.40933,4.40935,4.42801]}, - {"t":1.13983, "x":2.9993, "y":5.31626, "heading":-3.14158, "vx":-1.04693, "vy":0.44961, "omega":0.00086, "ax":-0.91805, "ay":0.39423, "alpha":0.00316, "fx":[-10.41831,-10.41813,-10.40292,-10.40289], "fy":[4.44588,4.56261,4.42771,4.44589]}, - {"t":1.18938, "x":2.94629, "y":5.33902, "heading":-3.14154, "vx":-1.09243, "vy":0.46914, "omega":0.00102, "ax":-0.91792, "ay":0.39417, "alpha":0.00604, "fx":[-10.31316,-10.31344,-10.29657,-10.71315], "fy":[4.64349,4.40639,4.4064,4.42278]}, - {"t":1.23894, "x":2.89102, "y":5.36276, "heading":-3.14148, "vx":-1.13792, "vy":0.48868, "omega":0.00132, "ax":-0.91774, "ay":0.39408, "alpha":0.00228, "fx":[-10.41262,-10.41246,-10.40145,-10.40142], "fy":[4.45096,4.53675,4.4363,4.45097]}, - {"t":1.2885, "x":2.8335, "y":5.38746, "heading":-3.14142, "vx":-1.1834, "vy":0.50821, "omega":0.00143, "ax":-0.91746, "ay":0.39394, "alpha":0.0013, "fx":[-10.34485,-10.34507,-10.33296,-10.59257], "fy":[4.61113,4.4153,4.41531,4.42707]}, - {"t":1.33806, "x":2.77373, "y":5.41313, "heading":-3.14135, "vx":-1.22887, "vy":0.52773, "omega":0.0015, "ax":-0.917, "ay":0.39371, "alpha":-0.00245, "fx":[-10.40077,-10.40069,-10.39656,-10.39655], "fy":[4.47571,4.44016,4.46695,4.47572]}, - {"t":1.38762, "x":2.7117, "y":5.43977, "heading":-3.14127, "vx":-1.27431, "vy":0.54724, "omega":0.00138, "ax":-0.91608, "ay":0.39327, "alpha":-0.01456, "fx":[-10.52649,-10.52647,-10.52403,-9.97578], "fy":[4.27676,4.51965,4.51966,4.52215]}, - {"t":1.43717, "x":2.64742, "y":5.46737, "heading":-3.14121, "vx":-1.31971, "vy":0.56673, "omega":0.00065, "ax":-0.91331, "ay":0.39196, "alpha":-0.063, "fx":[-10.35199,-10.35221,-10.36142,-10.3614], "fy":[4.78746,3.40604,4.79785,4.78745]}, - {"t":1.48673, "x":2.5809, "y":5.49594, "heading":-3.14117, "vx":-1.36497, "vy":0.58616, "omega":-0.00247, "ax":0.00064, "ay":0.00202, "alpha":-0.00829, "fx":[-0.05766,-0.05764,-0.05508,0.19953], "fy":[-0.04097,0.04327,0.04329,0.04584]}, - {"t":1.53629, "x":2.51326, "y":5.52499, "heading":-3.1413, "vx":-1.36494, "vy":0.58626, "omega":-0.00288, "ax":0.91322, "ay":-0.39217, "alpha":0.06468, "fx":[10.35857,10.35877,10.35284,10.35274], "fy":[-4.79133,-3.40326,-4.80247,-4.79126]}, - {"t":1.58585, "x":2.44674, "y":5.55356, "heading":-3.14144, "vx":-1.31968, "vy":0.56682, "omega":0.00033, "ax":0.91596, "ay":-0.39355, "alpha":0.01625, "fx":[10.53576,10.53577,10.5325,9.94326], "fy":[-4.2743,-4.52447,-4.52444,-4.52779]}, - {"t":1.6354, "x":2.38246, "y":5.58117, "heading":-3.14142, "vx":-1.27429, "vy":0.54732, "omega":0.00113, "ax":0.91692, "ay":-0.39391, "alpha":0.00439, "fx":[10.40748,10.40742,10.38794,10.38791], "fy":[-4.48066,-4.43553,-4.47059,-4.48065]}, - {"t":1.68496, "x":2.32044, "y":5.60781, "heading":-3.14137, "vx":-1.22885, "vy":0.5278, "omega":0.00135, "ax":0.9174, "ay":-0.39409, "alpha":-0.0002, "fx":[10.34911,10.34935,10.336,10.57808], "fy":[-4.6143,-4.41608,-4.41607,-4.42909]}, - {"t":1.73452, "x":2.26066, "y":5.63348, "heading":-3.1413, "vx":-1.18338, "vy":0.50827, "omega":0.00134, "ax":0.91769, "ay":-0.3942, "alpha":-0.00042, "fx":[10.42,10.41984,10.39291,10.39288], "fy":[-4.45436,-4.53383,-4.43785,-4.45437]}, - {"t":1.78408, "x":2.20314, "y":5.65818, "heading":-3.14123, "vx":-1.13791, "vy":0.48873, "omega":0.00132, "ax":0.91788, "ay":-0.39426, "alpha":-0.01629, "fx":[10.26469,10.26494,10.24518,10.85959], "fy":[-4.62063,-4.41459,-4.41461,-4.43371]}, - {"t":1.83364, "x":2.14788, "y":5.68192, "heading":-3.14117, "vx":-1.09242, "vy":0.46919, "omega":0.00051, "ax":0.91802, "ay":-0.39432, "alpha":-0.0016, "fx":[10.42593,10.42574,10.3945,10.39447], "fy":[-4.44722,-4.56434,-4.42702,-4.44724]}, - {"t":1.88319, "x":2.09487, "y":5.70469, "heading":-3.14114, "vx":-1.04692, "vy":0.44965, "omega":0.00043, "ax":0.91812, "ay":-0.39435, "alpha":-0.01067, "fx":[10.30158,10.30182,10.28037,10.76154], "fy":[-4.60799,-4.41955,-4.41958,-4.44046]}, - {"t":1.93275, "x":2.04411, "y":5.72649, "heading":-3.14112, "vx":-1.00142, "vy":0.43011, "omega":-0.0001, "ax":0.9182, "ay":-0.39438, "alpha":-0.0015, "fx":[10.42857,10.42837,10.39602,10.39599], "fy":[-4.44794,-4.56614,-4.42686,-4.44797]}, - {"t":1.98231, "x":1.99561, "y":5.74732, "heading":-3.14113, "vx":-0.95592, "vy":0.41056, "omega":-0.00017, "ax":0.91827, "ay":-0.39441, "alpha":-0.00468, "fx":[10.34502,10.34521,10.32422,10.63741], "fy":[-4.58005,-4.42976,-4.42979,-4.45039]}, - {"t":2.03187, "x":1.94937, "y":5.76718, "heading":-3.14113, "vx":-0.91041, "vy":0.39102, "omega":-0.0004, "ax":0.91832, "ay":-0.39443, "alpha":0.00026, "fx":[10.42943,10.42929,10.39778,10.39775], "fy":[-4.45833,-4.53604,-4.43815,-4.45836]}, - {"t":2.08142, "x":1.90538, "y":5.78607, "heading":-3.14115, "vx":-0.8649, "vy":0.37147, "omega":-0.00039, "ax":0.91836, "ay":-0.39444, "alpha":-0.0028, "fx":[10.35851,10.35871,10.33911,10.59991], "fy":[-4.57482,-4.43249,-4.43252,-4.45178]}, - {"t":2.13098, "x":1.86364, "y":5.804, "heading":-3.14117, "vx":-0.81939, "vy":0.35192, "omega":-0.00053, "ax":0.9184, "ay":-0.39446, "alpha":0.00071, "fx":[10.42949,10.42937,10.39954,10.39951], "fy":[-4.46193,-4.52496,-4.44336,-4.46196]}, - {"t":2.18054, "x":1.82416, "y":5.82095, "heading":-3.1412, "vx":-0.77388, "vy":0.33237, "omega":-0.00049, "ax":0.91843, "ay":-0.39447, "alpha":-0.00253, "fx":[10.37266,10.37281,10.35504,10.55885], "fy":[-4.54114,-4.44468,-4.44471,-4.46222]}, - {"t":2.2301, "x":1.78694, "y":5.83694, "heading":-3.14122, "vx":-0.72836, "vy":0.31282, "omega":-0.00062, "ax":0.91846, "ay":-0.39448, "alpha":0.00099, "fx":[10.42907,10.42897,10.4013,10.40127], "fy":[-4.46475,-4.5155,-4.44819,-4.46477]}, - {"t":2.27965, "x":1.75197, "y":5.85196, "heading":-3.14125, "vx":-0.68284, "vy":0.29328, "omega":-0.00057, "ax":0.91848, "ay":-0.39449, "alpha":-0.00093, "fx":[10.38666,10.38679,10.37126,10.51698], "fy":[-4.52914,-4.4497,-4.44972,-4.46506]}, - {"t":2.32921, "x":1.71926, "y":5.86601, "heading":-3.14128, "vx":-0.63733, "vy":0.27373, "omega":-0.00061, "ax":0.9185, "ay":-0.39449, "alpha":0.00085, "fx":[10.4284,10.42832,10.40298,10.40295], "fy":[-4.46537,-4.51222,-4.45099,-4.46539]}, - {"t":2.37877, "x":1.6888, "y":5.87909, "heading":-3.14131, "vx":-0.59181, "vy":0.25418, "omega":-0.00057, "ax":0.91852, "ay":-0.3945, "alpha":-0.00047, "fx":[10.39307,10.39318,10.37988,10.4974], "fy":[-4.52106,-4.45334,-4.45336,-4.46652]}, - {"t":2.42833, "x":1.6606, "y":5.8912, "heading":-3.14134, "vx":-0.54629, "vy":0.23462, "omega":-0.0006, "ax":0.91854, "ay":-0.39451, "alpha":0.00097, "fx":[10.42764,10.42758,10.40455,10.40451], "fy":[-4.46731,-4.50483,-4.45511,-4.46733]}, - {"t":2.47789, "x":1.63466, "y":5.90235, "heading":-3.14137, "vx":-0.50077, "vy":0.21507, "omega":-0.00055, "ax":0.91856, "ay":-0.39451, "alpha":0.00018, "fx":[10.39825,10.39836,10.3872,10.48117], "fy":[-4.51944,-4.45477,-4.45479,-4.46583]}, - {"t":2.52744, "x":1.61097, "y":5.91252, "heading":-3.1414, "vx":-0.45524, "vy":0.19552, "omega":-0.00054, "ax":0.91857, "ay":-0.39452, "alpha":0.00108, "fx":[10.42691,10.42686,10.40593,10.40589], "fy":[-4.46905,-4.49808,-4.45885,-4.46908]}, - {"t":2.577, "x":1.58953, "y":5.92172, "heading":-3.14142, "vx":-0.40972, "vy":0.17597, "omega":-0.00049, "ax":0.91858, "ay":-0.39452, "alpha":0.0001, "fx":[10.4018,10.4019,10.39259,10.46987], "fy":[-4.51023,-4.45859,-4.45861,-4.46783]}, - {"t":2.62656, "x":1.57036, "y":5.92996, "heading":-3.14145, "vx":-0.3642, "vy":0.15642, "omega":-0.00048, "ax":0.91859, "ay":-0.39453, "alpha":0.00096, "fx":[10.4263,10.42625,10.40708,10.40705], "fy":[-4.46941,-4.4957,-4.46091,-4.46943]}, - {"t":2.67612, "x":1.55344, "y":5.93723, "heading":-3.14147, "vx":-0.31868, "vy":0.13687, "omega":-0.00043, "ax":0.9186, "ay":-0.39453, "alpha":0.00015, "fx":[10.40379,10.40387,10.39614,10.46336], "fy":[-4.5075,-4.46014,-4.46016,-4.46782]}, - {"t":2.72567, "x":1.53877, "y":5.94353, "heading":-3.14149, "vx":-0.27315, "vy":0.11732, "omega":-0.00043, "ax":0.91861, "ay":-0.39453, "alpha":0.00111, "fx":[10.42581,10.42578,10.40801,10.40799], "fy":[-4.47099,-4.4899,-4.4639,-4.47101]}, - {"t":2.77523, "x":1.52636, "y":5.94886, "heading":-3.14152, "vx":-0.22763, "vy":0.09776, "omega":-0.00037, "ax":0.91862, "ay":-0.39454, "alpha":0.00006, "fx":[10.40588,10.40595,10.39942,10.45674], "fy":[-4.50164,-4.46259,-4.46261,-4.46909]}, - {"t":2.82479, "x":1.51621, "y":5.95322, "heading":-3.14153, "vx":-0.1821, "vy":0.07821, "omega":-0.00037, "ax":0.91863, "ay":-0.39454, "alpha":0.00119, "fx":[10.42552,10.4255,10.40869,10.40866], "fy":[-4.47196,-4.48625,-4.46589,-4.47199]}, - {"t":2.87435, "x":1.50831, "y":5.95661, "heading":-3.14155, "vx":-0.13658, "vy":0.05866, "omega":-0.00031, "ax":0.91864, "ay":-0.39454, "alpha":0.00041, "fx":[10.40802,10.40809,10.40237,10.45024], "fy":[-4.50231,-4.46272,-4.46275,-4.46843]}, - {"t":2.9239, "x":1.50267, "y":5.95903, "heading":-3.14157, "vx":-0.09105, "vy":0.03911, "omega":-0.00029, "ax":0.91865, "ay":-0.39455, "alpha":0.00118, "fx":[10.42547,10.42545,10.40907,10.40905], "fy":[-4.4722,-4.48522,-4.46667,-4.47223]}, - {"t":2.97346, "x":1.49929, "y":5.96048, "heading":-3.14158, "vx":-0.04553, "vy":0.01955, "omega":-0.00023, "ax":0.91865, "ay":-0.39455, "alpha":0.00465, "fx":[10.40592,10.46221,10.40062,10.40059], "fy":[-4.48231,-4.46582,-4.46597,-4.48234]}, - {"t":3.02302, "x":1.49816, "y":5.96097, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.89515, "ay":0.44484, "alpha":0.0006, "fx":[-10.15786,-10.15776,-10.14391,-10.14387], "fy":[5.03423,5.08504,5.02426,5.03427]}, + {"t":0.04979, "x":3.59489, "y":5.06055, "heading":3.14159, "vx":-0.04457, "vy":0.02215, "omega":0.00003, "ax":-0.89533, "ay":0.44493, "alpha":-0.00099, "fx":[-10.15966,-10.15964,-10.14606,-10.14603], "fy":[5.04392,5.05972,5.03417,5.04396]}, + {"t":0.09958, "x":3.59156, "y":5.06221, "heading":-3.14159, "vx":-0.08915, "vy":0.0443, "omega":-0.00002, "ax":-0.89532, "ay":0.44493, "alpha":-0.00085, "fx":[-10.14327,-10.14336,-10.13366,-10.19077], "fy":[5.07785,5.03135,5.03138,5.04101]}, + {"t":0.14937, "x":3.58601, "y":5.06496, "heading":-3.14159, "vx":-0.13373, "vy":0.06646, "omega":-0.00006, "ax":-0.89531, "ay":0.44492, "alpha":-0.00082, "fx":[-10.15947,-10.15944,-10.14592,-10.14588], "fy":[5.04295,5.06243,5.03306,5.04299]}, + {"t":0.19916, "x":3.57824, "y":5.06882, "heading":3.14159, "vx":-0.17831, "vy":0.08861, "omega":-0.0001, "ax":-0.8953, "ay":0.44492, "alpha":-0.00043, "fx":[-10.13996,-10.14006,-10.12995,-10.20035], "fy":[5.07984,5.03044,5.03047,5.04048]}, + {"t":0.24895, "x":3.56826, "y":5.07379, "heading":3.14159, "vx":-0.22288, "vy":0.11076, "omega":-0.00012, "ax":-0.89529, "ay":0.44492, "alpha":-0.00066, "fx":[-10.15953,-10.15948,-10.14546,-10.14542], "fy":[5.04177,5.06619,5.03126,5.0418]}, + {"t":0.29874, "x":3.55605, "y":5.07985, "heading":3.14158, "vx":-0.26746, "vy":0.13291, "omega":-0.00016, "ax":-0.89528, "ay":0.44491, "alpha":-0.00024, "fx":[-10.13717,-10.13728,-10.12635,-10.20864], "fy":[5.08362,5.02877,5.02879,5.03961]}, + {"t":0.34853, "x":3.54162, "y":5.08702, "heading":3.14157, "vx":-0.31204, "vy":0.15507, "omega":-0.00017, "ax":-0.89527, "ay":0.4449, "alpha":-0.0005, "fx":[-10.15977,-10.1597,-10.14475,-10.14472], "fy":[5.04029,5.07114,5.02879,5.04033]}, + {"t":0.39832, "x":3.52498, "y":5.09529, "heading":3.14156, "vx":-0.35661, "vy":0.17722, "omega":-0.00019, "ax":-0.89526, "ay":0.4449, "alpha":-0.00039, "fx":[-10.13476,-10.13487,-10.12282,-10.21593], "fy":[5.09121,5.02569,5.02572,5.03764]}, + {"t":0.44811, "x":3.50611, "y":5.10467, "heading":3.14155, "vx":-0.40119, "vy":0.19937, "omega":-0.00021, "ax":-0.89525, "ay":0.44489, "alpha":-0.00049, "fx":[-10.16011,-10.16004,-10.14382,-10.14379], "fy":[5.03945,5.07434,5.02668,5.03949]}, + {"t":0.49791, "x":3.48503, "y":5.11515, "heading":3.14154, "vx":-0.44576, "vy":0.22152, "omega":-0.00024, "ax":-0.89523, "ay":0.44488, "alpha":-0.00047, "fx":[-10.13183,-10.13196,-10.11852,-10.22478], "fy":[5.09886,5.02247,5.0225,5.03579]}, + {"t":0.5477, "x":3.46172, "y":5.12673, "heading":3.14153, "vx":-0.49034, "vy":0.24367, "omega":-0.00026, "ax":-0.89522, "ay":0.44488, "alpha":-0.00017, "fx":[-10.16055,-10.16046,-10.14268,-10.14265], "fy":[5.03677,5.0832,5.02247,5.0368]}, + {"t":0.59749, "x":3.4362, "y":5.13941, "heading":3.14152, "vx":-0.53491, "vy":0.26582, "omega":-0.00027, "ax":-0.8952, "ay":0.44487, "alpha":-0.00023, "fx":[-10.12714,-10.12729,-10.11224,-10.23881], "fy":[5.10595,5.01932,5.01934,5.0342]}, + {"t":0.64728, "x":3.40845, "y":5.1532, "heading":3.1415, "vx":-0.57948, "vy":0.28797, "omega":-0.00028, "ax":-0.89518, "ay":0.44486, "alpha":-0.00014, "fx":[-10.16095,-10.16085,-10.14138,-10.14134], "fy":[5.03554,5.08759,5.01962,5.03558]}, + {"t":0.69707, "x":3.37849, "y":5.16809, "heading":3.14149, "vx":-0.62405, "vy":0.31012, "omega":-0.00029, "ax":-0.89515, "ay":0.44484, "alpha":0.00116, "fx":[-10.11526,-10.11542,-10.09868,-10.27405], "fy":[5.11585,5.01513,5.01516,5.03164]}, + {"t":0.74686, "x":3.34631, "y":5.18408, "heading":3.14148, "vx":-0.66862, "vy":0.33227, "omega":-0.00023, "ax":-0.89512, "ay":0.44483, "alpha":0.00065, "fx":[-10.1612,-10.16106,-10.13995,-10.13991], "fy":[5.03016,5.10412,5.01266,5.03019]}, + {"t":0.79665, "x":3.31191, "y":5.20118, "heading":3.14146, "vx":-0.71319, "vy":0.35442, "omega":-0.0002, "ax":-0.89509, "ay":0.44481, "alpha":0.00163, "fx":[-10.1077,-10.10789,-10.08973,-10.29532], "fy":[5.12706,5.01048,5.0105,5.02835]}, + {"t":0.84644, "x":3.27529, "y":5.21938, "heading":3.14145, "vx":-0.75776, "vy":0.37657, "omega":-0.00012, "ax":-0.89505, "ay":0.44479, "alpha":0.00211, "fx":[-10.16104,-10.16085,-10.13852,-10.13848], "fy":[5.0213,5.13033,5.00256,5.02133]}, + {"t":0.89623, "x":3.23645, "y":5.23868, "heading":3.14145, "vx":-0.80233, "vy":0.39871, "omega":-0.00001, "ax":-0.89501, "ay":0.44477, "alpha":0.0027, "fx":[-10.08941,-10.08965,-10.07061,-10.34711], "fy":[5.16049,4.99843,4.99845,5.01709]}, + {"t":0.94602, "x":3.19539, "y":5.25908, "heading":3.14145, "vx":-0.84689, "vy":0.42086, "omega":0.00012, "ax":-0.89495, "ay":0.44474, "alpha":0.00306, "fx":[-10.16005,-10.15982,-10.13718,-10.13714], "fy":[5.01546,5.14593,4.99631,5.01548]}, + {"t":0.99581, "x":3.15212, "y":5.28059, "heading":3.14145, "vx":-0.89145, "vy":0.443, "omega":0.00028, "ax":-0.89488, "ay":0.44471, "alpha":0.00464, "fx":[-10.0688,-10.06909,-10.05014,-10.40294], "fy":[5.18618,4.98896,4.98898,5.00744]}, + {"t":1.0456, "x":3.10662, "y":5.3032, "heading":3.14147, "vx":-0.936, "vy":0.46515, "omega":0.00051, "ax":-0.89479, "ay":0.44466, "alpha":0.00596, "fx":[-10.15772,-10.15741,-10.13587,-10.13583], "fy":[4.99952,5.18929,4.98114,4.99954]}, + {"t":1.09539, "x":3.05891, "y":5.32691, "heading":3.14149, "vx":-0.98056, "vy":0.48729, "omega":0.0008, "ax":-0.89466, "ay":0.4446, "alpha":0.00707, "fx":[-10.02989,-10.03027,-10.0131,-10.50804], "fy":[5.25701,4.96439,4.9644,4.98093]}, + {"t":1.14518, "x":3.00898, "y":5.35172, "heading":3.14153, "vx":-1.0251, "vy":0.50942, "omega":0.00116, "ax":-0.89449, "ay":0.44452, "alpha":0.00491, "fx":[-10.15253,-10.15226,-10.13441,-10.13438], "fy":[5.00536,5.16199,4.99015,5.00537]}, + {"t":1.19497, "x":2.95683, "y":5.37764, "heading":3.14159, "vx":-1.06964, "vy":0.53156, "omega":0.0014, "ax":-0.89424, "ay":0.44439, "alpha":0.0024, "fx":[-10.05865,-10.05896,-10.04656,-10.39781], "fy":[5.23379,4.97044,4.97044,4.98238]}, + {"t":1.24476, "x":2.90246, "y":5.40465, "heading":-3.14152, "vx":-1.11416, "vy":0.55368, "omega":0.00152, "ax":-0.89381, "ay":0.44418, "alpha":-0.00119, "fx":[-10.14127,-10.14112,-10.13011,-10.13009], "fy":[5.0377,5.04358,5.02854,5.03771]}, + {"t":1.29455, "x":2.84588, "y":5.43277, "heading":-3.14145, "vx":-1.15867, "vy":0.5758, "omega":0.00146, "ax":-0.89296, "ay":0.44375, "alpha":-0.012, "fx":[-10.23772,-10.23772,-10.2329,-9.79578], "fy":[4.88531,5.07927,5.07928,5.0841]}, + {"t":1.34434, "x":2.78708, "y":5.46199, "heading":-3.14138, "vx":-1.20313, "vy":0.59789, "omega":0.00086, "ax":-0.89054, "ay":0.44222, "alpha":-0.07668, "fx":[-10.0965,-10.09681,-10.10034,-10.10037], "fy":[5.42816,3.76201,5.44061,5.42818]}, + {"t":1.39413, "x":2.72607, "y":5.49231, "heading":-3.14133, "vx":-1.24747, "vy":0.61991, "omega":-0.00295, "ax":-0.0001, "ay":0.00026, "alpha":0.00011, "fx":[0.00102,0.00102,0.00047,-0.0071], "fy":[0.00817,0.00132,0.00132,0.00077]}, + {"t":1.44393, "x":2.66396, "y":5.52317, "heading":-3.14148, "vx":-1.24747, "vy":0.61992, "omega":-0.00295, "ax":0.89049, "ay":-0.44232, "alpha":0.07271, "fx":[10.09705,10.09733,10.09876,10.09876], "fy":[-5.4069,-3.83156,-5.41786,-5.40689]}, + {"t":1.49372, "x":2.60295, "y":5.55349, "heading":3.14156, "vx":-1.20314, "vy":0.5979, "omega":0.00067, "ax":0.89296, "ay":-0.44376, "alpha":0.01214, "fx":[10.24327,10.24325,10.23736,9.77993], "fy":[-4.86991,-5.08424,-5.08425,-5.09015]}, + {"t":1.54351, "x":2.54415, "y":5.58271, "heading":3.14159, "vx":-1.15868, "vy":0.5758, "omega":0.00128, "ax":0.89381, "ay":-0.44418, "alpha":0.00133, "fx":[10.142,10.14185,10.12933,10.12933], "fy":[-5.03777,-5.04443,-5.02771,-5.03779]}, + {"t":1.5933, "x":2.48757, "y":5.61083, "heading":-3.14153, "vx":-1.11417, "vy":0.55369, "omega":0.00134, "ax":0.89424, "ay":-0.44439, "alpha":-0.00096, "fx":[10.06206,10.06237,10.04925,10.38815], "fy":[-5.24599,-4.96621,-4.96623,-4.9789]}, + {"t":1.64309, "x":2.4332, "y":5.63785, "heading":-3.14146, "vx":-1.06965, "vy":0.53156, "omega":0.00129, "ax":0.89449, "ay":-0.44452, "alpha":-0.00398, "fx":[10.1531,10.15283,10.13377,10.13376], "fy":[-5.00977,-5.14966,-4.99387,-5.00979]}, + {"t":1.69288, "x":2.38105, "y":5.66376, "heading":-3.1414, "vx":-1.02511, "vy":0.50943, "omega":0.0011, "ax":0.89466, "ay":-0.4446, "alpha":-0.006, "fx":[10.03272,10.0331,10.01524,10.50015], "fy":[-5.26494,-4.96156,-4.96159,-4.97883]}, + {"t":1.74267, "x":2.33112, "y":5.68858, "heading":-3.14134, "vx":-0.98057, "vy":0.48729, "omega":0.0008, "ax":0.89478, "ay":-0.44466, "alpha":-0.00497, "fx":[10.15832,10.15801,10.13521,10.1352], "fy":[-5.00427,-5.17589,-4.98518,-5.00431]}, + {"t":1.79246, "x":2.28341, "y":5.71229, "heading":-3.1413, "vx":-0.93601, "vy":0.46515, "omega":0.00055, "ax":0.89488, "ay":-0.44471, "alpha":-0.00986, "fx":[10.04515,10.04539,10.02529,10.47507], "fy":[-5.17186,-4.99342,-4.99345,-5.01297]}, + {"t":1.84225, "x":2.23791, "y":5.7349, "heading":-3.14128, "vx":-0.89146, "vy":0.44301, "omega":0.00006, "ax":0.89495, "ay":-0.44475, "alpha":-0.00284, "fx":[10.16062,10.16037,10.13658,10.13656], "fy":[-5.01613,-5.14465,-4.99635,-5.01617]}, + {"t":1.89204, "x":2.19464, "y":5.7564, "heading":-3.14127, "vx":-0.8469, "vy":0.42087, "omega":-0.00008, "ax":0.895, "ay":-0.44477, "alpha":-0.00232, "fx":[10.09028,10.09052,10.07104,10.34487], "fy":[-5.16349,-4.99731,-4.99734,-5.01644]}, + {"t":1.94183, "x":2.15358, "y":5.77681, "heading":-3.14128, "vx":-0.80233, "vy":0.39872, "omega":-0.0002, "ax":0.89505, "ay":-0.4448, "alpha":-0.002, "fx":[10.16141,10.1612,10.13812,10.1381], "fy":[-5.02157,-5.1299,-5.00253,-5.02161]}, + {"t":1.99162, "x":2.11474, "y":5.79611, "heading":-3.14129, "vx":-0.75777, "vy":0.37657, "omega":-0.0003, "ax":0.89509, "ay":-0.44482, "alpha":-0.00134, "fx":[10.10854,10.10872,10.09039,10.29294], "fy":[-5.12894,-5.00982,-5.00985,-5.02788]}, + {"t":2.04141, "x":2.07812, "y":5.81431, "heading":-3.1413, "vx":-0.7132, "vy":0.35443, "omega":-0.00036, "ax":0.89512, "ay":-0.44483, "alpha":-0.00035, "fx":[10.16141,10.16126,10.13972,10.1397], "fy":[-5.0316,-5.09995,-5.01405,-5.03163]}, + {"t":2.0912, "x":2.04372, "y":5.8314, "heading":-3.14132, "vx":-0.66863, "vy":0.33228, "omega":-0.00038, "ax":0.89515, "ay":-0.44485, "alpha":-0.00084, "fx":[10.11538,10.11554,10.09885,10.27359], "fy":[-5.1206,-5.01359,-5.01362,-5.03005]}, + {"t":2.14099, "x":2.01154, "y":5.8474, "heading":-3.14134, "vx":-0.62406, "vy":0.31013, "omega":-0.00042, "ax":0.89518, "ay":-0.44486, "alpha":0.00014, "fx":[10.16105,10.16093,10.14126,10.14123], "fy":[-5.03549,-5.0877,-5.01969,-5.03552]}, + {"t":2.19078, "x":1.98157, "y":5.86229, "heading":-3.14136, "vx":-0.57949, "vy":0.28798, "omega":-0.00042, "ax":0.8952, "ay":-0.44487, "alpha":0.00004, "fx":[10.12511,10.12526,10.11041,10.24467], "fy":[-5.10924,-5.01831,-5.01834,-5.03299]}, + {"t":2.24057, "x":1.95383, "y":5.87607, "heading":-3.14138, "vx":-0.53492, "vy":0.26583, "omega":-0.00041, "ax":0.89522, "ay":-0.44488, "alpha":0.00015, "fx":[10.16053,10.16044,10.14268,10.14265], "fy":[-5.03673,-5.08309,-5.02273,-5.03676]}, + {"t":2.29036, "x":1.92831, "y":5.88876, "heading":-3.1414, "vx":-0.49035, "vy":0.24368, "omega":-0.00041, "ax":0.89523, "ay":-0.44489, "alpha":-0.00003, "fx":[10.13,10.13013,10.11706,10.22987], "fy":[-5.09668,-5.02335,-5.02337,-5.03628]}, + {"t":2.34016, "x":1.905, "y":5.90034, "heading":-3.14142, "vx":-0.44577, "vy":0.22153, "omega":-0.00041, "ax":0.89525, "ay":-0.44489, "alpha":0.00042, "fx":[10.15996,10.15988,10.14397,10.14394], "fy":[-5.03924,-5.07451,-5.02699,-5.03928]}, + {"t":2.38995, "x":1.88392, "y":5.91082, "heading":-3.14144, "vx":-0.4012, "vy":0.19938, "omega":-0.00039, "ax":0.89526, "ay":-0.4449, "alpha":-0.00032, "fx":[10.13315,10.13326,10.12184,10.2201], "fy":[-5.08531,-5.02789,-5.02792,-5.0392]}, + {"t":2.43974, "x":1.86505, "y":5.92019, "heading":-3.14146, "vx":-0.35662, "vy":0.17722, "omega":-0.0004, "ax":0.89527, "ay":-0.44491, "alpha":0.00035, "fx":[10.15941,10.15934,10.14509,10.14506], "fy":[-5.03989,-5.07156,-5.02923,-5.03992]}, + {"t":2.48953, "x":1.8484, "y":5.92847, "heading":-3.14148, "vx":-0.31205, "vy":0.15507, "omega":-0.00039, "ax":0.89528, "ay":-0.44491, "alpha":0.00027, "fx":[10.13699,10.1371,10.12723,10.20809], "fy":[-5.08572,-5.02844,-5.02846,-5.03823]}, + {"t":2.53932, "x":1.83398, "y":5.93564, "heading":-3.1415, "vx":-0.26747, "vy":0.13292, "omega":-0.00037, "ax":0.89529, "ay":-0.44492, "alpha":0.00057, "fx":[10.15893,10.15888,10.14604,10.14601], "fy":[-5.04185,-5.06475,-5.03258,-5.04188]}, + {"t":2.58911, "x":1.82177, "y":5.9417, "heading":-3.14152, "vx":-0.22289, "vy":0.11077, "omega":-0.00034, "ax":0.8953, "ay":-0.44492, "alpha":0.00037, "fx":[10.13949,10.13959,10.13092,10.20029], "fy":[-5.08184,-5.03027,-5.0303,-5.03887]}, + {"t":2.6389, "x":1.81178, "y":5.94667, "heading":-3.14154, "vx":-0.17831, "vy":0.08861, "omega":-0.00033, "ax":0.89531, "ay":-0.44493, "alpha":0.00064, "fx":[10.15862,10.15858,10.14675,10.14672], "fy":[-5.04284,-5.06117,-5.03459,-5.04287]}, + {"t":2.68869, "x":1.80401, "y":5.95053, "heading":-3.14155, "vx":-0.13374, "vy":0.06646, "omega":-0.00029, "ax":0.89532, "ay":-0.44493, "alpha":0.00054, "fx":[10.14194,10.14204,10.13421,10.19285], "fy":[-5.07861,-5.03174,-5.03177,-5.03953]}, + {"t":2.73848, "x":1.79846, "y":5.95328, "heading":-3.14157, "vx":-0.08916, "vy":0.04431, "omega":-0.00027, "ax":0.89533, "ay":-0.44493, "alpha":0.00067, "fx":[10.15852,10.1585,10.14719,10.14716], "fy":[-5.04338,-5.05928,-5.03573,-5.04342]}, + {"t":2.78827, "x":1.79513, "y":5.95494, "heading":-3.14158, "vx":-0.04458, "vy":0.02215, "omega":-0.00023, "ax":0.89533, "ay":-0.44494, "alpha":0.00468, "fx":[10.14082,10.20396,10.13346,10.13343], "fy":[-5.05112,-5.03975,-5.03993,-5.05116]}, + {"t":2.83806, "x":1.79402, "y":5.95549, "heading":3.14159, "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 fd755f40c08b7a24e91380d3997059dceb06b783 Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 20:20:19 -0400 Subject: [PATCH 18/20] 1.8 max velocity, 0.8 max acceleration --- src/main/deploy/choreo/Depot_Fast.traj | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index 105fa39b..473713e4 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -21,8 +21,8 @@ {"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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.8 m / s", "val":1.8}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"0.8 m / s ^ 2", "val":0.8}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 From 314af1fe1d014a7b900c52dfc90cb6ac96576daa Mon Sep 17 00:00:00 2001 From: Michael Kovalev Date: Thu, 2 Apr 2026 20:20:54 -0400 Subject: [PATCH 19/20] generated the path --- src/main/deploy/choreo/Depot_Fast.traj | 133 +++++++++++++------------ 1 file changed, 70 insertions(+), 63 deletions(-) diff --git a/src/main/deploy/choreo/Depot_Fast.traj b/src/main/deploy/choreo/Depot_Fast.traj index 473713e4..aca4ea83 100644 --- a/src/main/deploy/choreo/Depot_Fast.traj +++ b/src/main/deploy/choreo/Depot_Fast.traj @@ -3,19 +3,19 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.794022798538208, "y":5.955489635467529, "heading":3.141592653589793, "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":16.541, "h":8.0692}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}, - {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.8}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":0.8}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":57, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.794022798538208 m", "val":1.794022798538208}, "y":{"exp":"5.955489635467529 m", "val":5.955489635467529}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -53,66 +53,73 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,2.83806], + "waypoints":[0.0,3.17257], "samples":[ - {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.89515, "ay":0.44484, "alpha":0.0006, "fx":[-10.15786,-10.15776,-10.14391,-10.14387], "fy":[5.03423,5.08504,5.02426,5.03427]}, - {"t":0.04979, "x":3.59489, "y":5.06055, "heading":3.14159, "vx":-0.04457, "vy":0.02215, "omega":0.00003, "ax":-0.89533, "ay":0.44493, "alpha":-0.00099, "fx":[-10.15966,-10.15964,-10.14606,-10.14603], "fy":[5.04392,5.05972,5.03417,5.04396]}, - {"t":0.09958, "x":3.59156, "y":5.06221, "heading":-3.14159, "vx":-0.08915, "vy":0.0443, "omega":-0.00002, "ax":-0.89532, "ay":0.44493, "alpha":-0.00085, "fx":[-10.14327,-10.14336,-10.13366,-10.19077], "fy":[5.07785,5.03135,5.03138,5.04101]}, - {"t":0.14937, "x":3.58601, "y":5.06496, "heading":-3.14159, "vx":-0.13373, "vy":0.06646, "omega":-0.00006, "ax":-0.89531, "ay":0.44492, "alpha":-0.00082, "fx":[-10.15947,-10.15944,-10.14592,-10.14588], "fy":[5.04295,5.06243,5.03306,5.04299]}, - {"t":0.19916, "x":3.57824, "y":5.06882, "heading":3.14159, "vx":-0.17831, "vy":0.08861, "omega":-0.0001, "ax":-0.8953, "ay":0.44492, "alpha":-0.00043, "fx":[-10.13996,-10.14006,-10.12995,-10.20035], "fy":[5.07984,5.03044,5.03047,5.04048]}, - {"t":0.24895, "x":3.56826, "y":5.07379, "heading":3.14159, "vx":-0.22288, "vy":0.11076, "omega":-0.00012, "ax":-0.89529, "ay":0.44492, "alpha":-0.00066, "fx":[-10.15953,-10.15948,-10.14546,-10.14542], "fy":[5.04177,5.06619,5.03126,5.0418]}, - {"t":0.29874, "x":3.55605, "y":5.07985, "heading":3.14158, "vx":-0.26746, "vy":0.13291, "omega":-0.00016, "ax":-0.89528, "ay":0.44491, "alpha":-0.00024, "fx":[-10.13717,-10.13728,-10.12635,-10.20864], "fy":[5.08362,5.02877,5.02879,5.03961]}, - {"t":0.34853, "x":3.54162, "y":5.08702, "heading":3.14157, "vx":-0.31204, "vy":0.15507, "omega":-0.00017, "ax":-0.89527, "ay":0.4449, "alpha":-0.0005, "fx":[-10.15977,-10.1597,-10.14475,-10.14472], "fy":[5.04029,5.07114,5.02879,5.04033]}, - {"t":0.39832, "x":3.52498, "y":5.09529, "heading":3.14156, "vx":-0.35661, "vy":0.17722, "omega":-0.00019, "ax":-0.89526, "ay":0.4449, "alpha":-0.00039, "fx":[-10.13476,-10.13487,-10.12282,-10.21593], "fy":[5.09121,5.02569,5.02572,5.03764]}, - {"t":0.44811, "x":3.50611, "y":5.10467, "heading":3.14155, "vx":-0.40119, "vy":0.19937, "omega":-0.00021, "ax":-0.89525, "ay":0.44489, "alpha":-0.00049, "fx":[-10.16011,-10.16004,-10.14382,-10.14379], "fy":[5.03945,5.07434,5.02668,5.03949]}, - {"t":0.49791, "x":3.48503, "y":5.11515, "heading":3.14154, "vx":-0.44576, "vy":0.22152, "omega":-0.00024, "ax":-0.89523, "ay":0.44488, "alpha":-0.00047, "fx":[-10.13183,-10.13196,-10.11852,-10.22478], "fy":[5.09886,5.02247,5.0225,5.03579]}, - {"t":0.5477, "x":3.46172, "y":5.12673, "heading":3.14153, "vx":-0.49034, "vy":0.24367, "omega":-0.00026, "ax":-0.89522, "ay":0.44488, "alpha":-0.00017, "fx":[-10.16055,-10.16046,-10.14268,-10.14265], "fy":[5.03677,5.0832,5.02247,5.0368]}, - {"t":0.59749, "x":3.4362, "y":5.13941, "heading":3.14152, "vx":-0.53491, "vy":0.26582, "omega":-0.00027, "ax":-0.8952, "ay":0.44487, "alpha":-0.00023, "fx":[-10.12714,-10.12729,-10.11224,-10.23881], "fy":[5.10595,5.01932,5.01934,5.0342]}, - {"t":0.64728, "x":3.40845, "y":5.1532, "heading":3.1415, "vx":-0.57948, "vy":0.28797, "omega":-0.00028, "ax":-0.89518, "ay":0.44486, "alpha":-0.00014, "fx":[-10.16095,-10.16085,-10.14138,-10.14134], "fy":[5.03554,5.08759,5.01962,5.03558]}, - {"t":0.69707, "x":3.37849, "y":5.16809, "heading":3.14149, "vx":-0.62405, "vy":0.31012, "omega":-0.00029, "ax":-0.89515, "ay":0.44484, "alpha":0.00116, "fx":[-10.11526,-10.11542,-10.09868,-10.27405], "fy":[5.11585,5.01513,5.01516,5.03164]}, - {"t":0.74686, "x":3.34631, "y":5.18408, "heading":3.14148, "vx":-0.66862, "vy":0.33227, "omega":-0.00023, "ax":-0.89512, "ay":0.44483, "alpha":0.00065, "fx":[-10.1612,-10.16106,-10.13995,-10.13991], "fy":[5.03016,5.10412,5.01266,5.03019]}, - {"t":0.79665, "x":3.31191, "y":5.20118, "heading":3.14146, "vx":-0.71319, "vy":0.35442, "omega":-0.0002, "ax":-0.89509, "ay":0.44481, "alpha":0.00163, "fx":[-10.1077,-10.10789,-10.08973,-10.29532], "fy":[5.12706,5.01048,5.0105,5.02835]}, - {"t":0.84644, "x":3.27529, "y":5.21938, "heading":3.14145, "vx":-0.75776, "vy":0.37657, "omega":-0.00012, "ax":-0.89505, "ay":0.44479, "alpha":0.00211, "fx":[-10.16104,-10.16085,-10.13852,-10.13848], "fy":[5.0213,5.13033,5.00256,5.02133]}, - {"t":0.89623, "x":3.23645, "y":5.23868, "heading":3.14145, "vx":-0.80233, "vy":0.39871, "omega":-0.00001, "ax":-0.89501, "ay":0.44477, "alpha":0.0027, "fx":[-10.08941,-10.08965,-10.07061,-10.34711], "fy":[5.16049,4.99843,4.99845,5.01709]}, - {"t":0.94602, "x":3.19539, "y":5.25908, "heading":3.14145, "vx":-0.84689, "vy":0.42086, "omega":0.00012, "ax":-0.89495, "ay":0.44474, "alpha":0.00306, "fx":[-10.16005,-10.15982,-10.13718,-10.13714], "fy":[5.01546,5.14593,4.99631,5.01548]}, - {"t":0.99581, "x":3.15212, "y":5.28059, "heading":3.14145, "vx":-0.89145, "vy":0.443, "omega":0.00028, "ax":-0.89488, "ay":0.44471, "alpha":0.00464, "fx":[-10.0688,-10.06909,-10.05014,-10.40294], "fy":[5.18618,4.98896,4.98898,5.00744]}, - {"t":1.0456, "x":3.10662, "y":5.3032, "heading":3.14147, "vx":-0.936, "vy":0.46515, "omega":0.00051, "ax":-0.89479, "ay":0.44466, "alpha":0.00596, "fx":[-10.15772,-10.15741,-10.13587,-10.13583], "fy":[4.99952,5.18929,4.98114,4.99954]}, - {"t":1.09539, "x":3.05891, "y":5.32691, "heading":3.14149, "vx":-0.98056, "vy":0.48729, "omega":0.0008, "ax":-0.89466, "ay":0.4446, "alpha":0.00707, "fx":[-10.02989,-10.03027,-10.0131,-10.50804], "fy":[5.25701,4.96439,4.9644,4.98093]}, - {"t":1.14518, "x":3.00898, "y":5.35172, "heading":3.14153, "vx":-1.0251, "vy":0.50942, "omega":0.00116, "ax":-0.89449, "ay":0.44452, "alpha":0.00491, "fx":[-10.15253,-10.15226,-10.13441,-10.13438], "fy":[5.00536,5.16199,4.99015,5.00537]}, - {"t":1.19497, "x":2.95683, "y":5.37764, "heading":3.14159, "vx":-1.06964, "vy":0.53156, "omega":0.0014, "ax":-0.89424, "ay":0.44439, "alpha":0.0024, "fx":[-10.05865,-10.05896,-10.04656,-10.39781], "fy":[5.23379,4.97044,4.97044,4.98238]}, - {"t":1.24476, "x":2.90246, "y":5.40465, "heading":-3.14152, "vx":-1.11416, "vy":0.55368, "omega":0.00152, "ax":-0.89381, "ay":0.44418, "alpha":-0.00119, "fx":[-10.14127,-10.14112,-10.13011,-10.13009], "fy":[5.0377,5.04358,5.02854,5.03771]}, - {"t":1.29455, "x":2.84588, "y":5.43277, "heading":-3.14145, "vx":-1.15867, "vy":0.5758, "omega":0.00146, "ax":-0.89296, "ay":0.44375, "alpha":-0.012, "fx":[-10.23772,-10.23772,-10.2329,-9.79578], "fy":[4.88531,5.07927,5.07928,5.0841]}, - {"t":1.34434, "x":2.78708, "y":5.46199, "heading":-3.14138, "vx":-1.20313, "vy":0.59789, "omega":0.00086, "ax":-0.89054, "ay":0.44222, "alpha":-0.07668, "fx":[-10.0965,-10.09681,-10.10034,-10.10037], "fy":[5.42816,3.76201,5.44061,5.42818]}, - {"t":1.39413, "x":2.72607, "y":5.49231, "heading":-3.14133, "vx":-1.24747, "vy":0.61991, "omega":-0.00295, "ax":-0.0001, "ay":0.00026, "alpha":0.00011, "fx":[0.00102,0.00102,0.00047,-0.0071], "fy":[0.00817,0.00132,0.00132,0.00077]}, - {"t":1.44393, "x":2.66396, "y":5.52317, "heading":-3.14148, "vx":-1.24747, "vy":0.61992, "omega":-0.00295, "ax":0.89049, "ay":-0.44232, "alpha":0.07271, "fx":[10.09705,10.09733,10.09876,10.09876], "fy":[-5.4069,-3.83156,-5.41786,-5.40689]}, - {"t":1.49372, "x":2.60295, "y":5.55349, "heading":3.14156, "vx":-1.20314, "vy":0.5979, "omega":0.00067, "ax":0.89296, "ay":-0.44376, "alpha":0.01214, "fx":[10.24327,10.24325,10.23736,9.77993], "fy":[-4.86991,-5.08424,-5.08425,-5.09015]}, - {"t":1.54351, "x":2.54415, "y":5.58271, "heading":3.14159, "vx":-1.15868, "vy":0.5758, "omega":0.00128, "ax":0.89381, "ay":-0.44418, "alpha":0.00133, "fx":[10.142,10.14185,10.12933,10.12933], "fy":[-5.03777,-5.04443,-5.02771,-5.03779]}, - {"t":1.5933, "x":2.48757, "y":5.61083, "heading":-3.14153, "vx":-1.11417, "vy":0.55369, "omega":0.00134, "ax":0.89424, "ay":-0.44439, "alpha":-0.00096, "fx":[10.06206,10.06237,10.04925,10.38815], "fy":[-5.24599,-4.96621,-4.96623,-4.9789]}, - {"t":1.64309, "x":2.4332, "y":5.63785, "heading":-3.14146, "vx":-1.06965, "vy":0.53156, "omega":0.00129, "ax":0.89449, "ay":-0.44452, "alpha":-0.00398, "fx":[10.1531,10.15283,10.13377,10.13376], "fy":[-5.00977,-5.14966,-4.99387,-5.00979]}, - {"t":1.69288, "x":2.38105, "y":5.66376, "heading":-3.1414, "vx":-1.02511, "vy":0.50943, "omega":0.0011, "ax":0.89466, "ay":-0.4446, "alpha":-0.006, "fx":[10.03272,10.0331,10.01524,10.50015], "fy":[-5.26494,-4.96156,-4.96159,-4.97883]}, - {"t":1.74267, "x":2.33112, "y":5.68858, "heading":-3.14134, "vx":-0.98057, "vy":0.48729, "omega":0.0008, "ax":0.89478, "ay":-0.44466, "alpha":-0.00497, "fx":[10.15832,10.15801,10.13521,10.1352], "fy":[-5.00427,-5.17589,-4.98518,-5.00431]}, - {"t":1.79246, "x":2.28341, "y":5.71229, "heading":-3.1413, "vx":-0.93601, "vy":0.46515, "omega":0.00055, "ax":0.89488, "ay":-0.44471, "alpha":-0.00986, "fx":[10.04515,10.04539,10.02529,10.47507], "fy":[-5.17186,-4.99342,-4.99345,-5.01297]}, - {"t":1.84225, "x":2.23791, "y":5.7349, "heading":-3.14128, "vx":-0.89146, "vy":0.44301, "omega":0.00006, "ax":0.89495, "ay":-0.44475, "alpha":-0.00284, "fx":[10.16062,10.16037,10.13658,10.13656], "fy":[-5.01613,-5.14465,-4.99635,-5.01617]}, - {"t":1.89204, "x":2.19464, "y":5.7564, "heading":-3.14127, "vx":-0.8469, "vy":0.42087, "omega":-0.00008, "ax":0.895, "ay":-0.44477, "alpha":-0.00232, "fx":[10.09028,10.09052,10.07104,10.34487], "fy":[-5.16349,-4.99731,-4.99734,-5.01644]}, - {"t":1.94183, "x":2.15358, "y":5.77681, "heading":-3.14128, "vx":-0.80233, "vy":0.39872, "omega":-0.0002, "ax":0.89505, "ay":-0.4448, "alpha":-0.002, "fx":[10.16141,10.1612,10.13812,10.1381], "fy":[-5.02157,-5.1299,-5.00253,-5.02161]}, - {"t":1.99162, "x":2.11474, "y":5.79611, "heading":-3.14129, "vx":-0.75777, "vy":0.37657, "omega":-0.0003, "ax":0.89509, "ay":-0.44482, "alpha":-0.00134, "fx":[10.10854,10.10872,10.09039,10.29294], "fy":[-5.12894,-5.00982,-5.00985,-5.02788]}, - {"t":2.04141, "x":2.07812, "y":5.81431, "heading":-3.1413, "vx":-0.7132, "vy":0.35443, "omega":-0.00036, "ax":0.89512, "ay":-0.44483, "alpha":-0.00035, "fx":[10.16141,10.16126,10.13972,10.1397], "fy":[-5.0316,-5.09995,-5.01405,-5.03163]}, - {"t":2.0912, "x":2.04372, "y":5.8314, "heading":-3.14132, "vx":-0.66863, "vy":0.33228, "omega":-0.00038, "ax":0.89515, "ay":-0.44485, "alpha":-0.00084, "fx":[10.11538,10.11554,10.09885,10.27359], "fy":[-5.1206,-5.01359,-5.01362,-5.03005]}, - {"t":2.14099, "x":2.01154, "y":5.8474, "heading":-3.14134, "vx":-0.62406, "vy":0.31013, "omega":-0.00042, "ax":0.89518, "ay":-0.44486, "alpha":0.00014, "fx":[10.16105,10.16093,10.14126,10.14123], "fy":[-5.03549,-5.0877,-5.01969,-5.03552]}, - {"t":2.19078, "x":1.98157, "y":5.86229, "heading":-3.14136, "vx":-0.57949, "vy":0.28798, "omega":-0.00042, "ax":0.8952, "ay":-0.44487, "alpha":0.00004, "fx":[10.12511,10.12526,10.11041,10.24467], "fy":[-5.10924,-5.01831,-5.01834,-5.03299]}, - {"t":2.24057, "x":1.95383, "y":5.87607, "heading":-3.14138, "vx":-0.53492, "vy":0.26583, "omega":-0.00041, "ax":0.89522, "ay":-0.44488, "alpha":0.00015, "fx":[10.16053,10.16044,10.14268,10.14265], "fy":[-5.03673,-5.08309,-5.02273,-5.03676]}, - {"t":2.29036, "x":1.92831, "y":5.88876, "heading":-3.1414, "vx":-0.49035, "vy":0.24368, "omega":-0.00041, "ax":0.89523, "ay":-0.44489, "alpha":-0.00003, "fx":[10.13,10.13013,10.11706,10.22987], "fy":[-5.09668,-5.02335,-5.02337,-5.03628]}, - {"t":2.34016, "x":1.905, "y":5.90034, "heading":-3.14142, "vx":-0.44577, "vy":0.22153, "omega":-0.00041, "ax":0.89525, "ay":-0.44489, "alpha":0.00042, "fx":[10.15996,10.15988,10.14397,10.14394], "fy":[-5.03924,-5.07451,-5.02699,-5.03928]}, - {"t":2.38995, "x":1.88392, "y":5.91082, "heading":-3.14144, "vx":-0.4012, "vy":0.19938, "omega":-0.00039, "ax":0.89526, "ay":-0.4449, "alpha":-0.00032, "fx":[10.13315,10.13326,10.12184,10.2201], "fy":[-5.08531,-5.02789,-5.02792,-5.0392]}, - {"t":2.43974, "x":1.86505, "y":5.92019, "heading":-3.14146, "vx":-0.35662, "vy":0.17722, "omega":-0.0004, "ax":0.89527, "ay":-0.44491, "alpha":0.00035, "fx":[10.15941,10.15934,10.14509,10.14506], "fy":[-5.03989,-5.07156,-5.02923,-5.03992]}, - {"t":2.48953, "x":1.8484, "y":5.92847, "heading":-3.14148, "vx":-0.31205, "vy":0.15507, "omega":-0.00039, "ax":0.89528, "ay":-0.44491, "alpha":0.00027, "fx":[10.13699,10.1371,10.12723,10.20809], "fy":[-5.08572,-5.02844,-5.02846,-5.03823]}, - {"t":2.53932, "x":1.83398, "y":5.93564, "heading":-3.1415, "vx":-0.26747, "vy":0.13292, "omega":-0.00037, "ax":0.89529, "ay":-0.44492, "alpha":0.00057, "fx":[10.15893,10.15888,10.14604,10.14601], "fy":[-5.04185,-5.06475,-5.03258,-5.04188]}, - {"t":2.58911, "x":1.82177, "y":5.9417, "heading":-3.14152, "vx":-0.22289, "vy":0.11077, "omega":-0.00034, "ax":0.8953, "ay":-0.44492, "alpha":0.00037, "fx":[10.13949,10.13959,10.13092,10.20029], "fy":[-5.08184,-5.03027,-5.0303,-5.03887]}, - {"t":2.6389, "x":1.81178, "y":5.94667, "heading":-3.14154, "vx":-0.17831, "vy":0.08861, "omega":-0.00033, "ax":0.89531, "ay":-0.44493, "alpha":0.00064, "fx":[10.15862,10.15858,10.14675,10.14672], "fy":[-5.04284,-5.06117,-5.03459,-5.04287]}, - {"t":2.68869, "x":1.80401, "y":5.95053, "heading":-3.14155, "vx":-0.13374, "vy":0.06646, "omega":-0.00029, "ax":0.89532, "ay":-0.44493, "alpha":0.00054, "fx":[10.14194,10.14204,10.13421,10.19285], "fy":[-5.07861,-5.03174,-5.03177,-5.03953]}, - {"t":2.73848, "x":1.79846, "y":5.95328, "heading":-3.14157, "vx":-0.08916, "vy":0.04431, "omega":-0.00027, "ax":0.89533, "ay":-0.44493, "alpha":0.00067, "fx":[10.15852,10.1585,10.14719,10.14716], "fy":[-5.04338,-5.05928,-5.03573,-5.04342]}, - {"t":2.78827, "x":1.79513, "y":5.95494, "heading":-3.14158, "vx":-0.04458, "vy":0.02215, "omega":-0.00023, "ax":0.89533, "ay":-0.44494, "alpha":0.00468, "fx":[10.14082,10.20396,10.13346,10.13343], "fy":[-5.05112,-5.03975,-5.03993,-5.05116]}, - {"t":2.83806, "x":1.79402, "y":5.95549, "heading":3.14159, "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":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.71612, "ay":0.35587, "alpha":0.00095, "fx":[-8.10451,-8.10456,-8.13681,-8.13682], "fy":[4.0466,3.98327,4.06573,4.04658]}, + {"t":0.04957, "x":3.59512, "y":5.06044, "heading":3.14159, "vx":-0.0355, "vy":0.01764, "omega":0.00005, "ax":-0.71626, "ay":0.35594, "alpha":0.00246, "fx":[-8.10663,-8.10665,-8.13792,-8.13794], "fy":[4.03879,4.01043,4.05738,4.03878]}, + {"t":0.09914, "x":3.59248, "y":5.06175, "heading":-3.14159, "vx":-0.07101, "vy":0.03529, "omega":0.00017, "ax":-0.71626, "ay":0.35594, "alpha":0.00108, "fx":[-8.13611,-8.13608,-8.15432,-8.06241], "fy":[3.99559,4.05596,4.05595,4.03776]}, + {"t":0.14871, "x":3.58808, "y":5.06394, "heading":-3.14158, "vx":-0.10651, "vy":0.05293, "omega":0.00022, "ax":-0.71625, "ay":0.35594, "alpha":0.00211, "fx":[-8.10725,-8.10727,-8.13706,-8.13708], "fy":[4.03989,4.00771,4.05766,4.03987]}, + {"t":0.19829, "x":3.58192, "y":5.067, "heading":-3.14157, "vx":-0.14202, "vy":0.07057, "omega":0.00033, "ax":-0.71625, "ay":0.35594, "alpha":0.00075, "fx":[-8.13749,-8.13746,-8.15498,-8.05846], "fy":[3.99556,4.05564,4.05563,4.03816]}, + {"t":0.24786, "x":3.574, "y":5.07093, "heading":-3.14155, "vx":-0.17752, "vy":0.08822, "omega":0.00036, "ax":-0.71624, "ay":0.35593, "alpha":0.00189, "fx":[-8.1077,-8.10773,-8.13633,-8.13635], "fy":[4.04037,4.00661,4.05751,4.04036]}, + {"t":0.29743, "x":3.56432, "y":5.07574, "heading":-3.14154, "vx":-0.21303, "vy":0.10586, "omega":0.00046, "ax":-0.71623, "ay":0.35593, "alpha":0.00057, "fx":[-8.13927,-8.13924,-8.1562,-8.05309], "fy":[3.99191,4.05657,4.05656,4.03965]}, + {"t":0.347, "x":3.55288, "y":5.08143, "heading":-3.14151, "vx":-0.24853, "vy":0.12351, "omega":0.00049, "ax":-0.71623, "ay":0.35593, "alpha":0.0015, "fx":[-8.108,-8.10803,-8.13571,-8.13572], "fy":[4.04194,4.00205,4.0586,4.04193]}, + {"t":0.39657, "x":3.53968, "y":5.08799, "heading":-3.14149, "vx":-0.28404, "vy":0.14115, "omega":0.00056, "ax":-0.71622, "ay":0.35592, "alpha":0.00028, "fx":[-8.14161,-8.14158,-8.15811,-8.0458], "fy":[3.98859,4.05741,4.05741,4.04092]}, + {"t":0.44614, "x":3.52472, "y":5.09542, "heading":-3.14146, "vx":-0.31954, "vy":0.15879, "omega":0.00057, "ax":-0.71621, "ay":0.35592, "alpha":0.00123, "fx":[-8.10818,-8.10821,-8.13515,-8.13517], "fy":[4.04294,3.99904,4.05922,4.04293]}, + {"t":0.49571, "x":3.508, "y":5.10373, "heading":-3.14143, "vx":-0.35504, "vy":0.17644, "omega":0.00064, "ax":-0.7162, "ay":0.35591, "alpha":0.00006, "fx":[-8.14362,-8.14358,-8.1598,-8.03928], "fy":[3.98507,4.05834,4.05833,4.04217]}, + {"t":0.54529, "x":3.48952, "y":5.11291, "heading":-3.1414, "vx":-0.39055, "vy":0.19408, "omega":0.00064, "ax":-0.71619, "ay":0.35591, "alpha":0.00097, "fx":[-8.10825,-8.10828,-8.13463,-8.13464], "fy":[4.04387,3.99605,4.05989,4.04386]}, + {"t":0.59486, "x":3.46928, "y":5.12297, "heading":-3.14137, "vx":-0.42605, "vy":0.21172, "omega":0.00069, "ax":-0.71618, "ay":0.3559, "alpha":-0.00043, "fx":[-8.14698,-8.14695,-8.16293,-8.02843], "fy":[3.9818,4.05918,4.05917,4.04325]}, + {"t":0.64443, "x":3.44728, "y":5.13391, "heading":-3.14134, "vx":-0.46155, "vy":0.22937, "omega":0.00067, "ax":-0.71617, "ay":0.35589, "alpha":0.00047, "fx":[-8.10822,-8.10826,-8.13411,-8.13413], "fy":[4.0462,3.9887,4.06201,4.04619]}, + {"t":0.694, "x":3.42352, "y":5.14571, "heading":-3.1413, "vx":-0.49705, "vy":0.24701, "omega":0.00069, "ax":-0.71615, "ay":0.35589, "alpha":-0.00092, "fx":[-8.15101,-8.15097,-8.16675,-8.01533], "fy":[3.9763,4.06073,4.06073,4.04501]}, + {"t":0.74357, "x":3.398, "y":5.15839, "heading":-3.14127, "vx":-0.53255, "vy":0.26465, "omega":0.00064, "ax":-0.71614, "ay":0.35588, "alpha":-0.0001, "fx":[-8.10813,-8.10817,-8.13352,-8.13354], "fy":[4.04885,3.98026,4.06445,4.04884]}, + {"t":0.79314, "x":3.37072, "y":5.17195, "heading":-3.14124, "vx":-0.56805, "vy":0.28229, "omega":0.00064, "ax":-0.71612, "ay":0.35587, "alpha":-0.00121, "fx":[-8.15547,-8.15543,-8.17096,-8.00067], "fy":[3.96602,4.06381,4.06381,4.04834]}, + {"t":0.84271, "x":3.34168, "y":5.18638, "heading":-3.14121, "vx":-0.60355, "vy":0.29993, "omega":0.00058, "ax":-0.7161, "ay":0.35586, "alpha":-0.00068, "fx":[-8.10799,-8.10803,-8.13278,-8.1328], "fy":[4.05141,3.97194,4.06675,4.05141]}, + {"t":0.89229, "x":3.31088, "y":5.20169, "heading":-3.14118, "vx":-0.63905, "vy":0.31757, "omega":0.00054, "ax":-0.71607, "ay":0.35585, "alpha":-0.00191, "fx":[-8.16122,-8.16117,-8.17639,-7.98177], "fy":[3.95801,4.06603,4.06603,4.05089]}, + {"t":0.94186, "x":3.27833, "y":5.21787, "heading":-3.14115, "vx":-0.67455, "vy":0.33521, "omega":0.00045, "ax":-0.71605, "ay":0.35583, "alpha":-0.00099, "fx":[-8.10783,-8.10788,-8.13179,-8.13181], "fy":[4.05235,3.96835,4.06728,4.05234]}, + {"t":0.99143, "x":3.24401, "y":5.23492, "heading":-3.14113, "vx":-0.71004, "vy":0.35285, "omega":0.0004, "ax":-0.71601, "ay":0.35582, "alpha":-0.00275, "fx":[-8.17012,-8.17006,-8.18481,-7.95286], "fy":[3.94201,4.07074,4.07074,4.05609]}, + {"t":1.041, "x":3.20793, "y":5.25285, "heading":-3.14111, "vx":-0.74554, "vy":0.37049, "omega":0.00026, "ax":-0.71598, "ay":0.3558, "alpha":-0.00245, "fx":[-8.10758,-8.10763,-8.13045,-8.13047], "fy":[4.05926,3.94655,4.07363,4.05925]}, + {"t":1.09057, "x":3.17009, "y":5.27165, "heading":-3.1411, "vx":-0.78103, "vy":0.38813, "omega":0.00014, "ax":-0.71593, "ay":0.35577, "alpha":-0.00432, "fx":[-8.1809,-8.18084,-8.19487,-7.91745], "fy":[3.93096,4.07352,4.07353,4.0596]}, + {"t":1.14014, "x":3.1305, "y":5.29133, "heading":-3.14109, "vx":-0.81652, "vy":0.40576, "omega":-0.00007, "ax":-0.71587, "ay":0.35574, "alpha":-0.00323, "fx":[-8.10727,-8.10733,-8.12843,-8.12845], "fy":[4.06195,3.93702,4.07536,4.06195]}, + {"t":1.18971, "x":3.08914, "y":5.31188, "heading":-3.14109, "vx":-0.852, "vy":0.4234, "omega":-0.00023, "ax":-0.7158, "ay":0.35571, "alpha":-0.00452, "fx":[-8.19111,-8.19104,-8.20385,-7.88222], "fy":[3.89711,4.08339,4.0834,4.0707]}, + {"t":1.23929, "x":3.04603, "y":5.33331, "heading":-3.1411, "vx":-0.88749, "vy":0.44103, "omega":-0.00046, "ax":-0.71571, "ay":0.35566, "alpha":-0.00487, "fx":[-8.10661,-8.10668,-8.12532,-8.12534], "fy":[4.06856,3.91475,4.08054,4.06856]}, + {"t":1.28886, "x":3.00115, "y":5.35561, "heading":-3.14113, "vx":-0.92297, "vy":0.45866, "omega":-0.0007, "ax":-0.71558, "ay":0.35559, "alpha":-0.00482, "fx":[-8.19779,-8.19771,-8.20872,-7.8539], "fy":[3.87126,4.08969,4.08969,4.0788]}, + {"t":1.33843, "x":2.95452, "y":5.37878, "heading":-3.14116, "vx":-0.95844, "vy":0.47629, "omega":-0.00094, "ax":-0.7154, "ay":0.3555, "alpha":-0.00586, "fx":[-8.10483,-8.10489,-8.12002,-8.12004], "fy":[4.0703,3.90418,4.08021,4.07031]}, + {"t":1.388, "x":2.90613, "y":5.40283, "heading":-3.14121, "vx":-0.9939, "vy":0.49391, "omega":-0.00123, "ax":-0.7151, "ay":0.35535, "alpha":-0.00887, "fx":[-8.20001,-8.19996,-8.20867,-7.82796], "fy":[3.90851,4.07276,4.07277,4.06416]}, + {"t":1.43757, "x":2.85598, "y":5.42775, "heading":-3.14127, "vx":-1.02935, "vy":0.51153, "omega":-0.00167, "ax":-0.71458, "ay":0.35509, "alpha":-0.00498, "fx":[-8.09816,-8.09818,-8.10817,-8.10819], "fy":[4.05834,3.92452,4.0652,4.05834]}, + {"t":1.48714, "x":2.80408, "y":5.45354, "heading":-3.14135, "vx":-1.06477, "vy":0.52913, "omega":-0.00191, "ax":-0.71337, "ay":0.35444, "alpha":-0.00875, "fx":[-8.16086,-8.16083,-8.16609,-7.87028], "fy":[3.9514,4.04357,4.04358,4.03837]}, + {"t":1.53671, "x":2.75042, "y":5.48021, "heading":-3.14145, "vx":-1.10014, "vy":0.5467, "omega":-0.00235, "ax":-0.70777, "ay":0.3505, "alpha":-0.02046, "fx":[-8.02372,-8.02374,-8.02813,-8.02816], "fy":[4.08656,3.63352,4.09159,4.08658]}, + {"t":1.58629, "x":2.69501, "y":5.50774, "heading":-3.14156, "vx":-1.13522, "vy":0.56407, "omega":-0.00336, "ax":0.7079, "ay":-0.34993, "alpha":0.02081, "fx":[8.22237,8.22235,8.22314,7.44179], "fy":[-3.71919,-4.05145,-4.05143,-4.0507]}, + {"t":1.63586, "x":2.63961, "y":5.53527, "heading":3.14146, "vx":-1.10013, "vy":0.54673, "omega":-0.00233, "ax":0.71328, "ay":-0.35458, "alpha":0.0051, "fx":[8.08569,8.08574,8.09126,8.09125], "fy":[-4.05107,-3.92838,-4.05317,-4.05106]}, + {"t":1.68543, "x":2.58595, "y":5.56194, "heading":3.14134, "vx":-1.06477, "vy":0.52915, "omega":-0.00208, "ax":0.71455, "ay":-0.35512, "alpha":0.00228, "fx":[8.1646,8.16457,8.16838,7.91409], "fy":[-3.88243,-4.07645,-4.07644,-4.07269]}, + {"t":1.735, "x":2.53405, "y":5.58773, "heading":3.14124, "vx":-1.02935, "vy":0.51155, "omega":-0.00196, "ax":0.71508, "ay":-0.35538, "alpha":0.0092, "fx":[8.1027,8.10279,8.11506,8.11505], "fy":[-4.08556,-3.85728,-4.09159,-4.08555]}, + {"t":1.78457, "x":2.4839, "y":5.61265, "heading":3.14114, "vx":-0.9939, "vy":0.49393, "omega":-0.00151, "ax":0.71538, "ay":-0.35553, "alpha":0.00939, "fx":[8.21173,8.21168,8.21921,7.80636], "fy":[-3.89184,-4.08068,-4.08067,-4.07327]}, + {"t":1.83414, "x":2.43551, "y":5.6367, "heading":3.14107, "vx":-0.95844, "vy":0.4763, "omega":-0.00104, "ax":0.71557, "ay":-0.35562, "alpha":0.00627, "fx":[8.10598,8.10607,8.12274,8.12273], "fy":[-4.07467,-3.89793,-4.08329,-4.07465]}, + {"t":1.88371, "x":2.38888, "y":5.65987, "heading":3.14101, "vx":-0.92297, "vy":0.45868, "omega":-0.00073, "ax":0.7157, "ay":-0.35568, "alpha":0.00431, "fx":[8.19467,8.19461,8.20415,7.87005], "fy":[-3.87581,-4.08897,-4.08896,-4.07954]}, + {"t":1.93329, "x":2.344, "y":5.68217, "heading":3.14098, "vx":-0.88749, "vy":0.44104, "omega":-0.00052, "ax":0.71579, "ay":-0.35572, "alpha":0.00468, "fx":[8.10701,8.10711,8.12684,8.12684], "fy":[-4.0689,-3.91828,-4.07933,-4.06888]}, + {"t":1.98286, "x":2.30089, "y":5.7036, "heading":3.14095, "vx":-0.85201, "vy":0.42341, "omega":-0.00029, "ax":0.71587, "ay":-0.35576, "alpha":0.00475, "fx":[8.18622,8.18617,8.19732,7.90141], "fy":[-3.91644,-4.0772,-4.07719,-4.06615]}, + {"t":2.03243, "x":2.25953, "y":5.72415, "heading":3.14094, "vx":-0.81652, "vy":0.40578, "omega":-0.00005, "ax":0.71592, "ay":-0.35579, "alpha":0.00268, "fx":[8.10747,8.10755,8.12936,8.12935], "fy":[-4.05992,-3.94687,-4.07154,-4.0599]}, + {"t":2.082, "x":2.21994, "y":5.74383, "heading":3.14094, "vx":-0.78103, "vy":0.38814, "omega":0.00008, "ax":0.71597, "ay":-0.35581, "alpha":0.00427, "fx":[8.17687,8.17683,8.189,7.93316], "fy":[-3.94192,-4.06981,-4.06979,-4.05773]}, + {"t":2.13157, "x":2.1821, "y":5.76263, "heading":3.14094, "vx":-0.74554, "vy":0.3705, "omega":0.00029, "ax":0.71601, "ay":-0.35583, "alpha":0.00195, "fx":[8.10773,8.10781,8.13104,8.13103], "fy":[-4.05714,-3.95626,-4.06954,-4.05712]}, + {"t":2.18114, "x":2.14602, "y":5.78056, "heading":3.14095, "vx":-0.71005, "vy":0.35286, "omega":0.00039, "ax":0.71604, "ay":-0.35584, "alpha":0.00252, "fx":[8.16576,8.16572,8.17843,7.96917], "fy":[-3.95042,-4.06767,-4.06765,-4.05503]}, + {"t":2.23071, "x":2.1117, "y":5.79762, "heading":3.14097, "vx":-0.67455, "vy":0.33522, "omega":0.00052, "ax":0.71607, "ay":-0.35586, "alpha":0.00119, "fx":[8.10798,8.10804,8.13216,8.13216], "fy":[-4.05384,-3.96701,-4.0667,-4.05383]}, + {"t":2.28029, "x":2.07914, "y":5.8138, "heading":3.141, "vx":-0.63906, "vy":0.31758, "omega":0.00057, "ax":0.71609, "ay":-0.35587, "alpha":0.00165, "fx":[8.15876,8.15872,8.17178,7.99216], "fy":[-3.95989,-4.065,-4.06499,-4.05201]}, + {"t":2.32986, "x":2.04835, "y":5.8291, "heading":3.14103, "vx":-0.60356, "vy":0.29994, "omega":0.00066, "ax":0.71611, "ay":-0.35588, "alpha":0.00044, "fx":[8.1082,8.10826,8.13295,8.13295], "fy":[-4.05035,-3.97819,-4.06347,-4.05033]}, + {"t":2.37943, "x":2.01931, "y":5.84353, "heading":3.14106, "vx":-0.56806, "vy":0.2823, "omega":0.00068, "ax":0.71613, "ay":-0.35589, "alpha":0.00152, "fx":[8.15449,8.15446,8.16775,8.00648], "fy":[-3.97229,-4.06123,-4.06122,-4.048]}, + {"t":2.429, "x":1.99203, "y":5.85709, "heading":3.14109, "vx":-0.53256, "vy":0.26466, "omega":0.00075, "ax":0.71615, "ay":-0.35589, "alpha":-0.00022, "fx":[8.1084,8.10845,8.13353,8.13353], "fy":[-4.04714,-3.98842,-4.06041,-4.04712]}, + {"t":2.47857, "x":1.96651, "y":5.86977, "heading":3.14113, "vx":-0.49706, "vy":0.24702, "omega":0.00074, "ax":0.71616, "ay":-0.3559, "alpha":0.00078, "fx":[8.14952,8.14949,8.1629,8.02265], "fy":[-3.97672,-4.06002,-4.06001,-4.04666]}, + {"t":2.52814, "x":1.94275, "y":5.88158, "heading":3.14117, "vx":-0.46156, "vy":0.22937, "omega":0.00078, "ax":0.71617, "ay":-0.35591, "alpha":-0.00057, "fx":[8.10855,8.1086,8.13399,8.134], "fy":[-4.04558,-3.99355,-4.05898,-4.04557]}, + {"t":2.57771, "x":1.92075, "y":5.89251, "heading":3.14121, "vx":-0.42606, "vy":0.21173, "omega":0.00075, "ax":0.71619, "ay":-0.35591, "alpha":0.00067, "fx":[8.14654,8.14652,8.16006,8.03255], "fy":[-3.9849,-4.05751,-4.0575,-4.04402]}, + {"t":2.62729, "x":1.90051, "y":5.90257, "heading":3.14124, "vx":-0.39055, "vy":0.19409, "omega":0.00079, "ax":0.7162, "ay":-0.35592, "alpha":-0.00089, "fx":[8.10865,8.10869,8.13441,8.13441], "fy":[-4.04413,-3.99826,-4.05765,-4.04412]}, + {"t":2.67686, "x":1.88203, "y":5.91176, "heading":3.14128, "vx":-0.35505, "vy":0.17644, "omega":0.00074, "ax":0.71621, "ay":-0.35592, "alpha":0.00042, "fx":[8.14397,8.14395,8.15763,8.04104], "fy":[-3.98948,-4.05618,-4.05617,-4.04254]}, + {"t":2.72643, "x":1.86531, "y":5.92007, "heading":3.14132, "vx":-0.31955, "vy":0.1588, "omega":0.00076, "ax":0.71622, "ay":-0.35593, "alpha":-0.00106, "fx":[8.10867,8.10871,8.1348,8.13481], "fy":[-4.04351,-4.00038,-4.05719,-4.04349]}, + {"t":2.776, "x":1.85035, "y":5.9275, "heading":3.14136, "vx":-0.28404, "vy":0.14116, "omega":0.00071, "ax":0.71622, "ay":-0.35593, "alpha":0.00024, "fx":[8.14187,8.14184,8.15571,8.04793], "fy":[-3.99389,-4.0549,-4.05489,-4.04107]}, + {"t":2.82557, "x":1.83714, "y":5.93406, "heading":3.14139, "vx":-0.24854, "vy":0.12351, "omega":0.00072, "ax":0.71623, "ay":-0.35593, "alpha":-0.00126, "fx":[8.10862,8.10865,8.13521,8.13522], "fy":[-4.04275,-4.00275,-4.05666,-4.04274]}, + {"t":2.87514, "x":1.8257, "y":5.93975, "heading":3.14143, "vx":-0.21303, "vy":0.10587, "omega":0.00066, "ax":0.71624, "ay":-0.35594, "alpha":-0.00021, "fx":[8.13958,8.13956,8.15366,8.0552], "fy":[-3.99419,-4.05498,-4.05497,-4.04091]}, + {"t":2.92472, "x":1.81602, "y":5.94456, "heading":3.14146, "vx":-0.17753, "vy":0.08822, "omega":0.00065, "ax":0.71624, "ay":-0.35594, "alpha":-0.00153, "fx":[8.10847,8.1085,8.13566,8.13567], "fy":[-4.04171,-4.0059,-4.0559,-4.04169]}, + {"t":2.97429, "x":1.8081, "y":5.94849, "heading":3.14149, "vx":-0.14202, "vy":0.07058, "omega":0.00057, "ax":0.71625, "ay":-0.35594, "alpha":-0.0002, "fx":[8.13836,8.13834,8.15281,8.05905], "fy":[-3.99891,-4.05362,-4.05361,-4.03918]}, + {"t":3.02386, "x":1.80194, "y":5.95155, "heading":3.14152, "vx":-0.10652, "vy":0.05293, "omega":0.00056, "ax":0.71626, "ay":-0.35595, "alpha":-0.00177, "fx":[8.10821,8.10823,8.13618,8.13619], "fy":[-4.04085,-4.00832,-4.05545,-4.04083]}, + {"t":3.07343, "x":1.79754, "y":5.95374, "heading":3.14155, "vx":-0.07101, "vy":0.03529, "omega":0.00048, "ax":0.71626, "ay":-0.35595, "alpha":-0.00194, "fx":[8.10802,8.10805,8.13648,8.1365], "fy":[-4.04022,-4.01006,-4.05508,-4.0402]}, + {"t":3.123, "x":1.7949, "y":5.95505, "heading":3.14157, "vx":-0.03551, "vy":0.01765, "omega":0.00038, "ax":0.71627, "ay":-0.35595, "alpha":-0.00766, "fx":[8.13406,8.05709,8.14905,8.14906], "fy":[-4.02205,-4.05082,-4.05076,-4.02204]}, + {"t":3.17257, "x":1.79402, "y":5.95549, "heading":3.14159, "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 9b852d0059602d2001a1a8e4f113e1811442623e Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 2 Apr 2026 21:23:35 -0400 Subject: [PATCH 20/20] Made depot shoot work and chnaged the auto chooser names --- .../frc/robot/autochooser/AutoAction.java | 2 +- .../frc/robot/autochooser/AutoChooser.java | 19 ++++--- .../java/frc/robot/autochooser/AutoEvent.java | 3 ++ .../{FastDepot.java => FastDepotBlue.java} | 11 ++-- .../commands/auto/newauto/FastDepotRed.java | 50 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 2 +- .../robot/subsystems/ControllerSubsystem.java | 4 +- 7 files changed, 74 insertions(+), 17 deletions(-) rename src/main/java/frc/robot/commands/auto/newauto/{FastDepot.java => FastDepotBlue.java} (91%) create mode 100644 src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index 142ace20..4ef777e4 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -8,7 +8,7 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), SHOOT("Shoot"), - FAST_SHOOT("Fast Shoot"), + FAST_SHOOT("Shoot depot"), //SHOOT_PICKUP("Shoot and Pickup"), //DISTURBANCE("Disturbance"), //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 99470516..ac253cad 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -11,7 +11,8 @@ import frc.robot.Robot; import frc.robot.commands.auto.neutral.DepotNeutral; import frc.robot.commands.auto.neutral.OutpostNeutral; -import frc.robot.commands.auto.newauto.FastDepot; +import frc.robot.commands.auto.newauto.FastDepotRed; +import frc.robot.commands.auto.newauto.FastDepotBlue; import frc.robot.commands.auto.shoot.ShootBlue; import frc.robot.commands.auto.shoot.ShootRed; import frc.robot.commands.auto.disturbance.DepotDisturbance; @@ -83,9 +84,9 @@ private void populateChoosers() { switch (location) { case INVALID -> {} // Skip the invalid case. case ZERO -> { // Default - locationChooser.addDefaultOption(location.toString(), location); + locationChooser.addDefaultOption(location.getShuffleboardName(), location); } - default -> {locationChooser.addOption(location.toString(), location);} + default -> {locationChooser.addOption(location.getShuffleboardName(), location);} }; } for (AutoAction action : AutoAction.values()) { @@ -127,9 +128,11 @@ private void populateCommandMap() { commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue), new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); - commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT), - new FastDepot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); - + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Red), + new FastDepotRed(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Blue), + new FastDepotBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + /* //shoot-pickup @@ -169,6 +172,8 @@ private void populateDescriptionMap() { "shoot from the middle"); descriptionMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE), "shoot from the outpost"); + descriptionMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT), + "depot and shoot from the depot"); /* descriptionMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.DEPOT_SIDE), "shoot and pickup from the depot"); @@ -209,7 +214,7 @@ public String getCommandDescription() { return "NO AUTO"; } else { String commandDescription = descriptionMap.get(event.withoutColor()); - return event.getAction() + " at " + event.getLocation() + " → " + commandDescription + "."; + return event.getActionName() + " at " + event.getLocation() + " -> " + commandDescription + "."; } } diff --git a/src/main/java/frc/robot/autochooser/AutoEvent.java b/src/main/java/frc/robot/autochooser/AutoEvent.java index 41d5fc26..c0dec2fe 100644 --- a/src/main/java/frc/robot/autochooser/AutoEvent.java +++ b/src/main/java/frc/robot/autochooser/AutoEvent.java @@ -40,6 +40,9 @@ public AutoEvent(AutoAction action, FieldLocation location, Alliance color) { public AutoAction getAction() { return action; } + public String getActionName() { + return action.name(); + } public FieldLocation getLocation() { return location; diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java similarity index 91% rename from src/main/java/frc/robot/commands/auto/newauto/FastDepot.java rename to src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java index a4d2d794..78d5af16 100644 --- a/src/main/java/frc/robot/commands/auto/newauto/FastDepot.java +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java @@ -18,8 +18,8 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; import frc.robot.utils.logging.commands.LoggableWaitCommand; -public class FastDepot extends LoggableSequentialCommandGroup { - public FastDepot( +public class FastDepotBlue extends LoggableSequentialCommandGroup { + public FastDepotBlue( SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, ControllerSubsystem controller, IntakeDeployerSubsystem intake) { @@ -30,15 +30,14 @@ public FastDepot( LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) ), - new AutoShoot(hopper, feeder, 2), + new AutoShoot(hopper, feeder, 1), new ToggleDeployment(intake, controller), - new LoggableWaitCommand(0.5), new LoggableParallelCommandGroup( new AutoShoot(hopper, feeder, 20), new LoggableSequentialCommandGroup( - new DriveSwerve(drivetrain, DriveDirection.FORWARD, 5, 0.2), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 0.5, 0.5), new DriveSwerve(drivetrain, DriveDirection.FORWARD, 6, 0.2), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 0.5, 0.5), + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 7, 0.2), new ToggleDeployment(intake, controller) ) ) diff --git a/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java b/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java new file mode 100644 index 00000000..47f9a8d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java @@ -0,0 +1,50 @@ +package frc.robot.commands.auto.newauto; + +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.auto.AutoReset; +import frc.robot.commands.auto.AutoShoot; +import frc.robot.commands.drive.DriveSwerve; +import frc.robot.commands.intakeDeployment.ToggleDeployment; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.DriveDirection; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.*; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; + +public class FastDepotRed extends LoggableSequentialCommandGroup { + public FastDepotRed( + SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intake) { + super( + + new LoggableParallelCommandGroup( + new LoggableSequentialCommandGroup( + new AutoReset(shootstate, turret, angler), + new SetShootingState(shootstate, ShootState.AUTO_AIM) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast")) + ), + new AutoShoot(hopper, feeder, 1), + new ToggleDeployment(intake, controller), + new LoggableParallelCommandGroup( + new AutoShoot(hopper, feeder, 20), + new LoggableSequentialCommandGroup( + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 6, 0.2), + new DriveSwerve(drivetrain, DriveDirection.FORWARD, 0.5, 0.5), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 7, 0.2), + new ToggleDeployment(intake, controller) + ) + ) + + ); + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 138ae2ee..4eea8e9e 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -107,7 +107,7 @@ public enum Mode { public static final double ANGLER_ANGLE_HIGH = 38; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState - public static final double ANGLER_LIMIT_SPEED = 0.5; + public static final double ANGLER_LIMIT_SPEED = 0.6; // turret (pan angle) PID diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 424a758b..a37826d9 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -313,7 +313,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc return (-0.583675*distance*distance*distance +20.18291*distance*distance -261.37309*distance - -1324.19278); + -1324.19278) * 1.05; }else if(state == ShootState.SHUTTLING){ return (((-distance*distance) - 5 * distance) - 2800); } @@ -321,7 +321,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc } private double calculateTurretAngleDegrees(ShootState state, Pose2d robotPose, PoseControlProfile profile) { - if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM){ + if(state == ShootState.SHOOTING_HUB || state == ShootState.AUTO_AIM || state == ShootState.SHUTTLING){ return Math.floor( Math.toDegrees(TurretCalculations.calculateTurretAngle(state,robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(),