WalnutBootstrap/DistanceMotor.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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
package com.walnuthillseagles.walnutlibrary;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;

/**
 * Created by Yan Vologzhanin on 1/2/2016.
 */
public class DistanceMotor extends LinearMotor implements Runnable, Auto {
    //Constants
    //How often the code will check if its current operation is done

    //Precisions of wheels.
    public static final int RANGEVAL = 30;
    public static final long CLEARWAIT = 20;
    //Fields
    //Please measure in Inches
    private double circumference;
    private double gearRatio;
    //Distance value used in operate
    private int distance;
    //Parrallel Thread
    private Thread runner;

    public int operateCount;
    public DistanceMotor(DcMotor myMotor, String myName, boolean encoderCheck,boolean isReveresed,
                         double myDiameter,double myGearRatio, int myEncoder){
        //Create Motor
        super(myMotor, myName, encoderCheck, isReveresed);
        motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
        while(motor.getMode()!= DcMotorController.RunMode.RUN_TO_POSITION){
            try{
                synchronized (this){
                    this.wait(WAITRESOLUTION);
                }
            }
            catch(InterruptedException e){
                Thread.currentThread().interrupt();
            }
        }
        //Values involving bot
        circumference = myDiameter*Math.PI;
        gearRatio = myGearRatio;
        encoderRot = myEncoder;

        //Values involving distances
        distance = 0;
        speedLimit = 0;

        motor.setTargetPosition(0);
        motor.setPower(0);

        runner = new Thread();

        operateCount = 0;

    }
    //Starts operation with given parameters
    public void operate(double inches, double mySpeedLimit){

        distance = (int)((inches / circumference / gearRatio) * encoderRot * orientation);
        //Speedlimit is always positive
        speedLimit = Math.abs(mySpeedLimit);

        //Start new process
        runner = new Thread(this);

        //Clear locks from main thread
        synchronized (this){
            try{
                this.wait(CLEARWAIT);
            }
            catch(InterruptedException e){
                this.stop();
            }

        }
        runner.start();


    }
    public void operate(double inches) {
        this.operate(inches, 1);
    }
    //Allows other methods to change speed midway through method
    public void changeSpeedLimit(double mySpeedLimit){
        speedLimit = mySpeedLimit;
    }
    public void run(){
        //Go for it
        try {
            this.motor.setTargetPosition(distance);
            this.setPower(speedLimit);
            //Debug
            //Wait until in Pos
            //@TODO Better way to do this?
            //Wait for target position to update
            while(motor.getTargetPosition()==0 && distance != 0){
                synchronized (this){
                    this.wait(WAITRESOLUTION);
                }
            }
            do   {
                synchronized (this){
                    this.wait(WAITRESOLUTION);
                }

            }while(!inRange(distance, motor.getCurrentPosition()));

            this.fullStop();
            motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
            motor.setTargetPosition(0);
            distance = 0;
            operateCount++;
            runner = new Thread(this);
        }
        catch (InterruptedException e) {
            Thread.currentThread().interrupt();
            stop();

        }
    }
    public int getDistance(){
        return distance;
    }
    public double getSpeedLimit(){return speedLimit;}
    public void fullStop(){
        motor.setPower(0);
        try{
            resetEncoder();
            motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
            while(motor.getMode()!= DcMotorController.RunMode.RUN_TO_POSITION){
                timer.sleep(WAITRESOLUTION);
        }
        }
        catch(InterruptedException e){
            motor.setPower(0);
            Thread.currentThread().interrupt();
        }
    }
    //Timers
    public void waitForCompletion() throws InterruptedException{
        if(runner != null && runner.isAlive())
            runner.join();
    }
    //Private helper methods

}