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

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorController;

/**
 * Created by Yan Vologzhanin on 2/7/2016.
 */
public class SimpleMotor {
    //Field
    protected DcMotor motor;
    //Used for telemetry
    protected String name;
    protected boolean hasEncoders;

    //Timer used for each object
    protected LinearOpMode timer;
    public static final long WAITRESOLUTION = 200;
    public static final double MAXPOW = 0.9;
    SimpleMotor(DcMotor myMotor, String myName, boolean encoderCheck){
        timer = new LinearOpMode() {
            @Override
            public void runOpMode() throws InterruptedException {
                return;
            }
        };
        motor = myMotor;
        this.stop();
        name = myName;
        if(encoderCheck){
            hasEncoders = true;
            try{
                resetEncoder();
            }
            catch (InterruptedException e){
                Thread.currentThread().interrupt();
            }

        }
        else{
            hasEncoders = false;
            motor.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
        }


    }
    public DcMotor getMotor(){
        return motor;
    }

    @Override
    public String toString(){
        return this.name;
    }
    public void setName(String myName){
        name = myName;
    }
    //Methods
    public void fullStop(){
        motor.setPower(0);
        if(hasEncoders){
            try{
                resetEncoder();
            }
            catch(InterruptedException e){
                motor.setPower(0);
                Thread.currentThread().interrupt();
            }

        }
    }
    public void stop(){
        motor.setPower(0);
    }
    public void setPower(double pow){
        //First statement should catch pow=0, but just to be sure...
        if(pow>=-1.0 * MAXPOW&&pow <=MAXPOW)
            motor.setPower(pow);
        else if(pow!=0)
            motor.setPower(MAXPOW * (pow/Math.abs(pow)));
        //val/|val| = 1 or -1, depending if val is negetive
    }
    protected void resetEncoder() throws InterruptedException {

        if (motor != null) {
            motor.setMode(DcMotorController.RunMode.RESET_ENCODERS);
            while (motor.getCurrentPosition() != 0) {
                synchronized (this){
                    this.wait(WAITRESOLUTION);
                }
            }
            motor.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
        }


    }
    protected void sleep(long time) throws InterruptedException{
        timer.sleep(time);
    }
}