december14Code/teleopProgramV2.java at main · GrumpisGrump/december14Code · 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
133
134
135
136
137
138
139
140
141
142
143
144
package december14Code;

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.Servo;

// Declare the OpMode and set its name for the Driver Station
@TeleOp(name = "TeleOpV2", group = "Drive")
public class teleopProgramV2 extends LinearOpMode {

    // Declares drivetrain motors
    public DcMotor leftFront, leftRear, rightFront, rightRear;
    // Declares lift motors
    public DcMotor leftLiftMotor, rightLiftMotor;
    // Declares lift servo
    public Servo liftServo;
    // lift servo controls
    boolean liftUp = gamepad1.dpad_up;
    boolean liftDown = gamepad1.dpad_down;

//    // Declares Intake motors
//    public DcMotor intakeSlideMotor, intakeMotor;

    private void moveLift(int targetPosition) {
        boolean stopLift = gamepad1.x;

        while (!stopLift) {
            leftLiftMotor.setTargetPosition(targetPosition);
            rightLiftMotor.setTargetPosition(targetPosition);

            leftLiftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            rightLiftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        }
    }

    @Override
    public void runOpMode() {
        // Initialize the hardware variables
        initHardware();

        // Wait for the start button to be pressed
        waitForStart();

        // Main loop to control the robot
        while (opModeIsActive()) {
            // Get the joystick inputs from the gamepad
            double drive = -gamepad1.left_stick_y; // Forward/reverse
            double strafe = gamepad1.left_stick_x; // Strafing (left/right)
            double rotate = gamepad1.right_stick_x; // Rotation (turning)
            double Lift = gamepad1.right_stick_y; // Lift Up/Down
            double basketUp = gamepad1.right_trigger; // Basket Up
            double basketDown = gamepad1.left_trigger; // Basket Down
//            boolean intakeSlides = gamepad1.a; // Intake Extender/Retracted
//            boolean Intake = gamepad1.x; // Spin up/down intake

            // Compute the wheel powers based on mecanum drive kinematics
            double leftFrontPower = drive + strafe + rotate;
            double rightFrontPower = drive - strafe - rotate;
            double leftRearPower = drive - strafe + rotate;
            double rightRearPower = drive + strafe - rotate;

            // Normalize the wheel powers so no value exceeds 1
            double maxPower = Math.max(Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)),
                    Math.max(Math.abs(leftRearPower), Math.abs(rightRearPower)));

            if (maxPower > 1.0) {
                leftFrontPower /= maxPower;
                rightFrontPower /= maxPower;
                leftRearPower /= maxPower;
                rightRearPower /= maxPower;
            }

            // Sets the drivetrain motor powers
            leftFront.setPower(leftFrontPower);
            rightFront.setPower(rightFrontPower);
            leftRear.setPower(leftRearPower);
            rightRear.setPower(rightRearPower);

            if (Lift < 0) {
                moveLift(0);
            }

            if (liftUp) {
                liftServo.setPosition(.5);
            }
            else if (liftDown) {
                liftServo.setPosition(-.25);
            }

//            // Sets the lift up/down motor powers
//            leftLiftMotor.setPower(Lift);
//            rightLiftMotor.setPower(-Lift);
//
//            // Sets the basket up/down motor powers
//            leftLiftMotor.setPower(basketUp);
//            rightLiftMotor.setPower(basketUp);
//            leftLiftMotor.setPower(-basketDown);
//            rightLiftMotor.setPower(-basketDown);

            // Telemetry to debug motor power and joystick input
            telemetry.addData("Left Stick Y", gamepad1.left_stick_y);
            telemetry.addData("Left Stick X", gamepad1.left_stick_x);
            telemetry.addData("Right Stick X", gamepad1.right_stick_x);
            telemetry.addData("Right Trigger", gamepad1.right_trigger);
            telemetry.addData("Left Trigger", gamepad1.left_trigger);
            telemetry.addData("Dpad Down", gamepad1.dpad_down);
            telemetry.addData("Dpad Up", gamepad1.dpad_up);
            telemetry.addData("LF Power", leftFrontPower);
            telemetry.addData("RF Power", rightFrontPower);
            telemetry.addData("LR Power", leftRearPower);
            telemetry.addData("RR Power", rightRearPower);
            telemetry.addData("Slide Lift Power", Lift);
            telemetry.addData("Basket Up Power", basketUp);
            telemetry.addData("Basket Down Power", basketDown);
            telemetry.update();
        }
    }

    // Initialize motors and encoders
    private void initHardware() {
        leftFront = hardwareMap.get(DcMotor.class, "leftFront");
        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
        leftRear = hardwareMap.get(DcMotor.class, "leftRear");
        rightRear = hardwareMap.get(DcMotor.class, "rightRear");
        leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLiftMotor");
        rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLiftMotor");
        liftServo = hardwareMap.get(Servo.class, "liftServo");

        // Set motor directions (adjust based on your setup)
        leftFront.setDirection(DcMotor.Direction.FORWARD);
        rightFront.setDirection(DcMotor.Direction.REVERSE);
        leftRear.setDirection(DcMotor.Direction.FORWARD);
        rightRear.setDirection(DcMotor.Direction.REVERSE);

        // Set motors to run using encoders
        leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        leftRear.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        rightRear.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        // intakeSlideMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

    }
}