Gyroscope

Part 1: Configuring Your Sensor

- Your Gyroscope should already be configured in I2C Port 0 and named “imu”. Check to make sure this is the case:

Part 2: Running a Test OpMode

- Create a new OpMode and called it “DriveAvoidIMU”. Do not use a sample program.

package org.firstinspires.ftc.teamcode;// Simple autonomous program that drives bot forward until end of period // or touch sensor is hit. If touched, backs up a bit and turns 90 degrees
// right and keeps going. Demonstrates obstacle avoidance and use of the // REV Hub's built in IMU in place of a gyro. Also uses gamepad1 buttons to // simulate touch sensor press and supports left as well as right turn.
//
// Also uses IMU to drive in a straight line when not avoiding an obstacle. package org.firstinspires.ftc.teamcode;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DigitalChannel;
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;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
@Autonomous(name="Drive Avoid Imu", group="Exercises")
//@Disabled
public class DriveAvoidIMU extends LinearOpMode
{
    private DcMotor backLeft   = null;
    private DcMotor backRight  = null;
    private DcMotor frontLeft = null;
    private DcMotor frontRight = null;

    BNO055IMU imu;
    Orientation lastAngles = new Orientation();
    double globalAngle, power = .30, correction;
    boolean aButton, bButton;
    // called when init button is pressed.
    @Override
    public void runOpMode() throws InterruptedException
    {
        backLeft  = hardwareMap.get(DcMotor.class, "backLeft");
        backRight = hardwareMap.get(DcMotor.class, "backRight");
        frontLeft   = hardwareMap.get(DcMotor.class, "frontLeft");
        frontRight   = hardwareMap.get(DcMotor.class, "frontRight");
        frontLeft.setDirection(DcMotor.Direction.REVERSE);
        backLeft.setDirection(DcMotor.Direction.REVERSE);


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

        // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port  // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",  // and named "imu".
        imu = hardwareMap.get(BNO055IMU.class, "imu");
        imu.initialize(parameters);
        telemetry.addData("Mode", "calibrating...");
        telemetry.update();

        // make sure the imu gyro is calibrated before continuing.
        while (!isStopRequested() && !imu.isGyroCalibrated())
        {
            sleep(50);
            idle();
        }
        telemetry.addData("Mode", "waiting for start");
        telemetry.addData("imu calib status", imu.getCalibrationStatus().toString());  telemetry.update();
        // wait for start button.
        waitForStart();
        telemetry.addData("Mode", "running");
        telemetry.update();
        sleep(1000);
        // drive until end of period.
        while (opModeIsActive())
        {
            driveStraight();
            // We record the sensor values because we will test them in more than  // one place with time passing between those places. See the lesson on  // Timing Considerations to know why.
            aButton = gamepad1.a;
            bButton = gamepad1.b;
            if (aButton || bButton)
            {
                // backup.
                backLeft.setPower(power);
                backRight.setPower(power);
                frontLeft.setPower(power);
                frontRight.setPower(power);

                sleep(500);
                // stop.
                backLeft.setPower(0);
                backRight.setPower(0);
                frontLeft.setPower(0);
                frontRight.setPower(0);
                // turn 90 degrees right.
                if (aButton)
                {
                    rotate(-90, power);
                }
                // turn 90 degrees left.
                if (bButton)
                {
                    rotate(90, power);
                }
            }
        }
        // turn the motors off when the program stops
        backLeft.setPower(0);
        backRight.setPower(0);
        frontLeft.setPower(0);
        frontRight.setPower(0);
    }
    private void driveStraight()
    {
        // Use gyro to drive in a straight line.
        correction = checkDirection();
        telemetry.addData("1 imu heading", lastAngles.firstAngle);  telemetry.addData("2 global heading", globalAngle);  telemetry.addData("3 correction", correction);  telemetry.update();

        backLeft.setPower(power + correction);
        backRight.setPower(power);
        frontLeft.setPower(power + correction);
        frontRight.setPower(power);

    }
    /**
     * Resets the cumulative angle tracking to zero.  */
    private void resetAngle()
    {
        lastAngles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX,  AngleUnit.DEGREES);
        globalAngle = 0;
    }
    /**
     * Get current cumulative angle rotation from last reset.
     * @return Angle in degrees. + = left, - = right.
     */
    private double getAngle()
    {
        // We experimentally determined the Z axis is the axis we want to use for heading angle.  // We have to process the angle because the imu works in euler angles so the Z axis is  // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes  // 180 degrees. We detect this transition and track the total cumulative angle of rotation.
        Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX,  AngleUnit.DEGREES);
        double deltaAngle = angles.firstAngle - lastAngles.firstAngle;
        if (deltaAngle < -180)
            deltaAngle += 360;
        else if (deltaAngle > 180)
            deltaAngle -= 360;
        globalAngle += deltaAngle;
        lastAngles = angles;
        return globalAngle;
    }
    /**
     * See if we are moving in a straight line and if not return a power correction value.  * @return Power adjustment, + is adjust left - is adjust right.
     */
    private double checkDirection()
    {
        // The gain value determines how sensitive the correction is to direction changes.
        // You will have to experiment with your robot to get small smooth direction changes  // to stay on a straight line.
        double correction, angle;
        double gain = .10;
        angle = getAngle();
        if (angle == 0){
            correction = 0; // no adjustment.
        }
        else{
            correction = -angle; // reverse sign of angle for correction.
            correction = correction * gain;
        }
        return correction;
    }
    /**
     * Rotate left or right the number of degrees. Does not support turning more than 180  degrees.
     * @param degrees Degrees to turn, + is left - is right
     */
    private void rotate(int degrees, double power)
    {
        double leftPower, rightPower;
        // restart imu movement tracking.
        resetAngle();
        // getAngle() returns + when rotating counter clockwise (left) and - when rotating  // clockwise (right).
        if (degrees < 0){
            // turn right.
            leftPower = -power;
            rightPower = power;
        }
        else if (degrees > 0)
        {
            // turn left.
            leftPower = power;
            rightPower = -power;
        }
        else return;

        // set power to rotate.
        backLeft.setPower(leftPower);
        backRight.setPower(rightPower);
        frontLeft.setPower(leftPower);
        frontRight.setPower(rightPower);

        // rotate until turn is completed.
        if (degrees < 0)
        {
            // On right turn we have to get off zero first.
            while (opModeIsActive() && getAngle() == 0) {}
            while (opModeIsActive() && getAngle() > degrees) {}
        }
        else{
            // left turn.
            while (opModeIsActive() && getAngle() < degrees) {}
        }

        // now that we are done turning, turn the motors off.
        backLeft.setPower(0);
        backRight.setPower(0);
        frontLeft.setPower(0);
        frontRight.setPower(0);
        // wait for rotation to stop.
        sleep(100);
        // reset angle tracking on new heading.
        resetAngle();
    }
}

Section 3: Combining Your Range Sensor and Gyroscope for Accurate Movement

- Piece by piece, copy and paste range sensor code into the correct areas of the gyroscope program. Make sure that the hardware initialization goes in its spot, hardwareMap goes in its spot, etc.

- Now in the main loop of your program you will want your routine to look something like this (note: I have placed comments in the code below that represent commands that you will need to write… I’m not giving you ALL the code here!) :

while( rangeSensor.rawUltrasonic() > 20)
{
    //backup straight using gyroscope
}

//rotate 90 degrees counter clockwise using gyroscope while (rangeSensor.rawUltrasonic() > 200)
{
    //drive straight forward using gyroscope feedback
}

//rotate 90 degrees counter clockwise using gyroscope //drop color sensor in between grey and brown blocks
if (sensorColor.alpha() >280)
{
    //rotate robot clockwise to knock it out of position
}

else
{
    //rotate counterclockwise to knock the brown cube out of position 
}