FtcRobotController/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLeft.java at master · FTC-19743/FtcRobotController · 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
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;

import org.firstinspires.ftc.teamcode.assemblies.AprilTagDetector;
import org.firstinspires.ftc.teamcode.assemblies.OpenCVSignalDetector;
import org.firstinspires.ftc.teamcode.assemblies.Robot23;
import org.firstinspires.ftc.teamcode.libs.teamUtil;
import org.openftc.easyopencv.OpenCvCameraRotation;

@Autonomous(name="AutoLeft")
public class AutoLeft extends LinearOpMode {
    public static void log(String logString) {
        RobotLog.d("19743LOG:" + Thread.currentThread().getStackTrace()[3].getMethodName() + ": " + logString);
    }

    Robot23 robot;
    //public OpenCVSignalDetector signalDetector;
    public AprilTagDetector tagDetector;

    @Override
    public void runOpMode() {
        teamUtil.init(this);
        robot = new Robot23();
        robot.initialize();
        robot.calibrate();
        robot.drive.initializePIDFCoefficients();
        robot.outake.closeGrabber();
        int defaultThreshold = robot.drive.backLeft.getTargetPositionTolerance();

        /*
        signalDetector = new OpenCVSignalDetector();
        signalDetector.initialize(true, true);
        signalDetector.activate();
         */
        tagDetector = new AprilTagDetector(1, OpenCvCameraRotation.UPRIGHT, telemetry, hardwareMap);
        tagDetector.initDetector(false);
        tagDetector.activate();
        telemetry.addLine("Waiting for Tag Detector");
        telemetry.update();
        while (!tagDetector.ready.get() ) { // Wait for detector to signal ready
            teamUtil.pause(250);
        }
        tagDetector.startProcessing();

        teamUtil.LEFT = true;

        telemetry.addLine("Waiting for start");
        telemetry.update();

        long now = System.currentTimeMillis();
        while (!opModeIsActive()) {
            //signalDetector.writeTelemetry();
            tagDetector.writeTelemetry();
            sleep(100); // save cpu for OpenCV
            if (System.currentTimeMillis() > now + 2000) {
                tagDetector.writeTelemetry();
                now = System.currentTimeMillis();
            }
        }

        waitForStart();
        int path = tagDetector.signalDetect();
        log("path" + path);
        tagDetector.deactivate();

        robot.newAutoV5(teamUtil.LEFT, path);
        //robot.halfwayJointTest();


    }
}