Vision49/AprilTagMServo.java at main · Niconut/Vision49 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import java.util.List;

/**
 * AprilTag-based turret centering with filtering and correct control direction.
 * Uses the FTC "easy" AprilTag initialization method.
 */
@TeleOp(name = "AprilTag Manual Servo ", group = "AprilTag")
public class AprilTagMServo extends LinearOpMode {

    private static final boolean USE_WEBCAM = true;  // true = webcam, false = phone camera

    private AprilTagProcessor aprilTag;
    private VisionPortal visionPortal;
    private CRServo mymotor;  // your servo name preserved
    private ElapsedTime timer = new ElapsedTime();

    // Persistent filter variable
    private double oldYaw = 0;

    @Override
    public void runOpMode() {

        // Initialize AprilTag vision using the easy API
        initAprilTag();

        // Initialize continuous rotation servo
        mymotor = hardwareMap.get(CRServo.class, "motor"); // your original name
        mymotor.setDirection(CRServo.Direction.REVERSE);
        telemetry.addData("Status", "Initialized — press START");
        telemetry.update();
        waitForStart();

        timer.reset();

        while (opModeIsActive()) {

            // --- Get current detections ---
            List<AprilTagDetection> detections = aprilTag.getDetections();
            telemetry.addData("# Tags Detected", detections.size());

            if (detections.isEmpty()) {
                // No tag visible → stop turret
                mymotor.setPower(0);
                telemetry.addLine("No tag detected → motor stopped");
            } else {
                // Use the first visible tag
                AprilTagDetection detection = detections.get(0);

                if (detection.metadata != null) {
                    double yaw = detection.ftcPose.yaw;  // degrees
                    double targetYaw = 0;                // centered target = 0°

                    // --- Low-pass filter parameters ---
                    double a = 10;
                    double b = 256;

                    // Proper filtering (no negative sign)
                    double yawFiltered = (a * yaw + (b - a) * oldYaw) / b;
                    oldYaw = yawFiltered;

                    // --- Proportional control ---
                    double kp = 0.01;
                    double error = targetYaw - yawFiltered;
                    double motorPower = kp * error;

                    // --- Deadband to prevent jitter ---
                    if (Math.abs(error) < 0.5) {
                        motorPower = 0;
                    }

                    // --- Safety clipping ---
                    motorPower = Math.max(-1, Math.min(1, motorPower));

                    // --- Apply power ---
                    mymotor.setPower(motorPower);

                    // --- Telemetry ---
                    telemetry.addLine(String.format("Tag ID: %d (%s)", detection.id, detection.metadata.name));
                    telemetry.addLine(String.format("Pose XYZ (in): x=%6.1f y=%6.1f z=%6.1f",
                            detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
                    telemetry.addData("Yaw (deg)", "%.2f", yaw);
                    telemetry.addData("Yaw Filtered", "%.2f", yawFiltered);
                    telemetry.addData("Error", "%.2f", error);
                    telemetry.addData("Motor Power", "%.2f", motorPower);
                } else {
                    telemetry.addLine(String.format("Unknown tag (ID %d)", detection.id));
                    telemetry.addLine(String.format("Center: (%.0f, %.0f)", detection.center.x, detection.center.y));
                }
            }

            telemetry.update();
            sleep(20);
        }

        // Stop vision safely
        visionPortal.close();
    }

    /**
     * Initialize AprilTag processor and VisionPortal the easy way.
     */
    private void initAprilTag() {
        // Create the AprilTag processor
        aprilTag = AprilTagProcessor.easyCreateWithDefaults();

        // Create the vision portal
        if (USE_WEBCAM) {
            visionPortal = VisionPortal.easyCreateWithDefaults(
                    hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); // your webcam name preserved
        } else {
            visionPortal = VisionPortal.easyCreateWithDefaults(
                    BuiltinCameraDirection.BACK, aprilTag);
        }
    }
}