FTC-2024/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/blueAuto.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
package org.firstinspires.ftc.teamcode;

import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.ElapsedTime;


@Autonomous

public class blueAuto extends LinearOpMode {

    MecanumDrive driveBase;

    @Override
    public void runOpMode() {

        ElapsedTime time = new ElapsedTime();

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

        driveBase = new MecanumDrive (frontLeftMotor, frontRightMotor, backLeftMotor, backRightMotor);


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

        strafeRight(1);
        //stop the robot
        requestOpModeStop();


    }

    public void driveFoward(double time) {

        double strafeSpeed = 0;
        double fowardSpeed = -1;
        double rotateSpeed = 0;
        double heading = 0;

        drive(strafeSpeed ,fowardSpeed, rotateSpeed, heading, time);

    }

    public void strafeLeft(double time) {

        double strafeSpeed = 1;
        double fowardSpeed = -1;
        double rotateSpeed = 0;
        double heading = 0;

        drive(strafeSpeed ,fowardSpeed, rotateSpeed, heading, time);

    }

    public void strafeRight(double time) {

        double strafeSpeed = -1;
        double fowardSpeed = -1;
        double rotateSpeed = 0;
        double heading = 0;

        drive(strafeSpeed ,fowardSpeed, rotateSpeed, heading, time);

    }


    public void drive(double strafeSpeed, double fowardSpeed, double rotateSpeed, double heading, double time ) {

        double startTime = getRuntime();

        while(getRuntime() - startTime < time && opModeIsActive()) {

            // this. is only needed if a parameter has the same name
            driveBase.driveFieldCentric(strafeSpeed, fowardSpeed, rotateSpeed,heading, false);

        }

    }


}