forked from acmerobotics/road-runner-quickstart
-
Notifications
You must be signed in to change notification settings - Fork 61
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
10 changed files
with
611 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
185 changes: 185 additions & 0 deletions
185
...rg/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,185 @@ | ||
| /* | ||
| Copyright (c) 2022 REV Robotics, FIRST | ||
| All rights reserved. | ||
| Redistribution and use in source and binary forms, with or without modification, | ||
| are permitted (subject to the limitations in the disclaimer below) provided that | ||
| the following conditions are met: | ||
| Redistributions of source code must retain the above copyright notice, this list | ||
| of conditions and the following disclaimer. | ||
| Redistributions in binary form must reproduce the above copyright notice, this | ||
| list of conditions and the following disclaimer in the documentation and/or | ||
| other materials provided with the distribution. | ||
| Neither the name of REV Robotics nor the names of its contributors may be used to | ||
| endorse or promote products derived from this software without specific prior | ||
| written permission. | ||
| NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS | ||
| LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, | ||
| THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
| ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE | ||
| FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL | ||
| DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR | ||
| SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR | ||
| TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF | ||
| THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
| */ | ||
| package org.firstinspires.ftc.robotcontroller.external.samples; | ||
|
|
||
| import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; | ||
| import com.qualcomm.robotcore.eventloop.opmode.Disabled; | ||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
| import com.qualcomm.robotcore.eventloop.opmode.TeleOp; | ||
| import com.qualcomm.robotcore.hardware.IMU; | ||
|
|
||
| import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; | ||
| import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; | ||
| import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; | ||
|
|
||
| /** | ||
| * This file demonstrates the impact of setting the IMU orientation correctly or incorrectly. This | ||
| * code assumes there is an IMU configured with the name "imu". | ||
| * <p> | ||
| * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code | ||
| * goes beyond simply showing how to interface to the IMU.<br> | ||
| * For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes. | ||
| * <p> | ||
| * This sample enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls. | ||
| * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved. | ||
| * <p> | ||
| * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted. <br> | ||
| * The first parameter specifies which direction the printed logo on the Hub is pointing. <br> | ||
| * The second parameter specifies which direction the USB connector on the Hub is pointing. <br> | ||
| * All directions are relative to the robot, and left/right is as viewed from behind the robot. | ||
| * <p> | ||
| * How will you know if you have chosen the correct Orientation? With the correct orientation | ||
| * parameters selected, pitch/roll/yaw should act as follows: | ||
| * <p> | ||
| * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br> | ||
| * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br> | ||
| * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br> | ||
| * <p> | ||
| * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) | ||
| * <p> | ||
| * The rotational velocities should follow the change in corresponding axes. | ||
| */ | ||
|
|
||
| @TeleOp(name="Concept: IMU Orientation", group="Concept") | ||
| @Disabled | ||
| public class ConceptExploringIMUOrientation extends LinearOpMode { | ||
| static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections | ||
| = RevHubOrientationOnRobot.LogoFacingDirection.values(); | ||
| static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections | ||
| = RevHubOrientationOnRobot.UsbFacingDirection.values(); | ||
| static int LAST_DIRECTION = logoFacingDirections.length - 1; | ||
| static float TRIGGER_THRESHOLD = 0.2f; | ||
|
|
||
| IMU imu; | ||
| int logoFacingDirectionPosition; | ||
| int usbFacingDirectionPosition; | ||
| boolean orientationIsValid = true; | ||
|
|
||
| @Override public void runOpMode() throws InterruptedException { | ||
| imu = hardwareMap.get(IMU.class, "imu"); | ||
| logoFacingDirectionPosition = 0; // Up | ||
| usbFacingDirectionPosition = 2; // Forward | ||
|
|
||
| updateOrientation(); | ||
|
|
||
| boolean justChangedLogoDirection = false; | ||
| boolean justChangedUsbDirection = false; | ||
|
|
||
| // Loop until stop requested | ||
| while (!isStopRequested()) { | ||
|
|
||
| // Check to see if Yaw reset is requested (Y button) | ||
| if (gamepad1.y) { | ||
| telemetry.addData("Yaw", "Resetting\n"); | ||
| imu.resetYaw(); | ||
| } else { | ||
| telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n"); | ||
| } | ||
|
|
||
| // Check to see if new Logo Direction is requested | ||
| if (gamepad1.left_bumper || gamepad1.right_bumper) { | ||
| if (!justChangedLogoDirection) { | ||
| justChangedLogoDirection = true; | ||
| if (gamepad1.left_bumper) { | ||
| logoFacingDirectionPosition--; | ||
| if (logoFacingDirectionPosition < 0) { | ||
| logoFacingDirectionPosition = LAST_DIRECTION; | ||
| } | ||
| } else { | ||
| logoFacingDirectionPosition++; | ||
| if (logoFacingDirectionPosition > LAST_DIRECTION) { | ||
| logoFacingDirectionPosition = 0; | ||
| } | ||
| } | ||
| updateOrientation(); | ||
| } | ||
| } else { | ||
| justChangedLogoDirection = false; | ||
| } | ||
|
|
||
| // Check to see if new USB Direction is requested | ||
| if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) { | ||
| if (!justChangedUsbDirection) { | ||
| justChangedUsbDirection = true; | ||
| if (gamepad1.left_trigger > TRIGGER_THRESHOLD) { | ||
| usbFacingDirectionPosition--; | ||
| if (usbFacingDirectionPosition < 0) { | ||
| usbFacingDirectionPosition = LAST_DIRECTION; | ||
| } | ||
| } else { | ||
| usbFacingDirectionPosition++; | ||
| if (usbFacingDirectionPosition > LAST_DIRECTION) { | ||
| usbFacingDirectionPosition = 0; | ||
| } | ||
| } | ||
| updateOrientation(); | ||
| } | ||
| } else { | ||
| justChangedUsbDirection = false; | ||
| } | ||
|
|
||
| // Display User instructions and IMU data | ||
| telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]); | ||
| telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n"); | ||
|
|
||
| if (orientationIsValid) { | ||
| 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); | ||
| } else { | ||
| telemetry.addData("Error", "Selected orientation on robot is invalid"); | ||
| } | ||
|
|
||
| telemetry.update(); | ||
| } | ||
| } | ||
|
|
||
| // apply any requested orientation changes. | ||
| void updateOrientation() { | ||
| RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition]; | ||
| RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition]; | ||
| try { | ||
| RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb); | ||
| imu.initialize(new IMU.Parameters(orientationOnRobot)); | ||
| orientationIsValid = true; | ||
| } catch (IllegalArgumentException e) { | ||
| orientationIsValid = false; | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.