FtcRC23535/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testDrive.java at master · msr09me/FtcRC23535 · 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
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

@TeleOp
public class testDrive extends OpMode {

    private DcMotor leftFrontDrive = null;
    private DcMotor rightFrontDrive = null;
    private DcMotor leftBackDrive = null;
    private DcMotor rightBackDrive = null;

    @Override
    public void init()
    {
        // Hardware mapping (to store the actual DC motors information to the objects

        leftFrontDrive = hardwareMap.get(DcMotor.class,"leftFront");
        rightFrontDrive = hardwareMap.get(DcMotor.class,"rightFront");
        leftBackDrive = hardwareMap.get(DcMotor.class,"leftBack");
        rightBackDrive = hardwareMap.get(DcMotor.class,"rightBack");

        leftFrontDrive.setDirection(DcMotorSimple.Direction.FORWARD);
        rightFrontDrive.setDirection(DcMotorSimple.Direction.FORWARD);
        leftBackDrive.setDirection(DcMotorSimple.Direction.FORWARD);
        rightBackDrive.setDirection(DcMotorSimple.Direction.FORWARD);

    }
    @Override
    public void loop()
    {
        double leftFrontPower = 0.0;
        double leftBackPower = 0.0;
        double rightFrontPower = 0.0;
        double rightBackPower = 0.0;

        double forward = -gamepad1.left_stick_y;
        double right = gamepad1.left_stick_x;
        double rotate = gamepad1.right_stick_x;

        leftFrontPower = forward + right + rotate;
        leftBackPower = forward - right + rotate;
        rightFrontPower = forward - right - rotate;
        rightBackPower = forward + right - rotate;

        double maxPower = 0.0;

        maxPower = Math.max(Math.abs(leftFrontPower), Math.abs(leftBackPower));

        maxPower = Math.max(maxPower, Math.abs(rightFrontPower));

        maxPower = Math.max(maxPower, Math.abs(rightBackPower));

        leftFrontPower = leftFrontPower / maxPower;
        leftBackPower = leftBackPower / maxPower;
        rightFrontPower = rightFrontPower / maxPower;
        rightBackPower = rightBackPower / maxPower;

        leftFrontDrive.setPower(leftFrontPower);
        rightFrontDrive.setPower(rightFrontPower);
        leftBackDrive.setPower(leftBackPower);
        rightBackDrive.setPower(rightBackPower);

    }
}