From 26055358992c0a2f168c040142841a684117d51e Mon Sep 17 00:00:00 2001 From: Zelina974 <155895303+Zelina974@users.noreply.github.com> Date: Sun, 25 Feb 2024 16:03:04 +0100 Subject: [PATCH] Create IMU.java --- IMU.java | 184 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 IMU.java diff --git a/IMU.java b/IMU.java new file mode 100644 index 0000000..6d21439 --- /dev/null +++ b/IMU.java @@ -0,0 +1,184 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + + +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; + +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@TeleOp(name="Yale Holo", group="WeRobot") +public class YaleHolo extends LinearOpMode { + + float rotate_angle = 0; + double reset_angle = 0; + + private DcMotor front_left_wheel = null; + private DcMotor back_left_wheel = null; + private DcMotor back_right_wheel = null; + private DcMotor front_right_wheel = null; + + IMU imu; + @Override + public void runOpMode() { + front_left_wheel = hardwareMap.dcMotor.get("flm"); + back_left_wheel = hardwareMap.dcMotor.get("blm"); + back_right_wheel = hardwareMap.dcMotor.get("brm"); + front_right_wheel = hardwareMap.dcMotor.get("frm"); + + front_left_wheel.setDirection(DcMotor.Direction.REVERSE); + back_left_wheel.setDirection(DcMotor.Direction.REVERSE); + front_right_wheel.setDirection(DcMotor.Direction.FORWARD); + back_right_wheel.setDirection(DcMotor.Direction.FORWARD); + + front_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + back_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + front_right_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + back_right_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + + imu = hardwareMap.get(IMU.class, "imu"); + + imu.initialize( + new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD + ) + ) + ); + imu.resetYaw(); + + while(!opModeIsActive()){} + + while(opModeIsActive()){ + drive(); + resetAngle(); + //driveSimple(); + telemetry.update(); + } + } + public void driveSimple(){ + double power = .5; + if(gamepad1.dpad_up){ //Forward + front_left_wheel.setPower(-power); + back_left_wheel.setPower(-power); + back_right_wheel.setPower(-power); + front_right_wheel.setPower(-power); + } + else if(gamepad1.dpad_left){ //Left + front_left_wheel.setPower(power); + back_left_wheel.setPower(-power); + back_right_wheel.setPower(power); + front_right_wheel.setPower(-power); + } + else if(gamepad1.dpad_down){ //Back + front_left_wheel.setPower(power); + back_left_wheel.setPower(power); + back_right_wheel.setPower(power); + front_right_wheel.setPower(power); + } + else if(gamepad1.dpad_right){ //Right + front_left_wheel.setPower(-power); + back_left_wheel.setPower(power); + back_right_wheel.setPower(-power); + front_right_wheel.setPower(power); + } + else if(Math.abs(gamepad1.right_stick_x) > 0){ //Rotation + front_left_wheel.setPower(-gamepad1.right_stick_x); + back_left_wheel.setPower(-gamepad1.right_stick_x); + back_right_wheel.setPower(gamepad1.right_stick_x); + front_right_wheel.setPower(gamepad1.right_stick_x); + } + else{ + front_left_wheel.setPower(0); + back_left_wheel.setPower(0); + back_right_wheel.setPower(0); + front_right_wheel.setPower(0); + } + } + public void drive() { + double Protate = gamepad1.right_stick_x/4; + double stick_x = gamepad1.left_stick_x * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2); //Accounts for Protate when limiting magnitude to be less than 1 + double stick_y = gamepad1.left_stick_y * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2); + double theta = 0; + double Px = 0; + double Py = 0; + + double gyroAngle = getHeading() * Math.PI / 180; //Converts gyroAngle into radians + if (gyroAngle <= 0) { + gyroAngle = gyroAngle + (Math.PI / 2); + } else if (0 < gyroAngle && gyroAngle < Math.PI / 2) { + gyroAngle = gyroAngle + (Math.PI / 2); + } else if (Math.PI / 2 <= gyroAngle) { + gyroAngle = gyroAngle - (3 * Math.PI / 2); + } + gyroAngle = -1 * gyroAngle; + + if(gamepad1.right_bumper){ //Disables gyro, sets to -Math.PI/2 so front is defined correctly. + gyroAngle = -Math.PI/2; + } + + //Linear directions in case you want to do straight lines. + if(gamepad1.dpad_right){ + stick_x = 0.5; + } + else if(gamepad1.dpad_left){ + stick_x = -0.5; + } + if(gamepad1.dpad_up){ + stick_y = -0.5; + } + else if(gamepad1.dpad_down){ + stick_y = 0.5; + } + + + //MOVEMENT + theta = Math.atan2(stick_y, stick_x) - gyroAngle - (Math.PI / 2); + Px = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta + Math.PI / 4)); + Py = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta - Math.PI / 4)); + + telemetry.addData("Stick_X", stick_x); + telemetry.addData("Stick_Y", stick_y); + telemetry.addData("Magnitude", Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2))); + telemetry.addData("Front Left", Py - Protate); + telemetry.addData("Back Left", Px - Protate); + telemetry.addData("Back Right", Py + Protate); + telemetry.addData("Front Right", Px + Protate); + + front_left_wheel.setPower(Py - Protate); + back_left_wheel.setPower(Px - Protate); + back_right_wheel.setPower(Py + Protate); + front_right_wheel.setPower(Px + Protate); + } + public void resetAngle(){ + if(gamepad1.a){ + reset_angle = getHeading() + reset_angle; + } + } + public double getHeading(){ + + + double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + if(heading < -180) { + heading = heading + 360; + } + else if(heading > 180){ + heading = heading - 360; + } + heading = heading - reset_angle; + return heading; + } +} +