Decode-DemoBot/DecodeTeleop.java at main · Pattonville-Robotics/Decode-DemoBot · 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
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

@TeleOp

public class DecodeTeleop extends LinearOpMode {

    CommonAprilTag aprilTag = new CommonAprilTag ();
    CommonDecode common = new CommonDecode ();
    CommonMecanum mecanum = new CommonMecanum ();

    // gamepad for non-driving buttons
    Gamepad gamepad = gamepad1;

    @Override
    public void runOpMode() {

        aprilTag.initialize (this, hardwareMap);
        common.initialize (hardwareMap);
        mecanum.initialize (this, hardwareMap);

        telemetry.addData("Status", "Initialized");
        telemetry.update();

        // Wait for the game to start (driver presses START)
        waitForStart();

        // if gamepad 2 is connected, use it for non-driving buttons
        if (gamepad2.getGamepadId () > 0)
        {
            gamepad = gamepad2;
        telemetry.addData("Gamepad2", "Initialized");
        telemetry.update();
        }
        else
        {
            gamepad = gamepad1;
        telemetry.addData("Gamepad", "Only 1 gamepad!!");
        telemetry.update();
        }

        sleep(2000);

        // run until the end of the match (driver presses STOP)
        while (opModeIsActive()) {

            checkGamepadButtons ();

            common.checkShooterState ();

            mecanumDrive ();

            telemetry.addData("Ball Slot", common.getCurrentBallSlot ());
            telemetry.update();
        }
    }

    public void checkGamepadButtons ()
    {
        // shoot
        if (gamepad.left_trigger > 0)
        {
            common.shootOn (true);
        }
        if (gamepad.left_bumper)
        {
            common.shootOn (false);
        }

        // shoot all 3
        if (gamepad.dpadUpWasPressed())
        {
            common.shootAllBalls (true);
        }
        if (gamepad.dpadDownWasPressed())
        {
            common.shootAllBalls (false);
        }


        // retrieve
        if (gamepad.right_trigger > 0)
        {
            common.retrieveOn (true);
        }
        if (gamepad.right_bumper)
        {
            common.retrieveOn (false);
        }

        // rotate balls
        if (gamepad.bWasPressed())
        {
            common.rotateCW ();
        }
        if (gamepad.xWasPressed())
        {
            common.rotateCCW ();
        }

        // reverse intake
        if (gamepad.yWasPressed())
        {
            common.setIntakeReverse (true);
        }
        if (gamepad.aWasPressed())
        {
            common.setIntakeReverse (false);
        }
    }

    public void mecanumDrive ()
    {
        double y = -gamepad1.left_stick_y; // Remember, this is reversed!
        double x = gamepad1.left_stick_x * 0.9;  // 1.1 // Counteract imperfect strafing
        double rx = gamepad1.right_stick_x;

        // use a fifth order equation to make it slowly ramp up
        x = x * x * x * x * x;
        y = y * y * y * y * y;
        rx = rx * rx * rx * rx * rx;

        // limit max speed so students don't break it
        x = x * 0.4;
        y = y * 0.4;
        rx = rx * 0.4;

        mecanum.driveViaJoystick (x, y, rx);
    }
}