From 049262e8a3bf23218452fe3a4f7d75f1c843afce Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 21 Mar 2026 09:46:47 -0700 Subject: [PATCH 1/4] account for rotational vellcity i think --- .../java/frc/robot/utils/autoaim/AutoAim.java | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 9f32c7b4..f437a688 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -225,19 +225,26 @@ public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPos public static Rotation2d getVirtualTargetYaw( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { - Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); + Translation2d vtarget = getVirtualSOTMTarget(robotPose, target, fieldRelativeSpeeds, tof); return getTargetRotation(vtarget, robotPose); } // lock in public static Translation2d getVirtualSOTMTarget( - Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { - // velocity times shot time is how translated it is - Translation2d vtarget = - target.minus( - new Translation2d( - fieldRelativeSpeeds.vxMetersPerSecond * timeOfFlightSecs, - fieldRelativeSpeeds.vyMetersPerSecond * timeOfFlightSecs)); + Pose2d robotPose, + Translation2d target, + ChassisSpeeds fieldRelativeSpeeds, + double timeOfFlightSecs) { + Pose2d vrobot = + robotPose.exp( + new Twist2d( + fieldRelativeSpeeds.vxMetersPerSecond * (timeOfFlightSecs), + fieldRelativeSpeeds.vyMetersPerSecond * (timeOfFlightSecs), + fieldRelativeSpeeds.omegaRadiansPerSecond * (timeOfFlightSecs))); + + Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation()); + + Translation2d vtarget = target.minus(delta); Logger.recordOutput("Autoaim/Virtual Target", vtarget); return vtarget; } @@ -315,7 +322,7 @@ public static ShotData getSOTMShotData( ShotData unadjustedShot = tree.calculateShot(robotPose, targetTranslation); Translation2d virtualTarget = getVirtualSOTMTarget( - targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); + robotPose, targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); Pose2d turretPose = getTurretPose(robotPose); return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); From aec33d95137866fadda65e83d55b6cccf18e0d60 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 21 Mar 2026 09:51:26 -0700 Subject: [PATCH 2/4] fix because turret pose was being adjusted for twice --- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 9 +++++---- 1 file changed, 5 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 f437a688..d73f5d48 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -336,15 +336,16 @@ public static ShotData getSOTMShotDataNewtonsMethod( ShotData baseline = tree.calculateShot(robotPose, targetTranslation); - Pose2d turretPose = getTurretPose(robotPose); - Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); + // Pose2d turretPose = getTurretPose(robotPose); + // Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); - double distance = turretToTarget.getNorm(); + Translation2d robotToTarget = targetTranslation.minus(robotPose.getTranslation()); + double distance = robotToTarget.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); + Translation2d shotDirection = robotToTarget.div(distance); // dot product! <3 // get how fast we are going towards where we are shooting From a4262a200541d269ec91928a0e0030531b169582 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 21 Mar 2026 10:16:48 -0700 Subject: [PATCH 3/4] iterate for turret because why not --- .../java/frc/robot/utils/autoaim/AutoAim.java | 20 +++++++++++++------ 1 file changed, 14 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 d73f5d48..408b9278 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -235,18 +235,26 @@ public static Translation2d getVirtualSOTMTarget( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { - Pose2d vrobot = + + Translation2d vtarget = target; + Pose2d vrobot= robotPose; + + for (int i = 0; i < 20; i++) { + double tof = COMP_HUB_SHOT_TREE.get(distanceToHub(vrobot)).timeOfFlightSecs(); + vrobot = robotPose.exp( new Twist2d( - fieldRelativeSpeeds.vxMetersPerSecond * (timeOfFlightSecs), - fieldRelativeSpeeds.vyMetersPerSecond * (timeOfFlightSecs), - fieldRelativeSpeeds.omegaRadiansPerSecond * (timeOfFlightSecs))); + fieldRelativeSpeeds.vxMetersPerSecond * (tof), + fieldRelativeSpeeds.vyMetersPerSecond * (tof), + fieldRelativeSpeeds.omegaRadiansPerSecond * (tof))); Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation()); - Translation2d vtarget = target.minus(delta); - Logger.recordOutput("Autoaim/Virtual Target", vtarget); + vtarget = target.minus(delta); + } + Logger.recordOutput("Autoaim/Virtual Target", vtarget); return vtarget; + } public static Rotation2d getVirtualTargetYaw( From e6be9e2d385ee7d09b8889a7d6ec5846169d1df2 Mon Sep 17 00:00:00 2001 From: vivi-o Date: Sat, 21 Mar 2026 11:17:53 -0700 Subject: [PATCH 4/4] build --- .../java/frc/robot/utils/autoaim/AutoAim.java | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 408b9278..508e25b4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -237,24 +237,23 @@ public static Translation2d getVirtualSOTMTarget( double timeOfFlightSecs) { Translation2d vtarget = target; - Pose2d vrobot= robotPose; + Pose2d vrobot = robotPose; - for (int i = 0; i < 20; i++) { - double tof = COMP_HUB_SHOT_TREE.get(distanceToHub(vrobot)).timeOfFlightSecs(); - vrobot = - robotPose.exp( - new Twist2d( - fieldRelativeSpeeds.vxMetersPerSecond * (tof), - fieldRelativeSpeeds.vyMetersPerSecond * (tof), - fieldRelativeSpeeds.omegaRadiansPerSecond * (tof))); + for (int i = 0; i < 20; i++) { + double tof = COMP_HUB_SHOT_TREE.get(distanceToHub(vrobot)).timeOfFlightSecs(); + vrobot = + robotPose.exp( + new Twist2d( + fieldRelativeSpeeds.vxMetersPerSecond * (tof), + fieldRelativeSpeeds.vyMetersPerSecond * (tof), + fieldRelativeSpeeds.omegaRadiansPerSecond * (tof))); - Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation()); + Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation()); - vtarget = target.minus(delta); - } - Logger.recordOutput("Autoaim/Virtual Target", vtarget); + vtarget = target.minus(delta); + } + Logger.recordOutput("Autoaim/Virtual Target", vtarget); return vtarget; - } public static Rotation2d getVirtualTargetYaw( @@ -344,8 +343,8 @@ public static ShotData getSOTMShotDataNewtonsMethod( ShotData baseline = tree.calculateShot(robotPose, targetTranslation); - // Pose2d turretPose = getTurretPose(robotPose); - // Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); + // Pose2d turretPose = getTurretPose(robotPose); + // Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); Translation2d robotToTarget = targetTranslation.minus(robotPose.getTranslation()); double distance = robotToTarget.getNorm();