velocity_vortex/VelocityTeleOp.java at master · CommandoRobotics/velocity_vortex · 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
package org.firstinspires.ftc.robotcontroller.external.samples;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;

/**
 * Created by CHHS on 10/16/2016.
 */
public class VelocityTeleOp extends OpMode {

    HDriveAPI driveTrain;
    HopperAPI hopper;
    CollecterAPI collector;
//    ShooterAPI shooter;
    FingerAPI fingers;

    boolean slow;
    boolean yPressed;
    boolean xPressed;
    boolean bPressed;

    public void init() {
        driveTrain = new HDriveAPI(hardwareMap);
        hopper = new HopperAPI(hardwareMap);
        collector = new CollecterAPI(hardwareMap);
//        shooter = new ShooterAPI(hardwareMap);
        fingers = new FingerAPI(hardwareMap);

        if(collector.getOn()) collector.toggleOn(0.8f);
        collector.collect();
        if(hopper.getOn()) hopper.toggleOn(0.8f);

        slow = false;
        yPressed = false;
        xPressed = false;
        bPressed = false;
    }

    public void loop() {
        moveRobot();
        collect();
        moveBalls();
//        shoot();
        thrustFingers();
    }

    private void moveRobot() {
        if(gamepad1.y && !yPressed) {
           slow = !slow;
            yPressed = true;
        } else if(!gamepad1.y) {
            yPressed = false;
        }

        float leftPowerY = -gamepad1.left_stick_y; float rightPowerY = -gamepad1.right_stick_y;
        float leftPowerX = gamepad1.left_stick_x; float rightPowerX = gamepad1.right_stick_x;

//        telemetry.addData("LeftX: ", leftPowerX);
//        telemetry.addData("LeftY: ", leftPowerY);
//        telemetry.addData("RightX: ", rightPowerX);
//        telemetry.addData("RightY: ", rightPowerY);

        if(Math.abs(leftPowerX) < 0.1) leftPowerX = 0;
        if(Math.abs(rightPowerX) < 0.1) rightPowerX = 0;
        if(Math.abs(leftPowerY) < 0.1) leftPowerY = 0;
        if(Math.abs(rightPowerY) < 0.1) rightPowerY = 0;

        if(Math.abs(leftPowerY) + Math.abs(rightPowerY) > Math.abs(leftPowerX) + Math.abs(rightPowerX)) {
            if (!slow) driveTrain.drive(leftPowerY, rightPowerY);
            else driveTrain.driveSlower(leftPowerY, rightPowerY);
        } else {
            driveTrain.strafe((leftPowerX + rightPowerX) / 2);
        }
    }

    private void collect() {
        //bumpers for collector
//        if(gamepad1.left_bumper) {
//            if(!collector.getOn()) collector.toggleOn(0.80f);
//            collector.collect();
//        } else if(gamepad1.right_bumper) {
//            if (!collector.getOn()) collector.toggleOn(0.80f);
//            collector.expel();
//        } else
        if(gamepad1.b && !bPressed) {
            collector.toggleOn(0.8f);
            bPressed = true;
        } else if(!gamepad1.b) {
            bPressed = false;
        }
    }

    private void moveBalls() {
        if(gamepad1.left_trigger > 0.1) {
            if(!hopper.getOn()) hopper.toggleOn(0.80f);
            hopper.beginLifting();
            collector.collect();
        } else if(gamepad1.right_trigger > 0.1) {
            if (!hopper.getOn()) hopper.toggleOn(0.8f);
            hopper.beginLowering();
            collector.expel();
        } else if(gamepad1.x && !xPressed) {
            hopper.toggleOn(0.8f);
            xPressed = true;
        } else if(!gamepad1.x) {
            xPressed = false;
        }
    }

    private void shoot() {
//        if(gamepad1.y) shooter.toggleOn(0.8f);
    }

   private void thrustFingers() {
        //TODO: Set this to the d-pad for gamepad1
        if(gamepad1.left_bumper) {
            fingers.setLeftFingerTarget((float)(fingers.getLeftFingerPosition()) - 0.1f);
        }

       if(gamepad1.right_bumper) {
            fingers.setRightFingerTarget((float)(fingers.getRightFingerPosition()) - 0.1f);
        }

       fingers.updateFingers();
       telemetry.addData("Left Bumper: ", gamepad1.left_bumper);
       telemetry.addData("Right Bumper: ", gamepad1.right_bumper);
       telemetry.addData("Left Finger Position: ", fingers.getLeftFingerPosition());
       telemetry.addData("Right Finger Position: ", fingers.getRightFingerPosition());
       telemetry.addData("Left Finger Target: ", fingers.getLeftFingerTarget());
       telemetry.addData("Right Finger Target: ", fingers.getRightFingerTarget());
    }
}