LynxRobotics2016/turnTest.java at master · Lynx-Robotics/LynxRobotics2016 · 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
package org.firstinspires.ftc.teamcode;

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

/**
 * Created by Student on 10/18/2016.
 */
@Autonomous(name="Turn test", group="Iterative Opmode")
public class turnTest extends LinearOpMode {
    public GyroSensor gyroSensor=null;
    public DcMotor leftMotor   = null;
    public DcMotor rightMotor   = null;

    public void turnRightDegrees(double X) {
        gyroSensor.calibrate();
        while (gyroSensor.isCalibrating()) {
            telemetry.addData("Calibrating", true);
            telemetry.update();
        }
        while (gyroSensor.getHeading() < X - 18||gyroSensor.getHeading()>357) {
            leftMotor.setPower(-.2);
            rightMotor.setPower(.2);
            telemetry.addData("current angle: ", gyroSensor.getHeading());
            telemetry.addData("Fast", true);
            telemetry.update();
        }
        while (gyroSensor.getHeading() <= X) {
            leftMotor.setPower(-.07);
            rightMotor.setPower(.07);
            telemetry.addData("current angle: ", gyroSensor.getHeading());
            telemetry.addData("Slow", true);
            telemetry.update();
        }

        leftMotor.setPower(0);
        rightMotor.setPower(0);
    }
    public void turnLeftDegrees(double X){
            //turn left
        gyroSensor.calibrate();
        while (gyroSensor.isCalibrating()) {
            telemetry.addData("Calibrating", true);
            telemetry.update();
        }
            X=360-X;
            while(gyroSensor.getHeading()>X+18||gyroSensor.getHeading()<3){
                leftMotor.setPower(.2);
                rightMotor.setPower(-.2);
                telemetry.addData("current angle: ",gyroSensor.getHeading());
                telemetry.addData("Fast",true);
                telemetry.update();
            }
            while(gyroSensor.getHeading()>=X){
                leftMotor.setPower(.07);
                rightMotor.setPower(-.07);
                telemetry.addData("current angle: ",gyroSensor.getHeading());
                telemetry.addData("Slow",true);
                telemetry.update();
            }
        leftMotor.setPower(0);
        rightMotor.setPower(0);
    }
    @Override
    public void runOpMode() throws InterruptedException {
        waitForStart();
        leftMotor=hardwareMap.dcMotor.get("left_drive");
        rightMotor=hardwareMap.dcMotor.get("right_drive");
        leftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
        rightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
        gyroSensor=hardwareMap.gyroSensor.get("gyro");
        leftMotor.setPower(1);
        rightMotor.setPower(-1);
        //turnRightDegrees(135);
        telemetry.addData("current angle: ",gyroSensor.getHeading());
        telemetry.addData("Calibrating",false);
        telemetry.update();
        while(opModeIsActive()){
            telemetry.addData("current angle: ",gyroSensor.getHeading());
            telemetry.addData("Calibrating",false);
            telemetry.update();
        }
    }
}