WalnutBootstrap/GyroDrive.java at master · yanvolo/WalnutBootstrap · 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
104
105
106
107
108
109
110
111
112
113
114
package com.walnuthillseagles.walnutlibrary;

import com.qualcomm.robotcore.hardware.GyroSensor;


/**
 * Created by Yan Vologzhanin on 1/4/2016.
 */
@Deprecated
public class GyroDrive extends DistanceDrive implements Runnable {
    public static final long WAITRESOLUTION = 200;
    //Hardware
    private GyroSensor gyro;
    private Thread runner;
    private int targetHeading;
    private boolean runningLinear;
    private boolean runningTurn;
    //Constatns that allow you to modify behaviors
    //Value used when motors need to be realigned
    public static final double MOTORADJUSTMENTPOW = 0.8;
    public static final int GYROTOLERANCE = 2;

    public GyroDrive(DistanceMotor myLeft, DistanceMotor myRight, double myWidth, GyroSensor myGyro){
        super(myLeft,myRight,myWidth);
        gyro = myGyro;
        runner = new Thread(this);
        targetHeading = 0;
        gyro.calibrate();
        //@TODO Make sure this does not stall
        while(gyro.isCalibrating()){
            try{
                Thread.sleep(WAITRESOLUTION);
            }
            catch(InterruptedException e){
                Thread.currentThread().interrupt();
            }
        }
        runningLinear = false;
        runningTurn = false;
        runner = new Thread(this);
    }
    public void LinearDrive(double inches){
        leftDrive.operate(inches);
        rightDrive.operate(inches);
    }
    public void tankTurn(int degrees){
        double turnOrientation = (degrees < 0) ? -1: 1;
        int tempHeading = degrees;
        int currentPos = gyro.getHeading();
        while(currentPos +tempHeading >= 360){
            tempHeading -= 360;
        }
        while(currentPos + tempHeading < 0){
            tempHeading += 360;
        }
        targetHeading = currentPos + tempHeading;
        runner = new Thread(this);
        runner.start();
    }
    public void waitForCompletion() throws InterruptedException{
        leftDrive.waitForCompletion();
        rightDrive.waitForCompletion();
        runner.join();
    }
    //Parralell Threads
    public void run(){

    }

    private int compareOrientation(int target, int currentVal){
        int tarMax = target + GYROTOLERANCE;
        int tarMin = target - GYROTOLERANCE;
        int midPoint = target + 180;

        boolean inRange = false;
        //Adjust Range and check if we are in said range
        //@TODO Pretty sure I messed this up
        if(tarMax >= 360 || tarMin < 0)
            inRange = (currentVal > adjustRange(tarMin)|| currentVal<adjustRange(tarMax));
        else
            inRange = ((currentVal > tarMin) && currentVal < tarMax);
        if(inRange)
            return 0;
        else //if(!inRange)
        {
            if(midPoint<target){
                if(currentVal>midPoint && currentVal<target)
                    return -1;
                else //if(currentVal<midPoint || currentVal>target)
                    return 1;
            }
            else //if(midPoint>target)
            {
                if(currentVal>target && currentVal < target)
                    return 1;
                else // if(currentVal>midPoint || currentVal<target)
                    return -1;
            }
        }
    }
    private int adjustRange(int tarValue){
        int temp = tarValue;
        while(temp>=360){
            temp-=360;
        }
        while(temp<0){
            temp += 360;
        }
        return temp;
    }

}
    //Stuff I worry about