Fallback Mode
IMU-only aiming is your most predictable backup mode when vision confidence drops. Build and tune this mode first.
Step 1
Read Heading and Pose
Heading source: GyroscopeBHIMU.
Position source: localizer pose estimate.
Use these to compute basket-relative angle.
Step 2
Apply Direction Logic
Core idea from ArtifactControl:
double calculatedAngle = Math.abs(Math.toDegrees(
Math.atan2(positive_x_position, positive_y_position)
));
if (isRedAlliance) targetAngle = calculatedAngle;
else targetAngle = 360 - calculatedAngle;
if (targetAngle - headingAngle > 0) {
basketAngle = targetAngle - headingAngle;
rotateToLeft = basketAngle < 180;
} else {
basketAngle = 360 - Math.abs(headingAngle - targetAngle);
rotateToLeft = basketAngle < 180;
}
if (basketAngle >= 180) basketAngle = 360 - basketAngle;
Step 3
Convert Angle to Turret + Flywheel Commands
Use turret position model + clamp bounds.
Use distance-based flywheel model + clamp bounds.
Apply deadzones before writing servos.
Step 4
Add Launch Gating
Require legal shooting zone.
Require stable motion state before auto release.
Allow emergency cancel anytime by driver.
Step 5
Tune for Repeatability
Test at short, mid, and long basket distances.
Measure shot grouping and adjust model coefficients carefully.
Re-check after drivetrain or turret maintenance.
Next
Compare with Webcam Only, then move to IMU + Webcam Fusion.