2867-Decode/BasicMovement2.java at main · Pattonville-Robotics/2867-Decode · 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
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

@TeleOp
public class BasicMovement2 extends LinearOpMode{
    @Override
    public void runOpMode() throws InterruptedException {

        DcMotor motorRight = hardwareMap.dcMotor.get("motorRight"); //
        DcMotor motorRightback = hardwareMap.dcMotor.get("motorRightback");   //
        DcMotor motorLeft = hardwareMap.dcMotor.get("motorLeft"); //
        DcMotor motorLeftback = hardwareMap.dcMotor.get("motorLeftback");   //
        DcMotorEx motorWheel = hardwareMap.get(DcMotorEx.class, "motorWheel");  //
        DcMotorEx motorWheelred = hardwareMap.get(DcMotorEx.class, "motorWheelred");
        Servo pusherahhting = hardwareMap.get(Servo.class, "pusherahhting");


        motorLeft.setDirection(DcMotor.Direction.REVERSE);
        motorLeftback.setDirection(DcMotor.Direction.REVERSE);
        waitForStart();
        //Stick power
        while (opModeIsActive()) {
            double y = gamepad1.left_stick_y; // Remember, this is reversed!
            double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
            double rx = -gamepad1.right_stick_x;

            //Maths
            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
            double LeftPower = (Math.pow((y + x + rx), 3) * 0.8) / denominator;
            double LeftbackPower = (Math.pow((y - x + rx), 3) * 0.8) / denominator;
            double RightPower = (Math.pow((y - x - rx), 3) * 0.8) / denominator;
            double RightbackPower = (Math.pow((y + x - rx), 3) * 0.8) / denominator;

            motorLeft.setPower(LeftPower);
            motorLeftback.setPower(LeftbackPower);
            motorRight.setPower(RightPower);
            motorRightback.setPower(RightbackPower);
            telemetry.addLine("motorWheel.1");
            telemetry.update();

             //shooting
            if (gamepad1.left_trigger>0){
                motorWheel.setVelocity(1500);
                motorWheelred.setVelocity(-1500);
            }
            if (gamepad1.left_trigger == 0){
                motorWheel.setVelocity(0);
                motorWheelred.setVelocity(0);
            }
            //Servo
            if (gamepad1.right_trigger>0){
                pusherahhting.setPosition(.25);
            }
            if (gamepad1.right_trigger == 0){
                pusherahhting.setPosition(.0);

            }


        }
        }
    }