FTC-Code/GriffinDrive.java at main · gmanandmarbles/FTC-Code · 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
package org.fireinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;

@Autonomous(name="Simple Drive Test", group="Drive")
public class GriffinDrive extends LinearOpMode {

    private DcMotor motor1 = null;
    private DcMotor motor2 = null;

    @Override
    public void runOpMode() {

        motor1 = hardwareMap.get(DcMotor.class, "Motor");
        motor2 = hardwareMap.get(DcMotor.class, "Motor2");

        motor1.setTargetPosition(0);
        motor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        motor1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        motor1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        motor2.setTargetPosition(0);
        motor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        motor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
        motor2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        telemetry.addData("Status", "Initialized and Ready");
        telemetry.update();

        waitForStart();

        if (opModeIsActive()) {

            double motorPower = 0.5;

            telemetry.addData("The Motor power is", motorPower);
            telemetry.update();

            motor1.setPower(motorPower);
            motor2.setPower(motorPower);

            telemetry.addData("Status", "Motors are spinning");
            telemetry.update();

            sleep(2000);

            motor1.setPower(0.0);
            motor2.setPower(0.0);

            motor1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
            motor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

            telemetry.addData("Status", "Motors stopped");
            telemetry.update();
        }
    }
}