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

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.JavaUtil;
@Autonomous(name = "SquareAuton")
public class SquareAuton extends LinearOpMode {
    private DcMotor rearLeft;
    private DcMotor frontLeft;
    private DcMotor rearRight;
    private DcMotor frontRight;

    double leftFrontPower;
    double leftBackPower;
    double rightFrontPower;
    double rightBackPower;
    double axial(ElapsedTime running_time) {
        if (running_time.seconds() < 3) {
            return 0.25;
        } else if (running_time.seconds() < 6) {
            return 0;
        } else if (running_time.seconds() < 9) {
            return -0.25;
        } else if (running_time.seconds() < 12) {
            return 0;
        } else {
            return 0;
        }
    }
    double lateral(ElapsedTime running_time) {
        if (running_time.seconds() < 3) {
            return 0;
        } else if (running_time.seconds() < 6) {
            return -0.25;
        } else if (running_time.seconds() < 9) {
            return 0;
        } else if (running_time.seconds() < 12) {
            return 0.25;
        } else {
            return 0;
        }
    }
    public void runOpMode() {
        rearLeft = hardwareMap.get(DcMotor.class, "rearLeft");
        frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
        rearRight = hardwareMap.get(DcMotor.class, "rearRight");
        frontRight = hardwareMap.get(DcMotor.class, "frontRight");

        // Set zero power behaviour
        rearLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rearRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        rearLeft.setDirection(DcMotor.Direction.REVERSE);
        frontLeft.setDirection(DcMotor.Direction.REVERSE);
        rearRight.setDirection(DcMotor.Direction.FORWARD);
        frontRight.setDirection(DcMotor.Direction.FORWARD);

        ElapsedTime runtime = new ElapsedTime();

        telemetry.addData("Status", "Initialized");
        telemetry.update();
        waitForStart();
        runtime.reset();

        while(opModeIsActive()) {
            leftFrontPower = axial(runtime) + lateral(runtime);
            rightFrontPower = axial(runtime) - lateral(runtime);
            leftBackPower = axial(runtime) - lateral(runtime);
            rightBackPower = axial(runtime) + lateral(runtime);
            // Send calculated power to wheels.
            frontLeft.setPower(leftFrontPower);
            frontRight.setPower(rightFrontPower);
            rearLeft.setPower(leftBackPower);
            rearRight.setPower(rightBackPower);
            // Show the elapsed game time and wheel power.
            telemetry.addData("Status", "Run Time: " + runtime);
            telemetry.addData("Front left/Right", JavaUtil.formatNumber(leftFrontPower, 4, 2) + ", " + JavaUtil.formatNumber(rightFrontPower, 4, 2));
            telemetry.addData("Back  left/Right", JavaUtil.formatNumber(leftBackPower, 4, 2) + ", " + JavaUtil.formatNumber(rightBackPower, 4, 2));
            telemetry.update();
        }
    }
}