From 0434c2c42f0c73b93d39af6c39f085822afb90c0 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 1 Mar 2026 13:41:25 -0800 Subject: [PATCH 01/21] Make feeding use SOTM --- .../subsystems/shooter/TurretSubsystem.java | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 2454e396..a9359509 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -204,18 +204,14 @@ public Command feed( return this.run( () -> { Logger.recordOutput("Robot/Feed Target", feedTarget.get()); - ShotData shotData = - AutoAim.FEED_SHOT_TREE.get( - robotPoseSupplier - .get() - .getTranslation() - .getDistance(feedTarget.get().getTranslation())); - // hoodIO.setHoodPosition(shotData.hoodAngle()); - // // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - // flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + ShotData shotData = AutoAim.getSOTMShotData(robotPoseSupplier.get(), feedTarget.get().getTranslation(), chassisSpeedsSupplier.get(), AutoAim.FEED_SHOT_TREE); - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + hoodIO.setHoodPosition(shotData.hoodAngle()); + // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + + // hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + // flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); turretIO.setTurretPosition( AutoAim.getTurretFeedTargetRotation( feedTarget.get().getTranslation(), From cb5d0ead8f3f8eccb761f00ded0b3a409e6e6271 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 17:21:55 -0800 Subject: [PATCH 02/21] reorder auto aim stuff for clarity --- .../java/frc/robot/utils/autoaim/AutoAim.java | 39 ++++++++++--------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 6a55e5c0..015bfacb 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -106,12 +106,26 @@ public class AutoAim { // TODO: POPULATE beyond 24 feet and time of flight } + //TODO should be turrets distance public static double distanceToHub(Pose2d pose) { double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); Logger.recordOutput("Autoaim/Distance To Hub", distance); return distance; } + public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { + Translation2d robotToTarget = target.minus(robotPose.getTranslation()); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); + Logger.recordOutput("Autoaim/Target Rotation", rot); + return rot; + } + + public static Rotation2d getVirtualTargetYaw( + Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { + Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); + return getTargetRotation(vtarget, robotPose); + } + // lock in public static Translation2d getVirtualSOTMTarget( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { @@ -126,16 +140,12 @@ public static Translation2d getVirtualSOTMTarget( } public static Rotation2d getVirtualTargetYaw( - Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { - Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); - return getTargetRotation(vtarget, robotPose); - } - - public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { - Translation2d robotToTarget = target.minus(robotPose.getTranslation()); - Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); - Logger.recordOutput("Autoaim/Target Rotation", rot); - return rot; + ChassisSpeeds fieldRelativeSpeeds, + Translation2d targetTranslation, + Pose2d robotPose, + InterpolatingShotTree tree) { + double tof = tree.calculateShot(robotPose, targetTranslation).timeOfFlightSecs(); + return getVirtualTargetYaw(targetTranslation, fieldRelativeSpeeds, robotPose, tof); } // if we have a turret im going to assume we're on comp @@ -193,15 +203,6 @@ public static Rotation2d getTurretFeedTargetRotation( return getTurretTargetRotation(target, robotPose, chassisSpeeds, FEED_SHOT_TREE); } - public static Rotation2d getVirtualTargetYaw( - ChassisSpeeds fieldRelativeSpeeds, - Translation2d targetTranslation, - Pose2d robotPose, - InterpolatingShotTree tree) { - double tof = tree.calculateShot(robotPose, targetTranslation).timeOfFlightSecs(); - return getVirtualTargetYaw(targetTranslation, fieldRelativeSpeeds, robotPose, tof); - } - public static ShotData getSOTMShotData( Pose2d robotPose, Translation2d targetTranslation, From 7c2638f9cca6f5fd12f0cb44ef7d7d9ceb9375fd Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 17:28:45 -0800 Subject: [PATCH 03/21] make score actually use sotm? --- .../frc/robot/subsystems/shooter/TurretSubsystem.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a9359509..a0aec4d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -388,14 +388,17 @@ public Command spinUpTest( public Command score( Supplier robotPoseSupplier, - Supplier shotDataSupplier, + // Supplier shotDataSupplier, Supplier chassisSpeedsSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + Logger.recordOutput("Robot/Hub Target", FieldUtils.getCurrentHubPose()); + ShotData shotData = AutoAim.getSOTMShotData(robotPoseSupplier.get(), FieldUtils.getCurrentHubTranslation(), chassisSpeedsSupplier.get(), AutoAim.COMP_HUB_SHOT_TREE); + + hoodIO.setHoodPosition(shotData.hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); + shotData.flywheelVelocityRotPerSec()); turretIO.setTurretPosition( AutoAim.getTurretHubTargetRotation( FieldUtils.getCurrentHubTranslation(), From 4359bf9ec29c7178f44a7c88954b91ca8dbb9b69 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 17:57:24 -0800 Subject: [PATCH 04/21] get sotm data based on turret pose not the entire robot --- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 2 +- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a0aec4d9..9de2fce3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -388,7 +388,7 @@ public Command spinUpTest( public Command score( Supplier robotPoseSupplier, - // Supplier shotDataSupplier, + Supplier shotDataSupplier, Supplier chassisSpeedsSupplier) { return this.run( () -> { diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 015bfacb..e78dd2b5 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -106,7 +106,6 @@ public class AutoAim { // TODO: POPULATE beyond 24 feet and time of flight } - //TODO should be turrets distance public static double distanceToHub(Pose2d pose) { double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); Logger.recordOutput("Autoaim/Distance To Hub", distance); @@ -212,7 +211,10 @@ public static ShotData getSOTMShotData( Translation2d virtualTarget = getVirtualSOTMTarget( targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); - return tree.get(robotPose.getTranslation().getDistance(virtualTarget)); + Pose2d turretPose = + robotPose.transformBy( + new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } public static ShotData getCompensatedSOTMShotData( From de2a7020bd89baebb2424f8f4b2fafb0fdfaaa05 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 18:29:55 -0800 Subject: [PATCH 05/21] iterate sotm so its more accurate --- .../java/frc/robot/utils/autoaim/AutoAim.java | 29 ++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e78dd2b5..b99d1bf7 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -201,7 +201,7 @@ public static Rotation2d getTurretFeedTargetRotation( Translation2d target, Pose2d robotPose, ChassisSpeeds chassisSpeeds) { return getTurretTargetRotation(target, robotPose, chassisSpeeds, FEED_SHOT_TREE); } - +/* public static ShotData getSOTMShotData( Pose2d robotPose, Translation2d targetTranslation, @@ -216,6 +216,33 @@ public static ShotData getSOTMShotData( new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } + */ + + public static ShotData getSOTMShotData( + Pose2d robotPose, + Translation2d targetTranslation, + ChassisSpeeds fieldRelativeSpeeds, + InterpolatingShotTree tree) { + + ShotData shot = tree.calculateShot(robotPose, targetTranslation); + Translation2d virtualTarget = + getVirtualSOTMTarget( + targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); + Pose2d turretPose = + robotPose.transformBy( + new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + + //adjust new virtual target and shot with iterations but idk if it makes it better + for (int i = 0; i < 3; i++) { + virtualTarget = targetTranslation.minus( + new Translation2d(fieldRelativeSpeeds.vxMetersPerSecond * shot.timeOfFlightSecs(), + fieldRelativeSpeeds.vyMetersPerSecond * shot.timeOfFlightSecs())); + + shot = tree.get(turretPose.getTranslation().getDistance(virtualTarget)); + } + + return shot; + } public static ShotData getCompensatedSOTMShotData( Pose2d robotPose, From 5c59a2b9cab5e11135f206d18a8991991aa48984 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 18:48:38 -0800 Subject: [PATCH 06/21] use existing virtual target method --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index b99d1bf7..8c976df1 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -129,7 +129,7 @@ public static Rotation2d getVirtualTargetYaw( public static Translation2d getVirtualSOTMTarget( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { // velocity times shot time is how translated it is - Translation2d vtarget = + Translation2d vtarget = target.minus( new Translation2d( fieldRelativeSpeeds.vxMetersPerSecond * timeOfFlightSecs, @@ -234,9 +234,8 @@ public static ShotData getSOTMShotData( //adjust new virtual target and shot with iterations but idk if it makes it better for (int i = 0; i < 3; i++) { - virtualTarget = targetTranslation.minus( - new Translation2d(fieldRelativeSpeeds.vxMetersPerSecond * shot.timeOfFlightSecs(), - fieldRelativeSpeeds.vyMetersPerSecond * shot.timeOfFlightSecs())); + virtualTarget = getVirtualSOTMTarget( + targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); shot = tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } From a86e10027c4c29f3360422d55ad75438d7488f9c Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 18:58:32 -0800 Subject: [PATCH 07/21] get turret pose --- .../java/frc/robot/utils/autoaim/AutoAim.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8c976df1..aba27067 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -106,6 +106,13 @@ public class AutoAim { // TODO: POPULATE beyond 24 feet and time of flight } + public static Pose2d getTurretPose(Pose2d robotPose) { + Pose2d turretPose = + robotPose.transformBy( + new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + return turretPose; + } + public static double distanceToHub(Pose2d pose) { double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); Logger.recordOutput("Autoaim/Distance To Hub", distance); @@ -153,9 +160,7 @@ public static Rotation2d getTurretTargetRotation( Pose2d robotPose, ChassisSpeeds chassisSpeeds, InterpolatingShotTree shotTree) { - Pose2d turretPose = - robotPose.transformBy( - new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + Pose2d turretPose = getTurretPose(robotPose); // get desired rotation to point at target Rotation2d turretTargetRotation = @@ -228,9 +233,7 @@ public static ShotData getSOTMShotData( Translation2d virtualTarget = getVirtualSOTMTarget( targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); - Pose2d turretPose = - robotPose.transformBy( - new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + Pose2d turretPose = getTurretPose(robotPose); //adjust new virtual target and shot with iterations but idk if it makes it better for (int i = 0; i < 3; i++) { From 20920c7dc7ee794d49cfa93f89cd3153d8a1381e Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 20:05:18 -0800 Subject: [PATCH 08/21] revert because that iteration wouldn't help --- .../java/frc/robot/utils/autoaim/AutoAim.java | 32 +++---------------- 1 file changed, 4 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index aba27067..eeaf3502 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -206,44 +206,20 @@ public static Rotation2d getTurretFeedTargetRotation( Translation2d target, Pose2d robotPose, ChassisSpeeds chassisSpeeds) { return getTurretTargetRotation(target, robotPose, chassisSpeeds, FEED_SHOT_TREE); } -/* + public static ShotData getSOTMShotData( Pose2d robotPose, Translation2d targetTranslation, ChassisSpeeds fieldRelativeSpeeds, InterpolatingShotTree tree) { + ShotData unadjustedShot = tree.calculateShot(robotPose, targetTranslation); Translation2d virtualTarget = getVirtualSOTMTarget( targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); - Pose2d turretPose = - robotPose.transformBy( - new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); - return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); - } - */ - - public static ShotData getSOTMShotData( - Pose2d robotPose, - Translation2d targetTranslation, - ChassisSpeeds fieldRelativeSpeeds, - InterpolatingShotTree tree) { - - ShotData shot = tree.calculateShot(robotPose, targetTranslation); - Translation2d virtualTarget = - getVirtualSOTMTarget( - targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); - Pose2d turretPose = getTurretPose(robotPose); - - //adjust new virtual target and shot with iterations but idk if it makes it better - for (int i = 0; i < 3; i++) { - virtualTarget = getVirtualSOTMTarget( - targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); + Pose2d turretPose = getTurretPose(robotPose); - shot = tree.get(turretPose.getTranslation().getDistance(virtualTarget)); - } - - return shot; + return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } public static ShotData getCompensatedSOTMShotData( From 691fe0ecffa74972635d99e359da9012507e74be Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 21:31:34 -0800 Subject: [PATCH 09/21] trying newtons method --- .../java/frc/robot/utils/autoaim/AutoAim.java | 62 +++++++++++++++++++ 1 file changed, 62 insertions(+) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index eeaf3502..162a7422 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -221,6 +221,68 @@ public static ShotData getSOTMShotData( return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } + + + public static ShotData getSOTMShotDataNewtonsMethod( + Pose2d robotPose, + Translation2d targetTranslation, + ChassisSpeeds fieldRelativeSpeeds, + InterpolatingShotTree tree) { + + Pose2d turretPose = getTurretPose(robotPose); + + ShotData currentShot = tree.calculateShot(robotPose, targetTranslation); + + double currentDistance = turretPose.getTranslation().getDistance(targetTranslation); + double currentTime = currentShot.timeOfFlightSecs(); + double currentVelocity = currentDistance / currentTime; + + Translation2d virtualTarget = + getVirtualSOTMTarget( + targetTranslation, fieldRelativeSpeeds, currentShot.timeOfFlightSecs()); + + ShotData targetShot = tree.get(turretPose.getTranslation().getDistance(virtualTarget)); + //TODO what is required velcity is it like horizontal so distance/tof? + double requiredVelocity = turretPose.getTranslation().getDistance(virtualTarget) / targetShot.timeOfFlightSecs(); + //currentShot.flywheelVelocityRotPerSec(); + + // 10 rounds for now + for (int i = 0; i < 10 && Math.abs(currentVelocity - requiredVelocity) > 0.005; i++) { + // estimate derivative by taking a tiny slope + final double EPSILON = 0.001; + double lowVel = + (currentDistance - EPSILON) / tree.get(currentDistance - EPSILON).timeOfFlightSecs(); + double highVel = + (currentDistance + EPSILON) / tree.get(currentDistance + EPSILON).timeOfFlightSecs(); + double velDeriv = (highVel - lowVel) / (EPSILON * 2); + currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; + // update currentVelocity with f(x+1) + currentShot = tree.get(currentDistance); + currentTime = currentShot.timeOfFlightSecs(); + currentVelocity = currentDistance / currentTime; + } + + return new ShotData(currentShot.hoodAngle(), currentShot.flywheelVelocityRotPerSec(), currentShot.timeOfFlightSecs()); + } + + // tof is different if ur driving towards or away because the ball will + // be pushed forward or back by the robots velocity bc physics not just how far it is + //but im not really sure how to implement this at the moment + public static ShotData getRobotVelocityCompensatedSOTMShotData( + Pose2d robotPose, + Translation2d targetTranslation, + ChassisSpeeds fieldRelativeSpeeds, + InterpolatingShotTree tree) { + + ShotData shot = getSOTMShotData(robotPose, targetTranslation, fieldRelativeSpeeds, tree); + Translation2d virtualTarget = + getVirtualSOTMTarget(targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); + Translation2d turretTranslation = getTurretPose(robotPose).getTranslation(); + + double translationNorm = (turretTranslation.minus(virtualTarget)).getNorm(); +//doesnt do anything rn, prob just delete + return shot; + } public static ShotData getCompensatedSOTMShotData( Pose2d robotPose, From 995766f1b4d2ea840932a17c3afded389f46809c Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sun, 1 Mar 2026 21:32:12 -0800 Subject: [PATCH 10/21] nvm velocity should already be accounted for --- .../java/frc/robot/utils/autoaim/AutoAim.java | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 162a7422..9ca85e55 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -265,25 +265,6 @@ public static ShotData getSOTMShotDataNewtonsMethod( return new ShotData(currentShot.hoodAngle(), currentShot.flywheelVelocityRotPerSec(), currentShot.timeOfFlightSecs()); } - // tof is different if ur driving towards or away because the ball will - // be pushed forward or back by the robots velocity bc physics not just how far it is - //but im not really sure how to implement this at the moment - public static ShotData getRobotVelocityCompensatedSOTMShotData( - Pose2d robotPose, - Translation2d targetTranslation, - ChassisSpeeds fieldRelativeSpeeds, - InterpolatingShotTree tree) { - - ShotData shot = getSOTMShotData(robotPose, targetTranslation, fieldRelativeSpeeds, tree); - Translation2d virtualTarget = - getVirtualSOTMTarget(targetTranslation, fieldRelativeSpeeds, shot.timeOfFlightSecs()); - Translation2d turretTranslation = getTurretPose(robotPose).getTranslation(); - - double translationNorm = (turretTranslation.minus(virtualTarget)).getNorm(); -//doesnt do anything rn, prob just delete - return shot; - } - public static ShotData getCompensatedSOTMShotData( Pose2d robotPose, Translation2d targetTranslation, From e853f2716878c01bd5c7c7532f5cad124e1b0b4d Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 4 Mar 2026 20:42:23 -0800 Subject: [PATCH 11/21] implement newtons method with correct required velocity --- .../subsystems/shooter/TurretSubsystem.java | 7 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 75 +++++++++++++------ 2 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 9de2fce3..a9359509 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -392,13 +392,10 @@ public Command score( Supplier chassisSpeedsSupplier) { return this.run( () -> { - Logger.recordOutput("Robot/Hub Target", FieldUtils.getCurrentHubPose()); - ShotData shotData = AutoAim.getSOTMShotData(robotPoseSupplier.get(), FieldUtils.getCurrentHubTranslation(), chassisSpeedsSupplier.get(), AutoAim.COMP_HUB_SHOT_TREE); - - hoodIO.setHoodPosition(shotData.hoodAngle()); + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotData.flywheelVelocityRotPerSec()); + shotDataSupplier.get().flywheelVelocityRotPerSec()); turretIO.setTurretPosition( AutoAim.getTurretHubTargetRotation( FieldUtils.getCurrentHubTranslation(), diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 9ca85e55..d6c96b94 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -221,48 +221,79 @@ public static ShotData getSOTMShotData( return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } - - public static ShotData getSOTMShotDataNewtonsMethod( +public static ShotData getSOTMShotDataNewtonsMethod( Pose2d robotPose, Translation2d targetTranslation, ChassisSpeeds fieldRelativeSpeeds, InterpolatingShotTree tree) { + + ShotData baseline = tree.calculateShot(robotPose, targetTranslation); Pose2d turretPose = getTurretPose(robotPose); + Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); + + double distance = turretToTarget.getNorm(); + + //get just direction of vector because its vector divded by length + //dont want to account for magnitude bc the speed we are going and shot tree do + //and we just want direction to find dot product + Translation2d shotDirection = turretToTarget.div(distance); + + //dot product! <3 + //get how fast we are going towards where we are shooting + //vectors of robot times direction + //positive if going towrds + //zero is moving perpedicular + //negative is going away + double robotVelocityAlongShot = fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); + + //vel is dis/time so velocity ball needs to go is our distance / tof - the dot product to account for the robots velocity along the shot itself + //because the ball velocity is robot velocity + ball velocity + //required velocity is like horizontal velocity the ball must have so it hits the target while the robot moving + double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; + + return calculateShotAdjustments( + distance, + baseline, + requiredVelocity, + tree + ); + } - ShotData currentShot = tree.calculateShot(robotPose, targetTranslation); - - double currentDistance = turretPose.getTranslation().getDistance(targetTranslation); - double currentTime = currentShot.timeOfFlightSecs(); + /** + * @param distance distance to target + * @param baseline daseline parameters from tree + * @param requiredVelocity required horizontal velocity magnitude + * @return adjusted shooter command + */ + private static ShotData calculateShotAdjustments( + double distance, ShotData baseline, double requiredVelocity, InterpolatingShotTree tree) { + + ShotData currentParams = baseline; + double currentDistance = distance; + double currentTime = currentParams.timeOfFlightSecs(); double currentVelocity = currentDistance / currentTime; - Translation2d virtualTarget = - getVirtualSOTMTarget( - targetTranslation, fieldRelativeSpeeds, currentShot.timeOfFlightSecs()); - - ShotData targetShot = tree.get(turretPose.getTranslation().getDistance(virtualTarget)); - //TODO what is required velcity is it like horizontal so distance/tof? - double requiredVelocity = turretPose.getTranslation().getDistance(virtualTarget) / targetShot.timeOfFlightSecs(); - //currentShot.flywheelVelocityRotPerSec(); - - // 10 rounds for now + // iterate for (int i = 0; i < 10 && Math.abs(currentVelocity - requiredVelocity) > 0.005; i++) { - // estimate derivative by taking a tiny slope final double EPSILON = 0.001; + // get deriv of velocity (dis/time) double lowVel = (currentDistance - EPSILON) / tree.get(currentDistance - EPSILON).timeOfFlightSecs(); double highVel = (currentDistance + EPSILON) / tree.get(currentDistance + EPSILON).timeOfFlightSecs(); double velDeriv = (highVel - lowVel) / (EPSILON * 2); + //newtons method: xn+1 = xn - f(xn)/deriv(xn) + //so estimate for new dist is difference between current velocity required velocity over the deriv + //this way if current vel is larger it will lower current distance to account for that and if requird is larger it will increase to account for that currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; - // update currentVelocity with f(x+1) - currentShot = tree.get(currentDistance); - currentTime = currentShot.timeOfFlightSecs(); + // update + currentParams = tree.get(currentDistance); + currentTime = currentParams.timeOfFlightSecs(); currentVelocity = currentDistance / currentTime; } - - return new ShotData(currentShot.hoodAngle(), currentShot.flywheelVelocityRotPerSec(), currentShot.timeOfFlightSecs()); + return new ShotData(currentParams.hoodAngle(), currentParams.flywheelVelocityRotPerSec(), currentParams.timeOfFlightSecs()); } public static ShotData getCompensatedSOTMShotData( From f772b8956be593fce75ad1b87391c0f92e08a02b Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 4 Mar 2026 21:00:50 -0800 Subject: [PATCH 12/21] update comments --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index d6c96b94..c8c9ff4d 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -247,10 +247,10 @@ public static ShotData getSOTMShotDataNewtonsMethod( //zero is moving perpedicular //negative is going away double robotVelocityAlongShot = fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); - - //vel is dis/time so velocity ball needs to go is our distance / tof - the dot product to account for the robots velocity along the shot itself + + //required velocity is like velocity the ball must have so it hits the target while the robot moving //because the ball velocity is robot velocity + ball velocity - //required velocity is like horizontal velocity the ball must have so it hits the target while the robot moving + //so velocity ball needs to go is our distance / tof - the dot product or velocity along that shot to account for the robots velocity along the shot double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; return calculateShotAdjustments( @@ -286,7 +286,7 @@ private static ShotData calculateShotAdjustments( double velDeriv = (highVel - lowVel) / (EPSILON * 2); //newtons method: xn+1 = xn - f(xn)/deriv(xn) //so estimate for new dist is difference between current velocity required velocity over the deriv - //this way if current vel is larger it will lower current distance to account for that and if requird is larger it will increase to account for that + //this makes sense because if current vel is larger it will lower current distance to account for that and if requird is larger it will increase to account for that currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; // update currentParams = tree.get(currentDistance); From 3d4074d6231cabee77e35a336c834ac5660bfaaf Mon Sep 17 00:00:00 2001 From: vivi-o Date: Wed, 4 Mar 2026 21:01:30 -0800 Subject: [PATCH 13/21] build to format --- .../subsystems/shooter/TurretSubsystem.java | 11 ++- .../java/frc/robot/utils/autoaim/AutoAim.java | 92 ++++++++++--------- 2 files changed, 56 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index a9359509..ad45f846 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -204,14 +204,19 @@ public Command feed( return this.run( () -> { Logger.recordOutput("Robot/Feed Target", feedTarget.get()); - ShotData shotData = AutoAim.getSOTMShotData(robotPoseSupplier.get(), feedTarget.get().getTranslation(), chassisSpeedsSupplier.get(), AutoAim.FEED_SHOT_TREE); + ShotData shotData = + AutoAim.getSOTMShotData( + robotPoseSupplier.get(), + feedTarget.get().getTranslation(), + chassisSpeedsSupplier.get(), + AutoAim.FEED_SHOT_TREE); hoodIO.setHoodPosition(shotData.hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - // hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - // flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + // hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + // flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); turretIO.setTurretPosition( AutoAim.getTurretFeedTargetRotation( feedTarget.get().getTranslation(), diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index c8c9ff4d..caa395a2 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -107,10 +107,10 @@ public class AutoAim { } public static Pose2d getTurretPose(Pose2d robotPose) { - Pose2d turretPose = + Pose2d turretPose = robotPose.transformBy( new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); - return turretPose; + return turretPose; } public static double distanceToHub(Pose2d pose) { @@ -136,7 +136,7 @@ public static Rotation2d getVirtualTargetYaw( public static Translation2d getVirtualSOTMTarget( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { // velocity times shot time is how translated it is - Translation2d vtarget = + Translation2d vtarget = target.minus( new Translation2d( fieldRelativeSpeeds.vxMetersPerSecond * timeOfFlightSecs, @@ -217,54 +217,53 @@ public static ShotData getSOTMShotData( Translation2d virtualTarget = getVirtualSOTMTarget( targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); - Pose2d turretPose = getTurretPose(robotPose); + Pose2d turretPose = getTurretPose(robotPose); return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); } -public static ShotData getSOTMShotDataNewtonsMethod( + public static ShotData getSOTMShotDataNewtonsMethod( Pose2d robotPose, Translation2d targetTranslation, ChassisSpeeds fieldRelativeSpeeds, InterpolatingShotTree tree) { - ShotData baseline = tree.calculateShot(robotPose, targetTranslation); - - Pose2d turretPose = getTurretPose(robotPose); - Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); - - double distance = turretToTarget.getNorm(); - - //get just direction of vector because its vector divded by length - //dont want to account for magnitude bc the speed we are going and shot tree do - //and we just want direction to find dot product - Translation2d shotDirection = turretToTarget.div(distance); - - //dot product! <3 - //get how fast we are going towards where we are shooting - //vectors of robot times direction - //positive if going towrds - //zero is moving perpedicular - //negative is going away - double robotVelocityAlongShot = fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); - - //required velocity is like velocity the ball must have so it hits the target while the robot moving - //because the ball velocity is robot velocity + ball velocity - //so velocity ball needs to go is our distance / tof - the dot product or velocity along that shot to account for the robots velocity along the shot - double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; - - return calculateShotAdjustments( - distance, - baseline, - requiredVelocity, - tree - ); - } + ShotData baseline = tree.calculateShot(robotPose, targetTranslation); + + Pose2d turretPose = getTurretPose(robotPose); + Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); + + double distance = turretToTarget.getNorm(); + + // get just direction of vector because its vector divded by length + // dont want to account for magnitude bc the speed we are going and shot tree do + // and we just want direction to find dot product + Translation2d shotDirection = turretToTarget.div(distance); + + // dot product! <3 + // get how fast we are going towards where we are shooting + // vectors of robot times direction + // positive if going towrds + // zero is moving perpedicular + // negative is going away + double robotVelocityAlongShot = + fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() + + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); + + // required velocity is like velocity the ball must have so it hits the target while the robot + // moving + // because the ball velocity is robot velocity + ball velocity + // so velocity ball needs to go is our distance / tof - the dot product or velocity along that + // shot to account for the robots velocity along the shot + double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; + + return calculateShotAdjustments(distance, baseline, requiredVelocity, tree); + } - /** + /** * @param distance distance to target * @param baseline daseline parameters from tree - * @param requiredVelocity required horizontal velocity magnitude + * @param requiredVelocity required horizontal velocity magnitude * @return adjusted shooter command */ private static ShotData calculateShotAdjustments( @@ -284,16 +283,21 @@ private static ShotData calculateShotAdjustments( double highVel = (currentDistance + EPSILON) / tree.get(currentDistance + EPSILON).timeOfFlightSecs(); double velDeriv = (highVel - lowVel) / (EPSILON * 2); - //newtons method: xn+1 = xn - f(xn)/deriv(xn) - //so estimate for new dist is difference between current velocity required velocity over the deriv - //this makes sense because if current vel is larger it will lower current distance to account for that and if requird is larger it will increase to account for that + // newtons method: xn+1 = xn - f(xn)/deriv(xn) + // so estimate for new dist is difference between current velocity required velocity over the + // deriv + // this makes sense because if current vel is larger it will lower current distance to account + // for that and if requird is larger it will increase to account for that currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; - // update + // update currentParams = tree.get(currentDistance); currentTime = currentParams.timeOfFlightSecs(); currentVelocity = currentDistance / currentTime; } - return new ShotData(currentParams.hoodAngle(), currentParams.flywheelVelocityRotPerSec(), currentParams.timeOfFlightSecs()); + return new ShotData( + currentParams.hoodAngle(), + currentParams.flywheelVelocityRotPerSec(), + currentParams.timeOfFlightSecs()); } public static ShotData getCompensatedSOTMShotData( From 1579c0af677e5d9b3f2279a110f7780690b09561 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 11 Mar 2026 07:54:37 -0700 Subject: [PATCH 14/21] wtf (what the fuel) --- src/main/deploy/choreo/OtoCR.traj | 122 +-- src/main/java/frc/robot/Robot.java | 25 +- .../subsystems/shooter/TurretSubsystem.java | 24 +- src/main/java/frc/robot/utils/FuelSim.java | 958 ++++++++++++++++++ 4 files changed, 1058 insertions(+), 71 deletions(-) create mode 100644 src/main/java/frc/robot/utils/FuelSim.java diff --git a/src/main/deploy/choreo/OtoCR.traj b/src/main/deploy/choreo/OtoCR.traj index b3c63403..79890d4e 100644 --- a/src/main/deploy/choreo/OtoCR.traj +++ b/src/main/deploy/choreo/OtoCR.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.918229579925537, "y":2.544593811035156, "heading":-1.5737992594811685, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.918229579925537, "y":2.544593811035156, "heading":1.5707963267948966, "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}, @@ -14,8 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.918229579925537 m", "val":1.918229579925537}, "y":{"exp":"2.5445938110351562 m", "val":2.544593811035156}, "heading":{"exp":"CR.heading", "val":-1.5737992594811685}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.918229579925537 m", "val":1.918229579925537}, "y":{"exp":"2.5445938110351562 m", "val":2.544593811035156}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "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}, @@ -51,72 +51,56 @@ "differentialTrackWidth":0.5427218 }, "sampleType":"Swerve", - "waypoints":[0.0,2.31226], + "waypoints":[0.0,2.31349], "samples":[ - {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.10002, "ay":6.30784, "alpha":0.01021, "fx":[67.76368,67.89908,67.81123,67.67586], "fy":[104.30559,104.21767,104.2751,104.36285]}, - {"t":0.0367, "x":0.73494, "y":0.72411, "heading":1.5708, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":4.09736, "ay":6.30374, "alpha":0.01427, "fx":[67.7102,67.89937,67.77674,67.58763], "fy":[104.24377,104.12099,104.20138,104.3238]}, - {"t":0.07341, "x":0.74322, "y":0.73685, "heading":1.57081, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":4.08943, "ay":6.29151, "alpha":0.02639, "fx":[67.55057,67.89998,67.67408,67.32486], "fy":[104.05914,103.83274,103.98204,104.20724]}, - {"t":0.11011, "x":0.75702, "y":0.75808, "heading":1.57084, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":2.55761, "ay":3.93519, "alpha":2.60714, "fx":[31.41257,61.50266,51.61639,24.61302], "fy":[59.48868,49.98538,71.41775,79.35702]}, - {"t":0.14681, "x":0.7753, "y":0.7862, "heading":1.57091, "vx":0.54483, "vy":0.83822, "omega":0.09756, "ax":0.00013, "ay":-0.00001, "alpha":5.28428, "fx":[-24.92379,24.93372,24.92799,-24.92956], "fy":[-24.93192,-24.92583,24.93137,24.92595]}, - {"t":0.18351, "x":0.79529, "y":0.81696, "heading":1.57449, "vx":0.54483, "vy":0.83822, "omega":0.2915, "ax":0.0, "ay":0.0, "alpha":4.90794, "fx":[-23.06761,23.23886,23.06771,-23.23877], "fy":[-23.23884,-23.06769,23.23878,23.06763]}, - {"t":0.22022, "x":0.81529, "y":0.84772, "heading":1.58519, "vx":0.54483, "vy":0.83822, "omega":0.47164, "ax":0.0, "ay":0.0, "alpha":4.54735, "fx":[-21.14127,21.75886,21.14127,-21.75886], "fy":[-21.75886,-21.14127,21.75886,21.14127]}, - {"t":0.25692, "x":0.83529, "y":0.87849, "heading":1.6025, "vx":0.54483, "vy":0.83822, "omega":0.63854, "ax":0.0, "ay":0.0, "alpha":4.20333, "fx":[-19.1908,20.44798,19.1908,-20.44798], "fy":[-20.44798,-19.1908,20.44798,19.1908]}, - {"t":0.29362, "x":0.85528, "y":0.90925, "heading":1.62594, "vx":0.54483, "vy":0.83822, "omega":0.79281, "ax":0.0, "ay":0.0, "alpha":3.87644, "fx":[-17.25159,19.26733,17.25159,-19.26733], "fy":[-19.26733,-17.25159,19.26733,17.25159]}, - {"t":0.33032, "x":0.87528, "y":0.94002, "heading":1.65504, "vx":0.54483, "vy":0.83822, "omega":0.93508, "ax":0.0, "ay":0.0, "alpha":3.56699, "fx":[-15.35189,18.1836,15.35189,-18.18361], "fy":[-18.18361,-15.35189,18.18361,15.35189]}, - {"t":0.36703, "x":0.89528, "y":0.97078, "heading":1.68936, "vx":0.54483, "vy":0.83822, "omega":1.066, "ax":0.0, "ay":0.0, "alpha":3.27504, "fx":[-13.5142,17.16914,13.5142,-17.16914], "fy":[-17.16914,-13.5142,17.16914,13.5142]}, - {"t":0.40373, "x":0.91527, "y":1.00155, "heading":1.72848, "vx":0.54483, "vy":0.83822, "omega":1.1862, "ax":0.0, "ay":0.0, "alpha":3.00043, "fx":[-11.75629,16.20174,11.75629,-16.20173], "fy":[-16.20174,-11.75629,16.20173,11.75629]}, - {"t":0.44043, "x":0.93527, "y":1.03231, "heading":1.77202, "vx":0.54483, "vy":0.83822, "omega":1.29633, "ax":0.0, "ay":0.0, "alpha":2.74279, "fx":[-10.09204,15.26425,10.09204,-15.26425], "fy":[-15.26425,-10.09204,15.26425,10.09204]}, - {"t":0.47713, "x":0.95527, "y":1.06308, "heading":1.8196, "vx":0.54483, "vy":0.83822, "omega":1.39699, "ax":0.0, "ay":0.0, "alpha":2.50162, "fx":[-8.53208,14.34408,8.53208,-14.34408], "fy":[-14.34408,-8.53208,14.34408,8.53208]}, - {"t":0.51384, "x":0.97526, "y":1.09384, "heading":1.87087, "vx":0.54483, "vy":0.83822, "omega":1.48881, "ax":0.0, "ay":0.0, "alpha":2.27624, "fx":[-7.0843,13.43254,7.0843,-13.43254], "fy":[-13.43254,-7.0843,13.43254,7.0843]}, - {"t":0.55054, "x":0.99526, "y":1.12461, "heading":1.92551, "vx":0.54483, "vy":0.83822, "omega":1.57235, "ax":0.0, "ay":0.0, "alpha":2.06591, "fx":[-5.75425,12.52431,5.75426,-12.5243], "fy":[-12.52431,-5.75426,12.5243,5.75425]}, - {"t":0.58724, "x":1.01526, "y":1.15537, "heading":1.98322, "vx":0.54483, "vy":0.83822, "omega":1.64818, "ax":0.0, "ay":0.0, "alpha":1.86978, "fx":[-4.54549,11.61677,4.5455,-11.61676], "fy":[-11.61677,-4.5455,11.61677,4.54549]}, - {"t":0.62394, "x":1.03525, "y":1.18614, "heading":2.04371, "vx":0.54483, "vy":0.83822, "omega":1.7168, "ax":0.0, "ay":0.0, "alpha":1.68693, "fx":[-3.45987,10.70953,3.45988,-10.70953], "fy":[-10.70953,-3.45988,10.70953,3.45987]}, - {"t":0.66065, "x":1.05525, "y":1.2169, "heading":2.10672, "vx":0.54483, "vy":0.83822, "omega":1.77872, "ax":0.0, "ay":0.0, "alpha":1.51644, "fx":[-2.49781,9.8039,2.49781,-9.80389], "fy":[-9.8039,-2.49781,9.80389,2.49781]}, - {"t":0.69735, "x":1.07525, "y":1.24767, "heading":2.17201, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":1.35734, "fx":[-1.6585,8.90244,1.65851,-8.90244], "fy":[-8.90244,-1.65851,8.90244,1.6585]}, - {"t":0.73405, "x":1.09524, "y":1.27843, "heading":2.23933, "vx":0.54483, "vy":0.83822, "omega":1.88419, "ax":0.0, "ay":0.0, "alpha":1.20866, "fx":[-0.94018,8.00867,0.94018,-8.00867], "fy":[-8.00867,-0.94018,8.00867,0.94018]}, - {"t":0.77075, "x":1.11524, "y":1.3092, "heading":2.30849, "vx":0.54483, "vy":0.83822, "omega":1.92855, "ax":0.0, "ay":0.0, "alpha":1.06943, "fx":[-0.34024,7.12671,0.34024,-7.12671], "fy":[-7.12671,-0.34024,7.12671,0.34024]}, - {"t":0.80746, "x":1.13524, "y":1.33996, "heading":2.37927, "vx":0.54483, "vy":0.83822, "omega":1.9678, "ax":0.0, "ay":0.0, "alpha":0.93872, "fx":[0.14451,6.26109,-0.14452,-6.2611], "fy":[-6.26109,0.14452,6.2611,-0.14451]}, - {"t":0.84416, "x":1.15523, "y":1.37073, "heading":2.45149, "vx":0.54483, "vy":0.83822, "omega":2.00226, "ax":0.0, "ay":0.0, "alpha":0.81559, "fx":[0.51777,5.41659,-0.51778,-5.4166], "fy":[-5.41659,0.51778,5.4166,-0.51777]}, - {"t":0.88086, "x":1.17523, "y":1.40149, "heading":2.52498, "vx":0.54483, "vy":0.83822, "omega":2.03219, "ax":0.0, "ay":0.0, "alpha":0.69914, "fx":[0.78355,4.59807,-0.78356,-4.59809], "fy":[-4.59808,0.78356,4.59809,-0.78355]}, - {"t":0.91756, "x":1.19523, "y":1.43226, "heading":2.59957, "vx":0.54483, "vy":0.83822, "omega":2.05785, "ax":0.0, "ay":0.0, "alpha":0.58848, "fx":[0.94611,3.81041,-0.94612,-3.81043], "fy":[-3.81042,0.94612,3.81043,-0.94611]}, - {"t":0.95427, "x":1.21522, "y":1.46302, "heading":2.6751, "vx":0.54483, "vy":0.83822, "omega":2.07945, "ax":0.0, "ay":0.0, "alpha":0.48277, "fx":[1.00981,3.05844,-1.00982,-3.05846], "fy":[-3.05845,1.00982,3.05846,-1.00981]}, - {"t":0.99097, "x":1.23522, "y":1.49379, "heading":2.75142, "vx":0.54483, "vy":0.83822, "omega":2.09717, "ax":0.0, "ay":0.0, "alpha":0.38116, "fx":[0.97907,2.34691,-0.97908,-2.34692], "fy":[-2.34691,0.97908,2.34692,-0.97907]}, - {"t":1.02767, "x":1.25522, "y":1.52455, "heading":2.82839, "vx":0.54483, "vy":0.83822, "omega":2.11116, "ax":0.0, "ay":0.0, "alpha":0.28283, "fx":[0.85826,1.68046,-0.85827,-1.68046], "fy":[-1.68046,0.85826,1.68046,-0.85826]}, - {"t":1.06437, "x":1.27521, "y":1.55532, "heading":2.90587, "vx":0.54483, "vy":0.83822, "omega":2.12154, "ax":0.0, "ay":0.0, "alpha":0.18698, "fx":[0.65167,1.06367,-0.65167,-1.06367], "fy":[-1.06367,0.65167,1.06367,-0.65167]}, - {"t":1.10108, "x":1.29521, "y":1.58608, "heading":2.98374, "vx":0.54483, "vy":0.83822, "omega":2.1284, "ax":0.0, "ay":0.0, "alpha":0.09278, "fx":[0.36346,0.50108,-0.36346,-0.50107], "fy":[-0.50108,0.36346,0.50107,-0.36346]}, - {"t":1.13778, "x":1.31521, "y":1.61685, "heading":3.06186, "vx":0.54483, "vy":0.83822, "omega":2.13181, "ax":0.0, "ay":0.0, "alpha":-0.00055, "fx":[-0.00237,-0.00278,0.00238,0.00279], "fy":[0.00278,-0.00238,-0.00279,0.00237]}, - {"t":1.17448, "x":1.3352, "y":1.64761, "heading":3.1401, "vx":0.54483, "vy":0.83822, "omega":2.13179, "ax":0.0, "ay":0.0, "alpha":-0.09383, "fx":[-0.44197,-0.4433,0.44199,0.44331], "fy":[0.4433,-0.44199,-0.44331,0.44198]}, - {"t":1.21118, "x":1.3552, "y":1.67838, "heading":-3.06484, "vx":0.54483, "vy":0.83822, "omega":2.12834, "ax":0.0, "ay":0.0, "alpha":-0.18787, "fx":[-0.95164,-0.81573,0.95166,0.81575], "fy":[0.81573,-0.95166,-0.81574,0.95164]}, - {"t":1.24789, "x":1.3752, "y":1.70914, "heading":-2.98673, "vx":0.54483, "vy":0.83822, "omega":2.12145, "ax":0.0, "ay":0.0, "alpha":-0.28351, "fx":[-1.52773,-1.11514,1.52775,1.11516], "fy":[1.11514,-1.52775,-1.11515,1.52774]}, - {"t":1.28459, "x":1.39519, "y":1.73991, "heading":-2.90886, "vx":0.54483, "vy":0.83822, "omega":2.11104, "ax":0.0, "ay":0.0, "alpha":-0.38156, "fx":[-2.16662,-1.33634,2.16664,1.33635], "fy":[1.33634,-2.16664,-1.33635,2.16662]}, - {"t":1.32129, "x":1.41519, "y":1.77067, "heading":-2.83138, "vx":0.54483, "vy":0.83822, "omega":2.09704, "ax":0.0, "ay":0.0, "alpha":-0.48287, "fx":[-2.8646,-1.47387,2.86462,1.47388], "fy":[1.47387,-2.86462,-1.47388,2.86461]}, - {"t":1.35799, "x":1.43519, "y":1.80144, "heading":-2.75442, "vx":0.54483, "vy":0.83822, "omega":2.07932, "ax":0.0, "ay":0.0, "alpha":-0.58831, "fx":[-3.61781,-1.52201,3.61782,1.52202], "fy":[1.52202,-3.61782,-1.52202,3.61782]}, - {"t":1.3947, "x":1.45518, "y":1.8322, "heading":-2.6781, "vx":0.54483, "vy":0.83822, "omega":2.05772, "ax":0.0, "ay":0.0, "alpha":-0.69872, "fx":[-4.42213,-1.47481,4.42214,1.47482], "fy":[1.47481,-4.42214,-1.47482,4.42213]}, - {"t":1.4314, "x":1.47518, "y":1.86296, "heading":-2.60258, "vx":0.54483, "vy":0.83822, "omega":2.03208, "ax":0.0, "ay":0.0, "alpha":-0.815, "fx":[-5.27312,-1.32616,5.27312,1.32615], "fy":[1.32616,-5.27312,-1.32615,5.27312]}, - {"t":1.4681, "x":1.49518, "y":1.89373, "heading":-2.528, "vx":0.54483, "vy":0.83822, "omega":2.00217, "ax":0.0, "ay":0.0, "alpha":-0.93802, "fx":[-6.16599,-1.06987,6.16598,1.06987], "fy":[1.06987,-6.16598,-1.06987,6.16599]}, - {"t":1.5048, "x":1.51517, "y":1.92449, "heading":-2.45451, "vx":0.54483, "vy":0.83822, "omega":1.96774, "ax":0.0, "ay":0.0, "alpha":-1.06871, "fx":[-7.09556,-0.69987,7.09555,0.69987], "fy":[0.69987,-7.09555,-0.69987,7.09556]}, - {"t":1.54151, "x":1.53517, "y":1.95526, "heading":-2.38229, "vx":0.54483, "vy":0.83822, "omega":1.92851, "ax":0.0, "ay":0.0, "alpha":-1.20797, "fx":[-8.05635,-0.21029,8.05634,0.21028], "fy":[0.21029,-8.05634,-0.21028,8.05635]}, - {"t":1.57821, "x":1.55517, "y":1.98602, "heading":-2.31151, "vx":0.54483, "vy":0.83822, "omega":1.88418, "ax":0.0, "ay":0.0, "alpha":-1.35674, "fx":[-9.04261,0.40434,9.0426,-0.40435], "fy":[-0.40434,-9.04261,0.40434,9.04261]}, - {"t":1.61491, "x":1.57516, "y":2.01679, "heading":-2.24235, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":-1.51597, "fx":[-10.04849,1.14888,10.04848,-1.14888], "fy":[-1.14888,-10.04848,1.14888,10.04849]}, - {"t":1.65161, "x":1.59516, "y":2.04755, "heading":-2.17503, "vx":0.54483, "vy":0.83822, "omega":1.77874, "ax":0.0, "ay":0.0, "alpha":-1.6866, "fx":[-11.06818,2.0274,11.06817,-2.02741], "fy":[-2.0274,-11.06817,2.02741,11.06818]}, - {"t":1.68832, "x":1.61516, "y":2.07832, "heading":-2.10974, "vx":0.54483, "vy":0.83822, "omega":1.71684, "ax":0.0, "ay":0.0, "alpha":-1.86958, "fx":[-12.09621,3.04297,12.09621,-3.04297], "fy":[-3.04297,-12.09621,3.04297,12.09621]}, - {"t":1.72502, "x":1.63515, "y":2.10908, "heading":-2.04673, "vx":0.54483, "vy":0.83822, "omega":1.64822, "ax":0.0, "ay":0.0, "alpha":-2.06583, "fx":[-13.12772,4.19739,13.12772,-4.19739], "fy":[-4.19739,-13.12772,4.19739,13.12772]}, - {"t":1.76172, "x":1.65515, "y":2.13985, "heading":-1.98624, "vx":0.54483, "vy":0.83822, "omega":1.5724, "ax":0.0, "ay":0.0, "alpha":-2.27626, "fx":[-14.15883,5.49098,14.15883,-5.49098], "fy":[-5.49098,-14.15883,5.49098,14.15883]}, - {"t":1.79842, "x":1.67515, "y":2.17061, "heading":-1.92853, "vx":0.54483, "vy":0.83822, "omega":1.48886, "ax":0.0, "ay":0.0, "alpha":-2.5017, "fx":[-15.18711,6.9223,15.18712,-6.9223], "fy":[-6.9223,-15.18711,6.9223,15.18711]}, - {"t":1.83513, "x":1.69514, "y":2.20138, "heading":-1.87388, "vx":0.54483, "vy":0.83822, "omega":1.39704, "ax":0.0, "ay":0.0, "alpha":-2.74291, "fx":[-16.21206,8.48788,16.21207,-8.48788], "fy":[-8.48788,-16.21206,8.48788,16.21206]}, - {"t":1.87183, "x":1.71514, "y":2.23214, "heading":-1.82261, "vx":0.54483, "vy":0.83822, "omega":1.29637, "ax":0.0, "ay":0.0, "alpha":-3.00056, "fx":[-17.23572,10.18193,17.23572,-10.18192], "fy":[-10.18193,-17.23572,10.18192,17.23572]}, - {"t":1.90853, "x":1.73514, "y":2.26291, "heading":-1.77503, "vx":0.54483, "vy":0.83822, "omega":1.18624, "ax":0.0, "ay":0.0, "alpha":-3.27517, "fx":[-18.26325,11.99599,18.26325,-11.99599], "fy":[-11.99599,-18.26325,11.99599,18.26325]}, - {"t":1.94523, "x":1.75513, "y":2.29367, "heading":-1.73149, "vx":0.54483, "vy":0.83822, "omega":1.06603, "ax":0.0, "ay":0.0, "alpha":-3.56711, "fx":[-19.30365,13.91863,19.30365,-13.91863], "fy":[-13.91863,-19.30365,13.91863,19.30365]}, - {"t":1.98194, "x":1.77513, "y":2.32444, "heading":-1.69236, "vx":0.54483, "vy":0.83822, "omega":0.93511, "ax":0.0, "ay":0.0, "alpha":-3.87653, "fx":[-20.3704,15.93499,20.37041,-15.93499], "fy":[-15.93499,-20.3704,15.93499,20.3704]}, - {"t":2.01864, "x":1.79513, "y":2.3552, "heading":-1.65804, "vx":0.54483, "vy":0.83822, "omega":0.79283, "ax":0.0, "ay":0.0, "alpha":-4.20339, "fx":[-21.48208,18.02635,21.48208,-18.02635], "fy":[-18.02635,-21.48208,18.02635,21.48208]}, - {"t":2.05534, "x":1.81512, "y":2.38597, "heading":-1.62894, "vx":0.54483, "vy":0.83822, "omega":0.63856, "ax":0.0, "ay":0.0, "alpha":-4.54738, "fx":[-22.66288,20.16948,22.66288,-20.16949], "fy":[-20.16948,-22.66288,20.16949,22.66288]}, - {"t":2.09204, "x":1.83512, "y":2.41673, "heading":-1.60551, "vx":0.54483, "vy":0.83822, "omega":0.47166, "ax":0.0, "ay":0.0, "alpha":-4.90795, "fx":[-23.94303,22.33592,23.94293,-22.33601], "fy":[-22.33593,-23.94295,22.336,23.943]}, - {"t":2.12875, "x":1.85512, "y":2.4475, "heading":-1.5882, "vx":0.54483, "vy":0.83822, "omega":0.29152, "ax":-0.00013, "ay":0.00001, "alpha":-5.28426, "fx":[-25.36075,24.48905,25.35649,-24.49329], "fy":[-24.49088,-25.35866,24.49148,25.35858]}, - {"t":2.16545, "x":1.87511, "y":2.47826, "heading":-1.5775, "vx":0.54483, "vy":0.83822, "omega":0.09758, "ax":-2.55761, "ay":-3.93519, "alpha":-2.6077, "fx":[-51.72521,-24.69926,-31.26056,-61.45938], "fy":[-71.33006,-79.3859,-59.60579,-49.92723]}, - {"t":2.20215, "x":1.89339, "y":2.50638, "heading":-1.57391, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":-4.08943, "ay":-6.29151, "alpha":-0.02639, "fx":[-67.67499,-67.32506,-67.54967,-67.89981], "fy":[-103.98145,-104.20712,-104.05972,-103.83285]}, - {"t":2.23885, "x":1.90719, "y":2.5276, "heading":-1.57385, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":-4.09736, "ay":-6.30374, "alpha":-0.01428, "fx":[-67.77723,-67.58773,-67.70973,-67.89928], "fy":[-104.20107,-104.32374,-104.24407,-104.12105]}, - {"t":2.27556, "x":1.91547, "y":2.54035, "heading":-1.57381, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":-4.10002, "ay":-6.30784, "alpha":-0.01021, "fx":[-67.81157,-67.67592,-67.76334,-67.89902], "fy":[-104.27488,-104.36281,-104.30581,-104.2177]}, - {"t":2.31226, "x":1.91823, "y":2.54459, "heading":-1.5738, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.10093, "ay":6.30926, "alpha":0.0, "fx":[67.80249,67.80249,67.80249,67.80249], "fy":[104.31375,104.31375,104.31375,104.31375]}, + {"t":0.04922, "x":0.73715, "y":0.72751, "heading":1.5708, "vx":0.20186, "vy":0.31056, "omega":0.0, "ax":4.09653, "ay":6.30248, "alpha":0.0, "fx":[67.72967,67.72967,67.72967,67.72967], "fy":[104.20171,104.20171,104.20171,104.20171]}, + {"t":0.09845, "x":0.75205, "y":0.75043, "heading":1.5708, "vx":0.40351, "vy":0.62079, "omega":0.0, "ax":2.8719, "ay":4.41841, "alpha":0.0, "fx":[47.48246,47.48246,47.48246,47.48246], "fy":[73.0515,73.0515,73.0515,73.0515]}, + {"t":0.14767, "x":0.77539, "y":0.78634, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.00001, "ay":0.00002, "alpha":0.0, "fx":[0.00022,0.00022,0.00022,0.00022], "fy":[0.00034,0.00034,0.00034,0.00034]}, + {"t":0.19689, "x":0.80221, "y":0.8276, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.24612, "x":0.82903, "y":0.86886, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.29534, "x":0.85585, "y":0.91013, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.34456, "x":0.88267, "y":0.95139, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.39379, "x":0.90949, "y":0.99265, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.44301, "x":0.93631, "y":1.03392, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.49223, "x":0.96313, "y":1.07518, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.54146, "x":0.98995, "y":1.11644, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.59068, "x":1.01677, "y":1.1577, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.6399, "x":1.04359, "y":1.19897, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.68913, "x":1.07041, "y":1.24023, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.73835, "x":1.09723, "y":1.28149, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.78757, "x":1.12405, "y":1.32276, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.8368, "x":1.15087, "y":1.36402, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.88602, "x":1.17769, "y":1.40528, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.93524, "x":1.20451, "y":1.44655, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.98447, "x":1.23133, "y":1.48781, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.03369, "x":1.25815, "y":1.52907, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.08291, "x":1.28497, "y":1.57033, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.13214, "x":1.31179, "y":1.6116, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.18136, "x":1.33861, "y":1.65286, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.23058, "x":1.36544, "y":1.69412, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.27981, "x":1.39226, "y":1.73539, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.32903, "x":1.41908, "y":1.77665, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.37825, "x":1.4459, "y":1.81791, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.42747, "x":1.47272, "y":1.85917, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.4767, "x":1.49954, "y":1.90044, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.52592, "x":1.52636, "y":1.9417, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.57514, "x":1.55318, "y":1.98296, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.62437, "x":1.58, "y":2.02423, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.67359, "x":1.60682, "y":2.06549, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.72281, "x":1.63364, "y":2.10675, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.77204, "x":1.66046, "y":2.14801, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.82126, "x":1.68728, "y":2.18928, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.87048, "x":1.7141, "y":2.23054, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.91971, "x":1.74092, "y":2.2718, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":1.96893, "x":1.76774, "y":2.31307, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.01815, "x":1.79456, "y":2.35433, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.06738, "x":1.82138, "y":2.39559, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, + {"t":2.1166, "x":1.8482, "y":2.43686, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":-0.00001, "ay":-0.00002, "alpha":0.0, "fx":[-0.00022,-0.00022,-0.00022,-0.00022], "fy":[-0.00034,-0.00034,-0.00034,-0.00034]}, + {"t":2.16582, "x":1.87502, "y":2.47812, "heading":1.5708, "vx":0.54487, "vy":0.83828, "omega":0.0, "ax":-2.8719, "ay":-4.41841, "alpha":0.0, "fx":[-47.48246,-47.48246,-47.48246,-47.48246], "fy":[-73.0515,-73.0515,-73.0515,-73.0515]}, + {"t":2.21505, "x":1.89836, "y":2.51403, "heading":1.5708, "vx":0.40351, "vy":0.62079, "omega":0.0, "ax":-4.09653, "ay":-6.30248, "alpha":0.0, "fx":[-67.72967,-67.72967,-67.72967,-67.72967], "fy":[-104.20171,-104.20171,-104.20171,-104.20171]}, + {"t":2.26427, "x":1.91326, "y":2.53695, "heading":1.5708, "vx":0.20186, "vy":0.31056, "omega":0.0, "ax":-4.10093, "ay":-6.30926, "alpha":0.0, "fx":[-67.80249,-67.80249,-67.80249,-67.80249], "fy":[-104.31375,-104.31375,-104.31375,-104.31375]}, + {"t":2.31349, "x":1.91823, "y":2.54459, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8440d11..ac7772ed 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -69,6 +69,7 @@ import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.ClimbTargets; import frc.robot.utils.FieldUtils.TrenchPoses; +import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; import java.util.Arrays; import java.util.Optional; @@ -201,6 +202,8 @@ public enum RobotEdition { private final CANdleSubsystem candle = new CANdleSubsystem(new CANdleIOReal(0, CANdleSubsystem.getCandleConfig(), canivore)); + private FuelSim fuelSim = new FuelSim(); + // climber only exists for the comp bot - this is accounted for later private final Superstructure superstructure; @@ -413,7 +416,8 @@ public Robot() { : new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore), ROBOT_MODE == RobotMode.REAL ? new CANcoderIO(4, TurretSubsystem.getCancoder26tConfigs(), canivore) - : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore)); + : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore), + fuelSim); break; } climber = @@ -608,6 +612,24 @@ public Robot() { "Interrputing: " + (interrupting.isPresent() ? interrupting.get().getName() : "none")); }); + + fuelSim.spawnStartingFuel(); + fuelSim.registerIntake( + Units.inchesToMeters(-14), + Units.inchesToMeters(14), + Units.inchesToMeters(14), + Units.inchesToMeters(20) // robot-centric coordinates for bounding box in meters + // (optional) BooleanSupplier for whether the intake should be active at a given moment + ); // (optional) Runnable called whenever a fuel is intaked + + fuelSim.registerRobot( + Units.inchesToMeters(28), // from left to right in meters + Units.inchesToMeters(28), // from front to back in meters + Units.inchesToMeters(4), // from floor to top of bumpers in meters + swerve::getPose, // Supplier of robot pose + swerve + ::getVelocityFieldRelative); // Supplier of field-centric chassis speeds + fuelSim.start(); } /** Scales a joystick value for teleop driving */ @@ -981,6 +1003,7 @@ public void simulationInit() { @Override public void simulationPeriodic() { + fuelSim.updateSim(); // Log zeroed poses for mechs and robot for debugging in sim Logger.recordOutput( "Robot/Zeroed Mechanism Poses", diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index aceb063c..6e7bb0e4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -19,6 +21,8 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; @@ -29,9 +33,11 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; +import frc.robot.utils.FuelSim; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -138,17 +144,21 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { "Turret may have gone past hardstop!! Reoffset cancoders + min/max position", AlertType.kError); + private FuelSim fuelSim; + public TurretSubsystem( FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, CANcoderIO cancoder24t, - CANcoderIO cancoder26t) { + CANcoderIO cancoder26t, + FuelSim fuelSim) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; this.cancoder24t = cancoder24t; this.cancoder26t = cancoder26t; + this.fuelSim = fuelSim; // assume we start up at min angle and not 0 hoodIO.resetEncoder(HOOD_MIN_ANGLE); @@ -202,6 +212,18 @@ public void periodic() { if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert } + @Override + public void simulationPeriodic() { + if (Superstructure.getState().isAScoreState()) + fuelSim.launchFuel( + LinearVelocity.ofBaseUnits( + flywheelIO.getSetpointRotPerSec() * Math.PI * FLYWHEEL_DIAMETER_INCHES, + MetersPerSecond), + Rotation2d.kCW_90deg.minus(hoodIO.getHoodSetpoint()).getMeasure(), + turretIO.getTurretSetpoint().getMeasure(), + Distance.ofBaseUnits(0.350341, Meters)); + } + public static CANcoderConfiguration getCancoder24tConfigs() { CANcoderConfiguration config = new CANcoderConfiguration(); diff --git a/src/main/java/frc/robot/utils/FuelSim.java b/src/main/java/frc/robot/utils/FuelSim.java new file mode 100644 index 00000000..e3f34055 --- /dev/null +++ b/src/main/java/frc/robot/utils/FuelSim.java @@ -0,0 +1,958 @@ +// https://github.com/hammerheads5000/FuelSim +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; + +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.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +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.wpilibj.Timer; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class FuelSim { + protected static final double PERIOD = 0.02; // sec + protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air + protected static final double AIR_DENSITY = 1.2041; // kg/m^3 + protected static final double FIELD_COR = + Math.sqrt(22 / 51.5); // coefficient of restitution with the field + protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + protected static final double NET_COR = 0.2; // coefficient of restitution with the net + protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + protected static final double FUEL_RADIUS = 0.075; + protected static final double FIELD_LENGTH = 16.51; + protected static final double FIELD_WIDTH = 8.04; + protected static final double TRENCH_WIDTH = 1.265; + protected static final double TRENCH_BLOCK_WIDTH = 0.305; + protected static final double TRENCH_HEIGHT = 0.565; + protected static final double TRENCH_BAR_HEIGHT = 0.102; + protected static final double TRENCH_BAR_WIDTH = 0.152; + protected static final double FRICTION = + 0.1; // proportion of horizontal vel to lose per sec while on ground + protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs + protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: + // https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + protected static final double DRAG_COF = 0.47; // dimensionless + protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH - 1.57, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected void update(boolean simulateAirResistance, int subticks) { + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); + } + handleFieldCollisions(subticks); + } + + protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) + return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = + start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) return; // not intersecting line + // Back into 3D + Translation3d normal = + new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } + + protected void handleFieldCollisions(int subticks) { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB, subticks); + handleHubCollisions(Hub.RED_HUB, subticks); + + handleTrenchCollisions(); + } + + protected void handleHubCollisions(Hub hub, int subticks) { + hub.handleHubInteraction(this, subticks); + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } + + protected void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d( + FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + protected static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + protected static final double CELL_SIZE = 0.25; + protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + + private final ArrayList> activeCells = new ArrayList<>(); + + protected void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (ArrayList cell : activeCells) { + cell.clear(); + } + activeCells.clear(); + + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + if (grid[col][row].size() == 1) { + activeCells.add(grid[col][row]); + } + } + } + + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); + } + } + } + } + } + } + } + } + + protected ArrayList fuels = new ArrayList<>(); + protected boolean running = false; + protected boolean simulateAirResistance = false; + protected Supplier robotPoseSupplier = null; + protected Supplier robotFieldSpeedsSupplier = null; + protected double robotWidth; // size along the robot's y axis + protected double robotLength; // size along the robot's x axis + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + protected double loggingFreqHz = 10; + protected Timer loggingTimer = new Timer(); + + /** + * Creates a new instance of FuelSim + * + * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} + * structs. + */ + public FuelSim(String tableKey) { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } + } + + fuelPublisher = + NetworkTableInstance.getDefault() + .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) + .publish(); + } + + /** Creates a new instance of FuelSim with log path "/Fuel Simulation" */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** Clears the field of fuel */ + public void clearFuel() { + fuels.clear(); + } + + /** Spawns fuel in the neutral zone and depots */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus( + new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } + + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } + } + + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + protected StructArrayPublisher fuelPublisher; + + /** Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** Start the simulation. `updateSim` must still be called every loop */ + public void start() { + running = true; + loggingTimer.restart(); + } + + /** Pause the simulation. */ + public void stop() { + running = false; + loggingTimer.stop(); + } + + /** Enables accounting for drag force in physics step * */ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * + * @param subticks physics iteration per loop (default: 5) + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Sets the frequency to publish fuel translations to NetworkTables Used to improve performance in + * AdvantageScope + * + * @param loggingFreqHz update frequency in hertz + */ + public void setLoggingFrequency(double loggingFreqHz) { + this.loggingFreqHz = loggingFreqHz; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight from the ground + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** To be called periodically Will do nothing if sim is not running */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + /** Run the simulation forward 1 time step (0.02s) */ + public void stepSim() { + for (int i = 0; i < subticks; i++) { + for (Fuel fuel : fuels) { + fuel.update(this.simulateAirResistance, this.subticks); + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } + } + + if (loggingTimer.advanceIfElapsed(1.0 / loggingFreqHz)) { + logFuels(); + } + } + + /** + * Adds a fuel onto the field + * + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot + * movement + * + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching + * straight up + * @param turretYaw Robot-relative turret yaw + * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's + * bumper height, or else it will collide with your robot immediately. + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel( + LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } + + Pose3d launchPose = + new Pose3d(this.robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d(Meters.zero(), Meters.zero(), launchHeight), + Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = + horizontalVel + * Math.cos(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = + horizontalVel + * Math.sin(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero).relativeTo(robot).getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) + return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); + } + + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d( + normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) + fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + protected void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } + + protected void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; + } + } + } + } + + protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) + return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); + } + + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); + } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, + Distance xMax, + Distance yMin, + Distance yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), + xMax.in(Meters), + yMin.in(Meters), + yMax.in(Meters), + ableToIntake, + intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); + } + + public static class Hub { + public static final Hub BLUE_HUB = + new Hub( + new Translation2d(4.61, FIELD_WIDTH / 2), + new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), + 1); + public static final Hub RED_HUB = + new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + protected static final double ENTRY_HEIGHT = 1.83; + protected static final double ENTRY_RADIUS = 0.56; + + protected static final double SIDE = 1.2; + + protected static final double NET_HEIGHT_MAX = 3.057; + protected static final double NET_HEIGHT_MIN = 1.5; + protected static final double NET_OFFSET = SIDE / 2 + 0.261; + protected static final double NET_WIDTH = 1.484; + + protected final Translation2d center; + protected final Translation3d exit; + protected final int exitVelXMult; + + protected int score = 0; + + protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + + protected boolean didFuelScore(Fuel fuel, int subticks) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + } + + protected Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + /** Reset this hub's score to 0 */ + public void resetScore() { + score = 0; + } + + /** + * Get the current count of fuel scored in this hub + * + * @return + */ + public int getScore() { + return score; + } + + protected void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d( + center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + } + + protected double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 + || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } + } + } + + protected class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + protected SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; + } + + protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; + + Translation2d fuelRelativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = + fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } +} From 2439ad8ab5febcf48552c819683ad3742a23ce18 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 18 Mar 2026 12:36:47 -0700 Subject: [PATCH 15/21] this isn't really working --- src/main/java/frc/robot/Robot.java | 23 +++++++---- src/main/java/frc/robot/Superstructure.java | 8 ++++ .../subsystems/shooter/TurretSubsystem.java | 38 +++++++++++++++---- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e02a72f8..bd48e378 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -593,14 +593,7 @@ public Robot() { + (interrupting.isPresent() ? interrupting.get().getName() : "none")); }); - fuelSim.spawnStartingFuel(); - fuelSim.registerIntake( - Units.inchesToMeters(-14), - Units.inchesToMeters(14), - Units.inchesToMeters(14), - Units.inchesToMeters(20) // robot-centric coordinates for bounding box in meters - // (optional) BooleanSupplier for whether the intake should be active at a given moment - ); // (optional) Runnable called whenever a fuel is intaked + // fuelSim.spawnStartingFuel(); fuelSim.registerRobot( Units.inchesToMeters(28), // from left to right in meters @@ -609,6 +602,20 @@ public Robot() { swerve::getPose, // Supplier of robot pose swerve ::getVelocityFieldRelative); // Supplier of field-centric chassis speeds + + fuelSim.registerIntake( + Units.inchesToMeters(-14), + Units.inchesToMeters(14), + Units.inchesToMeters(14), + Units.inchesToMeters(20), // robot-centric coordinates for bounding box in meters + () -> + Superstructure.getState() + .isAnIntakeState() // (optional) BooleanSupplier for whether the intake should be + // active at a given moment + ); // (optional) Runnable called whenever a fuel is intaked + + fuelSim.setSubticks(5); + fuelSim.start(); } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f9fdb6b4..3e0fb73c 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -68,6 +68,14 @@ public boolean isAScoreState() { public boolean isAFeedState() { return this == FEED || this == SPIN_UP_FEED || this == SPIN_UP_FEED_FLOW || this == FEED_FLOW; } + + public boolean isAnIntakeState() { + return this == INTAKE + || this == SPIN_UP_FEED_FLOW + || this == FEED_FLOW + || this == SPIN_UP_SCORE_FLOW + || this == SCORE_FLOW; + } } public enum ShotTarget { diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 8c6d84ae..da5e7b63 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,9 +4,10 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -29,16 +30,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; import frc.robot.utils.FuelSim; -import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.BooleanSupplier; @@ -192,15 +188,41 @@ public void periodic() { // && (getCalculatedTurretRotations().getDegrees() // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert + + if (Superstructure.getState().isAScoreState()) { + System.out.println("launching fuel"); + fuelSim.launchFuel( + // LinearVelocity.ofBaseUnits( + // flywheelIO.getSetpointRotPerSec() + // * Math.PI + // * Units.inchesToMeters(FLYWHEEL_DIAMETER_INCHES), + // // 2, + // MetersPerSecond), + // there are few things i despise more than the units library + // InchesPerSecond.of( + // flywheelIO.getSetpointRotPerSec() * Math.PI * FLYWHEEL_DIAMETER_INCHES), + InchesPerSecond.of(flywheelIO.getSetpointRotPerSec() * 2 * Math.PI), + // Rotation2d.kCW_90deg.minus( + Rotation2d.fromDegrees(90) + .minus(hoodIO.getHoodSetpoint()) + // ) + .getMeasure(), + // Rotation2d.fromDegrees(90).getMeasure(), + turretIO.getTurretSetpoint().getMeasure(), + // Distance.ofBaseUnits(0.350341, Meters) + Inches.of(13.75)); + } } - @Override + + @Override public void simulationPeriodic() { if (Superstructure.getState().isAScoreState()) fuelSim.launchFuel( LinearVelocity.ofBaseUnits( flywheelIO.getSetpointRotPerSec() * Math.PI * FLYWHEEL_DIAMETER_INCHES, MetersPerSecond), - Rotation2d.kCW_90deg.minus(hoodIO.getHoodSetpoint()).getMeasure(), + // Rotation2d.kCW_90deg.minus(hoodIO.getHoodSetpoint()).getMeasure(), + Rotation2d.fromDegrees(90).getMeasure(), turretIO.getTurretSetpoint().getMeasure(), Distance.ofBaseUnits(0.350341, Meters)); } From e246a59325a44ada24dc1791a63af2b5c18dd8ab Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 19 Mar 2026 12:59:49 -0700 Subject: [PATCH 16/21] i suspect there's some goofy fudge factor --- .../subsystems/shooter/TurretSubsystem.java | 45 +++++++------------ 1 file changed, 16 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index da5e7b63..683c1f5a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -5,9 +5,10 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -22,6 +23,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; @@ -55,7 +57,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0; public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find - public static final double FLYWHEEL_DIAMETER_INCHES = 4; + public static final double FLYWHEEL_DIAMETER_INCHES = 4.0; public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero; public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; @@ -192,41 +194,26 @@ public void periodic() { if (Superstructure.getState().isAScoreState()) { System.out.println("launching fuel"); fuelSim.launchFuel( - // LinearVelocity.ofBaseUnits( - // flywheelIO.getSetpointRotPerSec() - // * Math.PI - // * Units.inchesToMeters(FLYWHEEL_DIAMETER_INCHES), - // // 2, - // MetersPerSecond), - // there are few things i despise more than the units library + // there are few things i despise more than the units library\ // InchesPerSecond.of( - // flywheelIO.getSetpointRotPerSec() * Math.PI * FLYWHEEL_DIAMETER_INCHES), - InchesPerSecond.of(flywheelIO.getSetpointRotPerSec() * 2 * Math.PI), - // Rotation2d.kCW_90deg.minus( - Rotation2d.fromDegrees(90) - .minus(hoodIO.getHoodSetpoint()) - // ) - .getMeasure(), - // Rotation2d.fromDegrees(90).getMeasure(), + // flywheelIO.getSetpointRotPerSec() * FLYWHEEL_DIAMETER_INCHES * Math.PI), + // InchesPerSecond.of(200), + angularToLinearVelocity( + RotationsPerSecond.of(flywheelIO.getSetpointRotPerSec()), + Inches.of(FLYWHEEL_DIAMETER_INCHES / 2)), + Rotation2d.fromDegrees(90).minus(hoodIO.getHoodSetpoint()).getMeasure(), turretIO.getTurretSetpoint().getMeasure(), - // Distance.ofBaseUnits(0.350341, Meters) Inches.of(13.75)); } } - @Override - public void simulationPeriodic() { - if (Superstructure.getState().isAScoreState()) - fuelSim.launchFuel( - LinearVelocity.ofBaseUnits( - flywheelIO.getSetpointRotPerSec() * Math.PI * FLYWHEEL_DIAMETER_INCHES, - MetersPerSecond), - // Rotation2d.kCW_90deg.minus(hoodIO.getHoodSetpoint()).getMeasure(), - Rotation2d.fromDegrees(90).getMeasure(), - turretIO.getTurretSetpoint().getMeasure(), - Distance.ofBaseUnits(0.350341, Meters)); + public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { + return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters) * 0.54); } + @Override + public void simulationPeriodic() {} + @Override public Command feed( Supplier robotPoseSupplier, From 1ea19955a3cd9bbcc9b195882f357e8e0b3d692a Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 19 Mar 2026 13:15:39 -0700 Subject: [PATCH 17/21] oh what the heck --- src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 683c1f5a..1928be54 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -208,7 +208,7 @@ public void periodic() { } public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { - return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters) * 0.54); + return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters) * 0.54 + 1); } @Override From f31db0cf0a5f1cd4834f99b96e071f96fe728d77 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 19 Mar 2026 18:32:40 -0700 Subject: [PATCH 18/21] Fix flowstate and remove latency --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/Superstructure.java | 22 +++++++++++++++++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9eb09008..22ea89d0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -697,6 +697,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // .and( new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAScoreState()) + .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) .and(() -> superstructure.inScoringArea()) .whileTrue( @@ -713,6 +714,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAFeedState()) + .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) .and(() -> !superstructure.inScoringArea()) .whileTrue( diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f9fdb6b4..ba36ed57 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -68,6 +68,13 @@ public boolean isAScoreState() { public boolean isAFeedState() { return this == FEED || this == SPIN_UP_FEED || this == SPIN_UP_FEED_FLOW || this == FEED_FLOW; } + + public boolean isAFlowState() { + return this == FEED_FLOW + || this == SCORE_FLOW + || this == SPIN_UP_FEED_FLOW + || this == SPIN_UP_SCORE_FLOW; + } } public enum ShotTarget { @@ -293,7 +300,8 @@ private void addTriggers() { new Trigger(shooter::atFlywheelVelocitySetpoint) // .debounce(0.05) .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05)) - .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)); + .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)) + .and(new Trigger(AutoAim::targetInTurretDeadzone).negate()); } private void addTransitions() { @@ -325,6 +333,11 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.SPIN_UP_SCORE_FLOW, + new Trigger(AutoAim::targetInTurretDeadzone)); + bindTransition( SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, @@ -360,6 +373,11 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger); + bindTransition( + SuperState.FEED_FLOW, + SuperState.SPIN_UP_FEED_FLOW, + new Trigger(AutoAim::targetInTurretDeadzone)); + bindTransition( SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); @@ -572,7 +590,7 @@ private void addCommands() { bindCommands( SuperState.SPIN_UP_SCORE_FLOW, - intake.restExtended(), + intake.intake(), indexer.rest(), shooter.score( swerve::getPose, diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e461d583..0b313f4c 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -20,7 +20,7 @@ public class AutoAim { public static double LATENCY_COMPENSATION_SECS = // new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency // comp - 0.3; + 0.0; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); From d827cd90dcfd30c2afe1bdb32fe644bd9a662cce Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 19 Mar 2026 20:01:29 -0700 Subject: [PATCH 19/21] Use newtons method where already implemented --- src/main/java/frc/robot/Superstructure.java | 30 ++++++++++----------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 41b8d34a..4943c10c 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -481,7 +481,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -495,7 +495,7 @@ private void addCommands() { intake.agitate(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -504,7 +504,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -520,7 +520,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -534,7 +534,7 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -543,7 +543,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -559,7 +559,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -575,7 +575,7 @@ private void addCommands() { // intake.restExtended(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -586,7 +586,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -603,7 +603,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -618,7 +618,7 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -629,7 +629,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -679,7 +679,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -694,7 +694,7 @@ private void addCommands() { intake.restRetracted(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -705,7 +705,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), From c4775ed9b566c6642e4e57b24468c4190da7932b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 19 Mar 2026 20:35:06 -0700 Subject: [PATCH 20/21] Moar iterations --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8fb8790a..683a568a 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -374,7 +374,7 @@ private static ShotData calculateShotAdjustments( double currentVelocity = currentDistance / currentTime; // iterate - for (int i = 0; i < 10 && Math.abs(currentVelocity - requiredVelocity) > 0.005; i++) { + for (int i = 0; i < 20 && Math.abs(currentVelocity - requiredVelocity) > 0.005; i++) { final double EPSILON = 0.001; // get deriv of velocity (dis/time) double lowVel = From 277fa5e1a3e1ec036e873de1fb1837ee6c888406 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 19 Mar 2026 22:38:58 -0700 Subject: [PATCH 21/21] Remove velocity threshold condition in loop --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 683a568a..9f32c7b4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -374,7 +374,7 @@ private static ShotData calculateShotAdjustments( double currentVelocity = currentDistance / currentTime; // iterate - for (int i = 0; i < 20 && Math.abs(currentVelocity - requiredVelocity) > 0.005; i++) { + for (int i = 0; i < 20; i++) { final double EPSILON = 0.001; // get deriv of velocity (dis/time) double lowVel =