diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleops/TeleOp1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleops/TeleOp1.java new file mode 100644 index 000000000000..ed6ccfffe174 --- /dev/null +++ b/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); + + + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/MechanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/MechanumDrive.java new file mode 100644 index 000000000000..2c696867a6f3 --- /dev/null +++ b/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); + } +}