FTC-2024/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TheNemoOp.java at master · holyokecodes/FTC-2024 · 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
package org.firstinspires.ftc.teamcode;

import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.gamepad.ButtonReader;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.arcrobotics.ftclib.gamepad.TriggerReader;
import com.arcrobotics.ftclib.hardware.ServoEx;
import com.arcrobotics.ftclib.hardware.SimpleServo;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;


@TeleOp(name="TheNemoOp", group="Linear OpMode")
public class TheNemoOp extends LinearOpMode {

    // Declare OpMode members.
    private ElapsedTime runtime = new ElapsedTime();

    //this stuff happens when init pressed
    @Override
    public void runOpMode() {

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


        //set up motors
        Motor frontLeftMotor = new Motor (hardwareMap, "fl");
        Motor frontRightMotor = new Motor (hardwareMap, "fr");
        Motor backLeftMotor = new Motor (hardwareMap, "bl");
        Motor backRightMotor = new Motor (hardwareMap, "br");

        //set up servos
        ServoEx scooperServo = new SimpleServo(hardwareMap, "handRotator", 0, 180);
        ServoEx armServo = new SimpleServo(hardwareMap, "armServo", 0 , 180);


        int scooperTargetPosition = 0;
        int armTargetPosition = 180;

        boolean scooperFlipped = false;
        boolean armFlipped = false;


//        airplaneMotor.setRunMode(Motor.RunMode.RawPower);

        //set up mecanum
        MecanumDrive driveBase = new MecanumDrive (frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor);

        //set up controller
        GamepadEx controller1 = new GamepadEx(gamepad1);

        //set up buttons
        ButtonReader leftBumper = new ButtonReader(controller1, GamepadKeys.Button.LEFT_BUMPER);
        ButtonReader rightBumper = new ButtonReader(controller1, GamepadKeys.Button.RIGHT_BUMPER);
        ButtonReader xButton = new ButtonReader(controller1, GamepadKeys.Button.X);
        ButtonReader yButton = new ButtonReader(controller1, GamepadKeys.Button.Y);

        TriggerReader rightTrigger = new TriggerReader(controller1, GamepadKeys.Trigger.RIGHT_TRIGGER);
        TriggerReader leftTrigger = new TriggerReader(controller1, GamepadKeys.Trigger.LEFT_TRIGGER);


        scooperServo.setPosition(scooperTargetPosition);

        //wait for the game to start
        waitForStart();
        runtime.reset();

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


            //read buttons
            leftBumper.readValue();
            rightBumper.readValue();
            xButton.readValue();
            yButton.readValue();


            //set movement speed
            double strafeSpeed = -controller1.getLeftX();
            double forwardSpeed = -controller1.getLeftY();
            double rotateSpeed =  -controller1.getRightX();


            // scooper mover
            if (xButton.wasJustReleased()) {
                if(scooperFlipped) {
                    scooperTargetPosition = 0;
                    scooperFlipped = false;
                } else {
                    scooperTargetPosition= 180;
                    scooperFlipped = true;
                }
            }

            //  arm mover
            if (yButton.wasJustPressed()) {
                if(armFlipped) {
                    armTargetPosition = 0;
                    armFlipped = false;
                } else {
                    armTargetPosition= 180;
                    armFlipped = true;
                }
            }

            if (rightTrigger.isDown()) {
                armTargetPosition += 5;
            } else if (leftTrigger.isDown()) {
                armTargetPosition -= 5;
            }


            // move robot
            driveBase.driveFieldCentric(strafeSpeed, forwardSpeed, rotateSpeed,0, false);

            scooperServo.setPosition(scooperTargetPosition);
            armServo.setPosition(armTargetPosition);


            telemetry.update();


        }
    }
}