* The sample will display the current Yaw, Pitch and Roll of the robot.
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
*
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
*
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) *
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments. *
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at * the alternative SensorImuNonOrthogonal sample in this folder. *
* This "Orthogonal" requirement means that: *
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions: * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. *
* 2) The USB ports can only be pointing in one of the same six directions:
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* So, To fully define how your Hub is mounted to the robot, you must simply specify:
* logoFacingDirection
* usbFacingDirection
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. *
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode * to use those parameters. */ @TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor") @Disabled // Comment this out to add to the OpMode list public class SensorIMUOrthogonal extends LinearOpMode { // The IMU sensor object IMU imu; //---------------------------------------------------------------------------------------------- // Main logic //---------------------------------------------------------------------------------------------- @Override public void runOpMode() throws InterruptedException { // Retrieve and initialize the IMU. // This sample expects the IMU to be in a REV Hub and named "imu". imu = hardwareMap.get(IMU.class, "imu"); /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values. * * Two input parameters are required to fully specify the Orientation. * The first parameter specifies the direction the printed logo on the Hub is pointing. * The second parameter specifies the direction the USB connector on the Hub is pointing. * All directions are relative to the robot, and left/right is as-viewed from behind the robot. */ /* The next two lines define Hub orientation. * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. * * To Do: EDIT these two lines to match YOUR mounting configuration. */ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); // Now initialize the IMU with this mounting orientation // Note: if you choose two conflicting directions, this initialization will cause a code exception. imu.initialize(new IMU.Parameters(orientationOnRobot)); // Loop and update the dashboard while (!isStopRequested()) { telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection); // Check to see if heading reset is requested if (gamepad1.y) { telemetry.addData("Yaw", "Resetting\n"); imu.resetYaw(); } else { telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); } // Retrieve Rotational Angles and Velocities YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); telemetry.update(); } } }