Muchen (teleop, odom)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleops/TeleOp1.java

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.Teleops;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.teamcode.mechanisms.MechanumDrive;
+
+@TeleOp(name="TeleOp1", group="Linear OpMode")
+public class TeleOp1 extends LinearOpMode {
+
+    MechanumDrive drivetrain = new MechanumDrive();
+
+    @Override
+    public void runOpMode() {
+        //Initializes the motors in our Method aka function
+        drivetrain.init(hardwareMap);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+
+            //runs our movement method with our gamepad parameters
+            drivetrain.drive(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+
+
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/MechanumDrive.java

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.mechanisms;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+public class MechanumDrive {
+
+    private DcMotor frontLeft;
+    private DcMotor frontRight;
+    private DcMotor backLeft;
+    private DcMotor backRight;
+
+    public void init(HardwareMap hardwareMap) {
+        frontLeft = hardwareMap.get(DcMotor.class, "front_left_motor");
+        frontRight = hardwareMap.get(DcMotor.class, "front_right_motor");
+        backLeft = hardwareMap.get(DcMotor.class, "back_left_motor");
+        backRight = hardwareMap.get(DcMotor.class, "back_right_motor");
+
+
+        frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        backLeft.setDirection(DcMotor.Direction.REVERSE);
+    }
+
+    public void drive(double drive, double strafe, double turn) {
+
+        //Driving sideways usually takes more power than driving forwards
+        strafe = strafe * 1.25;
+
+        double SpeedMultiplier = 1;
+
+        double flPower = (drive + strafe + turn) * SpeedMultiplier;
+        double frPower = (drive - strafe - turn) * SpeedMultiplier;
+        double blPower = (drive - strafe + turn) * SpeedMultiplier;
+        double brPower = (drive + strafe - turn) * SpeedMultiplier;
+
+        // Normalize the values so no wheel power exceeds 100%
+        double max = Math.max(Math.abs(flPower), Math.abs(frPower));
+        max = Math.max(max, Math.abs(blPower));
+        max = Math.max(max, Math.abs(brPower));
+
+        if (max > 1.0) {
+            flPower /= max;
+            frPower /= max;
+            blPower /= max;
+            brPower /= max;
+        }
+
+        frontLeft.setPower(flPower);
+        frontRight.setPower(frPower);
+        backLeft.setPower(blPower);
+        backRight.setPower(brPower);
+    }
+}