From 85f036ab6ebeb23f0b0451c67b80287d3b66abd2 Mon Sep 17 00:00:00 2001
From: PenguinEncounter <49845522+penguinencounter@users.noreply.github.com>
Date: Mon, 1 Jul 2024 18:14:20 -0700
Subject: [PATCH] Mecanum drive teleop (#1)

It worked at the metting.
---
 .../ftc/teamcode/MecanumTeleOp.java           | 43 +++++++++++++++++++
 1 file changed, 43 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java
new file mode 100644
index 000000000000..19d30e878b0d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java
@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode;
+
+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.DcMotorSimple;
+
+@TeleOp
+public class MecanumTeleOp extends LinearOpMode {
+    @Override
+    public void runOpMode() {
+        DcMotor frontLeft = hardwareMap.dcMotor.get("frontLeft");
+        DcMotor backLeft = hardwareMap.dcMotor.get("backLeft");
+        DcMotor frontRight = hardwareMap.dcMotor.get("frontRight");
+        DcMotor backRight = hardwareMap.dcMotor.get("backRight");
+
+        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()) {
+            double y = -gamepad1.left_stick_y;
+            double x = gamepad1.left_stick_x * 1.1;
+            double rx = gamepad1.right_stick_x;
+
+            // Denominator is the largest motor power (absolute value) or 1
+            // This ensures all the powers maintain the same ratio,
+            // but only if at least one is out of the range [-1, 1]
+            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+            double frontLeftPower = (y + x + rx) / denominator;
+            double backLeftPower = (y - x + rx) / denominator;
+            double frontRightPower = (y - x - rx) / denominator;
+            double backRightPower = (y + x - rx) / denominator;
+
+            frontLeft.setPower(frontLeftPower);
+            backLeft.setPower(backLeftPower);
+            frontRight.setPower(frontRightPower);
+            backRight.setPower(backRightPower);
+        }
+    }
+}