From 0434c2c42f0c73b93d39af6c39f085822afb90c0 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 1 Mar 2026 13:41:25 -0800 Subject: [PATCH 01/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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 = From 3eed5e7f733f96451794d855d7c71ac71721fff4 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 20 Mar 2026 00:11:14 -0700 Subject: [PATCH 22/34] hack together mech a + newton's method i dont know please dont ask me to explain this --- .../frc/robot/utils/autoaim/NewAutoAim.java | 153 ++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 src/main/java/frc/robot/utils/autoaim/NewAutoAim.java diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java new file mode 100644 index 00000000..450ff7b7 --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -0,0 +1,153 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils.autoaim; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.subsystems.shooter.TurretSubsystem; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import org.littletonrobotics.junction.Logger; + +/** Add your docs here. */ +public class NewAutoAim { + private static boolean outOfRange = false; // TODO not sure if this should be true by default + + public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} + + public ShotParams calculate(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Translation2d target, InterpolatingShotTree tree) { + ChassisSpeeds robotRelativeSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, robotPose.getRotation()); + // this is robot relative speeds because "The twist is a change in pose in the robot's + // coordinate frame" + Pose2d compensatedPose = + robotPose.exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS), + robotRelativeSpeeds.vyMetersPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS), + robotRelativeSpeeds.omegaRadiansPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS))); + Pose2d turretPose = AutoAim.getTurretPose(compensatedPose); + double distanceToTarget = robotPose.getTranslation().getDistance(target); + + // the turret isn't in the center of the robot, so if the robot is rotating the turret's x and y + // are changing slightly + double turretFieldRelativeVelocityX = + fieldRelativeSpeeds.vxMetersPerSecond + + fieldRelativeSpeeds.omegaRadiansPerSecond + * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() + * Math.cos(compensatedPose.getRotation().getRadians()) + - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() + * Math.sin(compensatedPose.getRotation().getRadians())); + double turretFieldRelativeVelocityY = + fieldRelativeSpeeds.vyMetersPerSecond + + fieldRelativeSpeeds.omegaRadiansPerSecond + * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() + * Math.cos(compensatedPose.getRotation().getRadians()) + - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() + * Math.sin(compensatedPose.getRotation().getRadians())); + + Pose2d lookaheadPose = turretPose; + + // for (int i = 0; i < 20; i++) { + // double tof = tree.get(distanceToTarget).timeOfFlightSecs(); + // lookaheadPose = + // turretPose.transformBy( + // new Transform2d( + // new Translation2d(turretFieldRelativeVelocityX * tof, turretFieldRelativeVelocityY * tof), + // Rotation2d.kZero)); + // distanceToTarget = lookaheadPose.getTranslation().getDistance(target); + // } + double tof = tree.get(distanceToTarget).timeOfFlightSecs(); + + ShotData baseline = tree.calculateShot(robotPose, target); + + Translation2d turretToTarget = target.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 turretVelocityAlongShot = + turretFieldRelativeVelocityX * shotDirection.getX() + + turretFieldRelativeVelocityY * 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()) - turretVelocityAlongShot; + + //we be newtoning + for (int i = 0; i < 20; i++) { + final double EPSILON = 0.001; + // get deriv of velocity (dis/time) + double lowVel = + (distanceToTarget - EPSILON) / tree.get(distanceToTarget - EPSILON).timeOfFlightSecs(); + double highVel = + (distanceToTarget + EPSILON) / tree.get(distanceToTarget + 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 + distanceToTarget -= (distanceToTarget / tof - requiredVelocity) / velDeriv; + // update + tof = tree.get(distanceToTarget).timeOfFlightSecs(); + } + + //TODO if we're behind the hub just don't shoot + + Rotation2d turretTarget = + target.minus(lookaheadPose.getTranslation()).getAngle(); + return new ShotParams(tree.get(distanceToTarget), getTurretTargetRotation(turretTarget, robotPose)); + } + + public static Rotation2d getTurretTargetRotation( + Rotation2d turretTargetRotation, Pose2d robotPose) { + + // subtract that from rotation to point at target + turretTargetRotation = turretTargetRotation.minus(robotPose.getRotation()); + Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); + // -5 is some insane fudge factor i forgot where it's from + double turretTargetDegrees = turretTargetRotation.getDegrees() - 5; + // If its in the deadzone, clamp to nearest hardstop + outOfRange = + turretTargetDegrees > TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() + && (turretTargetDegrees < TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()); + if (outOfRange) { + turretTargetDegrees = + // If the requested angle is greater than the halfway point in the deadzone, go to the + // left hardstop, otherwise go to forward hardstop + turretTargetDegrees + > (TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() + + TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()) + / 2 + ? TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees() + 2 + : TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 2; + } + + Logger.recordOutput("Turret/Clamped target", Rotation2d.fromDegrees(turretTargetDegrees)); + // Now we need to rewrap this angle to always be negative, with 0 as the forward hardstop + turretTargetDegrees = MathUtil.inputModulus(turretTargetDegrees, -360, 0); + Logger.recordOutput("Turret/Wrapped target", Rotation2d.fromDegrees(turretTargetDegrees)); + // ship it + return Rotation2d.fromDegrees(turretTargetDegrees); + } +} From ba7e1c85ee2b985207bcd6bcc9adecdb1debc687 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Fri, 20 Mar 2026 07:36:53 -0700 Subject: [PATCH 23/34] try simming it but my sim sucks too so --- src/main/java/frc/robot/Superstructure.java | 105 ++++++++---------- .../frc/robot/subsystems/shooter/Shooter.java | 14 +-- .../subsystems/shooter/ShooterSubsystem.java | 19 ++-- .../subsystems/shooter/TurretSubsystem.java | 37 +++--- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 +- .../frc/robot/utils/autoaim/NewAutoAim.java | 82 ++++++++++++-- 6 files changed, 146 insertions(+), 113 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 4943c10c..274333f5 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -24,6 +24,7 @@ import frc.robot.utils.FieldUtils; import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.NewAutoAim; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -479,15 +480,14 @@ private void addCommands() { intake.restExtended(), indexer.rest(), shooter.feed( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), - swerve::getVelocityFieldRelative, - () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), + swerve::getPose), climber.retract()); bindCommands( @@ -502,15 +502,14 @@ private void addCommands() { AutoAim.FEED_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.feed( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), - swerve::getVelocityFieldRelative, - () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), + swerve::getPose), climber.retract()); bindCommands( @@ -518,15 +517,14 @@ private void addCommands() { intake.intake(), indexer.index(), shooter.feed( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), - swerve::getVelocityFieldRelative, - () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), + swerve::getPose), climber.retract()); bindCommands( @@ -541,15 +539,14 @@ private void addCommands() { AutoAim.FEED_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.feed( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), - swerve::getVelocityFieldRelative, - () -> FeedTargets.getFeedTarget(feedTarget).getPose()), + () -> FeedTargets.getFeedTarget(feedTarget).getPose(), + swerve::getPose), climber.retract()); bindCommands( @@ -557,16 +554,14 @@ private void addCommands() { intake.restExtended(), indexer.rest(), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -584,16 +579,14 @@ private void addCommands() { : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -601,16 +594,14 @@ private void addCommands() { intake.intake(), indexer.rest(), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -627,16 +618,14 @@ private void addCommands() { : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -677,16 +666,14 @@ private void addCommands() { intake.restRetracted(), indexer.rest(), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( @@ -703,16 +690,14 @@ private void addCommands() { : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.score( - swerve::getPose, () -> - AutoAim.getSOTMShotDataNewtonsMethod( - shooter.getTurretPose(swerve.getPose()), - FieldUtils.getCurrentHubTranslation(), + NewAutoAim.getParameters( + swerve.getPose(), swerve.getVelocityFieldRelative(), + FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE), - swerve::getVelocityFieldRelative), + : AutoAim.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6b9f01a4..3bd80b5f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import frc.robot.utils.autoaim.NewAutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -21,20 +21,16 @@ public interface Shooter extends Subsystem { * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current * pose */ - public Command score( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier); + public Command score(Supplier shotParamsSupplier); /** * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current * pose + feed target */ public Command feed( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier, - Supplier feedTarget); + Supplier shotParamsSupplier, + Supplier feedTarget, + Supplier robotPoseSupplier); /** Not running (set to 0) */ public Command rest( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 054753b7..6eb40a64 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -25,6 +25,7 @@ import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import frc.robot.utils.autoaim.NewAutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -83,25 +84,21 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; } - public Command score( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier) { + public Command score(Supplier shotParamsSupplier) { return this.run( () -> { - hoodSetpoint = shotDataSupplier.get().hoodAngle(); - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + hoodSetpoint = shotParamsSupplier.get().shotData().hoodAngle(); + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); }); } @Override public Command feed( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier, - Supplier feedTarget) { + Supplier shotParamsSupplier, + Supplier feedTarget, + Supplier robotPoseSupplier) { return this.run( () -> { ShotData shotData = diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 1928be54..e6be500d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -32,13 +32,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; 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.autoaim.AutoAim; -import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import frc.robot.utils.autoaim.NewAutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -191,7 +192,7 @@ public void periodic() { // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert - if (Superstructure.getState().isAScoreState()) { + if (Superstructure.getState().isAScoreState() && Robot.isSimulation()) { System.out.println("launching fuel"); fuelSim.launchFuel( // there are few things i despise more than the units library\ @@ -216,22 +217,17 @@ public void simulationPeriodic() {} @Override public Command feed( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier, - Supplier feedTarget) { + Supplier shotParamsSupplier, + Supplier feedTarget, + Supplier robotPoseSupplier) { return this.run( () -> { Logger.recordOutput("Robot/Feed Target", feedTarget.get()); - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretFeedTargetRotation( - feedTarget.get().getTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); }); } @@ -293,10 +289,7 @@ public Command runHoodCurrentZeroing() { .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); } - public Command score( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier) { + public Command score(Supplier shotParamsSupplier) { return resetTurretToCalculatedPosition() .andThen( this.run( @@ -321,14 +314,10 @@ public Command score( // AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); // turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); // case NONE: - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); // } })); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 9f32c7b4..d9a9de7f 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.0; + 0.3; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 450ff7b7..f0567211 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -21,7 +21,11 @@ public class NewAutoAim { public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} - public ShotParams calculate(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, Translation2d target, InterpolatingShotTree tree) { + public static ShotParams calculate( + Pose2d robotPose, + ChassisSpeeds fieldRelativeSpeeds, + Translation2d target, + InterpolatingShotTree tree) { ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, robotPose.getRotation()); // this is robot relative speeds because "The twist is a change in pose in the robot's @@ -59,13 +63,14 @@ public ShotParams calculate(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, // lookaheadPose = // turretPose.transformBy( // new Transform2d( - // new Translation2d(turretFieldRelativeVelocityX * tof, turretFieldRelativeVelocityY * tof), + // new Translation2d(turretFieldRelativeVelocityX * tof, + // turretFieldRelativeVelocityY * tof), // Rotation2d.kZero)); // distanceToTarget = lookaheadPose.getTranslation().getDistance(target); // } double tof = tree.get(distanceToTarget).timeOfFlightSecs(); - ShotData baseline = tree.calculateShot(robotPose, target); + ShotData baseline = tree.calculateShot(robotPose, target); Translation2d turretToTarget = target.minus(turretPose.getTranslation()); @@ -93,7 +98,7 @@ public ShotParams calculate(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, // shot to account for the robots velocity along the shot double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - turretVelocityAlongShot; - //we be newtoning + // we be newtoning for (int i = 0; i < 20; i++) { final double EPSILON = 0.001; // get deriv of velocity (dis/time) @@ -112,11 +117,11 @@ public ShotParams calculate(Pose2d robotPose, ChassisSpeeds fieldRelativeSpeeds, tof = tree.get(distanceToTarget).timeOfFlightSecs(); } - //TODO if we're behind the hub just don't shoot + // TODO if we're behind the hub just don't shoot - Rotation2d turretTarget = - target.minus(lookaheadPose.getTranslation()).getAngle(); - return new ShotParams(tree.get(distanceToTarget), getTurretTargetRotation(turretTarget, robotPose)); + Rotation2d turretTarget = target.minus(lookaheadPose.getTranslation()).getAngle(); + return new ShotParams( + tree.get(distanceToTarget), getTurretTargetRotation(turretTarget, robotPose)); } public static Rotation2d getTurretTargetRotation( @@ -150,4 +155,65 @@ public static Rotation2d getTurretTargetRotation( // ship it return Rotation2d.fromDegrees(turretTargetDegrees); } + + public static ShotParams getParameters( + Pose2d estimatedPose, + ChassisSpeeds robotRelativeVelocity, + Translation2d target, + InterpolatingShotTree tree) { + + // Calculate estimated pose while accounting for phase delay + estimatedPose = + estimatedPose.exp( + new Twist2d( + robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS, + robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS, + robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS)); + + // Calculate distance from turret to target + Pose2d turretPosition = + estimatedPose.transformBy( + new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); + + // Calculate field relative turret velocity + ChassisSpeeds robotVelocity = + ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity, estimatedPose.getRotation()); + double robotAngle = estimatedPose.getRotation().getRadians(); + double turretVelocityX = + robotVelocity.vxMetersPerSecond + + robotVelocity.omegaRadiansPerSecond + * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.cos(robotAngle) + - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.sin(robotAngle)); + double turretVelocityY = + robotVelocity.vyMetersPerSecond + + robotVelocity.omegaRadiansPerSecond + * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.cos(robotAngle) + - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.sin(robotAngle)); + + // Account for imparted velocity by robot (turret) to offset + double timeOfFlight; + Pose2d lookaheadPose = turretPosition; + double lookaheadTurretToTargetDistance = turretToTargetDistance; + for (int i = 0; i < 20; i++) { + timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + lookaheadPose = + new Pose2d( + turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), + turretPosition.getRotation()); + lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); + } + + // Calculate parameters accounted for imparted velocity + Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + turretAngle = getTurretTargetRotation(turretAngle, estimatedPose); + + // Log calculated values + Logger.recordOutput("LaunchCalculator/LookaheadPose", lookaheadPose); + Logger.recordOutput("LaunchCalculator/TurretToTargetDistance", lookaheadTurretToTargetDistance); + + return new ShotParams(tree.get(lookaheadTurretToTargetDistance), turretAngle); + } } From 64a0b2150ae6aadad483bf3b5ab92cb8f8236c98 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 16:47:06 -0700 Subject: [PATCH 24/34] assorted tuning of the mecha solution but it's kind of bad --- src/main/java/frc/robot/Superstructure.java | 20 ++--- .../subsystems/shooter/TurretSubsystem.java | 25 ++++-- .../java/frc/robot/utils/autoaim/AutoAim.java | 77 +------------------ .../frc/robot/utils/autoaim/NewAutoAim.java | 26 ++++--- 4 files changed, 47 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 274333f5..8bd4e7a9 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -481,7 +481,7 @@ private void addCommands() { indexer.rest(), shooter.feed( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), @@ -503,7 +503,7 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter.feed( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), @@ -518,7 +518,7 @@ private void addCommands() { indexer.index(), shooter.feed( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), @@ -540,7 +540,7 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter.feed( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), @@ -555,7 +555,7 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), @@ -580,7 +580,7 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), @@ -595,7 +595,7 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), @@ -619,7 +619,7 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), @@ -667,7 +667,7 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), @@ -691,7 +691,7 @@ private void addCommands() { .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParameters( + NewAutoAim.getParametersMechA( swerve.getPose(), swerve.getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 621602a0..0afb9e12 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -33,13 +33,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; +import frc.robot.Robot.RobotEdition; 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.NewAutoAim; import frc.robot.utils.autoaim.NewAutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -250,11 +251,21 @@ public Command rest( // chassisSpeedsSupplier.get())); // } else { if (inScoringArea.getAsBoolean()) { + turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + NewAutoAim.getParametersMechA( + robotPoseSupplier.get(), + chassisSpeedsSupplier.get(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE) + .turretAngle()); + // turretIO.setTurretPosition( + // AutoAim.getTurretHubTargetRotation( + // FieldUtils.getCurrentHubTranslation(), + // robotPoseSupplier.get(), + // chassisSpeedsSupplier.get())); } else { turretIO.setTurretPosition( AutoAim.getTurretFeedTargetRotation( @@ -534,8 +545,8 @@ public static TalonFXConfiguration getTurretConfig() { config.Slot0.kV = 5.7; config.Slot0.kP = 240.0; - config.MotionMagic.MotionMagicAcceleration = 20; // 2.064; - config.MotionMagic.MotionMagicCruiseVelocity = 50; // 8.0; + config.MotionMagic.MotionMagicAcceleration = 10; // 2.064; + config.MotionMagic.MotionMagicCruiseVelocity = 20; // 8.0; return config; } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index ed9c9687..e3581db1 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; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); @@ -73,7 +73,7 @@ public class AutoAim { new ShotData(Rotation2d.fromDegrees(31), 40, 1.3)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 5 * 24), - new ShotData(Rotation2d.fromDegrees(33), 42, 1.02)); + new ShotData(Rotation2d.fromDegrees(33), 41, 1.02)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 * 24), new ShotData(Rotation2d.fromDegrees(35), 43, 1.3)); @@ -82,79 +82,6 @@ public class AutoAim { new ShotData(Rotation2d.fromDegrees(37), 47, 1.46)); } - // TODO update tof - static { // For hub shot tree - // TODO min shot - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), - new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 0.9)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData( - Rotation2d.fromDegrees(26), - 37 - 6 + 3 - // - 6 - , - 1.02)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData( - Rotation2d.fromDegrees(30), - 37 - 6 + 3 - // - 6 - , - 1.09)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData( - Rotation2d.fromDegrees(33), - 37 - 6 + 3 - // - 6 - , - 1.15)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData( - Rotation2d.fromDegrees(36), - 38 - 6 + 3 - // - 6 - , - 1.23)); - - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData( - Rotation2d.fromDegrees(38), - 39 - 6 + 3 - // - 6 - , - 1.33)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData( - Rotation2d.fromDegrees(39), - 40.5 - 6 + 3 - // - 6 - , - 1.35)); - COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12 + 6), - new ShotData( - Rotation2d.fromDegrees(39), - 40.5 - 6 + 3 + 2 - // - 6 - , - 1.35)); - } - // Ig we'll see if we need more than 1 feed shot tree public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index f0567211..77a463c5 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -120,6 +120,8 @@ public static ShotParams calculate( // TODO if we're behind the hub just don't shoot Rotation2d turretTarget = target.minus(lookaheadPose.getTranslation()).getAngle(); + turretTarget = getTurretTargetRotation(turretTarget, robotPose); + return new ShotParams( tree.get(distanceToTarget), getTurretTargetRotation(turretTarget, robotPose)); } @@ -156,7 +158,7 @@ public static Rotation2d getTurretTargetRotation( return Rotation2d.fromDegrees(turretTargetDegrees); } - public static ShotParams getParameters( + public static ShotParams getParametersMechA( Pose2d estimatedPose, ChassisSpeeds robotRelativeVelocity, Translation2d target, @@ -177,17 +179,17 @@ public static ShotParams getParameters( double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); // Calculate field relative turret velocity - ChassisSpeeds robotVelocity = + ChassisSpeeds robotFieldRelVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity, estimatedPose.getRotation()); double robotAngle = estimatedPose.getRotation().getRadians(); double turretVelocityX = - robotVelocity.vxMetersPerSecond - + robotVelocity.omegaRadiansPerSecond + robotFieldRelVelocity.vxMetersPerSecond + + robotFieldRelVelocity.omegaRadiansPerSecond * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.cos(robotAngle) - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.sin(robotAngle)); double turretVelocityY = - robotVelocity.vyMetersPerSecond - + robotVelocity.omegaRadiansPerSecond + robotFieldRelVelocity.vyMetersPerSecond + + robotFieldRelVelocity.omegaRadiansPerSecond * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.cos(robotAngle) - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.sin(robotAngle)); @@ -200,9 +202,15 @@ public static ShotParams getParameters( double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; lookaheadPose = - new Pose2d( - turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), - turretPosition.getRotation()); + turretPosition.transformBy(new Transform2d(offsetX, offsetY, Rotation2d.kZero)); + // lookaheadPose = + // lookaheadPose.plus( + // new Transform2d(new Translation2d(offsetX, offsetY), Rotation2d.kZero)); + // lookaheadPose = + // new Pose2d( + // new Translation2d(turretPosition.getX() + offsetX, turretPosition.getY() + + // offsetY), + // turretPosition.getRotation()); lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } From 596056b052aeef8c33ef5b96315ac8ab2ad40860 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sat, 21 Mar 2026 17:07:15 -0700 Subject: [PATCH 25/34] stop being stupid about where the target actually is --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- src/main/java/frc/robot/utils/autoaim/NewAutoAim.java | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f4bd3f6f..44dc3917 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1005,7 +1005,7 @@ public void teleopPeriodic() {} @Override public void teleopExit() { System.out.println("Saving BFG Log"); - bfg.saveLog(""); + if (bfg.isConnected()) bfg.saveLog(""); } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e3581db1..2ff473b3 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -73,7 +73,7 @@ public class AutoAim { new ShotData(Rotation2d.fromDegrees(31), 40, 1.3)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 5 * 24), - new ShotData(Rotation2d.fromDegrees(33), 41, 1.02)); + new ShotData(Rotation2d.fromDegrees(33), 41, 1.3)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 * 24), new ShotData(Rotation2d.fromDegrees(35), 43, 1.3)); diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 77a463c5..b217f8dd 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -215,7 +215,8 @@ public static ShotParams getParametersMechA( } // Calculate parameters accounted for imparted velocity - Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + // Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();\ + Rotation2d turretAngle = AutoAim.getTargetRotation(target, lookaheadPose); turretAngle = getTurretTargetRotation(turretAngle, estimatedPose); // Log calculated values From 7ecb0d622608db9d26f12d3bb026171e04e5714d Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 21 Mar 2026 18:20:31 -0700 Subject: [PATCH 26/34] Whole rewrite lol (works in sim) --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Superstructure.java | 40 ++++++++-------- .../subsystems/shooter/TurretSubsystem.java | 6 +-- .../frc/robot/utils/autoaim/NewAutoAim.java | 46 ++++++++++--------- 4 files changed, 49 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f4bd3f6f..71508203 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -519,7 +519,7 @@ public Robot() { shooter.setDefaultCommand( shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, superstructure::inScoringArea, () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose())); swerve.setDefaultCommand( diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 8bd4e7a9..ceb06e93 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -459,7 +459,7 @@ private void addCommands() { indexer.rest(), shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); @@ -470,7 +470,7 @@ private void addCommands() { indexer.rest(), shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); @@ -483,7 +483,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), () -> FeedTargets.getFeedTarget(feedTarget).getPose(), @@ -498,14 +498,14 @@ private void addCommands() { AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), AutoAim.FEED_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.feed( () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), () -> FeedTargets.getFeedTarget(feedTarget).getPose(), @@ -520,7 +520,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), () -> FeedTargets.getFeedTarget(feedTarget).getPose(), @@ -535,14 +535,14 @@ private void addCommands() { AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), AutoAim.FEED_SHOT_TREE) .flywheelVelocityRotPerSec()), shooter.feed( () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), AutoAim.FEED_SHOT_TREE), () -> FeedTargets.getFeedTarget(feedTarget).getPose(), @@ -557,7 +557,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -573,7 +573,7 @@ private void addCommands() { AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) @@ -582,7 +582,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -597,7 +597,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -612,7 +612,7 @@ private void addCommands() { AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) @@ -621,7 +621,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -636,7 +636,7 @@ private void addCommands() { indexer.rest(), shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); @@ -646,7 +646,7 @@ private void addCommands() { indexer.rest(), shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); @@ -656,7 +656,7 @@ private void addCommands() { indexer.rest(), shooter.rest( swerve::getPose, - swerve::getVelocityFieldRelative, + swerve::getVelocityRobotRelative, this::inScoringArea, () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); @@ -669,7 +669,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -684,7 +684,7 @@ private void addCommands() { AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) @@ -693,7 +693,7 @@ private void addCommands() { () -> NewAutoAim.getParametersMechA( swerve.getPose(), - swerve.getVelocityFieldRelative(), + swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 0afb9e12..25e15e0c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -236,7 +236,7 @@ public Command feed( @Override public Command rest( Supplier robotPoseSupplier, - Supplier chassisSpeedsSupplier, + Supplier chassisSpeedsSupplierRobotRel, BooleanSupplier inScoringArea, Supplier feedTarget) { return this.run( @@ -255,7 +255,7 @@ public Command rest( turretIO.setTurretPosition( NewAutoAim.getParametersMechA( robotPoseSupplier.get(), - chassisSpeedsSupplier.get(), + chassisSpeedsSupplierRobotRel.get(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA ? AutoAim.ALPHA_HUB_SHOT_TREE @@ -271,7 +271,7 @@ public Command rest( AutoAim.getTurretFeedTargetRotation( feedTarget.get().getTranslation(), robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + chassisSpeedsSupplierRobotRel.get())); } // } }); diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 77a463c5..2ffbff54 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -178,21 +178,29 @@ public static ShotParams getParametersMechA( new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); - // Calculate field relative turret velocity - ChassisSpeeds robotFieldRelVelocity = - ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity, estimatedPose.getRotation()); - double robotAngle = estimatedPose.getRotation().getRadians(); + double turretRadiusMeters = + Math.hypot( + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX(), + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY()); + Rotation2d turretToRobotAngleRads = + Rotation2d.fromRadians( + Math.atan2( + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY(), + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX())); + Rotation2d turretLinearVelAngle = turretToRobotAngleRads.minus(Rotation2d.kCCW_90deg); + double turretVelocityX = - robotFieldRelVelocity.vxMetersPerSecond - + robotFieldRelVelocity.omegaRadiansPerSecond - * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.cos(robotAngle) - - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.sin(robotAngle)); + robotRelativeVelocity.vxMetersPerSecond + + (robotRelativeVelocity.omegaRadiansPerSecond + * turretRadiusMeters + * turretLinearVelAngle.getSin()); double turretVelocityY = - robotFieldRelVelocity.vyMetersPerSecond - + robotFieldRelVelocity.omegaRadiansPerSecond - * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() * Math.cos(robotAngle) - - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() * Math.sin(robotAngle)); - + robotRelativeVelocity.vyMetersPerSecond + + (robotRelativeVelocity.omegaRadiansPerSecond + * turretRadiusMeters + * turretLinearVelAngle.getCos()); + Logger.recordOutput( + "LaunchCalculator/Turret Velocity", new Translation2d(turretVelocityX, turretVelocityY)); // Account for imparted velocity by robot (turret) to offset double timeOfFlight; Pose2d lookaheadPose = turretPosition; @@ -201,16 +209,12 @@ public static ShotParams getParametersMechA( timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; + + Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); + lookaheadPose = turretPosition.transformBy(new Transform2d(offsetX, offsetY, Rotation2d.kZero)); - // lookaheadPose = - // lookaheadPose.plus( - // new Transform2d(new Translation2d(offsetX, offsetY), Rotation2d.kZero)); - // lookaheadPose = - // new Pose2d( - // new Translation2d(turretPosition.getX() + offsetX, turretPosition.getY() + - // offsetY), - // turretPosition.getRotation()); + lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } From 12287a40b2ff989e2202cd4d78eb82c545dec6a6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 22 Mar 2026 12:32:03 -0700 Subject: [PATCH 27/34] fmt --- .../java/frc/robot/subsystems/shooter/TurretSubsystem.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 9968ac70..85eef58f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -9,7 +9,6 @@ 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 static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -34,13 +33,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot; -import frc.robot.Robot.RobotEdition; -import frc.robot.Superstructure; 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.Robot; +import frc.robot.Robot.RobotEdition; +import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; From 266ca3490df82b446e8bc8080ccc3ee151ff4482 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 22 Mar 2026 12:36:59 -0700 Subject: [PATCH 28/34] fmt --- .../java/frc/robot/utils/autoaim/AutoAim.java | 65 +++++++++++++++---- 1 file changed, 52 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 67f90693..006b1218 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -181,44 +181,83 @@ public class AutoAim { FEED_SHOT_TREE.put( Units.feetToMeters(18), - new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); // - 2, 1.42)); + new ShotData( + Rotation2d.fromDegrees(40 - 13.16), + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.42)); // - 2, 1.42)); FEED_SHOT_TREE.put( Units.feetToMeters(20), - new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); // - 2, 1.36)); + new ShotData( + Rotation2d.fromDegrees(43 - 13.16), + 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.36)); // - 2, 1.36)); FEED_SHOT_TREE.put( Units.feetToMeters(22), - new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); // - 2, 1.34)); + new ShotData( + Rotation2d.fromDegrees(45 - 13.16), + 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.34)); // - 2, 1.34)); FEED_SHOT_TREE.put( Units.feetToMeters(24), - new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); // - 2, 1.25)); + new ShotData( + Rotation2d.fromDegrees(47 - 13.16), + 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.25)); // - 2, 1.25)); FEED_SHOT_TREE.put( Units.feetToMeters(26), - new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); // - 2, 1.28)); + new ShotData( + Rotation2d.fromDegrees(48 - 13.16), + 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.28)); // - 2, 1.28)); FEED_SHOT_TREE.put( Units.feetToMeters(28), - new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); // - 2, 1.27)); + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.27)); // - 2, 1.27)); FEED_SHOT_TREE.put( Units.feetToMeters(30), - new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); // - 2, 1.32)); + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.32)); // - 2, 1.32)); FEED_SHOT_TREE.put( Units.feetToMeters(32), - new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); // - 2, 1.4)); + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.4)); // - 2, 1.4)); FEED_SHOT_TREE.put( Units.feetToMeters(34), - new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); // - 2, 1.3)); + new ShotData( + Rotation2d.fromDegrees(52 - 13.16), + 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(36), - new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); // - 2, 1.33)); + new ShotData( + Rotation2d.fromDegrees(53 - 13.16), + 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.33)); // - 2, 1.33)); FEED_SHOT_TREE.put( Units.feetToMeters(38), - new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); // - 2, 1.3)); + new ShotData( + Rotation2d.fromDegrees(53 - 13.16), + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.3)); // - 2, 1.3)); FEED_SHOT_TREE.put( Units.feetToMeters(40), - new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); // - 2, 1.2)); + new ShotData( + Rotation2d.fromDegrees(55 - 13.16), + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); // - 2, 1.2)); FEED_SHOT_TREE.put( Units.feetToMeters(42), - new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // - 2, 1.2)); + new ShotData( + Rotation2d.fromDegrees(56 - 13.16), + 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); // - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight } From 22fda5123ab9763897ee4de157aab46c6c87353a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 12:56:56 -0700 Subject: [PATCH 29/34] Make out of range update with new rewrite --- src/main/java/frc/robot/Robot.java | 10 +++++----- .../subsystems/shooter/TurretSubsystem.java | 20 ++++++++++--------- .../frc/robot/utils/autoaim/NewAutoAim.java | 4 ++++ 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92fe33da..8d047929 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,7 +72,7 @@ import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; import frc.robot.utils.FuelSim; -import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.NewAutoAim; import java.io.File; import java.util.Arrays; import java.util.Optional; @@ -680,7 +680,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta Commands.parallel( shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing()))); - new Trigger(() -> AutoAim.targetInTurretDeadzone()) + new Trigger(() -> NewAutoAim.targetInTurretDeadzone()) .onTrue( driver .rumbleCmd(1, 1) @@ -725,7 +725,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // driver // .leftBumper() // .and( - new Trigger(AutoAim::targetInTurretDeadzone) + new Trigger(NewAutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAScoreState()) .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) @@ -742,7 +742,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), shooter::getTurretPosition)); - new Trigger(AutoAim::targetInTurretDeadzone) + new Trigger(NewAutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAFeedState()) .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) @@ -881,7 +881,7 @@ public void robotPeriodic() { "trench poses", Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new)); - Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone()); + Logger.recordOutput("Turret/out of range", NewAutoAim.targetInTurretDeadzone()); noLogStickAlert.set(!directory.exists()); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 85eef58f..77632be9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -245,15 +245,17 @@ public Command feed( Supplier shotParamsSupplier, Supplier feedTarget, Supplier robotPoseSupplier) { - return this.run( - () -> { - Logger.recordOutput("Robot/Feed Target", feedTarget.get()); - hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); - // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); - }); + return resetTurretToCalculatedPosition() + .andThen( + this.run( + () -> { + Logger.recordOutput("Robot/Feed Target", feedTarget.get()); + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); + // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); + })); } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index f8abac74..7365ec61 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -229,4 +229,8 @@ public static ShotParams getParametersMechA( return new ShotParams(tree.get(lookaheadTurretToTargetDistance), turretAngle); } + + public static boolean targetInTurretDeadzone() { + return outOfRange; + } } From d43bff8450f0ae1ef80fc5ef3f832da498c914f9 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 14:08:17 -0700 Subject: [PATCH 30/34] Add acceleration (untested) --- .../frc/robot/utils/autoaim/NewAutoAim.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 7365ec61..4eabb7fc 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -11,6 +11,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Robot; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import org.littletonrobotics.junction.Logger; @@ -18,6 +20,10 @@ /** Add your docs here. */ public class NewAutoAim { private static boolean outOfRange = false; // TODO not sure if this should be true by default + + private static double prevTurretVx = 0.0; + private static double prevTurretVy = 0.0; + private static double lastRunTimeSecs = 0.0; public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} @@ -201,14 +207,25 @@ public static ShotParams getParametersMechA( * turretLinearVelAngle.getCos()); Logger.recordOutput( "LaunchCalculator/Turret Velocity", new Translation2d(turretVelocityX, turretVelocityY)); + + double currentTimeSecs = Timer.getFPGATimestamp(); + double dtSecs = currentTimeSecs - lastRunTimeSecs; + lastRunTimeSecs = currentTimeSecs; + + double turretAccelerationX = (turretVelocityX - prevTurretVx) / dtSecs; + double turretAccelerationY = (turretVelocityY - prevTurretVy) / dtSecs; + + prevTurretVx = turretVelocityX; + prevTurretVy = turretVelocityY; + // Account for imparted velocity by robot (turret) to offset double timeOfFlight; Pose2d lookaheadPose = turretPosition; double lookaheadTurretToTargetDistance = turretToTargetDistance; for (int i = 0; i < 20; i++) { timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; + double offsetX = (turretVelocityX * timeOfFlight) + (0.5 * turretAccelerationX * timeOfFlight * timeOfFlight); + double offsetY = (turretVelocityY * timeOfFlight) + (0.5 * turretAccelerationY * timeOfFlight * timeOfFlight); Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); From bcc58a639dccb0e784fc602dae203852fa2c1d7c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 14:37:20 -0700 Subject: [PATCH 31/34] Revert "Add acceleration (untested)" This reverts commit d43bff8450f0ae1ef80fc5ef3f832da498c914f9. --- .../frc/robot/utils/autoaim/NewAutoAim.java | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 4eabb7fc..7365ec61 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -11,8 +11,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Robot; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import org.littletonrobotics.junction.Logger; @@ -20,10 +18,6 @@ /** Add your docs here. */ public class NewAutoAim { private static boolean outOfRange = false; // TODO not sure if this should be true by default - - private static double prevTurretVx = 0.0; - private static double prevTurretVy = 0.0; - private static double lastRunTimeSecs = 0.0; public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} @@ -207,25 +201,14 @@ public static ShotParams getParametersMechA( * turretLinearVelAngle.getCos()); Logger.recordOutput( "LaunchCalculator/Turret Velocity", new Translation2d(turretVelocityX, turretVelocityY)); - - double currentTimeSecs = Timer.getFPGATimestamp(); - double dtSecs = currentTimeSecs - lastRunTimeSecs; - lastRunTimeSecs = currentTimeSecs; - - double turretAccelerationX = (turretVelocityX - prevTurretVx) / dtSecs; - double turretAccelerationY = (turretVelocityY - prevTurretVy) / dtSecs; - - prevTurretVx = turretVelocityX; - prevTurretVy = turretVelocityY; - // Account for imparted velocity by robot (turret) to offset double timeOfFlight; Pose2d lookaheadPose = turretPosition; double lookaheadTurretToTargetDistance = turretToTargetDistance; for (int i = 0; i < 20; i++) { timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); - double offsetX = (turretVelocityX * timeOfFlight) + (0.5 * turretAccelerationX * timeOfFlight * timeOfFlight); - double offsetY = (turretVelocityY * timeOfFlight) + (0.5 * turretAccelerationY * timeOfFlight * timeOfFlight); + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); From a2e609973bb860781d4d1a54047d918a5a5bea9c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 14:50:58 -0700 Subject: [PATCH 32/34] Smarter acceleration compensation --- .../frc/robot/utils/autoaim/NewAutoAim.java | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index 7365ec61..b44398df 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import org.littletonrobotics.junction.Logger; @@ -19,6 +20,11 @@ public class NewAutoAim { private static boolean outOfRange = false; // TODO not sure if this should be true by default + private static double lastVxMetersPerSec = 0.0; + private static double lastVyMetersPerSec = 0.0; + private static double lastOmegaRadPerSec = 0.0; + private static double lastRunTimeSec = 0.0; + public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} public static ShotParams calculate( @@ -164,13 +170,27 @@ public static ShotParams getParametersMechA( Translation2d target, InterpolatingShotTree tree) { + double currentTimeSec = Timer.getFPGATimestamp(); + double deltaTime = currentTimeSec - lastRunTimeSec; + + double axMetersPerSecSq = (robotRelativeVelocity.vxMetersPerSecond - lastVxMetersPerSec) / deltaTime; + double ayMetersPerSecSq = (robotRelativeVelocity.vyMetersPerSecond - lastVyMetersPerSec) / deltaTime; + double alphaRadPerSecSq = (robotRelativeVelocity.omegaRadiansPerSecond - lastOmegaRadPerSec) / deltaTime; + + lastVxMetersPerSec = robotRelativeVelocity.vxMetersPerSecond; + lastVyMetersPerSec = robotRelativeVelocity.vyMetersPerSecond; + lastOmegaRadPerSec = robotRelativeVelocity.omegaRadiansPerSecond; + + lastRunTimeSec = currentTimeSec; + // Calculate estimated pose while accounting for phase delay estimatedPose = estimatedPose.exp( new Twist2d( - robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS, - robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS, - robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS)); + (robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * axMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * ayMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * alphaRadPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)) + )); // Calculate distance from turret to target Pose2d turretPosition = From 82ddc76cac813efba5b59a9cd32ba143e127a334 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 15:39:41 -0700 Subject: [PATCH 33/34] Tuning --- src/main/java/frc/robot/Superstructure.java | 2 +- .../subsystems/intake/SlapdownSubsystem.java | 2 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 +- .../frc/robot/utils/autoaim/NewAutoAim.java | 19 ++++++++++++------- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index ceb06e93..a7b2ddeb 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -310,7 +310,7 @@ private void addTriggers() { // .debounce(0.05) .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05)) .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)) - .and(new Trigger(AutoAim::targetInTurretDeadzone).negate()); + .and(new Trigger(NewAutoAim::targetInTurretDeadzone).negate()); } private void addTransitions() { diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index fbfe3039..4832f206 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -220,7 +220,7 @@ public static TalonFXConfiguration getPivotConfig() { config.CurrentLimits.SupplyCurrentLimitEnable = true; // TODO: TUNE - config.MotionMagic.MotionMagicCruiseVelocity = 10; + config.MotionMagic.MotionMagicCruiseVelocity = 3; config.MotionMagic.MotionMagicAcceleration = 10; return config; diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 006b1218..ac3dab99 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; + 0.0; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index b44398df..a7cca5e6 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -173,9 +173,12 @@ public static ShotParams getParametersMechA( double currentTimeSec = Timer.getFPGATimestamp(); double deltaTime = currentTimeSec - lastRunTimeSec; - double axMetersPerSecSq = (robotRelativeVelocity.vxMetersPerSecond - lastVxMetersPerSec) / deltaTime; - double ayMetersPerSecSq = (robotRelativeVelocity.vyMetersPerSecond - lastVyMetersPerSec) / deltaTime; - double alphaRadPerSecSq = (robotRelativeVelocity.omegaRadiansPerSecond - lastOmegaRadPerSec) / deltaTime; + double axMetersPerSecSq = + (robotRelativeVelocity.vxMetersPerSecond - lastVxMetersPerSec) / deltaTime; + double ayMetersPerSecSq = + (robotRelativeVelocity.vyMetersPerSecond - lastVyMetersPerSec) / deltaTime; + double alphaRadPerSecSq = + (robotRelativeVelocity.omegaRadiansPerSecond - lastOmegaRadPerSec) / deltaTime; lastVxMetersPerSec = robotRelativeVelocity.vxMetersPerSecond; lastVyMetersPerSec = robotRelativeVelocity.vyMetersPerSecond; @@ -187,10 +190,12 @@ public static ShotParams getParametersMechA( estimatedPose = estimatedPose.exp( new Twist2d( - (robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * axMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), - (robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * ayMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), - (robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * alphaRadPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)) - )); + (robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + + (0.5 * axMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + + (0.5 * ayMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + + (0.5 * alphaRadPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)))); // Calculate distance from turret to target Pose2d turretPosition = From 99daa8472ed5d7170f609ab6d931e02239c1ddf1 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 22 Mar 2026 15:44:26 -0700 Subject: [PATCH 34/34] Comments --- .../java/frc/robot/utils/autoaim/NewAutoAim.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java index a7cca5e6..579f6702 100644 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java @@ -186,7 +186,7 @@ public static ShotParams getParametersMechA( lastRunTimeSec = currentTimeSec; - // Calculate estimated pose while accounting for phase delay + // Calculate estimated pose while accounting movement and acceleration during phase delay estimatedPose = estimatedPose.exp( new Twist2d( @@ -197,12 +197,14 @@ public static ShotParams getParametersMechA( (robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) + (0.5 * alphaRadPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)))); - // Calculate distance from turret to target + // Calculate turret position Pose2d turretPosition = estimatedPose.transformBy( new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + // Calculate distance from turret to target double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); + // Calculate angle of linear velocity from angular velocity double turretRadiusMeters = Math.hypot( TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX(), @@ -214,6 +216,7 @@ public static ShotParams getParametersMechA( TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX())); Rotation2d turretLinearVelAngle = turretToRobotAngleRads.minus(Rotation2d.kCCW_90deg); + // Calculate turret velocity, accounting for angular velocity double turretVelocityX = robotRelativeVelocity.vxMetersPerSecond + (robotRelativeVelocity.omegaRadiansPerSecond @@ -231,15 +234,17 @@ public static ShotParams getParametersMechA( Pose2d lookaheadPose = turretPosition; double lookaheadTurretToTargetDistance = turretToTargetDistance; for (int i = 0; i < 20; i++) { + // Find time of flight for a shot from the current lookahead pose timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); + // Extrapolate velocity over time of flight of the shot double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); - + // Update lookahead pose lookaheadPose = turretPosition.transformBy(new Transform2d(offsetX, offsetY, Rotation2d.kZero)); - + // Update distance lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); }