WalnutBootstrap/EncoderMotor.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
package com.walnuthillseagles.walnutlibrary;

import com.qualcomm.robotcore.hardware.DcMotor;

/**
 * Created by Yan Vologzhanin on 2/9/2016.
 */
public class EncoderMotor extends LinearMotor implements Runnable, Auto{
    private double circumference;
    private double gearRatio;
    private double encoderRot;
    private int distance;
    //Constants
    public static final int ENCODERTOLERANCE = 10;

    EncoderMotor(DcMotor myMotor, String myName,  boolean isReversed,
                 double myDiameter,double myGearRatio, int myEncoder){
        super(myMotor, myName, true, isReversed);
        circumference = myDiameter*Math.PI;
        gearRatio = myGearRatio;
        encoderRot = myEncoder;

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

        runner = new Thread();
    }
    public void operate(double inches, double mySpeedLimit){
        distance = (int)((inches / circumference / gearRatio) * encoderRot * orientation);
        speedLimit = mySpeedLimit;
        //Start new process
        runner = new Thread(this);
        runner.start();
    }
    public void operate(double inches){
        if(inches *orientation > 0)
            this.operate(inches, 1);
        else if(inches *orientation < 0)
            this.operate(inches, -1);
    }
    public void run(){
        motor.setPower(speedLimit);
        boolean canProcess = true;
        while(canProcess){
            try{
                if(Math.abs(motor.getCurrentPosition())>= Math.abs(distance)-ENCODERTOLERANCE){
                    //Includes Reset command
                    this.fullStop();
                    canProcess = false;
                }
                else
                    motor.wait(WAITRESOLUTION);
            }
            catch(InterruptedException e){
                canProcess= false;
                Thread.currentThread().interrupt();
            }

        }
        this.fullStop();

    }
    public void waitForCompletion() throws InterruptedException{
        runner.join();
    }
    public void setPower(double pow){
        motor.setPower(pow * orientation);
    }

}