TeamCode/TeleopTesting at CenterStage · MechaKnights/TeamCode · 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
package org.firstinspires.ftc.teamcode.drive.opmode;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;

@TeleOp(group = "drive")
public class TeleopTesting extends LinearOpMode {
    private DcMotorEx lift;
    private DcMotorEx lift2;
    private static double speed1 = 0.5;
    private static double speed2 = speed1*2;
    private static double speed3 = speed1*1.75;
    private DcMotor swingBar;
    private CRServo claw;
    private CRServo claw2;
    public static double curr = 0;
    private static double swing = 0;
    private boolean planeSwitch = false;
    private Servo yellowServo;
    private Servo plane;
    private DistanceSensor distance;

    @Override
    public void runOpMode() throws InterruptedException {
        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);

        drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);

        lift = hardwareMap.get(DcMotorEx.class, "lift");
        lift2 = hardwareMap.get(DcMotorEx.class, "lift2");
        claw = hardwareMap.get(CRServo.class, "claw");
        claw2 = hardwareMap.get(CRServo.class, "claw2");
        yellowServo = hardwareMap.get(Servo.class, "yellowServo");
        plane = hardwareMap.get(Servo.class, "plane");
        distance = hardwareMap.get(DistanceSensor.class, "distance");
        swingBar = hardwareMap.get(DcMotor.class, "swingBar");

        lift2.setDirection(DcMotorEx.Direction.REVERSE);
        claw.setDirection(DcMotorSimple.Direction.REVERSE);
//        swingBar.setDirection(DcMotorSimple.Direction.REVERSE);

        waitForStart();
        while (!isStopRequested()) {
            curr = swingBar.getCurrentPosition();
            drive.setWeightedDrivePower(
                    new Pose2d(
                            -gamepad1.left_stick_y * speed1,
                            -gamepad1.left_stick_x * speed2,
                            -gamepad1.right_stick_x * speed3
                    )
            );

            drive.update();

            Pose2d poseEstimate = drive.getPoseEstimate();
            telemetry.addData("x", poseEstimate.getX());
            telemetry.addData("y", poseEstimate.getY());
            telemetry.addData("heading", poseEstimate.getHeading());
            telemetry.addData("curr: ", curr);
            telemetry.addData("distance: ", distance.getDistance(DistanceUnit.INCH));
            telemetry.update();

            if (gamepad1.right_bumper) {
                speed1 = 0.23;
            } else {
                speed1 = 0.5;
            }
            speed2 = speed1*2;
            speed3 = speed1*1.75;

            if (gamepad2.dpad_up) {
                lift.setPower(1);
                lift2.setPower(1);
            } else if (gamepad2.dpad_down) {
                lift.setPower(-1);
                lift2.setPower(-1);
            }else {
                lift.setPower(0.1);
                lift2.setPower(0.1);
            }

            if (gamepad2.left_bumper) {
                claw.setPower(-0.6);
                claw2.setPower(-0.6);
            } else if (gamepad2.right_bumper) {
                claw.setPower(0.6);
                claw2.setPower(0.6);
            } else {
                claw.setPower(0);
                claw2.setPower(0);
            }
            if (gamepad2.x) {
                swingBar.setPower(0.5);
                swing = 0;
            } else if (gamepad2.a) {
                swingBar.setPower(-0.5);
                swing = 1;
            } else if (swing == 0) {
                swingBar.setPower(0.08);
            } else if (swing == 1) {
                swingBar.setPower(-0.08);
            }

            if (gamepad1.left_bumper) {
                yellowServo.setPosition(0.7);
            }

            if (gamepad2.y || planeSwitch == true) {
                plane.setPosition(1);
                plane.setDirection(Servo.Direction.REVERSE);
                planeSwitch = false;
            } else if (gamepad2.y || planeSwitch == false) {
                plane.setPosition(0);
                plane.setDirection(Servo.Direction.FORWARD);
                planeSwitch = true;
            }
        }
    }
}