diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 9f32c7b4..508e25b4 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -225,19 +225,33 @@ 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) { + + 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 * (tof), + fieldRelativeSpeeds.vyMetersPerSecond * (tof), + fieldRelativeSpeeds.omegaRadiansPerSecond * (tof))); + + Translation2d delta = vrobot.getTranslation().minus(robotPose.getTranslation()); + + vtarget = target.minus(delta); + } Logger.recordOutput("Autoaim/Virtual Target", vtarget); return vtarget; } @@ -315,7 +329,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)); @@ -329,15 +343,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