FTC Programming

Gyro Control

Controlling the Gyro

What is the Gyro?

Inside of the REV Expansion/Control Hubs there is an IMU sensor. Using this sensor, we can retrieve many different values, such as Z axis data(rotation). This is especially helpful to make perfect degree turns, as well as maintain in a heading.

Initializing the Gyro

Initializing the Gyro is just as easy as adding any other hardware. The difference is that we need to also set the parameters of it.

Initializing the Gyro is just as easy as adding any other hardware. The difference is that we need to also set the parameters of it.
public BNO055IMU imu;

BNO055IMU.Parameters params = new BNO055IMU.Parameters();
params.mode = BNO055IMU.SensorMode.IMU;
params.angleUnit = BNO055IMU.AngleUnit.DEGREES;
params.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
params.loggingEnabled = false;

imu = hwMap.get(BNO055IMU.class, "imu");

imu.initialize(params);

Now, let’s see the output. Inside of your while(opModeIsActive()), add the code below:

while(opModeIsActive()) {
    Orientation angles = imu.getAngularOrientation();
    telemetry.addData(“IMU Output”,angles.toString());
    telemetry.update();
}

When you move your robot, this telemetry will update with the imu value.

Forcing the robot to point in a certain direction

Now, let’s force the robot to correct itself. Let’s create a new variable to control the Z axis.

double imuZ = angles.firstAngle;
move(0,(-imuZ)*0.02);

// ADD THIS BEFORE OPMODE ENDS
public void move(double forward, double turn) {
    leftMotor.setPower(forward + turn);
    rightMotor.setPower(forward - turn);
}

Correction in paths

You can modify 0.02 for how fast you want to re correct the robot. Now, let’s change a bit of this up. Rename imuZ to heading, and let’s make the robot go forward. Try pushing it off its path and see what happens.

double heading = angles.firstAngle;
move(0.5,(heading)*0.02);

Turning the heading to something different than 0

Now, create a double-type bearing variable. We will be changing the heading our robot is facing, from 0 to 60 degrees.

double bearing = 60;
double heading = angles.firstAngle;
move(0.5,(bearing-heading)*0.02);

Pushing this into a function

Let’s make everything easier. If you need a referesher/don’t know what/how to use functions, check out this lesson.


    while(opModeIsActive()) {
        moveGyro(0.5,45);
    }
}
public void moveGyro(double forward double bearing) {
    Orientation angles = imu.getAngularOrientation();
    double heading = angles.firstAngle;
    move(forward,(bearing-heading)*0.02);
}

Improving this function with Timers

We can go one step further by adding full time into this, allowing you to use this function in your autonomous.

public void moveGT(double forward double bearing, double sec) {
    ElapsedTime timer = new ElapsedTime();
    timer.reset();
    while (opModeIsActive()) {
        If (timer.seconds > sec) break;
        moveGyro(forward, bearing)
    }
}

public void moveGyro(double forward, double bearing) {
    Orientation angles = imu.getAngularOrientation();
    double heading = angles.firstAngle;
    move(forward,(bearing-heading)*0.02);
}


// Examples
moveGT(0.5,0,3);
moveGT(0.2,60,2);
moveGT(0.3,-90,4);

What next?

There’s a lot of different things you could do with the IMU, especially with encoders. All gyro corrections are power based keep in mind.

chevron_up