Created
February 20, 2017 02:15
-
-
Save pjtnt11/28e442d111a0a6d860fd1829f8bc7a80 to your computer and use it in GitHub Desktop.
Example code for using the modern robotics gyro.
This file contains hidden or 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
| package org.firstinspires.ftc.teamcode; | |
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | |
| import com.qualcomm.robotcore.hardware.DcMotor; | |
| import com.qualcomm.robotcore.hardware.GyroSensor; | |
| public class GyroExample extends LinearOpMode | |
| { | |
| private DcMotor rightMotor; | |
| private DcMotor leftMotor; | |
| private GyroSensor gyro; | |
| private int lastRawGyroHeading_ = 0; | |
| private int gyroHeading_ = 0; | |
| /** | |
| * The modern robotics gyro only gives values between 0 and 359 degrees. | |
| * This has you will find is a hard value to work with, so what I do is | |
| * translate this value into a move usable value that can go into the | |
| * negatives (counterclockwise) and also into the positives. | |
| * | |
| * For example, if the robot is turned 90 degrees counterclockwise, the | |
| * value that will be returned is -90. | |
| * | |
| * The way that I do this is first by detecting the change from the last | |
| * time that I requested the gyro heading. In most case, all I have to do | |
| * is add or subtract that change from the last known heading, but | |
| * because the gyro only returns a value between 0 and 359, it will jump | |
| * from 0 to 359 when it turns enough. To fix this, I detect when the | |
| * change is greater then 180 and then add any additional degrees. | |
| */ | |
| private int getGyroHeading() | |
| { | |
| int rawGyroHeading = gyro.getHeading(); | |
| // Rollover from 0 to 359 detected. | |
| if (rawGyroHeading - 180 > lastRawGyroHeading_) | |
| { | |
| gyroHeading_ = | |
| gyroHeading_ + rawGyroHeading - 360 - lastRawGyroHeading_; | |
| } | |
| // Roll over from 359 to 0 detected. | |
| else if (rawGyroHeading + 180 < lastRawGyroHeading_) | |
| { | |
| gyroHeading_ = | |
| gyroHeading_ + rawGyroHeading + 360 - lastRawGyroHeading_; | |
| } | |
| // No rollover detected, add the change in angle to the know value. | |
| else | |
| { | |
| gyroHeading_ = | |
| gyroHeading_ + rawGyroHeading - lastRawGyroHeading_; | |
| } | |
| lastRawGyroHeading_ = rawGyroHeading; | |
| return gyroHeading_; | |
| } | |
| /** | |
| * Between turns, you are going to want to reset the gyro heading. | |
| */ | |
| private void resetGyroHeading() | |
| { | |
| lastRawGyroHeading_ = 0; | |
| gyroHeading_ = 0; | |
| gyro.resetZAxisIntegrator(); | |
| } | |
| private void turn(double power, int angle) | |
| { | |
| resetGyroHeading(); | |
| if (opModeIsActive()) | |
| { | |
| // Keeps turning unto the heading that you want is reached. | |
| while (Math.abs(angle) < getGyroHeading() && opModeIsActive()) | |
| { | |
| // (angle / Math.abs(angle) is 1 if angle is positive and -1 | |
| // if angle is negative so the power is set in the right | |
| // direction. | |
| rightMotor.setPower(power * (angle / Math.abs(angle))); | |
| leftMotor.setPower(-power * (angle / Math.abs(angle))); | |
| idle(); | |
| sleep(50); | |
| } | |
| } | |
| rightMotor.setPower(0.0); | |
| leftMotor.setPower(0.0); | |
| } | |
| @Override public void runOpMode() throws InterruptedException | |
| { | |
| rightMotor = hardwareMap.dcMotor.get("right_motor"); | |
| leftMotor = hardwareMap.dcMotor.get("left_motor"); | |
| gyro = hardwareMap.gyroSensor.get("gyro"); | |
| // Before you use the gyro you must ALWAYS calibrate it. If you fail | |
| // to calibrate the gyro, it will not give you correct values or any | |
| // values at all. | |
| gyro.calibrate(); | |
| sleep(1000); | |
| // Waits until the gyro is done calibrating. | |
| while(gyro.isCalibrating()) | |
| { | |
| idle(); | |
| sleep(50); | |
| } | |
| waitForStart(); | |
| // When you start your program, its a good idea to reset the gyro | |
| // becasue the gyro value may have drifted while the refs where | |
| // getting ready to start the game. | |
| resetGyroHeading(); | |
| turn(1.0, 90); | |
| sleep(5000); | |
| turn(1.0, -90); | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment