diff --git a/src/main/java/frc/robot/subsystems/Cannon.java b/src/main/java/frc/robot/subsystems/Cannon.java index 83212601..2a31a8bb 100644 --- a/src/main/java/frc/robot/subsystems/Cannon.java +++ b/src/main/java/frc/robot/subsystems/Cannon.java @@ -337,39 +337,32 @@ public Command shootOTF() { double robotX = pose.getX(); double robotY = pose.getY(); - double sin = pose.getRotation().getSin(); - double cos = pose.getRotation().getCos(); + double poseTheta = pose.getRotation().getRadians(); + double omega = speeds.omegaRadiansPerSecond; double targetX = target.getX(); double targetY = target.getY(); - // Shooter offset in field frame (constant for this loop since dtheta=0) + // Shooter offset (robot-relative, constant) double shooterOffX = CannonConstants.SHOOTER_TRANSLATION.getX(); double shooterOffY = CannonConstants.SHOOTER_TRANSLATION.getY(); - double shooterFieldX = shooterOffX * cos - shooterOffY * sin; - double shooterFieldY = shooterOffX * sin + shooterOffY * cos; - - // Robot-relative velocity of the shooter offset due to chassis rotation, - // then rotated to field frame and added to chassis velocity. - // This matches getFuturePoseFromTime's existing math. - double rrXVel = -speeds.omegaRadiansPerSecond * shooterOffY; - double rrYVel = speeds.omegaRadiansPerSecond * shooterOffX; - double totalVx = speeds.vxMetersPerSecond + (rrXVel * cos - rrYVel * sin); - double totalVy = speeds.vyMetersPerSecond + (rrXVel * sin + rrYVel * cos); double driveMultiplier = LightningShuffleboard.getDouble("Cannon", "OTF Multiplier", 1); // ── Initial state: current shooter position ────────────── + double sin0 = Math.sin(poseTheta); + double cos0 = Math.cos(poseTheta); double futureX = robotX; double futureY = robotY; - double sx = robotX + shooterFieldX; - double sy = robotY + shooterFieldY; + double sx = robotX + shooterOffX * cos0 - shooterOffY * sin0; + double sy = robotY + shooterOffX * sin0 + shooterOffY * cos0; double futureDist = Math.sqrt((targetX - sx) * (targetX - sx) + (targetY - sy) * (targetY - sy)); + double futureTheta = poseTheta; double toleranceSq = CannonConstants.OTF_TOLERANCE.in(Meters); - toleranceSq *= toleranceSq; // compare squared to avoid sqrt in convergence check + toleranceSq *= toleranceSq; // ── Convergence loop (zero allocations) ────────────────── @@ -377,12 +370,31 @@ public Command shootOTF() { double tof = CannonConstants.TIME_OF_FLIGHT_MAP.getBaseUnit(futureDist); double dt = tof * driveMultiplier; - // Predict future robot position: pose.exp(twist) with dtheta=0 - // simplifies to rotating the twist by the heading then translating. - double twistDx = totalVx * dt; - double twistDy = totalVy * dt; - double rawX = robotX + twistDx * cos - twistDy * sin; - double rawY = robotY + twistDx * sin + twistDy * cos; + // Future heading after rotation during the ball's flight + futureTheta = poseTheta + omega * dt; + + // Use the midpoint heading for a better displacement estimate. + // This is a trapezoidal approximation: as the robot rotates, + // its velocity direction (in field frame) sweeps through an arc. + // Using the heading halfway through the TOF averages the start + // and end directions, which is much more accurate than using + // only the current heading. + double midTheta = poseTheta + omega * dt * 0.5; + double midSin = Math.sin(midTheta); + double midCos = Math.cos(midTheta); + + // Robot-relative velocity → field-relative using midpoint heading + double vxField = speeds.vxMetersPerSecond * midCos - speeds.vyMetersPerSecond * midSin; + double vyField = speeds.vxMetersPerSecond * midSin + speeds.vyMetersPerSecond * midCos; + + // Add shooter offset velocity from rotation, also at midpoint heading + double rrXVel = -omega * shooterOffY; + double rrYVel = omega * shooterOffX; + vxField += rrXVel * midCos - rrYVel * midSin; + vyField += rrXVel * midSin + rrYVel * midCos; + + double rawX = robotX + vxField * dt; + double rawY = robotY + vyField * dt; // Under-relaxation: blend toward prediction to guarantee convergence double prevX = futureX; @@ -390,9 +402,11 @@ public Command shootOTF() { futureX = prevX + CannonConstants.OTF_RELAXATION * (rawX - prevX); futureY = prevY + CannonConstants.OTF_RELAXATION * (rawY - prevY); - // Distance from future shooter position to target - sx = futureX + shooterFieldX; - sy = futureY + shooterFieldY; + // Shooter offset at the FUTURE heading (robot will have rotated) + double futureSin = Math.sin(futureTheta); + double futureCos = Math.cos(futureTheta); + sx = futureX + shooterOffX * futureCos - shooterOffY * futureSin; + sy = futureY + shooterOffX * futureSin + shooterOffY * futureCos; futureDist = Math.sqrt((targetX - sx) * (targetX - sx) + (targetY - sy) * (targetY - sy)); // Convergence check (squared distance avoids sqrt) @@ -412,10 +426,22 @@ public Command shootOTF() { hood.setPosition(hoodAngle); shooter.setVelocity(shooterVelocity); - Pose2d futurePose = new Pose2d(futureX, futureY, pose.getRotation()); + Pose2d futurePose = new Pose2d(futureX, futureY, new Rotation2d(futureTheta)); LightningShuffleboard.setPose2d("Cannon", "Future Pose", futurePose); - turret.turretAim(new Pose2d(getShooterTranslation(futurePose), futurePose.getRotation()), getTarget(), getRobotAngularVelocity(), getHubAngularVelocity()); + // Compute hub angular velocity from FUTURE pose (not current), + // so the feedforward matches where we're actually aiming. + double futureAngle = Math.atan2(targetY - sy, targetX - sx); + Translation2d fieldVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) + .rotateBy(pose.getRotation()); + double hubOmega = (-fieldVel.getX() * Math.sin(futureAngle) + + fieldVel.getY() * Math.cos(futureAngle)) / futureDist; + + turret.turretAim( + new Pose2d(getShooterTranslation(futurePose), futurePose.getRotation()), + getTarget(), + getRobotAngularVelocity(), + RadiansPerSecond.of(hubOmega)); }, turret, shooter, hood) .alongWith(indexWhenOnTarget().onlyWhile(() -> turret.isOnTarget(Degrees.of(12))).repeatedly()); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index b3df66ee..bab2cccb 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -49,6 +49,7 @@ import frc.robot.constants.FieldConstants; import frc.robot.constants.MirageTunerConstants.TunerSwerveDrivetrain; import frc.util.AllianceHelpers; +import frc.util.shuffleboard.LightningShuffleboard; import frc.util.simulation.SwerveSim; /** @@ -433,22 +434,30 @@ public Pose2d getFuturePoseFromTime(Time time) { ChassisSpeeds speeds = getCurrentRobotChassisSpeeds(); double dt = time.in(Seconds); + double driveMultiplier = LightningShuffleboard.getDouble("Cannon", "OTF Multiplier", 1); + Pose2d pose = getPose(); - double sin = pose.getRotation().getSin(); - double cos = pose.getRotation().getCos(); + // Use midpoint heading for better accuracy during rotation + double omega = speeds.omegaRadiansPerSecond; + double midTheta = pose.getRotation().getRadians() + omega * dt * 0.5; + double midSin = Math.sin(midTheta); + double midCos = Math.cos(midTheta); - double rrXVel = (speeds.omegaRadiansPerSecond * Cannon.CannonConstants.SHOOTER_TRANSLATION.getX()); - double rrYVel = (speeds.omegaRadiansPerSecond * Cannon.CannonConstants.SHOOTER_TRANSLATION.getY()); + double rrXVel = (-omega * Cannon.CannonConstants.SHOOTER_TRANSLATION.getY()); + double rrYVel = (omega * Cannon.CannonConstants.SHOOTER_TRANSLATION.getX()); - double frXVel = (rrXVel * cos) - (rrYVel * sin); - double frYVel = (rrXVel * sin) + (rrYVel * cos); + double frXVel = (rrXVel * midCos) - (rrYVel * midSin); + double frYVel = (rrXVel * midSin) + (rrYVel * midCos); + + // Robot-relative chassis velocity rotated by midpoint heading + double vxField = speeds.vxMetersPerSecond * midCos - speeds.vyMetersPerSecond * midSin; + double vyField = speeds.vxMetersPerSecond * midSin + speeds.vyMetersPerSecond * midCos; - Twist2d twist = new Twist2d( - (speeds.vxMetersPerSecond + frXVel) * dt, - (speeds.vyMetersPerSecond + frYVel) * dt, - 0 + (vxField + frXVel) * dt * driveMultiplier, + (vyField + frYVel) * dt * driveMultiplier, + omega * dt ); return pose.exp(twist);