From 2066f41b9310ced2175e1c579c67db92b208242b Mon Sep 17 00:00:00 2001 From: Blue-Flag-666 <64064866+Blue-Flag-666@users.noreply.github.com> Date: Sun, 15 Dec 2024 22:38:46 +0800 Subject: [PATCH] Add REVIMU and rumble for Xbox --- .../ftc/teamcode/MecanumOpMode.java | 19 +-- .../ftc/teamcode/lib/REVIMU.java | 135 ++++++++++++++++++ .../firstinspires/ftc/teamcode/lib/Xbox.java | 8 ++ 3 files changed, 154 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/REVIMU.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java index 5091732516a0..e94ef3dbd067 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java @@ -2,14 +2,13 @@ import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.arcrobotics.ftclib.geometry.Translation2d; -import com.arcrobotics.ftclib.hardware.RevIMU; import com.arcrobotics.ftclib.hardware.motors.Motor; -import com.qualcomm.hardware.bosch.BHI260IMU; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.teamcode.lib.Mecanum; +import org.firstinspires.ftc.teamcode.lib.REVIMU; import org.firstinspires.ftc.teamcode.lib.Xbox; @TeleOp(name = "Mecanum", group = "Iterative OpMode") @@ -17,17 +16,21 @@ public class MecanumOpMode extends OpMode { private final ElapsedTime timer = new ElapsedTime(); private final Translation2d size = new Translation2d(0.1675, 0.2075); private Mecanum chassis; - private BHI260IMU imu; + private REVIMU imu; private Xbox driver; @Override public void init() { - imu = hardwareMap.get(BHI260IMU.class, "imu"); + imu = new REVIMU(hardwareMap); chassis = new Mecanum(hardwareMap, size, "frontLeft", "frontRight", "rearLeft", "rearRight", Motor.GoBILDA.RPM_312, imu); - driver = new Xbox(gamepad1).onPressed(GamepadKeys.Button.B, () -> chassis.toggleHeadless()); + driver = new Xbox(gamepad1).onPressed(GamepadKeys.Button.B, () -> { + if (chassis.toggleHeadless()) { + driver.rumble(); + } + }); telemetry.addData("Status", "Initialized"); } @@ -47,9 +50,9 @@ public void loop() { chassis.drive(-driver.getLeftX(), driver.getLeftY(), -driver.getRightX()); - telemetry.addData("Status", "Time: " + timer); - telemetry.addData("Speed", "Speed: " + chassis.getWheelSpeeds()); - telemetry.addData("Pose", "Pose: " + chassis.getPose()); + telemetry.addData("Time", timer); + telemetry.addData("Speed", chassis.getWheelSpeeds()); + telemetry.addData("Pose", chassis.getPose()); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/REVIMU.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/REVIMU.java new file mode 100644 index 000000000000..bed8c0999f74 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/REVIMU.java @@ -0,0 +1,135 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.arcrobotics.ftclib.geometry.Rotation2d; +import com.arcrobotics.ftclib.hardware.GyroEx; +import com.qualcomm.hardware.bosch.BHI260IMU; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +public class REVIMU extends GyroEx { + private final BHI260IMU imu; + /*** + * Heading relative to starting position + */ + double globalHeading; + /** + * Heading relative to last offset + */ + double relativeHeading; + /** + * Offset between global heading and relative heading + */ + double offset; + private int multiplier; + + /** + * Create a new object for the built-in gyro/imu in the REV Expansion Hub with the default configuration name of "imu" + * + * @param hw Hardware map + */ + public REVIMU(HardwareMap hw) { + this(hw, "imu"); + } + + /** + * Create a new object for the built-in gyro/imu in the REV Expansion Hub + * + * @param hw Hardware map + * @param imuName Name of sensor in configuration + */ + public REVIMU(HardwareMap hw, String imuName) { + imu = hw.get(BHI260IMU.class, imuName); + multiplier = 1; + } + + /** + * Initializes gyro with default parameters. + */ + public void init() { + init(new BHI260IMU.Parameters( + new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD))); + } + + /** + * Initializes gyro with custom parameters. + */ + public void init(BHI260IMU.Parameters parameters) { + imu.initialize(parameters); + + globalHeading = 0; + relativeHeading = 0; + offset = 0; + } + + /** + * @return Relative heading of the robot + */ + public double getHeading() { + // Return yaw + return getAbsoluteHeading() - offset; + } + + /** + * @return Absolute heading of the robot + */ + @Override + public double getAbsoluteHeading() { + return imu.getRobotOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, + AngleUnit.DEGREES).firstAngle * multiplier; + } + + /** + * @return X, Y, Z angles of gyro + */ + public double[] getAngles() { + // make a singular hardware call + Orientation orientation = imu.getRobotOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, + AngleUnit.DEGREES); + + return new double[]{orientation.firstAngle, orientation.secondAngle, + orientation.thirdAngle}; + } + + /** + * @return Transforms heading into {@link Rotation2d} + */ + @Override + public Rotation2d getRotation2d() { + return Rotation2d.fromDegrees(getHeading()); + } + + @Override + public void reset() { + offset += getHeading(); + } + + /** + * Inverts the ouptut of gyro + */ + public void invertGyro() { + multiplier *= -1; + } + + @Override + public void disable() { + imu.close(); + } + + @Override + public String getDeviceType() { + return "REV Internal IMU"; + } + + /** + * @return the internal sensor being wrapped + */ + public BHI260IMU getRevImu() { + return imu; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Xbox.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Xbox.java index 163096e098a7..6144a33b4618 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Xbox.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Xbox.java @@ -15,6 +15,14 @@ public Xbox(Gamepad gamepad) { super(gamepad); } + public void rumble() { + rumble(0.5, 0.5); + } + + public void rumble(double power, double seconds) { + gamepad.rumble(power, power, (int) (seconds * 1000)); + } + public Xbox onPressed(Button button, Runnable func) { Set runnables = onPressedActions.get(button); if (runnables == null) {