Add REVIMU and rumble for Xbox

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java

@@ -2,32 +2,35 @@
 
 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")
 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

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;
+    }
+}

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<Runnable> runnables = onPressedActions.get(button);
         if (runnables == null) {