FTC-Code/Bot.java at master · IsaacRatliff/FTC-Code · 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
package workspace_;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.ElapsedTime;


public class Bot {
    private DcMotor leftDrive_0 = null;
    private DcMotor rightDrive_0 = null;
    private DcMotor leftDrive_1 = null;
    private DcMotor rightDrive_1 = null;
    private DcMotor arm = null;
    private Servo claw1 = null;
    private Servo claw2 = null;
    public final double speed = 6.45/4;  // it took 6.45 sec to go 4 tiles at full spd
    public final double turn90 = 1.8;  // 1.8 seconds to turn 90 degrees
    private ElapsedTime runtime = new ElapsedTime();
    private Telemetry telemetry = null;

    public void leftWheels(double spd){
        leftDrive_0.setPower(spd);
        leftDrive_1.setPower(spd);
    }

    public void rightWheels(double spd){
        rightDrive_0.setPower(spd);
        rightDrive_1.setPower(spd);
    }

    public void forward(double tiles){
        leftWheels(1.0);
        rightWheels(1.0);
        runtime.reset();
        while(runtime.seconds() < speed * tiles){
            telemetry.addData("Path", "Forward: %2.5f S Elapsed", runtime.seconds());
            telemetry.update();
        }
    }

    public void backward(double tiles){
        leftWheels(-1.0);
        rightWheels(-1.0);
        runtime.reset();
        while(runtime.seconds() < speed * tiles){
            telemetry.addData("Path", "Backward: %2.5f S Elapsed", runtime.seconds());
            telemetry.update();
        }
    }

    public void left(double quarters){
        leftWheels(-1.0);
        rightWheels(1.0);
        runtime.reset();
        while(runtime.seconds() < turn90 * quarters){
            telemetry.addData("Path", "Turning Left: %2.5f S Elapsed", runtime.seconds());
            telemetry.update();
        }
    }

    public void right(double quarters){
        leftWheels(1.0);
        rightWheels(-1.0);
        runtime.reset();
        while(runtime.seconds() < turn90 * quarters){
            telemetry.addData("Path", "Turning Right: %2.5f S Elapsed", runtime.seconds());
            telemetry.update();
        }
    }

    public void stop(){
        leftWheels(0.0);
        rightWheels(0.0);
    }

    public Bot(HardwareMap hardwareMap, Telemetry telemetry, boolean has_arm, boolean has_claw){
        this.telemetry = telemetry;
        leftDrive_0  = hardwareMap.get(DcMotor.class, "leftDrive_0");
        rightDrive_0 = hardwareMap.get(DcMotor.class, "rightDrive_0");
        leftDrive_1  = hardwareMap.get(DcMotor.class, "leftDrive_1");
        rightDrive_1 = hardwareMap.get(DcMotor.class, "rightDrive_1");

        leftDrive_0.setDirection(DcMotor.Direction.FORWARD);
        rightDrive_0.setDirection(DcMotor.Direction.REVERSE);
        leftDrive_1.setDirection(DcMotor.Direction.FORWARD);
        rightDrive_1.setDirection(DcMotor.Direction.REVERSE);

        if(has_arm){
            arm = hardwareMap.get(DcMotor.class, "arm");
        }

        if(has_claw){
            claw1 = hardwareMap.get(Servo.class, "claw1");
            claw2 = hardwareMap.get(Servo.class, "claw2");
        }
    }

    public Bot(HardwareMap hardwareMap, Telemetry telemetry, boolean has_arm){
        this(hardwareMap, telemetry, has_arm, has_arm);
    }

    public Bot(HardwareMap hardwareMap, Telemetry telemetry){
        this(hardwareMap, telemetry, true, true);
    }
}