RobotCoreExtensions/GyroAutoDriver.java at master · FTC3486/RobotCoreExtensions · 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
package org.firstinspires.ftc.teamcode.RobotCoreExtensions;

import com.qualcomm.robotcore.util.Range;

public class GyroAutoDriver extends AutoDriver
{
    public GyroAutoDriver(HardwareConfiguration hw) {
        super(hw);
    }

    private double getAdjustedHeading() {
        double sensorValue = hw.gyroSensor.getHeading();

        if(sensorValue > 180)
        {
            sensorValue -= 360;
        }
        return sensorValue;
    }

    public void driveStraightForwards(float distance, double power)
    {
        setupMotion("Driving straight forwards using the gyro sensor.");
        double target = hw.gyroSensor.getIntegratedZValue();
        hw.drivetrain.setPowers(power, power);
        int checkCount = 0;
        while (hw.drivetrain.getLeftEncoderCount() < hw.drivetrain.convertInchesToEncoderCounts(distance)
                && hw.drivetrain.getRightEncoderCount() < hw.drivetrain.convertInchesToEncoderCounts(distance)
                && hw.opMode.opModeIsActive())
        {
            checkCount++;
            int currentHeading = hw.gyroSensor.getIntegratedZValue();
            hw.drivetrain.setPowers(power + (currentHeading - target) / 50,
                                    power - (currentHeading - target) / 50);
            hw.opMode.telemetry.addData("Times checked:", checkCount);
            hw.opMode.telemetry.addData("Gyro Target:", target);
            hw.opMode.telemetry.addData("Gyro Heading:", currentHeading);
            hw.opMode.telemetry.update();
        }
        endMotion();
    }

    public void driveStraightBackwards(float distance, double power)
    {
        setupMotion("Driving straight backwards using the gyro sensor.");
        double target = hw.gyroSensor.getIntegratedZValue();  //Starting direction

        while (hw.drivetrain.getLeftEncoderCount() > -hw.drivetrain.convertInchesToEncoderCounts(distance) &&
                hw.drivetrain.getRightEncoderCount() > -hw.drivetrain.convertInchesToEncoderCounts(distance) &&
                hw.opMode.opModeIsActive())
        {
            int currentHeading = hw.gyroSensor.getIntegratedZValue();  //Current direction
            hw.drivetrain.setPowers(-power + (currentHeading - target) / 50,
                                    -power - (currentHeading - target) / 50);
        }
        endMotion();
    }

    public void turn(int target)
    {
        setupMotion("Turning using gyro.");
        double gyroHeading = this.getAdjustedHeading();

        while(gyroHeading  < target - 1 || gyroHeading > target + 1 && hw.opMode.opModeIsActive())
        {
            double power = ( (target - gyroHeading) / Math.abs(target)) * (1.0/4.0);

            power = Math.signum(power) * Range.clip(Math.abs(power), 0.05, 1.0);

            hw.drivetrain.setPowers(power, -power);
            gyroHeading = this.getAdjustedHeading();
        }
        endMotion();
    }
}