FtcRobotController/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WCRobotField.java at master · WilliamChou123/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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
package org.firstinspires.ftc.teamcode;

import static com.shprobotics.pestocore.processing.FrontalLobe.teleOpController;
import static com.shprobotics.pestocore.processing.FrontalLobe.tracker;

import android.os.CountDownTimer;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.shprobotics.pestocore.processing.MotorCortex;


import org.opencv.core.Mat;

@TeleOp(name = "WCRobotField", group = "Linear Opmode")
@Disabled

public class WCRobotField extends BaseRobot {
    WCRobotMethods robotNew;

    @Override
    public void runOpMode() {


        robotNew = new WCRobotMethods(hardwareMap, telemetry);
        double lastTime = getRuntime();
//        robotNew.Outtake.setDirection(DcMotor.Direction.REVERSE);
        robotNew.initImu();

        waitForStart();
        int pos = 45;
        int slidePos = 0;
        boolean pauseSlide = false;
        PestoFTCConfig.initializePinpoint = true;
        super.runOpMode();
        while (opModeIsActive() && !isStopRequested()) {
            if (gamepad1.left_bumper) {
                robotNew.imu.resetYaw();
                robotNew.resetIMU();
            }

//
//            telemetry.addData("pos", pos);
//            telemetry.addData("lastTime", getRuntime() - lastTime);
//            telemetry.addData("slidePos", slidePos);
//            telemetry.addData("armROTATION", robotNew.rotation);

            robotNew.updateTelemetry(telemetry);
            if (gamepad1.right_bumper) {
                if (getRuntime() - lastTime > 2.2) {
                    robotNew.Outtake.setPower(.8);
                    robotNew.Middle.setPower(.8);
                } else {
                    robotNew.Outtake.setPower(.8);
                }
            } else {
                //no Right trigger
                robotNew.Intake.setPower(gamepad1.left_trigger - (gamepad1.right_trigger));
                robotNew.Outtake.setPower(-(gamepad1.right_trigger));
                robotNew.Gecko.setPower(-(gamepad1.right_trigger));
                robotNew.Middle.setPower(-(gamepad1.right_trigger));

                lastTime = getRuntime();
            }
            robotNew.moveMid(gamepad1.left_trigger);

            if (gamepad1.dpad_left) {
                while (gamepad1.dpad_left) {
                }
                robotNew.armTop();
            }
            if (gamepad1.dpad_right) {
                while (gamepad1.dpad_right) {
                }
                robotNew.armBlock();
            }

            if (gamepad1.b) {
                tracker.reset();
                teleOpController.resetIMU();
            }

            telemetry.addData("x", tracker.getCurrentPosition().getX());
            telemetry.addData("y", tracker.getCurrentPosition().getY());
            telemetry.addData("r", tracker.getCurrentPosition().getHeadingRadians());
            telemetry.update();
            MotorCortex.update();
            tracker.update();

            if (gamepad1.right_trigger > 0.5) {
                teleOpController.driveFieldCentric(-gamepad1.left_stick_y * 0.3, gamepad1.left_stick_x * 0.3, gamepad1.right_stick_x * 0.3);

            } else {

                teleOpController.driveFieldCentric(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
            }


        }
    }
}