From d97097152a745e500a549d4dd00ad54b4d281ae2 Mon Sep 17 00:00:00 2001 From: alexandra Date: Tue, 10 Mar 2026 19:00:16 -0400 Subject: [PATCH 01/40] (untested) added intaking and shooting commandsfor automated ops check --- src/main/java/frc/robot/Robot.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e39a4404..fb59e957 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -544,7 +544,13 @@ public void testInit() { drive.withVelocityX(0) .withVelocityY(0) .withRotationalRate(0) - ) + ), + Commands.waitSeconds(2.5), + m_drivetrain.xBrake(), + Commands.waitSeconds(2.5), + m_superstructure.intake(() -> false).withTimeout(5), + Commands.waitSeconds(2.5), + m_superstructure.activateOuttake(kShooterRPS).withTimeout(4) ) ); } From 3d0c729d2b35fe797887fcbf217c18cec9943ac2 Mon Sep 17 00:00:00 2001 From: alexandra Date: Tue, 10 Mar 2026 22:07:36 -0400 Subject: [PATCH 02/40] added a long ops check ad a short ops check short ops check is what we did for dalton (called intake and outtake command and then run swerve); short ops check is running every subsystem individually and then running a short ops check --- src/main/java/frc/robot/Constants.java | 13 +++ src/main/java/frc/robot/Robot.java | 94 ++++++++++--------- .../robot/dashboards/TestingDashboard.java | 20 ++++ .../frc/robot/subsystems/Superstructure.java | 72 +++++++++++++- .../java/frc/robot/subsystems/Swerve.java | 55 +++++++++++ 5 files changed, 207 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ad2c811..7e2305cd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,9 +36,11 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import frc.robot.generated.TunerConstants; import frc.util.AllianceFlipUtil; import frc.util.VisionUtil; import frc.util.WaltLogger; @@ -319,6 +321,11 @@ public static class RobotK { public static final int kMiniPCChannel = 14; + public static final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed + public static final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity + + public static final LinearVelocity kTranslationOpsCheckSpeed = MetersPerSecond.of(0.1); + // real values public static final Distance kRobotFullWidth = Inches.of(33.6875); public static final Distance kRobotFullLength = Inches.of(32.6875); @@ -328,6 +335,12 @@ public static class RobotK { public static class SuperstructureK { public static final String kLogTab = "Superstructure"; + + public static final double kLongOpsCheckPause = 4; //in secs + public static final double kShortOpsCheckPause = 2.5; //in secs + + public static final double kShortOpsCheckShooterTime = 5; + public static final double kShortOpsCheckIntakeTime = 5; } public static class IntakeK { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fb59e957..05a23c6c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,8 +61,6 @@ public class Robot extends TimedRobot { /* CLASS VARIABLES */ //---CONSTANTS - private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed - private final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); @@ -89,7 +87,7 @@ public class Robot extends TimedRobot { private final Indexer m_indexer = new Indexer(); // private final WaltVisualSim m_visualSim; - private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter); + private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter, m_drivetrain); //---AUTONS private Command m_autonomousCommand; @@ -511,48 +509,54 @@ public void teleopExit() {} @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); - - CommandScheduler.getInstance().schedule( - Commands.sequence( - m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), - Commands.waitSeconds(1), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(kMaxTranslationSpeed) - .withVelocityY(0) - .withRotationalRate(0) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrake(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(kMaxTranslationSpeed.times(-1)) - .withVelocityY(0) - .withRotationalRate(0) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrake(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(kMaxAngularRate) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrake(), - Commands.waitSeconds(2.5), - m_drivetrain.applyRequest(() -> - drive.withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(0) - ), - Commands.waitSeconds(2.5), - m_drivetrain.xBrake(), - Commands.waitSeconds(2.5), - m_superstructure.intake(() -> false).withTimeout(5), - Commands.waitSeconds(2.5), - m_superstructure.activateOuttake(kShooterRPS).withTimeout(4) - ) - ); + + TestingDashboard.initialize(); + + TestingDashboard.trg_letOpsCheckBeLong + .onTrue(m_superstructure.longOpsCheck()) + .onFalse(m_superstructure.shortOpsCheck()); + + // CommandScheduler.getInstance().schedule( + // Commands.sequence( + // m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), + // Commands.waitSeconds(1), + // m_drivetrain.applyRequest(() -> + // drive.withVelocityX(kMaxTranslationSpeed) + // .withVelocityY(0) + // .withRotationalRate(0) + // ), + // Commands.waitSeconds(2.5), + // m_drivetrain.xBrake(), + // Commands.waitSeconds(2.5), + // m_drivetrain.applyRequest(() -> + // drive.withVelocityX(kMaxTranslationSpeed.times(-1)) + // .withVelocityY(0) + // .withRotationalRate(0) + // ), + // Commands.waitSeconds(2.5), + // m_drivetrain.xBrake(), + // Commands.waitSeconds(2.5), + // m_drivetrain.applyRequest(() -> + // drive.withVelocityX(0) + // .withVelocityY(0) + // .withRotationalRate(kMaxAngularRate) + // ), + // Commands.waitSeconds(2.5), + // m_drivetrain.xBrake(), + // Commands.waitSeconds(2.5), + // m_drivetrain.applyRequest(() -> + // drive.withVelocityX(0) + // .withVelocityY(0) + // .withRotationalRate(0) + // ), + // Commands.waitSeconds(2.5), + // m_drivetrain.xBrake(), + // Commands.waitSeconds(2.5), + // m_superstructure.intake(() -> false).withTimeout(5), + // Commands.waitSeconds(2.5), + // m_superstructure.activateOuttake(kShooterRPS).withTimeout(4) + // ) + // ); } @Override diff --git a/src/main/java/frc/robot/dashboards/TestingDashboard.java b/src/main/java/frc/robot/dashboards/TestingDashboard.java index 500917bc..233f627a 100644 --- a/src/main/java/frc/robot/dashboards/TestingDashboard.java +++ b/src/main/java/frc/robot/dashboards/TestingDashboard.java @@ -39,6 +39,9 @@ public class TestingDashboard { public static BooleanTopic BT_letIntakeArmPositionRotsChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeArmPositionRotsChange"); public static BooleanTopic BT_letIntakeRollersVelocityRPSChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeRollersVelocityRPSChange"); + //---SELECT OPS CHECK SWITCHES + public static BooleanTopic BT_letOpsCheckBeLong = nte_inst.getBooleanTopic("/TestingDashboard/letOpsCheckBeLong"); + /* PUBLISHERS */ //---SHOOTER public static DoublePublisher pub_shooterVelocityRPS; @@ -64,6 +67,9 @@ public class TestingDashboard { public static BooleanPublisher pub_letIntakeArmPositionRotsChange; public static BooleanPublisher pub_letIntakeRollersVelocityRPSChange; + //---SELECT OPS CHECK SWITCHES + public static BooleanPublisher pub_letOpsCheckBeLong; + /* SUBSCRIBERS */ //---SHOOTER public static DoubleSubscriber sub_shooterVelocityRPS; @@ -89,6 +95,9 @@ public class TestingDashboard { public static BooleanSubscriber sub_letIntakeArmPositionRotsChange; public static BooleanSubscriber sub_letIntakeRollersVelocityRPSChange; + //---SELECT OPS CHECK SWITCHES + public static BooleanSubscriber sub_letOpsCheckBeLong; + /* TRIGGERS */ public static Trigger trg_letShooterVelocityRPSChange; public static Trigger trg_letTurretPositionRotsChange; @@ -100,6 +109,8 @@ public class TestingDashboard { public static Trigger trg_letIntakeArmPositionRotsChange; public static Trigger trg_letIntakeRollersVelocityRPSChange; + public static Trigger trg_letOpsCheckBeLong; + public static void initialize() { //---SHOOTER pub_shooterVelocityRPS = DT_shooterVelocityRPS.publish(); @@ -159,6 +170,13 @@ public static void initialize() { sub_letIntakeArmPositionRotsChange = BT_letIntakeArmPositionRotsChange.subscribe(false); sub_letIntakeRollersVelocityRPSChange = BT_letIntakeRollersVelocityRPSChange.subscribe(false); + //--SELECT OPS CHECK SWITCHES + pub_letOpsCheckBeLong = BT_letOpsCheckBeLong.publish(); + + pub_letOpsCheckBeLong.setDefault(false); + + sub_letOpsCheckBeLong = BT_letOpsCheckBeLong.subscribe(false); + //---TRIGGERS trg_letShooterVelocityRPSChange = new Trigger(() -> sub_letShooterVelocityRPSChange.get()); trg_letTurretPositionRotsChange = new Trigger(() -> sub_letTurretPositionRotsChange.get()); @@ -169,5 +187,7 @@ public static void initialize() { trg_letIntakeArmPositionRotsChange = new Trigger(() -> sub_letIntakeArmPositionRotsChange.get()); trg_letIntakeRollersVelocityRPSChange = new Trigger(() -> sub_letIntakeRollersVelocityRPSChange.get()); + + trg_letOpsCheckBeLong = new Trigger(() -> sub_letOpsCheckBeLong.get()); } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3965ff42..9f044b41 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -8,12 +8,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.IndexerK; +import frc.robot.Constants.SuperstructureK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; import frc.util.WaltLogger; import frc.util.WaltLogger.StringArrayLogger; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static frc.robot.Constants.SuperstructureK.*; @@ -28,6 +28,7 @@ public class Superstructure extends SubsystemBase { private final Intake m_intake; private final Indexer m_indexer; private final Shooter m_shooter; + private final Swerve m_drivetrain; /* LOGGERS */ private HashSet m_activeCommands = new HashSet<>(); @@ -37,10 +38,11 @@ public class Superstructure extends SubsystemBase { private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); /* CONSTRUCTOR */ - public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { + public Superstructure(Intake intake, Indexer indexer, Shooter shooter, Swerve drivetrain) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; + m_drivetrain = drivetrain; } /* BUTTON BIND SEQUENCES */ @@ -373,6 +375,72 @@ public Command intakeTo(IntakeArmPosition pos) { ); } + /** + * runs each subsytem in order of how the ball will flow through the robot (intake -> spindxer -> tunnel -> shooter -> swerve) + * and then everything runs together like they would in a match + * + * @return an automated ops check that runs each subsystem individually + */ + public Command longOpsCheck() { + return Commands.sequence( + Commands.print("======================STARTING LONG OPS CHECK======================"), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //deploy intake + m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //run intake rollers + m_intake.setIntakeRollersVelocityCmd(IntakeK.kIntakeRollersMaxRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //run spindexer + m_intake.stopIntakeRollers(), + m_indexer.setSpindexerVelocityCmd(Constants.IndexerK.kSpindexerShootRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //Stop Spindexer; run tunnel + m_indexer.stopSpindexerCmd(), + m_indexer.setTunnelVelocityCmd(IndexerK.kTunnelShootRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //Stop tunnel; run shooter + m_indexer.stopTunnelCmd(), + m_shooter.setShooterVelocityCmd(ShooterK.kShooterRPS), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //shooter stopped + m_shooter.setShooterVelocityCmd(RotationsPerSecond.of(0)), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + + //Run short Ops check (intake, shoot, swerve) + shortOpsCheck() + ); + } + + /** + * runs the intaking cmd – 5 seconds, + * the outaking cmd – 4 seconds, + * and then the swerve tests (run wheels forward, backward, and then rotates them) – 2.5 seconds between each test + * + * @return a sequence of commands that runs the subsystems in groups – how they would run together in a match + * (ex. intaking cmd runs intake arm and rollers, spindexer and turret) + */ + public Command shortOpsCheck() { + return Commands.sequence( + //runs intaking cmd + intake(() -> false).withTimeout(SuperstructureK.kShortOpsCheckIntakeTime), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //runs shooting cmd + activateOuttake(ShooterK.kShooterRPS).withTimeout(SuperstructureK.kShortOpsCheckShooterTime), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Swerve test + m_drivetrain.swerveAutomatedOpsCheck() + ); + } + /** * Adds and removes specified Command names from the ActiveCommands ArrayList, then logs the ArrayList. * @param toAdd Command name to add. diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f2f5c88b..c2df2e22 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -36,6 +36,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.RobotK; +import frc.robot.Constants.SuperstructureK; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.vision.Detection; @@ -390,6 +392,59 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) ); } + /** + * drive the robot forward slow, then fast + * backward slow, then fast + * then spin the wheels driverMax speed + * + * @return Ops check for swerve drive + */ + public Command swerveAutomatedOpsCheck() { + return Commands.sequence( + //Slow speed forward into high speed + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kTranslationOpsCheckSpeed) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kMaxTranslationSpeed) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrake(), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Slow speed backward into high speed + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kTranslationOpsCheckSpeed.times(-1)) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + applyRequest(() -> + swreq_drive.withVelocityX(RobotK.kMaxTranslationSpeed.times(-1)) + .withVelocityY(0) + .withRotationalRate(0) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrake(), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + + //Turn wheels + applyRequest(() -> + swreq_drive.withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(RobotK.kMaxAngularRate) + ), + Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), + xBrake() + ); + + } + /** * robot goes to specified pose * @param destination From 1185c55a117a0d5bafaa7dd8bd9162280ddfd5c0 Mon Sep 17 00:00:00 2001 From: alexandra Date: Sat, 14 Mar 2026 22:45:50 -0400 Subject: [PATCH 03/40] added a vs. commodores fast auton path in choreo --- src/main/deploy/choreo/RightSweepFast.traj | 304 +++++++++++++++++++++ 1 file changed, 304 insertions(+) create mode 100644 src/main/deploy/choreo/RightSweepFast.traj diff --git a/src/main/deploy/choreo/RightSweepFast.traj b/src/main/deploy/choreo/RightSweepFast.traj new file mode 100644 index 00000000..8529c907 --- /dev/null +++ b/src/main/deploy/choreo/RightSweepFast.traj @@ -0,0 +1,304 @@ +{ + "name":"RightSweepFast", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.592900037765503, "y":2.455909967422486, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.639349937438965, "y":2.475399971008301, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.646820068359375, "y":2.5143799781799316, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.841720104217529, "y":3.11857008934021, "heading":1.8086322542523472, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.750279903411865, "y":2.767750024795532, "heading":1.3068325198957504, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.658840179443359, "y":2.6508100032806396, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.592900037765503, "y":2.6897900104522705, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.5794200897216797, "y":2.6897900104522705, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.1969258785247803, "y":2.070849151611328, "heading":-0.7853984616619369, "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.5}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, + {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.592900037765503 m", "val":3.592900037765503}, "y":{"exp":"2.4559099674224854 m", "val":2.455909967422486}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.639349937438965 m", "val":5.639349937438965}, "y":{"exp":"2.475399971008301 m", "val":2.475399971008301}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.646820068359375 m", "val":7.646820068359375}, "y":{"exp":"2.5143799781799316 m", "val":2.5143799781799316}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.841720104217529 m", "val":7.841720104217529}, "y":{"exp":"3.11857008934021 m", "val":3.11857008934021}, "heading":{"exp":"1.8086322542523474 rad", "val":1.8086322542523472}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.750279903411865 m", "val":6.750279903411865}, "y":{"exp":"2.7677500247955322 m", "val":2.767750024795532}, "heading":{"exp":"1.3068325198957504 rad", "val":1.3068325198957504}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.658840179443359 m", "val":5.658840179443359}, "y":{"exp":"2.6508100032806396 m", "val":2.6508100032806396}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.592900037765503 m", "val":3.592900037765503}, "y":{"exp":"2.6897900104522705 m", "val":2.6897900104522705}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.5794200897216797 m", "val":2.5794200897216797}, "y":{"exp":"2.6897900104522705 m", "val":2.6897900104522705}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.1969258785247803 m", "val":2.1969258785247803}, "y":{"exp":"7.97 m - 5.899150848388672 m", "val":2.070849151611328}, "heading":{"exp":"-2.3561947884568335 rad + 90 deg", "val":-0.7853984616619369}, "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":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2.5 m / s", "val":2.5}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, + {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2667, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2667, + "y":0.2794 + }, + "mass":72.57477920000001, + "inertia":6.0, + "gearing":6.122448979591837, + "radius":0.0508, + "vmax":604.0235475301976, + "tmax":0.7, + "cof":2.255, + "bumper":{ + "front":0.41513125, + "side":0.42783125, + "back":0.41513125 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.08833,1.89954,2.66621,3.2942,3.69335,4.72798,5.20581,5.84852], + "samples":[ + {"t":0.0, "x":3.5929, "y":2.45591, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.64442, "ay":0.05761, "alpha":0.03331, "fx":[84.26438,84.26904,84.2695,84.26489], "fy":[1.23516,0.86013,0.85614,1.22944]}, + {"t":0.03887, "x":3.59641, "y":2.45595, "heading":0.0, "vx":0.18052, "vy":0.00224, "omega":0.00129, "ax":4.64401, "ay":0.0576, "alpha":0.034, "fx":[84.25679,84.26155,84.26206,84.25735], "fy":[1.23903,0.85618,0.85213,1.23317]}, + {"t":0.07774, "x":3.60693, "y":2.45608, "heading":0.00005, "vx":0.36103, "vy":0.00448, "omega":0.00262, "ax":4.64352, "ay":0.0576, "alpha":0.03483, "fx":[84.24784,84.25271,84.25327,84.24845], "fy":[1.24358,0.85151,0.8474,1.23758]}, + {"t":0.11661, "x":3.62447, "y":2.4563, "heading":0.00015, "vx":0.54152, "vy":0.00672, "omega":0.00397, "ax":4.64293, "ay":0.05759, "alpha":0.03581, "fx":[84.23711,84.24212,84.24275,84.23779], "fy":[1.24903,0.8459,0.84175,1.24287]}, + {"t":0.15548, "x":3.64903, "y":2.45661, "heading":0.00031, "vx":0.72198, "vy":0.00896, "omega":0.00536, "ax":4.64222, "ay":0.05758, "alpha":0.03701, "fx":[84.22403,84.2292,84.22992,84.2248], "fy":[1.25566,0.83905,0.83486,1.24933]}, + {"t":0.19434, "x":3.6806, "y":2.457, "heading":0.00051, "vx":0.90242, "vy":0.01119, "omega":0.0068, "ax":4.64133, "ay":0.05757, "alpha":0.03851, "fx":[84.20771,84.21309,84.21392,84.2086], "fy":[1.26392,0.8305,0.82629,1.25739]}, + {"t":0.23321, "x":3.71918, "y":2.45748, "heading":0.00078, "vx":1.08282, "vy":0.01343, "omega":0.0083, "ax":4.64019, "ay":0.05756, "alpha":0.04043, "fx":[84.1868,84.19245,84.19343,84.18785], "fy":[1.2745,0.81954,0.81531,1.26771]}, + {"t":0.27208, "x":3.76477, "y":2.45804, "heading":0.0011, "vx":1.26318, "vy":0.01567, "omega":0.00987, "ax":4.63867, "ay":0.05754, "alpha":0.04298, "fx":[84.15902,84.16502,84.16623,84.1603], "fy":[1.28855,0.80499,0.80074,1.28141]}, + {"t":0.31095, "x":3.81738, "y":2.45869, "heading":0.00149, "vx":1.44348, "vy":0.0179, "omega":0.01154, "ax":4.63656, "ay":0.05751, "alpha":0.04652, "fx":[84.12035,84.12684,84.1284,84.12198], "fy":[1.3081,0.78475,0.78049,1.30046]}, + {"t":0.34982, "x":3.87699, "y":2.45943, "heading":0.00193, "vx":1.6237, "vy":0.02014, "omega":0.01335, "ax":4.63343, "ay":0.05747, "alpha":0.0518, "fx":[84.0628,84.07003,84.07216,84.06503], "fy":[1.33717,0.75465,0.7504,1.32874]}, + {"t":0.38869, "x":3.9436, "y":2.46026, "heading":0.00245, "vx":1.8038, "vy":0.02237, "omega":0.01536, "ax":4.62827, "ay":0.05741, "alpha":0.06049, "fx":[83.96811,83.97654,83.97981,83.9715], "fy":[1.38498,0.70521,0.70104,1.37512]}, + {"t":0.42756, "x":4.0172, "y":2.46117, "heading":0.00305, "vx":1.98369, "vy":0.02461, "omega":0.01771, "ax":4.61823, "ay":0.05728, "alpha":0.07744, "fx":[83.78324,83.79401,83.80012,83.78955], "fy":[1.47814,0.6089,0.60515,1.4651]}, + {"t":0.46643, "x":4.0978, "y":2.46217, "heading":0.00374, "vx":2.1632, "vy":0.02683, "omega":0.02072, "ax":4.59006, "ay":0.05693, "alpha":0.12516, "fx":[83.26251,83.27982,83.29847,83.28165], "fy":[1.73916,0.33917,0.33876,1.71484]}, + {"t":0.5053, "x":4.18535, "y":2.46326, "heading":0.00454, "vx":2.34161, "vy":0.02904, "omega":0.02559, "ax":4.05176, "ay":0.05026, "alpha":1.088, "fx":[72.67832,72.81158,74.33709,74.22863], "fy":[6.60432,-4.71304,-4.09028,5.84682]}, + {"t":0.54416, "x":4.27942, "y":2.46443, "heading":0.00554, "vx":2.4991, "vy":0.031, "omega":0.06788, "ax":0.00191, "ay":0.00002, "alpha":6.03629, "fx":[-17.01129,-16.83196,17.08068,16.90137], "fy":[16.09476,-16.28191,-16.08863,16.27719]}, + {"t":0.58303, "x":4.37656, "y":2.46563, "heading":0.00818, "vx":2.49917, "vy":0.031, "omega":0.3025, "ax":0.0, "ay":0.0, "alpha":4.54135, "fx":[-12.8563,-12.65713,12.8563,12.65714], "fy":[12.07251,-12.28122,-12.07257,12.28116]}, + {"t":0.6219, "x":4.4737, "y":2.46684, "heading":0.01993, "vx":2.49917, "vy":0.031, "omega":0.47902, "ax":0.0, "ay":0.0, "alpha":2.93649, "fx":[-8.40424,-8.09031,8.40423,8.09031], "fy":[7.70792,-8.03687,-7.708,8.0368]}, + {"t":0.66077, "x":4.57084, "y":2.46804, "heading":0.03855, "vx":2.49917, "vy":0.031, "omega":0.59315, "ax":0.0, "ay":-0.00001, "alpha":1.25183, "fx":[-3.64329,-3.38453,3.6433,3.38453], "fy":[3.21852,-3.48986,-3.21878,3.4896]}, + {"t":0.69964, "x":4.66798, "y":2.46924, "heading":0.06161, "vx":2.49917, "vy":0.031, "omega":0.64181, "ax":0.0, "ay":-0.00003, "alpha":-0.46924, "fx":[1.39313,1.23819,-1.39312,-1.23818], "fy":[-1.17524,1.33647,1.17414,-1.33756]}, + {"t":0.73851, "x":4.76512, "y":2.47045, "heading":0.08655, "vx":2.49917, "vy":0.031, "omega":0.62357, "ax":0.0, "ay":-0.00013, "alpha":-2.17649, "fx":[6.59562,5.58665,-6.59561,-5.58654], "fy":[-5.28801,6.34047,5.28339,-6.34507]}, + {"t":0.77738, "x":4.86226, "y":2.47165, "heading":0.11079, "vx":2.49917, "vy":0.03099, "omega":0.53898, "ax":0.00001, "ay":-0.00052, "alpha":-3.82233, "fx":[11.80456,9.53872,-11.8049,-9.5379], "fy":[-9.00882,11.36421,8.98974,-11.38304]}, + {"t":0.81625, "x":4.9594, "y":2.47286, "heading":0.13174, "vx":2.49917, "vy":0.03097, "omega":0.39041, "ax":0.00003, "ay":-0.00206, "alpha":-5.37014, "fx":[16.84437,13.06574,-16.84785,-13.0604], "fy":[-12.33117,16.21962,12.2553,-16.29322]}, + {"t":0.85511, "x":5.05654, "y":2.47406, "heading":0.14692, "vx":2.49917, "vy":0.03089, "omega":0.18167, "ax":0.0001, "ay":-0.00773, "alpha":-6.79784, "fx":[21.54714,16.23676,-21.56994,-16.20706], "fy":[-15.38019,20.69037,15.092,-20.96332]}, + {"t":0.89398, "x":5.15368, "y":2.47526, "heading":0.15398, "vx":2.49918, "vy":0.03059, "omega":-0.08255, "ax":0.00033, "ay":-0.02757, "alpha":-8.09662, "fx":[25.74578,19.21834,-25.86568,-19.07437], "fy":[-18.48501,24.46094,17.44398,-25.42099]}, + {"t":0.93285, "x":5.25082, "y":2.47642, "heading":0.15077, "vx":2.49919, "vy":0.02952, "omega":-0.39726, "ax":0.00103, "ay":-0.09335, "alpha":-9.26335, "fx":[29.19464,22.31857,-29.7416,-21.69653], "fy":[-22.41861,26.84821,18.85516,-30.05962]}, + {"t":0.97172, "x":5.34796, "y":2.4775, "heading":0.13533, "vx":2.49923, "vy":0.02589, "omega":-0.75731, "ax":0.00241, "ay":-0.30039, "alpha":-10.25499, "fx":[31.2614,26.18026,-33.49639,-23.77053], "fy":[-28.97978,25.86036,17.45437,-36.13592]}, + {"t":1.01059, "x":5.44511, "y":2.47828, "heading":0.10589, "vx":2.49932, "vy":0.01422, "omega":-1.15591, "ax":-0.00139, "ay":-0.92321, "alpha":-10.61318, "fx":[29.94005,31.8962,-37.81351,-24.12355], "fy":[-41.32235,14.13169,6.26353,-46.07433]}, + {"t":1.04946, "x":5.54225, "y":2.47814, "heading":0.06096, "vx":2.49927, "vy":-0.02167, "omega":-1.56843, "ax":-0.05786, "ay":-2.50656, "alpha":-7.74978, "fx":[21.64766,31.83562,-36.72378,-20.95886], "fy":[-59.43144,-30.29124,-31.80778,-60.38263]}, + {"t":1.08833, "x":5.63935, "y":2.4754, "heading":0.0, "vx":2.49702, "vy":-0.1191, "omega":-1.86966, "ax":4.61853, "ay":-0.27623, "alpha":0.13948, "fx":[83.83743,83.74313,83.75889,83.84962], "fy":[-4.2757,-5.84494,-5.73315,-4.19374]}, + {"t":1.11837, "x":5.71646, "y":2.4717, "heading":-0.05617, "vx":2.63578, "vy":-0.1274, "omega":-1.86547, "ax":4.61535, "ay":-0.2392, "alpha":0.40001, "fx":[83.84368,83.60647,83.64137,83.86684], "fy":[-2.03613,-6.6217,-6.52009,-2.18184]}, + {"t":1.14842, "x":5.79773, "y":2.46776, "heading":-0.11222, "vx":2.77445, "vy":-0.13458, "omega":-1.85345, "ax":4.60804, "ay":-0.18733, "alpha":0.7669, "fx":[83.74531,83.41046,83.45511,83.81691], "fy":[1.39618,-7.55634,-7.74705,0.31196]}, + {"t":1.17846, "x":5.88317, "y":2.46363, "heading":-0.16791, "vx":2.9129, "vy":-0.14021, "omega":-1.83041, "ax":4.59069, "ay":-0.10973, "alpha":1.32495, "fx":[83.2962,83.0999,83.13614,83.63626], "fy":[6.95851,-8.78355,-9.67552,3.53699]}, + {"t":1.20851, "x":5.97276, "y":2.45937, "heading":-0.2229, "vx":3.05083, "vy":-0.14351, "omega":-1.7906, "ax":4.54424, "ay":0.01699, "alpha":2.27714, "fx":[81.5881,82.51182,82.52175,83.17564], "fy":[16.80008,-10.68833,-12.84856,7.96959]}, + {"t":1.23855, "x":6.06648, "y":2.45507, "heading":-0.2767, "vx":3.18736, "vy":-0.143, "omega":-1.72218, "ax":4.38691, "ay":0.23939, "alpha":4.21677, "fx":[74.39341,80.87173,81.0933,82.02061], "fy":[36.28138,-14.93371,-18.59792,14.62421]}, + {"t":1.2686, "x":6.16422, "y":2.45088, "heading":-0.32845, "vx":3.31917, "vy":-0.1358, "omega":-1.59549, "ax":3.47192, "ay":0.25403, "alpha":10.2094, "fx":[40.82787,55.98957,76.43729,78.71928], "fy":[71.08886,-47.51028,-31.10784,25.96511]}, + {"t":1.29864, "x":6.26551, "y":2.44691, "heading":-0.37638, "vx":3.42348, "vy":-0.12817, "omega":-1.28875, "ax":0.1187, "ay":0.77771, "alpha":20.44275, "fx":[-25.12594,-81.18842,48.50209,66.42667], "fy":[78.32259,-5.51634,-64.55271,48.18854]}, + {"t":1.32869, "x":6.36842, "y":2.44341, "heading":-0.4151, "vx":3.42705, "vy":-0.10481, "omega":-0.67454, "ax":-2.38626, "ay":1.06911, "alpha":15.28763, "fx":[-56.23632,-82.9774,-49.03919,15.07062], "fy":[61.038,1.32095,-64.42231,79.65346]}, + {"t":1.35873, "x":6.47031, "y":2.44075, "heading":-0.43537, "vx":3.35535, "vy":-0.07268, "omega":-0.21523, "ax":-3.75683, "ay":1.15556, "alpha":8.42608, "fx":[-67.27236,-83.41425,-74.61754,-47.34675], "fy":[49.31724,3.05531,-35.4272,66.91898]}, + {"t":1.38878, "x":6.56943, "y":2.43908, "heading":-0.44184, "vx":3.24248, "vy":-0.03797, "omega":0.03794, "ax":-4.17017, "ay":0.97195, "alpha":5.72255, "fx":[-72.21039,-83.62299,-79.71301,-67.10312], "fy":[42.22186,3.62707,-23.91901,48.60906]}, + {"t":1.41882, "x":6.66497, "y":2.43838, "heading":-0.4407, "vx":3.11718, "vy":-0.00876, "omega":0.20987, "ax":-4.32315, "ay":0.85724, "alpha":4.53535, "fx":[-74.87213,-83.75086,-81.51487,-73.61399], "fy":[37.62426,3.81256,-18.22014,38.99744]}, + {"t":1.44887, "x":6.75667, "y":2.43851, "heading":-0.43439, "vx":2.98729, "vy":0.01699, "omega":0.34613, "ax":-4.39829, "ay":0.78611, "alpha":3.87961, "fx":[-76.5062,-83.84,-82.37692,-76.48212], "fy":[34.41885,3.82222,-14.83475,33.64573]}, + {"t":1.47891, "x":6.84444, "y":2.43937, "heading":-0.42399, "vx":2.85515, "vy":0.04061, "omega":0.4627, "ax":-4.44213, "ay":0.73832, "alpha":3.46299, "fx":[-77.60674,-83.90748,-82.86779,-78.00491], "fy":[32.04404,3.73178,-12.57428,30.38208]}, + {"t":1.50896, "x":6.92822, "y":2.44093, "heading":-0.41009, "vx":2.72168, "vy":0.06279, "omega":0.56674, "ax":-4.47062, "ay":0.70408, "alpha":3.17415, "fx":[-78.40066,-83.96152,-83.18072,-78.91124], "fy":[30.19581,3.57535,-10.93601,28.26347]}, + {"t":1.539, "x":7.00797, "y":2.44313, "heading":-0.39306, "vx":2.58736, "vy":0.08395, "omega":0.66211, "ax":-4.49052, "ay":0.67835, "alpha":2.96177, "fx":[-79.0047,-84.00649,-83.39677,-79.49044], "fy":[28.69776,3.37085,-9.67208,26.83483]}, + {"t":1.56905, "x":7.08368, "y":2.44596, "heading":-0.37317, "vx":2.45244, "vy":0.10433, "omega":0.7511, "ax":-4.50517, "ay":0.65831, "alpha":2.79882, "fx":[-79.48435,-84.04491,-83.55512,-79.87704], "fy":[27.44112,3.12879,-8.64599,25.8529]}, + {"t":1.59909, "x":7.15533, "y":2.44939, "heading":-0.3506, "vx":2.31709, "vy":0.12411, "omega":0.83519, "ax":-4.51638, "ay":0.64226, "alpha":2.66971, "fx":[-79.87895,-84.07829,-83.67673,-80.1412], "fy":[26.35527,2.85597,-7.77598,25.17626]}, + {"t":1.62914, "x":7.22291, "y":2.45341, "heading":-0.32551, "vx":2.18139, "vy":0.14341, "omega":0.9154, "ax":-4.52523, "ay":0.6291, "alpha":2.56475, "fx":[-80.21341,-84.10753,-83.77369,-80.32288], "fy":[25.39241,2.55715,-7.00964,24.71691]}, + {"t":1.65918, "x":7.28641, "y":2.458, "heading":-0.29801, "vx":2.04543, "vy":0.16231, "omega":0.99246, "ax":-4.53239, "ay":0.61812, "alpha":2.47761, "fx":[-80.50423,-84.1332,-83.85336,-80.4465], "fy":[24.51891,2.23594,-6.31147,24.41686]}, + {"t":1.68923, "x":7.34582, "y":2.46316, "heading":-0.26819, "vx":1.90926, "vy":0.18088, "omega":1.0669, "ax":-4.53831, "ay":0.60883, "alpha":2.40395, "fx":[-80.76269,-84.15566,-83.92039,-80.5279], "fy":[23.71042,1.89529,-5.65621,24.23594]}, + {"t":1.71927, "x":7.40113, "y":2.46887, "heading":-0.23613, "vx":1.7729, "vy":0.19917, "omega":1.13912, "ax":-4.54328, "ay":0.60085, "alpha":2.34071, "fx":[-80.99676,-84.17509,-83.97781,-80.57789], "fy":[22.94878,1.53778,-5.0251,24.14509]}, + {"t":1.74932, "x":7.45235, "y":2.47512, "heading":-0.20191, "vx":1.6364, "vy":0.21722, "omega":1.20945, "ax":-4.54752, "ay":0.59393, "alpha":2.28563, "fx":[-81.21217,-84.19163,-84.02755,-80.60425], "fy":[22.22017,1.16577,-4.40373,24.12244]}, + {"t":1.77936, "x":7.49946, "y":2.48192, "heading":-0.16557, "vx":1.49977, "vy":0.23507, "omega":1.27812, "ax":-4.5512, "ay":0.58789, "alpha":2.23703, "fx":[-81.41312,-84.20534,-84.07082,-80.61278], "fy":[21.51385,0.78154,-3.78065,24.1509]}, + {"t":1.80941, "x":7.54247, "y":2.48924, "heading":-0.12717, "vx":1.36303, "vy":0.25273, "omega":1.34533, "ax":-4.55441, "ay":0.58256, "alpha":2.19364, "fx":[-81.6027,-84.21625,-84.1083,-80.60806], "fy":[20.82133,0.38735,-3.14654,24.21668]}, + {"t":1.83945, "x":7.58137, "y":2.4971, "heading":-0.08675, "vx":1.22619, "vy":0.27023, "omega":1.41124, "ax":-4.55725, "ay":0.57783, "alpha":2.15449, "fx":[-81.7832,-84.2244,-84.14024,-80.5938], "fy":[20.13584,-0.01445,-2.49369,24.3083]}, + {"t":1.8695, "x":7.61615, "y":2.50548, "heading":-0.04435, "vx":1.08927, "vy":0.2876, "omega":1.47597, "ax":-4.55979, "ay":0.57363, "alpha":2.11883, "fx":[-81.95631,-84.22983,-84.16657,-80.57317], "fy":[19.45192,-0.42143,-1.81565,24.416]}, + {"t":1.89954, "x":7.64682, "y":2.51438, "heading":0.0, "vx":0.95227, "vy":0.30483, "omega":1.53963, "ax":-1.56341, "ay":4.05255, "alpha":5.45127, "fx":[-41.75707,-64.18288,-5.06905,-2.45519], "fy":[72.90067,53.91589,83.37239,83.92366]}, + {"t":1.92278, "x":7.66852, "y":2.52256, "heading":0.03577, "vx":0.91595, "vy":0.39898, "omega":1.66628, "ax":-1.88866, "ay":3.82115, "alpha":6.08756, "fx":[-48.26115,-72.13402,-10.50717,-6.16667], "fy":[68.6858,42.48978,82.5054,83.63845]}, + {"t":1.94601, "x":7.68929, "y":2.53286, "heading":0.07448, "vx":0.87207, "vy":0.48775, "omega":1.80771, "ax":-2.19811, "ay":3.52824, "alpha":6.77732, "fx":[-54.78569,-78.56823,-16.42675,-9.74696], "fy":[63.49016,28.54372,80.8592,83.16834]}, + {"t":1.96924, "x":7.70896, "y":2.54514, "heading":0.11648, "vx":0.821, "vy":0.56972, "omega":1.96516, "ax":-2.43957, "ay":3.19191, "alpha":7.54027, "fx":[-60.80238,-82.39072,-21.2234,-12.63465], "fy":[57.5944,13.21487,78.2385,82.60433]}, + {"t":1.99247, "x":7.72737, "y":2.55924, "heading":0.16213, "vx":0.76432, "vy":0.64388, "omega":2.14034, "ax":-2.58374, "ay":2.80796, "alpha":8.46958, "fx":[-66.18359,-83.23925,-23.13293,-14.95886], "fy":[51.09237,-2.32955,73.04356,81.98099]}, + {"t":2.01571, "x":7.74443, "y":2.57495, "heading":0.21186, "vx":0.7043, "vy":0.70911, "omega":2.3371, "ax":-2.17174, "ay":2.01693, "alpha":12.28291, "fx":[-70.57668,-81.47307,11.1805,-16.74399], "fy":[44.50291,-16.20324,36.74421,81.33449]}, + {"t":2.03894, "x":7.76021, "y":2.59197, "heading":0.26615, "vx":0.65384, "vy":0.75597, "omega":2.62246, "ax":-1.34779, "ay":1.11893, "alpha":17.70652, "fx":[-72.41014,-78.69369,67.8335,-14.54499], "fy":[40.98367,-25.54677,-15.66995,81.4389]}, + {"t":2.06217, "x":7.77504, "y":2.60984, "heading":0.32708, "vx":0.62253, "vy":0.78197, "omega":3.03383, "ax":-1.20977, "ay":0.92867, "alpha":18.52925, "fx":[-74.01112,-74.51362,73.06295,-12.33694], "fy":[37.29106,-34.85097,-16.4277,81.38536]}, + {"t":2.0854, "x":7.78917, "y":2.62825, "heading":0.39756, "vx":0.59443, "vy":0.80354, "omega":3.4643, "ax":-1.10108, "ay":0.78812, "alpha":18.96917, "fx":[-75.53694,-68.51265,75.07109,-10.93194], "fy":[32.87249,-44.2327,-12.43246,80.99006]}, + {"t":2.10863, "x":7.80269, "y":2.64714, "heading":0.47805, "vx":0.56884, "vy":0.82185, "omega":3.905, "ax":-0.99556, "ay":0.66878, "alpha":19.16372, "fx":[-76.70823,-60.47126,75.49335,-10.56652], "fy":[27.71027,-52.95733,-6.31243,80.0961]}, + {"t":2.13187, "x":7.81563, "y":2.66641, "heading":0.56877, "vx":0.54572, "vy":0.83739, "omega":4.35022, "ax":-0.90259, "ay":0.57234, "alpha":18.97502, "fx":[-76.9551,-50.45751,73.61496,-11.70783], "fy":[21.74822,-59.63802,1.2712,78.15583]}, + {"t":2.1551, "x":7.82807, "y":2.68602, "heading":0.66983, "vx":0.52475, "vy":0.85068, "omega":4.79105, "ax":-0.89789, "ay":0.53886, "alpha":17.58111, "fx":[-74.1453,-40.00452,64.73859,-15.75269], "fy":[15.2457,-59.71389,10.95933,72.61641]}, + {"t":2.17833, "x":7.84002, "y":2.70593, "heading":0.78114, "vx":0.50389, "vy":0.8632, "omega":5.1995, "ax":-1.82653, "ay":1.00753, "alpha":0.58386, "fx":[-35.13635,-33.59692,-31.10548,-32.72101], "fy":[17.87024,15.75941,18.73146,20.76019]}, + {"t":2.20156, "x":7.85123, "y":2.72625, "heading":0.90194, "vx":0.46145, "vy":0.88661, "omega":5.21306, "ax":-0.71428, "ay":0.36335, "alpha":-17.95883, "fx":[63.45556,-24.20428,-74.64515,-16.4446], "fy":[25.313,69.88457,-0.16803,-68.65921]}, + {"t":2.2248, "x":7.86176, "y":2.74695, "heading":1.02305, "vx":0.44486, "vy":0.89505, "omega":4.79584, "ax":-0.72323, "ay":0.35108, "alpha":-19.48298, "fx":[66.12526,-32.70599,-78.87698,-7.03069], "fy":[38.04669,72.01411,-7.97848,-76.60258]}, + {"t":2.24803, "x":7.8719, "y":2.76784, "heading":1.13446, "vx":0.42806, "vy":0.90321, "omega":4.3432, "ax":-0.87095, "ay":0.40103, "alpha":-19.77479, "fx":[59.64247,-41.42983,-79.90851,-1.51316], "fy":[51.75131,69.59611,-13.2618,-78.98076]}, + {"t":2.27126, "x":7.88161, "y":2.78893, "heading":1.23537, "vx":0.40782, "vy":0.91253, "omega":3.88379, "ax":-1.08028, "ay":0.46538, "alpha":-19.6373, "fx":[48.6378,-48.60204,-80.22975,1.7928], "fy":[64.02714,66.02916,-16.40866,-79.87268]}, + {"t":2.29449, "x":7.89079, "y":2.81026, "heading":1.3256, "vx":0.38272, "vy":0.92334, "omega":3.42757, "ax":-1.33375, "ay":0.52722, "alpha":-19.20771, "fx":[35.01177,-54.04399,-80.55667,2.79224], "fy":[73.45821,62.46368,-17.47975,-80.17893]}, + {"t":2.31773, "x":7.89932, "y":2.83185, "heading":1.40523, "vx":0.35174, "vy":0.93559, "omega":2.98133, "ax":-1.63107, "ay":0.57617, "alpha":-18.4881, "fx":[20.69192,-58.09751,-81.0895,0.12024], "fy":[79.43195,59.2865,-16.74336,-80.15939]}, + {"t":2.34096, "x":7.90705, "y":2.85374, "heading":1.47449, "vx":0.31384, "vy":0.94897, "omega":2.55181, "ax":-2.0182, "ay":0.61191, "alpha":-17.30184, "fx":[6.92601,-61.43588,-81.70997,-10.2509], "fy":[82.30225,56.24652,-15.0204,-79.11904]}, + {"t":2.36419, "x":7.9138, "y":2.87595, "heading":1.53377, "vx":0.26696, "vy":0.96319, "omega":2.14985, "ax":-2.58313, "ay":0.62758, "alpha":-15.26066, "fx":[-7.27758,-65.72254,-81.88397,-32.58611], "fy":[82.58274,51.49927,-15.39985,-73.13557]}, + {"t":2.38742, "x":7.9193, "y":2.8985, "heading":1.58372, "vx":0.20695, "vy":0.97777, "omega":1.79531, "ax":-3.13028, "ay":0.53987, "alpha":-13.06826, "fx":[-24.3285,-71.41686,-81.19466,-50.23902], "fy":[79.41939,43.53632,-19.62214,-64.15245]}, + {"t":2.41065, "x":7.92327, "y":2.92136, "heading":1.62543, "vx":0.13422, "vy":0.99031, "omega":1.49171, "ax":-3.5454, "ay":0.32159, "alpha":-11.19344, "fx":[-42.55721,-76.33615,-80.11257,-58.30079], "fy":[71.49368,34.46923,-24.24329,-58.38037]}, + {"t":2.43389, "x":7.92543, "y":2.94445, "heading":1.66009, "vx":0.05185, "vy":0.99778, "omega":1.23166, "ax":-3.84581, "ay":-0.21567, "alpha":-9.53396, "fx":[-62.993,-80.9801,-77.56711,-57.56879], "fy":[54.49109,21.74418,-31.81841,-60.06887]}, + {"t":2.45712, "x":7.9256, "y":2.96758, "heading":1.6887, "vx":-0.03749, "vy":0.99277, "omega":1.01016, "ax":-3.93337, "ay":-1.37253, "alpha":-7.14004, "fx":[-82.3068,-83.92188,-71.01507,-48.21958], "fy":[13.26037,0.30164,-44.79444,-68.37879]}, + {"t":2.48035, "x":7.92366, "y":2.99027, "heading":1.71217, "vx":-0.12887, "vy":0.96088, "omega":0.84429, "ax":-3.74879, "ay":-2.22949, "alpha":-5.27676, "fx":[-80.94673,-82.39406,-65.46472,-43.26232], "fy":[-20.91589,-16.31769,-52.70733,-71.86418]}, + {"t":2.50358, "x":7.91966, "y":3.01199, "heading":1.73178, "vx":-0.21597, "vy":0.90909, "omega":0.72169, "ax":-3.51648, "ay":-2.72949, "alpha":-4.28757, "fx":[-74.08523,-79.33269,-61.32439,-40.46522], "fy":[-39.15853,-27.77316,-57.5549,-73.60569]}, + {"t":2.52682, "x":7.91369, "y":3.03238, "heading":1.74855, "vx":-0.29766, "vy":0.84568, "omega":0.62208, "ax":-3.32488, "ay":-3.03032, "alpha":-3.70895, "fx":[-68.22066,-76.15998,-58.22463,-38.69742], "fy":[-48.87825,-35.67148,-60.74486,-74.63008]}, + {"t":2.55005, "x":7.90588, "y":3.0512, "heading":1.763, "vx":-0.37491, "vy":0.77528, "omega":0.53592, "ax":-3.17624, "ay":-3.22602, "alpha":-3.32504, "fx":[-63.86063,-73.31613,-55.85075,-37.48736], "fy":[-54.58033,-41.27416,-62.97445,-75.29899]}, + {"t":2.57328, "x":7.89631, "y":3.06835, "heading":1.77545, "vx":-0.4487, "vy":0.70033, "omega":0.45867, "ax":-3.06033, "ay":-3.36202, "alpha":-3.04852, "fx":[-60.62282,-70.8791,-53.98935,-36.61123], "fy":[-58.23969,-45.38364,-64.60733,-75.76754]}, + {"t":2.59651, "x":7.88506, "y":3.08371, "heading":1.78611, "vx":-0.5198, "vy":0.62222, "omega":0.38784, "ax":-2.9683, "ay":-3.46144, "alpha":-2.83855, "fx":[-58.15826,-68.81588,-52.49924,-35.95042], "fy":[-60.7598,-48.49376,-65.84712,-76.11247]}, + {"t":2.61974, "x":7.87218, "y":3.09723, "heading":1.79512, "vx":-0.58876, "vy":0.5418, "omega":0.3219, "ax":-2.89384, "ay":-3.53699, "alpha":-2.67315, "fx":[-56.22908,-67.06851,-51.28563,-35.43658], "fy":[-62.59268,-50.91288,-66.81519,-76.37582]}, + {"t":2.64298, "x":7.85772, "y":3.10886, "heading":1.8026, "vx":-0.65599, "vy":0.45963, "omega":0.2598, "ax":-2.83254, "ay":-3.5962, "alpha":-2.53922, "fx":[-54.67921,-65.5809,-50.28335,-35.02774], "fy":[-63.98417,-52.83893,-67.58778,-76.58244]}, + {"t":2.66621, "x":7.84172, "y":3.11857, "heading":1.80863, "vx":-0.72179, "vy":0.37608, "omega":0.2008, "ax":-2.82032, "ay":-3.61261, "alpha":-2.424, "fx":[-54.28272,-64.80193,-50.04509,-35.55442], "fy":[-64.31407,-53.78135,-67.75661,-76.33213]}, + {"t":2.68786, "x":7.82543, "y":3.12587, "heading":1.81298, "vx":-0.78287, "vy":0.29785, "omega":0.14831, "ax":-2.84665, "ay":-3.58871, "alpha":-2.47026, "fx":[-55.04916,-65.36053,-50.38365,-35.80147], "fy":[-63.64781,-53.09289,-67.49962,-76.20958]}, + {"t":2.70952, "x":7.80781, "y":3.13148, "heading":1.81619, "vx":-0.84451, "vy":0.22014, "omega":0.09482, "ax":-2.87481, "ay":-3.5627, "alpha":-2.5196, "fx":[-55.84935,-65.95428,-50.76177,-36.07309], "fy":[-62.93424,-52.34456,-67.20969,-76.07386]}, + {"t":2.73117, "x":7.78885, "y":3.13541, "heading":1.81825, "vx":-0.90677, "vy":0.14299, "omega":0.04026, "ax":-2.90499, "ay":-3.5343, "alpha":-2.57239, "fx":[-56.68824,-66.58612,-51.1828,-36.37184], "fy":[-62.1658,-51.52864,-66.88311,-75.92336]}, + {"t":2.75283, "x":7.76853, "y":3.13768, "heading":1.81912, "vx":-0.96967, "vy":0.06645, "omega":-0.01545, "ax":-2.93741, "ay":-3.50316, "alpha":-2.62902, "fx":[-57.5715,-67.25917,-51.65049,-36.70073], "fy":[-61.33333,-50.63623,-66.51558,-75.75612]}, + {"t":2.77448, "x":7.74684, "y":3.13829, "heading":1.81878, "vx":-1.03328, "vy":-0.00941, "omega":-0.07238, "ax":-2.97231, "ay":-3.4689, "alpha":-2.68999, "fx":[-58.50567,-67.97671,-52.16906,-37.06338], "fy":[-60.42575,-49.65697,-66.10202,-75.56978]}, + {"t":2.79614, "x":7.72377, "y":3.13728, "heading":1.81722, "vx":-1.09765, "vy":-0.08453, "omega":-0.13063, "ax":-3.00997, "ay":-3.43103, "alpha":-2.75586, "fx":[-59.49826,-68.74211,-52.74331,-37.46411], "fy":[-59.42945,-48.57875,-65.63644,-75.36147]}, + {"t":2.81779, "x":7.69929, "y":3.13464, "heading":1.81439, "vx":-1.16283, "vy":-0.15883, "omega":-0.19031, "ax":-3.05069, "ay":-3.38898, "alpha":-2.82731, "fx":[-60.5579,-69.55881,-53.37868,-37.90812], "fy":[-58.32768,-47.38735,-65.11172,-75.12765]}, + {"t":2.83945, "x":7.6734, "y":3.13041, "heading":1.81027, "vx":-1.22889, "vy":-0.23222, "omega":-0.25154, "ax":-3.09484, "ay":-3.34205, "alpha":-2.90516, "fx":[-61.69451,-70.43015,-54.08137,-38.40167], "fy":[-57.09956,-46.06598,-64.51933,-74.86396]}, + {"t":2.8611, "x":7.64606, "y":3.12459, "heading":1.80482, "vx":-1.29591, "vy":-0.30459, "omega":-0.31445, "ax":-3.14282, "ay":-3.2894, "alpha":-2.99039, "fx":[-62.91935,-71.35925,-54.85841,-38.95235], "fy":[-55.71888,-44.59474,-63.849,-74.56496]}, + {"t":2.88276, "x":7.61726, "y":3.11723, "heading":1.79801, "vx":-1.36397, "vy":-0.37582, "omega":-0.3792, "ax":-3.19506, "ay":-3.22997, "alpha":-3.08423, "fx":[-64.24514,-72.3487,-55.71786,-39.56943], "fy":[-54.1524,-42.94995,-63.08828,-74.22379]}, + {"t":2.90441, "x":7.58697, "y":3.10833, "heading":1.7898, "vx":-1.43316, "vy":-0.44576, "omega":-0.44599, "ax":-3.25208, "ay":-3.16246, "alpha":-3.18817, "fx":[-65.68591,-73.40013,-56.66887,-40.26428], "fy":[-52.35753,-41.10328,-62.22195,-73.83176]}, + {"t":2.92607, "x":7.55518, "y":3.09794, "heading":1.78014, "vx":-1.50358, "vy":-0.51425, "omega":-0.51503, "ax":-3.31442, "ay":-3.08522, "alpha":-3.30409, "fx":[-67.25659,-74.51353,-57.72189,-41.05097], "fy":[-50.27921,-39.02081,-61.23125,-73.37764]}, + {"t":2.94772, "x":7.52184, "y":3.08608, "heading":1.76899, "vx":-1.57536, "vy":-0.58106, "omega":-0.58658, "ax":-3.38263, "ay":-2.99618, "alpha":-3.43441, "fx":[-68.97189,-75.68628,-58.88871,-41.94704], "fy":[-47.84548,-36.66186,-60.09289,-72.84678]}, + {"t":2.96938, "x":7.48693, "y":3.07279, "heading":1.75628, "vx":-1.64861, "vy":-0.64594, "omega":-0.66096, "ax":-3.4573, "ay":-2.89269, "alpha":-3.58222, "fx":[-70.84403,-76.91162,-60.18262,-42.97461], "fy":[-44.96164,-33.97764,-58.77767,-72.21974]}, + {"t":2.99103, "x":7.45042, "y":3.05813, "heading":1.74197, "vx":-1.72347, "vy":-0.70858, "omega":-0.73853, "ax":-3.5389, "ay":-2.77137, "alpha":-3.75162, "fx":[-72.87808,-78.17646,-61.61827,-44.16175], "fy":[-41.50229,-30.90993,-57.2487,-71.4703]}, + {"t":3.01269, "x":7.41227, "y":3.04213, "heading":1.72598, "vx":-1.80011, "vy":-0.7686, "omega":-0.81977, "ax":-3.62767, "ay":-2.62781, "alpha":-3.94802, "fx":[-75.06298,-79.45813,-63.21143,-45.5445], "fy":[-37.3014,-27.38984,-55.45895,-70.56238]}, + {"t":3.03434, "x":7.37244, "y":3.02487, "heading":1.70823, "vx":-1.87867, "vy":-0.8255, "omega":-0.90526, "ax":-3.72337, "ay":-2.45639, "alpha":-4.1787, "fx":[-77.35492,-80.71973,-64.97824,-47.16954], "fy":[-32.14141,-23.33723,-53.348,-69.44522]}, + {"t":3.056, "x":7.33088, "y":3.00642, "heading":1.68862, "vx":-1.9593, "vy":-0.87869, "omega":-0.99575, "ax":-3.82478, "ay":-2.24996, "alpha":-4.45321, "fx":[-79.64764,-81.90382,-66.93355,-49.09773], "fy":[-25.74559,-18.66156,-50.83765,-68.04566]}, + {"t":3.07765, "x":7.28756, "y":2.98686, "heading":1.66706, "vx":-2.04212, "vy":-0.92742, "omega":-1.09219, "ax":-3.92897, "ay":-1.99978, "alpha":-4.78342, "fx":[-81.7235,-82.92401,-69.08774,-51.40864], "fy":[-17.78593,-13.26566,-47.82633,-66.2555]}, + {"t":3.09931, "x":7.24241, "y":2.96631, "heading":1.64341, "vx":-2.1272, "vy":-0.97072, "omega":-1.19577, "ax":-4.03014, "ay":-1.69591, "alpha":-5.18175, "fx":[-83.18486,-83.65537,-71.44072,-54.2055], "fy":[-7.93377,-7.05478,-44.18208,-63.90995]}, + {"t":3.12096, "x":7.1954, "y":2.94489, "heading":1.61751, "vx":-2.21448, "vy":-1.00745, "omega":-1.30798, "ax":-4.1186, "ay":-1.32882, "alpha":-5.65448, "fx":[-83.39225,-83.92527,-73.97101,-57.618], "fy":[4.00114,0.04598,-39.73502,-60.75075]}, + {"t":3.14262, "x":7.14648, "y":2.92277, "heading":1.58919, "vx":-2.30367, "vy":-1.03622, "omega":-1.43043, "ax":-4.18081, "ay":-0.8923, "alpha":-6.18525, "fx":[-81.50025,-83.51,-76.61682,-61.79437], "fy":[17.8122,8.06276,-34.27162,-56.36201]}, + {"t":3.16427, "x":7.09562, "y":2.90012, "heading":1.55821, "vx":-2.3942, "vy":-1.05555, "omega":-1.56437, "ax":-4.20237, "ay":-0.3865, "alpha":-6.70788, "fx":[-76.73858,-82.14606,-79.2455,-66.85602], "fy":[32.61813,16.93239,-27.53735,-50.06328]}, + {"t":3.18593, "x":7.04278, "y":2.87717, "heading":1.52434, "vx":-2.4852, "vy":-1.06392, "omega":-1.70963, "ax":-4.17299, "ay":0.18376, "alpha":-7.08493, "fx":[-68.93637,-79.56779,-81.60968,-72.73967], "fy":[46.89836,26.45807,-19.26166,-40.75848]}, + {"t":3.20758, "x":6.98799, "y":2.85417, "heading":1.48731, "vx":-2.57557, "vy":-1.05994, "omega":-1.86306, "ax":-4.08537, "ay":0.81688, "alpha":-7.13071, "fx":[-58.8591,-75.57767,-83.29994,-78.75821], "fy":[59.09512,36.28334,-9.22821,-26.86562]}, + {"t":3.22924, "x":6.93126, "y":2.83141, "heading":1.44697, "vx":-2.66404, "vy":-1.04225, "omega":-2.01747, "ax":-3.92023, "ay":1.51482, "alpha":-6.71356, "fx":[-47.88188,-70.1348,-83.73045,-82.76283], "fy":[68.36257,45.91666,2.58731,-6.92889]}, + {"t":3.25089, "x":6.87265, "y":2.8092, "heading":1.40328, "vx":-2.74893, "vy":-1.00944, "omega":-2.16286, "ax":-3.63645, "ay":2.25163, "alpha":-5.91774, "fx":[-37.28177,-63.41705,-82.23019,-80.98524], "fy":[74.75297,54.82526,15.83113,18.00207]}, + {"t":3.27255, "x":6.81227, "y":2.78786, "heading":1.35644, "vx":-2.82768, "vy":-0.96068, "omega":-2.29101, "ax":-3.21525, "ay":2.9398, "alpha":-5.05654, "fx":[-27.81849,-55.80594,-78.29632,-71.42536], "fy":[78.844,62.56999,29.65729,42.28387]}, + {"t":3.2942, "x":6.75028, "y":2.76775, "heading":1.30683, "vx":-2.8973, "vy":-0.89702, "omega":-2.4005, "ax":-2.86988, "ay":3.26035, "alpha":-4.81332, "fx":[-20.76906,-49.96184,-74.40152,-63.14823], "fy":[80.39653,66.80315,37.21477,52.20484]}, + {"t":3.3063, "x":6.71503, "y":2.75714, "heading":1.2778, "vx":-2.93202, "vy":-0.85759, "omega":-2.45872, "ax":-2.69989, "ay":3.37697, "alpha":-4.94512, "fx":[-15.73261,-46.94917,-73.1206,-60.14151], "fy":[81.45323,68.88637,39.49761,55.24598]}, + {"t":3.31839, "x":6.67937, "y":2.74701, "heading":1.24806, "vx":-2.96467, "vy":-0.81674, "omega":-2.51854, "ax":-2.49741, "ay":3.5036, "alpha":-5.07061, "fx":[-10.08219,-43.49465,-71.55863,-56.11362], "fy":[82.26383,71.04714,42.07427,58.88786]}, + {"t":3.33049, "x":6.64332, "y":2.73739, "heading":1.2176, "vx":-2.99488, "vy":-0.77437, "omega":-2.57987, "ax":-2.25354, "ay":3.63976, "alpha":-5.18516, "fx":[-3.79443,-39.52293,-69.629,-50.60366], "fy":[82.71294,73.2567,44.99504,63.18979]}, + {"t":3.34259, "x":6.60694, "y":2.72829, "heading":1.18639, "vx":-3.02214, "vy":-0.73034, "omega":-2.64258, "ax":-1.95638, "ay":3.78284, "alpha":-5.28809, "fx":[3.1166,-34.95082,-67.21143,-42.93853], "fy":[82.66454,75.46795,48.31645,68.09004]}, + {"t":3.35468, "x":6.57024, "y":2.71973, "heading":1.15443, "vx":-3.0458, "vy":-0.68459, "omega":-2.70654, "ax":-1.59119, "ay":3.92537, "alpha":-5.3913, "fx":[10.58234,-29.69154,-64.13752,-32.2335], "fy":[81.97286,77.60858,52.09695,73.20477]}, + {"t":3.36678, "x":6.53328, "y":2.71174, "heading":1.12169, "vx":-3.06504, "vy":-0.63711, "omega":-2.77175, "ax":-1.144, "ay":4.05043, "alpha":-5.53121, "fx":[18.4662,-23.66354,-60.17112,-17.65697], "fy":[80.50292,79.5737,56.3852,77.49688]}, + {"t":3.37887, "x":6.49613, "y":2.70433, "heading":1.08817, "vx":-3.07888, "vy":-0.58812, "omega":-2.83865, "ax":-0.61337, "ay":4.12936, "alpha":-5.76436, "fx":[26.56148,-16.80558,-54.98544,0.71456], "fy":[78.15937,81.21986,61.19285,79.11507]}, + {"t":3.39097, "x":6.45884, "y":2.69752, "heading":1.05383, "vx":-3.0863, "vy":-0.53817, "omega":-2.90837, "ax":-0.02548, "ay":4.13348, "alpha":-6.10716, "fx":[34.60803,-9.0991,-48.14721,20.78935], "fy":[74.91619,82.36426,66.43839,76.26733]}, + {"t":3.40306, "x":6.42151, "y":2.69131, "heading":1.01866, "vx":-3.08661, "vy":-0.48818, "omega":-2.98224, "ax":0.57347, "ay":4.05831, "alpha":-6.45335, "fx":[42.32786,-0.59656,-39.13921,39.02723], "fy":[70.83542,82.79487,71.84714,69.0533]}, + {"t":3.41516, "x":6.38422, "y":2.6857, "heading":0.98259, "vx":-3.07967, "vy":-0.43909, "omega":-3.0603, "ax":1.1514, "ay":3.92438, "alpha":-6.61563, "fx":[49.47016,8.55127,-27.48916,53.03027], "fy":[66.06484,82.29727,76.81564,59.63347]}, + {"t":3.42725, "x":6.34705, "y":2.68068, "heading":0.94557, "vx":-3.06575, "vy":-0.39163, "omega":-3.14031, "ax":1.70212, "ay":3.74923, "alpha":-6.46811, "fx":[55.84991,18.08261,-13.08863,62.68699], "fy":[60.81346,80.69931,80.33755,50.24944]}, + {"t":3.43935, "x":6.3101, "y":2.67622, "heading":0.90759, "vx":-3.04516, "vy":-0.34628, "omega":-3.21855, "ax":2.22426, "ay":3.53426, "alpha":-6.01547, "fx":[61.3677,27.64272,3.36076,69.05414], "fy":[55.31399,77.92351,81.22593,42.03465]}, + {"t":3.45144, "x":6.27343, "y":2.67229, "heading":0.86866, "vx":-3.01826, "vy":-0.30353, "omega":-3.2913, "ax":2.70477, "ay":3.2768, "alpha":-5.36571, "fx":[66.00754,36.8388,20.21621,73.23553], "fy":[49.78572,74.02486,78.7627,35.23942]}, + {"t":3.46354, "x":6.23712, "y":2.66886, "heading":0.82885, "vx":-2.98554, "vy":-0.2639, "omega":-3.3562, "ax":3.1238, "ay":2.98473, "alpha":-4.6572, "fx":[69.81859,45.31618,35.54721,76.02708], "fy":[44.40866,69.19112,73.28682,29.72982]}, + {"t":3.47563, "x":6.20124, "y":2.66588, "heading":0.78826, "vx":-2.94776, "vy":-0.2278, "omega":-3.41253, "ax":3.46904, "ay":2.67691, "alpha":-3.99006, "fx":[72.89062,52.8238,48.11304,77.93726], "fy":[39.31234,63.70145,65.98912,25.27333]}, + {"t":3.48773, "x":6.16584, "y":2.66332, "heading":0.74698, "vx":-2.9058, "vy":-0.19542, "omega":-3.46079, "ax":3.74159, "ay":2.37318, "alpha":-3.40591, "fx":[75.33127,59.24266,57.69142,79.27989], "fy":[34.57703,57.86265,58.14594,21.64763]}, + {"t":3.49982, "x":6.13097, "y":2.66113, "heading":0.70512, "vx":-2.86054, "vy":-0.16671, "omega":-3.50199, "ax":3.9515, "ay":2.08716, "alpha":-2.90766, "fx":[77.24928,64.57389,64.70723,80.24877], "fy":[30.24223,51.95191,50.61179,18.66957]}, + {"t":3.51192, "x":6.09666, "y":2.65927, "heading":0.66276, "vx":-2.81275, "vy":-0.14147, "omega":-3.53716, "ax":4.11136, "ay":1.82544, "alpha":-2.48364, "fx":[78.74448,68.90273,69.76846,80.9655], "fy":[26.31742,46.18365,43.78353,16.19598]}, + {"t":3.52401, "x":6.06294, "y":2.65769, "heading":0.61998, "vx":-2.76302, "vy":-0.11939, "omega":-3.5672, "ax":4.23273, "ay":1.58973, "alpha":-2.12034, "fx":[79.90326,72.35851,73.41941,81.50798], "fy":[22.79216,40.70176,37.76336,14.11686]}, + {"t":3.53611, "x":6.02983, "y":2.65636, "heading":0.57684, "vx":-2.71183, "vy":-0.10016, "omega":-3.59284, "ax":4.32494, "ay":1.37914, "alpha":-1.80638, "fx":[80.79742,75.0831,76.07359,81.92723], "fy":[19.64417,35.58827,32.51054,12.34795]}, + {"t":3.5482, "x":5.99734, "y":2.65525, "heading":0.53338, "vx":-2.65951, "vy":-0.08348, "omega":-3.61469, "ax":4.39517, "ay":1.1916, "alpha":-1.53295, "fx":[81.48519,77.21141,78.02429,82.25746], "fy":[16.84503,30.87858,27.93178,10.82442]}, + {"t":3.5603, "x":5.9655, "y":2.65433, "heading":0.48966, "vx":-2.60635, "vy":-0.06907, "omega":-3.63323, "ax":4.44881, "ay":1.02462, "alpha":-1.29326, "fx":[82.01293,78.86189,79.47426,82.52203], "fy":[14.36397,26.57673,23.92456,9.49603]}, + {"t":3.57239, "x":5.9343, "y":2.65357, "heading":0.44571, "vx":-2.55254, "vy":-0.05668, "omega":-3.64888, "ax":4.48987, "ay":0.87573, "alpha":-1.08196, "fx":[82.41715,80.13368,80.56295,82.73723], "fy":[12.17027,22.66771,20.39476,8.32354]}, + {"t":3.58449, "x":5.90375, "y":2.65295, "heading":0.40158, "vx":-2.49824, "vy":-0.04608, "omega":-3.66196, "ax":4.52134, "ay":0.74267, "alpha":-0.89476, "fx":[82.72636,81.10733,81.38685,82.91462], "fy":[10.2346,19.12622,17.26196,7.27602]}, + {"t":3.59659, "x":5.87387, "y":2.65245, "heading":0.35729, "vx":-2.44355, "vy":-0.0371, "omega":-3.67279, "ax":4.54546, "ay":0.62337, "alpha":-0.72816, "fx":[82.96274,81.84715,82.01351,83.06252], "fy":[8.52966,15.92256,14.45973,6.32894]}, + {"t":3.60868, "x":5.84464, "y":2.65204, "heading":0.31287, "vx":-2.38857, "vy":-0.02956, "omega":-3.68159, "ax":4.56392, "ay":0.51606, "alpha":-0.57924, "fx":[83.14348,82.40393,82.49085,83.18698], "fy":[7.03049,13.02608,11.93398,5.46268]}, + {"t":3.62078, "x":5.81609, "y":2.65172, "heading":0.26834, "vx":-2.33337, "vy":-0.02332, "omega":-3.6886, "ax":4.57797, "ay":0.41921, "alpha":-0.44557, "fx":[83.28183,82.81762,82.85341,83.29252], "fy":[5.71451,10.40727,9.64093,4.66146]}, + {"t":3.63287, "x":5.7882, "y":2.65147, "heading":0.22372, "vx":-2.278, "vy":-0.01825, "omega":-3.69399, "ax":4.5886, "ay":0.33149, "alpha":-0.32508, "fx":[83.38803,83.11957,83.12643,83.38247], "fy":[4.56138,8.03871,7.54516,3.91254]}, + {"t":3.64497, "x":5.76098, "y":2.65128, "heading":0.17904, "vx":-2.2225, "vy":-0.01424, "omega":-3.69792, "ax":4.59653, "ay":0.25177, "alpha":-0.21603, "fx":[83.46994,83.33441,83.32866,83.45937], "fy":[3.55285,5.89559,5.61794,3.20554]}, + {"t":3.65706, "x":5.73444, "y":2.65112, "heading":0.13431, "vx":-2.1669, "vy":-0.01119, "omega":-3.70053, "ax":4.60235, "ay":0.17907, "alpha":-0.11692, "fx":[83.53359,83.48152,83.47423,83.52515], "fy":[2.67256,3.95573,3.83588,2.53204]}, + {"t":3.66916, "x":5.70856, "y":2.651, "heading":0.08956, "vx":-2.11124, "vy":-0.00903, "omega":-3.70195, "ax":4.60649, "ay":0.11258, "alpha":-0.02649, "fx":[83.58358,83.5762,83.57392,83.58126], "fy":[1.90585,2.19948,2.17985,1.88514]}, + {"t":3.68125, "x":5.68337, "y":2.6509, "heading":0.04478, "vx":-2.05552, "vy":-0.00767, "omega":-3.70227, "ax":4.6093, "ay":0.05157, "alpha":0.05636, "fx":[83.62342,83.63057,83.63609,83.62884], "fy":[1.23952,0.60954,0.63414,1.25924]}, + {"t":3.69335, "x":5.65884, "y":2.65081, "heading":0.0, "vx":-1.99977, "vy":-0.00704, "omega":-3.70159, "ax":0.01024, "ay":0.28631, "alpha":12.21024, "fx":[-32.65647,-35.887,36.26122,33.02555], "fy":[37.83167,-27.43723,-27.32463,37.70902]}, + {"t":3.72783, "x":5.58988, "y":2.65074, "heading":-0.12766, "vx":-1.99942, "vy":0.00283, "omega":-3.28048, "ax":0.00021, "ay":0.09332, "alpha":11.80356, "fx":[-28.39464,-37.40418,29.35565,36.45833], "fy":[37.16723,-25.33597,-34.03064,28.97239]}, + {"t":3.76232, "x":5.52092, "y":2.65089, "heading":-0.2408, "vx":-1.99941, "vy":0.00605, "omega":-2.8734, "ax":0.0001, "ay":0.02961, "alpha":11.33181, "fx":[-23.53966,-38.28727,23.79883,38.03512], "fy":[37.56872,-21.31137,-36.62853,22.52046]}, + {"t":3.79681, "x":5.45197, "y":2.65112, "heading":-0.33989, "vx":-1.99941, "vy":0.00707, "omega":-2.48259, "ax":0.00003, "ay":0.00915, "alpha":10.84327, "fx":[-18.99176,-38.44191,19.05645,38.3796], "fy":[37.70696,-17.06507,-37.42667,17.4488]}, + {"t":3.8313, "x":5.38301, "y":2.65137, "heading":-0.42551, "vx":-1.99941, "vy":0.00739, "omega":-2.10863, "ax":0.00001, "ay":0.00276, "alpha":10.34305, "fx":[-15.00802,-37.91935,15.02314,37.90496], "fy":[37.29591,-13.20901,-37.21288,13.326]}, + {"t":3.86579, "x":5.31406, "y":2.65162, "heading":-0.49824, "vx":-1.99941, "vy":0.00748, "omega":-1.75192, "ax":0.0, "ay":0.00081, "alpha":9.83183, "fx":[-11.66097,-36.86093,11.66431,36.85779], "fy":[36.36871,-9.94269,-36.3443,9.9772]}, + {"t":3.90027, "x":5.2451, "y":2.65188, "heading":-0.55866, "vx":-1.99941, "vy":0.00751, "omega":-1.41284, "ax":0.0, "ay":0.00024, "alpha":9.31006, "fx":[-8.9444,-35.40941,8.94511,35.40876], "fy":[35.03471,-7.30163,-35.02757,7.31159]}, + {"t":3.93476, "x":5.17615, "y":2.65214, "heading":-0.60738, "vx":-1.99941, "vy":0.00752, "omega":-1.09176, "ax":0.0, "ay":0.00007, "alpha":8.77841, "fx":[-6.81508,-33.68304,6.81522,33.68291], "fy":[33.40333,-5.25359,-33.40122,5.25646]}, + {"t":3.96925, "x":5.10719, "y":2.6524, "heading":-0.64503, "vx":-1.99941, "vy":0.00752, "omega":-0.78901, "ax":0.0, "ay":0.00002, "alpha":8.23776, "fx":[-5.21093,-31.77171,5.21096,31.77169], "fy":[31.5639,-3.73761,-31.56326,3.73845]}, + {"t":4.00374, "x":5.03824, "y":2.65266, "heading":-0.67224, "vx":-1.99941, "vy":0.00752, "omega":-0.50491, "ax":0.0, "ay":0.00001, "alpha":7.68916, "fx":[-4.06053,-29.73979,4.06054,29.73978], "fy":[29.58311,-2.68081,-29.58291,2.68107]}, + {"t":4.03823, "x":4.96928, "y":2.65292, "heading":-0.68966, "vx":-1.99941, "vy":0.00752, "omega":-0.23973, "ax":0.0, "ay":0.0, "alpha":7.13383, "fx":[-3.2888,-27.63106,3.28881,27.63106], "fy":[27.50794,-2.00647,-27.50788,2.00654]}, + {"t":4.07271, "x":4.90033, "y":2.65318, "heading":-0.69793, "vx":-1.99941, "vy":0.00752, "omega":0.00631, "ax":0.0, "ay":0.0, "alpha":6.5731, "fx":[-2.82065,-25.47362,2.82065,25.47362], "fy":[25.36992,-1.63824,-25.36992,1.63824]}, + {"t":4.1072, "x":4.83137, "y":2.65344, "heading":-0.69771, "vx":-1.99941, "vy":0.00752, "omega":0.233, "ax":0.0, "ay":0.0, "alpha":6.00831, "fx":[-2.58333,-23.28449,2.58333,23.2845], "fy":[23.18946,-1.50256,-23.18949,1.50253]}, + {"t":4.14169, "x":4.76242, "y":2.6537, "heading":-0.68967, "vx":-1.99941, "vy":0.00752, "omega":0.44021, "ax":0.0, "ay":0.0, "alpha":5.44082, "fx":[-2.508,-21.07362,2.508,21.07362], "fy":[20.97969,-1.53004,-20.97974,1.52998]}, + {"t":4.17618, "x":4.69346, "y":2.65396, "heading":-0.67449, "vx":-1.99941, "vy":0.00752, "omega":0.62785, "ax":0.0, "ay":0.0, "alpha":4.87184, "fx":[-2.53066,-18.84687,2.53066,18.84687], "fy":[18.74946,-1.65636,-18.74953,1.65629]}, + {"t":4.21066, "x":4.62451, "y":2.65422, "heading":-0.65284, "vx":-1.99941, "vy":0.00752, "omega":0.79587, "ax":0.0, "ay":0.0, "alpha":4.30253, "fx":[-2.59292,-16.60888,2.59292,16.60888], "fy":[16.50616,-1.82283,-16.50622,1.82277]}, + {"t":4.24515, "x":4.55555, "y":2.65447, "heading":-0.62539, "vx":-1.99941, "vy":0.00752, "omega":0.94426, "ax":0.0, "ay":0.0, "alpha":3.73374, "fx":[-2.6424,-14.36437,2.64241,14.36437], "fy":[14.25693,-1.97681,-14.25697,1.97677]}, + {"t":4.27964, "x":4.4866, "y":2.65473, "heading":-0.59282, "vx":-1.99941, "vy":0.00752, "omega":1.07303, "ax":0.0, "ay":0.0, "alpha":3.16621, "fx":[-2.63321,-12.11996,2.63321,12.11996], "fy":[12.01056,-2.07202,-12.01051,2.07206]}, + {"t":4.31413, "x":4.41764, "y":2.65499, "heading":-0.55582, "vx":-1.99941, "vy":0.00752, "omega":1.18222, "ax":0.0, "ay":0.00001, "alpha":2.60031, "fx":[-2.52603,-9.88397,2.52604,9.88397], "fy":[9.77731,-2.0686,-9.77691,2.069]}, + {"t":4.34862, "x":4.34868, "y":2.65525, "heading":-0.51505, "vx":-1.99941, "vy":0.00752, "omega":1.2719, "ax":0.0, "ay":0.00005, "alpha":2.03627, "fx":[-2.28852,-7.66753,2.28854,7.66753], "fy":[7.57029,-1.93321,-7.56836,1.93517]}, + {"t":4.3831, "x":4.27973, "y":2.65551, "heading":-0.47118, "vx":-1.99941, "vy":0.00752, "omega":1.34213, "ax":0.0, "ay":0.00025, "alpha":1.47394, "fx":[-1.89517,-5.48335,1.89523,5.48336], "fy":[5.40553,-1.6376,-5.39665,1.64655]}, + {"t":4.41759, "x":4.21077, "y":2.65577, "heading":-0.42489, "vx":-1.99941, "vy":0.00753, "omega":1.39296, "ax":0.0, "ay":0.00112, "alpha":0.91308, "fx":[-1.32751,-3.3461,1.32771,3.34621], "fy":[3.30824,-1.15302,-3.26775,1.19363]}, + {"t":4.45208, "x":4.14182, "y":2.65603, "heading":-0.37685, "vx":-1.99941, "vy":0.00757, "omega":1.42445, "ax":0.00002, "ay":0.00509, "alpha":0.35321, "fx":[-0.57368,-1.27078,0.57442,1.27145], "fy":[1.33805,-0.4232,-1.15348,0.60783]}, + {"t":4.48657, "x":4.07286, "y":2.6563, "heading":-0.32773, "vx":-1.9994, "vy":0.00775, "omega":1.43663, "ax":0.00009, "ay":0.02315, "alpha":-0.20623, "fx":[0.37223,0.72818,-0.36875,-0.72481], "fy":[-0.29003,0.75708,1.12998,0.08295]}, + {"t":4.52106, "x":4.00391, "y":2.65658, "heading":-0.27818, "vx":-1.9994, "vy":0.00854, "omega":1.42952, "ax":0.00054, "ay":0.10505, "alpha":-0.7651, "fx":[1.5159,2.63914,-1.49212,-2.62341], "fy":[-0.65675,3.28899,4.46631,0.52551]}, + {"t":4.55554, "x":3.93495, "y":2.65693, "heading":-0.22888, "vx":-1.99938, "vy":0.01217, "omega":1.40313, "ax":0.00478, "ay":0.47043, "alpha":-1.30006, "fx":[2.90853,4.44854,-2.67966,-4.33079], "fy":[4.34315,11.08614,12.70185,6.01012]}, + {"t":4.59003, "x":3.866, "y":2.65763, "heading":-0.18049, "vx":-1.99922, "vy":0.02839, "omega":1.3583, "ax":0.05406, "ay":1.81059, "alpha":-1.40276, "fx":[4.83347,6.01088,-2.57687,-4.34437], "fy":[29.07913,35.3585,36.56665,30.39896]}, + {"t":4.62452, "x":3.79709, "y":2.65969, "heading":-0.13364, "vx":-1.99735, "vy":0.09083, "omega":1.30992, "ax":0.27345, "ay":3.5723, "alpha":-0.64786, "fx":[7.64873,7.88717,2.44683,1.86295], "fy":[63.88161,65.23595,65.73887,64.4025]}, + {"t":4.65901, "x":3.72836, "y":2.66495, "heading":-0.08847, "vx":-1.98792, "vy":0.21404, "omega":1.28757, "ax":0.60879, "ay":4.19995, "alpha":-0.28916, "fx":[12.54308,12.38353,9.59354,9.66261], "fy":[75.86586,76.14295,76.5307,76.27091]}, + {"t":4.69349, "x":3.66017, "y":2.67483, "heading":-0.04406, "vx":-1.96693, "vy":0.35888, "omega":1.2776, "ax":0.95499, "ay":4.34906, "alpha":-0.16389, "fx":[18.28017,18.00972,16.38846,16.63006], "fy":[78.66855,78.8013,79.1419,79.02062]}, + {"t":4.72798, "x":3.5929, "y":2.68979, "heading":0.0, "vx":-1.93399, "vy":0.50887, "omega":1.27195, "ax":-4.27828, "ay":1.70964, "alpha":-0.72908, "fx":[-78.50232,-75.52404,-76.97753,-79.49106], "fy":[28.87529,36.01534,32.94491,26.24124]}, + {"t":4.75313, "x":3.54291, "y":2.70313, "heading":0.03199, "vx":-2.04158, "vy":0.55187, "omega":1.25361, "ax":-4.4215, "ay":1.13928, "alpha":-1.87456, "fx":[-82.32798,-76.21138,-79.30441,-83.04593], "fy":[12.91674,33.92447,26.49124,9.35087]}, + {"t":4.77828, "x":3.49017, "y":2.71737, "heading":0.06351, "vx":-2.15278, "vy":0.58052, "omega":1.20647, "ax":-4.41931, "ay":0.10391, "alpha":-4.08295, "fx":[-79.59762,-77.40511,-81.81353,-81.91417], "fy":[-22.7767,29.68055,16.45182,-15.81458]}, + {"t":4.80343, "x":3.43463, "y":2.732, "heading":0.09386, "vx":-2.26392, "vy":0.58313, "omega":1.10379, "ax":-3.72878, "ay":-1.41927, "alpha":-7.79088, "fx":[-37.95807,-79.71713,-83.23909,-69.70079], "fy":[-73.33582,15.55721,0.50953,-45.73404]}, + {"t":4.82858, "x":3.37652, "y":2.74622, "heading":0.12161, "vx":-2.35769, "vy":0.54744, "omega":0.90786, "ax":-1.46733, "ay":-3.44934, "alpha":-8.91899, "fx":[10.2652,10.38495,-79.62597,-47.51506], "fy":[-82.50659,-75.76041,-23.46593,-68.60216]}, + {"t":4.85373, "x":3.31676, "y":2.75889, "heading":0.14445, "vx":-2.39459, "vy":0.46069, "omega":0.68356, "ax":0.03851, "ay":-3.62504, "alpha":-10.74568, "fx":[31.56672,61.68419,-64.58823,-25.86818], "fy":[-77.34777,-54.28121,-51.95643,-79.50115]}, + {"t":4.87887, "x":3.25655, "y":2.76933, "heading":0.16164, "vx":-2.39363, "vy":0.36953, "omega":0.41332, "ax":0.81759, "ay":-3.82444, "alpha":-9.07017, "fx":[41.379,67.43541,-39.58385,-9.89422], "fy":[-72.82429,-48.627,-72.94919,-83.15757]}, + {"t":4.90402, "x":3.19661, "y":2.77742, "heading":0.17203, "vx":-2.37307, "vy":0.27335, "omega":0.18522, "ax":1.39586, "ay":-3.87995, "alpha":-7.47749, "fx":[46.67299,69.47972,-15.88929,1.04099], "fy":[-69.70717,-46.34559,-81.68852,-83.84508]}, + {"t":4.92917, "x":3.13738, "y":2.78306, "heading":0.17669, "vx":-2.33796, "vy":0.17577, "omega":-0.00283, "ax":1.79524, "ay":-3.85282, "alpha":-6.38701, "fx":[49.87219,70.50371,1.31579,8.59746], "fy":[-67.56147,-45.14743,-83.41892,-83.48971]}, + {"t":4.95432, "x":3.07915, "y":2.78626, "heading":0.17662, "vx":-2.29281, "vy":0.07888, "omega":-0.16346, "ax":2.06862, "ay":-3.80106, "alpha":-5.66421, "fx":[51.95209,71.10565,13.08831,13.98341], "fy":[-66.05166,-44.42988,-82.56063,-82.81872]}, + {"t":4.97947, "x":3.02214, "y":2.78705, "heading":0.17251, "vx":-2.24079, "vy":-0.01671, "omega":-0.30591, "ax":2.26167, "ay":-3.74767, "alpha":-5.16424, "fx":[53.36558,71.49206,21.33526,17.94711], "fy":[-64.97213,-43.96845,-80.94812,-82.09792]}, + {"t":5.00462, "x":2.9665, "y":2.78544, "heading":0.16481, "vx":-2.18391, "vy":-0.11096, "omega":-0.43578, "ax":2.40343, "ay":-3.69941, "alpha":-4.79947, "fx":[54.34779,71.75237,27.38185,20.94614], "fy":[-64.19714,-43.66161,-79.20386,-81.42149]}, + {"t":5.02977, "x":2.91234, "y":2.78148, "heading":0.15386, "vx":-2.12347, "vy":-0.204, "omega":-0.55648, "ax":2.51135, "ay":-3.65747, "alpha":-4.52047, "fx":[55.03192,71.93115,32.02978,23.26761], "fy":[-63.64704,-43.45753,-77.51657,-80.8186]}, + {"t":5.05492, "x":2.85973, "y":2.77519, "heading":0.13986, "vx":-2.06031, "vy":-0.29598, "omega":-0.67016, "ax":2.59608, "ay":-3.62135, "alpha":-4.29872, "fx":[55.49869,72.05268,35.76041,25.09813], "fy":[-63.26927,-43.32762,-75.92847,-80.29351]}, + {"t":5.08006, "x":2.80874, "y":2.76661, "heading":0.12301, "vx":-1.99502, "vy":-0.38705, "omega":-0.77827, "ax":2.66435, "ay":-3.59021, "alpha":-4.11707, "fx":[55.79977,72.131,38.87044,26.56346], "fy":[-63.0278,-43.25527,-74.43414,-79.84112]}, + {"t":5.10521, "x":2.75941, "y":2.75574, "heading":0.10343, "vx":-1.92802, "vy":-0.47734, "omega":-0.88181, "ax":2.72056, "ay":-3.56316, "alpha":-3.96481, "fx":[55.9697,72.17454,41.54886,27.75127], "fy":[-62.89707,-43.23062,-73.01455,-79.45326]}, + {"t":5.13036, "x":2.71178, "y":2.74261, "heading":0.08126, "vx":-1.8596, "vy":-0.56695, "omega":-0.98152, "ax":2.7677, "ay":-3.53947, "alpha":-3.83503, "fx":[56.0324,72.18843,43.91985,28.72472], "fy":[-62.85834,-43.24777,-71.64883,-79.12117]}, + {"t":5.15551, "x":2.66589, "y":2.72723, "heading":0.05657, "vx":-1.79, "vy":-0.65596, "omega":-1.07797, "ax":2.80784, "ay":-3.51851, "alpha":-3.72319, "fx":[56.00498,72.17573,46.06706,29.5307], "fy":[-62.8975,-43.30331,-70.31812,-78.83648]}, + {"t":5.18066, "x":2.62176, "y":2.70962, "heading":0.02946, "vx":-1.71938, "vy":-0.74445, "omega":-1.1716, "ax":2.84246, "ay":-3.4998, "alpha":-3.62629, "fx":[55.89996,72.13819,48.04791,30.20506], "fy":[-63.00363,-43.39537,-69.00659,-78.59147]}, + {"t":5.20581, "x":2.57942, "y":2.68979, "heading":0.0, "vx":-1.6479, "vy":-0.83246, "omega":-1.2628, "ax":2.92997, "ay":-3.4578, "alpha":-3.27021, "fx":[56.19057,71.41542,51.54452,33.49147], "fy":[-62.72888,-44.55712,-66.43057,-77.23223]}, + {"t":5.22961, "x":2.54102, "y":2.66899, "heading":-0.03006, "vx":-1.57815, "vy":-0.91477, "omega":-1.34064, "ax":3.05757, "ay":-3.35622, "alpha":-3.12372, "fx":[57.38017,72.21417,55.75172,36.55647], "fy":[-61.62436,-43.227,-62.9169,-75.80841]}, + {"t":5.25342, "x":2.50432, "y":2.64627, "heading":-0.06197, "vx":-1.50537, "vy":-0.99466, "omega":-1.415, "ax":3.20137, "ay":-3.23119, "alpha":-2.96371, "fx":[58.86488,73.10323,60.07263,40.29817], "fy":[-60.18546,-41.67742,-58.77822,-73.86171]}, + {"t":5.27722, "x":2.4694, "y":2.62168, "heading":-0.09566, "vx":-1.42916, "vy":-1.07158, "omega":-1.48555, "ax":3.36362, "ay":-3.07567, "alpha":-2.78062, "fx":[60.73302,74.10468,64.40769,44.86832], "fy":[-58.27194,-39.83465,-53.96276,-71.14651]}, + {"t":5.30102, "x":2.43633, "y":2.5953, "heading":-0.13102, "vx":-1.3491, "vy":-1.14479, "omega":-1.55174, "ax":3.54652, "ay":-2.87969, "alpha":-2.55783, "fx":[63.09585,75.2447,68.63443,50.41291], "fy":[-55.66914,-37.59272,-48.43914,-67.29211]}, + {"t":5.32483, "x":2.40522, "y":2.56723, "heading":-0.16795, "vx":-1.26467, "vy":-1.21334, "omega":-1.61262, "ax":3.75136, "ay":-2.62896, "alpha":-2.27074, "fx":[66.08439,76.55181,72.61315,57.00505], "fy":[-52.03897,-34.79619,-42.20319,-61.7576]}, + {"t":5.34863, "x":2.37618, "y":2.5376, "heading":-0.20634, "vx":-1.17538, "vy":-1.27592, "omega":-1.66668, "ax":3.97611, "ay":-2.30322, "alpha":-1.88992, "fx":[69.82298,78.05025,76.19611,64.49596], "fy":[-46.83771,-31.21169,-35.28137,-53.82506]}, + {"t":5.37244, "x":2.34933, "y":2.50658, "heading":-0.24602, "vx":-1.08073, "vy":-1.33075, "omega":-1.71166, "ax":4.21054, "ay":-1.87604, "alpha":-1.39099, "fx":[74.33163,79.74071,79.23841,72.2684], "fy":[-39.19357,-26.48152,-27.72894,-42.74912]}, + {"t":5.39624, "x":2.32479, "y":2.47437, "heading":-0.28676, "vx":-0.9805, "vy":-1.3754, "omega":-1.74478, "ax":4.42906, "ay":-1.3188, "alpha":-0.76513, "fx":[79.24138,81.5479,81.60739,79.0414], "fy":[-27.80135,-20.05082,-19.62276,-28.23646]}, + {"t":5.42004, "x":2.30271, "y":2.44126, "heading":-0.32829, "vx":-0.87507, "vy":-1.4068, "omega":-1.76299, "ax":4.58447, "ay":-0.61253, "alpha":-0.00956, "fx":[83.17105,83.18501,83.1873,83.17335], "fy":[-11.17617,-11.07263,-11.05099,-11.15466]}, + {"t":5.44385, "x":2.28318, "y":2.40759, "heading":-0.37026, "vx":-0.76594, "vy":-1.42138, "omega":-1.76322, "ax":4.60991, "ay":0.23046, "alpha":0.88159, "fx":[83.17052,83.83243,83.87803,83.68204], "fy":[10.84475,1.6251,-2.10437,6.36013]}, + {"t":5.46765, "x":2.26625, "y":2.37382, "heading":-0.41223, "vx":-0.65621, "vy":-1.41589, "omega":-1.74223, "ax":4.44442, "ay":1.14197, "alpha":1.84093, "fx":[76.39852,81.56149,83.58942,81.00308], "fy":[34.60171,19.11807,7.12556,22.0329]}, + {"t":5.49146, "x":2.25189, "y":2.34044, "heading":-0.4537, "vx":-0.55041, "vy":-1.38871, "omega":-1.69841, "ax":4.07902, "ay":2.01129, "alpha":2.75878, "fx":[64.12429,73.19314,82.23644,76.48041], "fy":[54.11346,40.63208,16.53849,34.68503]}, + {"t":5.51526, "x":2.23994, "y":2.30796, "heading":-0.49413, "vx":-0.45331, "vy":-1.34083, "omega":-1.63274, "ax":3.56806, "ay":2.73445, "alpha":3.67748, "fx":[50.81208,57.02364,79.74238,71.37337], "fy":[66.84254,61.2869,26.00723,44.3153]}, + {"t":5.53906, "x":2.23016, "y":2.27681, "heading":-0.533, "vx":-0.36838, "vy":-1.27574, "omega":-1.5452, "ax":3.01473, "ay":3.25523, "alpha":4.55332, "fx":[39.42358,36.88371,76.05834,66.42784], "fy":[74.19648,75.21327,35.35294,51.48509]}, + {"t":5.56287, "x":2.22225, "y":2.24737, "heading":-0.56978, "vx":-0.29662, "vy":-1.19825, "omega":-1.43681, "ax":2.51303, "ay":3.59945, "alpha":5.17346, "fx":[30.57301,18.65409,71.19984,61.95539], "fy":[78.31184,81.75274,44.33406,56.8303]}, + {"t":5.58667, "x":2.2159, "y":2.21987, "heading":-0.60398, "vx":-0.2368, "vy":-1.11257, "omega":-1.31366, "ax":2.09288, "ay":3.83037, "alpha":5.46259, "fx":[23.87396,4.70605,65.28453,58.02602], "fy":[80.64762,83.80174,52.6681,60.87113]}, + {"t":5.61048, "x":2.21086, "y":2.19447, "heading":-0.63525, "vx":-0.18698, "vy":-1.02139, "omega":-1.18363, "ax":1.74377, "ay":3.99469, "alpha":5.50108, "fx":[18.78219,-5.37959,58.54545,54.606], "fy":[82.01507,83.82709,60.08628,63.98511]}, + {"t":5.63428, "x":2.2069, "y":2.17129, "heading":-0.66343, "vx":-0.14547, "vy":-0.9263, "omega":-1.05268, "ax":1.44839, "ay":4.11667, "alpha":5.4002, "fx":[14.84812,-12.66328,51.30313,51.6289], "fy":[82.84314,83.09193,66.39746,66.43403]}, + {"t":5.65809, "x":2.20385, "y":2.1504, "heading":-0.68849, "vx":-0.11099, "vy":-0.82831, "omega":-0.92414, "ax":1.1939, "ay":4.20837, "alpha":5.24438, "fx":[11.74494,-18.02978,43.90621,49.02533], "fy":[83.36029,82.13637,71.52747,68.39774]}, + {"t":5.68189, "x":2.20154, "y":2.13188, "heading":-0.71048, "vx":-0.08257, "vy":-0.72813, "omega":-0.7993, "ax":0.97219, "ay":4.27673, "alpha":5.08413, "fx":[9.24302,-22.08894,36.66941,46.73318], "fy":[83.69157,81.1723,75.51801,70.00087]}, + {"t":5.70569, "x":2.19985, "y":2.11576, "heading":-0.72951, "vx":-0.05943, "vy":-0.62633, "omega":-0.67827, "ax":0.77811, "ay":4.32669, "alpha":4.94408, "fx":[7.18103,-25.24269,29.83327,44.69989], "fy":[83.90776,80.27461,78.49489,71.33114]}, + {"t":5.7295, "x":2.19866, "y":2.10207, "heading":-0.74566, "vx":-0.04091, "vy":-0.52334, "omega":-0.56058, "ax":0.60796, "ay":4.36227, "alpha":4.83272, "fx":[5.44409,-27.75642,23.55268,42.88199], "fy":[84.05027,79.46274,80.6263,72.45148]}, + {"t":5.7533, "x":2.19786, "y":2.09085, "heading":-0.759, "vx":-0.02644, "vy":-0.4195, "omega":-0.44555, "ax":0.45871, "ay":4.38684, "alpha":4.74976, "fx":[3.94887,-29.80864,17.90673,41.24383], "fy":[84.14399,78.73446,82.08785,73.40781]}, + {"t":5.77711, "x":2.19736, "y":2.08211, "heading":-0.76961, "vx":-0.01552, "vy":-0.31507, "omega":-0.33248, "ax":0.32774, "ay":4.40317, "alpha":4.69104, "fx":[2.63369,-31.52229,12.91789,39.75617], "fy":[84.20425,78.07985,83.04052,74.23426]}, + {"t":5.80091, "x":2.19708, "y":2.07586, "heading":-0.77752, "vx":-0.00771, "vy":-0.21026, "omega":-0.22082, "ax":0.21267, "ay":4.41345, "alpha":4.65121, "fx":[1.45204,-32.98414,8.57183,38.39498], "fy":[84.24063,77.48701,83.62064,74.95655]}, + {"t":5.82471, "x":2.19696, "y":2.0721, "heading":-0.78278, "vx":-0.00265, "vy":-0.1052, "omega":-0.1101, "ax":0.11141, "ay":4.41938, "alpha":4.62519, "fx":[0.36821,-34.25685,4.83352,37.1404], "fy":[84.25913,76.94432,83.93779,75.5944]}, + {"t":5.84852, "x":2.19693, "y":2.07085, "heading":-0.7854, "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 15614456e84a0c20ddd072b0759fc92fe70d3df4 Mon Sep 17 00:00:00 2001 From: alexandra Date: Sat, 14 Mar 2026 22:46:50 -0400 Subject: [PATCH 04/40] Revert "added a vs. commodores fast auton path in choreo" This reverts commit 1185c55a117a0d5bafaa7dd8bd9162280ddfd5c0. --- src/main/deploy/choreo/RightSweepFast.traj | 304 --------------------- 1 file changed, 304 deletions(-) delete mode 100644 src/main/deploy/choreo/RightSweepFast.traj diff --git a/src/main/deploy/choreo/RightSweepFast.traj b/src/main/deploy/choreo/RightSweepFast.traj deleted file mode 100644 index 8529c907..00000000 --- a/src/main/deploy/choreo/RightSweepFast.traj +++ /dev/null @@ -1,304 +0,0 @@ -{ - "name":"RightSweepFast", - "version":3, - "snapshot":{ - "waypoints":[ - {"x":3.592900037765503, "y":2.455909967422486, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.639349937438965, "y":2.475399971008301, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.646820068359375, "y":2.5143799781799316, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.841720104217529, "y":3.11857008934021, "heading":1.8086322542523472, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.750279903411865, "y":2.767750024795532, "heading":1.3068325198957504, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.658840179443359, "y":2.6508100032806396, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.592900037765503, "y":2.6897900104522705, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.5794200897216797, "y":2.6897900104522705, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.1969258785247803, "y":2.070849151611328, "heading":-0.7853984616619369, "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.5}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"3.592900037765503 m", "val":3.592900037765503}, "y":{"exp":"2.4559099674224854 m", "val":2.455909967422486}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.639349937438965 m", "val":5.639349937438965}, "y":{"exp":"2.475399971008301 m", "val":2.475399971008301}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.646820068359375 m", "val":7.646820068359375}, "y":{"exp":"2.5143799781799316 m", "val":2.5143799781799316}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.841720104217529 m", "val":7.841720104217529}, "y":{"exp":"3.11857008934021 m", "val":3.11857008934021}, "heading":{"exp":"1.8086322542523474 rad", "val":1.8086322542523472}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.750279903411865 m", "val":6.750279903411865}, "y":{"exp":"2.7677500247955322 m", "val":2.767750024795532}, "heading":{"exp":"1.3068325198957504 rad", "val":1.3068325198957504}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.658840179443359 m", "val":5.658840179443359}, "y":{"exp":"2.6508100032806396 m", "val":2.6508100032806396}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.592900037765503 m", "val":3.592900037765503}, "y":{"exp":"2.6897900104522705 m", "val":2.6897900104522705}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.5794200897216797 m", "val":2.5794200897216797}, "y":{"exp":"2.6897900104522705 m", "val":2.6897900104522705}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.1969258785247803 m", "val":2.1969258785247803}, "y":{"exp":"7.97 m - 5.899150848388672 m", "val":2.070849151611328}, "heading":{"exp":"-2.3561947884568335 rad + 90 deg", "val":-0.7853984616619369}, "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":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2.5 m / s", "val":2.5}}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":5, "to":6, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "config":{ - "frontLeft":{ - "x":0.2667, - "y":0.2794 - }, - "backLeft":{ - "x":-0.2667, - "y":0.2794 - }, - "mass":72.57477920000001, - "inertia":6.0, - "gearing":6.122448979591837, - "radius":0.0508, - "vmax":604.0235475301976, - "tmax":0.7, - "cof":2.255, - "bumper":{ - "front":0.41513125, - "side":0.42783125, - "back":0.41513125 - }, - "differentialTrackWidth":0.5588 - }, - "sampleType":"Swerve", - "waypoints":[0.0,1.08833,1.89954,2.66621,3.2942,3.69335,4.72798,5.20581,5.84852], - "samples":[ - {"t":0.0, "x":3.5929, "y":2.45591, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.64442, "ay":0.05761, "alpha":0.03331, "fx":[84.26438,84.26904,84.2695,84.26489], "fy":[1.23516,0.86013,0.85614,1.22944]}, - {"t":0.03887, "x":3.59641, "y":2.45595, "heading":0.0, "vx":0.18052, "vy":0.00224, "omega":0.00129, "ax":4.64401, "ay":0.0576, "alpha":0.034, "fx":[84.25679,84.26155,84.26206,84.25735], "fy":[1.23903,0.85618,0.85213,1.23317]}, - {"t":0.07774, "x":3.60693, "y":2.45608, "heading":0.00005, "vx":0.36103, "vy":0.00448, "omega":0.00262, "ax":4.64352, "ay":0.0576, "alpha":0.03483, "fx":[84.24784,84.25271,84.25327,84.24845], "fy":[1.24358,0.85151,0.8474,1.23758]}, - {"t":0.11661, "x":3.62447, "y":2.4563, "heading":0.00015, "vx":0.54152, "vy":0.00672, "omega":0.00397, "ax":4.64293, "ay":0.05759, "alpha":0.03581, "fx":[84.23711,84.24212,84.24275,84.23779], "fy":[1.24903,0.8459,0.84175,1.24287]}, - {"t":0.15548, "x":3.64903, "y":2.45661, "heading":0.00031, "vx":0.72198, "vy":0.00896, "omega":0.00536, "ax":4.64222, "ay":0.05758, "alpha":0.03701, "fx":[84.22403,84.2292,84.22992,84.2248], "fy":[1.25566,0.83905,0.83486,1.24933]}, - {"t":0.19434, "x":3.6806, "y":2.457, "heading":0.00051, "vx":0.90242, "vy":0.01119, "omega":0.0068, "ax":4.64133, "ay":0.05757, "alpha":0.03851, "fx":[84.20771,84.21309,84.21392,84.2086], "fy":[1.26392,0.8305,0.82629,1.25739]}, - {"t":0.23321, "x":3.71918, "y":2.45748, "heading":0.00078, "vx":1.08282, "vy":0.01343, "omega":0.0083, "ax":4.64019, "ay":0.05756, "alpha":0.04043, "fx":[84.1868,84.19245,84.19343,84.18785], "fy":[1.2745,0.81954,0.81531,1.26771]}, - {"t":0.27208, "x":3.76477, "y":2.45804, "heading":0.0011, "vx":1.26318, "vy":0.01567, "omega":0.00987, "ax":4.63867, "ay":0.05754, "alpha":0.04298, "fx":[84.15902,84.16502,84.16623,84.1603], "fy":[1.28855,0.80499,0.80074,1.28141]}, - {"t":0.31095, "x":3.81738, "y":2.45869, "heading":0.00149, "vx":1.44348, "vy":0.0179, "omega":0.01154, "ax":4.63656, "ay":0.05751, "alpha":0.04652, "fx":[84.12035,84.12684,84.1284,84.12198], "fy":[1.3081,0.78475,0.78049,1.30046]}, - {"t":0.34982, "x":3.87699, "y":2.45943, "heading":0.00193, "vx":1.6237, "vy":0.02014, "omega":0.01335, "ax":4.63343, "ay":0.05747, "alpha":0.0518, "fx":[84.0628,84.07003,84.07216,84.06503], "fy":[1.33717,0.75465,0.7504,1.32874]}, - {"t":0.38869, "x":3.9436, "y":2.46026, "heading":0.00245, "vx":1.8038, "vy":0.02237, "omega":0.01536, "ax":4.62827, "ay":0.05741, "alpha":0.06049, "fx":[83.96811,83.97654,83.97981,83.9715], "fy":[1.38498,0.70521,0.70104,1.37512]}, - {"t":0.42756, "x":4.0172, "y":2.46117, "heading":0.00305, "vx":1.98369, "vy":0.02461, "omega":0.01771, "ax":4.61823, "ay":0.05728, "alpha":0.07744, "fx":[83.78324,83.79401,83.80012,83.78955], "fy":[1.47814,0.6089,0.60515,1.4651]}, - {"t":0.46643, "x":4.0978, "y":2.46217, "heading":0.00374, "vx":2.1632, "vy":0.02683, "omega":0.02072, "ax":4.59006, "ay":0.05693, "alpha":0.12516, "fx":[83.26251,83.27982,83.29847,83.28165], "fy":[1.73916,0.33917,0.33876,1.71484]}, - {"t":0.5053, "x":4.18535, "y":2.46326, "heading":0.00454, "vx":2.34161, "vy":0.02904, "omega":0.02559, "ax":4.05176, "ay":0.05026, "alpha":1.088, "fx":[72.67832,72.81158,74.33709,74.22863], "fy":[6.60432,-4.71304,-4.09028,5.84682]}, - {"t":0.54416, "x":4.27942, "y":2.46443, "heading":0.00554, "vx":2.4991, "vy":0.031, "omega":0.06788, "ax":0.00191, "ay":0.00002, "alpha":6.03629, "fx":[-17.01129,-16.83196,17.08068,16.90137], "fy":[16.09476,-16.28191,-16.08863,16.27719]}, - {"t":0.58303, "x":4.37656, "y":2.46563, "heading":0.00818, "vx":2.49917, "vy":0.031, "omega":0.3025, "ax":0.0, "ay":0.0, "alpha":4.54135, "fx":[-12.8563,-12.65713,12.8563,12.65714], "fy":[12.07251,-12.28122,-12.07257,12.28116]}, - {"t":0.6219, "x":4.4737, "y":2.46684, "heading":0.01993, "vx":2.49917, "vy":0.031, "omega":0.47902, "ax":0.0, "ay":0.0, "alpha":2.93649, "fx":[-8.40424,-8.09031,8.40423,8.09031], "fy":[7.70792,-8.03687,-7.708,8.0368]}, - {"t":0.66077, "x":4.57084, "y":2.46804, "heading":0.03855, "vx":2.49917, "vy":0.031, "omega":0.59315, "ax":0.0, "ay":-0.00001, "alpha":1.25183, "fx":[-3.64329,-3.38453,3.6433,3.38453], "fy":[3.21852,-3.48986,-3.21878,3.4896]}, - {"t":0.69964, "x":4.66798, "y":2.46924, "heading":0.06161, "vx":2.49917, "vy":0.031, "omega":0.64181, "ax":0.0, "ay":-0.00003, "alpha":-0.46924, "fx":[1.39313,1.23819,-1.39312,-1.23818], "fy":[-1.17524,1.33647,1.17414,-1.33756]}, - {"t":0.73851, "x":4.76512, "y":2.47045, "heading":0.08655, "vx":2.49917, "vy":0.031, "omega":0.62357, "ax":0.0, "ay":-0.00013, "alpha":-2.17649, "fx":[6.59562,5.58665,-6.59561,-5.58654], "fy":[-5.28801,6.34047,5.28339,-6.34507]}, - {"t":0.77738, "x":4.86226, "y":2.47165, "heading":0.11079, "vx":2.49917, "vy":0.03099, "omega":0.53898, "ax":0.00001, "ay":-0.00052, "alpha":-3.82233, "fx":[11.80456,9.53872,-11.8049,-9.5379], "fy":[-9.00882,11.36421,8.98974,-11.38304]}, - {"t":0.81625, "x":4.9594, "y":2.47286, "heading":0.13174, "vx":2.49917, "vy":0.03097, "omega":0.39041, "ax":0.00003, "ay":-0.00206, "alpha":-5.37014, "fx":[16.84437,13.06574,-16.84785,-13.0604], "fy":[-12.33117,16.21962,12.2553,-16.29322]}, - {"t":0.85511, "x":5.05654, "y":2.47406, "heading":0.14692, "vx":2.49917, "vy":0.03089, "omega":0.18167, "ax":0.0001, "ay":-0.00773, "alpha":-6.79784, "fx":[21.54714,16.23676,-21.56994,-16.20706], "fy":[-15.38019,20.69037,15.092,-20.96332]}, - {"t":0.89398, "x":5.15368, "y":2.47526, "heading":0.15398, "vx":2.49918, "vy":0.03059, "omega":-0.08255, "ax":0.00033, "ay":-0.02757, "alpha":-8.09662, "fx":[25.74578,19.21834,-25.86568,-19.07437], "fy":[-18.48501,24.46094,17.44398,-25.42099]}, - {"t":0.93285, "x":5.25082, "y":2.47642, "heading":0.15077, "vx":2.49919, "vy":0.02952, "omega":-0.39726, "ax":0.00103, "ay":-0.09335, "alpha":-9.26335, "fx":[29.19464,22.31857,-29.7416,-21.69653], "fy":[-22.41861,26.84821,18.85516,-30.05962]}, - {"t":0.97172, "x":5.34796, "y":2.4775, "heading":0.13533, "vx":2.49923, "vy":0.02589, "omega":-0.75731, "ax":0.00241, "ay":-0.30039, "alpha":-10.25499, "fx":[31.2614,26.18026,-33.49639,-23.77053], "fy":[-28.97978,25.86036,17.45437,-36.13592]}, - {"t":1.01059, "x":5.44511, "y":2.47828, "heading":0.10589, "vx":2.49932, "vy":0.01422, "omega":-1.15591, "ax":-0.00139, "ay":-0.92321, "alpha":-10.61318, "fx":[29.94005,31.8962,-37.81351,-24.12355], "fy":[-41.32235,14.13169,6.26353,-46.07433]}, - {"t":1.04946, "x":5.54225, "y":2.47814, "heading":0.06096, "vx":2.49927, "vy":-0.02167, "omega":-1.56843, "ax":-0.05786, "ay":-2.50656, "alpha":-7.74978, "fx":[21.64766,31.83562,-36.72378,-20.95886], "fy":[-59.43144,-30.29124,-31.80778,-60.38263]}, - {"t":1.08833, "x":5.63935, "y":2.4754, "heading":0.0, "vx":2.49702, "vy":-0.1191, "omega":-1.86966, "ax":4.61853, "ay":-0.27623, "alpha":0.13948, "fx":[83.83743,83.74313,83.75889,83.84962], "fy":[-4.2757,-5.84494,-5.73315,-4.19374]}, - {"t":1.11837, "x":5.71646, "y":2.4717, "heading":-0.05617, "vx":2.63578, "vy":-0.1274, "omega":-1.86547, "ax":4.61535, "ay":-0.2392, "alpha":0.40001, "fx":[83.84368,83.60647,83.64137,83.86684], "fy":[-2.03613,-6.6217,-6.52009,-2.18184]}, - {"t":1.14842, "x":5.79773, "y":2.46776, "heading":-0.11222, "vx":2.77445, "vy":-0.13458, "omega":-1.85345, "ax":4.60804, "ay":-0.18733, "alpha":0.7669, "fx":[83.74531,83.41046,83.45511,83.81691], "fy":[1.39618,-7.55634,-7.74705,0.31196]}, - {"t":1.17846, "x":5.88317, "y":2.46363, "heading":-0.16791, "vx":2.9129, "vy":-0.14021, "omega":-1.83041, "ax":4.59069, "ay":-0.10973, "alpha":1.32495, "fx":[83.2962,83.0999,83.13614,83.63626], "fy":[6.95851,-8.78355,-9.67552,3.53699]}, - {"t":1.20851, "x":5.97276, "y":2.45937, "heading":-0.2229, "vx":3.05083, "vy":-0.14351, "omega":-1.7906, "ax":4.54424, "ay":0.01699, "alpha":2.27714, "fx":[81.5881,82.51182,82.52175,83.17564], "fy":[16.80008,-10.68833,-12.84856,7.96959]}, - {"t":1.23855, "x":6.06648, "y":2.45507, "heading":-0.2767, "vx":3.18736, "vy":-0.143, "omega":-1.72218, "ax":4.38691, "ay":0.23939, "alpha":4.21677, "fx":[74.39341,80.87173,81.0933,82.02061], "fy":[36.28138,-14.93371,-18.59792,14.62421]}, - {"t":1.2686, "x":6.16422, "y":2.45088, "heading":-0.32845, "vx":3.31917, "vy":-0.1358, "omega":-1.59549, "ax":3.47192, "ay":0.25403, "alpha":10.2094, "fx":[40.82787,55.98957,76.43729,78.71928], "fy":[71.08886,-47.51028,-31.10784,25.96511]}, - {"t":1.29864, "x":6.26551, "y":2.44691, "heading":-0.37638, "vx":3.42348, "vy":-0.12817, "omega":-1.28875, "ax":0.1187, "ay":0.77771, "alpha":20.44275, "fx":[-25.12594,-81.18842,48.50209,66.42667], "fy":[78.32259,-5.51634,-64.55271,48.18854]}, - {"t":1.32869, "x":6.36842, "y":2.44341, "heading":-0.4151, "vx":3.42705, "vy":-0.10481, "omega":-0.67454, "ax":-2.38626, "ay":1.06911, "alpha":15.28763, "fx":[-56.23632,-82.9774,-49.03919,15.07062], "fy":[61.038,1.32095,-64.42231,79.65346]}, - {"t":1.35873, "x":6.47031, "y":2.44075, "heading":-0.43537, "vx":3.35535, "vy":-0.07268, "omega":-0.21523, "ax":-3.75683, "ay":1.15556, "alpha":8.42608, "fx":[-67.27236,-83.41425,-74.61754,-47.34675], "fy":[49.31724,3.05531,-35.4272,66.91898]}, - {"t":1.38878, "x":6.56943, "y":2.43908, "heading":-0.44184, "vx":3.24248, "vy":-0.03797, "omega":0.03794, "ax":-4.17017, "ay":0.97195, "alpha":5.72255, "fx":[-72.21039,-83.62299,-79.71301,-67.10312], "fy":[42.22186,3.62707,-23.91901,48.60906]}, - {"t":1.41882, "x":6.66497, "y":2.43838, "heading":-0.4407, "vx":3.11718, "vy":-0.00876, "omega":0.20987, "ax":-4.32315, "ay":0.85724, "alpha":4.53535, "fx":[-74.87213,-83.75086,-81.51487,-73.61399], "fy":[37.62426,3.81256,-18.22014,38.99744]}, - {"t":1.44887, "x":6.75667, "y":2.43851, "heading":-0.43439, "vx":2.98729, "vy":0.01699, "omega":0.34613, "ax":-4.39829, "ay":0.78611, "alpha":3.87961, "fx":[-76.5062,-83.84,-82.37692,-76.48212], "fy":[34.41885,3.82222,-14.83475,33.64573]}, - {"t":1.47891, "x":6.84444, "y":2.43937, "heading":-0.42399, "vx":2.85515, "vy":0.04061, "omega":0.4627, "ax":-4.44213, "ay":0.73832, "alpha":3.46299, "fx":[-77.60674,-83.90748,-82.86779,-78.00491], "fy":[32.04404,3.73178,-12.57428,30.38208]}, - {"t":1.50896, "x":6.92822, "y":2.44093, "heading":-0.41009, "vx":2.72168, "vy":0.06279, "omega":0.56674, "ax":-4.47062, "ay":0.70408, "alpha":3.17415, "fx":[-78.40066,-83.96152,-83.18072,-78.91124], "fy":[30.19581,3.57535,-10.93601,28.26347]}, - {"t":1.539, "x":7.00797, "y":2.44313, "heading":-0.39306, "vx":2.58736, "vy":0.08395, "omega":0.66211, "ax":-4.49052, "ay":0.67835, "alpha":2.96177, "fx":[-79.0047,-84.00649,-83.39677,-79.49044], "fy":[28.69776,3.37085,-9.67208,26.83483]}, - {"t":1.56905, "x":7.08368, "y":2.44596, "heading":-0.37317, "vx":2.45244, "vy":0.10433, "omega":0.7511, "ax":-4.50517, "ay":0.65831, "alpha":2.79882, "fx":[-79.48435,-84.04491,-83.55512,-79.87704], "fy":[27.44112,3.12879,-8.64599,25.8529]}, - {"t":1.59909, "x":7.15533, "y":2.44939, "heading":-0.3506, "vx":2.31709, "vy":0.12411, "omega":0.83519, "ax":-4.51638, "ay":0.64226, "alpha":2.66971, "fx":[-79.87895,-84.07829,-83.67673,-80.1412], "fy":[26.35527,2.85597,-7.77598,25.17626]}, - {"t":1.62914, "x":7.22291, "y":2.45341, "heading":-0.32551, "vx":2.18139, "vy":0.14341, "omega":0.9154, "ax":-4.52523, "ay":0.6291, "alpha":2.56475, "fx":[-80.21341,-84.10753,-83.77369,-80.32288], "fy":[25.39241,2.55715,-7.00964,24.71691]}, - {"t":1.65918, "x":7.28641, "y":2.458, "heading":-0.29801, "vx":2.04543, "vy":0.16231, "omega":0.99246, "ax":-4.53239, "ay":0.61812, "alpha":2.47761, "fx":[-80.50423,-84.1332,-83.85336,-80.4465], "fy":[24.51891,2.23594,-6.31147,24.41686]}, - {"t":1.68923, "x":7.34582, "y":2.46316, "heading":-0.26819, "vx":1.90926, "vy":0.18088, "omega":1.0669, "ax":-4.53831, "ay":0.60883, "alpha":2.40395, "fx":[-80.76269,-84.15566,-83.92039,-80.5279], "fy":[23.71042,1.89529,-5.65621,24.23594]}, - {"t":1.71927, "x":7.40113, "y":2.46887, "heading":-0.23613, "vx":1.7729, "vy":0.19917, "omega":1.13912, "ax":-4.54328, "ay":0.60085, "alpha":2.34071, "fx":[-80.99676,-84.17509,-83.97781,-80.57789], "fy":[22.94878,1.53778,-5.0251,24.14509]}, - {"t":1.74932, "x":7.45235, "y":2.47512, "heading":-0.20191, "vx":1.6364, "vy":0.21722, "omega":1.20945, "ax":-4.54752, "ay":0.59393, "alpha":2.28563, "fx":[-81.21217,-84.19163,-84.02755,-80.60425], "fy":[22.22017,1.16577,-4.40373,24.12244]}, - {"t":1.77936, "x":7.49946, "y":2.48192, "heading":-0.16557, "vx":1.49977, "vy":0.23507, "omega":1.27812, "ax":-4.5512, "ay":0.58789, "alpha":2.23703, "fx":[-81.41312,-84.20534,-84.07082,-80.61278], "fy":[21.51385,0.78154,-3.78065,24.1509]}, - {"t":1.80941, "x":7.54247, "y":2.48924, "heading":-0.12717, "vx":1.36303, "vy":0.25273, "omega":1.34533, "ax":-4.55441, "ay":0.58256, "alpha":2.19364, "fx":[-81.6027,-84.21625,-84.1083,-80.60806], "fy":[20.82133,0.38735,-3.14654,24.21668]}, - {"t":1.83945, "x":7.58137, "y":2.4971, "heading":-0.08675, "vx":1.22619, "vy":0.27023, "omega":1.41124, "ax":-4.55725, "ay":0.57783, "alpha":2.15449, "fx":[-81.7832,-84.2244,-84.14024,-80.5938], "fy":[20.13584,-0.01445,-2.49369,24.3083]}, - {"t":1.8695, "x":7.61615, "y":2.50548, "heading":-0.04435, "vx":1.08927, "vy":0.2876, "omega":1.47597, "ax":-4.55979, "ay":0.57363, "alpha":2.11883, "fx":[-81.95631,-84.22983,-84.16657,-80.57317], "fy":[19.45192,-0.42143,-1.81565,24.416]}, - {"t":1.89954, "x":7.64682, "y":2.51438, "heading":0.0, "vx":0.95227, "vy":0.30483, "omega":1.53963, "ax":-1.56341, "ay":4.05255, "alpha":5.45127, "fx":[-41.75707,-64.18288,-5.06905,-2.45519], "fy":[72.90067,53.91589,83.37239,83.92366]}, - {"t":1.92278, "x":7.66852, "y":2.52256, "heading":0.03577, "vx":0.91595, "vy":0.39898, "omega":1.66628, "ax":-1.88866, "ay":3.82115, "alpha":6.08756, "fx":[-48.26115,-72.13402,-10.50717,-6.16667], "fy":[68.6858,42.48978,82.5054,83.63845]}, - {"t":1.94601, "x":7.68929, "y":2.53286, "heading":0.07448, "vx":0.87207, "vy":0.48775, "omega":1.80771, "ax":-2.19811, "ay":3.52824, "alpha":6.77732, "fx":[-54.78569,-78.56823,-16.42675,-9.74696], "fy":[63.49016,28.54372,80.8592,83.16834]}, - {"t":1.96924, "x":7.70896, "y":2.54514, "heading":0.11648, "vx":0.821, "vy":0.56972, "omega":1.96516, "ax":-2.43957, "ay":3.19191, "alpha":7.54027, "fx":[-60.80238,-82.39072,-21.2234,-12.63465], "fy":[57.5944,13.21487,78.2385,82.60433]}, - {"t":1.99247, "x":7.72737, "y":2.55924, "heading":0.16213, "vx":0.76432, "vy":0.64388, "omega":2.14034, "ax":-2.58374, "ay":2.80796, "alpha":8.46958, "fx":[-66.18359,-83.23925,-23.13293,-14.95886], "fy":[51.09237,-2.32955,73.04356,81.98099]}, - {"t":2.01571, "x":7.74443, "y":2.57495, "heading":0.21186, "vx":0.7043, "vy":0.70911, "omega":2.3371, "ax":-2.17174, "ay":2.01693, "alpha":12.28291, "fx":[-70.57668,-81.47307,11.1805,-16.74399], "fy":[44.50291,-16.20324,36.74421,81.33449]}, - {"t":2.03894, "x":7.76021, "y":2.59197, "heading":0.26615, "vx":0.65384, "vy":0.75597, "omega":2.62246, "ax":-1.34779, "ay":1.11893, "alpha":17.70652, "fx":[-72.41014,-78.69369,67.8335,-14.54499], "fy":[40.98367,-25.54677,-15.66995,81.4389]}, - {"t":2.06217, "x":7.77504, "y":2.60984, "heading":0.32708, "vx":0.62253, "vy":0.78197, "omega":3.03383, "ax":-1.20977, "ay":0.92867, "alpha":18.52925, "fx":[-74.01112,-74.51362,73.06295,-12.33694], "fy":[37.29106,-34.85097,-16.4277,81.38536]}, - {"t":2.0854, "x":7.78917, "y":2.62825, "heading":0.39756, "vx":0.59443, "vy":0.80354, "omega":3.4643, "ax":-1.10108, "ay":0.78812, "alpha":18.96917, "fx":[-75.53694,-68.51265,75.07109,-10.93194], "fy":[32.87249,-44.2327,-12.43246,80.99006]}, - {"t":2.10863, "x":7.80269, "y":2.64714, "heading":0.47805, "vx":0.56884, "vy":0.82185, "omega":3.905, "ax":-0.99556, "ay":0.66878, "alpha":19.16372, "fx":[-76.70823,-60.47126,75.49335,-10.56652], "fy":[27.71027,-52.95733,-6.31243,80.0961]}, - {"t":2.13187, "x":7.81563, "y":2.66641, "heading":0.56877, "vx":0.54572, "vy":0.83739, "omega":4.35022, "ax":-0.90259, "ay":0.57234, "alpha":18.97502, "fx":[-76.9551,-50.45751,73.61496,-11.70783], "fy":[21.74822,-59.63802,1.2712,78.15583]}, - {"t":2.1551, "x":7.82807, "y":2.68602, "heading":0.66983, "vx":0.52475, "vy":0.85068, "omega":4.79105, "ax":-0.89789, "ay":0.53886, "alpha":17.58111, "fx":[-74.1453,-40.00452,64.73859,-15.75269], "fy":[15.2457,-59.71389,10.95933,72.61641]}, - {"t":2.17833, "x":7.84002, "y":2.70593, "heading":0.78114, "vx":0.50389, "vy":0.8632, "omega":5.1995, "ax":-1.82653, "ay":1.00753, "alpha":0.58386, "fx":[-35.13635,-33.59692,-31.10548,-32.72101], "fy":[17.87024,15.75941,18.73146,20.76019]}, - {"t":2.20156, "x":7.85123, "y":2.72625, "heading":0.90194, "vx":0.46145, "vy":0.88661, "omega":5.21306, "ax":-0.71428, "ay":0.36335, "alpha":-17.95883, "fx":[63.45556,-24.20428,-74.64515,-16.4446], "fy":[25.313,69.88457,-0.16803,-68.65921]}, - {"t":2.2248, "x":7.86176, "y":2.74695, "heading":1.02305, "vx":0.44486, "vy":0.89505, "omega":4.79584, "ax":-0.72323, "ay":0.35108, "alpha":-19.48298, "fx":[66.12526,-32.70599,-78.87698,-7.03069], "fy":[38.04669,72.01411,-7.97848,-76.60258]}, - {"t":2.24803, "x":7.8719, "y":2.76784, "heading":1.13446, "vx":0.42806, "vy":0.90321, "omega":4.3432, "ax":-0.87095, "ay":0.40103, "alpha":-19.77479, "fx":[59.64247,-41.42983,-79.90851,-1.51316], "fy":[51.75131,69.59611,-13.2618,-78.98076]}, - {"t":2.27126, "x":7.88161, "y":2.78893, "heading":1.23537, "vx":0.40782, "vy":0.91253, "omega":3.88379, "ax":-1.08028, "ay":0.46538, "alpha":-19.6373, "fx":[48.6378,-48.60204,-80.22975,1.7928], "fy":[64.02714,66.02916,-16.40866,-79.87268]}, - {"t":2.29449, "x":7.89079, "y":2.81026, "heading":1.3256, "vx":0.38272, "vy":0.92334, "omega":3.42757, "ax":-1.33375, "ay":0.52722, "alpha":-19.20771, "fx":[35.01177,-54.04399,-80.55667,2.79224], "fy":[73.45821,62.46368,-17.47975,-80.17893]}, - {"t":2.31773, "x":7.89932, "y":2.83185, "heading":1.40523, "vx":0.35174, "vy":0.93559, "omega":2.98133, "ax":-1.63107, "ay":0.57617, "alpha":-18.4881, "fx":[20.69192,-58.09751,-81.0895,0.12024], "fy":[79.43195,59.2865,-16.74336,-80.15939]}, - {"t":2.34096, "x":7.90705, "y":2.85374, "heading":1.47449, "vx":0.31384, "vy":0.94897, "omega":2.55181, "ax":-2.0182, "ay":0.61191, "alpha":-17.30184, "fx":[6.92601,-61.43588,-81.70997,-10.2509], "fy":[82.30225,56.24652,-15.0204,-79.11904]}, - {"t":2.36419, "x":7.9138, "y":2.87595, "heading":1.53377, "vx":0.26696, "vy":0.96319, "omega":2.14985, "ax":-2.58313, "ay":0.62758, "alpha":-15.26066, "fx":[-7.27758,-65.72254,-81.88397,-32.58611], "fy":[82.58274,51.49927,-15.39985,-73.13557]}, - {"t":2.38742, "x":7.9193, "y":2.8985, "heading":1.58372, "vx":0.20695, "vy":0.97777, "omega":1.79531, "ax":-3.13028, "ay":0.53987, "alpha":-13.06826, "fx":[-24.3285,-71.41686,-81.19466,-50.23902], "fy":[79.41939,43.53632,-19.62214,-64.15245]}, - {"t":2.41065, "x":7.92327, "y":2.92136, "heading":1.62543, "vx":0.13422, "vy":0.99031, "omega":1.49171, "ax":-3.5454, "ay":0.32159, "alpha":-11.19344, "fx":[-42.55721,-76.33615,-80.11257,-58.30079], "fy":[71.49368,34.46923,-24.24329,-58.38037]}, - {"t":2.43389, "x":7.92543, "y":2.94445, "heading":1.66009, "vx":0.05185, "vy":0.99778, "omega":1.23166, "ax":-3.84581, "ay":-0.21567, "alpha":-9.53396, "fx":[-62.993,-80.9801,-77.56711,-57.56879], "fy":[54.49109,21.74418,-31.81841,-60.06887]}, - {"t":2.45712, "x":7.9256, "y":2.96758, "heading":1.6887, "vx":-0.03749, "vy":0.99277, "omega":1.01016, "ax":-3.93337, "ay":-1.37253, "alpha":-7.14004, "fx":[-82.3068,-83.92188,-71.01507,-48.21958], "fy":[13.26037,0.30164,-44.79444,-68.37879]}, - {"t":2.48035, "x":7.92366, "y":2.99027, "heading":1.71217, "vx":-0.12887, "vy":0.96088, "omega":0.84429, "ax":-3.74879, "ay":-2.22949, "alpha":-5.27676, "fx":[-80.94673,-82.39406,-65.46472,-43.26232], "fy":[-20.91589,-16.31769,-52.70733,-71.86418]}, - {"t":2.50358, "x":7.91966, "y":3.01199, "heading":1.73178, "vx":-0.21597, "vy":0.90909, "omega":0.72169, "ax":-3.51648, "ay":-2.72949, "alpha":-4.28757, "fx":[-74.08523,-79.33269,-61.32439,-40.46522], "fy":[-39.15853,-27.77316,-57.5549,-73.60569]}, - {"t":2.52682, "x":7.91369, "y":3.03238, "heading":1.74855, "vx":-0.29766, "vy":0.84568, "omega":0.62208, "ax":-3.32488, "ay":-3.03032, "alpha":-3.70895, "fx":[-68.22066,-76.15998,-58.22463,-38.69742], "fy":[-48.87825,-35.67148,-60.74486,-74.63008]}, - {"t":2.55005, "x":7.90588, "y":3.0512, "heading":1.763, "vx":-0.37491, "vy":0.77528, "omega":0.53592, "ax":-3.17624, "ay":-3.22602, "alpha":-3.32504, "fx":[-63.86063,-73.31613,-55.85075,-37.48736], "fy":[-54.58033,-41.27416,-62.97445,-75.29899]}, - {"t":2.57328, "x":7.89631, "y":3.06835, "heading":1.77545, "vx":-0.4487, "vy":0.70033, "omega":0.45867, "ax":-3.06033, "ay":-3.36202, "alpha":-3.04852, "fx":[-60.62282,-70.8791,-53.98935,-36.61123], "fy":[-58.23969,-45.38364,-64.60733,-75.76754]}, - {"t":2.59651, "x":7.88506, "y":3.08371, "heading":1.78611, "vx":-0.5198, "vy":0.62222, "omega":0.38784, "ax":-2.9683, "ay":-3.46144, "alpha":-2.83855, "fx":[-58.15826,-68.81588,-52.49924,-35.95042], "fy":[-60.7598,-48.49376,-65.84712,-76.11247]}, - {"t":2.61974, "x":7.87218, "y":3.09723, "heading":1.79512, "vx":-0.58876, "vy":0.5418, "omega":0.3219, "ax":-2.89384, "ay":-3.53699, "alpha":-2.67315, "fx":[-56.22908,-67.06851,-51.28563,-35.43658], "fy":[-62.59268,-50.91288,-66.81519,-76.37582]}, - {"t":2.64298, "x":7.85772, "y":3.10886, "heading":1.8026, "vx":-0.65599, "vy":0.45963, "omega":0.2598, "ax":-2.83254, "ay":-3.5962, "alpha":-2.53922, "fx":[-54.67921,-65.5809,-50.28335,-35.02774], "fy":[-63.98417,-52.83893,-67.58778,-76.58244]}, - {"t":2.66621, "x":7.84172, "y":3.11857, "heading":1.80863, "vx":-0.72179, "vy":0.37608, "omega":0.2008, "ax":-2.82032, "ay":-3.61261, "alpha":-2.424, "fx":[-54.28272,-64.80193,-50.04509,-35.55442], "fy":[-64.31407,-53.78135,-67.75661,-76.33213]}, - {"t":2.68786, "x":7.82543, "y":3.12587, "heading":1.81298, "vx":-0.78287, "vy":0.29785, "omega":0.14831, "ax":-2.84665, "ay":-3.58871, "alpha":-2.47026, "fx":[-55.04916,-65.36053,-50.38365,-35.80147], "fy":[-63.64781,-53.09289,-67.49962,-76.20958]}, - {"t":2.70952, "x":7.80781, "y":3.13148, "heading":1.81619, "vx":-0.84451, "vy":0.22014, "omega":0.09482, "ax":-2.87481, "ay":-3.5627, "alpha":-2.5196, "fx":[-55.84935,-65.95428,-50.76177,-36.07309], "fy":[-62.93424,-52.34456,-67.20969,-76.07386]}, - {"t":2.73117, "x":7.78885, "y":3.13541, "heading":1.81825, "vx":-0.90677, "vy":0.14299, "omega":0.04026, "ax":-2.90499, "ay":-3.5343, "alpha":-2.57239, "fx":[-56.68824,-66.58612,-51.1828,-36.37184], "fy":[-62.1658,-51.52864,-66.88311,-75.92336]}, - {"t":2.75283, "x":7.76853, "y":3.13768, "heading":1.81912, "vx":-0.96967, "vy":0.06645, "omega":-0.01545, "ax":-2.93741, "ay":-3.50316, "alpha":-2.62902, "fx":[-57.5715,-67.25917,-51.65049,-36.70073], "fy":[-61.33333,-50.63623,-66.51558,-75.75612]}, - {"t":2.77448, "x":7.74684, "y":3.13829, "heading":1.81878, "vx":-1.03328, "vy":-0.00941, "omega":-0.07238, "ax":-2.97231, "ay":-3.4689, "alpha":-2.68999, "fx":[-58.50567,-67.97671,-52.16906,-37.06338], "fy":[-60.42575,-49.65697,-66.10202,-75.56978]}, - {"t":2.79614, "x":7.72377, "y":3.13728, "heading":1.81722, "vx":-1.09765, "vy":-0.08453, "omega":-0.13063, "ax":-3.00997, "ay":-3.43103, "alpha":-2.75586, "fx":[-59.49826,-68.74211,-52.74331,-37.46411], "fy":[-59.42945,-48.57875,-65.63644,-75.36147]}, - {"t":2.81779, "x":7.69929, "y":3.13464, "heading":1.81439, "vx":-1.16283, "vy":-0.15883, "omega":-0.19031, "ax":-3.05069, "ay":-3.38898, "alpha":-2.82731, "fx":[-60.5579,-69.55881,-53.37868,-37.90812], "fy":[-58.32768,-47.38735,-65.11172,-75.12765]}, - {"t":2.83945, "x":7.6734, "y":3.13041, "heading":1.81027, "vx":-1.22889, "vy":-0.23222, "omega":-0.25154, "ax":-3.09484, "ay":-3.34205, "alpha":-2.90516, "fx":[-61.69451,-70.43015,-54.08137,-38.40167], "fy":[-57.09956,-46.06598,-64.51933,-74.86396]}, - {"t":2.8611, "x":7.64606, "y":3.12459, "heading":1.80482, "vx":-1.29591, "vy":-0.30459, "omega":-0.31445, "ax":-3.14282, "ay":-3.2894, "alpha":-2.99039, "fx":[-62.91935,-71.35925,-54.85841,-38.95235], "fy":[-55.71888,-44.59474,-63.849,-74.56496]}, - {"t":2.88276, "x":7.61726, "y":3.11723, "heading":1.79801, "vx":-1.36397, "vy":-0.37582, "omega":-0.3792, "ax":-3.19506, "ay":-3.22997, "alpha":-3.08423, "fx":[-64.24514,-72.3487,-55.71786,-39.56943], "fy":[-54.1524,-42.94995,-63.08828,-74.22379]}, - {"t":2.90441, "x":7.58697, "y":3.10833, "heading":1.7898, "vx":-1.43316, "vy":-0.44576, "omega":-0.44599, "ax":-3.25208, "ay":-3.16246, "alpha":-3.18817, "fx":[-65.68591,-73.40013,-56.66887,-40.26428], "fy":[-52.35753,-41.10328,-62.22195,-73.83176]}, - {"t":2.92607, "x":7.55518, "y":3.09794, "heading":1.78014, "vx":-1.50358, "vy":-0.51425, "omega":-0.51503, "ax":-3.31442, "ay":-3.08522, "alpha":-3.30409, "fx":[-67.25659,-74.51353,-57.72189,-41.05097], "fy":[-50.27921,-39.02081,-61.23125,-73.37764]}, - {"t":2.94772, "x":7.52184, "y":3.08608, "heading":1.76899, "vx":-1.57536, "vy":-0.58106, "omega":-0.58658, "ax":-3.38263, "ay":-2.99618, "alpha":-3.43441, "fx":[-68.97189,-75.68628,-58.88871,-41.94704], "fy":[-47.84548,-36.66186,-60.09289,-72.84678]}, - {"t":2.96938, "x":7.48693, "y":3.07279, "heading":1.75628, "vx":-1.64861, "vy":-0.64594, "omega":-0.66096, "ax":-3.4573, "ay":-2.89269, "alpha":-3.58222, "fx":[-70.84403,-76.91162,-60.18262,-42.97461], "fy":[-44.96164,-33.97764,-58.77767,-72.21974]}, - {"t":2.99103, "x":7.45042, "y":3.05813, "heading":1.74197, "vx":-1.72347, "vy":-0.70858, "omega":-0.73853, "ax":-3.5389, "ay":-2.77137, "alpha":-3.75162, "fx":[-72.87808,-78.17646,-61.61827,-44.16175], "fy":[-41.50229,-30.90993,-57.2487,-71.4703]}, - {"t":3.01269, "x":7.41227, "y":3.04213, "heading":1.72598, "vx":-1.80011, "vy":-0.7686, "omega":-0.81977, "ax":-3.62767, "ay":-2.62781, "alpha":-3.94802, "fx":[-75.06298,-79.45813,-63.21143,-45.5445], "fy":[-37.3014,-27.38984,-55.45895,-70.56238]}, - {"t":3.03434, "x":7.37244, "y":3.02487, "heading":1.70823, "vx":-1.87867, "vy":-0.8255, "omega":-0.90526, "ax":-3.72337, "ay":-2.45639, "alpha":-4.1787, "fx":[-77.35492,-80.71973,-64.97824,-47.16954], "fy":[-32.14141,-23.33723,-53.348,-69.44522]}, - {"t":3.056, "x":7.33088, "y":3.00642, "heading":1.68862, "vx":-1.9593, "vy":-0.87869, "omega":-0.99575, "ax":-3.82478, "ay":-2.24996, "alpha":-4.45321, "fx":[-79.64764,-81.90382,-66.93355,-49.09773], "fy":[-25.74559,-18.66156,-50.83765,-68.04566]}, - {"t":3.07765, "x":7.28756, "y":2.98686, "heading":1.66706, "vx":-2.04212, "vy":-0.92742, "omega":-1.09219, "ax":-3.92897, "ay":-1.99978, "alpha":-4.78342, "fx":[-81.7235,-82.92401,-69.08774,-51.40864], "fy":[-17.78593,-13.26566,-47.82633,-66.2555]}, - {"t":3.09931, "x":7.24241, "y":2.96631, "heading":1.64341, "vx":-2.1272, "vy":-0.97072, "omega":-1.19577, "ax":-4.03014, "ay":-1.69591, "alpha":-5.18175, "fx":[-83.18486,-83.65537,-71.44072,-54.2055], "fy":[-7.93377,-7.05478,-44.18208,-63.90995]}, - {"t":3.12096, "x":7.1954, "y":2.94489, "heading":1.61751, "vx":-2.21448, "vy":-1.00745, "omega":-1.30798, "ax":-4.1186, "ay":-1.32882, "alpha":-5.65448, "fx":[-83.39225,-83.92527,-73.97101,-57.618], "fy":[4.00114,0.04598,-39.73502,-60.75075]}, - {"t":3.14262, "x":7.14648, "y":2.92277, "heading":1.58919, "vx":-2.30367, "vy":-1.03622, "omega":-1.43043, "ax":-4.18081, "ay":-0.8923, "alpha":-6.18525, "fx":[-81.50025,-83.51,-76.61682,-61.79437], "fy":[17.8122,8.06276,-34.27162,-56.36201]}, - {"t":3.16427, "x":7.09562, "y":2.90012, "heading":1.55821, "vx":-2.3942, "vy":-1.05555, "omega":-1.56437, "ax":-4.20237, "ay":-0.3865, "alpha":-6.70788, "fx":[-76.73858,-82.14606,-79.2455,-66.85602], "fy":[32.61813,16.93239,-27.53735,-50.06328]}, - {"t":3.18593, "x":7.04278, "y":2.87717, "heading":1.52434, "vx":-2.4852, "vy":-1.06392, "omega":-1.70963, "ax":-4.17299, "ay":0.18376, "alpha":-7.08493, "fx":[-68.93637,-79.56779,-81.60968,-72.73967], "fy":[46.89836,26.45807,-19.26166,-40.75848]}, - {"t":3.20758, "x":6.98799, "y":2.85417, "heading":1.48731, "vx":-2.57557, "vy":-1.05994, "omega":-1.86306, "ax":-4.08537, "ay":0.81688, "alpha":-7.13071, "fx":[-58.8591,-75.57767,-83.29994,-78.75821], "fy":[59.09512,36.28334,-9.22821,-26.86562]}, - {"t":3.22924, "x":6.93126, "y":2.83141, "heading":1.44697, "vx":-2.66404, "vy":-1.04225, "omega":-2.01747, "ax":-3.92023, "ay":1.51482, "alpha":-6.71356, "fx":[-47.88188,-70.1348,-83.73045,-82.76283], "fy":[68.36257,45.91666,2.58731,-6.92889]}, - {"t":3.25089, "x":6.87265, "y":2.8092, "heading":1.40328, "vx":-2.74893, "vy":-1.00944, "omega":-2.16286, "ax":-3.63645, "ay":2.25163, "alpha":-5.91774, "fx":[-37.28177,-63.41705,-82.23019,-80.98524], "fy":[74.75297,54.82526,15.83113,18.00207]}, - {"t":3.27255, "x":6.81227, "y":2.78786, "heading":1.35644, "vx":-2.82768, "vy":-0.96068, "omega":-2.29101, "ax":-3.21525, "ay":2.9398, "alpha":-5.05654, "fx":[-27.81849,-55.80594,-78.29632,-71.42536], "fy":[78.844,62.56999,29.65729,42.28387]}, - {"t":3.2942, "x":6.75028, "y":2.76775, "heading":1.30683, "vx":-2.8973, "vy":-0.89702, "omega":-2.4005, "ax":-2.86988, "ay":3.26035, "alpha":-4.81332, "fx":[-20.76906,-49.96184,-74.40152,-63.14823], "fy":[80.39653,66.80315,37.21477,52.20484]}, - {"t":3.3063, "x":6.71503, "y":2.75714, "heading":1.2778, "vx":-2.93202, "vy":-0.85759, "omega":-2.45872, "ax":-2.69989, "ay":3.37697, "alpha":-4.94512, "fx":[-15.73261,-46.94917,-73.1206,-60.14151], "fy":[81.45323,68.88637,39.49761,55.24598]}, - {"t":3.31839, "x":6.67937, "y":2.74701, "heading":1.24806, "vx":-2.96467, "vy":-0.81674, "omega":-2.51854, "ax":-2.49741, "ay":3.5036, "alpha":-5.07061, "fx":[-10.08219,-43.49465,-71.55863,-56.11362], "fy":[82.26383,71.04714,42.07427,58.88786]}, - {"t":3.33049, "x":6.64332, "y":2.73739, "heading":1.2176, "vx":-2.99488, "vy":-0.77437, "omega":-2.57987, "ax":-2.25354, "ay":3.63976, "alpha":-5.18516, "fx":[-3.79443,-39.52293,-69.629,-50.60366], "fy":[82.71294,73.2567,44.99504,63.18979]}, - {"t":3.34259, "x":6.60694, "y":2.72829, "heading":1.18639, "vx":-3.02214, "vy":-0.73034, "omega":-2.64258, "ax":-1.95638, "ay":3.78284, "alpha":-5.28809, "fx":[3.1166,-34.95082,-67.21143,-42.93853], "fy":[82.66454,75.46795,48.31645,68.09004]}, - {"t":3.35468, "x":6.57024, "y":2.71973, "heading":1.15443, "vx":-3.0458, "vy":-0.68459, "omega":-2.70654, "ax":-1.59119, "ay":3.92537, "alpha":-5.3913, "fx":[10.58234,-29.69154,-64.13752,-32.2335], "fy":[81.97286,77.60858,52.09695,73.20477]}, - {"t":3.36678, "x":6.53328, "y":2.71174, "heading":1.12169, "vx":-3.06504, "vy":-0.63711, "omega":-2.77175, "ax":-1.144, "ay":4.05043, "alpha":-5.53121, "fx":[18.4662,-23.66354,-60.17112,-17.65697], "fy":[80.50292,79.5737,56.3852,77.49688]}, - {"t":3.37887, "x":6.49613, "y":2.70433, "heading":1.08817, "vx":-3.07888, "vy":-0.58812, "omega":-2.83865, "ax":-0.61337, "ay":4.12936, "alpha":-5.76436, "fx":[26.56148,-16.80558,-54.98544,0.71456], "fy":[78.15937,81.21986,61.19285,79.11507]}, - {"t":3.39097, "x":6.45884, "y":2.69752, "heading":1.05383, "vx":-3.0863, "vy":-0.53817, "omega":-2.90837, "ax":-0.02548, "ay":4.13348, "alpha":-6.10716, "fx":[34.60803,-9.0991,-48.14721,20.78935], "fy":[74.91619,82.36426,66.43839,76.26733]}, - {"t":3.40306, "x":6.42151, "y":2.69131, "heading":1.01866, "vx":-3.08661, "vy":-0.48818, "omega":-2.98224, "ax":0.57347, "ay":4.05831, "alpha":-6.45335, "fx":[42.32786,-0.59656,-39.13921,39.02723], "fy":[70.83542,82.79487,71.84714,69.0533]}, - {"t":3.41516, "x":6.38422, "y":2.6857, "heading":0.98259, "vx":-3.07967, "vy":-0.43909, "omega":-3.0603, "ax":1.1514, "ay":3.92438, "alpha":-6.61563, "fx":[49.47016,8.55127,-27.48916,53.03027], "fy":[66.06484,82.29727,76.81564,59.63347]}, - {"t":3.42725, "x":6.34705, "y":2.68068, "heading":0.94557, "vx":-3.06575, "vy":-0.39163, "omega":-3.14031, "ax":1.70212, "ay":3.74923, "alpha":-6.46811, "fx":[55.84991,18.08261,-13.08863,62.68699], "fy":[60.81346,80.69931,80.33755,50.24944]}, - {"t":3.43935, "x":6.3101, "y":2.67622, "heading":0.90759, "vx":-3.04516, "vy":-0.34628, "omega":-3.21855, "ax":2.22426, "ay":3.53426, "alpha":-6.01547, "fx":[61.3677,27.64272,3.36076,69.05414], "fy":[55.31399,77.92351,81.22593,42.03465]}, - {"t":3.45144, "x":6.27343, "y":2.67229, "heading":0.86866, "vx":-3.01826, "vy":-0.30353, "omega":-3.2913, "ax":2.70477, "ay":3.2768, "alpha":-5.36571, "fx":[66.00754,36.8388,20.21621,73.23553], "fy":[49.78572,74.02486,78.7627,35.23942]}, - {"t":3.46354, "x":6.23712, "y":2.66886, "heading":0.82885, "vx":-2.98554, "vy":-0.2639, "omega":-3.3562, "ax":3.1238, "ay":2.98473, "alpha":-4.6572, "fx":[69.81859,45.31618,35.54721,76.02708], "fy":[44.40866,69.19112,73.28682,29.72982]}, - {"t":3.47563, "x":6.20124, "y":2.66588, "heading":0.78826, "vx":-2.94776, "vy":-0.2278, "omega":-3.41253, "ax":3.46904, "ay":2.67691, "alpha":-3.99006, "fx":[72.89062,52.8238,48.11304,77.93726], "fy":[39.31234,63.70145,65.98912,25.27333]}, - {"t":3.48773, "x":6.16584, "y":2.66332, "heading":0.74698, "vx":-2.9058, "vy":-0.19542, "omega":-3.46079, "ax":3.74159, "ay":2.37318, "alpha":-3.40591, "fx":[75.33127,59.24266,57.69142,79.27989], "fy":[34.57703,57.86265,58.14594,21.64763]}, - {"t":3.49982, "x":6.13097, "y":2.66113, "heading":0.70512, "vx":-2.86054, "vy":-0.16671, "omega":-3.50199, "ax":3.9515, "ay":2.08716, "alpha":-2.90766, "fx":[77.24928,64.57389,64.70723,80.24877], "fy":[30.24223,51.95191,50.61179,18.66957]}, - {"t":3.51192, "x":6.09666, "y":2.65927, "heading":0.66276, "vx":-2.81275, "vy":-0.14147, "omega":-3.53716, "ax":4.11136, "ay":1.82544, "alpha":-2.48364, "fx":[78.74448,68.90273,69.76846,80.9655], "fy":[26.31742,46.18365,43.78353,16.19598]}, - {"t":3.52401, "x":6.06294, "y":2.65769, "heading":0.61998, "vx":-2.76302, "vy":-0.11939, "omega":-3.5672, "ax":4.23273, "ay":1.58973, "alpha":-2.12034, "fx":[79.90326,72.35851,73.41941,81.50798], "fy":[22.79216,40.70176,37.76336,14.11686]}, - {"t":3.53611, "x":6.02983, "y":2.65636, "heading":0.57684, "vx":-2.71183, "vy":-0.10016, "omega":-3.59284, "ax":4.32494, "ay":1.37914, "alpha":-1.80638, "fx":[80.79742,75.0831,76.07359,81.92723], "fy":[19.64417,35.58827,32.51054,12.34795]}, - {"t":3.5482, "x":5.99734, "y":2.65525, "heading":0.53338, "vx":-2.65951, "vy":-0.08348, "omega":-3.61469, "ax":4.39517, "ay":1.1916, "alpha":-1.53295, "fx":[81.48519,77.21141,78.02429,82.25746], "fy":[16.84503,30.87858,27.93178,10.82442]}, - {"t":3.5603, "x":5.9655, "y":2.65433, "heading":0.48966, "vx":-2.60635, "vy":-0.06907, "omega":-3.63323, "ax":4.44881, "ay":1.02462, "alpha":-1.29326, "fx":[82.01293,78.86189,79.47426,82.52203], "fy":[14.36397,26.57673,23.92456,9.49603]}, - {"t":3.57239, "x":5.9343, "y":2.65357, "heading":0.44571, "vx":-2.55254, "vy":-0.05668, "omega":-3.64888, "ax":4.48987, "ay":0.87573, "alpha":-1.08196, "fx":[82.41715,80.13368,80.56295,82.73723], "fy":[12.17027,22.66771,20.39476,8.32354]}, - {"t":3.58449, "x":5.90375, "y":2.65295, "heading":0.40158, "vx":-2.49824, "vy":-0.04608, "omega":-3.66196, "ax":4.52134, "ay":0.74267, "alpha":-0.89476, "fx":[82.72636,81.10733,81.38685,82.91462], "fy":[10.2346,19.12622,17.26196,7.27602]}, - {"t":3.59659, "x":5.87387, "y":2.65245, "heading":0.35729, "vx":-2.44355, "vy":-0.0371, "omega":-3.67279, "ax":4.54546, "ay":0.62337, "alpha":-0.72816, "fx":[82.96274,81.84715,82.01351,83.06252], "fy":[8.52966,15.92256,14.45973,6.32894]}, - {"t":3.60868, "x":5.84464, "y":2.65204, "heading":0.31287, "vx":-2.38857, "vy":-0.02956, "omega":-3.68159, "ax":4.56392, "ay":0.51606, "alpha":-0.57924, "fx":[83.14348,82.40393,82.49085,83.18698], "fy":[7.03049,13.02608,11.93398,5.46268]}, - {"t":3.62078, "x":5.81609, "y":2.65172, "heading":0.26834, "vx":-2.33337, "vy":-0.02332, "omega":-3.6886, "ax":4.57797, "ay":0.41921, "alpha":-0.44557, "fx":[83.28183,82.81762,82.85341,83.29252], "fy":[5.71451,10.40727,9.64093,4.66146]}, - {"t":3.63287, "x":5.7882, "y":2.65147, "heading":0.22372, "vx":-2.278, "vy":-0.01825, "omega":-3.69399, "ax":4.5886, "ay":0.33149, "alpha":-0.32508, "fx":[83.38803,83.11957,83.12643,83.38247], "fy":[4.56138,8.03871,7.54516,3.91254]}, - {"t":3.64497, "x":5.76098, "y":2.65128, "heading":0.17904, "vx":-2.2225, "vy":-0.01424, "omega":-3.69792, "ax":4.59653, "ay":0.25177, "alpha":-0.21603, "fx":[83.46994,83.33441,83.32866,83.45937], "fy":[3.55285,5.89559,5.61794,3.20554]}, - {"t":3.65706, "x":5.73444, "y":2.65112, "heading":0.13431, "vx":-2.1669, "vy":-0.01119, "omega":-3.70053, "ax":4.60235, "ay":0.17907, "alpha":-0.11692, "fx":[83.53359,83.48152,83.47423,83.52515], "fy":[2.67256,3.95573,3.83588,2.53204]}, - {"t":3.66916, "x":5.70856, "y":2.651, "heading":0.08956, "vx":-2.11124, "vy":-0.00903, "omega":-3.70195, "ax":4.60649, "ay":0.11258, "alpha":-0.02649, "fx":[83.58358,83.5762,83.57392,83.58126], "fy":[1.90585,2.19948,2.17985,1.88514]}, - {"t":3.68125, "x":5.68337, "y":2.6509, "heading":0.04478, "vx":-2.05552, "vy":-0.00767, "omega":-3.70227, "ax":4.6093, "ay":0.05157, "alpha":0.05636, "fx":[83.62342,83.63057,83.63609,83.62884], "fy":[1.23952,0.60954,0.63414,1.25924]}, - {"t":3.69335, "x":5.65884, "y":2.65081, "heading":0.0, "vx":-1.99977, "vy":-0.00704, "omega":-3.70159, "ax":0.01024, "ay":0.28631, "alpha":12.21024, "fx":[-32.65647,-35.887,36.26122,33.02555], "fy":[37.83167,-27.43723,-27.32463,37.70902]}, - {"t":3.72783, "x":5.58988, "y":2.65074, "heading":-0.12766, "vx":-1.99942, "vy":0.00283, "omega":-3.28048, "ax":0.00021, "ay":0.09332, "alpha":11.80356, "fx":[-28.39464,-37.40418,29.35565,36.45833], "fy":[37.16723,-25.33597,-34.03064,28.97239]}, - {"t":3.76232, "x":5.52092, "y":2.65089, "heading":-0.2408, "vx":-1.99941, "vy":0.00605, "omega":-2.8734, "ax":0.0001, "ay":0.02961, "alpha":11.33181, "fx":[-23.53966,-38.28727,23.79883,38.03512], "fy":[37.56872,-21.31137,-36.62853,22.52046]}, - {"t":3.79681, "x":5.45197, "y":2.65112, "heading":-0.33989, "vx":-1.99941, "vy":0.00707, "omega":-2.48259, "ax":0.00003, "ay":0.00915, "alpha":10.84327, "fx":[-18.99176,-38.44191,19.05645,38.3796], "fy":[37.70696,-17.06507,-37.42667,17.4488]}, - {"t":3.8313, "x":5.38301, "y":2.65137, "heading":-0.42551, "vx":-1.99941, "vy":0.00739, "omega":-2.10863, "ax":0.00001, "ay":0.00276, "alpha":10.34305, "fx":[-15.00802,-37.91935,15.02314,37.90496], "fy":[37.29591,-13.20901,-37.21288,13.326]}, - {"t":3.86579, "x":5.31406, "y":2.65162, "heading":-0.49824, "vx":-1.99941, "vy":0.00748, "omega":-1.75192, "ax":0.0, "ay":0.00081, "alpha":9.83183, "fx":[-11.66097,-36.86093,11.66431,36.85779], "fy":[36.36871,-9.94269,-36.3443,9.9772]}, - {"t":3.90027, "x":5.2451, "y":2.65188, "heading":-0.55866, "vx":-1.99941, "vy":0.00751, "omega":-1.41284, "ax":0.0, "ay":0.00024, "alpha":9.31006, "fx":[-8.9444,-35.40941,8.94511,35.40876], "fy":[35.03471,-7.30163,-35.02757,7.31159]}, - {"t":3.93476, "x":5.17615, "y":2.65214, "heading":-0.60738, "vx":-1.99941, "vy":0.00752, "omega":-1.09176, "ax":0.0, "ay":0.00007, "alpha":8.77841, "fx":[-6.81508,-33.68304,6.81522,33.68291], "fy":[33.40333,-5.25359,-33.40122,5.25646]}, - {"t":3.96925, "x":5.10719, "y":2.6524, "heading":-0.64503, "vx":-1.99941, "vy":0.00752, "omega":-0.78901, "ax":0.0, "ay":0.00002, "alpha":8.23776, "fx":[-5.21093,-31.77171,5.21096,31.77169], "fy":[31.5639,-3.73761,-31.56326,3.73845]}, - {"t":4.00374, "x":5.03824, "y":2.65266, "heading":-0.67224, "vx":-1.99941, "vy":0.00752, "omega":-0.50491, "ax":0.0, "ay":0.00001, "alpha":7.68916, "fx":[-4.06053,-29.73979,4.06054,29.73978], "fy":[29.58311,-2.68081,-29.58291,2.68107]}, - {"t":4.03823, "x":4.96928, "y":2.65292, "heading":-0.68966, "vx":-1.99941, "vy":0.00752, "omega":-0.23973, "ax":0.0, "ay":0.0, "alpha":7.13383, "fx":[-3.2888,-27.63106,3.28881,27.63106], "fy":[27.50794,-2.00647,-27.50788,2.00654]}, - {"t":4.07271, "x":4.90033, "y":2.65318, "heading":-0.69793, "vx":-1.99941, "vy":0.00752, "omega":0.00631, "ax":0.0, "ay":0.0, "alpha":6.5731, "fx":[-2.82065,-25.47362,2.82065,25.47362], "fy":[25.36992,-1.63824,-25.36992,1.63824]}, - {"t":4.1072, "x":4.83137, "y":2.65344, "heading":-0.69771, "vx":-1.99941, "vy":0.00752, "omega":0.233, "ax":0.0, "ay":0.0, "alpha":6.00831, "fx":[-2.58333,-23.28449,2.58333,23.2845], "fy":[23.18946,-1.50256,-23.18949,1.50253]}, - {"t":4.14169, "x":4.76242, "y":2.6537, "heading":-0.68967, "vx":-1.99941, "vy":0.00752, "omega":0.44021, "ax":0.0, "ay":0.0, "alpha":5.44082, "fx":[-2.508,-21.07362,2.508,21.07362], "fy":[20.97969,-1.53004,-20.97974,1.52998]}, - {"t":4.17618, "x":4.69346, "y":2.65396, "heading":-0.67449, "vx":-1.99941, "vy":0.00752, "omega":0.62785, "ax":0.0, "ay":0.0, "alpha":4.87184, "fx":[-2.53066,-18.84687,2.53066,18.84687], "fy":[18.74946,-1.65636,-18.74953,1.65629]}, - {"t":4.21066, "x":4.62451, "y":2.65422, "heading":-0.65284, "vx":-1.99941, "vy":0.00752, "omega":0.79587, "ax":0.0, "ay":0.0, "alpha":4.30253, "fx":[-2.59292,-16.60888,2.59292,16.60888], "fy":[16.50616,-1.82283,-16.50622,1.82277]}, - {"t":4.24515, "x":4.55555, "y":2.65447, "heading":-0.62539, "vx":-1.99941, "vy":0.00752, "omega":0.94426, "ax":0.0, "ay":0.0, "alpha":3.73374, "fx":[-2.6424,-14.36437,2.64241,14.36437], "fy":[14.25693,-1.97681,-14.25697,1.97677]}, - {"t":4.27964, "x":4.4866, "y":2.65473, "heading":-0.59282, "vx":-1.99941, "vy":0.00752, "omega":1.07303, "ax":0.0, "ay":0.0, "alpha":3.16621, "fx":[-2.63321,-12.11996,2.63321,12.11996], "fy":[12.01056,-2.07202,-12.01051,2.07206]}, - {"t":4.31413, "x":4.41764, "y":2.65499, "heading":-0.55582, "vx":-1.99941, "vy":0.00752, "omega":1.18222, "ax":0.0, "ay":0.00001, "alpha":2.60031, "fx":[-2.52603,-9.88397,2.52604,9.88397], "fy":[9.77731,-2.0686,-9.77691,2.069]}, - {"t":4.34862, "x":4.34868, "y":2.65525, "heading":-0.51505, "vx":-1.99941, "vy":0.00752, "omega":1.2719, "ax":0.0, "ay":0.00005, "alpha":2.03627, "fx":[-2.28852,-7.66753,2.28854,7.66753], "fy":[7.57029,-1.93321,-7.56836,1.93517]}, - {"t":4.3831, "x":4.27973, "y":2.65551, "heading":-0.47118, "vx":-1.99941, "vy":0.00752, "omega":1.34213, "ax":0.0, "ay":0.00025, "alpha":1.47394, "fx":[-1.89517,-5.48335,1.89523,5.48336], "fy":[5.40553,-1.6376,-5.39665,1.64655]}, - {"t":4.41759, "x":4.21077, "y":2.65577, "heading":-0.42489, "vx":-1.99941, "vy":0.00753, "omega":1.39296, "ax":0.0, "ay":0.00112, "alpha":0.91308, "fx":[-1.32751,-3.3461,1.32771,3.34621], "fy":[3.30824,-1.15302,-3.26775,1.19363]}, - {"t":4.45208, "x":4.14182, "y":2.65603, "heading":-0.37685, "vx":-1.99941, "vy":0.00757, "omega":1.42445, "ax":0.00002, "ay":0.00509, "alpha":0.35321, "fx":[-0.57368,-1.27078,0.57442,1.27145], "fy":[1.33805,-0.4232,-1.15348,0.60783]}, - {"t":4.48657, "x":4.07286, "y":2.6563, "heading":-0.32773, "vx":-1.9994, "vy":0.00775, "omega":1.43663, "ax":0.00009, "ay":0.02315, "alpha":-0.20623, "fx":[0.37223,0.72818,-0.36875,-0.72481], "fy":[-0.29003,0.75708,1.12998,0.08295]}, - {"t":4.52106, "x":4.00391, "y":2.65658, "heading":-0.27818, "vx":-1.9994, "vy":0.00854, "omega":1.42952, "ax":0.00054, "ay":0.10505, "alpha":-0.7651, "fx":[1.5159,2.63914,-1.49212,-2.62341], "fy":[-0.65675,3.28899,4.46631,0.52551]}, - {"t":4.55554, "x":3.93495, "y":2.65693, "heading":-0.22888, "vx":-1.99938, "vy":0.01217, "omega":1.40313, "ax":0.00478, "ay":0.47043, "alpha":-1.30006, "fx":[2.90853,4.44854,-2.67966,-4.33079], "fy":[4.34315,11.08614,12.70185,6.01012]}, - {"t":4.59003, "x":3.866, "y":2.65763, "heading":-0.18049, "vx":-1.99922, "vy":0.02839, "omega":1.3583, "ax":0.05406, "ay":1.81059, "alpha":-1.40276, "fx":[4.83347,6.01088,-2.57687,-4.34437], "fy":[29.07913,35.3585,36.56665,30.39896]}, - {"t":4.62452, "x":3.79709, "y":2.65969, "heading":-0.13364, "vx":-1.99735, "vy":0.09083, "omega":1.30992, "ax":0.27345, "ay":3.5723, "alpha":-0.64786, "fx":[7.64873,7.88717,2.44683,1.86295], "fy":[63.88161,65.23595,65.73887,64.4025]}, - {"t":4.65901, "x":3.72836, "y":2.66495, "heading":-0.08847, "vx":-1.98792, "vy":0.21404, "omega":1.28757, "ax":0.60879, "ay":4.19995, "alpha":-0.28916, "fx":[12.54308,12.38353,9.59354,9.66261], "fy":[75.86586,76.14295,76.5307,76.27091]}, - {"t":4.69349, "x":3.66017, "y":2.67483, "heading":-0.04406, "vx":-1.96693, "vy":0.35888, "omega":1.2776, "ax":0.95499, "ay":4.34906, "alpha":-0.16389, "fx":[18.28017,18.00972,16.38846,16.63006], "fy":[78.66855,78.8013,79.1419,79.02062]}, - {"t":4.72798, "x":3.5929, "y":2.68979, "heading":0.0, "vx":-1.93399, "vy":0.50887, "omega":1.27195, "ax":-4.27828, "ay":1.70964, "alpha":-0.72908, "fx":[-78.50232,-75.52404,-76.97753,-79.49106], "fy":[28.87529,36.01534,32.94491,26.24124]}, - {"t":4.75313, "x":3.54291, "y":2.70313, "heading":0.03199, "vx":-2.04158, "vy":0.55187, "omega":1.25361, "ax":-4.4215, "ay":1.13928, "alpha":-1.87456, "fx":[-82.32798,-76.21138,-79.30441,-83.04593], "fy":[12.91674,33.92447,26.49124,9.35087]}, - {"t":4.77828, "x":3.49017, "y":2.71737, "heading":0.06351, "vx":-2.15278, "vy":0.58052, "omega":1.20647, "ax":-4.41931, "ay":0.10391, "alpha":-4.08295, "fx":[-79.59762,-77.40511,-81.81353,-81.91417], "fy":[-22.7767,29.68055,16.45182,-15.81458]}, - {"t":4.80343, "x":3.43463, "y":2.732, "heading":0.09386, "vx":-2.26392, "vy":0.58313, "omega":1.10379, "ax":-3.72878, "ay":-1.41927, "alpha":-7.79088, "fx":[-37.95807,-79.71713,-83.23909,-69.70079], "fy":[-73.33582,15.55721,0.50953,-45.73404]}, - {"t":4.82858, "x":3.37652, "y":2.74622, "heading":0.12161, "vx":-2.35769, "vy":0.54744, "omega":0.90786, "ax":-1.46733, "ay":-3.44934, "alpha":-8.91899, "fx":[10.2652,10.38495,-79.62597,-47.51506], "fy":[-82.50659,-75.76041,-23.46593,-68.60216]}, - {"t":4.85373, "x":3.31676, "y":2.75889, "heading":0.14445, "vx":-2.39459, "vy":0.46069, "omega":0.68356, "ax":0.03851, "ay":-3.62504, "alpha":-10.74568, "fx":[31.56672,61.68419,-64.58823,-25.86818], "fy":[-77.34777,-54.28121,-51.95643,-79.50115]}, - {"t":4.87887, "x":3.25655, "y":2.76933, "heading":0.16164, "vx":-2.39363, "vy":0.36953, "omega":0.41332, "ax":0.81759, "ay":-3.82444, "alpha":-9.07017, "fx":[41.379,67.43541,-39.58385,-9.89422], "fy":[-72.82429,-48.627,-72.94919,-83.15757]}, - {"t":4.90402, "x":3.19661, "y":2.77742, "heading":0.17203, "vx":-2.37307, "vy":0.27335, "omega":0.18522, "ax":1.39586, "ay":-3.87995, "alpha":-7.47749, "fx":[46.67299,69.47972,-15.88929,1.04099], "fy":[-69.70717,-46.34559,-81.68852,-83.84508]}, - {"t":4.92917, "x":3.13738, "y":2.78306, "heading":0.17669, "vx":-2.33796, "vy":0.17577, "omega":-0.00283, "ax":1.79524, "ay":-3.85282, "alpha":-6.38701, "fx":[49.87219,70.50371,1.31579,8.59746], "fy":[-67.56147,-45.14743,-83.41892,-83.48971]}, - {"t":4.95432, "x":3.07915, "y":2.78626, "heading":0.17662, "vx":-2.29281, "vy":0.07888, "omega":-0.16346, "ax":2.06862, "ay":-3.80106, "alpha":-5.66421, "fx":[51.95209,71.10565,13.08831,13.98341], "fy":[-66.05166,-44.42988,-82.56063,-82.81872]}, - {"t":4.97947, "x":3.02214, "y":2.78705, "heading":0.17251, "vx":-2.24079, "vy":-0.01671, "omega":-0.30591, "ax":2.26167, "ay":-3.74767, "alpha":-5.16424, "fx":[53.36558,71.49206,21.33526,17.94711], "fy":[-64.97213,-43.96845,-80.94812,-82.09792]}, - {"t":5.00462, "x":2.9665, "y":2.78544, "heading":0.16481, "vx":-2.18391, "vy":-0.11096, "omega":-0.43578, "ax":2.40343, "ay":-3.69941, "alpha":-4.79947, "fx":[54.34779,71.75237,27.38185,20.94614], "fy":[-64.19714,-43.66161,-79.20386,-81.42149]}, - {"t":5.02977, "x":2.91234, "y":2.78148, "heading":0.15386, "vx":-2.12347, "vy":-0.204, "omega":-0.55648, "ax":2.51135, "ay":-3.65747, "alpha":-4.52047, "fx":[55.03192,71.93115,32.02978,23.26761], "fy":[-63.64704,-43.45753,-77.51657,-80.8186]}, - {"t":5.05492, "x":2.85973, "y":2.77519, "heading":0.13986, "vx":-2.06031, "vy":-0.29598, "omega":-0.67016, "ax":2.59608, "ay":-3.62135, "alpha":-4.29872, "fx":[55.49869,72.05268,35.76041,25.09813], "fy":[-63.26927,-43.32762,-75.92847,-80.29351]}, - {"t":5.08006, "x":2.80874, "y":2.76661, "heading":0.12301, "vx":-1.99502, "vy":-0.38705, "omega":-0.77827, "ax":2.66435, "ay":-3.59021, "alpha":-4.11707, "fx":[55.79977,72.131,38.87044,26.56346], "fy":[-63.0278,-43.25527,-74.43414,-79.84112]}, - {"t":5.10521, "x":2.75941, "y":2.75574, "heading":0.10343, "vx":-1.92802, "vy":-0.47734, "omega":-0.88181, "ax":2.72056, "ay":-3.56316, "alpha":-3.96481, "fx":[55.9697,72.17454,41.54886,27.75127], "fy":[-62.89707,-43.23062,-73.01455,-79.45326]}, - {"t":5.13036, "x":2.71178, "y":2.74261, "heading":0.08126, "vx":-1.8596, "vy":-0.56695, "omega":-0.98152, "ax":2.7677, "ay":-3.53947, "alpha":-3.83503, "fx":[56.0324,72.18843,43.91985,28.72472], "fy":[-62.85834,-43.24777,-71.64883,-79.12117]}, - {"t":5.15551, "x":2.66589, "y":2.72723, "heading":0.05657, "vx":-1.79, "vy":-0.65596, "omega":-1.07797, "ax":2.80784, "ay":-3.51851, "alpha":-3.72319, "fx":[56.00498,72.17573,46.06706,29.5307], "fy":[-62.8975,-43.30331,-70.31812,-78.83648]}, - {"t":5.18066, "x":2.62176, "y":2.70962, "heading":0.02946, "vx":-1.71938, "vy":-0.74445, "omega":-1.1716, "ax":2.84246, "ay":-3.4998, "alpha":-3.62629, "fx":[55.89996,72.13819,48.04791,30.20506], "fy":[-63.00363,-43.39537,-69.00659,-78.59147]}, - {"t":5.20581, "x":2.57942, "y":2.68979, "heading":0.0, "vx":-1.6479, "vy":-0.83246, "omega":-1.2628, "ax":2.92997, "ay":-3.4578, "alpha":-3.27021, "fx":[56.19057,71.41542,51.54452,33.49147], "fy":[-62.72888,-44.55712,-66.43057,-77.23223]}, - {"t":5.22961, "x":2.54102, "y":2.66899, "heading":-0.03006, "vx":-1.57815, "vy":-0.91477, "omega":-1.34064, "ax":3.05757, "ay":-3.35622, "alpha":-3.12372, "fx":[57.38017,72.21417,55.75172,36.55647], "fy":[-61.62436,-43.227,-62.9169,-75.80841]}, - {"t":5.25342, "x":2.50432, "y":2.64627, "heading":-0.06197, "vx":-1.50537, "vy":-0.99466, "omega":-1.415, "ax":3.20137, "ay":-3.23119, "alpha":-2.96371, "fx":[58.86488,73.10323,60.07263,40.29817], "fy":[-60.18546,-41.67742,-58.77822,-73.86171]}, - {"t":5.27722, "x":2.4694, "y":2.62168, "heading":-0.09566, "vx":-1.42916, "vy":-1.07158, "omega":-1.48555, "ax":3.36362, "ay":-3.07567, "alpha":-2.78062, "fx":[60.73302,74.10468,64.40769,44.86832], "fy":[-58.27194,-39.83465,-53.96276,-71.14651]}, - {"t":5.30102, "x":2.43633, "y":2.5953, "heading":-0.13102, "vx":-1.3491, "vy":-1.14479, "omega":-1.55174, "ax":3.54652, "ay":-2.87969, "alpha":-2.55783, "fx":[63.09585,75.2447,68.63443,50.41291], "fy":[-55.66914,-37.59272,-48.43914,-67.29211]}, - {"t":5.32483, "x":2.40522, "y":2.56723, "heading":-0.16795, "vx":-1.26467, "vy":-1.21334, "omega":-1.61262, "ax":3.75136, "ay":-2.62896, "alpha":-2.27074, "fx":[66.08439,76.55181,72.61315,57.00505], "fy":[-52.03897,-34.79619,-42.20319,-61.7576]}, - {"t":5.34863, "x":2.37618, "y":2.5376, "heading":-0.20634, "vx":-1.17538, "vy":-1.27592, "omega":-1.66668, "ax":3.97611, "ay":-2.30322, "alpha":-1.88992, "fx":[69.82298,78.05025,76.19611,64.49596], "fy":[-46.83771,-31.21169,-35.28137,-53.82506]}, - {"t":5.37244, "x":2.34933, "y":2.50658, "heading":-0.24602, "vx":-1.08073, "vy":-1.33075, "omega":-1.71166, "ax":4.21054, "ay":-1.87604, "alpha":-1.39099, "fx":[74.33163,79.74071,79.23841,72.2684], "fy":[-39.19357,-26.48152,-27.72894,-42.74912]}, - {"t":5.39624, "x":2.32479, "y":2.47437, "heading":-0.28676, "vx":-0.9805, "vy":-1.3754, "omega":-1.74478, "ax":4.42906, "ay":-1.3188, "alpha":-0.76513, "fx":[79.24138,81.5479,81.60739,79.0414], "fy":[-27.80135,-20.05082,-19.62276,-28.23646]}, - {"t":5.42004, "x":2.30271, "y":2.44126, "heading":-0.32829, "vx":-0.87507, "vy":-1.4068, "omega":-1.76299, "ax":4.58447, "ay":-0.61253, "alpha":-0.00956, "fx":[83.17105,83.18501,83.1873,83.17335], "fy":[-11.17617,-11.07263,-11.05099,-11.15466]}, - {"t":5.44385, "x":2.28318, "y":2.40759, "heading":-0.37026, "vx":-0.76594, "vy":-1.42138, "omega":-1.76322, "ax":4.60991, "ay":0.23046, "alpha":0.88159, "fx":[83.17052,83.83243,83.87803,83.68204], "fy":[10.84475,1.6251,-2.10437,6.36013]}, - {"t":5.46765, "x":2.26625, "y":2.37382, "heading":-0.41223, "vx":-0.65621, "vy":-1.41589, "omega":-1.74223, "ax":4.44442, "ay":1.14197, "alpha":1.84093, "fx":[76.39852,81.56149,83.58942,81.00308], "fy":[34.60171,19.11807,7.12556,22.0329]}, - {"t":5.49146, "x":2.25189, "y":2.34044, "heading":-0.4537, "vx":-0.55041, "vy":-1.38871, "omega":-1.69841, "ax":4.07902, "ay":2.01129, "alpha":2.75878, "fx":[64.12429,73.19314,82.23644,76.48041], "fy":[54.11346,40.63208,16.53849,34.68503]}, - {"t":5.51526, "x":2.23994, "y":2.30796, "heading":-0.49413, "vx":-0.45331, "vy":-1.34083, "omega":-1.63274, "ax":3.56806, "ay":2.73445, "alpha":3.67748, "fx":[50.81208,57.02364,79.74238,71.37337], "fy":[66.84254,61.2869,26.00723,44.3153]}, - {"t":5.53906, "x":2.23016, "y":2.27681, "heading":-0.533, "vx":-0.36838, "vy":-1.27574, "omega":-1.5452, "ax":3.01473, "ay":3.25523, "alpha":4.55332, "fx":[39.42358,36.88371,76.05834,66.42784], "fy":[74.19648,75.21327,35.35294,51.48509]}, - {"t":5.56287, "x":2.22225, "y":2.24737, "heading":-0.56978, "vx":-0.29662, "vy":-1.19825, "omega":-1.43681, "ax":2.51303, "ay":3.59945, "alpha":5.17346, "fx":[30.57301,18.65409,71.19984,61.95539], "fy":[78.31184,81.75274,44.33406,56.8303]}, - {"t":5.58667, "x":2.2159, "y":2.21987, "heading":-0.60398, "vx":-0.2368, "vy":-1.11257, "omega":-1.31366, "ax":2.09288, "ay":3.83037, "alpha":5.46259, "fx":[23.87396,4.70605,65.28453,58.02602], "fy":[80.64762,83.80174,52.6681,60.87113]}, - {"t":5.61048, "x":2.21086, "y":2.19447, "heading":-0.63525, "vx":-0.18698, "vy":-1.02139, "omega":-1.18363, "ax":1.74377, "ay":3.99469, "alpha":5.50108, "fx":[18.78219,-5.37959,58.54545,54.606], "fy":[82.01507,83.82709,60.08628,63.98511]}, - {"t":5.63428, "x":2.2069, "y":2.17129, "heading":-0.66343, "vx":-0.14547, "vy":-0.9263, "omega":-1.05268, "ax":1.44839, "ay":4.11667, "alpha":5.4002, "fx":[14.84812,-12.66328,51.30313,51.6289], "fy":[82.84314,83.09193,66.39746,66.43403]}, - {"t":5.65809, "x":2.20385, "y":2.1504, "heading":-0.68849, "vx":-0.11099, "vy":-0.82831, "omega":-0.92414, "ax":1.1939, "ay":4.20837, "alpha":5.24438, "fx":[11.74494,-18.02978,43.90621,49.02533], "fy":[83.36029,82.13637,71.52747,68.39774]}, - {"t":5.68189, "x":2.20154, "y":2.13188, "heading":-0.71048, "vx":-0.08257, "vy":-0.72813, "omega":-0.7993, "ax":0.97219, "ay":4.27673, "alpha":5.08413, "fx":[9.24302,-22.08894,36.66941,46.73318], "fy":[83.69157,81.1723,75.51801,70.00087]}, - {"t":5.70569, "x":2.19985, "y":2.11576, "heading":-0.72951, "vx":-0.05943, "vy":-0.62633, "omega":-0.67827, "ax":0.77811, "ay":4.32669, "alpha":4.94408, "fx":[7.18103,-25.24269,29.83327,44.69989], "fy":[83.90776,80.27461,78.49489,71.33114]}, - {"t":5.7295, "x":2.19866, "y":2.10207, "heading":-0.74566, "vx":-0.04091, "vy":-0.52334, "omega":-0.56058, "ax":0.60796, "ay":4.36227, "alpha":4.83272, "fx":[5.44409,-27.75642,23.55268,42.88199], "fy":[84.05027,79.46274,80.6263,72.45148]}, - {"t":5.7533, "x":2.19786, "y":2.09085, "heading":-0.759, "vx":-0.02644, "vy":-0.4195, "omega":-0.44555, "ax":0.45871, "ay":4.38684, "alpha":4.74976, "fx":[3.94887,-29.80864,17.90673,41.24383], "fy":[84.14399,78.73446,82.08785,73.40781]}, - {"t":5.77711, "x":2.19736, "y":2.08211, "heading":-0.76961, "vx":-0.01552, "vy":-0.31507, "omega":-0.33248, "ax":0.32774, "ay":4.40317, "alpha":4.69104, "fx":[2.63369,-31.52229,12.91789,39.75617], "fy":[84.20425,78.07985,83.04052,74.23426]}, - {"t":5.80091, "x":2.19708, "y":2.07586, "heading":-0.77752, "vx":-0.00771, "vy":-0.21026, "omega":-0.22082, "ax":0.21267, "ay":4.41345, "alpha":4.65121, "fx":[1.45204,-32.98414,8.57183,38.39498], "fy":[84.24063,77.48701,83.62064,74.95655]}, - {"t":5.82471, "x":2.19696, "y":2.0721, "heading":-0.78278, "vx":-0.00265, "vy":-0.1052, "omega":-0.1101, "ax":0.11141, "ay":4.41938, "alpha":4.62519, "fx":[0.36821,-34.25685,4.83352,37.1404], "fy":[84.25913,76.94432,83.93779,75.5944]}, - {"t":5.84852, "x":2.19693, "y":2.07085, "heading":-0.7854, "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 07b75d3c6141d086edf136e6cee551ad510fce58 Mon Sep 17 00:00:00 2001 From: alexandra Date: Mon, 23 Mar 2026 19:29:19 -0400 Subject: [PATCH 05/40] continuation of fixing merge conflicts --- .../robot/dashboards/TestingDashboard.java | 5 ++++ .../frc/robot/subsystems/Superstructure.java | 24 +++---------------- .../java/frc/robot/subsystems/Swerve.java | 6 ++--- 3 files changed, 11 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/dashboards/TestingDashboard.java b/src/main/java/frc/robot/dashboards/TestingDashboard.java index 8a73c9e6..6deabc1c 100644 --- a/src/main/java/frc/robot/dashboards/TestingDashboard.java +++ b/src/main/java/frc/robot/dashboards/TestingDashboard.java @@ -39,6 +39,8 @@ public class TestingDashboard { // public static BooleanTopic BT_letIntakeArmPositionRotsChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeArmPositionRotsChange"); // public static BooleanTopic BT_letIntakeRollersVelocityRPSChange = nte_inst.getBooleanTopic("/TestingDashboard/letIntakeRollersVelocityRPSChange"); + public static BooleanTopic BT_letOpsCheckBeLong = nte_inst.getBooleanTopic("/TestingDashboard/letOpsCheckBeLong"); + // /* PUBLISHERS */ // //---SHOOTER public static DoublePublisher pub_shooterVelocityRPS; @@ -64,6 +66,9 @@ public class TestingDashboard { // public static BooleanPublisher pub_letIntakeArmPositionRotsChange; // public static BooleanPublisher pub_letIntakeRollersVelocityRPSChange; + //---SELECT OPS CHECK SWITCHES + public static BooleanPublisher pub_letOpsCheckBeLong; + // /* SUBSCRIBERS */ // //---SHOOTER public static DoubleSubscriber sub_shooterVelocityRPS; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 93106718..ba4f5f22 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -9,6 +9,7 @@ import frc.robot.Constants; import frc.robot.Constants.IndexerK; import frc.robot.Constants.ShooterK; +import frc.robot.Constants.SuperstructureK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; import frc.util.WaltLogger; @@ -383,7 +384,7 @@ public Command longOpsCheck() { Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run intake rollers - m_intake.setIntakeRollersVelocityCmd(IntakeK.kIntakeRollersMaxRPS), + m_intake.startIntakeRollers(), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run spindexer @@ -425,7 +426,7 @@ public Command shortOpsCheck() { Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //runs shooting cmd - activateOuttake(ShooterK.kShooterRPS).withTimeout(SuperstructureK.kShortOpsCheckShooterTime), + activateOuttakeShotCalc(), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //Swerve test @@ -433,25 +434,6 @@ public Command shortOpsCheck() { ); } - /** - * Adds and removes specified Command names from the ActiveCommands ArrayList, then logs the ArrayList. - * @param toAdd Command name to add. - * @param toRemove Command names to remove. - */ - private Command logActiveCommands(String toAdd, String... toRemove) { - Command addTo = Commands.runOnce( - () -> m_activeCommands.add(toAdd) - ); - Command removeFrom = Commands.runOnce( - () -> { - for (String s : toRemove) { - m_activeCommands.remove(s); - } - } - ); - Command updateLog = Commands.runOnce( - () -> log_activeCommands.accept(m_activeCommands.toArray(new String[m_activeCommands.size()])) - ); // /** // * Adds and removes specified Command names from the ActiveCommands ArrayList, then logs the ArrayList. // * @param toAdd Command name to add. diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 01706c22..f2a5f9a8 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -423,7 +423,7 @@ public Command swerveAutomatedOpsCheck() { .withRotationalRate(0) ), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), - xBrake(), + xBrakeCmd(), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //Slow speed backward into high speed @@ -439,7 +439,7 @@ public Command swerveAutomatedOpsCheck() { .withRotationalRate(0) ), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), - xBrake(), + xBrakeCmd(), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //Turn wheels @@ -449,7 +449,7 @@ public Command swerveAutomatedOpsCheck() { .withRotationalRate(RobotK.kMaxAngularRate) ), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), - xBrake() + xBrakeCmd() ); } From 8aa2db2c165ed6dc2b8c2c325c93905644f4da26 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Wed, 25 Mar 2026 16:33:06 -0400 Subject: [PATCH 06/40] sotm working (kinda?) --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16404921..6975c87f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,6 +75,8 @@ public static class ShooterK { public static final Transform3d kTurretTransform = new Transform3d(new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)), new Rotation3d(kTurretAngleOffset)); //DUMMY VALS public static final Distance kInchesAboveFunnel = Inches.of(20);// distance the ball must travel above the funnel opening to arc correctly into the hub + public static final boolean kUseStaticShot = false; + // private static final Pose3dLogger log_turretTransform = WaltLogger.logPose3d(kLogTab, "TurretTransformRaw"); // static { // log_turretTransform.accept(kTurretTransform); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5241de09..1e534574 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -121,6 +121,8 @@ public Shooter(Supplier poseSupplier, Supplier threads m_threadsafeSwerveSup = threadsafeSwerveStateSup; m_shooterCalc = new ShooterCalc(m_threadsafeSwerveSup, () -> m_latestTurretPositionRots); + m_shooterCalc.shouldUseStaticShot(kUseStaticShot); + m_shooterA.getConfigurator().apply(kShooterATalonFXConfiguration); m_shooterB.getConfigurator().apply(kShooterBTalonFXConfiguration); // m_hood.getConfigurator().apply(kHoodTalonFXSConfiguration); From cc76b9825a0dffa01e6e286c65617ae4bc7740b6 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Wed, 25 Mar 2026 18:48:35 -0400 Subject: [PATCH 07/40] no more rpsboost --- .../subsystems/shooter/ShotCalculator.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index d58464ce..7df1b9c0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -52,7 +52,7 @@ public class ShotCalculator { maxDistance = 5.8; //right mid grid - m_shotMap.put(3.903, new ShotData(RotationsPerSecond.of(73.18 + kRPSBoost), Degrees.of(327.75))); + m_shotMap.put(3.903, new ShotData(RotationsPerSecond.of(73.18), Degrees.of(327.75))); m_timeOfFlightMap.put(3.903, 1.35); //backright grid - THIS ONE IS INCONSISTENT UNTIL SHOOTER V3 @@ -60,40 +60,40 @@ public class ShotCalculator { m_timeOfFlightMap.put(5.657, 1.56); //frontright grid - m_shotMap.put(3.763, new ShotData(RotationsPerSecond.of(77 + kRPSBoost), Degrees.of(300))); + m_shotMap.put(3.763, new ShotData(RotationsPerSecond.of(77), Degrees.of(300))); m_timeOfFlightMap.put(3.763, 1.43); //frontleft grid - m_shotMap.put(1.476, new ShotData(RotationsPerSecond.of(57.72 + kRPSBoost), Degrees.of(150.77))); + m_shotMap.put(1.476, new ShotData(RotationsPerSecond.of(57.72), Degrees.of(150.77))); m_timeOfFlightMap.put(1.476, 1.125); //backleft grid - m_shotMap.put(3.854, new ShotData(RotationsPerSecond.of(79 + kRPSBoost), Degrees.of(345))); + m_shotMap.put(3.854, new ShotData(RotationsPerSecond.of(79), Degrees.of(345))); m_timeOfFlightMap.put(3.854, 1.38); //against the hub - m_shotMap.put(0.835, new ShotData(RotationsPerSecond.of(66.21 + kRPSBoost), Degrees.of(85.51))); + m_shotMap.put(0.835, new ShotData(RotationsPerSecond.of(66.21), Degrees.of(85.51))); m_timeOfFlightMap.put(0.835, 1.17); //against the tower - m_shotMap.put(2.977, new ShotData(RotationsPerSecond.of(71.14 + kRPSBoost), Degrees.of(338.81))); + m_shotMap.put(2.977, new ShotData(RotationsPerSecond.of(71.14), Degrees.of(338.81))); m_timeOfFlightMap.put(2.977, 1.03); //backleft grid - m_shotMap.put(4.348, new ShotData(RotationsPerSecond.of(77.94 + kRPSBoost), Degrees.of(396.32))); + m_shotMap.put(4.348, new ShotData(RotationsPerSecond.of(77.94), Degrees.of(396.32))); m_timeOfFlightMap.put(4.348, 1.38); //left mid grid - m_shotMap.put(2.799, new ShotData(RotationsPerSecond.of(67.91 + kRPSBoost), Degrees.of(308.94))); + m_shotMap.put(2.799, new ShotData(RotationsPerSecond.of(67.91), Degrees.of(308.94))); m_timeOfFlightMap.put(2.799, 1.08); - m_shotMap.put(3.561, new ShotData(RotationsPerSecond.of(79.30 + kRPSBoost), Degrees.of(287.93))); + m_shotMap.put(3.561, new ShotData(RotationsPerSecond.of(79.30), Degrees.of(287.93))); m_timeOfFlightMap.put(3.561, 1.42); - m_shotMap.put(1.870, new ShotData(RotationsPerSecond.of(62.31 + kRPSBoost), Degrees.of(231.52))); + m_shotMap.put(1.870, new ShotData(RotationsPerSecond.of(62.31), Degrees.of(231.52))); m_timeOfFlightMap.put(1.870, 1.1); - m_shotMap.put(3.259, new ShotData(RotationsPerSecond.of(75.64 + kRPSBoost), Degrees.of(268.02))); + m_shotMap.put(3.259, new ShotData(RotationsPerSecond.of(75.64), Degrees.of(268.02))); m_timeOfFlightMap.put(3.259, 1.37); //AT COMPETITION From 148d74f12d92d3ddfb24cb3b2d37ddad5c65ff64 Mon Sep 17 00:00:00 2001 From: alexandra Date: Wed, 25 Mar 2026 20:58:37 -0400 Subject: [PATCH 08/40] hood current pos should be logging properly now :D still need to remove magic # --- src/main/java/frc/robot/subsystems/shooter/Hood.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 1844c991..2395ee6f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -68,10 +68,13 @@ public Command setHoodPositionCmd(DoubleSubscriber sub_rots) { return run(() -> setHoodPos(sub_rots.get())); } - private Angle getHoodAngle() { - return m_hood.getPosition().getValue(); + //TODO: REMOVE MAGIC NUMBERS + private double getHoodAngleDeg() { + double hoodPositionDeg = m_hood.getPosition().getValue().in(Degrees); + + return 10 + (hoodPositionDeg - 36) * (38/619.902); } - + public boolean isHoodHomed() { return m_isHoodHomed; } @@ -111,6 +114,6 @@ public void setHoodNeutralMode(NeutralModeValue value) { @Override public void periodic() { - // log_hoodCurrentPos.accept(getHoodAngle().in(Degrees)); + log_hoodCurrentPos.accept(getHoodAngleDeg()); } } From f780f4767bc9f0035669d3acc80b060706848b44 Mon Sep 17 00:00:00 2001 From: alexandra Date: Wed, 25 Mar 2026 21:13:07 -0400 Subject: [PATCH 09/40] removed magic numbers!!! --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/shooter/Hood.java | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16404921..fbdbf946 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -144,6 +144,8 @@ public static class ShooterK { //double versions public static final double kHoodMinPosition_double = 0.1; public static final double kHoodMaxRots_double = 1.82195; + public static final double kPhysicalHoodMinPosition_double = 10; + public static final double kPhysicalHoodMaxPosition_double = 48; public static final double kHoodLockRots_double = kHoodMaxRots_double * 0.75; //TODO: ensure this is the home value diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 2395ee6f..450d25ad 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -68,11 +68,11 @@ public Command setHoodPositionCmd(DoubleSubscriber sub_rots) { return run(() -> setHoodPos(sub_rots.get())); } - //TODO: REMOVE MAGIC NUMBERS private double getHoodAngleDeg() { double hoodPositionDeg = m_hood.getPosition().getValue().in(Degrees); + double absoluteToPhysicalAngleRatio = (360 * (kHoodMaxRots_double - kHoodMinPosition_double))/(kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double); - return 10 + (hoodPositionDeg - 36) * (38/619.902); + return kPhysicalHoodMinPosition_double + (hoodPositionDeg - (kHoodMinPosition_double * 360)) * (absoluteToPhysicalAngleRatio); } public boolean isHoodHomed() { From a1921028c3267babe72b47874645432a1103fdd7 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 26 Mar 2026 20:25:17 -0400 Subject: [PATCH 10/40] new lerp points for shotdata --- .../frc/robot/subsystems/Superstructure.java | 3 +- .../subsystems/shooter/ShotCalculator.java | 30 ++++++++++++------- .../frc/robot/subsystems/shooter/Turret.java | 1 - 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index d918f20e..9b8f4d1e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -36,12 +36,11 @@ public Command intake(BooleanSupplier isShooting) { return Commands.sequence( m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), Commands.waitUntil(() -> m_intake.isIntakeArmAtPos()).withTimeout(0.25), + m_intake.setIntakeRollersVelocityCmd(12), Commands.run( () -> { boolean shooting = isShooting.getAsBoolean(); - m_shooter.m_turret.setIntaking(!shooting); - m_intake.setIntakeRollersVelocity(12); m_indexer.setSpindexerVelocity(shooting ? IndexerK.kSpindexerShootRPS : IndexerK.kSpindexerIntakeRPS); }) ).finallyDo( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 7df1b9c0..19a8a533 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static frc.robot.Constants.ShooterK.*; @@ -98,18 +99,25 @@ public class ShotCalculator { //AT COMPETITION - m_shotMap.put(2.755, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(221.02))); - m_timeOfFlightMap.put(2.755, 1.28); + // m_shotMap.put(2.755, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(221.02))); + // m_timeOfFlightMap.put(2.755, 1.28); - m_shotMap.put(2.352, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(169.77))); - m_timeOfFlightMap.put(2.352, 0.9); + // m_shotMap.put(2.352, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(169.77))); + // m_timeOfFlightMap.put(2.352, 0.9); - //3/21/26 8:40 AM - m_shotMap.put(3.425, new ShotData(RotationsPerSecond.of(87), Degrees.of(280))); - m_timeOfFlightMap.put(3.425, 1.41); + // //3/21/26 8:40 AM + // m_shotMap.put(3.425, new ShotData(RotationsPerSecond.of(87), Degrees.of(280))); + // m_timeOfFlightMap.put(3.425, 1.41); - m_shotMap.put(3.933, new ShotData(RotationsPerSecond.of(80.5), Degrees.of(310))); - m_timeOfFlightMap.put(3.933, 1.38); + // m_shotMap.put(3.933, new ShotData(RotationsPerSecond.of(80.5), Degrees.of(310))); + // m_timeOfFlightMap.put(3.933, 1.38); + + //3/26 + m_shotMap.put(2.462, new ShotData(RotationsPerSecond.of(69.27), Rotations.of(0.67))); + m_timeOfFlightMap.put(2.462, 1.15); + + m_shotMap.put(3.335, new ShotData(RotationsPerSecond.of(81.37), Rotations.of(0.68))); + m_timeOfFlightMap.put(3.335, 1.38); } @@ -125,7 +133,9 @@ public static double getDistanceToTargetM(double robotX, double robotY, double r double turretY = robotY + kTurretOffsetX_m * sinR + kTurretOffsetY_m * cosR; double dx = turretX - targetX; double dy = turretY - targetY; - return Math.sqrt(dx * dx + dy * dy); + double dist = Math.sqrt(dx * dx + dy * dy); + log_distToTargetMeters.accept(dist); + return dist; } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 49d06062..89ad1306 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -67,7 +67,6 @@ private void homeTurret(boolean useLCM) { } public void setIntaking(boolean intaking) { - System.out.println("setIntaking: " + intaking); m_holdTurretAtIntakePos = intaking; } From 5cf44d49f14b0dab11c6e3c6f8af5094b5e27aad Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 26 Mar 2026 21:36:30 -0400 Subject: [PATCH 11/40] turret atPos working --- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/shooter/Turret.java | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6790fca2..41ce98df 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -274,7 +274,7 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 89ad1306..00f3c56a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -35,6 +36,8 @@ public class Turret extends SubsystemBase { // ---LOGIC BOOLEANS private boolean m_isTurretHomed = true; public BooleanSupplier turretHomedSupp = () -> m_isTurretHomed; + private boolean m_turretAtPos = false; + public BooleanSupplier turretAtPosSupp = () -> m_turretAtPos; private final CANcoder m_lcmEncA = new CANcoder(19, Constants.kCanivoreBus); private final DutyCycleEncoder m_lcmEncB = new DutyCycleEncoder(3); @@ -46,6 +49,10 @@ public class Turret extends SubsystemBase { private final DoubleLogger log_turretControlPos = WaltLogger.logDouble(kLogTab, "turretControlPos"); private final DoubleLogger log_turretLCMPos = WaltLogger.logDouble(kLogTab, "turretCRTPos"); + private final DoubleLogger log_turretClosedLoopError = WaltLogger.logDouble(kLogTab, "turretCLE"); + private final BooleanLogger log_atPos = WaltLogger.logBoolean(kLogTab, "atPos"); + + StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); public Turret() { m_turret.getConfigurator().apply(kTurretTalonFXConfiguration); @@ -66,6 +73,16 @@ private void homeTurret(boolean useLCM) { } } + public boolean atPosition() { + sig_turretCLErr.refresh(); + boolean isNear = sig_turretCLErr.isNear(0, kTurretMaxErrD); + log_turretClosedLoopError.accept(sig_turretCLErr.getValueAsDouble()); + + m_turretAtPos = isNear; + log_atPos.accept(isNear); + return isNear; + } + public void setIntaking(boolean intaking) { m_holdTurretAtIntakePos = intaking; } From 41ca7b3f8de311b42180cc63f8d9c619ce73fe89 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 26 Mar 2026 21:36:41 -0400 Subject: [PATCH 12/40] better static shot hood angle --- src/main/java/frc/robot/Constants.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6975c87f..cbd29164 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,6 +119,8 @@ public static class ShooterK { public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.75); //0.75 rots in each direction from home public static final Angle kTurretMinRots = Rotations.of(-kTurretMaxRotsFromHome.in(Rotations)); public static final Angle kTurretMaxRots = Rotations.of(kTurretMaxRotsFromHome.in(Rotations)); + public static final double kTurretMaxErrD = Rotations.of(0.05).in(Rotations); + public static final AngularVelocity kShooterMaxRPS = MotorK.kX60MaxVelocity.div(kShooterGearing); public static final double kShooterMaxRPSd = 96.42; @@ -146,7 +148,7 @@ public static class ShooterK { //double versions public static final double kHoodMinPosition_double = 0.1; public static final double kHoodMaxRots_double = 1.82195; - public static final double kHoodLockRots_double = kHoodMaxRots_double * 0.75; + public static final double kHoodLockRots_double = kHoodMaxRots_double * 0.50; //TODO: ensure this is the home value // public static final Angle kHoodHomePosition = Degrees.of(10); From 090780f4928cc476ee3ddab64f3ac0f4fae389e1 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Thu, 26 Mar 2026 21:36:49 -0400 Subject: [PATCH 13/40] shotCalc unit testing back --- .../subsystems/shooter/ShotCalcPerfTest.java | 829 ++++++----- .../subsystems/shooter/ShotCalcTest.java | 1238 ++++++++--------- 2 files changed, 1029 insertions(+), 1038 deletions(-) diff --git a/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java b/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java index 9e199997..a32db405 100644 --- a/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java +++ b/src/test/java/frc/robot/subsystems/shooter/ShotCalcPerfTest.java @@ -1,419 +1,410 @@ -// package frc.robot.subsystems.shooter; - -// import static edu.wpi.first.units.Units.*; - -// import edu.wpi.first.hal.HAL; -// import edu.wpi.first.math.MathUtil; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Pose3d; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Rotation3d; -// import edu.wpi.first.math.geometry.Translation3d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.units.measure.*; - -// import frc.robot.Constants.ShooterK; -// import frc.robot.FieldConstants; -// import frc.robot.subsystems.shooter.ShotCalculator.ShotData; - -// import org.junit.jupiter.api.*; - -// /** -// * Performance benchmark comparing three approaches for the shot calculation hot path: -// * 1. Immutable WPILib Units (current implementation) -// * 2. Mutable WPILib Units (MutAngle, MutDistance, etc.) -// * 3. Raw doubles (no Units library at all) -// * -// * Each approach is timed over many iterations of the full calcShot pipeline. -// * Results are printed as a comparison table. -// */ -// class ShotCalcPerfTest { - -// private static Translation3d HUB_TARGET; -// private static Pose2d MID_POSE; -// private static final ChassisSpeeds MOVING_SPEEDS = new ChassisSpeeds(1.5, -0.5, 0.3); - -// // Precomputed constants as raw doubles (mirrors Constants.ShooterK) -// private static final double kTurretOffsetX_m = edu.wpi.first.math.util.Units.inchesToMeters(-4.744); -// private static final double kTurretOffsetY_m = edu.wpi.first.math.util.Units.inchesToMeters(-4.239); -// private static final double kTurretOffsetZ_m = edu.wpi.first.math.util.Units.inchesToMeters(17.260); -// private static final double kTurretAngleOffsetRad = Math.toRadians(-135); -// private static final double kFlywheelRadiusM = edu.wpi.first.math.util.Units.inchesToMeters(1.5); -// private static final double kGravityInPerSec2 = 9.81 * 39.3701; // m/s^2 -> in/s^2 -// private static final double kTurretMinRotsD = -0.75; -// private static final double kTurretMaxRotsD = 0.75; - -// // LERP map data as raw double arrays, SORTED BY DISTANCE (must match TreeMap ordering) -// // Original entries: 1.476, 3.763, 3.854, 3.903, 5.657 -// private static final double[] LERP_DISTANCES = {1.476, 3.763, 3.854, 3.903, 5.657}; -// private static final double[] LERP_EXIT_VEL; -// private static final double[] LERP_HOOD_ANGLE; -// private static final double[] LERP_TOF = {1.125, 1.43, 1.38, 1.35, 1.56}; - -// static { -// // Convert the LERP map entries to raw radians, in sorted distance order -// LERP_EXIT_VEL = new double[] { -// RotationsPerSecond.of(57.72).in(RadiansPerSecond), // 1.476m -// RotationsPerSecond.of(77).in(RadiansPerSecond), // 3.763m -// RotationsPerSecond.of(79).in(RadiansPerSecond), // 3.854m -// RotationsPerSecond.of(73.18).in(RadiansPerSecond), // 3.903m -// RotationsPerSecond.of(95.00).in(RadiansPerSecond), // 5.657m -// }; -// LERP_HOOD_ANGLE = new double[] { -// Degrees.of(150.77).in(Radians), // 1.476m -// Degrees.of(300).in(Radians), // 3.763m -// Degrees.of(345).in(Radians), // 3.854m -// Degrees.of(327.75).in(Radians), // 3.903m -// Degrees.of(374.20).in(Radians), // 5.657m -// }; -// } - -// @BeforeAll -// static void setup() { -// HAL.initialize(500, 0); -// HUB_TARGET = FieldConstants.Hub.blueInnerCenterPoint; -// double hubX = HUB_TARGET.getX(); -// double hubY = HUB_TARGET.getY(); -// MID_POSE = new Pose2d(hubX - 3.9, hubY + 1.0, Rotation2d.fromDegrees(15)); -// } - -// // ================================================================ -// // Approach 1: Immutable Units (current implementation) -// // ================================================================ - -// private static ShotData immutableCalcShot(Pose2d robot, ChassisSpeeds fieldSpeeds, Translation3d target) { -// return ShotCalculator.iterativeMovingShotFromInterpolationMap(robot, fieldSpeeds, target, 3); -// } - -// // ================================================================ -// // Approach 2: Mutable Units -// // Reuses MutDistance, MutTime, etc. to avoid allocations in the loop. -// // ================================================================ - -// // Preallocated mutable measures for the mutable approach -// private static final MutDistance mut_dist = Meters.of(0).mutableCopy(); -// private static final MutTime mut_tof = Seconds.of(0).mutableCopy(); -// private static final MutLinearVelocity mut_exitVel = MetersPerSecond.of(0).mutableCopy(); -// private static final MutAngle mut_hoodAngle = Radians.of(0).mutableCopy(); - -// private static double mutableGetDistanceToTarget(Pose2d robot, Translation3d target) { -// Pose3d turretPose = new Pose3d(robot).transformBy(ShooterK.kTurretTransform); -// double dist = turretPose.getTranslation().toTranslation2d().getDistance(target.toTranslation2d()); -// mut_dist.mut_replace(dist, Meters); -// return dist; -// } - -// private static ShotData mutableIterativeMovingShot(Pose2d robot, ChassisSpeeds fieldSpeeds, Translation3d target) { -// double distance = mutableGetDistanceToTarget(robot, target); -// ShotData shot = ShotCalculator.m_shotMap.get(distance); -// shot = new ShotData(shot.exitVelocity(), shot.hoodAngle(), target); - -// // Use the mutable TOF instead of allocating -// double tofSec = ShotCalculator.m_timeOfFlightMap.get(distance); -// mut_tof.mut_replace(tofSec, Seconds); - -// Translation3d predictedTarget = target; - -// for (int i = 0; i < 3; i++) { -// ShotData prevShot = shot; -// double prevTOF = tofSec; -// Translation3d prevPredTarget = predictedTarget; - -// // predictTargetPos inline using mutable TOF -// double predX = target.getX() - fieldSpeeds.vxMetersPerSecond * tofSec; -// double predY = target.getY() - fieldSpeeds.vyMetersPerSecond * tofSec; -// predictedTarget = new Translation3d(predX, predY, target.getZ()); - -// distance = mutableGetDistanceToTarget(robot, predictedTarget); -// shot = ShotCalculator.m_shotMap.get(distance); -// shot = new ShotData(shot.exitVelocity(), shot.hoodAngle(), predictedTarget); -// tofSec = ShotCalculator.m_timeOfFlightMap.get(distance); -// mut_tof.mut_replace(tofSec, Seconds); - -// ShotData shotDiff = shot.minus(prevShot); -// double tofDiff = prevTOF - tofSec; - -// if (shotDiff.hoodAngle() < .05 && shotDiff.exitVelocity() < .05 -// && prevShot.target().getDistance(shot.getTarget()) < .05 -// && Math.abs(tofDiff) < .005 -// && prevPredTarget.getDistance(predictedTarget) < .05) { -// break; -// } -// } -// return shot; -// } - -// // ================================================================ -// // Approach 3: Raw Doubles (zero Units library usage) -// // ================================================================ - -// /** Linear interpolation between LERP table entries, all in raw doubles. */ -// private static double lerpLookup(double dist, double[] values) { -// if (dist <= LERP_DISTANCES[0]) return values[0]; -// if (dist >= LERP_DISTANCES[LERP_DISTANCES.length - 1]) return values[values.length - 1]; -// for (int i = 0; i < LERP_DISTANCES.length - 1; i++) { -// if (dist <= LERP_DISTANCES[i + 1]) { -// double t = (dist - LERP_DISTANCES[i]) / (LERP_DISTANCES[i + 1] - LERP_DISTANCES[i]); -// return values[i] + t * (values[i + 1] - values[i]); -// } -// } -// return values[values.length - 1]; -// } - -// /** -// * Turret-transformed distance to target in meters, all raw doubles. -// * Avoids Pose3d/Transform3d allocations entirely. -// */ -// private static double rawGetDistanceToTarget(double robotX, double robotY, double robotHeadingRad, -// double targetX, double targetY) { -// // Apply turret transform: rotate offset by robot heading, then translate -// double cosH = Math.cos(robotHeadingRad + kTurretAngleOffsetRad); -// double sinH = Math.sin(robotHeadingRad + kTurretAngleOffsetRad); -// // Turret offset in field frame (rotate the local offset by robot heading + turret angle offset) -// double cosR = Math.cos(robotHeadingRad); -// double sinR = Math.sin(robotHeadingRad); -// double turretX = robotX + kTurretOffsetX_m * cosR - kTurretOffsetY_m * sinR; -// double turretY = robotY + kTurretOffsetX_m * sinR + kTurretOffsetY_m * cosR; -// double dx = turretX - targetX; -// double dy = turretY - targetY; -// return Math.sqrt(dx * dx + dy * dy); -// } - -// /** -// * Full iterative moving shot pipeline using only raw doubles. -// * Returns [exitVelocity_radPerSec, hoodAngle_rad, predTargetX, predTargetY, predTargetZ]. -// */ -// private static double[] rawIterativeMovingShot(double robotX, double robotY, double robotHeadingRad, -// double vxMps, double vyMps, double targetX, double targetY, double targetZ) { - -// double distance = rawGetDistanceToTarget(robotX, robotY, robotHeadingRad, targetX, targetY); -// double exitVel = lerpLookup(distance, LERP_EXIT_VEL); -// double hoodAngle = lerpLookup(distance, LERP_HOOD_ANGLE); -// double tofSec = lerpLookup(distance, LERP_TOF); - -// double predX = targetX; -// double predY = targetY; - -// double prevExitVel, prevHoodAngle, prevTOF, prevPredX, prevPredY; - -// for (int i = 0; i < 3; i++) { -// prevExitVel = exitVel; -// prevHoodAngle = hoodAngle; -// prevTOF = tofSec; -// prevPredX = predX; -// prevPredY = predY; - -// predX = targetX - vxMps * tofSec; -// predY = targetY - vyMps * tofSec; - -// distance = rawGetDistanceToTarget(robotX, robotY, robotHeadingRad, predX, predY); -// exitVel = lerpLookup(distance, LERP_EXIT_VEL); -// hoodAngle = lerpLookup(distance, LERP_HOOD_ANGLE); -// tofSec = lerpLookup(distance, LERP_TOF); - -// double dExitVel = prevExitVel - exitVel; -// double dHood = prevHoodAngle - hoodAngle; -// double dPredX = prevPredX - predX; -// double dPredY = prevPredY - predY; -// double dTOF = prevTOF - tofSec; - -// if (Math.abs(dHood) < .05 && Math.abs(dExitVel) < .05 -// && Math.sqrt(dPredX * dPredX + dPredY * dPredY) < .05 -// && Math.abs(dTOF) < .005) { -// break; -// } -// } - -// return new double[]{exitVel, hoodAngle, predX, predY, targetZ}; -// } - -// /** -// * Full calcAzimuth equivalent, raw doubles only. -// * Returns [turretReferenceRots, turretFFRadPerSec]. -// */ -// private static double[] rawCalcAzimuth(double targetX, double targetY, double targetZ, -// double robotX, double robotY, double robotHeadingRad, -// double turretPositionRots, double vxMps, double vyMps, double omegaRps) { -// // Turret pivot in field space -// double cosR = Math.cos(robotHeadingRad); -// double sinR = Math.sin(robotHeadingRad); -// double turretX = robotX + kTurretOffsetX_m * cosR - kTurretOffsetY_m * sinR; -// double turretY = robotY + kTurretOffsetX_m * sinR + kTurretOffsetY_m * cosR; - -// // Vector from turret to target -// double dx = targetX - turretX; -// double dy = targetY - turretY; - -// // Field yaw to target -// double fieldYawRad = Math.atan2(dy, dx); - -// // Turret zero field direction -// double turretZeroRad = robotHeadingRad + kTurretAngleOffsetRad; - -// // Direction in turret frame -// double dirRad = fieldYawRad - turretZeroRad; -// double dirRots = dirRad / (2 * Math.PI); - -// // Normalize to turret range -// double angleRots = MathUtil.inputModulus(dirRots, kTurretMinRotsD, kTurretMaxRotsD); - -// // Snapback -// double safeAngleRots = angleRots; -// if (turretPositionRots > 0 && angleRots + 1 <= kTurretMaxRotsD) { -// safeAngleRots += 1; -// } else if (turretPositionRots < 0 && angleRots - 1 >= kTurretMinRotsD) { -// safeAngleRots -= 1; -// } - -// // Turret velocity FF -// double d2 = dx * dx + dy * dy; -// double turretFF = d2 > 0 -// ? (dy * vxMps - dx * vyMps) / d2 - omegaRps -// : 0.0; - -// return new double[]{safeAngleRots, turretFF}; -// } - -// /** -// * Full calcShot pipeline equivalent, raw doubles. -// * Returns [turretReferenceRots, hoodAngleDeg, shooterVelRadPerSec, turretFFRadPerSec]. -// */ -// private static double[] rawCalcShot(double robotX, double robotY, double robotHeadingRad, -// double turretPositionRots, double vxMps, double vyMps, double omegaRps, -// double targetX, double targetY, double targetZ) { - -// double[] shotResult = rawIterativeMovingShot( -// robotX, robotY, robotHeadingRad, vxMps, vyMps, targetX, targetY, targetZ); -// double exitVelRadPerSec = shotResult[0]; -// double hoodAngleRad = shotResult[1]; -// double predTargetX = shotResult[2]; -// double predTargetY = shotResult[3]; - -// double[] azResult = rawCalcAzimuth( -// predTargetX, predTargetY, targetZ, -// robotX, robotY, robotHeadingRad, -// turretPositionRots, vxMps, vyMps, omegaRps); - -// double turretRefRots = azResult[0]; -// double turretFFRadPerSec = azResult[1]; -// double shooterVelRadPerSec = exitVelRadPerSec; // already in rad/s from LERP - -// return new double[]{turretRefRots, hoodAngleRad, shooterVelRadPerSec, turretFFRadPerSec}; -// } - -// // ================================================================ -// // Benchmark -// // ================================================================ - -// private static final int WARMUP_ITERS = 50_000; -// private static final int BENCH_ITERS = 200_000; - -// @Test -// void benchmarkComparison() { -// System.out.println("\n=== Shot Calc Performance Benchmark ==="); -// System.out.println("Warmup iterations: " + WARMUP_ITERS); -// System.out.println("Benchmark iterations: " + BENCH_ITERS); -// System.out.println(); - -// // Extract raw values for approaches 2 & 3 -// double robotX = MID_POSE.getX(); -// double robotY = MID_POSE.getY(); -// double robotHeadingRad = MID_POSE.getRotation().getRadians(); -// double turretPosRots = 0.0; -// double vx = MOVING_SPEEDS.vxMetersPerSecond; -// double vy = MOVING_SPEEDS.vyMetersPerSecond; -// double omega = MOVING_SPEEDS.omegaRadiansPerSecond; -// double tX = HUB_TARGET.getX(); -// double tY = HUB_TARGET.getY(); -// double tZ = HUB_TARGET.getZ(); - -// // --- Warmup all three approaches --- -// for (int i = 0; i < WARMUP_ITERS; i++) { -// immutableCalcShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); -// } -// for (int i = 0; i < WARMUP_ITERS; i++) { -// mutableIterativeMovingShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); -// } -// for (int i = 0; i < WARMUP_ITERS; i++) { -// rawCalcShot(robotX, robotY, robotHeadingRad, turretPosRots, vx, vy, omega, tX, tY, tZ); -// } - -// // --- Benchmark 1: Immutable Units --- -// long t0 = System.nanoTime(); -// ShotData immutableResult = null; -// for (int i = 0; i < BENCH_ITERS; i++) { -// immutableResult = immutableCalcShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); -// } -// long immutableNs = System.nanoTime() - t0; - -// // --- Benchmark 2: Mutable Units --- -// long t1 = System.nanoTime(); -// ShotData mutableResult = null; -// for (int i = 0; i < BENCH_ITERS; i++) { -// mutableResult = mutableIterativeMovingShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); -// } -// long mutableNs = System.nanoTime() - t1; - -// // --- Benchmark 3: Raw Doubles --- -// long t2 = System.nanoTime(); -// double[] rawResult = null; -// for (int i = 0; i < BENCH_ITERS; i++) { -// rawResult = rawCalcShot(robotX, robotY, robotHeadingRad, turretPosRots, vx, vy, omega, tX, tY, tZ); -// } -// long rawNs = System.nanoTime() - t2; - -// // --- Results --- -// double immutableUs = immutableNs / 1000.0 / BENCH_ITERS; -// double mutableUs = mutableNs / 1000.0 / BENCH_ITERS; -// double rawUs = rawNs / 1000.0 / BENCH_ITERS; - -// System.out.println("┌─────────────────────┬──────────────┬───────────┐"); -// System.out.println("│ Approach │ us/call │ vs raw │"); -// System.out.println("├─────────────────────┼──────────────┼───────────┤"); -// System.out.printf("│ Immutable Units │ %10.3f │ %6.2fx │%n", immutableUs, immutableUs / rawUs); -// System.out.printf("│ Mutable Units │ %10.3f │ %6.2fx │%n", mutableUs, mutableUs / rawUs); -// System.out.printf("│ Raw Doubles │ %10.3f │ %6.2fx │%n", rawUs, 1.0); -// System.out.println("└─────────────────────┴──────────────┴───────────┘"); -// System.out.println(); - -// // Print estimated allocations per call -// System.out.println("Estimated heap allocations per call:"); -// System.out.println(" Immutable Units: ~30-50 objects (Pose3d, Translation3d, Distance, Time, Angle, ShotData...)"); -// System.out.println(" Mutable Units: ~20-35 objects (still Pose3d/Translation3d, but reuses Distance/Time)"); -// System.out.println(" Raw Doubles: 1 object (the double[] result array)"); -// System.out.println(); - -// // Print at 25Hz (the ShooterCalc Notifier rate) -// System.out.println("At 25Hz ShooterCalc rate:"); -// System.out.printf(" Immutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", -// immutableUs, immutableUs / 40000.0 * 100); -// System.out.printf(" Mutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", -// mutableUs, mutableUs / 40000.0 * 100); -// System.out.printf(" Raw Doubles: %.1f us/cycle (%.1f%% of 40ms budget)%n", -// rawUs, rawUs / 40000.0 * 100); -// System.out.println(); - -// // Verify raw doubles produce similar results to immutable -// System.out.println("Correctness check (immutable vs raw doubles):"); -// System.out.printf(" Exit velocity: immutable=%.4f rad/s, raw=%.4f rad/s%n", -// immutableResult.exitVelocity(), rawResult[2]); -// System.out.printf(" Hood angle: immutable=%.4f rad, raw=%.4f rad%n", -// immutableResult.hoodAngle(), rawResult[1]); -// System.out.println(); - -// // The test always passes — it's a benchmark, not an assertion. -// // But let's sanity-check that raw doubles aren't wildly wrong. -// double exitVelDiff = Math.abs(immutableResult.exitVelocity() - rawResult[2]); -// double hoodDiff = Math.abs(immutableResult.hoodAngle() - rawResult[1]); -// System.out.printf(" Exit velocity delta: %.6f rad/s%n", exitVelDiff); -// System.out.printf(" Hood angle delta: %.6f rad%n", hoodDiff); - -// // These can differ somewhat because the raw LERP is a simplified reimplementation -// // of InterpolatingTreeMap, but should be in the same ballpark -// if (exitVelDiff < 5.0 && hoodDiff < 0.5) { -// System.out.println(" -> Results are in reasonable agreement."); -// } else { -// System.out.println(" -> WARNING: Results differ significantly. LERP reimplementation may need tuning."); -// } -// } -// } +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.*; + +import frc.robot.Constants.ShooterK; +import frc.robot.FieldConstants; +import frc.robot.subsystems.shooter.ShotCalculator.ShotData; + +import org.junit.jupiter.api.*; + +/** + * Performance benchmark comparing three approaches for the shot calculation hot path: + * 1. Immutable WPILib Units (current implementation) + * 2. Mutable WPILib Units (MutAngle, MutDistance, etc.) + * 3. Raw doubles (no Units library at all) + * + * Each approach is timed over many iterations of the full calcShot pipeline. + * Results are printed as a comparison table. + */ +class ShotCalcPerfTest { + + private static Translation3d HUB_TARGET; + private static Pose2d MID_POSE; + private static final ChassisSpeeds MOVING_SPEEDS = new ChassisSpeeds(1.5, -0.5, 0.3); + + // Precomputed constants as raw doubles (mirrors Constants.ShooterK) + private static final double kTurretOffsetX_m = edu.wpi.first.math.util.Units.inchesToMeters(-4.744); + private static final double kTurretOffsetY_m = edu.wpi.first.math.util.Units.inchesToMeters(-4.239); + private static final double kTurretOffsetZ_m = edu.wpi.first.math.util.Units.inchesToMeters(17.260); + private static final double kTurretAngleOffsetRad = Math.toRadians(-135); + private static final double kFlywheelRadiusM = edu.wpi.first.math.util.Units.inchesToMeters(1.5); + private static final double kGravityInPerSec2 = 9.81 * 39.3701; // m/s^2 -> in/s^2 + private static final double kTurretMinRotsD = -0.75; + private static final double kTurretMaxRotsD = 0.75; + + // LERP map data as raw double arrays, SORTED BY DISTANCE (must match TreeMap ordering) + // Original entries: 1.476, 3.763, 3.854, 3.903, 5.657 + private static final double[] LERP_DISTANCES = {1.476, 3.763, 3.854, 3.903, 5.657}; + private static final double[] LERP_EXIT_VEL; + private static final double[] LERP_HOOD_ANGLE; + private static final double[] LERP_TOF = {1.125, 1.43, 1.38, 1.35, 1.56}; + + static { + // Convert the LERP map entries to raw radians, in sorted distance order + LERP_EXIT_VEL = new double[] { + RotationsPerSecond.of(57.72).in(RadiansPerSecond), // 1.476m + RotationsPerSecond.of(77).in(RadiansPerSecond), // 3.763m + RotationsPerSecond.of(79).in(RadiansPerSecond), // 3.854m + RotationsPerSecond.of(73.18).in(RadiansPerSecond), // 3.903m + RotationsPerSecond.of(95.00).in(RadiansPerSecond), // 5.657m + }; + LERP_HOOD_ANGLE = new double[] { + Degrees.of(150.77).in(Radians), // 1.476m + Degrees.of(300).in(Radians), // 3.763m + Degrees.of(345).in(Radians), // 3.854m + Degrees.of(327.75).in(Radians), // 3.903m + Degrees.of(374.20).in(Radians), // 5.657m + }; + } + + @BeforeAll + static void setup() { + HAL.initialize(500, 0); + HUB_TARGET = FieldConstants.Hub.blueInnerCenterPoint; + double hubX = HUB_TARGET.getX(); + double hubY = HUB_TARGET.getY(); + MID_POSE = new Pose2d(hubX - 3.9, hubY + 1.0, Rotation2d.fromDegrees(15)); + } + + // ================================================================ + // Approach 1: Immutable Units (current implementation) + // ================================================================ + + private static ShotData immutableCalcShot(Pose2d robot, ChassisSpeeds fieldSpeeds, Translation3d target) { + return ShotCalculator.iterativeMovingShotFromInterpolationMap(robot, fieldSpeeds, target, 3); + } + + // ================================================================ + // Approach 2: Mutable Units + // Reuses MutDistance, MutTime, etc. to avoid allocations in the loop. + // ================================================================ + + // Preallocated mutable measures for the mutable approach + private static final MutDistance mut_dist = Meters.of(0).mutableCopy(); + private static final MutTime mut_tof = Seconds.of(0).mutableCopy(); + private static final MutLinearVelocity mut_exitVel = MetersPerSecond.of(0).mutableCopy(); + private static final MutAngle mut_hoodAngle = Radians.of(0).mutableCopy(); + + private static double mutableGetDistanceToTarget(Pose2d robot, Translation3d target) { + Pose3d turretPose = new Pose3d(robot).transformBy(ShooterK.kTurretTransform); + double dist = turretPose.getTranslation().toTranslation2d().getDistance(target.toTranslation2d()); + mut_dist.mut_replace(dist, Meters); + return dist; + } + + private static ShotData mutableIterativeMovingShot(Pose2d robot, ChassisSpeeds fieldSpeeds, Translation3d target) { + double distance = mutableGetDistanceToTarget(robot, target); + ShotData shot = ShotCalculator.m_shotMap.get(distance); + shot = new ShotData(shot.exitVelocity(), shot.hoodAngle(), target); + + // Use the mutable TOF instead of allocating + double tofSec = ShotCalculator.m_timeOfFlightMap.get(distance); + mut_tof.mut_replace(tofSec, Seconds); + + Translation3d predictedTarget = target; + + for (int i = 0; i < 3; i++) { + ShotData prevShot = shot; + double prevTOF = tofSec; + Translation3d prevPredTarget = predictedTarget; + + // predictTargetPos inline using mutable TOF + double predX = target.getX() - fieldSpeeds.vxMetersPerSecond * tofSec; + double predY = target.getY() - fieldSpeeds.vyMetersPerSecond * tofSec; + predictedTarget = new Translation3d(predX, predY, target.getZ()); + + distance = mutableGetDistanceToTarget(robot, predictedTarget); + shot = ShotCalculator.m_shotMap.get(distance); + shot = new ShotData(shot.exitVelocity(), shot.hoodAngle(), predictedTarget); + tofSec = ShotCalculator.m_timeOfFlightMap.get(distance); + mut_tof.mut_replace(tofSec, Seconds); + + ShotData shotDiff = shot.minus(prevShot); + double tofDiff = prevTOF - tofSec; + + if (shotDiff.hoodAngle() < .05 && shotDiff.exitVelocity() < .05 + && prevShot.target().getDistance(shot.getTarget()) < .05 + && Math.abs(tofDiff) < .005 + && prevPredTarget.getDistance(predictedTarget) < .05) { + break; + } + } + return shot; + } + + // ================================================================ + // Approach 3: Raw Doubles (zero Units library usage) + // ================================================================ + + /** Linear interpolation between LERP table entries, all in raw doubles. */ + private static double lerpLookup(double dist, double[] values) { + if (dist <= LERP_DISTANCES[0]) return values[0]; + if (dist >= LERP_DISTANCES[LERP_DISTANCES.length - 1]) return values[values.length - 1]; + for (int i = 0; i < LERP_DISTANCES.length - 1; i++) { + if (dist <= LERP_DISTANCES[i + 1]) { + double t = (dist - LERP_DISTANCES[i]) / (LERP_DISTANCES[i + 1] - LERP_DISTANCES[i]); + return values[i] + t * (values[i + 1] - values[i]); + } + } + return values[values.length - 1]; + } + + /** + * Turret-transformed distance to target in meters, all raw doubles. + * Avoids Pose3d/Transform3d allocations entirely. + */ + private static double rawGetDistanceToTarget(double robotX, double robotY, double robotHeadingRad, + double targetX, double targetY) { + // Apply turret transform: rotate offset by robot heading, then translate + double cosH = Math.cos(robotHeadingRad + kTurretAngleOffsetRad); + double sinH = Math.sin(robotHeadingRad + kTurretAngleOffsetRad); + // Turret offset in field frame (rotate the local offset by robot heading + turret angle offset) + double cosR = Math.cos(robotHeadingRad); + double sinR = Math.sin(robotHeadingRad); + double turretX = robotX + kTurretOffsetX_m * cosR - kTurretOffsetY_m * sinR; + double turretY = robotY + kTurretOffsetX_m * sinR + kTurretOffsetY_m * cosR; + double dx = turretX - targetX; + double dy = turretY - targetY; + return Math.sqrt(dx * dx + dy * dy); + } + + /** + * Full iterative moving shot pipeline using only raw doubles. + * Returns [exitVelocity_radPerSec, hoodAngle_rad, predTargetX, predTargetY, predTargetZ]. + */ + private static double[] rawIterativeMovingShot(double robotX, double robotY, double robotHeadingRad, + double vxMps, double vyMps, double targetX, double targetY, double targetZ) { + + double distance = rawGetDistanceToTarget(robotX, robotY, robotHeadingRad, targetX, targetY); + double exitVel = lerpLookup(distance, LERP_EXIT_VEL); + double hoodAngle = lerpLookup(distance, LERP_HOOD_ANGLE); + double tofSec = lerpLookup(distance, LERP_TOF); + + double predX = targetX; + double predY = targetY; + + double prevExitVel, prevHoodAngle, prevTOF, prevPredX, prevPredY; + + for (int i = 0; i < 3; i++) { + prevExitVel = exitVel; + prevHoodAngle = hoodAngle; + prevTOF = tofSec; + prevPredX = predX; + prevPredY = predY; + + predX = targetX - vxMps * tofSec; + predY = targetY - vyMps * tofSec; + + distance = rawGetDistanceToTarget(robotX, robotY, robotHeadingRad, predX, predY); + exitVel = lerpLookup(distance, LERP_EXIT_VEL); + hoodAngle = lerpLookup(distance, LERP_HOOD_ANGLE); + tofSec = lerpLookup(distance, LERP_TOF); + + double dExitVel = prevExitVel - exitVel; + double dHood = prevHoodAngle - hoodAngle; + double dPredX = prevPredX - predX; + double dPredY = prevPredY - predY; + double dTOF = prevTOF - tofSec; + + if (Math.abs(dHood) < .05 && Math.abs(dExitVel) < .05 + && Math.sqrt(dPredX * dPredX + dPredY * dPredY) < .05 + && Math.abs(dTOF) < .005) { + break; + } + } + + return new double[]{exitVel, hoodAngle, predX, predY, targetZ}; + } + + /** + * Full calcAzimuth equivalent, raw doubles only. + * Returns [turretReferenceRots, turretFFRadPerSec]. + */ + private static double[] rawCalcAzimuth(double targetX, double targetY, double targetZ, + double robotX, double robotY, double robotHeadingRad, + double turretPositionRots, double vxMps, double vyMps, double omegaRps) { + // Turret pivot in field space + double cosR = Math.cos(robotHeadingRad); + double sinR = Math.sin(robotHeadingRad); + double turretX = robotX + kTurretOffsetX_m * cosR - kTurretOffsetY_m * sinR; + double turretY = robotY + kTurretOffsetX_m * sinR + kTurretOffsetY_m * cosR; + + // Vector from turret to target + double dx = targetX - turretX; + double dy = targetY - turretY; + + // Field yaw to target + double fieldYawRad = Math.atan2(dy, dx); + + // Turret zero field direction + double turretZeroRad = robotHeadingRad + kTurretAngleOffsetRad; + + // Direction in turret frame + double dirRad = fieldYawRad - turretZeroRad; + double dirRots = dirRad / (2 * Math.PI); + + // Normalize to turret range + double angleRots = MathUtil.inputModulus(dirRots, kTurretMinRotsD, kTurretMaxRotsD); + + // Snapback + double safeAngleRots = angleRots; + if (turretPositionRots > 0 && angleRots + 1 <= kTurretMaxRotsD) { + safeAngleRots += 1; + } else if (turretPositionRots < 0 && angleRots - 1 >= kTurretMinRotsD) { + safeAngleRots -= 1; + } + + // Turret velocity FF + double d2 = dx * dx + dy * dy; + double turretFF = d2 > 0 + ? (dy * vxMps - dx * vyMps) / d2 - omegaRps + : 0.0; + + return new double[]{safeAngleRots, turretFF}; + } + + /** + * Full calcShot pipeline equivalent, raw doubles. + * Returns [turretReferenceRots, hoodAngleDeg, shooterVelRadPerSec, turretFFRadPerSec]. + */ + private static double[] rawCalcShot(double robotX, double robotY, double robotHeadingRad, + double turretPositionRots, double vxMps, double vyMps, double omegaRps, + double targetX, double targetY, double targetZ) { + + double[] shotResult = rawIterativeMovingShot( + robotX, robotY, robotHeadingRad, vxMps, vyMps, targetX, targetY, targetZ); + double exitVelRadPerSec = shotResult[0]; + double hoodAngleRad = shotResult[1]; + double predTargetX = shotResult[2]; + double predTargetY = shotResult[3]; + + double[] azResult = rawCalcAzimuth( + predTargetX, predTargetY, targetZ, + robotX, robotY, robotHeadingRad, + turretPositionRots, vxMps, vyMps, omegaRps); + + double turretRefRots = azResult[0]; + double turretFFRadPerSec = azResult[1]; + double shooterVelRadPerSec = exitVelRadPerSec; // already in rad/s from LERP + + return new double[]{turretRefRots, hoodAngleRad, shooterVelRadPerSec, turretFFRadPerSec}; + } + + // ================================================================ + // Benchmark + // ================================================================ + + private static final int WARMUP_ITERS = 50_000; + private static final int BENCH_ITERS = 200_000; + + @Test + void benchmarkComparison() { + System.out.println("\n=== Shot Calc Performance Benchmark ==="); + System.out.println("Warmup iterations: " + WARMUP_ITERS); + System.out.println("Benchmark iterations: " + BENCH_ITERS); + System.out.println(); + + // Extract raw values for approaches 2 & 3 + double robotX = MID_POSE.getX(); + double robotY = MID_POSE.getY(); + double robotHeadingRad = MID_POSE.getRotation().getRadians(); + double turretPosRots = 0.0; + double vx = MOVING_SPEEDS.vxMetersPerSecond; + double vy = MOVING_SPEEDS.vyMetersPerSecond; + double omega = MOVING_SPEEDS.omegaRadiansPerSecond; + double tX = HUB_TARGET.getX(); + double tY = HUB_TARGET.getY(); + double tZ = HUB_TARGET.getZ(); + + // --- Warmup all three approaches --- + for (int i = 0; i < WARMUP_ITERS; i++) { + immutableCalcShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); + } + for (int i = 0; i < WARMUP_ITERS; i++) { + mutableIterativeMovingShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); + } + for (int i = 0; i < WARMUP_ITERS; i++) { + rawCalcShot(robotX, robotY, robotHeadingRad, turretPosRots, vx, vy, omega, tX, tY, tZ); + } + + // --- Benchmark 1: Immutable Units --- + long t0 = System.nanoTime(); + ShotData immutableResult = null; + for (int i = 0; i < BENCH_ITERS; i++) { + immutableResult = immutableCalcShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); + } + long immutableNs = System.nanoTime() - t0; + + // --- Benchmark 2: Mutable Units --- + long t1 = System.nanoTime(); + ShotData mutableResult = null; + for (int i = 0; i < BENCH_ITERS; i++) { + mutableResult = mutableIterativeMovingShot(MID_POSE, MOVING_SPEEDS, HUB_TARGET); + } + long mutableNs = System.nanoTime() - t1; + + // --- Benchmark 3: Raw Doubles --- + long t2 = System.nanoTime(); + double[] rawResult = null; + for (int i = 0; i < BENCH_ITERS; i++) { + rawResult = rawCalcShot(robotX, robotY, robotHeadingRad, turretPosRots, vx, vy, omega, tX, tY, tZ); + } + long rawNs = System.nanoTime() - t2; + + // --- Results --- + double immutableUs = immutableNs / 1000.0 / BENCH_ITERS; + double mutableUs = mutableNs / 1000.0 / BENCH_ITERS; + double rawUs = rawNs / 1000.0 / BENCH_ITERS; + + // Print estimated allocations per call + System.out.println("Estimated heap allocations per call:"); + System.out.println(" Immutable Units: ~30-50 objects (Pose3d, Translation3d, Distance, Time, Angle, ShotData...)"); + System.out.println(" Mutable Units: ~20-35 objects (still Pose3d/Translation3d, but reuses Distance/Time)"); + System.out.println(" Raw Doubles: 1 object (the double[] result array)"); + System.out.println(); + + // Print at 25Hz (the ShooterCalc Notifier rate) + System.out.println("At 25Hz ShooterCalc rate:"); + System.out.printf(" Immutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", + immutableUs, immutableUs / 40000.0 * 100); + System.out.printf(" Mutable Units: %.1f us/cycle (%.1f%% of 40ms budget)%n", + mutableUs, mutableUs / 40000.0 * 100); + System.out.printf(" Raw Doubles: %.1f us/cycle (%.1f%% of 40ms budget)%n", + rawUs, rawUs / 40000.0 * 100); + System.out.println(); + + // Verify raw doubles produce similar results to immutable + System.out.println("Correctness check (immutable vs raw doubles):"); + System.out.printf(" Exit velocity: immutable=%.4f rad/s, raw=%.4f rad/s%n", + immutableResult.exitVelocity(), rawResult[2]); + System.out.printf(" Hood angle: immutable=%.4f rad, raw=%.4f rad%n", + immutableResult.hoodAngle(), rawResult[1]); + System.out.println(); + + // The test always passes — it's a benchmark, not an assertion. + // But let's sanity-check that raw doubles aren't wildly wrong. + double exitVelDiff = Math.abs(immutableResult.exitVelocity() - rawResult[2]); + double hoodDiff = Math.abs(immutableResult.hoodAngle() - rawResult[1]); + System.out.printf(" Exit velocity delta: %.6f rad/s%n", exitVelDiff); + System.out.printf(" Hood angle delta: %.6f rad%n", hoodDiff); + + // These can differ somewhat because the raw LERP is a simplified reimplementation + // of InterpolatingTreeMap, but should be in the same ballpark + if (exitVelDiff < 5.0 && hoodDiff < 0.5) { + System.out.println(" -> Results are in reasonable agreement."); + } else { + System.out.println(" -> WARNING: Results differ significantly. LERP reimplementation may need tuning."); + } + } +} diff --git a/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java b/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java index 264765d2..b65799e2 100644 --- a/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java +++ b/src/test/java/frc/robot/subsystems/shooter/ShotCalcTest.java @@ -1,619 +1,619 @@ -// package frc.robot.subsystems.shooter; - -// import static edu.wpi.first.units.Units.*; -// import static org.junit.jupiter.api.Assertions.*; - -// import edu.wpi.first.hal.HAL; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.geometry.Translation3d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.units.measure.*; - -// import frc.robot.subsystems.shooter.ShotCalculator.ShotData; -// import frc.robot.subsystems.shooter.ShooterCalc.AzimuthCalcDetails; -// import frc.robot.subsystems.shooter.ShooterCalc.ShotCalcOutputs; - -// import org.junit.jupiter.api.*; - -// /** -// * Characterization tests for the static shot calculation math. -// * These lock down current behavior before refactoring to raw-double equivalents. -// */ -// class ShotCalcTest { - -// // ---- Shared test fixtures ---- - -// // Blue hub inner center (from FieldConstants, loaded at class init) -// private static Translation3d HUB_TARGET; - -// // Representative robot poses at various distances from the blue hub -// private static Pose2d CLOSE_POSE; // ~1.5m from hub -// private static Pose2d MID_POSE; // ~3.9m from hub -// private static Pose2d FAR_POSE; // ~5.7m from hub - -// private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(0, 0, 0); -// private static final ChassisSpeeds TRANSLATING_SPEEDS = new ChassisSpeeds(2.0, 1.0, 0); -// private static final ChassisSpeeds ROTATING_SPEEDS = new ChassisSpeeds(0, 0, 1.0); -// private static final ChassisSpeeds COMBINED_SPEEDS = new ChassisSpeeds(1.5, -0.5, 0.5); - -// @BeforeAll -// static void setup() { -// HAL.initialize(500, 0); - -// // These depend on FieldConstants which loads AprilTag layout -// HUB_TARGET = frc.robot.FieldConstants.Hub.blueInnerCenterPoint; - -// // Poses facing the hub from different distances (blue alliance side) -// // Hub is roughly at x~4.6, y~4.0 (field center-ish) -// double hubX = HUB_TARGET.getX(); -// double hubY = HUB_TARGET.getY(); - -// CLOSE_POSE = new Pose2d(hubX - 1.5, hubY, Rotation2d.kZero); -// MID_POSE = new Pose2d(hubX - 3.9, hubY + 1.0, Rotation2d.fromDegrees(15)); -// FAR_POSE = new Pose2d(hubX - 5.7, hubY - 1.5, Rotation2d.fromDegrees(-20)); -// } - -// // ================================================================ -// // ShotCalculator: simple unit conversion functions -// // ================================================================ - -// @Nested -// class LinearAngularConversions { -// @Test -// void linearToAngular_basicConversion() { -// // v = 10 m/s, r = 0.5 m -> omega = v/r = 20 rad/s -// var result = ShotCalculator.linearToAngularVelocity( -// MetersPerSecond.of(10.0), Meters.of(0.5)); -// assertEquals(20.0, result.in(RadiansPerSecond), 1e-9); -// } - -// @Test -// void angularToLinear_basicConversion() { -// // omega = 20 rad/s, r = 0.5 m -> v = omega*r = 10 m/s -// var result = ShotCalculator.angularToLinearVelocity( -// RadiansPerSecond.of(20.0), Meters.of(0.5)); -// assertEquals(10.0, result.in(MetersPerSecond), 1e-9); -// } - -// @Test -// void roundTrip_linearToAngularAndBack() { -// LinearVelocity original = MetersPerSecond.of(7.3); -// Distance radius = Inches.of(1.5); // flywheel radius -// AngularVelocity angular = ShotCalculator.linearToAngularVelocity(original, radius); -// LinearVelocity recovered = ShotCalculator.angularToLinearVelocity(angular, radius); -// assertEquals(original.in(MetersPerSecond), recovered.in(MetersPerSecond), 1e-9); -// } - -// @Test -// void zeroVelocity() { -// var result = ShotCalculator.linearToAngularVelocity( -// MetersPerSecond.of(0), Meters.of(0.5)); -// assertEquals(0.0, result.in(RadiansPerSecond), 1e-9); -// } -// } - -// // ================================================================ -// // ShotCalculator.getDistanceToTarget -// // ================================================================ - -// @Nested -// class GetDistanceToTarget { -// @Test -// void closePose_returnsPositiveDistance() { -// Distance d = ShotCalculator.getDistanceToTarget(CLOSE_POSE, HUB_TARGET); -// assertTrue(d.in(Meters) > 0, "Distance should be positive"); -// // Close pose is ~1.5m from hub, but turret transform offsets it -// assertTrue(d.in(Meters) < 3.0, "Close pose should be within 3m"); -// } - -// @Test -// void midPose_returnsReasonableDistance() { -// Distance d = ShotCalculator.getDistanceToTarget(MID_POSE, HUB_TARGET); -// assertTrue(d.in(Meters) > 2.0); -// assertTrue(d.in(Meters) < 6.0); -// } - -// @Test -// void farPose_returnsLargerDistance() { -// Distance dClose = ShotCalculator.getDistanceToTarget(CLOSE_POSE, HUB_TARGET); -// Distance dFar = ShotCalculator.getDistanceToTarget(FAR_POSE, HUB_TARGET); -// assertTrue(dFar.in(Meters) > dClose.in(Meters), -// "Far pose should be further than close pose"); -// } - -// @Test -// void samePosition_returnsSmallDistance() { -// // Robot right at the hub X,Y — distance should just be the turret transform offset -// Pose2d atHub = new Pose2d(HUB_TARGET.getX(), HUB_TARGET.getY(), Rotation2d.kZero); -// Distance d = ShotCalculator.getDistanceToTarget(atHub, HUB_TARGET); -// // Should be small but nonzero due to turret offset -// assertTrue(d.in(Meters) < 1.0); -// assertTrue(d.in(Meters) >= 0); -// } - -// @Test -// void isConsistent_withDifferentHeadings() { -// // Robot heading shouldn't drastically change the 2D distance -// // (turret transform rotates with robot, so there IS some effect) -// Pose2d heading0 = new Pose2d(2.0, 4.0, Rotation2d.kZero); -// Pose2d heading90 = new Pose2d(2.0, 4.0, Rotation2d.kCCW_90deg); -// Distance d0 = ShotCalculator.getDistanceToTarget(heading0, HUB_TARGET); -// Distance d90 = ShotCalculator.getDistanceToTarget(heading90, HUB_TARGET); -// // Both should be in a similar ballpark (within turret offset range ~0.17m) -// assertEquals(d0.in(Meters), d90.in(Meters), 0.5); -// } -// } - -// // ================================================================ -// // ShotCalculator.calculateTimeOfFlight -// // ================================================================ - -// @Nested -// class CalculateTimeOfFlight { -// @Test -// void basicTimeOfFlight() { -// // v = 10 m/s at 45 deg hood -> horizontal = 10*cos(45deg) ≈ 7.07 m/s -// // distance = 3m -> tof = 3/7.07 ≈ 0.424 s -// // Note: hood angle 0 = horizontal in this function (pi/2 - hoodAngle) -// // Actually from code: angle = PI/2 - hoodAngle.in(Radians) -// // So hoodAngle of 45 deg -> effective angle = 45 deg -> cos(45deg) component -// Time tof = ShotCalculator.calculateTimeOfFlight( -// MetersPerSecond.of(10.0), -// Degrees.of(45), -// Meters.of(3.0)); -// double tofSec = tof.in(Seconds); -// assertFalse(Double.isNaN(tofSec), "TOF should not be NaN"); -// assertTrue(tofSec > 0, "TOF should be positive"); -// } - -// @Test -// void longerDistance_longerTOF() { -// Time tofShort = ShotCalculator.calculateTimeOfFlight( -// MetersPerSecond.of(10.0), Degrees.of(30), Meters.of(2.0)); -// Time tofLong = ShotCalculator.calculateTimeOfFlight( -// MetersPerSecond.of(10.0), Degrees.of(30), Meters.of(5.0)); -// assertTrue(tofLong.in(Seconds) > tofShort.in(Seconds)); -// } - -// @Test -// void fasterVelocity_shorterTOF() { -// Time tofSlow = ShotCalculator.calculateTimeOfFlight( -// MetersPerSecond.of(5.0), Degrees.of(30), Meters.of(3.0)); -// Time tofFast = ShotCalculator.calculateTimeOfFlight( -// MetersPerSecond.of(15.0), Degrees.of(30), Meters.of(3.0)); -// assertTrue(tofFast.in(Seconds) < tofSlow.in(Seconds)); -// } -// } - -// // ================================================================ -// // ShotCalculator.predictTargetPos -// // ================================================================ - -// @Nested -// class PredictTargetPos { -// @Test -// void stationaryRobot_targetUnchanged() { -// Translation3d target = new Translation3d(5, 4, 1.5); -// Translation3d predicted = ShotCalculator.predictTargetPos( -// target, ZERO_SPEEDS, Seconds.of(1.0)); -// assertEquals(target.getX(), predicted.getX(), 1e-9); -// assertEquals(target.getY(), predicted.getY(), 1e-9); -// assertEquals(target.getZ(), predicted.getZ(), 1e-9); -// } - -// @Test -// void movingRobot_targetShiftsOppositeToVelocity() { -// Translation3d target = new Translation3d(5, 4, 1.5); -// ChassisSpeeds speeds = new ChassisSpeeds(2.0, 0, 0); // moving +x at 2 m/s -// Translation3d predicted = ShotCalculator.predictTargetPos( -// target, speeds, Seconds.of(1.0)); - -// // predicted.x = target.x - vx * tof = 5 - 2*1 = 3 -// assertEquals(3.0, predicted.getX(), 1e-9); -// assertEquals(4.0, predicted.getY(), 1e-9); -// assertEquals(1.5, predicted.getZ(), 1e-9); // Z unchanged -// } - -// @Test -// void zeroTOF_targetUnchanged() { -// Translation3d target = new Translation3d(5, 4, 1.5); -// Translation3d predicted = ShotCalculator.predictTargetPos( -// target, TRANSLATING_SPEEDS, Seconds.of(0)); -// assertEquals(target.getX(), predicted.getX(), 1e-9); -// assertEquals(target.getY(), predicted.getY(), 1e-9); -// } - -// @Test -// void movingInY_shiftsY() { -// Translation3d target = new Translation3d(5, 4, 1.5); -// ChassisSpeeds speeds = new ChassisSpeeds(0, 3.0, 0); // moving +y at 3 m/s -// Translation3d predicted = ShotCalculator.predictTargetPos( -// target, speeds, Seconds.of(0.5)); -// assertEquals(5.0, predicted.getX(), 1e-9); -// assertEquals(4.0 - 3.0 * 0.5, predicted.getY(), 1e-9); // 4 - 1.5 = 2.5 -// } -// } - -// // ================================================================ -// // ShotCalculator.iterativeMovingShotFromInterpolationMap -// // ================================================================ - -// @Nested -// class IterativeMovingShotFromInterpolationMap { -// @Test -// void stationaryClose_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// CLOSE_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void stationaryMid_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void stationaryFar_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// FAR_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void movingRobot_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void rotatingRobot_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, ROTATING_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void stationaryShot_targetMatchesInput() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// // With zero speeds, predicted target should equal actual target -// assertEquals(HUB_TARGET.getX(), shot.getTarget().getX(), 1e-6); -// assertEquals(HUB_TARGET.getY(), shot.getTarget().getY(), 1e-6); -// assertEquals(HUB_TARGET.getZ(), shot.getTarget().getZ(), 1e-6); -// } - -// @Test -// void movingShot_targetDiffersFromStatic() { -// ShotData staticShot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// ShotData movingShot = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); -// // Moving shot should shift the predicted target -// assertNotEquals(staticShot.getTarget().getX(), movingShot.getTarget().getX(), 1e-6, -// "Moving shot predicted target X should differ from static"); -// } - -// @Test -// void moreIterations_convergesToSameResult() { -// ShotData shot1 = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 1); -// ShotData shot3 = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); -// ShotData shot10 = ShotCalculator.iterativeMovingShotFromInterpolationMap( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 10); -// // 3 and 10 iterations should converge closely -// assertEquals(shot3.exitVelocity(), shot10.exitVelocity(), 0.5, -// "3 and 10 iterations should converge on exit velocity"); -// assertEquals(shot3.hoodAngle(), shot10.hoodAngle(), 0.1, -// "3 and 10 iterations should converge on hood angle"); -// } -// } - -// // ================================================================ -// // ShotCalculator.calculateShotFromFunnelClearance -// // ================================================================ - -// @Nested -// class CalculateShotFromFunnelClearance { -// @Test -// void stationaryClose_returnsValidShot() { -// ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( -// CLOSE_POSE, HUB_TARGET, HUB_TARGET); -// assertShotDataValid(shot); -// } - -// @Test -// void stationaryMid_returnsValidShot() { -// ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( -// MID_POSE, HUB_TARGET, HUB_TARGET); -// assertShotDataValid(shot); -// } - -// @Test -// void stationaryFar_returnsValidShot() { -// ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( -// FAR_POSE, HUB_TARGET, HUB_TARGET); -// assertShotDataValid(shot); -// } -// } - -// // ================================================================ -// // ShotCalculator.iterativeMovingShotFromFunnelClearance -// // ================================================================ - -// @Nested -// class IterativeMovingShotFromFunnelClearance { -// @Test -// void stationaryMid_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromFunnelClearance( -// MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } - -// @Test -// void movingRobot_returnsValidShot() { -// ShotData shot = ShotCalculator.iterativeMovingShotFromFunnelClearance( -// MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); -// assertShotDataValid(shot); -// } -// } - -// // ================================================================ -// // ShooterCalc.calcAzimuth (static method) -// // ================================================================ - -// @Nested -// class CalcAzimuth { -// @Test -// void turretAtZero_closeTarget_returnsValidAzimuth() { -// AzimuthCalcDetails details = ShooterCalc.calcAzimuth( -// HUB_TARGET, CLOSE_POSE, 0.0, ZERO_SPEEDS); -// assertAzimuthValid(details); -// } - -// @Test -// void turretAtZero_midTarget_returnsValidAzimuth() { -// AzimuthCalcDetails details = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); -// assertAzimuthValid(details); -// } - -// @Test -// void turretAtPositiveQuarter_returnsValidAzimuth() { -// AzimuthCalcDetails details = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, 0.25, ZERO_SPEEDS); -// assertAzimuthValid(details); -// } - -// @Test -// void turretAtNegativeQuarter_returnsValidAzimuth() { -// AzimuthCalcDetails details = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, -0.25, ZERO_SPEEDS); -// assertAzimuthValid(details); -// } - -// @Test -// void movingRobot_producesVelocityFF() { -// AzimuthCalcDetails stationary = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); -// AzimuthCalcDetails moving = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, 0.0, TRANSLATING_SPEEDS); -// // Moving robot should produce nonzero velocity feedforward -// assertNotEquals(0.0, moving.turretVelocityFFRotPerSec(), 1e-6, -// "Moving robot should produce nonzero turret velocity FF"); -// } - -// @Test -// void stationaryRobot_zeroVelocityFF() { -// AzimuthCalcDetails details = ShooterCalc.calcAzimuth( -// HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); -// assertEquals(0.0, details.turretVelocityFFRotPerSec(), 1e-6, -// "Stationary robot should have zero velocity FF (omega=0, v=0)"); -// } - -// @Test -// void turretReference_withinPhysicalLimits() { -// // Test across many poses and turret positions -// Pose2d[] poses = {CLOSE_POSE, MID_POSE, FAR_POSE}; -// double[] turretRots = {-0.5, -0.25, 0, 0.25, 0.5}; -// for (Pose2d pose : poses) { -// for (double rot : turretRots) { -// AzimuthCalcDetails d = ShooterCalc.calcAzimuth( -// HUB_TARGET, pose, rot, ZERO_SPEEDS); -// double refRots = d.turretReferenceRots(); -// assertTrue(refRots >= -0.76 && refRots <= 0.76, -// String.format("Turret reference %.4f out of [-0.75, 0.75] for pose=%s, turret=%.2f", -// refRots, pose, rot)); -// } -// } -// } -// } - -// // ================================================================ -// // ShooterCalc.calcShot (static method, full pipeline) -// // ================================================================ - -// @Nested -// class CalcShot { -// @Test -// void staticShot_close_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void staticShot_mid_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void staticShot_far_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void dynamicShot_translating_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// MID_POSE, false, HUB_TARGET, 0.0, TRANSLATING_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void dynamicShot_rotating_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// MID_POSE, false, HUB_TARGET, 0.0, ROTATING_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void dynamicShot_combined_returnsValidOutputs() { -// ShotCalcOutputs out = ShooterCalc.calcShot( -// MID_POSE, false, HUB_TARGET, 0.1, COMBINED_SPEEDS); -// assertShotCalcOutputsValid(out); -// } - -// @Test -// void staticVsDynamic_sameWhenStationary() { -// ShotCalcOutputs staticOut = ShooterCalc.calcShot( -// MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// ShotCalcOutputs dynamicOut = ShooterCalc.calcShot( -// MID_POSE, false, HUB_TARGET, 0.0, ZERO_SPEEDS); -// // With zero chassis speeds, static and dynamic should produce identical results -// assertEquals( -// staticOut.shooterReferenceRotPerSec(), -// dynamicOut.shooterReferenceRotPerSec(), -// 1e-6, "Zero-speed dynamic should match static shot"); -// assertEquals( -// staticOut.hoodReferenceRots(), -// dynamicOut.hoodReferenceRots(), -// 1e-6); -// assertEquals( -// staticOut.turretReferenceRots(), -// dynamicOut.turretReferenceRots(), -// 1e-6); -// } - -// @Test -// void farShot_higherVelocityThanClose() { -// ShotCalcOutputs closeOut = ShooterCalc.calcShot( -// CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// ShotCalcOutputs farOut = ShooterCalc.calcShot( -// FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); -// assertTrue( -// farOut.shooterReferenceRotPerSec() > -// closeOut.shooterReferenceRotPerSec(), -// "Far shot should require higher flywheel velocity"); -// } -// } - -// // ================================================================ -// // ShotData record tests -// // ================================================================ - -// @Nested -// class ShotDataTests { -// @Test -// void interpolate_midpoint() { -// ShotData a = new ShotData(10.0, 1.0); -// ShotData b = new ShotData(20.0, 2.0); -// ShotData mid = ShotData.interpolate(a, b, 0.5); -// assertEquals(15.0, mid.exitVelocity(), 1e-9); -// assertEquals(1.5, mid.hoodAngle(), 1e-9); -// } - -// @Test -// void interpolate_atStart() { -// ShotData a = new ShotData(10.0, 1.0); -// ShotData b = new ShotData(20.0, 2.0); -// ShotData result = ShotData.interpolate(a, b, 0.0); -// assertEquals(10.0, result.exitVelocity(), 1e-9); -// assertEquals(1.0, result.hoodAngle(), 1e-9); -// } - -// @Test -// void interpolate_atEnd() { -// ShotData a = new ShotData(10.0, 1.0); -// ShotData b = new ShotData(20.0, 2.0); -// ShotData result = ShotData.interpolate(a, b, 1.0); -// assertEquals(20.0, result.exitVelocity(), 1e-9); -// assertEquals(2.0, result.hoodAngle(), 1e-9); -// } - -// @Test -// void minus_computesDifference() { -// ShotData a = new ShotData(15.0, 1.5); -// ShotData b = new ShotData(10.0, 1.0); -// ShotData diff = a.minus(b); -// assertEquals(10.0 - 15.0, diff.exitVelocity(), 1e-9); -// assertEquals(1.0 - 1.5, diff.hoodAngle(), 1e-9); -// } - -// @Test -// void getExitVelocity_convertsFromRadPerSec() { -// ShotData data = new ShotData( -// RotationsPerSecond.of(50), Degrees.of(45)); -// LinearVelocity vel = data.getExitVelocity(); -// assertFalse(Double.isNaN(vel.in(MetersPerSecond))); -// assertTrue(vel.in(MetersPerSecond) > 0); -// } - -// @Test -// void getHoodAngle_convertsFromRadians() { -// ShotData data = new ShotData( -// RotationsPerSecond.of(50), Degrees.of(45)); -// Angle angle = data.getHoodAngle(); -// assertEquals(45.0, angle.in(Degrees), 0.01); -// } -// } - -// // ================================================================ -// // Assertion helpers -// // ================================================================ - -// private static void assertShotDataValid(ShotData shot) { -// assertNotNull(shot, "ShotData should not be null"); -// assertFalse(Double.isNaN(shot.exitVelocity()), -// "Exit velocity should not be NaN"); -// assertFalse(Double.isNaN(shot.hoodAngle()), -// "Hood angle should not be NaN"); -// assertFalse(Double.isInfinite(shot.exitVelocity()), -// "Exit velocity should not be infinite"); -// assertFalse(Double.isInfinite(shot.hoodAngle()), -// "Hood angle should not be infinite"); -// assertNotNull(shot.getTarget(), "Shot target should not be null"); -// } - -// private static void assertAzimuthValid(AzimuthCalcDetails details) { -// assertNotNull(details, "AzimuthCalcDetails should not be null"); -// assertFalse(Double.isNaN(details.turretReferenceRots()), -// "Turret reference should not be NaN"); -// assertFalse(Double.isNaN(details.rawDesiredRotations()), -// "Raw desired rotations should not be NaN"); -// assertFalse(Double.isNaN(details.turretVelocityFFRotPerSec()), -// "Turret velocity FF should not be NaN"); -// } - -// private static void assertShotCalcOutputsValid(ShotCalcOutputs out) { -// assertNotNull(out, "ShotCalcOutputs should not be null"); -// assertAzimuthValid(out.turretCalcDetails()); -// assertNotNull(out.shotData(), "ShotData should not be null"); -// assertShotDataValid(out.shotData()); -// assertFalse(Double.isNaN(out.turretReferenceRots()), -// "Turret reference should not be NaN"); -// assertFalse(Double.isNaN(out.hoodReferenceRots()), -// "Hood reference should not be NaN"); -// assertFalse(Double.isNaN(out.shooterReferenceRotPerSec()), -// "Shooter reference should not be NaN"); -// assertTrue(out.shooterReferenceRotPerSec() >= 0, -// "Shooter velocity should be non-negative"); -// } -// } +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.*; +import static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.*; + +import frc.robot.subsystems.shooter.ShotCalculator.ShotData; +import frc.robot.subsystems.shooter.ShooterCalc.AzimuthCalcDetails; +import frc.robot.subsystems.shooter.ShooterCalc.ShotCalcOutputs; + +import org.junit.jupiter.api.*; + +/** + * Characterization tests for the static shot calculation math. + * These lock down current behavior before refactoring to raw-double equivalents. + */ +class ShotCalcTest { + + // ---- Shared test fixtures ---- + + // Blue hub inner center (from FieldConstants, loaded at class init) + private static Translation3d HUB_TARGET; + + // Representative robot poses at various distances from the blue hub + private static Pose2d CLOSE_POSE; // ~1.5m from hub + private static Pose2d MID_POSE; // ~3.9m from hub + private static Pose2d FAR_POSE; // ~5.7m from hub + + private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(0, 0, 0); + private static final ChassisSpeeds TRANSLATING_SPEEDS = new ChassisSpeeds(2.0, 1.0, 0); + private static final ChassisSpeeds ROTATING_SPEEDS = new ChassisSpeeds(0, 0, 1.0); + private static final ChassisSpeeds COMBINED_SPEEDS = new ChassisSpeeds(1.5, -0.5, 0.5); + + @BeforeAll + static void setup() { + HAL.initialize(500, 0); + + // These depend on FieldConstants which loads AprilTag layout + HUB_TARGET = frc.robot.FieldConstants.Hub.blueInnerCenterPoint; + + // Poses facing the hub from different distances (blue alliance side) + // Hub is roughly at x~4.6, y~4.0 (field center-ish) + double hubX = HUB_TARGET.getX(); + double hubY = HUB_TARGET.getY(); + + CLOSE_POSE = new Pose2d(hubX - 1.5, hubY, Rotation2d.kZero); + MID_POSE = new Pose2d(hubX - 3.9, hubY + 1.0, Rotation2d.fromDegrees(15)); + FAR_POSE = new Pose2d(hubX - 5.7, hubY - 1.5, Rotation2d.fromDegrees(-20)); + } + + // ================================================================ + // ShotCalculator: simple unit conversion functions + // ================================================================ + + @Nested + class LinearAngularConversions { + @Test + void linearToAngular_basicConversion() { + // v = 10 m/s, r = 0.5 m -> omega = v/r = 20 rad/s + var result = ShotCalculator.linearToAngularVelocity( + MetersPerSecond.of(10.0), Meters.of(0.5)); + assertEquals(20.0, result.in(RadiansPerSecond), 1e-9); + } + + @Test + void angularToLinear_basicConversion() { + // omega = 20 rad/s, r = 0.5 m -> v = omega*r = 10 m/s + var result = ShotCalculator.angularToLinearVelocity( + RadiansPerSecond.of(20.0), Meters.of(0.5)); + assertEquals(10.0, result.in(MetersPerSecond), 1e-9); + } + + @Test + void roundTrip_linearToAngularAndBack() { + LinearVelocity original = MetersPerSecond.of(7.3); + Distance radius = Inches.of(1.5); // flywheel radius + AngularVelocity angular = ShotCalculator.linearToAngularVelocity(original, radius); + LinearVelocity recovered = ShotCalculator.angularToLinearVelocity(angular, radius); + assertEquals(original.in(MetersPerSecond), recovered.in(MetersPerSecond), 1e-9); + } + + @Test + void zeroVelocity() { + var result = ShotCalculator.linearToAngularVelocity( + MetersPerSecond.of(0), Meters.of(0.5)); + assertEquals(0.0, result.in(RadiansPerSecond), 1e-9); + } + } + + // ================================================================ + // ShotCalculator.getDistanceToTarget + // ================================================================ + + @Nested + class GetDistanceToTarget { + @Test + void closePose_returnsPositiveDistance() { + Distance d = ShotCalculator.getDistanceToTarget(CLOSE_POSE, HUB_TARGET); + assertTrue(d.in(Meters) > 0, "Distance should be positive"); + // Close pose is ~1.5m from hub, but turret transform offsets it + assertTrue(d.in(Meters) < 3.0, "Close pose should be within 3m"); + } + + @Test + void midPose_returnsReasonableDistance() { + Distance d = ShotCalculator.getDistanceToTarget(MID_POSE, HUB_TARGET); + assertTrue(d.in(Meters) > 2.0); + assertTrue(d.in(Meters) < 6.0); + } + + @Test + void farPose_returnsLargerDistance() { + Distance dClose = ShotCalculator.getDistanceToTarget(CLOSE_POSE, HUB_TARGET); + Distance dFar = ShotCalculator.getDistanceToTarget(FAR_POSE, HUB_TARGET); + assertTrue(dFar.in(Meters) > dClose.in(Meters), + "Far pose should be further than close pose"); + } + + @Test + void samePosition_returnsSmallDistance() { + // Robot right at the hub X,Y — distance should just be the turret transform offset + Pose2d atHub = new Pose2d(HUB_TARGET.getX(), HUB_TARGET.getY(), Rotation2d.kZero); + Distance d = ShotCalculator.getDistanceToTarget(atHub, HUB_TARGET); + // Should be small but nonzero due to turret offset + assertTrue(d.in(Meters) < 1.0); + assertTrue(d.in(Meters) >= 0); + } + + @Test + void isConsistent_withDifferentHeadings() { + // Robot heading shouldn't drastically change the 2D distance + // (turret transform rotates with robot, so there IS some effect) + Pose2d heading0 = new Pose2d(2.0, 4.0, Rotation2d.kZero); + Pose2d heading90 = new Pose2d(2.0, 4.0, Rotation2d.kCCW_90deg); + Distance d0 = ShotCalculator.getDistanceToTarget(heading0, HUB_TARGET); + Distance d90 = ShotCalculator.getDistanceToTarget(heading90, HUB_TARGET); + // Both should be in a similar ballpark (within turret offset range ~0.17m) + assertEquals(d0.in(Meters), d90.in(Meters), 0.5); + } + } + + // ================================================================ + // ShotCalculator.calculateTimeOfFlight + // ================================================================ + + @Nested + class CalculateTimeOfFlight { + @Test + void basicTimeOfFlight() { + // v = 10 m/s at 45 deg hood -> horizontal = 10*cos(45deg) ≈ 7.07 m/s + // distance = 3m -> tof = 3/7.07 ≈ 0.424 s + // Note: hood angle 0 = horizontal in this function (pi/2 - hoodAngle) + // Actually from code: angle = PI/2 - hoodAngle.in(Radians) + // So hoodAngle of 45 deg -> effective angle = 45 deg -> cos(45deg) component + Time tof = ShotCalculator.calculateTimeOfFlight( + MetersPerSecond.of(10.0), + Degrees.of(45), + Meters.of(3.0)); + double tofSec = tof.in(Seconds); + assertFalse(Double.isNaN(tofSec), "TOF should not be NaN"); + assertTrue(tofSec > 0, "TOF should be positive"); + } + + @Test + void longerDistance_longerTOF() { + Time tofShort = ShotCalculator.calculateTimeOfFlight( + MetersPerSecond.of(10.0), Degrees.of(30), Meters.of(2.0)); + Time tofLong = ShotCalculator.calculateTimeOfFlight( + MetersPerSecond.of(10.0), Degrees.of(30), Meters.of(5.0)); + assertTrue(tofLong.in(Seconds) > tofShort.in(Seconds)); + } + + @Test + void fasterVelocity_shorterTOF() { + Time tofSlow = ShotCalculator.calculateTimeOfFlight( + MetersPerSecond.of(5.0), Degrees.of(30), Meters.of(3.0)); + Time tofFast = ShotCalculator.calculateTimeOfFlight( + MetersPerSecond.of(15.0), Degrees.of(30), Meters.of(3.0)); + assertTrue(tofFast.in(Seconds) < tofSlow.in(Seconds)); + } + } + + // ================================================================ + // ShotCalculator.predictTargetPos + // ================================================================ + + @Nested + class PredictTargetPos { + @Test + void stationaryRobot_targetUnchanged() { + Translation3d target = new Translation3d(5, 4, 1.5); + Translation3d predicted = ShotCalculator.predictTargetPos( + target, ZERO_SPEEDS, Seconds.of(1.0)); + assertEquals(target.getX(), predicted.getX(), 1e-9); + assertEquals(target.getY(), predicted.getY(), 1e-9); + assertEquals(target.getZ(), predicted.getZ(), 1e-9); + } + + @Test + void movingRobot_targetShiftsOppositeToVelocity() { + Translation3d target = new Translation3d(5, 4, 1.5); + ChassisSpeeds speeds = new ChassisSpeeds(2.0, 0, 0); // moving +x at 2 m/s + Translation3d predicted = ShotCalculator.predictTargetPos( + target, speeds, Seconds.of(1.0)); + + // predicted.x = target.x - vx * tof = 5 - 2*1 = 3 + assertEquals(3.0, predicted.getX(), 1e-9); + assertEquals(4.0, predicted.getY(), 1e-9); + assertEquals(1.5, predicted.getZ(), 1e-9); // Z unchanged + } + + @Test + void zeroTOF_targetUnchanged() { + Translation3d target = new Translation3d(5, 4, 1.5); + Translation3d predicted = ShotCalculator.predictTargetPos( + target, TRANSLATING_SPEEDS, Seconds.of(0)); + assertEquals(target.getX(), predicted.getX(), 1e-9); + assertEquals(target.getY(), predicted.getY(), 1e-9); + } + + @Test + void movingInY_shiftsY() { + Translation3d target = new Translation3d(5, 4, 1.5); + ChassisSpeeds speeds = new ChassisSpeeds(0, 3.0, 0); // moving +y at 3 m/s + Translation3d predicted = ShotCalculator.predictTargetPos( + target, speeds, Seconds.of(0.5)); + assertEquals(5.0, predicted.getX(), 1e-9); + assertEquals(4.0 - 3.0 * 0.5, predicted.getY(), 1e-9); // 4 - 1.5 = 2.5 + } + } + + // ================================================================ + // ShotCalculator.iterativeMovingShotFromInterpolationMap + // ================================================================ + + @Nested + class IterativeMovingShotFromInterpolationMap { + @Test + void stationaryClose_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + CLOSE_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void stationaryMid_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void stationaryFar_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + FAR_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void movingRobot_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void rotatingRobot_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, ROTATING_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void stationaryShot_targetMatchesInput() { + ShotData shot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + // With zero speeds, predicted target should equal actual target + assertEquals(HUB_TARGET.getX(), shot.getTarget().getX(), 1e-6); + assertEquals(HUB_TARGET.getY(), shot.getTarget().getY(), 1e-6); + assertEquals(HUB_TARGET.getZ(), shot.getTarget().getZ(), 1e-6); + } + + @Test + void movingShot_targetDiffersFromStatic() { + ShotData staticShot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + ShotData movingShot = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); + // Moving shot should shift the predicted target + assertNotEquals(staticShot.getTarget().getX(), movingShot.getTarget().getX(), 1e-6, + "Moving shot predicted target X should differ from static"); + } + + @Test + void moreIterations_convergesToSameResult() { + ShotData shot1 = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 1); + ShotData shot3 = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); + ShotData shot10 = ShotCalculator.iterativeMovingShotFromInterpolationMap( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 10); + // 3 and 10 iterations should converge closely + assertEquals(shot3.exitVelocity(), shot10.exitVelocity(), 0.5, + "3 and 10 iterations should converge on exit velocity"); + assertEquals(shot3.hoodAngle(), shot10.hoodAngle(), 0.1, + "3 and 10 iterations should converge on hood angle"); + } + } + + // ================================================================ + // ShotCalculator.calculateShotFromFunnelClearance + // ================================================================ + + @Nested + class CalculateShotFromFunnelClearance { + @Test + void stationaryClose_returnsValidShot() { + ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( + CLOSE_POSE, HUB_TARGET, HUB_TARGET); + assertShotDataValid(shot); + } + + @Test + void stationaryMid_returnsValidShot() { + ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( + MID_POSE, HUB_TARGET, HUB_TARGET); + assertShotDataValid(shot); + } + + @Test + void stationaryFar_returnsValidShot() { + ShotData shot = ShotCalculator.calculateShotFromFunnelClearance( + FAR_POSE, HUB_TARGET, HUB_TARGET); + assertShotDataValid(shot); + } + } + + // ================================================================ + // ShotCalculator.iterativeMovingShotFromFunnelClearance + // ================================================================ + + @Nested + class IterativeMovingShotFromFunnelClearance { + @Test + void stationaryMid_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromFunnelClearance( + MID_POSE, ZERO_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + + @Test + void movingRobot_returnsValidShot() { + ShotData shot = ShotCalculator.iterativeMovingShotFromFunnelClearance( + MID_POSE, TRANSLATING_SPEEDS, HUB_TARGET, 3); + assertShotDataValid(shot); + } + } + + // ================================================================ + // ShooterCalc.calcAzimuth (static method) + // ================================================================ + + @Nested + class CalcAzimuth { + @Test + void turretAtZero_closeTarget_returnsValidAzimuth() { + AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + HUB_TARGET, CLOSE_POSE, 0.0, ZERO_SPEEDS); + assertAzimuthValid(details); + } + + @Test + void turretAtZero_midTarget_returnsValidAzimuth() { + AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); + assertAzimuthValid(details); + } + + @Test + void turretAtPositiveQuarter_returnsValidAzimuth() { + AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, 0.25, ZERO_SPEEDS); + assertAzimuthValid(details); + } + + @Test + void turretAtNegativeQuarter_returnsValidAzimuth() { + AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, -0.25, ZERO_SPEEDS); + assertAzimuthValid(details); + } + + @Test + void movingRobot_producesVelocityFF() { + AzimuthCalcDetails stationary = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); + AzimuthCalcDetails moving = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, 0.0, TRANSLATING_SPEEDS); + // Moving robot should produce nonzero velocity feedforward + assertNotEquals(0.0, moving.turretVelocityFF(), 1e-6, + "Moving robot should produce nonzero turret velocity FF"); + } + + @Test + void stationaryRobot_zeroVelocityFF() { + AzimuthCalcDetails details = ShooterCalc.calcAzimuth( + HUB_TARGET, MID_POSE, 0.0, ZERO_SPEEDS); + assertEquals(0.0, details.turretVelocityFF(), 1e-6, + "Stationary robot should have zero velocity FF (omega=0, v=0)"); + } + + @Test + void turretReference_withinPhysicalLimits() { + // Test across many poses and turret positions + Pose2d[] poses = {CLOSE_POSE, MID_POSE, FAR_POSE}; + double[] turretRots = {-0.5, -0.25, 0, 0.25, 0.5}; + for (Pose2d pose : poses) { + for (double rot : turretRots) { + AzimuthCalcDetails d = ShooterCalc.calcAzimuth( + HUB_TARGET, pose, rot, ZERO_SPEEDS); + double refRots = d.turretReferenceRots(); + assertTrue(refRots >= -0.76 && refRots <= 0.76, + String.format("Turret reference %.4f out of [-0.75, 0.75] for pose=%s, turret=%.2f", + refRots, pose, rot)); + } + } + } + } + + // ================================================================ + // ShooterCalc.calcShot (static method, full pipeline) + // ================================================================ + + @Nested + class CalcShot { + @Test + void staticShot_close_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void staticShot_mid_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void staticShot_far_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void dynamicShot_translating_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + MID_POSE, false, HUB_TARGET, 0.0, TRANSLATING_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void dynamicShot_rotating_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + MID_POSE, false, HUB_TARGET, 0.0, ROTATING_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void dynamicShot_combined_returnsValidOutputs() { + ShotCalcOutputs out = ShooterCalc.calcShot( + MID_POSE, false, HUB_TARGET, 0.1, COMBINED_SPEEDS); + assertShotCalcOutputsValid(out); + } + + @Test + void staticVsDynamic_sameWhenStationary() { + ShotCalcOutputs staticOut = ShooterCalc.calcShot( + MID_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + ShotCalcOutputs dynamicOut = ShooterCalc.calcShot( + MID_POSE, false, HUB_TARGET, 0.0, ZERO_SPEEDS); + // With zero chassis speeds, static and dynamic should produce identical results + assertEquals( + staticOut.shooterReferenceRps(), + dynamicOut.shooterReferenceRps(), + 1e-6, "Zero-speed dynamic should match static shot"); + assertEquals( + staticOut.hoodReferenceRots(), + dynamicOut.hoodReferenceRots(), + 1e-6); + assertEquals( + staticOut.turretReferenceRots(), + dynamicOut.turretReferenceRots(), + 1e-6); + } + + @Test + void farShot_higherVelocityThanClose() { + ShotCalcOutputs closeOut = ShooterCalc.calcShot( + CLOSE_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + ShotCalcOutputs farOut = ShooterCalc.calcShot( + FAR_POSE, true, HUB_TARGET, 0.0, ZERO_SPEEDS); + assertTrue( + farOut.shooterReferenceRps() > + closeOut.shooterReferenceRps(), + "Far shot should require higher flywheel velocity"); + } + } + + // ================================================================ + // ShotData record tests + // ================================================================ + + @Nested + class ShotDataTests { + @Test + void interpolate_midpoint() { + ShotData a = new ShotData(10.0, 1.0); + ShotData b = new ShotData(20.0, 2.0); + ShotData mid = ShotData.interpolate(a, b, 0.5); + assertEquals(15.0, mid.exitVelocity(), 1e-9); + assertEquals(1.5, mid.hoodAngle(), 1e-9); + } + + @Test + void interpolate_atStart() { + ShotData a = new ShotData(10.0, 1.0); + ShotData b = new ShotData(20.0, 2.0); + ShotData result = ShotData.interpolate(a, b, 0.0); + assertEquals(10.0, result.exitVelocity(), 1e-9); + assertEquals(1.0, result.hoodAngle(), 1e-9); + } + + @Test + void interpolate_atEnd() { + ShotData a = new ShotData(10.0, 1.0); + ShotData b = new ShotData(20.0, 2.0); + ShotData result = ShotData.interpolate(a, b, 1.0); + assertEquals(20.0, result.exitVelocity(), 1e-9); + assertEquals(2.0, result.hoodAngle(), 1e-9); + } + + @Test + void minus_computesDifference() { + ShotData a = new ShotData(15.0, 1.5); + ShotData b = new ShotData(10.0, 1.0); + ShotData diff = a.minus(b); + assertEquals(10.0 - 15.0, diff.exitVelocity(), 1e-9); + assertEquals(1.0 - 1.5, diff.hoodAngle(), 1e-9); + } + + @Test + void getExitVelocity_convertsFromRadPerSec() { + ShotData data = new ShotData( + RotationsPerSecond.of(50), Degrees.of(45)); + LinearVelocity vel = data.getExitVelocity(); + assertFalse(Double.isNaN(vel.in(MetersPerSecond))); + assertTrue(vel.in(MetersPerSecond) > 0); + } + + @Test + void getHoodAngle_convertsFromRadians() { + ShotData data = new ShotData( + RotationsPerSecond.of(50), Degrees.of(45)); + Angle angle = data.getHoodAngle(); + assertEquals(45.0, angle.in(Degrees), 0.01); + } + } + + // ================================================================ + // Assertion helpers + // ================================================================ + + private static void assertShotDataValid(ShotData shot) { + assertNotNull(shot, "ShotData should not be null"); + assertFalse(Double.isNaN(shot.exitVelocity()), + "Exit velocity should not be NaN"); + assertFalse(Double.isNaN(shot.hoodAngle()), + "Hood angle should not be NaN"); + assertFalse(Double.isInfinite(shot.exitVelocity()), + "Exit velocity should not be infinite"); + assertFalse(Double.isInfinite(shot.hoodAngle()), + "Hood angle should not be infinite"); + assertNotNull(shot.getTarget(), "Shot target should not be null"); + } + + private static void assertAzimuthValid(AzimuthCalcDetails details) { + assertNotNull(details, "AzimuthCalcDetails should not be null"); + assertFalse(Double.isNaN(details.turretReferenceRots()), + "Turret reference should not be NaN"); + assertFalse(Double.isNaN(details.rawDesiredRotations()), + "Raw desired rotations should not be NaN"); + assertFalse(Double.isNaN(details.turretVelocityFF()), + "Turret velocity FF should not be NaN"); + } + + private static void assertShotCalcOutputsValid(ShotCalcOutputs out) { + assertNotNull(out, "ShotCalcOutputs should not be null"); + assertAzimuthValid(out.turretCalcDetails()); + assertNotNull(out.shotData(), "ShotData should not be null"); + assertShotDataValid(out.shotData()); + assertFalse(Double.isNaN(out.turretReferenceRots()), + "Turret reference should not be NaN"); + assertFalse(Double.isNaN(out.hoodReferenceRots()), + "Hood reference should not be NaN"); + assertFalse(Double.isNaN(out.shooterReferenceRps()), + "Shooter reference should not be NaN"); + assertTrue(out.shooterReferenceRps() >= 0, + "Shooter velocity should be non-negative"); + } +} From ca5adf3eb6dc3ba0a682037c8a3d907863441535 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 10:15:08 -0400 Subject: [PATCH 14/40] fixing magic numbers --- src/main/java/frc/robot/Constants.java | 5 +++++ src/main/java/frc/robot/Robot.java | 6 ++++-- .../robot/dashboards/TestingDashboard.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 19 +++++++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 2 +- 5 files changed, 30 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a913b61f..b43c494d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,6 +119,7 @@ public static class ShooterK { public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.75); //0.75 rots in each direction from home public static final Angle kTurretMinRots = Rotations.of(-kTurretMaxRotsFromHome.in(Rotations)); public static final Angle kTurretMaxRots = Rotations.of(kTurretMaxRotsFromHome.in(Rotations)); + public static final Angle kTurretIntakeLockPos = Rotations.of(-0.250); public static final AngularVelocity kShooterMaxRPS = MotorK.kX60MaxVelocity.div(kShooterGearing); public static final double kShooterMaxRPSd = 96.42; @@ -241,6 +242,9 @@ public static class ShooterK { private static final VoltageConfigs kHoodVoltageConfigs = new VoltageConfigs() .withPeakForwardVoltage(16) .withPeakReverseVoltage(-16); + private static final SoftwareLimitSwitchConfigs kHoodSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(kHoodMaxRots_double); private static final CommutationConfigs kHoodCommutationConfigs = new CommutationConfigs() .withAdvancedHallSupport(AdvancedHallSupportValue.Enabled) .withMotorArrangement(MotorArrangementValue.NEO550_JST); @@ -252,6 +256,7 @@ public static class ShooterK { .withMotorOutput(kHoodOutputConfigs) .withExternalFeedback(kHoodFeedbackConfigs) .withVoltage(kHoodVoltageConfigs) + .withSoftwareLimitSwitch(kHoodSoftwareLimitSwitchConfigs) .withCommutation(kHoodCommutationConfigs); //---TURRET diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 98496cca..8dc55fc1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -478,13 +478,15 @@ public void teleopPeriodic() {} @Override public void teleopExit() {} + private final Trigger trg_letOpsCheckBeLong = new Trigger(() -> true); + @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); - TestingDashboard.initialize(); + // TestingDashboard.initialize(); - TestingDashboard.trg_letOpsCheckBeLong + trg_letOpsCheckBeLong .onTrue(m_superstructure.longOpsCheck()) .onFalse(m_superstructure.shortOpsCheck()); diff --git a/src/main/java/frc/robot/dashboards/TestingDashboard.java b/src/main/java/frc/robot/dashboards/TestingDashboard.java index 6deabc1c..ff9cc028 100644 --- a/src/main/java/frc/robot/dashboards/TestingDashboard.java +++ b/src/main/java/frc/robot/dashboards/TestingDashboard.java @@ -108,7 +108,7 @@ public class TestingDashboard { // public static Trigger trg_letIntakeArmPositionRotsChange; // public static Trigger trg_letIntakeRollersVelocityRPSChange; - public static Trigger trg_letOpsCheckBeLong; + // public static Trigger trg_letOpsCheckBeLong; public static void initialize() { //---SHOOTER diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index c94ca403..ca2adf93 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IndexerK; +import frc.robot.Constants.ShooterK; import frc.robot.Constants.SuperstructureK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; @@ -196,32 +197,50 @@ public Command longOpsCheck() { Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //deploy intake + Commands.print("================================= DEPLOY INTAKE ================================="), m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run intake rollers + Commands.print("================================= RUN INTAKE ROLLERS ================================="), m_intake.startIntakeRollers(), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run spindexer + Commands.print("================================= RUN SPINDEXER ================================="), m_intake.stopIntakeRollers(), m_indexer.setSpindexerVelocityCmd(IndexerK.kSpindexerShootRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //Stop Spindexer; run tunnel + Commands.print("================================= RUN TUNNEL ================================="), m_indexer.stopSpindexerCmd(), m_indexer.setTunnelVelocityCmd(IndexerK.kTunnelShootRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //Stop tunnel; run shooter + Commands.print("================================= RUN SHOOTER ================================="), m_indexer.stopTunnelCmd(), m_shooter.setShooterVelocityCmd(ShooterK.kShooterRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //shooter stopped + Commands.print("================================= STOP SHOOTER ================================="), m_shooter.setShooterVelocityCmd(RotationsPerSecond.zero()), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + //turret move + Commands.print("================================= TURRET MOVE ================================="), + m_shooter.m_turret.setTurretPosCmd(ShooterK.kTurretIntakeLockPos), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + m_shooter.m_turret.setTurretPosCmd(ShooterK.kHomePosition), + + //hood move + Commands.print("================================= HOOD MOVE ================================="), + m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodLockRots_double), + Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + m_shooter.m_hood.setHoodPosCmd(0.1), + //Run short Ops check (intake, shoot, swerve) shortOpsCheck() ); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5241de09..b42fb0e2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -312,7 +312,7 @@ public void periodic() { m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { - m_turret.setTurretPos(Rotations.of(-0.250)); + m_turret.setTurretPos(kTurretIntakeLockPos); } else { m_turret.setTurretPos(turretReference, turretVelocityFF); m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); From 9cd205f60074be8761a84e29f91b5a0aed8d9436 Mon Sep 17 00:00:00 2001 From: alexandra Date: Sat, 28 Mar 2026 11:58:19 -0400 Subject: [PATCH 15/40] ops check works except for the hood position not moving properly? --- src/main/java/frc/robot/Robot.java | 13 ++++++++----- .../java/frc/robot/subsystems/Superstructure.java | 11 ++++++++++- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8dc55fc1..f2b0fffa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -478,17 +478,20 @@ public void teleopPeriodic() {} @Override public void teleopExit() {} - private final Trigger trg_letOpsCheckBeLong = new Trigger(() -> true); - @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); // TestingDashboard.initialize(); + Command opsCheckCommand = m_superstructure.m_isLongOpsCheck ? m_superstructure.longOpsCheck() : m_superstructure.shortOpsCheck(); + + CommandScheduler.getInstance().schedule( + Commands.sequence( + Commands.print("==========START LONG OPS CHECK=========="), + opsCheckCommand + )); - trg_letOpsCheckBeLong - .onTrue(m_superstructure.longOpsCheck()) - .onFalse(m_superstructure.shortOpsCheck()); + // .whileFalse(m_superstructure.shortOpsCheck()); // CommandScheduler.getInstance().schedule( // Commands.sequence( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ca2adf93..ec9e7e3e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IndexerK; import frc.robot.Constants.ShooterK; import frc.robot.Constants.SuperstructureK; @@ -21,6 +22,8 @@ public class Superstructure extends SubsystemBase { private final Indexer m_indexer; private final Shooter m_shooter; private final Swerve m_drivetrain; + + public boolean m_isLongOpsCheck = true; /* CONSTRUCTOR */ public Superstructure(Intake intake, Indexer indexer, Shooter shooter, Swerve drivetrain) { @@ -238,10 +241,13 @@ public Command longOpsCheck() { //hood move Commands.print("================================= HOOD MOVE ================================="), m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodLockRots_double), + Commands.print("================================= LOCK HOOD POS ================================="), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), + Commands.print("================================= SET HOOD POS TO MIN ================================="), m_shooter.m_hood.setHoodPosCmd(0.1), //Run short Ops check (intake, shoot, swerve) + Commands.print("================================= START SHORT OPS CHECK ================================="), shortOpsCheck() ); } @@ -257,14 +263,17 @@ public Command longOpsCheck() { public Command shortOpsCheck() { return Commands.sequence( //runs intaking cmd + Commands.print("================================= ACTIVATE INTAKE CMD ================================="), intake(() -> false).withTimeout(SuperstructureK.kShortOpsCheckIntakeTime), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //runs shooting cmd - activateOuttakeShotCalc(), + Commands.print("================================= ACTIVATE OUTTAKE CMD ================================="), + activateOuttakeShotCalc().withTimeout(SuperstructureK.kShortOpsCheckShooterTime), Commands.waitSeconds(SuperstructureK.kShortOpsCheckPause), //Swerve test + Commands.print("================================= START SWERVE OPSP CHECK ================================="), m_drivetrain.swerveAutomatedOpsCheck() ); } From 255c26af6bfe68ac5ce8ab930a98d61798e5ae24 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 12:58:40 -0400 Subject: [PATCH 16/40] new shooter configs --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cbd29164..e0101906 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -186,10 +186,10 @@ public static class ShooterK { /* CONFIGS */ // TODO: Check what more configs would be necessary private static final Slot0Configs kShooterASlot0Configs = new Slot0Configs() //Note to self (hrehaan) (and saarth cuz i did the same thing): the default PID sets ZERO volts to a motor, which makes all sim effectively useless cuz the motor has ZERO supplyV - .withKS(0) - .withKV(0.12) + .withKS(0.2998046875) + .withKV(0.09) .withKA(0) - .withKP(0.5) + .withKP(0.3) .withKI(0) .withKD(0); // kP was causing the werid sinusoid behavior, kS and kA were adding inconsistency with the destination values private static final CurrentLimitsConfigs kShooterACurrentLimitConfigs = new CurrentLimitsConfigs() From c69a532efe8944958447291ac78b6b5456fde3c0 Mon Sep 17 00:00:00 2001 From: Saarth Date: Sat, 28 Mar 2026 13:37:35 -0400 Subject: [PATCH 17/40] fixed and added swerve methods --- .../java/frc/robot/subsystems/Swerve.java | 72 +++++++++++++++---- 1 file changed, 58 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index beda904b..86f994d1 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; @@ -400,22 +401,65 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) } /** - * robot goes to specified pose - * @param destination - * @return + * @param desPose Posd2d to move to + * @return a Command that makes the robot move to the desired Pose2d */ - public Command toPose(Pose2d destination) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + public Command roboToPose(Pose2d desPose, double tolerance) { + return Commands.runOnce(() -> { + Pose2d curPose = getState().Pose; + double xSpeed = m_pathXController.calculate(curPose.getX(), desPose.getX()); + double ySpeed = m_pathYController.calculate(curPose.getY(), desPose.getY()); + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + } - double xSpeed = m_pathXController.calculate(curPose.getX(), destination.getX()); - double ySpeed = m_pathYController.calculate(curPose.getY(), destination.getY()); - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destination.getRotation().getRadians()); + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) + && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + } - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - } - ); + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); + } + + /** + * @param desRotation Rotation2d to turn to + * @return a Command that makes the robot turn to the desired Rotation2d + */ + public Command roboToRotation(Rotation2d desRotation, double tolerance) { + return Commands.runOnce(() -> { + Rotation2d curRotation = getState().Pose.getRotation(); + double thetaSpeed = m_pathThetaController.calculate(curRotation.getRadians(), desRotation.getRadians()); + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); + } + + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + } + + /** + * @param desTranslation Translation2d to go to + * @return a Command that makes the robot go to the desired Translation2d + */ + public Command roboToTranslation(Translation2d desTranslation, double tolerance) { + return Commands.runOnce(() -> { + Translation2d curTranslation = getState().Pose.getTranslation(); + double xSpeed = m_pathXController.calculate(curTranslation.getX(), desTranslation.getX()); + double ySpeed = m_pathYController.calculate(curTranslation.getY(), desTranslation.getY()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + } + + public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) { + return Math.hypot( + desTranslation.getMeasureX().minus(curTranslation.getMeasureX()).baseUnitMagnitude(), + desTranslation.getMeasureY().minus(curTranslation.getMeasureY()).baseUnitMagnitude() + ) <= tolerance; } /** @@ -426,7 +470,7 @@ public Command swerveToObject() { Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - return toPose(destination); + return roboToPose(destination, 0.1); } public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { From a33c5256db65ba3100214c5700b2d0b14bc9d64b Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 17:20:32 -0400 Subject: [PATCH 18/40] NEW SHOOTER RAHHHH --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/Robot.java | 2 +- .../robot/autons/WaltSimpleAutonFactory.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 20 ++++++------------- 4 files changed, 12 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0101906..ca94b6d4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -122,9 +122,9 @@ public static class ShooterK { public static final double kTurretMaxErrD = Rotations.of(0.05).in(Rotations); - public static final AngularVelocity kShooterMaxRPS = MotorK.kX60MaxVelocity.div(kShooterGearing); - public static final double kShooterMaxRPSd = 96.42; - public static final AngularVelocity kShooterRPS = kShooterMaxRPS.times(0.65); //Kraken X60Foc Max (RPM: 5785) //(0.9) + public static final AngularVelocity kShooterMaxRPS = MotorK.kX44MaxVelocity.div(kShooterGearing); + public static final double kShooterMaxRPSd = kShooterMaxRPS.in(RotationsPerSecond); + public static final AngularVelocity kShooterRPS = kShooterMaxRPS.times(0.65); //Kraken X44 Max RPM: 7758 public static final double kShooterRPSd = kShooterMaxRPSd * 0.65; public static final AngularVelocity kShooterAutonCloseRPS = kShooterMaxRPS.times(0.60); //auton pose is closer to the hub than teleop scoring public static final AngularVelocity kShooterAuton_EndSweep_RPS = kShooterMaxRPS.times(0.70); // end of sweep paths @@ -198,7 +198,7 @@ public static class ShooterK { .withSupplyCurrentLowerLimit(20) .withStatorCurrentLimitEnable(true); private static final MotorOutputConfigs kShooterAOutputConfigs = new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) + .withInverted(InvertedValue.CounterClockwise_Positive ) .withNeutralMode(NeutralModeValue.Coast); private static final FeedbackConfigs kShooterAFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kShooterGearing); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 35ee4595..8bc936e9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -274,7 +274,7 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + trg_shoot/*.and(() -> m_shooter.m_turret.atPosition()).*/.whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); diff --git a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index 254d64e4..08f72843 100644 --- a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java @@ -55,7 +55,7 @@ private Command tp(String message) { } private Command waitTurretHomedCmd() { - return Commands.waitUntil(m_shooter.turretHomedSupp); + return Commands.waitUntil(() -> true); } private Command waitIntakeHomedCmd() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1e534574..ee1d78a0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -63,8 +63,8 @@ public class Shooter extends SubsystemBase { // private final FuelSim m_fuelSim; // ---MOTORS + CONTROL REQUESTS - private final TalonFX m_shooterA = new TalonFX(kShooterA_CANID, Constants.kShooterBus); // X60Foc - private final TalonFX m_shooterB = new TalonFX(kShooterB_CANID, Constants.kShooterBus); // X60Foc + private final TalonFX m_shooterA = new TalonFX(kShooterA_CANID, Constants.kShooterBus); // X44 + private final TalonFX m_shooterB = new TalonFX(kShooterB_CANID, Constants.kShooterBus); // X44 private final VelocityVoltage m_velocityRequest = new VelocityVoltage(0).withEnableFOC(false); private final NeutralOut m_neutralOutReq = new NeutralOut(); @@ -76,7 +76,6 @@ public class Shooter extends SubsystemBase { // thread copde private double m_latestTurretPositionRots = 0.0; private final ShooterCalc m_shooterCalc; - private final BooleanLogger log_turretHomingHall = new BooleanLogger(kLogTab, "turretHomeHall"); private double m_calcTurretRots = 0.0; private double m_calcFlywheelVelocityRotPerSec = kShooterRPSd; @@ -86,16 +85,9 @@ public class Shooter extends SubsystemBase { private final DoubleLogger log_calcTurretPos = new DoubleLogger("Shooter/Turret", "calcTurretPos"); private final DoubleLogger log_driverAddedRPS = WaltLogger.logDouble(kLogTab, "driverAddedRPS"); - private final BooleanLogger log_turretHomed = WaltLogger.logBoolean("Shooter/Turret", "Homed"); - - // ---LOGIC BOOLEANS - private boolean m_isTurretHomed = false; - public BooleanSupplier turretHomedSupp = () -> m_isTurretHomed; - // private boolean m_isHoodHomed = false; - /* SIM OBJECTS */ private final FlywheelSim m_shooterSim = new FlywheelSim(LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60Foc(2), kShooterMoI, kShooterGearing), DCMotor.getKrakenX60Foc(2) // returns gearbox + DCMotor.getKrakenX44(2), kShooterMoI, kShooterGearing), DCMotor.getKrakenX60Foc(2) // returns gearbox ); private final DCMotorSim m_turretSim = new DCMotorSim(LinearSystemId.createDCMotorSystem( @@ -310,13 +302,13 @@ public void periodic() { // set outputs var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { - m_turret.setTurretPos(m_turret.getTurretLockAngle()); + // m_turret.setTurretPos(m_turret.getTurretLockAngle()); m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { - m_turret.setTurretPos(Rotations.of(-0.250)); + // m_turret.setTurretPos(Rotations.of(-0.250)); } else { - m_turret.setTurretPos(turretReference, turretVelocityFF); + // m_turret.setTurretPos(turretReference, turretVelocityFF); m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); if (true) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; From ae428a4fbb19cbf309ca18c6a490c867d3bedc58 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 17:31:57 -0400 Subject: [PATCH 19/40] PREVIOUS COMMIT IS THE NEW LCM VALUES --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8d179089..f127552b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -570,7 +570,7 @@ public static class TurretK { public static final double kGearTwoToothCount = 19; public static final double kLCMAtHomeRots = 0.0; // measure: turretLCMPos log value when turret is at home - public static final double kEncBOffset = 0.610415; // measure: encB reading when turret is at encA=0 + public static final double kEncBOffset = 0.610415; // measure: encB reading when turret is at encA=0 //NEW ONE } public static class AutonK { public static final String kLogTab = "Auton"; From 1e75cbb82bf54100ffec2553d00050b751fa6fb6 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 19:43:49 -0400 Subject: [PATCH 20/40] turret working --- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/subsystems/shooter/Shooter.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f127552b..b899f331 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -71,7 +71,7 @@ public static class WpiK { } public static class ShooterK { public static final String kLogTab = "Shooter"; - private static final Rotation2d kTurretAngleOffset = Rotation2d.fromDegrees(-135); + private static final Rotation2d kTurretAngleOffset = Rotation2d.fromDegrees(180 + -135); public static final Transform3d kTurretTransform = new Transform3d(new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)), new Rotation3d(kTurretAngleOffset)); //DUMMY VALS public static final Distance kInchesAboveFunnel = Inches.of(20);// distance the ball must travel above the funnel opening to arc correctly into the hub @@ -116,7 +116,7 @@ public static class ShooterK { public static final int kPeakShooterVolts = 16; - public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.75); //0.75 rots in each direction from home + public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.60 ); //0.75 rots in each direction from home public static final Angle kTurretMinRots = Rotations.of(-kTurretMaxRotsFromHome.in(Rotations)); public static final Angle kTurretMaxRots = Rotations.of(kTurretMaxRotsFromHome.in(Rotations)); public static final double kTurretMaxErrD = Rotations.of(0.05).in(Rotations); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ee1d78a0..5eb2cf86 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -302,13 +302,13 @@ public void periodic() { // set outputs var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { - // m_turret.setTurretPos(m_turret.getTurretLockAngle()); + m_turret.setTurretPos(m_turret.getTurretLockAngle()); m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { - // m_turret.setTurretPos(Rotations.of(-0.250)); + m_turret.setTurretPos(Rotations.of(-0.250)); } else { - // m_turret.setTurretPos(turretReference, turretVelocityFF); + m_turret.setTurretPos(turretReference, turretVelocityFF); m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); if (true) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; @@ -322,10 +322,10 @@ public void periodic() { double hoodReference = calcData.hoodReferenceRots(); if (m_turret.getTurretLocked()) { - m_hood.setHoodPos(kHoodLockRots_double); + // m_hood.setHoodPos(kHoodLockRots_double); } else { if (!m_turret.getHoldTurretAtIntake()) { - m_hood.setHoodPos(hoodReference); + // m_hood.setHoodPos(hoodReference); } } } From 8669aadad0d7fa3d18e0ed0754abcabe74b25595 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 21:13:42 -0400 Subject: [PATCH 21/40] we in the hood now --- src/main/java/frc/robot/Constants.java | 33 +++++++++++++----- src/main/java/frc/robot/Robot.java | 34 +++++++------------ .../frc/robot/subsystems/shooter/Hood.java | 22 ++++++------ 3 files changed, 48 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b899f331..850a0089 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -140,15 +140,15 @@ public static class ShooterK { //---HOOD CONSTANTS public static final double kHoodMoI = 0.00027505; - public static final Angle kHoodMinPosition = Rotations.of(0.1); - private static final Angle kHoodMaxRots = Rotations.of(1.82195); //apparently this is 26 or 27 degrees? idk thats where we're telling it to go sooo.. - public static final Angle kHoodMaxDegs = Degrees.of(kHoodMaxRots.in(Degrees)); + public static final Angle kHoodAbsoluteMinRots = Rotations.of(0.0); //ABSOLUTE MIN + private static final Angle kHoodAbsoluteMaxRots = Rotations.of(1.174805); //ABSOLUTE MAX + public static final Angle kHoodMaxDegs = Degrees.of(kHoodAbsoluteMaxRots.in(Degrees)); public static final Angle kHoodLockDegs = Degrees.of(kHoodMaxDegs.times(0.75).in(Degrees)); //double versions - public static final double kHoodMinPosition_double = 0.1; - public static final double kHoodMaxRots_double = 1.82195; - public static final double kPhysicalHoodMinPosition_double = 10; + public static final double kHoodMinRots_double = 0.0; + public static final double kHoodMaxRots_double = kHoodAbsoluteMaxRots.in(Rotations); + public static final double kPhysicalHoodMinPosition_double = 0; public static final double kPhysicalHoodMaxPosition_double = 48; public static final double kHoodLockRots_double = kHoodMaxRots_double * 0.75; @@ -170,7 +170,7 @@ public static class ShooterK { /* HOMING */ public static final Current kWireTugMinAmps = Amps.of(8); public static final double kWireTugMinSecs = 0.125; - public static final double kHomingVoltage = -0.75 * 2.0; + public static final double kHoodHomingVoltage = -0.75; public static final Angle kHomingRetryReturnRots = Rotations.of(0.2); public static final Angle kHomePosition = Rotations.of(-0.2175); public static final Angle kInitPosition = Rotations.of(-0.145); @@ -250,13 +250,30 @@ public static class ShooterK { .withMotorArrangement(MotorArrangementValue.NEO550_JST); private static final ExternalFeedbackConfigs kHoodFeedbackConfigs = new ExternalFeedbackConfigs() .withSensorToMechanismRatio(kHoodGearing); + public static final SoftwareLimitSwitchConfigs kHoodSoftLimitConfigs = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold(kHoodAbsoluteMaxRots.minus(Rotations.of(0.05))) + .withReverseSoftLimitThreshold(kHoodAbsoluteMinRots.plus(Rotations.of(0.05))) + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true); + public static final SoftwareLimitSwitchConfigs kHoodSoftLimitConfigsNoEnable = kHoodSoftLimitConfigs + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); public static final TalonFXSConfiguration kHoodTalonFXSConfiguration = new TalonFXSConfiguration() .withSlot0(kHoodSlot0Configs) .withCurrentLimits(kHoodCurrentLimitConfig) .withMotorOutput(kHoodOutputConfigs) .withExternalFeedback(kHoodFeedbackConfigs) .withVoltage(kHoodVoltageConfigs) - .withCommutation(kHoodCommutationConfigs); + .withCommutation(kHoodCommutationConfigs) + .withSoftwareLimitSwitch(kHoodSoftLimitConfigs); + public static final TalonFXSConfiguration kHoodTalonFXSConfigurationNoSoftLimit = new TalonFXSConfiguration() + .withSlot0(kHoodSlot0Configs) + .withCurrentLimits(kHoodCurrentLimitConfig) + .withMotorOutput(kHoodOutputConfigs) + .withExternalFeedback(kHoodFeedbackConfigs) + .withVoltage(kHoodVoltageConfigs) + .withCommutation(kHoodCommutationConfigs) + .withSoftwareLimitSwitch(kHoodSoftLimitConfigsNoEnable); //---TURRET private static final Slot0Configs kTurretSlot0Configs = new Slot0Configs() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8bc936e9..f4bab388 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -122,9 +122,6 @@ public class Robot extends TimedRobot { private Trigger trg_intakeShimmy = m_manipulator.leftBumper(); //---OVERRIDE TRIGGERS - private Trigger trg_deployIntakeOverride = trg_manipOverride.and(m_manipulator.rightTrigger()); - private Trigger trg_intakeUpOverride = trg_manipOverride.and(m_manipulator.leftTrigger()); - private final Trigger trg_limitFPS = RobotModeTriggers.disabled(); private final Trigger trg_unlimitFps = RobotModeTriggers.autonomous().or(RobotModeTriggers.teleop()); @@ -267,14 +264,14 @@ private void configureBindings() { m_superstructure.intake(() -> false) ); - trg_retractIntake.onTrue( - m_intake.setIntakeArmPosCmd(IntakeArmPosition.RETRACTED) - ); + // trg_retractIntake.onTrue( + // m_intake.setIntakeArmPosCmd(IntakeArmPosition.RETRACTED) + // ); //Shooting // NORMAL FIXED SHOT // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot/*.and(() -> m_shooter.m_turret.atPosition()).*/.whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -283,8 +280,8 @@ private void configureBindings() { // m_driver.leftBumper().whileTrue(m_shooter.driverRPSIncreaseWhileHeldCmd()); - m_manipulator.povUp().onTrue(m_intake.setIntakeFlapServoCmd(IntakeK.kIntakeFlapDeployPos)); - m_manipulator.povDown().onTrue(m_intake.setIntakeFlapServoCmd(0)); + // m_manipulator.povUp().onTrue(m_intake.setIntakeFlapServoCmd(IntakeK.kIntakeFlapDeployPos)); + // m_manipulator.povDown().onTrue(m_intake.setIntakeFlapServoCmd(0)); // snapshot on each shoot press trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); @@ -310,17 +307,10 @@ private void configureBindings() { //---OVERRIDE COMMANDS m_manipulator.x().and(trg_manipOverride).onTrue(m_intake.intakeArmCurrentSenseHoming()); - // m_manipulator.y().and(trg_manipOverride).onTrue(m_shooter.setHoodPositionCmd(Degrees.of(35))); - // m_manipulator.a().and(trg_manipOverride).onTrue(m_shooter.setHoodPositionCmd(Degrees.of(1))); - - trg_deployIntakeOverride.onTrue( - m_superstructure.intakeTo(IntakeArmPosition.DEPLOYED) - ).onFalse( - m_superstructure.intakeTo(IntakeArmPosition.SAFE) - ); - trg_intakeUpOverride.onTrue( - m_superstructure.intakeTo(IntakeArmPosition.RETRACTED) - ); + m_manipulator.y().and(trg_manipOverride).onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMaxRots_double)); + m_manipulator.a().and(trg_manipOverride).onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMinRots_double)); + m_manipulator.start().and(trg_manipOverride).onTrue(m_shooter.m_hood.hoodCurrentSenseHomingCmd()); + m_manipulator.leftTrigger().onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMaxRots_double / 2.0)); // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false)); //false? im not sure @@ -338,7 +328,7 @@ private void configureBindings() { } private void configureTestBindings() { - m_driver.povLeft().onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMinPosition_double)); + m_driver.povLeft().onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMinRots_double)); m_driver.povUp().onTrue(m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodMaxRots_double)); } @@ -448,7 +438,7 @@ public void disabledPeriodic() { m_disableChangeDelayTimer.stop(); m_disableChangeDelayTimer.reset(); m_shooter.m_turret.setTurretNeutralMode(NeutralModeValue.Coast); - m_intake.setIntakeArmNeutralMode(NeutralModeValue.Coast); + // m_intake.setIntakeArmNeutralMode(NeutralModeValue.Coast); m_shooter.m_hood.setHoodNeutralMode(NeutralModeValue.Coast); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 450d25ad..2a0c2ba8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterK; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; @@ -33,11 +34,9 @@ public class Hood extends SubsystemBase { private final DoubleLogger log_hoodControlPos = WaltLogger.logDouble("Shooter/Hood", "hoodControlPos"); private final DoubleLogger log_hoodCurrentPos = WaltLogger.logDouble("Shooter/Hood", "hoodCurrentPos"); - private Debouncer m_currentDebouncer = new Debouncer(0.100, DebounceType.kRising); - private Debouncer m_velocityDebouncer = new Debouncer(0.125, DebounceType.kRising); + private Debouncer m_currentDebouncer = new Debouncer(0.125, DebounceType.kRising); - private BooleanSupplier m_currentSpike = () -> m_hood.getStatorCurrent().getValueAsDouble() > 10.0; - private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_hood.getVelocity().getValueAsDouble()) < 0.005; + private BooleanSupplier m_currentSpike = () -> m_hood.getStatorCurrent().getValueAsDouble() > 5.0; private final StaticBrake m_BrakeReq = new StaticBrake(); @@ -46,7 +45,7 @@ public class Hood extends SubsystemBase { public Hood() { m_hood.getConfigurator().apply(kHoodTalonFXSConfiguration); - m_hood.setPosition(kHoodMinPosition); + m_hood.setPosition(0); m_isHoodHomed = true; log_hoodHomed.accept(m_isHoodHomed); @@ -70,9 +69,9 @@ public Command setHoodPositionCmd(DoubleSubscriber sub_rots) { private double getHoodAngleDeg() { double hoodPositionDeg = m_hood.getPosition().getValue().in(Degrees); - double absoluteToPhysicalAngleRatio = (360 * (kHoodMaxRots_double - kHoodMinPosition_double))/(kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double); + double absoluteToPhysicalAngleRatio = (360 * (kHoodMaxRots_double - kHoodMinRots_double))/(kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double); - return kPhysicalHoodMinPosition_double + (hoodPositionDeg - (kHoodMinPosition_double * 360)) * (absoluteToPhysicalAngleRatio); + return kPhysicalHoodMinPosition_double + (hoodPositionDeg - (kHoodMinRots_double * 360)) * (absoluteToPhysicalAngleRatio); } public boolean isHoodHomed() { @@ -81,7 +80,8 @@ public boolean isHoodHomed() { public Command hoodCurrentSenseHomingCmd(){ Runnable init = () -> { - m_hood.setControl(m_hoodZeroReq.withOutput(kHomingVoltage)); + m_hood.getConfigurator().apply(ShooterK.kHoodTalonFXSConfigurationNoSoftLimit); + m_hood.setControl(m_hoodZeroReq.withOutput(kHoodHomingVoltage)); m_isHoodHomed = false; log_hoodHomed.accept(m_isHoodHomed); }; @@ -94,16 +94,16 @@ public Command hoodCurrentSenseHomingCmd(){ return; } - m_hood.setPosition(kHoodMinPosition); + m_hood.setPosition(kHoodAbsoluteMinRots); m_hood.setControl(m_BrakeReq); removeDefaultCommand(); m_isHoodHomed = true; + m_hood.getConfigurator().apply(ShooterK.kHoodTalonFXSConfiguration); log_hoodHomed.accept(m_isHoodHomed); }; BooleanSupplier isFinished = () -> - m_currentDebouncer.calculate(m_currentSpike.getAsBoolean()) && - m_velocityDebouncer.calculate(m_veloIsNearZero.getAsBoolean()); + m_currentDebouncer.calculate(m_currentSpike.getAsBoolean()); return new FunctionalCommand(init, () -> {}, end, isFinished, this).withTimeout(5); } From edd02d93cd8bf1fcd239442c1871dbef9f2be0be Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 21:32:52 -0400 Subject: [PATCH 22/40] tuning for tunnel --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 850a0089..fd607f9d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -550,10 +550,10 @@ public static class IndexerK { .withVoltage(kSpindexerVoltageConfigs); private static final Slot0Configs kTunnelSlot0Configs = new Slot0Configs() - .withKS(0.1124) - .withKV(0.102) + .withKS(0.4) + .withKV(0.047) .withKA(0) - .withKP(0.06) + .withKP(0.1) .withKI(0) .withKD(0); private static final CurrentLimitsConfigs kTunnelCurrentLimitConfigs = new CurrentLimitsConfigs() From 0c70ce8a2a0ba720b00569cbb765bb3e34f6ac51 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 21:42:38 -0400 Subject: [PATCH 23/40] more hood logging #cope --- src/main/java/frc/robot/subsystems/shooter/Hood.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 2a0c2ba8..96e500d4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -33,6 +33,7 @@ public class Hood extends SubsystemBase { private final BooleanLogger log_hoodHomed = WaltLogger.logBoolean("Shooter/Hood", "Homed"); private final DoubleLogger log_hoodControlPos = WaltLogger.logDouble("Shooter/Hood", "hoodControlPos"); private final DoubleLogger log_hoodCurrentPos = WaltLogger.logDouble("Shooter/Hood", "hoodCurrentPos"); + private final DoubleLogger log_hoodPositionDeg = WaltLogger.logDouble("Shooter/Hood", "hoodPositionDeg"); //for debugging purposes private Debouncer m_currentDebouncer = new Debouncer(0.125, DebounceType.kRising); @@ -114,6 +115,7 @@ public void setHoodNeutralMode(NeutralModeValue value) { @Override public void periodic() { - log_hoodCurrentPos.accept(getHoodAngleDeg()); + log_hoodCurrentPos.accept(getHoodAngleDeg()); + log_hoodPositionDeg.accept(m_hood.getPosition().getValue().in(Degrees)); } } From 0204376841c6d66de3e7d21024d8bb5b60f17447 Mon Sep 17 00:00:00 2001 From: sohnshaik Date: Sat, 28 Mar 2026 23:22:50 -0400 Subject: [PATCH 24/40] started lerping, turret is not aiming correctly --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 17 +++-- .../frc/robot/subsystems/shooter/Hood.java | 5 +- .../frc/robot/subsystems/shooter/Shooter.java | 8 ++- .../robot/subsystems/shooter/ShooterCalc.java | 7 +- .../subsystems/shooter/ShotCalculator.java | 68 +------------------ .../frc/robot/subsystems/shooter/Turret.java | 5 ++ 7 files changed, 33 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fd607f9d..afad0dc7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -71,7 +71,7 @@ public static class WpiK { } public static class ShooterK { public static final String kLogTab = "Shooter"; - private static final Rotation2d kTurretAngleOffset = Rotation2d.fromDegrees(180 + -135); + private static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.132324 ); //4.87 public static final Transform3d kTurretTransform = new Transform3d(new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)), new Rotation3d(kTurretAngleOffset)); //DUMMY VALS public static final Distance kInchesAboveFunnel = Inches.of(20);// distance the ball must travel above the funnel opening to arc correctly into the hub diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f4bab388..ba564424 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ import choreo.auto.AutoFactory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DataLogManager; @@ -155,7 +156,7 @@ public class Robot extends TimedRobot { public Robot() { configureBindings(); // configureTestBindings(); //this should be commented out during competition matches - // configureTestingDashboard(); + configureTestingDashboard(); lastGotTagMsmtTimer.start(); @@ -270,8 +271,8 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT - // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); + // trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -314,8 +315,14 @@ private void configureBindings() { // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false)); //false? im not sure - m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); - m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); + // m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); + // m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); + + m_driver.povDown().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() - Inches.of(20).magnitude()), 0.001)); + m_driver.povUp().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() + Inches.of(20).magnitude()), 0.001)); + m_driver.povRight().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() + Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); + m_driver.povLeft().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() - Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); + // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT()); // trg_optimalPrefireTime.whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 96e500d4..edeb43c9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -69,10 +69,9 @@ public Command setHoodPositionCmd(DoubleSubscriber sub_rots) { } private double getHoodAngleDeg() { - double hoodPositionDeg = m_hood.getPosition().getValue().in(Degrees); - double absoluteToPhysicalAngleRatio = (360 * (kHoodMaxRots_double - kHoodMinRots_double))/(kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double); + double absoluteToPhysicalAngleRatio = (kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double)/(360 * (kHoodMaxRots_double - kHoodMinRots_double)); - return kPhysicalHoodMinPosition_double + (hoodPositionDeg - (kHoodMinRots_double * 360)) * (absoluteToPhysicalAngleRatio); + return kPhysicalHoodMinPosition_double + (m_hood.getPosition().getValue().in(Degrees) - (kHoodMinRots_double * 360)) * (absoluteToPhysicalAngleRatio); } public boolean isHoodHomed() { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5eb2cf86..8a1d42a7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -97,6 +97,7 @@ public class Shooter extends SubsystemBase { /* LOGGERS */ private final DoubleLogger log_shooterVelocityRPS = WaltLogger.logDouble("Shooter/Flywheel", "shooterVelocityRPS"); private final DoubleLogger log_turretPositionRots = WaltLogger.logDouble("Shooter/Turret", "turretPositionRots"); + private final DoubleLogger log_turretPositionRobotRelativeRots = WaltLogger.logDouble("Shooter/Turret", "turretPositionRobotRelativeRots"); private final BooleanLogger log_spunUp = WaltLogger.logBoolean(kLogTab, "spunUp"); @@ -303,14 +304,14 @@ public void periodic() { var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { m_turret.setTurretPos(m_turret.getTurretLockAngle()); - m_calcFlywheelVelocityRotPerSec = kShooterRPSd; + // m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { m_turret.setTurretPos(Rotations.of(-0.250)); } else { m_turret.setTurretPos(turretReference, turretVelocityFF); - m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); - if (true) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK + // m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); + if (false) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; m_calcFlywheelVelocityRotPerSec = MathUtil.clamp(m_calcFlywheelVelocityRotPerSec, 0, kShooterMaxRPSd); //clamp here or clamp only when setShooterVel is called? } @@ -330,6 +331,7 @@ public void periodic() { } } + log_turretPositionRobotRelativeRots.accept(kDriverRPSIncreaseD); log_shooterVelocityRPS.accept(m_latestFlywheelVelocityRotPerSec); log_turretPositionRots.accept(m_latestTurretPositionRots); log_spunUp.accept(isShooterSpunUp()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java index d70e7649..2440ba46 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -44,6 +45,8 @@ public class ShooterCalc { private final Pose3dLogger log_desiredAimPose = WaltLogger.logPose3d("ShotCalc", "DesiredAimPose"); private final Pose3dLogger log_currentAimPose = WaltLogger.logPose3d("ShotCalc", "CurrentAimPose"); private final DoubleLogger log_loopTime = WaltLogger.logDouble("ShotCalc", "LoopTimeMsec"); + private static final Pose3dLogger log_turretFieldPose = WaltLogger.logPose3d("ShotCalc", "turretFieldPose"); + // private static final Pose3dLogger log_turret // Precomputed doubles for hot-path unit conversions @@ -148,9 +151,11 @@ public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotP /* Calculation Zone */ // turret pivot location in field space (no extra rotateBy — that's for // visualization only) - Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform); + Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform).rotateBy(new Rotation3d(new Rotation2d(turretPosition))); Translation3d turretTranslation = turretPose.getTranslation(); + log_turretFieldPose.accept(turretPose); + // vector from turret pivot to target in field space Translation3d distance = target.minus(turretTranslation); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 19a8a533..3853eca1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -52,72 +52,8 @@ public class ShotCalculator { minDistance = 0.94; maxDistance = 5.8; - //right mid grid - m_shotMap.put(3.903, new ShotData(RotationsPerSecond.of(73.18), Degrees.of(327.75))); - m_timeOfFlightMap.put(3.903, 1.35); - - //backright grid - THIS ONE IS INCONSISTENT UNTIL SHOOTER V3 - m_shotMap.put(5.657, new ShotData(RotationsPerSecond.of(96.42), Degrees.of(374.20))); //real: 95.00 - m_timeOfFlightMap.put(5.657, 1.56); - - //frontright grid - m_shotMap.put(3.763, new ShotData(RotationsPerSecond.of(77), Degrees.of(300))); - m_timeOfFlightMap.put(3.763, 1.43); - - //frontleft grid - m_shotMap.put(1.476, new ShotData(RotationsPerSecond.of(57.72), Degrees.of(150.77))); - m_timeOfFlightMap.put(1.476, 1.125); - - //backleft grid - m_shotMap.put(3.854, new ShotData(RotationsPerSecond.of(79), Degrees.of(345))); - m_timeOfFlightMap.put(3.854, 1.38); - - //against the hub - m_shotMap.put(0.835, new ShotData(RotationsPerSecond.of(66.21), Degrees.of(85.51))); - m_timeOfFlightMap.put(0.835, 1.17); - - //against the tower - m_shotMap.put(2.977, new ShotData(RotationsPerSecond.of(71.14), Degrees.of(338.81))); - m_timeOfFlightMap.put(2.977, 1.03); - - //backleft grid - m_shotMap.put(4.348, new ShotData(RotationsPerSecond.of(77.94), Degrees.of(396.32))); - m_timeOfFlightMap.put(4.348, 1.38); - - //left mid grid - m_shotMap.put(2.799, new ShotData(RotationsPerSecond.of(67.91), Degrees.of(308.94))); - m_timeOfFlightMap.put(2.799, 1.08); - - m_shotMap.put(3.561, new ShotData(RotationsPerSecond.of(79.30), Degrees.of(287.93))); - m_timeOfFlightMap.put(3.561, 1.42); - - m_shotMap.put(1.870, new ShotData(RotationsPerSecond.of(62.31), Degrees.of(231.52))); - m_timeOfFlightMap.put(1.870, 1.1); - - m_shotMap.put(3.259, new ShotData(RotationsPerSecond.of(75.64), Degrees.of(268.02))); - m_timeOfFlightMap.put(3.259, 1.37); - - //AT COMPETITION - - // m_shotMap.put(2.755, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(221.02))); - // m_timeOfFlightMap.put(2.755, 1.28); - - // m_shotMap.put(2.352, new ShotData(RotationsPerSecond.of(75.41), Degrees.of(169.77))); - // m_timeOfFlightMap.put(2.352, 0.9); - - // //3/21/26 8:40 AM - // m_shotMap.put(3.425, new ShotData(RotationsPerSecond.of(87), Degrees.of(280))); - // m_timeOfFlightMap.put(3.425, 1.41); - - // m_shotMap.put(3.933, new ShotData(RotationsPerSecond.of(80.5), Degrees.of(310))); - // m_timeOfFlightMap.put(3.933, 1.38); - - //3/26 - m_shotMap.put(2.462, new ShotData(RotationsPerSecond.of(69.27), Rotations.of(0.67))); - m_timeOfFlightMap.put(2.462, 1.15); - - m_shotMap.put(3.335, new ShotData(RotationsPerSecond.of(81.37), Rotations.of(0.68))); - m_timeOfFlightMap.put(3.335, 1.38); + m_shotMap.put(5.303, new ShotData(RotationsPerSecond.of(58.47), Rotations.of(0.90))); + m_timeOfFlightMap.put(5.303, 1.65); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 00f3c56a..b94afe1b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -24,6 +25,7 @@ import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; import frc.util.WaltLogger.IntLogger; +import frc.util.WaltLogger.Pose3dLogger; public class Turret extends SubsystemBase { private boolean m_holdTurretAtIntakePos = false; @@ -51,6 +53,7 @@ public class Turret extends SubsystemBase { private final DoubleLogger log_turretLCMPos = WaltLogger.logDouble(kLogTab, "turretCRTPos"); private final DoubleLogger log_turretClosedLoopError = WaltLogger.logDouble(kLogTab, "turretCLE"); private final BooleanLogger log_atPos = WaltLogger.logBoolean(kLogTab, "atPos"); + private final Pose3dLogger log_turretTransform = WaltLogger.logPose3d(kLogTab, "turretTransform"); StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); @@ -61,6 +64,8 @@ public Turret() { m_lcmEncB.setAssumedFrequency(488); m_lcmEncB.setConnectedFrequencyThreshold(400); + log_turretTransform.accept(kTurretTransform); + homeTurret(true); } From 670d7c7320dde7838b722e93623ad38b5d332cbe Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 14:43:49 -0400 Subject: [PATCH 25/40] turret tracking working (offset changed) --- src/main/java/frc/robot/Constants.java | 9 ++++-- .../robot/subsystems/shooter/ShooterCalc.java | 32 +++++++++++++------ 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index afad0dc7..ef4f484c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,6 +2,8 @@ package frc.robot; import static edu.wpi.first.units.Units.*; +import static frc.robot.Constants.ShooterK.kTurretAngleOffset3d; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CommutationConfigs; @@ -71,8 +73,11 @@ public static class WpiK { } public static class ShooterK { public static final String kLogTab = "Shooter"; - private static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.132324 ); //4.87 - public static final Transform3d kTurretTransform = new Transform3d(new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)), new Rotation3d(kTurretAngleOffset)); //DUMMY VALS + public static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.106); //4.87 //0.132324 // was 0.12, decreased 0.014 (~5deg) to fix consistent rightward aim error + public static final Rotation3d kTurretAngleOffset3d = new Rotation3d(kTurretAngleOffset); + public static final Translation3d kTurretTranslation = new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)); + public static final Transform3d kTurretTransformNoRotation = new Transform3d(kTurretTranslation, Rotation3d.kZero); + public static final Transform3d kTurretTransform = new Transform3d(kTurretTranslation, kTurretAngleOffset3d); public static final Distance kInchesAboveFunnel = Inches.of(20);// distance the ball must travel above the funnel opening to arc correctly into the hub public static final boolean kUseStaticShot = false; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java index 2440ba46..1107670f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -28,6 +27,7 @@ import frc.util.AllianceZoneUtil; import frc.util.WaltLogger; import frc.util.WaltLogger.DoubleLogger; +import frc.util.WaltLogger.Pose2dLogger; import frc.util.WaltLogger.Pose3dLogger; import static edu.wpi.first.units.Units.*; @@ -45,6 +45,7 @@ public class ShooterCalc { private final Pose3dLogger log_desiredAimPose = WaltLogger.logPose3d("ShotCalc", "DesiredAimPose"); private final Pose3dLogger log_currentAimPose = WaltLogger.logPose3d("ShotCalc", "CurrentAimPose"); private final DoubleLogger log_loopTime = WaltLogger.logDouble("ShotCalc", "LoopTimeMsec"); + private static final Pose3dLogger log_turretRobotPose = WaltLogger.logPose3d("ShotCalc", "turretRobotPose"); private static final Pose3dLogger log_turretFieldPose = WaltLogger.logPose3d("ShotCalc", "turretFieldPose"); // private static final Pose3dLogger log_turret @@ -100,6 +101,10 @@ private void calcCallback() { // Logging log_globalShotTarget.accept(m_aimTarget); + log_desiredAimPose.accept(m_shotCalcOutputs.turretCalcDetails().desiredAimPose()); + log_currentAimPose.accept(m_shotCalcOutputs.turretCalcDetails().currentAimPose()); + log_rawDesiredTurretRot.accept(m_shotCalcOutputs.turretCalcDetails().rawDesiredRotations()); + log_desiredTurretRot.accept(m_shotCalcOutputs.turretCalcDetails().turretReferenceRots()); log_loopTime.accept(m_calcTimer.get() * 1000.0); } @@ -144,17 +149,23 @@ public record AzimuthCalcDetails(double turretReferenceRots, Pose3d desiredAimPo * of kTurretMaxAngle * and kTurretMinAngle */ - public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotPose, double turretPosition, ChassisSpeeds fieldSpeeds) { + public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotPose, double turretHeading, ChassisSpeeds fieldSpeeds) { + Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform); + // Convert once; reused below in both snapback and current-aim logging - double turretPositionRots = turretPosition; + double turretHeadingRots = turretHeading; + // Field-space pose: turret pivot in field space, rotated by (turret zero field direction + encoder position) + Pose3d turretRobotPose = new Pose3d( + turretPose.getTranslation(), + new Rotation3d(0, 0, turretPose.getRotation().toRotation2d().getRadians() + turretHeadingRots * (2 * Math.PI))); + log_turretRobotPose.accept(turretRobotPose); /* Calculation Zone */ // turret pivot location in field space (no extra rotateBy — that's for // visualization only) - Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform).rotateBy(new Rotation3d(new Rotation2d(turretPosition))); + Pose3d turretRealPose = new Pose3d(turretPose.getTranslation(), new Rotation3d(0, 0, (-kTurretAngleOffset.plus(Rotation2d.fromDegrees(180)).getRadians()))); Translation3d turretTranslation = turretPose.getTranslation(); - - log_turretFieldPose.accept(turretPose); + log_turretFieldPose.accept(turretRealPose); // vector from turret pivot to target in field space Translation3d distance = target.minus(turretTranslation); @@ -176,7 +187,7 @@ public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotP var desiredAimPose = new Pose3d(turretTranslation, new Rotation3d(0, 0, fieldYawRad)); // current aim: turret pivot with X-axis showing where the turret is actually // pointing right now - double currentFieldYaw = turretZeroFieldDir.getRadians() + turretPositionRots * (2 * Math.PI); + double currentFieldYaw = turretZeroFieldDir.getRadians() + turretHeadingRots * (2 * Math.PI); var currentAimPose = new Pose3d(turretTranslation, new Rotation3d(0, 0, currentFieldYaw)); // normalizes the angle to be fit in the range of the max rotations @@ -187,9 +198,9 @@ public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotP double snapbackSafeAngleRotations = angleRotations; // this is the snapback function, to make sure that you will always be tracking // and you will not go over your physical limits. - if (turretPositionRots > 0 && angleRotations + 1 <= kTurretMaxRotsD) { + if (turretHeadingRots > 0 && angleRotations + 1 <= kTurretMaxRotsD) { snapbackSafeAngleRotations += 1; - } else if (turretPositionRots < 0 && angleRotations - 1 >= kTurretMinRotsD) { + } else if (turretHeadingRots < 0 && angleRotations - 1 >= kTurretMinRotsD) { snapbackSafeAngleRotations -= 1; } @@ -232,7 +243,8 @@ public static ShotCalcOutputs calcShot( ChassisSpeeds chassisSpeeds ) { // How fast the robot is currently going, (CURRENT ROBOT VELOCITY) - ChassisSpeeds fieldSpeeds = staticShot ? WpiK.kZeroChassisSpeeds : chassisSpeeds; + // double speedMps = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + ChassisSpeeds fieldSpeeds = (staticShot /*|| speedMps < 0.1*/) ? WpiK.kZeroChassisSpeeds : chassisSpeeds; // The Calculated shot itself, according to the current robotPose, robotSpeeds, // and the currentTarget ShotData calculatedShot = ShotCalculator.iterativeMovingShotFromInterpolationMap( From 246c0b2b872c2e46505391a873c107d8309dc450 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 14:44:03 -0400 Subject: [PATCH 26/40] new lerp points + method --- .../subsystems/shooter/ShotCalculator.java | 24 +++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 3853eca1..347a1020 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -52,8 +52,28 @@ public class ShotCalculator { minDistance = 0.94; maxDistance = 5.8; - m_shotMap.put(5.303, new ShotData(RotationsPerSecond.of(58.47), Rotations.of(0.90))); - m_timeOfFlightMap.put(5.303, 1.65); + //Ordered via DistanceToTarget + addLerpPoint(5.700, 59.47, 0.88, 1.33); + addLerpPoint(5.628, 59.47, 0.91, 1.39); + addLerpPoint(5.515, 59.50, 0.87, 1.30); + addLerpPoint(5.412, 58.50, 0.86, 1.27); + addLerpPoint(5.303, 58.47, 0.90, 1.65); + addLerpPoint(5.060, 57.00, 0.85, 1.26); + addLerpPoint(4.910, 56.00, 0.81, 1.17); + addLerpPoint(4.874, 55.50, 0.80, 1.29); + addLerpPoint(4.684, 54.00, 0.77, 1.21); + addLerpPoint(4.562, 54.50, 0.55, 1.13); + addLerpPoint(4.496, 55.86, 0.45, 1.40); + addLerpPoint(4.407, 55.86, 0.45, 1.38); + addLerpPoint(4.108, 55.86, 0.42, 1.40); + addLerpPoint(4.000, 55.46, 0.42, 1.37); + addLerpPoint(3.944, 54.90, 0.37, 1.33); + addLerpPoint(3.823, 55.20, 0.20, 1.47); + } + + public static void addLerpPoint(double distanceToTarget, double shooterRPS, double hoodRots, double timeOfFlight) { + m_shotMap.put(distanceToTarget, new ShotData(RotationsPerSecond.of(shooterRPS), Rotations.of(hoodRots))); + m_timeOfFlightMap.put(distanceToTarget, timeOfFlight); } From d326533d9a134b427294830b6f2c2124d7993fc5 Mon Sep 17 00:00:00 2001 From: Saarth Date: Sun, 29 Mar 2026 14:46:20 -0400 Subject: [PATCH 27/40] slimed out the flap --- src/main/java/frc/robot/Constants.java | 9 +++------ src/main/java/frc/robot/Robot.java | 3 --- .../java/frc/robot/subsystems/Intake.java | 19 +------------------ 3 files changed, 4 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ba51d3d0..9f0f4447 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -401,13 +401,10 @@ public static class IntakeK { public static final AngularVelocity kIntakeRollersMaxRPS = MotorK.kX60FOCMaxVelocity.div(kIntakeRollersGearing); public static final AngularVelocity kIntakeRollersShootRPS = kIntakeRollersMaxRPS.times(0.2); - public static final double kIntakeFlapDeployPos = 300; - public static final double kIntakeFlapResetPos = 0; - /* IDS */ - public static final int kIntakeArmCANID = 40; - public static final int kIntakeRollersCANID = 41; - public static final int kIntakeFlapChannel = 0; + public static final int kIntakeArmA_CANID = 40; + public static final int kIntakeArmB_CANID = 41; + public static final int kIntakeRollersCANID = 42; /* CONFIGS */ //IntakeArm Motor diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a85ede05..09ae431c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -283,9 +283,6 @@ private void configureBindings() { // m_driver.leftBumper().whileTrue(m_shooter.driverRPSIncreaseWhileHeldCmd()); - m_manipulator.povUp().onTrue(m_intake.setIntakeFlapServoCmd(IntakeK.kIntakeFlapDeployPos)); - m_manipulator.povDown().onTrue(m_intake.setIntakeFlapServoCmd(0)); - // snapshot on each shoot press trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5c336626..e1b6f1b6 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -37,9 +37,8 @@ public class Intake extends SubsystemBase { /* CLASS VARIABLES */ //---MOTORS + CONTROL REQUESTS - private final TalonFX m_intakeArm = new TalonFX(kIntakeArmCANID); //x44Foc + private final TalonFX m_intakeArm = new TalonFX(kIntakeArmA_CANID); //x44Foc private final TalonFX m_intakeRollers = new TalonFX(kIntakeRollersCANID); //x60Foc - private final GobildaServoAngled m_intakeFlapServo = new GobildaServoAngled(kIntakeFlapChannel); private DynamicMotionMagicVoltage m_MMVReq = new DynamicMotionMagicVoltage(0, 1, 1).withEnableFOC(true); private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(false); @@ -84,8 +83,6 @@ public class Intake extends SubsystemBase { private final BooleanLogger log_isIntakeArmHomed = WaltLogger.logBoolean(kLogTab, "isIntakeArmHomed"); - private final DoubleLogger log_intakeFlapServoDesiredPosition = WaltLogger.logDouble(kLogTab, "servoDesiredPosition"); - /* CONSTRUCTOR */ public Intake() { m_intakeArm.getConfigurator().apply(kIntakeArmConfiguration); @@ -165,20 +162,9 @@ public Command setIntakeRollersVelocityCmd(double volts) { return runOnce(() -> setIntakeRollersVelocity(volts)); } - public void setIntakeFlapServo(double pos) { - m_intakeFlapServo.setAngle(pos); - - log_intakeFlapServoDesiredPosition.accept(m_intakeFlapServo.getAngle()); //does getting angle use lots of time/cpu? should we just pass in pos? - } - - public Command setIntakeFlapServoCmd(double pos) { - return runOnce(() -> setIntakeFlapServo(pos)); - } - // TESTING TO SEE IF WE CAN JUST SAY 0 AS 0 public Command intakeArmHome() { return Commands.parallel( - setIntakeFlapServoCmd(kIntakeFlapDeployPos), Commands.sequence( runOnce(() -> m_intakeArm.setPosition(0)), @@ -193,7 +179,6 @@ public Command intakeArmHome() { public Command intakeArmCurrentSenseHoming() { Runnable init = () -> { m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); - setIntakeFlapServo(kIntakeFlapDeployPos); m_isIntakeArmHomed = false; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); @@ -208,8 +193,6 @@ public Command intakeArmCurrentSenseHoming() { setIntakeArmPosCmd(IntakeArmPosition.RETRACTED); m_isIntakeArmHomed = true; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); - - // DONT TELL FLAP_SERVO TO GO BACK ON END BECAUSE IF HOMING FINISHES TOO EARLY, THE SERVO WONT MOVE ENOUGH OUT TO DEPLOY FLAP }; BooleanSupplier isFinished = () -> From e9e3ac8205d3e96f19d69631b18eb59b13be5b70 Mon Sep 17 00:00:00 2001 From: Saarth Date: Sun, 29 Mar 2026 15:11:02 -0400 Subject: [PATCH 28/40] added intake follower --- src/main/java/frc/robot/Constants.java | 43 ++++++++++++------- .../java/frc/robot/subsystems/Intake.java | 38 +++++++++------- 2 files changed, 50 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9f0f4447..dc3a4c01 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -407,41 +407,52 @@ public static class IntakeK { public static final int kIntakeRollersCANID = 42; /* CONFIGS */ - //IntakeArm Motor - private static final CurrentLimitsConfigs kIntakeArmCurrentLimitConfigs = new CurrentLimitsConfigs() + // IntakeArm Motors + private static final CurrentLimitsConfigs kIntakeArmACurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(20) .withSupplyCurrentLimit(20) .withSupplyCurrentLowerLimit(20) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true); - private static final Slot0Configs kIntakeArmSlot0Configs = new Slot0Configs() + private static final Slot0Configs kIntakeArmASlot0Configs = new Slot0Configs() .withKS(1.5) .withKV(0) .withKA(0) .withKP(50) .withKI(0) .withKD(0); - public static final MotorOutputConfigs kIntakeArmMotorOutputConfigs = new MotorOutputConfigs() + public static final MotorOutputConfigs kIntakeArmAMotorOutputConfigs = new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withInverted(InvertedValue.Clockwise_Positive); - private static final MotionMagicConfigs kIntakeArmMotionMagicConfigs = new MotionMagicConfigs() + private static final MotionMagicConfigs kIntakeArmAMotionMagicConfigs = new MotionMagicConfigs() .withMotionMagicCruiseVelocity(0.75) .withMotionMagicAcceleration(10) .withMotionMagicJerk(0); - public static final FeedbackConfigs kIntakeArmFeedbackConfigs = new FeedbackConfigs() + public static final FeedbackConfigs kIntakeArmAFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kIntakeArmGearing); - private static final VoltageConfigs kIntakeArmVoltageConfigs = new VoltageConfigs() + private static final VoltageConfigs kIntakeArmAVoltageConfigs = new VoltageConfigs() .withPeakForwardVoltage(12) .withPeakReverseVoltage(-12); - public static final TalonFXConfiguration kIntakeArmConfiguration = new TalonFXConfiguration() - .withCurrentLimits(kIntakeArmCurrentLimitConfigs) - .withSlot0(kIntakeArmSlot0Configs) - .withMotorOutput(kIntakeArmMotorOutputConfigs) - .withMotionMagic(kIntakeArmMotionMagicConfigs) - .withVoltage(kIntakeArmVoltageConfigs) - .withFeedback(kIntakeArmFeedbackConfigs); - - //Intake Rollers Motor + public static final TalonFXConfiguration kIntakeArmAConfiguration = new TalonFXConfiguration() + .withCurrentLimits(kIntakeArmACurrentLimitConfigs) + .withSlot0(kIntakeArmASlot0Configs) + .withMotorOutput(kIntakeArmAMotorOutputConfigs) + .withMotionMagic(kIntakeArmAMotionMagicConfigs) + .withVoltage(kIntakeArmAVoltageConfigs) + .withFeedback(kIntakeArmAFeedbackConfigs); + + public static final MotorOutputConfigs kIntakeArmBMotorOutputConfigs = new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive); + public static final TalonFXConfiguration kIntakeArmBConfiguration = new TalonFXConfiguration() + .withCurrentLimits(kIntakeArmACurrentLimitConfigs) + .withSlot0(kIntakeArmASlot0Configs) + .withMotorOutput(kIntakeArmBMotorOutputConfigs) + .withMotionMagic(kIntakeArmAMotionMagicConfigs) + .withVoltage(kIntakeArmAVoltageConfigs) + .withFeedback(kIntakeArmAFeedbackConfigs); + + // IntakeRollers Motor private static final CurrentLimitsConfigs kIntakeRollersCurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(85) .withSupplyCurrentLimit(50) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e1b6f1b6..23796451 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.ChassisReference; @@ -37,14 +39,16 @@ public class Intake extends SubsystemBase { /* CLASS VARIABLES */ //---MOTORS + CONTROL REQUESTS - private final TalonFX m_intakeArm = new TalonFX(kIntakeArmA_CANID); //x44Foc + private final TalonFX m_intakeArmA = new TalonFX(kIntakeArmA_CANID); //x60Foc + private final TalonFX m_intakeArmB = new TalonFX(kIntakeArmB_CANID); //x60Foc + private final TalonFX m_intakeRollers = new TalonFX(kIntakeRollersCANID); //x60Foc private DynamicMotionMagicVoltage m_MMVReq = new DynamicMotionMagicVoltage(0, 1, 1).withEnableFOC(true); private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(false); - private BooleanSupplier m_currentSpike = () -> m_intakeArm.getStatorCurrent().getValueAsDouble() > 5.0; - private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArm.getVelocity().getValueAsDouble()) < 0.005; + private BooleanSupplier m_currentSpike = () -> m_intakeArmA.getStatorCurrent().getValueAsDouble() > 5.0; + private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArmA.getVelocity().getValueAsDouble()) < 0.005; private VoltageOut m_intakeArmZeroingReq = new VoltageOut(0); @@ -85,9 +89,13 @@ public class Intake extends SubsystemBase { /* CONSTRUCTOR */ public Intake() { - m_intakeArm.getConfigurator().apply(kIntakeArmConfiguration); + m_intakeArmA.getConfigurator().apply(kIntakeArmAConfiguration); + m_intakeArmB.getConfigurator().apply(kIntakeArmBConfiguration); + m_intakeRollers.getConfigurator().apply(kIntakeRollersConfiguration); + m_intakeArmB.setControl(new Follower(kIntakeArmA_CANID, MotorAlignmentValue.Opposed)); + if (Robot.isReal()) { setDefaultCommand(intakeArmCurrentSenseHoming()); // setDefaultCommand(intakeArmHome()); @@ -97,7 +105,7 @@ public Intake() { } private void initSim() { - WaltMotorSim.initSimFX(m_intakeArm, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX44); + WaltMotorSim.initSimFX(m_intakeArmA, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX44); WaltMotorSim.initSimFX(m_intakeRollers, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX60); } @@ -115,11 +123,11 @@ public Command setIntakeArmPosCmd(Angle rots, double RPSPS) { } public void setIntakeArmPos(Angle rots, double RPSPS) { - m_intakeArm.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS)); + m_intakeArmA.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS)); } public boolean isIntakeArmAtDest() { - return m_intakeArm.getMotionMagicAtTarget().getValue(); + return m_intakeArmA.getMotionMagicAtTarget().getValue(); } public Command shimmy() { @@ -138,12 +146,12 @@ public Command shimmy() { } public void setIntakeArmNeutralMode(NeutralModeValue value) { - m_intakeArm.setNeutralMode(value); + m_intakeArmA.setNeutralMode(value); } //for TestingDashboard public Command setIntakeArmPos(DoubleSubscriber sub_rots) { - return run(() -> m_intakeArm.setControl(m_MMVReq.withPosition(Rotations.of(sub_rots.get())))); + return run(() -> m_intakeArmA.setControl(m_MMVReq.withPosition(Rotations.of(sub_rots.get())))); } public Command startIntakeRollers() { @@ -166,7 +174,7 @@ public Command setIntakeRollersVelocityCmd(double volts) { public Command intakeArmHome() { return Commands.parallel( Commands.sequence( - runOnce(() -> m_intakeArm.setPosition(0)), + runOnce(() -> m_intakeArmA.setPosition(0)), runOnce(() -> m_isIntakeArmHomed = true), runOnce(() -> log_isIntakeArmHomed.accept(m_isIntakeArmHomed)), @@ -178,7 +186,7 @@ public Command intakeArmHome() { public Command intakeArmCurrentSenseHoming() { Runnable init = () -> { - m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); + m_intakeArmA.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); m_isIntakeArmHomed = false; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); @@ -187,8 +195,8 @@ public Command intakeArmCurrentSenseHoming() { Runnable execute = () -> {}; Consumer onEnd = (Boolean interrupted) -> { - m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(0)); - m_intakeArm.setPosition(0); + m_intakeArmA.setControl(m_intakeArmZeroingReq.withOutput(0)); + m_intakeArmA.setPosition(0); removeDefaultCommand(); setIntakeArmPosCmd(IntakeArmPosition.RETRACTED); m_isIntakeArmHomed = true; @@ -208,12 +216,12 @@ public void periodic() { log_targetIntakeArmRots.accept(m_MMVReq.Position); log_targetIntakeRollersRPS.accept(m_VVReq.Output); log_intakeRollersRPS.accept(m_intakeRollers.getVelocity().getValueAsDouble()); - log_intakeArmRots.accept(m_intakeArm.getPosition().getValueAsDouble()); + log_intakeArmRots.accept(m_intakeArmA.getPosition().getValueAsDouble()); } @Override public void simulationPeriodic() { - WaltMotorSim.updateSimFX(m_intakeArm, m_intakeArmSim); + WaltMotorSim.updateSimFX(m_intakeArmA, m_intakeArmSim); WaltMotorSim.updateSimFX(m_intakeRollers, m_intakeRollersSim); } From bc11129be8f316638fd6264bdae0d0cb7edb7583 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 17:11:55 -0400 Subject: [PATCH 29/40] =?UTF-8?q?an=20iq=20too=20low=3F=20made=20an=20inta?= =?UTF-8?q?ke=20arm=20follower=20instead=20of=20a=20intake=20rollers=20fol?= =?UTF-8?q?lower=20=F0=9F=98=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/Constants.java | 81 +++++++++---------- .../java/frc/robot/subsystems/Intake.java | 48 +++++------ .../frc/robot/subsystems/shooter/Shooter.java | 10 +-- 3 files changed, 69 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc3a4c01..e0271e2a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -402,84 +402,83 @@ public static class IntakeK { public static final AngularVelocity kIntakeRollersShootRPS = kIntakeRollersMaxRPS.times(0.2); /* IDS */ - public static final int kIntakeArmA_CANID = 40; - public static final int kIntakeArmB_CANID = 41; - public static final int kIntakeRollersCANID = 42; + public static final int kIntakeArmCANID = 40; + public static final int kIntakeRollersA_CANID = 41; + public static final int kIntakeRollersB_CANID = 42; /* CONFIGS */ - // IntakeArm Motors - private static final CurrentLimitsConfigs kIntakeArmACurrentLimitConfigs = new CurrentLimitsConfigs() + // IntakeArm Motor + private static final CurrentLimitsConfigs kIntakeArmCurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(20) .withSupplyCurrentLimit(20) .withSupplyCurrentLowerLimit(20) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true); - private static final Slot0Configs kIntakeArmASlot0Configs = new Slot0Configs() + private static final Slot0Configs kIntakeArmSlot0Configs = new Slot0Configs() .withKS(1.5) .withKV(0) .withKA(0) .withKP(50) .withKI(0) .withKD(0); - public static final MotorOutputConfigs kIntakeArmAMotorOutputConfigs = new MotorOutputConfigs() + public static final MotorOutputConfigs kIntakeArmMotorOutputConfigs = new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) .withInverted(InvertedValue.Clockwise_Positive); - private static final MotionMagicConfigs kIntakeArmAMotionMagicConfigs = new MotionMagicConfigs() + private static final MotionMagicConfigs kIntakeArmMotionMagicConfigs = new MotionMagicConfigs() .withMotionMagicCruiseVelocity(0.75) .withMotionMagicAcceleration(10) .withMotionMagicJerk(0); - public static final FeedbackConfigs kIntakeArmAFeedbackConfigs = new FeedbackConfigs() + public static final FeedbackConfigs kIntakeArmFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kIntakeArmGearing); - private static final VoltageConfigs kIntakeArmAVoltageConfigs = new VoltageConfigs() + private static final VoltageConfigs kIntakeArmVoltageConfigs = new VoltageConfigs() .withPeakForwardVoltage(12) .withPeakReverseVoltage(-12); - public static final TalonFXConfiguration kIntakeArmAConfiguration = new TalonFXConfiguration() - .withCurrentLimits(kIntakeArmACurrentLimitConfigs) - .withSlot0(kIntakeArmASlot0Configs) - .withMotorOutput(kIntakeArmAMotorOutputConfigs) - .withMotionMagic(kIntakeArmAMotionMagicConfigs) - .withVoltage(kIntakeArmAVoltageConfigs) - .withFeedback(kIntakeArmAFeedbackConfigs); - - public static final MotorOutputConfigs kIntakeArmBMotorOutputConfigs = new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(InvertedValue.CounterClockwise_Positive); - public static final TalonFXConfiguration kIntakeArmBConfiguration = new TalonFXConfiguration() - .withCurrentLimits(kIntakeArmACurrentLimitConfigs) - .withSlot0(kIntakeArmASlot0Configs) - .withMotorOutput(kIntakeArmBMotorOutputConfigs) - .withMotionMagic(kIntakeArmAMotionMagicConfigs) - .withVoltage(kIntakeArmAVoltageConfigs) - .withFeedback(kIntakeArmAFeedbackConfigs); - - // IntakeRollers Motor - private static final CurrentLimitsConfigs kIntakeRollersCurrentLimitConfigs = new CurrentLimitsConfigs() + public static final TalonFXConfiguration kIntakeArmConfiguration = new TalonFXConfiguration() + .withCurrentLimits(kIntakeArmCurrentLimitConfigs) + .withSlot0(kIntakeArmSlot0Configs) + .withMotorOutput(kIntakeArmMotorOutputConfigs) + .withMotionMagic(kIntakeArmMotionMagicConfigs) + .withVoltage(kIntakeArmVoltageConfigs) + .withFeedback(kIntakeArmFeedbackConfigs); + + // IntakeRollers Motors + private static final CurrentLimitsConfigs kIntakeRollersACurrentLimitConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(85) .withSupplyCurrentLimit(50) .withSupplyCurrentLowerTime(0) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true); - private static final Slot0Configs kIntakeRollersSlot0Configs = new Slot0Configs() + private static final Slot0Configs kIntakeRollersASlot0Configs = new Slot0Configs() .withKS(0) .withKV(0.488599348534) // 0.19543973941 .withKA(0) .withKP(0) .withKI(0) .withKD(0); - public static final MotorOutputConfigs kIntakeRollersMotorOutputConfigs = new MotorOutputConfigs() + public static final MotorOutputConfigs kIntakeRollersAMotorOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); - public static final FeedbackConfigs kIntakeRollersFeedbackConfigs = new FeedbackConfigs() + public static final FeedbackConfigs kIntakeRollersAFeedbackConfigs = new FeedbackConfigs() .withSensorToMechanismRatio(kIntakeRollersGearing); - private static final VoltageConfigs kIntakeRollersVoltageConfigs = new VoltageConfigs() + private static final VoltageConfigs kIntakeRollersAVoltageConfigs = new VoltageConfigs() .withPeakForwardVoltage(12) //1.2 .withPeakReverseVoltage(-12); //-1.2 - public static final TalonFXConfiguration kIntakeRollersConfiguration = new TalonFXConfiguration() - .withCurrentLimits(kIntakeRollersCurrentLimitConfigs) - .withSlot0(kIntakeRollersSlot0Configs) - .withMotorOutput(kIntakeRollersMotorOutputConfigs) - .withFeedback(kIntakeRollersFeedbackConfigs) - .withVoltage(kIntakeRollersVoltageConfigs); + public static final TalonFXConfiguration kIntakeRollersAConfiguration = new TalonFXConfiguration() + .withCurrentLimits(kIntakeRollersACurrentLimitConfigs) + .withSlot0(kIntakeRollersASlot0Configs) + .withMotorOutput(kIntakeRollersAMotorOutputConfigs) + .withFeedback(kIntakeRollersAFeedbackConfigs) + .withVoltage(kIntakeRollersAVoltageConfigs); + + public static final MotorOutputConfigs kIntakeRollersBMotorOutputConfigs = new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + public static final TalonFXConfiguration kIntakeRollersBConfiguration = new TalonFXConfiguration() + .withCurrentLimits(kIntakeRollersACurrentLimitConfigs) + .withSlot0(kIntakeRollersASlot0Configs) + .withMotorOutput(kIntakeRollersBMotorOutputConfigs) + .withFeedback(kIntakeRollersAFeedbackConfigs) + .withVoltage(kIntakeRollersAVoltageConfigs); } public static class IndexerK { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 23796451..d1088a07 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -39,16 +39,16 @@ public class Intake extends SubsystemBase { /* CLASS VARIABLES */ //---MOTORS + CONTROL REQUESTS - private final TalonFX m_intakeArmA = new TalonFX(kIntakeArmA_CANID); //x60Foc - private final TalonFX m_intakeArmB = new TalonFX(kIntakeArmB_CANID); //x60Foc + private final TalonFX m_intakeArm = new TalonFX(kIntakeArmCANID); //x60Foc - private final TalonFX m_intakeRollers = new TalonFX(kIntakeRollersCANID); //x60Foc + private final TalonFX m_intakeRollersA = new TalonFX(kIntakeRollersA_CANID); //x60Foc + private final TalonFX m_intakeRollersB = new TalonFX(kIntakeRollersB_CANID); //x60Foc private DynamicMotionMagicVoltage m_MMVReq = new DynamicMotionMagicVoltage(0, 1, 1).withEnableFOC(true); private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(false); - private BooleanSupplier m_currentSpike = () -> m_intakeArmA.getStatorCurrent().getValueAsDouble() > 5.0; - private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArmA.getVelocity().getValueAsDouble()) < 0.005; + private BooleanSupplier m_currentSpike = () -> m_intakeArm.getStatorCurrent().getValueAsDouble() > 5.0; + private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArm.getVelocity().getValueAsDouble()) < 0.005; private VoltageOut m_intakeArmZeroingReq = new VoltageOut(0); @@ -89,12 +89,12 @@ public class Intake extends SubsystemBase { /* CONSTRUCTOR */ public Intake() { - m_intakeArmA.getConfigurator().apply(kIntakeArmAConfiguration); - m_intakeArmB.getConfigurator().apply(kIntakeArmBConfiguration); + m_intakeArm.getConfigurator().apply(kIntakeArmConfiguration); - m_intakeRollers.getConfigurator().apply(kIntakeRollersConfiguration); + m_intakeRollersA.getConfigurator().apply(kIntakeRollersAConfiguration); + m_intakeRollersB.getConfigurator().apply(kIntakeRollersBConfiguration); - m_intakeArmB.setControl(new Follower(kIntakeArmA_CANID, MotorAlignmentValue.Opposed)); + m_intakeRollersB.setControl(new Follower(kIntakeRollersA_CANID, MotorAlignmentValue.Opposed)); if (Robot.isReal()) { setDefaultCommand(intakeArmCurrentSenseHoming()); @@ -105,8 +105,8 @@ public Intake() { } private void initSim() { - WaltMotorSim.initSimFX(m_intakeArmA, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX44); - WaltMotorSim.initSimFX(m_intakeRollers, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX60); + WaltMotorSim.initSimFX(m_intakeArm, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX44); + WaltMotorSim.initSimFX(m_intakeRollersA, ChassisReference.CounterClockwise_Positive, TalonFXSimState.MotorType.KrakenX60); } /* COMMANDS */ @@ -123,11 +123,11 @@ public Command setIntakeArmPosCmd(Angle rots, double RPSPS) { } public void setIntakeArmPos(Angle rots, double RPSPS) { - m_intakeArmA.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS)); + m_intakeArm.setControl(m_MMVReq.withPosition(rots).withAcceleration(RPSPS)); } public boolean isIntakeArmAtDest() { - return m_intakeArmA.getMotionMagicAtTarget().getValue(); + return m_intakeArm.getMotionMagicAtTarget().getValue(); } public Command shimmy() { @@ -146,12 +146,12 @@ public Command shimmy() { } public void setIntakeArmNeutralMode(NeutralModeValue value) { - m_intakeArmA.setNeutralMode(value); + m_intakeArm.setNeutralMode(value); } //for TestingDashboard public Command setIntakeArmPos(DoubleSubscriber sub_rots) { - return run(() -> m_intakeArmA.setControl(m_MMVReq.withPosition(Rotations.of(sub_rots.get())))); + return run(() -> m_intakeArm.setControl(m_MMVReq.withPosition(Rotations.of(sub_rots.get())))); } public Command startIntakeRollers() { @@ -163,7 +163,7 @@ public Command stopIntakeRollers() { } public void setIntakeRollersVelocity(double volts) { - m_intakeRollers.setControl(m_VVReq.withOutput(volts)); + m_intakeRollersA.setControl(m_VVReq.withOutput(volts)); } public Command setIntakeRollersVelocityCmd(double volts) { @@ -174,7 +174,7 @@ public Command setIntakeRollersVelocityCmd(double volts) { public Command intakeArmHome() { return Commands.parallel( Commands.sequence( - runOnce(() -> m_intakeArmA.setPosition(0)), + runOnce(() -> m_intakeArm.setPosition(0)), runOnce(() -> m_isIntakeArmHomed = true), runOnce(() -> log_isIntakeArmHomed.accept(m_isIntakeArmHomed)), @@ -186,7 +186,7 @@ public Command intakeArmHome() { public Command intakeArmCurrentSenseHoming() { Runnable init = () -> { - m_intakeArmA.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); + m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(-3.25)); m_isIntakeArmHomed = false; log_isIntakeArmHomed.accept(m_isIntakeArmHomed); @@ -195,8 +195,8 @@ public Command intakeArmCurrentSenseHoming() { Runnable execute = () -> {}; Consumer onEnd = (Boolean interrupted) -> { - m_intakeArmA.setControl(m_intakeArmZeroingReq.withOutput(0)); - m_intakeArmA.setPosition(0); + m_intakeArm.setControl(m_intakeArmZeroingReq.withOutput(0)); + m_intakeArm.setPosition(0); removeDefaultCommand(); setIntakeArmPosCmd(IntakeArmPosition.RETRACTED); m_isIntakeArmHomed = true; @@ -215,14 +215,14 @@ public Command intakeArmCurrentSenseHoming() { public void periodic() { log_targetIntakeArmRots.accept(m_MMVReq.Position); log_targetIntakeRollersRPS.accept(m_VVReq.Output); - log_intakeRollersRPS.accept(m_intakeRollers.getVelocity().getValueAsDouble()); - log_intakeArmRots.accept(m_intakeArmA.getPosition().getValueAsDouble()); + log_intakeRollersRPS.accept(m_intakeRollersA.getVelocity().getValueAsDouble()); + log_intakeArmRots.accept(m_intakeArm.getPosition().getValueAsDouble()); } @Override public void simulationPeriodic() { - WaltMotorSim.updateSimFX(m_intakeArmA, m_intakeArmSim); - WaltMotorSim.updateSimFX(m_intakeRollers, m_intakeRollersSim); + WaltMotorSim.updateSimFX(m_intakeArm, m_intakeArmSim); + WaltMotorSim.updateSimFX(m_intakeRollersA, m_intakeRollersSim); } /* ENUMS */ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5241de09..0dc48f1b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -308,13 +308,13 @@ public void periodic() { // set outputs var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { - m_turret.setTurretPos(m_turret.getTurretLockAngle()); + // m_turret.setTurretPos(m_turret.getTurretLockAngle()); m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { - m_turret.setTurretPos(Rotations.of(-0.250)); + // m_turret.setTurretPos(Rotations.of(-0.250)); } else { - m_turret.setTurretPos(turretReference, turretVelocityFF); + // m_turret.setTurretPos(turretReference, turretVelocityFF); m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); if (true) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; @@ -328,10 +328,10 @@ public void periodic() { double hoodReference = calcData.hoodReferenceRots(); if (m_turret.getTurretLocked()) { - m_hood.setHoodPos(kHoodLockRots_double); + // m_hood.setHoodPos(kHoodLockRots_double); } else { if (!m_turret.getHoldTurretAtIntake()) { - m_hood.setHoodPos(hoodReference); + // m_hood.setHoodPos(hoodReference); } } } From ae6ae1bde11bf2d860897ab35354b672a4ba0930 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 17:54:05 -0400 Subject: [PATCH 30/40] made rollers foc --- src/main/java/frc/robot/subsystems/Intake.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d1088a07..e0736e5b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -45,7 +45,7 @@ public class Intake extends SubsystemBase { private final TalonFX m_intakeRollersB = new TalonFX(kIntakeRollersB_CANID); //x60Foc private DynamicMotionMagicVoltage m_MMVReq = new DynamicMotionMagicVoltage(0, 1, 1).withEnableFOC(true); - private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(false); + private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(true); private BooleanSupplier m_currentSpike = () -> m_intakeArm.getStatorCurrent().getValueAsDouble() > 5.0; private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArm.getVelocity().getValueAsDouble()) < 0.005; @@ -62,20 +62,20 @@ public class Intake extends SubsystemBase { /* SIM OBJECTS */ private final DCMotorSim m_intakeArmSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), + DCMotor.getKrakenX60Foc(1), kIntakeArmMOI, kIntakeArmGearing ), - DCMotor.getKrakenX44Foc(1) + DCMotor.getKrakenX60Foc(1) ); private final DCMotorSim m_intakeRollersSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), + DCMotor.getKrakenX60Foc(2), kIntakeRollersMOI, kIntakeRollersGearing ), - DCMotor.getKrakenX44Foc(1) // returns gearbox + DCMotor.getKrakenX60Foc(2) // returns gearbox ); /* LOGGERS */ From 625a27110b922d139cdf875d3007bf5aab184f93 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 19:46:45 -0400 Subject: [PATCH 31/40] new intake testing --- src/main/java/frc/robot/Robot.java | 19 ++++++------------- .../frc/robot/subsystems/shooter/Shooter.java | 8 ++++---- 2 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66efc236..80b436bd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -111,9 +111,6 @@ public class Robot extends TimedRobot { private Trigger trg_manipOverride = m_manipulator.b(); //---COMMAND SEQUENCE TRIGGERS - - // private Trigger trg_activateIntake = m_manipulator.a().and(trg_manipOverride.negate()); - // private Trigger trg_safeIntake = m_manipulator.x().and(trg_manipOverride.negate()); private Trigger trg_intake = m_manipulator.rightTrigger().and(trg_manipOverride.negate()); private Trigger trg_retractIntake = m_manipulator.rightBumper().and(trg_manipOverride.negate()); @@ -128,11 +125,6 @@ public class Robot extends TimedRobot { private Trigger trg_unjam = m_driver.rightBumper(); - - /* LOGGERS */ - private final DoubleLogger log_stickDesiredFieldX = WaltLogger.logDouble("Swerve", "stick desired teleop x"); - private final DoubleLogger log_stickDesiredFieldY = WaltLogger.logDouble("Swerve", "stick desired teleop y"); - private final DoubleLogger log_stickDesiredFieldZRot = WaltLogger.logDouble("Swerve", "stick desired teleop z rot"); private final DoubleLogger log_miniPCCurrent = WaltLogger.logDouble(kLogTab, "MiniPC current"); private final Pose2dLogger log_robotPose = WaltLogger.logPose2d("Drive", "Pose"); @@ -156,7 +148,7 @@ public class Robot extends TimedRobot { public Robot() { configureBindings(); // configureTestBindings(); //this should be commented out during competition matches - configureTestingDashboard(); + // configureTestingDashboard(); lastGotTagMsmtTimer.start(); @@ -271,8 +263,8 @@ private void configureBindings() { //Shooting // NORMAL FIXED SHOT - trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); - // trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above + // trg_shoot.whileTrue(m_superstructure.activateOuttake(() -> RotationsPerSecond.of(TestingDashboard.sub_shooterVelocityRPS.get()))); + trg_shoot.and(() -> m_shooter.m_turret.atPosition()).whileTrue(m_superstructure.activateOuttakeShotCalc()); //comment out for LERP with above // m_driver.y().onTrue(m_shooter.driverRPSAlter(true)); // m_driver.a().onTrue(m_shooter.driverRPSAlter(false)); @@ -288,6 +280,8 @@ private void configureBindings() { m_superstructure.intake(() -> true) ); + trg_retractIntake.onTrue(m_intake.setIntakeArmPosCmd(IntakeArmPosition.RETRACTED)); + trg_emergencyBarf.whileTrue( m_superstructure.emergencyBarf() ); @@ -320,7 +314,6 @@ private void configureBindings() { m_driver.povRight().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() + Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); m_driver.povLeft().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() - Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); - // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT()); // trg_optimalPrefireTime.whileTrue( // Commands.run(() -> setBothRumble(RumbleType.kBothRumble, 0.5)).finallyDo(() -> setBothRumble(RumbleType.kBothRumble, 0)) @@ -442,7 +435,7 @@ public void disabledPeriodic() { m_disableChangeDelayTimer.stop(); m_disableChangeDelayTimer.reset(); m_shooter.m_turret.setTurretNeutralMode(NeutralModeValue.Coast); - // m_intake.setIntakeArmNeutralMode(NeutralModeValue.Coast); + m_intake.setIntakeArmNeutralMode(NeutralModeValue.Coast); m_shooter.m_hood.setHoodNeutralMode(NeutralModeValue.Coast); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0b8096dd..9d0d151e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -304,13 +304,13 @@ public void periodic() { var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { m_turret.setTurretPos(m_turret.getTurretLockAngle()); - // m_calcFlywheelVelocityRotPerSec = kShooterRPSd; + m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { // m_turret.setTurretPos(Rotations.of(-0.250)); } else { m_turret.setTurretPos(turretReference, turretVelocityFF); - // m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); + m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); if (false) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; m_calcFlywheelVelocityRotPerSec = MathUtil.clamp(m_calcFlywheelVelocityRotPerSec, 0, kShooterMaxRPSd); //clamp here or clamp only when setShooterVel is called? @@ -323,10 +323,10 @@ public void periodic() { double hoodReference = calcData.hoodReferenceRots(); if (m_turret.getTurretLocked()) { - // m_hood.setHoodPos(kHoodLockRots_double); + m_hood.setHoodPos(kHoodLockRots_double); } else { if (!m_turret.getHoldTurretAtIntake()) { - // m_hood.setHoodPos(hoodReference); + m_hood.setHoodPos(hoodReference); } } } From f28d55ac0ecc89c4b85858133ef9873aacde4d4f Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 22:06:12 -0400 Subject: [PATCH 32/40] new lerp points --- src/main/java/frc/robot/Robot.java | 12 ++++++------ .../frc/robot/subsystems/shooter/ShotCalculator.java | 12 ++++++++++++ 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 80b436bd..22e81a94 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -306,13 +306,13 @@ private void configureBindings() { // m_driver.y().and(trg_driverOverride).onTrue(m_shooter.turretHomingCmd(false)); //false? im not sure - // m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); - // m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); + m_driver.povDown().onTrue(m_shooter.m_turret.setTurretLockCmd(false)); + m_driver.povRight().onTrue(m_shooter.m_turret.setTurretLockCmd(true)); - m_driver.povDown().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() - Inches.of(20).magnitude()), 0.001)); - m_driver.povUp().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() + Inches.of(20).magnitude()), 0.001)); - m_driver.povRight().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() + Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); - m_driver.povLeft().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() - Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); + // m_driver.povDown().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() - Inches.of(20).magnitude()), 0.001)); + // m_driver.povUp().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY() + Inches.of(20).magnitude()), 0.001)); + // m_driver.povRight().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() + Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); + // m_driver.povLeft().onTrue(m_drivetrain.roboToTranslation(new Translation2d(m_drivetrain.getState().Pose.getX() - Inches.of(20).magnitude(), m_drivetrain.getState().Pose.getY()), 0.001)); // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT()); // trg_optimalPrefireTime.whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 347a1020..55a4d8b6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -58,6 +58,7 @@ public class ShotCalculator { addLerpPoint(5.515, 59.50, 0.87, 1.30); addLerpPoint(5.412, 58.50, 0.86, 1.27); addLerpPoint(5.303, 58.47, 0.90, 1.65); +//add 5.15 addLerpPoint(5.060, 57.00, 0.85, 1.26); addLerpPoint(4.910, 56.00, 0.81, 1.17); addLerpPoint(4.874, 55.50, 0.80, 1.29); @@ -69,6 +70,17 @@ public class ShotCalculator { addLerpPoint(4.000, 55.46, 0.42, 1.37); addLerpPoint(3.944, 54.90, 0.37, 1.33); addLerpPoint(3.823, 55.20, 0.20, 1.47); +//add 3.6 +//add 3.4 + addLerpPoint(3.201, 52.58, 0.10, 1.27); + addLerpPoint(3.055, 51.58, 0.10, 1.41); + addLerpPoint(2.734, 50.68, 0.10, 1.16); +//add 2.5 ish + addLerpPoint(2.318, 48.12, 0.10, 1.12); +//add 2.something here + addLerpPoint(2.014, 48.12, 0.00, 1.22); + addLerpPoint(1.562, 46.12, 0.00, 1.36); + addLerpPoint(1.072, 40.04, 0.00, 1.04); } public static void addLerpPoint(double distanceToTarget, double shooterRPS, double hoodRots, double timeOfFlight) { From 7e15ea078554778113d74b4bed58de325cb0aeb8 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sun, 29 Mar 2026 22:06:30 -0400 Subject: [PATCH 33/40] new shimmy shoutout saarth also i love sohan for sotm --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/Intake.java | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc679105..b143e978 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -427,6 +427,7 @@ public static class IntakeK { public static final AngularVelocity kIntakeRollersMaxRPS = MotorK.kX60FOCMaxVelocity.div(kIntakeRollersGearing); public static final AngularVelocity kIntakeRollersShootRPS = kIntakeRollersMaxRPS.times(0.2); + public static final AngularVelocity kIntakeRollersShimmyRPS = kIntakeRollersMaxRPS.times(0.2); /* IDS */ public static final int kIntakeArmCANID = 40; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index fd69025d..89cdaf37 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -111,11 +111,11 @@ private void initSim() { /* COMMANDS */ public void setIntakeArmPos(IntakeArmPosition rots) { - setIntakeArmPos(rots.rots, rots == IntakeArmPosition.RETRACTED ? 4 : 2); + setIntakeArmPos(rots.rots, rots == IntakeArmPosition.RETRACTED ? 6 : 3); } public Command setIntakeArmPosCmd(IntakeArmPosition rots) { - return setIntakeArmPosCmd(rots.rots, rots == IntakeArmPosition.RETRACTED ? 4 : 2); + return setIntakeArmPosCmd(rots.rots, rots == IntakeArmPosition.RETRACTED ? 6 : 3); } public Command setIntakeArmPosCmd(Angle rots, double RPSPS) { @@ -137,12 +137,15 @@ public Command shimmy() { Commands.waitUntil(intakeArmAtDest); return Commands.repeatingSequence( - setIntakeRollersVelocityCmd(0), + setIntakeRollersVelocityCmd(kIntakeRollersShimmyRPS.baseUnitMagnitude()), setIntakeArmPosCmd(IntakeArmPosition.SHIMMY), Commands.waitUntil(intakeArmAtDest), setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), Commands.waitUntil(intakeArmAtDest) - ).finallyDo(() -> setIntakeArmPosCmd(IntakeArmPosition.SAFE)); + ).finallyDo(() -> { + setIntakeArmPosCmd(IntakeArmPosition.SAFE); + setIntakeRollersVelocityCmd(0); + }); } public void setIntakeArmNeutralMode(NeutralModeValue value) { From 63b717edc02f72990cb177af4ed7309a5864dfe3 Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Mon, 30 Mar 2026 17:18:28 -0400 Subject: [PATCH 34/40] speedy speed boy --- .../java/frc/robot/subsystems/Indexer.java | 18 ++-- .../java/frc/robot/subsystems/Swerve.java | 7 +- .../frc/robot/subsystems/shooter/Hood.java | 13 +-- .../frc/robot/subsystems/shooter/Shooter.java | 20 ++-- .../robot/subsystems/shooter/ShooterCalc.java | 100 +++++++++--------- .../frc/robot/subsystems/shooter/Turret.java | 39 ++++--- 6 files changed, 104 insertions(+), 93 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 9ca69b87..ba42dc33 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -121,23 +121,21 @@ public void stopTunnel() { setTunnelVelocity(RotationsPerSecond.zero()); } - public boolean isTunnelSpunUp() { + private void refreshTunnelState() { + m_tunnelVelocityRotPerSec = m_tunnel.getVelocity().getValueAsDouble(); + log_tunnelRPS.accept(m_tunnelVelocityRotPerSec); + sig_tunnelCLErr.refresh(); log_tunnelClosedLoopError.accept(sig_tunnelCLErr.getValueAsDouble()); - - boolean isNear = sig_tunnelCLErr.isNear(0, 30); - - log_isTunnelSpunUp.accept(isNear); - return isNear; + m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 30); + log_isTunnelSpunUp.accept(m_isTunnelSpunUp); } - public boolean getTunnelSpunUp() { + public boolean isTunnelSpunUp() { return m_isTunnelSpunUp; } public double getTunnelVelocityRotPerSec() { - m_tunnelVelocityRotPerSec = m_tunnel.getVelocity().getValueAsDouble(); - return m_tunnelVelocityRotPerSec; } @@ -175,7 +173,7 @@ public Command setTunnelVelocityCmd(DoubleSubscriber sub_RPS) { @Override public void periodic() { log_spindexerRPS.accept(m_spindexer.getVelocity().getValueAsDouble()); - log_tunnelRPS.accept(m_tunnel.getVelocity().getValueAsDouble()); + refreshTunnelState(); } @Override diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 86f994d1..1bf826d5 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -20,6 +20,7 @@ import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -438,7 +439,7 @@ public Command roboToRotation(Rotation2d desRotation, double tolerance) { } public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { - return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + return Math.abs(MathUtil.angleModulus(curRotation.getRadians() - desRotation.getRadians())) <= tolerance; } /** @@ -457,8 +458,8 @@ public Command roboToTranslation(Translation2d desTranslation, double tolerance) public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) { return Math.hypot( - desTranslation.getMeasureX().minus(curTranslation.getMeasureX()).baseUnitMagnitude(), - desTranslation.getMeasureY().minus(curTranslation.getMeasureY()).baseUnitMagnitude() + desTranslation.getX() - curTranslation.getX(), + desTranslation.getY() - curTranslation.getY() ) <= tolerance; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index edeb43c9..b93135fa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -26,6 +26,8 @@ import frc.util.WaltLogger.DoubleLogger; public class Hood extends SubsystemBase { + private static final double kAbsoluteToPhysicalAngleRatio = + (kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double) / (360.0 * (kHoodMaxRots_double - kHoodMinRots_double)); private final TalonFXS m_hood = new TalonFXS(kHoodCANID, kShooterBus); private final PositionVoltage m_hoodPVRequest = new PositionVoltage(0).withEnableFOC(false); private final VoltageOut m_hoodZeroReq = new VoltageOut(0); @@ -68,10 +70,8 @@ public Command setHoodPositionCmd(DoubleSubscriber sub_rots) { return run(() -> setHoodPos(sub_rots.get())); } - private double getHoodAngleDeg() { - double absoluteToPhysicalAngleRatio = (kPhysicalHoodMaxPosition_double - kPhysicalHoodMinPosition_double)/(360 * (kHoodMaxRots_double - kHoodMinRots_double)); - - return kPhysicalHoodMinPosition_double + (m_hood.getPosition().getValue().in(Degrees) - (kHoodMinRots_double * 360)) * (absoluteToPhysicalAngleRatio); + private static double getHoodAngleDeg(double posRots) { + return kPhysicalHoodMinPosition_double + (posRots * 360.0 - kHoodMinRots_double * 360.0) * kAbsoluteToPhysicalAngleRatio; } public boolean isHoodHomed() { @@ -114,7 +114,8 @@ public void setHoodNeutralMode(NeutralModeValue value) { @Override public void periodic() { - log_hoodCurrentPos.accept(getHoodAngleDeg()); - log_hoodPositionDeg.accept(m_hood.getPosition().getValue().in(Degrees)); + double posRots = m_hood.getPosition().getValueAsDouble(); + log_hoodCurrentPos.accept(getHoodAngleDeg(posRots)); + log_hoodPositionDeg.accept(posRots * 360.0); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9d0d151e..819277ad 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -74,7 +74,7 @@ public class Shooter extends SubsystemBase { public final Turret m_turret; // thread copde - private double m_latestTurretPositionRots = 0.0; + private volatile double m_latestTurretPositionRots = 0.0; private final ShooterCalc m_shooterCalc; private double m_calcTurretRots = 0.0; @@ -197,17 +197,15 @@ public Command setShooterVelocityCmd(DoubleSubscriber sub_RPS) { return run(() -> setShooterVelocity(RotationsPerSecond.of(sub_RPS.get()))); } - public boolean isShooterSpunUp() { + private void refreshShooterSpunUp() { sig_shooterCLErr.refresh(); log_shooterClosedLoopError.accept(sig_shooterCLErr.getValueAsDouble()); - - boolean isNear = sig_shooterCLErr.isNear(0, 3); - - m_isShooterSpunUp = isNear; - log_spunUp.accept(isNear); - return isNear; + m_isShooterSpunUp = sig_shooterCLErr.isNear(0, 3); } + public boolean isShooterSpunUp() { + return m_isShooterSpunUp; + } /* GETTERS */ public double getShooterVelocityRotPerSec() { @@ -303,7 +301,7 @@ public void periodic() { // set outputs var turretVelocityFF = calcData.turretCalcDetails().turretVelocityFF(); if (m_turret.getTurretLocked()) { - m_turret.setTurretPos(m_turret.getTurretLockAngle()); + m_turret.setTurretPos(m_turret.getTurretLockAngleRots(), 0.0); m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { @@ -331,10 +329,12 @@ public void periodic() { } } + refreshShooterSpunUp(); + log_turretPositionRobotRelativeRots.accept(kDriverRPSIncreaseD); log_shooterVelocityRPS.accept(m_latestFlywheelVelocityRotPerSec); log_turretPositionRots.accept(m_latestTurretPositionRots); - log_spunUp.accept(isShooterSpunUp()); + log_spunUp.accept(m_isShooterSpunUp); log_calcFlywheelVelocity.accept(m_calcFlywheelVelocityRotPerSec); log_calcTurretPos.accept(m_calcTurretRots); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java index af07ffc6..e8c66265 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterCalc.java @@ -52,6 +52,11 @@ public class ShooterCalc { // private static final Pose3dLogger log_turret + // Precomputed doubles for calculateTarget zone checks + private static final double kRedHubCenterX = AllianceZoneUtil.redHubCenter.getX(); + private static final double kBlueHubCenterX = AllianceZoneUtil.blueHubCenter.getX(); + private static final double kCenterFieldYM = AllianceZoneUtil.centerField_y_pos.baseUnitMagnitude(); + // Precomputed doubles for hot-path unit conversions private static final double kTurretMinRotsD = kTurretMinRots.in(Rotations); private static final double kTurretMinRotsMagnitudeD = kTurretMinRots.magnitude(); @@ -62,9 +67,9 @@ public class ShooterCalc { private final AzimuthCalcDetails kEmptyAzimuthCalcDetails = new AzimuthCalcDetails(0, new Pose3d(), new Pose3d(), 0, 0); private final ShotCalcOutputs kEmptyShotCalcOutputs = new ShotCalcOutputs(kEmptyAzimuthCalcDetails, kEmptyShotData, 0, 0, 0); - private boolean m_useStaticShot = true; - private Translation3d m_aimTarget = Translation3d.kZero; - private ShotCalcOutputs m_shotCalcOutputs = kEmptyShotCalcOutputs; + private volatile boolean m_useStaticShot = true; + private volatile Translation3d m_aimTarget = Translation3d.kZero; + private volatile ShotCalcOutputs m_shotCalcOutputs = kEmptyShotCalcOutputs; private final Notifier m_notifier = new Notifier(this::calcCallback); private final Timer m_calcTimer = new Timer(); @@ -124,14 +129,15 @@ private Translation3d calculateTarget(Pose2d robotPose) { boolean isRed = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; Translation3d theTarget = FieldConstants.Hub.blueInnerCenterPoint; - boolean robotPastOurZoneX = isRed ? robotPose.getMeasureX().lt(AllianceZoneUtil.redHubCenter.getMeasureX()) - : robotPose.getMeasureX().gt(AllianceZoneUtil.blueHubCenter.getMeasureX()); + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + + boolean robotPastOurZoneX = isRed ? robotX < kRedHubCenterX : robotX > kBlueHubCenterX; if (robotPastOurZoneX) { - Translation3d leftPassPoseShifted = new Translation3d(ShooterK.kPassingXAsDouble, MathUtil.clamp(FieldConstants.fieldWidth / 2 + robotPose.getY(), FieldConstants.fieldWidth / 2 + 1, FieldConstants.fieldWidth - 1), 0); - Translation3d rightPassPoseShifted = new Translation3d(ShooterK.kPassingXAsDouble, MathUtil.clamp(robotPose.getY() - FieldConstants.fieldWidth / 2, 1, FieldConstants.fieldWidth / 2 - 1), 0); - boolean robotLeftOfCenter = isRed ? robotPose.getMeasureY().lt(AllianceZoneUtil.centerField_y_pos) - : robotPose.getMeasureY().gt(AllianceZoneUtil.centerField_y_pos); + Translation3d leftPassPoseShifted = new Translation3d(ShooterK.kPassingXAsDouble, MathUtil.clamp(FieldConstants.fieldWidth / 2 + robotY, FieldConstants.fieldWidth / 2 + 1, FieldConstants.fieldWidth - 1), 0); + Translation3d rightPassPoseShifted = new Translation3d(ShooterK.kPassingXAsDouble, MathUtil.clamp(robotY - FieldConstants.fieldWidth / 2, 1, FieldConstants.fieldWidth / 2 - 1), 0); + boolean robotLeftOfCenter = isRed ? robotY < kCenterFieldYM : robotY > kCenterFieldYM; theTarget = robotLeftOfCenter ? leftPassPoseShifted : rightPassPoseShifted; } log_ballTrajectory.accept(new Translation3d[]{new Translation3d(FieldConstants.fieldLength - robotPose.getX(), FieldConstants.fieldWidth - robotPose.getY(), 0), theTarget}); @@ -154,50 +160,51 @@ public record AzimuthCalcDetails(double turretReferenceRots, Pose3d desiredAimPo * of kTurretMaxAngle * and kTurretMinAngle */ + // Precomputed for calcAzimuth logging + private static final double kTurretOffsetZ_m = kTurretTransform.getTranslation().getZ(); + private static final Rotation3d kTurretRealPoseRotation = + new Rotation3d(0, 0, -(kTurretAngleOffset.plus(Rotation2d.kPi)).getRadians()); + public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotPose, double turretHeading, ChassisSpeeds fieldSpeeds) { - Pose3d turretPose = new Pose3d(robotPose).transformBy(kTurretTransform); - - // Convert once; reused below in both snapback and current-aim logging + // Compute turret pivot position and zero direction with raw doubles + // (eliminates Pose3d(robotPose).transformBy() + Rotation2d allocations) + double headingRad = robotPose.getRotation().getRadians(); + double cosH = Math.cos(headingRad); + double sinH = Math.sin(headingRad); + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + double turretX = robotX + kTurretOffsetX_m * cosH - kTurretOffsetY_m * sinH; + double turretY = robotY + kTurretOffsetX_m * sinH + kTurretOffsetY_m * cosH; + double turretZeroFieldDirRad = headingRad + kTurretAngleOffsetRad; + + // Build Translation3d once for logging Pose3d objects + Translation3d turretTranslation = new Translation3d(turretX, turretY, kTurretOffsetZ_m); + double turretHeadingRots = turretHeading; - // Field-space pose: turret pivot in field space, rotated by (turret zero field direction + encoder position) - Pose3d turretRobotPose = new Pose3d( - turretPose.getTranslation(), - new Rotation3d(0, 0, turretPose.getRotation().toRotation2d().getRadians() + turretHeadingRots * (2 * Math.PI))); + Pose3d turretRobotPose = new Pose3d(turretTranslation, + new Rotation3d(0, 0, turretZeroFieldDirRad + turretHeadingRots * (2 * Math.PI))); log_turretRobotPose.accept(turretRobotPose); - /* Calculation Zone */ - // turret pivot location in field space (no extra rotateBy — that's for - // visualization only) - Pose3d turretRealPose = new Pose3d(turretPose.getTranslation(), new Rotation3d(0, 0, (-kTurretAngleOffset.plus(Rotation2d.fromDegrees(180)).getRadians()))); - Translation3d turretTranslation = turretPose.getTranslation(); + Pose3d turretRealPose = new Pose3d(turretTranslation, kTurretRealPoseRotation); log_turretFieldPose.accept(turretRealPose); - // vector from turret pivot to target in field space - Translation3d distance = target.minus(turretTranslation); - - // field-frame yaw to target, converted to turret-relative by subtracting - // turret's zero direction - // kTurretYawOffsetRad = robot heading + kTurretAngleOffset, so this correctly - // accounts for - // the physical offset of the turret's zero position relative to the robot's - // forward direction - double fieldYawRad = Math.atan2(distance.getY(), distance.getX()); - Rotation2d turretZeroFieldDir = turretPose.getRotation().toRotation2d(); + // Vector from turret to target for yaw calculation + double toTargetX = target.getX() - turretX; + double toTargetY = target.getY() - turretY; + double fieldYawRad = Math.atan2(toTargetY, toTargetX); - // Avoid Rotation2d allocation — subtract in radians and convert to rotations - // directly - Rotation2d direction = new Rotation2d(fieldYawRad).minus(turretZeroFieldDir); + // Direction in rotations: normalize to [-0.5, 0.5] first (matches Rotation2d.minus behavior), + // then clamp to turret range + double directionRots = MathUtil.inputModulus( + (fieldYawRad - turretZeroFieldDirRad) / (2.0 * Math.PI), -0.5, 0.5); - // desired aim: turret pivot with X-axis pointing at target in field space + // Logging poses var desiredAimPose = new Pose3d(turretTranslation, new Rotation3d(0, 0, fieldYawRad)); - // current aim: turret pivot with X-axis showing where the turret is actually - // pointing right now - double currentFieldYaw = turretZeroFieldDir.getRadians() + turretHeadingRots * (2 * Math.PI); + double currentFieldYaw = turretZeroFieldDirRad + turretHeadingRots * (2 * Math.PI); var currentAimPose = new Pose3d(turretTranslation, new Rotation3d(0, 0, currentFieldYaw)); - // normalizes the angle to be fit in the range of the max rotations double angleRotations = MathUtil.inputModulus( - direction.getRotations(), kTurretMinRotsMagnitudeD, kTurretMaxRotsMagnitudeD); + directionRots, kTurretMinRotsMagnitudeD, kTurretMaxRotsMagnitudeD); /* Snapback Zone */ double snapbackSafeAngleRotations = angleRotations; @@ -212,11 +219,9 @@ public static AzimuthCalcDetails calcAzimuth(Translation3d target, Pose2d robotP double turretReferenceRots = snapbackSafeAngleRotations; - double dx = distance.getX(); - double dy = distance.getY(); - double d2 = dx * dx + dy * dy; + double d2 = toTargetX * toTargetX + toTargetY * toTargetY; double turretFFRadPerSec = d2 > 0 - ? (dy * fieldSpeeds.vxMetersPerSecond - dx * fieldSpeeds.vyMetersPerSecond) / d2 + ? (toTargetY * fieldSpeeds.vxMetersPerSecond - toTargetX * fieldSpeeds.vyMetersPerSecond) / d2 - fieldSpeeds.omegaRadiansPerSecond : 0.0; @@ -259,9 +264,8 @@ public static ShotCalcOutputs calcShot( AzimuthCalcDetails azCalcDetails = calcAzimuth(calculatedShot.getTarget(), robotPose, turretPositionRots, fieldSpeeds); double turretReferenceRots = azCalcDetails.turretReferenceRots(); - double hoodReferenceRots = calculatedShot.getHoodAngle().in(Rotations); - double shooterReferenceRPS = ShotCalculator.linearToAngularVelocity( - calculatedShot.getExitVelocity(), kFlywheelRadius).in(RotationsPerSecond); + double hoodReferenceRots = calculatedShot.hoodAngle() / (2.0 * Math.PI); + double shooterReferenceRPS = calculatedShot.exitVelocity() / (2.0 * Math.PI); return new ShotCalcOutputs( azCalcDetails, calculatedShot, turretReferenceRots, hoodReferenceRots, shooterReferenceRPS); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index b94afe1b..61a85352 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -28,9 +28,10 @@ import frc.util.WaltLogger.Pose3dLogger; public class Turret extends SubsystemBase { + private static final double kTurretMaxRotsFromHomeDeg = kTurretMaxRotsFromHome.in(Degrees); private boolean m_holdTurretAtIntakePos = false; private boolean m_turretLocked = false; - private Angle m_turretLockAngle = Degrees.zero(); + private double m_turretLockAngleRots = 0.0; private final TalonFX m_turret = new TalonFX(kTurretCANID, Constants.kCanivoreBus); // X44Foc private final PositionVoltage m_PVRequest = new PositionVoltage(0).withEnableFOC(true); @@ -78,14 +79,15 @@ private void homeTurret(boolean useLCM) { } } - public boolean atPosition() { + private void refreshTurretAtPos() { sig_turretCLErr.refresh(); - boolean isNear = sig_turretCLErr.isNear(0, kTurretMaxErrD); log_turretClosedLoopError.accept(sig_turretCLErr.getValueAsDouble()); + m_turretAtPos = sig_turretCLErr.isNear(0, kTurretMaxErrD); + log_atPos.accept(m_turretAtPos); + } - m_turretAtPos = isNear; - log_atPos.accept(isNear); - return isNear; + public boolean atPosition() { + return m_turretAtPos; } public void setIntaking(boolean intaking) { @@ -95,7 +97,7 @@ public void setIntaking(boolean intaking) { public void setTurretLock(boolean locked) { m_turretLocked = locked; if (m_turretLocked) { - m_turretLockAngle = m_turret.getPosition().getValue(); + m_turretLockAngleRots = m_turret.getPosition().getValueAsDouble(); } } @@ -117,7 +119,8 @@ public void setTurretPos(Angle rots, AngularVelocity velocityFF) { } public void setTurretPos(double rots, double velocityFF) { - setTurretPos(Rotations.of(rots), RotationsPerSecond.of(velocityFF)); + m_turret.setControl(m_PVRequest.withPosition(rots).withVelocity(velocityFF)); + log_turretControlPos.accept(rots); } public void setTurretNeutralMode(NeutralModeValue value) { @@ -126,7 +129,7 @@ public void setTurretNeutralMode(NeutralModeValue value) { // for TestingDashboard public Command setTurretPositionCmd(DoubleSubscriber sub_rots) { - return run(() -> setTurretPos(Rotations.of(sub_rots.get()))); + return run(() -> setTurretPos(sub_rots.get(), 0.0)); } public double getCurrTurretPos() { @@ -137,8 +140,8 @@ public boolean getTurretLocked() { return m_turretLocked; } - public Angle getTurretLockAngle() { - return m_turretLockAngle; + public double getTurretLockAngleRots() { + return m_turretLockAngleRots; } public boolean getHoldTurretAtIntake() { @@ -151,13 +154,17 @@ public boolean isTurretHomed() { public void periodic() { - log_lcmEncAPos.accept(m_lcmEncA.getAbsolutePosition().getValueAsDouble()); - log_lcmEncBPos.accept(m_lcmEncB.get()); + double encAVal = m_lcmEncA.getAbsolutePosition().getValueAsDouble(); + double encBVal = m_lcmEncB.get(); + log_lcmEncAPos.accept(encAVal); + log_lcmEncBPos.accept(encBVal); log_lcmEncBFreq.accept(m_lcmEncB.getFrequency()); log_lcmEncBConn.accept(m_lcmEncB.isConnected()); - Angle startAngle = Degrees.of(calcTurretAngleLCM(m_lcmEncA.getAbsolutePosition().getValueAsDouble() * 360, -(m_lcmEncB.get() - kEncBOffset) * 360)); - log_turretLCMPos.accept(startAngle.in(Rotations)); + refreshTurretAtPos(); + + double turretAngleDeg = calcTurretAngleLCM(encAVal * 360, -(encBVal - kEncBOffset) * 360); + log_turretLCMPos.accept(turretAngleDeg / 360.0); } public static double calcTurretAngleLCM(double e1, double e2) { @@ -170,7 +177,7 @@ public static double calcTurretAngleLCM(double e1, double e2) { // Center the search on the expected LCM output at home, spanning ±turret range double centerDeg = kLCMAtHomeRots * 360.0; - double rangeDeg = kTurretMaxRotsFromHome.in(Degrees); + double rangeDeg = kTurretMaxRotsFromHomeDeg; int kMin = (int) Math.floor((centerDeg - rangeDeg - e1 / encARatio) / encAPeriod); int kMax = (int) Math.ceil((centerDeg + rangeDeg - e1 / encARatio) / encAPeriod); From 76ad0c26f98a1b4fcca3eb5dc76a29095ecd15f6 Mon Sep 17 00:00:00 2001 From: Banks Troutman Date: Mon, 30 Mar 2026 17:37:58 -0400 Subject: [PATCH 35/40] Add SignalManager tfor global refreshAll on CAN signals --- src/main/java/frc/robot/Robot.java | 4 ++- .../java/frc/robot/subsystems/Indexer.java | 10 +++++-- .../java/frc/robot/subsystems/Intake.java | 22 +++++++++++---- .../frc/robot/subsystems/shooter/Hood.java | 12 ++++++-- .../frc/robot/subsystems/shooter/Shooter.java | 12 +++++--- .../frc/robot/subsystems/shooter/Turret.java | 16 +++++++---- src/main/java/frc/util/SignalManager.java | 28 +++++++++++++++++++ 7 files changed, 83 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/util/SignalManager.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 22e81a94..f52686b4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -53,6 +53,7 @@ import frc.robot.subsystems.Indexer; import frc.robot.vision.WaltCamera; import frc.util.HubShiftUtil; +import frc.util.SignalManager; // import frc.util.WaltVisualSim; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; @@ -356,7 +357,8 @@ private void configureTestingDashboard() { @Override public void robotPeriodic() { // m_periodicTracer.addEpoch("Entry (Unused Time)"); - CommandScheduler.getInstance().run(); + SignalManager.refreshAll(); + CommandScheduler.getInstance().run(); // m_periodicTracer.addEpoch("CommandScheduler"); log_robotPose.accept(m_drivetrain.getState().Pose); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index ba42dc33..057c78be 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -20,6 +20,7 @@ import static frc.robot.Constants.IndexerK.*; import frc.robot.Constants; +import frc.util.SignalManager; import frc.util.WaltMotorSim; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; @@ -60,6 +61,8 @@ public class Indexer extends SubsystemBase { private final DoubleLogger log_desiredSpindexerRPS = WaltLogger.logDouble(kLogTab, "desiredSpindexerRPS"); private final DoubleLogger log_desiredTunnelRPS = WaltLogger.logDouble(kLogTab, "desiredTunnelRPS"); + private final StatusSignal sig_spindexerVelo = m_spindexer.getVelocity(); + private final StatusSignal sig_tunnelVelo = m_tunnel.getVelocity(); private final StatusSignal sig_tunnelCLErr = m_tunnel.getClosedLoopError(); private final DoubleLogger log_tunnelClosedLoopError = WaltLogger.logDouble(kLogTab, "tunnelClosedLoopError"); private final BooleanLogger log_isTunnelSpunUp = WaltLogger.logBoolean(kLogTab, "isTunnelSpunUp"); @@ -72,6 +75,8 @@ public Indexer() { m_spindexer.getConfigurator().apply(kSpindexerTalonFXConfiguration); m_tunnel.getConfigurator().apply(kTunnelTalonFXConfiguration); + SignalManager.register(Constants.kCanivoreBus, sig_spindexerVelo, sig_tunnelVelo, sig_tunnelCLErr); + initSim(); } @@ -122,10 +127,9 @@ public void stopTunnel() { } private void refreshTunnelState() { - m_tunnelVelocityRotPerSec = m_tunnel.getVelocity().getValueAsDouble(); + m_tunnelVelocityRotPerSec = sig_tunnelVelo.getValueAsDouble(); log_tunnelRPS.accept(m_tunnelVelocityRotPerSec); - sig_tunnelCLErr.refresh(); log_tunnelClosedLoopError.accept(sig_tunnelCLErr.getValueAsDouble()); m_isTunnelSpunUp = sig_tunnelCLErr.isNear(0, 30); log_isTunnelSpunUp.accept(m_isTunnelSpunUp); @@ -172,7 +176,7 @@ public Command setTunnelVelocityCmd(DoubleSubscriber sub_RPS) { /* PERIODICS */ @Override public void periodic() { - log_spindexerRPS.accept(m_spindexer.getVelocity().getValueAsDouble()); + log_spindexerRPS.accept(sig_spindexerVelo.getValueAsDouble()); refreshTunnelState(); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 89cdaf37..309a9912 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -23,6 +24,8 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -34,6 +37,7 @@ import frc.util.WaltMotorSim; import frc.robot.Robot; import frc.util.GobildaServoAngled; +import frc.util.SignalManager; import frc.util.WaltLogger; public class Intake extends SubsystemBase { @@ -47,8 +51,14 @@ public class Intake extends SubsystemBase { private DynamicMotionMagicVoltage m_MMVReq = new DynamicMotionMagicVoltage(0, 1, 1).withEnableFOC(true); private VoltageOut m_VVReq = new VoltageOut(0).withEnableFOC(true); - private BooleanSupplier m_currentSpike = () -> m_intakeArm.getStatorCurrent().getValueAsDouble() > 5.0; - private BooleanSupplier m_veloIsNearZero = () -> Math.abs(m_intakeArm.getVelocity().getValueAsDouble()) < 0.005; + private final StatusSignal sig_intakeArmStatorCurrent = m_intakeArm.getStatorCurrent(); + private final StatusSignal sig_intakeArmVelo = m_intakeArm.getVelocity(); + private final StatusSignal sig_intakeRollersAVelo = m_intakeRollersA.getVelocity(); + private final StatusSignal sig_intakeArmPos = m_intakeArm.getPosition(); + private final StatusSignal sig_intakeArmMMAtTarget = m_intakeArm.getMotionMagicAtTarget(); + + private BooleanSupplier m_currentSpike = () -> sig_intakeArmStatorCurrent.getValueAsDouble() > 5.0; + private BooleanSupplier m_veloIsNearZero = () -> Math.abs(sig_intakeArmVelo.getValueAsDouble()) < 0.005; private VoltageOut m_intakeArmZeroingReq = new VoltageOut(0); @@ -96,6 +106,8 @@ public Intake() { m_intakeRollersB.setControl(new Follower(kIntakeRollersA_CANID, MotorAlignmentValue.Opposed)); + SignalManager.register("rio", sig_intakeArmStatorCurrent, sig_intakeArmVelo, sig_intakeRollersAVelo, sig_intakeArmPos, sig_intakeArmMMAtTarget); + if (Robot.isReal()) { setDefaultCommand(intakeArmCurrentSenseHoming()); // setDefaultCommand(intakeArmHome()); @@ -127,7 +139,7 @@ public void setIntakeArmPos(Angle rots, double RPSPS) { } public boolean isIntakeArmAtDest() { - return m_intakeArm.getMotionMagicAtTarget().getValue(); + return sig_intakeArmMMAtTarget.getValue(); } public Command shimmy() { @@ -218,8 +230,8 @@ public Command intakeArmCurrentSenseHoming() { public void periodic() { log_targetIntakeArmRots.accept(m_MMVReq.Position); log_targetIntakeRollersRPS.accept(m_VVReq.Output); - log_intakeRollersRPS.accept(m_intakeRollersA.getVelocity().getValueAsDouble()); - log_intakeArmRots.accept(m_intakeArm.getPosition().getValueAsDouble()); + log_intakeRollersRPS.accept(sig_intakeRollersAVelo.getValueAsDouble()); + log_intakeArmRots.accept(sig_intakeArmPos.getValueAsDouble()); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index b93135fa..cd8fbd19 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -7,6 +7,7 @@ import java.util.function.BooleanSupplier; import java.util.function.Consumer; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.VoltageOut; @@ -17,10 +18,12 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterK; +import frc.util.SignalManager; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; @@ -39,7 +42,10 @@ public class Hood extends SubsystemBase { private Debouncer m_currentDebouncer = new Debouncer(0.125, DebounceType.kRising); - private BooleanSupplier m_currentSpike = () -> m_hood.getStatorCurrent().getValueAsDouble() > 5.0; + private final StatusSignal sig_hoodStatorCurrent = m_hood.getStatorCurrent(); + private final StatusSignal sig_hoodPos = m_hood.getPosition(); + + private BooleanSupplier m_currentSpike = () -> sig_hoodStatorCurrent.getValueAsDouble() > 5.0; private final StaticBrake m_BrakeReq = new StaticBrake(); @@ -48,6 +54,8 @@ public class Hood extends SubsystemBase { public Hood() { m_hood.getConfigurator().apply(kHoodTalonFXSConfiguration); + SignalManager.register(kShooterBus, sig_hoodStatorCurrent, sig_hoodPos); + m_hood.setPosition(0); m_isHoodHomed = true; log_hoodHomed.accept(m_isHoodHomed); @@ -114,7 +122,7 @@ public void setHoodNeutralMode(NeutralModeValue value) { @Override public void periodic() { - double posRots = m_hood.getPosition().getValueAsDouble(); + double posRots = sig_hoodPos.getValueAsDouble(); log_hoodCurrentPos.accept(getHoodAngleDeg(posRots)); log_hoodPositionDeg.accept(posRots * 360.0); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 819277ad..03a4370c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -42,6 +42,7 @@ import frc.robot.Constants; import frc.robot.subsystems.shooter.ShooterCalc.ShotCalcOutputs; +import frc.util.SignalManager; import frc.util.WaltMotorSim; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; @@ -105,7 +106,8 @@ public class Shooter extends SubsystemBase { // private final Tracer m_periodicTracer = new Tracer(); - StatusSignal sig_shooterCLErr = m_shooterA.getClosedLoopError(); + private final StatusSignal sig_shooterCLErr = m_shooterA.getClosedLoopError(); + private final StatusSignal sig_shooterAVelo = m_shooterA.getVelocity(); /* CONSTRUCTOR */ public Shooter(Supplier poseSupplier, Supplier threadsafeSwerveStateSup, Supplier fieldSpeedsSupplier) { @@ -123,7 +125,10 @@ public Shooter(Supplier poseSupplier, Supplier threads m_shooterB.setControl(new Follower(kShooterA_CANID, MotorAlignmentValue.Opposed)); sig_shooterCLErr.setUpdateFrequency(Hertz.of(50)); - m_latestFlywheelVelocityRotPerSec = m_shooterA.getVelocity().getValueAsDouble(); + + SignalManager.register(Constants.kShooterBus, sig_shooterAVelo, sig_shooterCLErr); + + m_latestFlywheelVelocityRotPerSec = sig_shooterAVelo.getValueAsDouble(); m_poseSupplier = poseSupplier; m_fieldSpeedsSupplier = fieldSpeedsSupplier; @@ -198,7 +203,6 @@ public Command setShooterVelocityCmd(DoubleSubscriber sub_RPS) { } private void refreshShooterSpunUp() { - sig_shooterCLErr.refresh(); log_shooterClosedLoopError.accept(sig_shooterCLErr.getValueAsDouble()); m_isShooterSpunUp = sig_shooterCLErr.isNear(0, 3); } @@ -290,7 +294,7 @@ public void periodic() { // Cache all signals at the top so every consumer in this loop sees the same values // THIS IS USED SNEAKILY BY SHOTCALC DO NOT MOVE THIS m_latestTurretPositionRots = m_turret.getCurrTurretPos(); - m_latestFlywheelVelocityRotPerSec = m_shooterA.getVelocity().getValueAsDouble(); + m_latestFlywheelVelocityRotPerSec = sig_shooterAVelo.getValueAsDouble(); ShotCalcOutputs calcData = m_shooterCalc.getLatestShotCalcOutputs(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 61a85352..814197e8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -21,6 +21,7 @@ import static frc.robot.Constants.TurretK.*; import java.util.function.BooleanSupplier; +import frc.util.SignalManager; import frc.util.WaltLogger; import frc.util.WaltLogger.BooleanLogger; import frc.util.WaltLogger.DoubleLogger; @@ -56,7 +57,9 @@ public class Turret extends SubsystemBase { private final BooleanLogger log_atPos = WaltLogger.logBoolean(kLogTab, "atPos"); private final Pose3dLogger log_turretTransform = WaltLogger.logPose3d(kLogTab, "turretTransform"); - StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); + private final StatusSignal sig_turretCLErr = m_turret.getClosedLoopError(); + private final StatusSignal sig_turretPos = m_turret.getPosition(); + private final StatusSignal sig_lcmEncAAbsPos = m_lcmEncA.getAbsolutePosition(); public Turret() { m_turret.getConfigurator().apply(kTurretTalonFXConfiguration); @@ -65,6 +68,8 @@ public Turret() { m_lcmEncB.setAssumedFrequency(488); m_lcmEncB.setConnectedFrequencyThreshold(400); + SignalManager.register(Constants.kCanivoreBus, sig_turretCLErr, sig_turretPos, sig_lcmEncAAbsPos); + log_turretTransform.accept(kTurretTransform); homeTurret(true); @@ -72,7 +77,7 @@ public Turret() { private void homeTurret(boolean useLCM) { if (useLCM) { - double lcmRots = calcTurretAngleLCM(m_lcmEncA.getAbsolutePosition().getValueAsDouble() * 360, -(m_lcmEncB.get() - kEncBOffset) * 360) / 360.0; + double lcmRots = calcTurretAngleLCM(sig_lcmEncAAbsPos.getValueAsDouble() * 360, -(m_lcmEncB.get() - kEncBOffset) * 360) / 360.0; m_turret.setPosition(lcmRots); } else { m_turret.setPosition(kInitPosition); @@ -80,7 +85,6 @@ private void homeTurret(boolean useLCM) { } private void refreshTurretAtPos() { - sig_turretCLErr.refresh(); log_turretClosedLoopError.accept(sig_turretCLErr.getValueAsDouble()); m_turretAtPos = sig_turretCLErr.isNear(0, kTurretMaxErrD); log_atPos.accept(m_turretAtPos); @@ -97,7 +101,7 @@ public void setIntaking(boolean intaking) { public void setTurretLock(boolean locked) { m_turretLocked = locked; if (m_turretLocked) { - m_turretLockAngleRots = m_turret.getPosition().getValueAsDouble(); + m_turretLockAngleRots = sig_turretPos.getValueAsDouble(); } } @@ -133,7 +137,7 @@ public Command setTurretPositionCmd(DoubleSubscriber sub_rots) { } public double getCurrTurretPos() { - return m_turret.getPosition().getValueAsDouble(); + return sig_turretPos.getValueAsDouble(); } public boolean getTurretLocked() { @@ -154,7 +158,7 @@ public boolean isTurretHomed() { public void periodic() { - double encAVal = m_lcmEncA.getAbsolutePosition().getValueAsDouble(); + double encAVal = sig_lcmEncAAbsPos.getValueAsDouble(); double encBVal = m_lcmEncB.get(); log_lcmEncAPos.accept(encAVal); log_lcmEncBPos.accept(encBVal); diff --git a/src/main/java/frc/util/SignalManager.java b/src/main/java/frc/util/SignalManager.java new file mode 100644 index 00000000..8af68247 --- /dev/null +++ b/src/main/java/frc/util/SignalManager.java @@ -0,0 +1,28 @@ +package frc.util; + +import java.util.HashMap; +import java.util.Map; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignalCollection; + +public final class SignalManager { + private static final Map m_signalsByBus = new HashMap<>(); + + private SignalManager() {} + + public static void register(String bus, BaseStatusSignal... signals) { + m_signalsByBus.computeIfAbsent(bus, k -> new StatusSignalCollection()).addSignals(signals); + } + + public static void register(CANBus bus, BaseStatusSignal... signals) { + register(bus.getName(), signals); + } + + public static void refreshAll() { + for (var collection : m_signalsByBus.values()) { + collection.refreshAll(); + } + } +} From ba8c61734bc58ce203312228507db0afa5a1eed9 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Tue, 31 Mar 2026 22:31:09 -0400 Subject: [PATCH 36/40] new turret offset --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b143e978..a03e0979 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -74,7 +74,7 @@ public static class WpiK { } public static class ShooterK { public static final String kLogTab = "Shooter"; - public static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.106); //4.87 //0.132324 // was 0.12, decreased 0.014 (~5deg) to fix consistent rightward aim error + public static final Rotation2d kTurretAngleOffset = Rotation2d.fromRotations(0.106 + 0.0067); //4.87 //0.132324 // was 0.12, decreased 0.014 (~5deg) to fix consistent rightward aim error public static final Rotation3d kTurretAngleOffset3d = new Rotation3d(kTurretAngleOffset); public static final Translation3d kTurretTranslation = new Translation3d(Inches.of(-4.744), Inches.of(-4.239), Inches.of(17.260)); public static final Transform3d kTurretTransformNoRotation = new Transform3d(kTurretTranslation, Rotation3d.kZero); From c7980a488069d9601a08b54fe2716f2b70cdeeb5 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Tue, 31 Mar 2026 23:02:33 -0400 Subject: [PATCH 37/40] rps boost cope lol --- src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 55a4d8b6..64f383df 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -84,7 +84,7 @@ public class ShotCalculator { } public static void addLerpPoint(double distanceToTarget, double shooterRPS, double hoodRots, double timeOfFlight) { - m_shotMap.put(distanceToTarget, new ShotData(RotationsPerSecond.of(shooterRPS), Rotations.of(hoodRots))); + m_shotMap.put(distanceToTarget, new ShotData(RotationsPerSecond.of(shooterRPS /* + kRPSBoost */), Rotations.of(hoodRots))); m_timeOfFlightMap.put(distanceToTarget, timeOfFlight); } From bfec2409503ca72e70eeb1ab3131bc3d860f0071 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Tue, 31 Mar 2026 23:08:22 -0400 Subject: [PATCH 38/40] changes pre sotm merge --- src/main/java/frc/robot/Robot.java | 55 +------------------ .../frc/robot/subsystems/Superstructure.java | 12 ---- 2 files changed, 1 insertion(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f2b0fffa..4403bf5b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -480,60 +480,7 @@ public void teleopExit() {} @Override public void testInit() { - CommandScheduler.getInstance().cancelAll(); - - // TestingDashboard.initialize(); - Command opsCheckCommand = m_superstructure.m_isLongOpsCheck ? m_superstructure.longOpsCheck() : m_superstructure.shortOpsCheck(); - - CommandScheduler.getInstance().schedule( - Commands.sequence( - Commands.print("==========START LONG OPS CHECK=========="), - opsCheckCommand - )); - - // .whileFalse(m_superstructure.shortOpsCheck()); - - // CommandScheduler.getInstance().schedule( - // Commands.sequence( - // m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), - // Commands.waitSeconds(1), - // m_drivetrain.applyRequest(() -> - // drive.withVelocityX(kMaxTranslationSpeed) - // .withVelocityY(0) - // .withRotationalRate(0) - // ), - // Commands.waitSeconds(2.5), - // m_drivetrain.xBrake(), - // Commands.waitSeconds(2.5), - // m_drivetrain.applyRequest(() -> - // drive.withVelocityX(kMaxTranslationSpeed.times(-1)) - // .withVelocityY(0) - // .withRotationalRate(0) - // ), - // Commands.waitSeconds(2.5), - // m_drivetrain.xBrake(), - // Commands.waitSeconds(2.5), - // m_drivetrain.applyRequest(() -> - // drive.withVelocityX(0) - // .withVelocityY(0) - // .withRotationalRate(kMaxAngularRate) - // ), - // Commands.waitSeconds(2.5), - // m_drivetrain.xBrake(), - // Commands.waitSeconds(2.5), - // m_drivetrain.applyRequest(() -> - // drive.withVelocityX(0) - // .withVelocityY(0) - // .withRotationalRate(0) - // ), - // Commands.waitSeconds(2.5), - // m_drivetrain.xBrake(), - // Commands.waitSeconds(2.5), - // m_superstructure.intake(() -> false).withTimeout(5), - // Commands.waitSeconds(2.5), - // m_superstructure.activateOuttake(kShooterRPS).withTimeout(4) - // ) - // ); + CommandScheduler.getInstance().schedule(m_superstructure.longOpsCheck()); } @Override diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ec9e7e3e..f571990c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -196,58 +196,46 @@ public Command intakeTo(IntakeArmPosition pos) { */ public Command longOpsCheck() { return Commands.sequence( - Commands.print("======================STARTING LONG OPS CHECK======================"), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //deploy intake - Commands.print("================================= DEPLOY INTAKE ================================="), m_intake.setIntakeArmPosCmd(IntakeArmPosition.DEPLOYED), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run intake rollers - Commands.print("================================= RUN INTAKE ROLLERS ================================="), m_intake.startIntakeRollers(), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //run spindexer - Commands.print("================================= RUN SPINDEXER ================================="), m_intake.stopIntakeRollers(), m_indexer.setSpindexerVelocityCmd(IndexerK.kSpindexerShootRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //Stop Spindexer; run tunnel - Commands.print("================================= RUN TUNNEL ================================="), m_indexer.stopSpindexerCmd(), m_indexer.setTunnelVelocityCmd(IndexerK.kTunnelShootRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //Stop tunnel; run shooter - Commands.print("================================= RUN SHOOTER ================================="), m_indexer.stopTunnelCmd(), m_shooter.setShooterVelocityCmd(ShooterK.kShooterRPS), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //shooter stopped - Commands.print("================================= STOP SHOOTER ================================="), m_shooter.setShooterVelocityCmd(RotationsPerSecond.zero()), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //turret move - Commands.print("================================= TURRET MOVE ================================="), m_shooter.m_turret.setTurretPosCmd(ShooterK.kTurretIntakeLockPos), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), m_shooter.m_turret.setTurretPosCmd(ShooterK.kHomePosition), //hood move - Commands.print("================================= HOOD MOVE ================================="), m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodLockRots_double), - Commands.print("================================= LOCK HOOD POS ================================="), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), - Commands.print("================================= SET HOOD POS TO MIN ================================="), m_shooter.m_hood.setHoodPosCmd(0.1), //Run short Ops check (intake, shoot, swerve) - Commands.print("================================= START SHORT OPS CHECK ================================="), shortOpsCheck() ); } From 3d94890c69f788986cc8543adeb8f76e7b4385c0 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Tue, 31 Mar 2026 23:12:26 -0400 Subject: [PATCH 39/40] other merge schenanigans --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61a278a6..8727abd5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -129,7 +129,7 @@ public static class ShooterK { public static final Angle kTurretMaxRotsFromHome = Rotations.of(0.60 ); //0.75 rots in each direction from home public static final Angle kTurretMinRots = Rotations.of(-kTurretMaxRotsFromHome.in(Rotations)); public static final Angle kTurretMaxRots = Rotations.of(kTurretMaxRotsFromHome.in(Rotations)); - public static final Angle kTurretIntakeLockPos = Rotations.of(-0.250); + public static final double kTurretIntakeLockPos = -0.25; public static final double kTurretMaxErrD = Rotations.of(0.05).in(Rotations); public static final AngularVelocity kShooterMaxRPS = MotorK.kX44MaxVelocity.div(kShooterGearing); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 03a4370c..8362b791 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -309,11 +309,11 @@ public void periodic() { m_calcFlywheelVelocityRotPerSec = kShooterRPSd; } else { if (m_turret.getHoldTurretAtIntake()) { - // m_turret.setTurretPos(Rotations.of(-0.250)); + m_turret.setTurretPos(kTurretIntakeLockPos, 0.0); } else { m_turret.setTurretPos(turretReference, turretVelocityFF); m_calcFlywheelVelocityRotPerSec = calcData.shooterReferenceRps(); - if (false) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK + if (true) { // ENABLE THIS TO ALLOW DRIVER RPS TWEAK m_calcFlywheelVelocityRotPerSec += m_driverRPSTweak; m_calcFlywheelVelocityRotPerSec = MathUtil.clamp(m_calcFlywheelVelocityRotPerSec, 0, kShooterMaxRPSd); //clamp here or clamp only when setShooterVel is called? } From 70ce4cea057a0cb2d9882f2520a28994e88e4c9f Mon Sep 17 00:00:00 2001 From: alexandra Date: Mon, 13 Apr 2026 18:41:55 -0400 Subject: [PATCH 40/40] fixed error from changed method --- src/main/java/frc/robot/subsystems/Superstructure.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 599cd916..70365c5d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -225,9 +226,9 @@ public Command longOpsCheck() { Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), //turret move - m_shooter.m_turret.setTurretPosCmd(ShooterK.kTurretIntakeLockPos), + m_shooter.m_turret.setTurretLockCmd(true), Commands.waitSeconds(SuperstructureK.kLongOpsCheckPause), - m_shooter.m_turret.setTurretPosCmd(ShooterK.kHomePosition), + m_shooter.m_turret.setTurretLockCmd(true), //hood move m_shooter.m_hood.setHoodPosCmd(ShooterK.kHoodLockRots_double),