Announcement

Collapse
No announcement yet.

Rev imu

Collapse
X
  • Filter
  • Time
  • Show
Clear All
new posts

  • Rev imu

    Is there an example program to drive a push bot using the REV internal IMU? Or, is there a way to alter the Pushbot drive by gyro which is using the modern robotics gyro instead of the REV IMU. We are trying to learn how to use a gyro before next season.

  • #2
    You could use the Bno055 example code to replace the Modern robotics parts in PushBot.
    It states that it is for the Adafruit, but the REV hub has the same chip. It is the code I used to program the REV HUB Imu.

    I found the BNo055 example hard to figure out, so I simplified a few things. ( I still don't understand half of what's going on in the example and it took a lot of trial and error to actually get a number I could work with.)

    This example rotates the robot without PID so it will overshoot. This assumes Expansion Hub is flat horizontal face up, you want to use IMU mode, and otherwise defaults are used..

    Code:
    /*
    Copyright (c) 2016 Robert Atkinson
    
    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 Robert Atkinson nor the names of his 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 FITNESSFOR 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.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.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.Servo;
    
    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;
    
    @Autonomous(name = "Gyro Dev1 no meth no hard", group = "Sensor")
    //@Disabled                            // Comment this out to add to the opmode list
    public class GyroDev1NoMethNoHard extends LinearOpMode
        {
    
            public DcMotor  FL   = null;
            public DcMotor  FR  = null;
            public DcMotor  BR  = null;
            public DcMotor  BL  = null;
    
            BNO055IMU imu;         // The IMU sensor object
    
            Orientation angles;
    
        @Override public void runOpMode() {
    
    
            // Set up the parameters with which we will use our IMU.
            BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
            parameters.mode = BNO055IMU.SensorMode.IMU;
            // 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);
    
    
            FL  = hardwareMap.dcMotor.get("LF");
            FR = hardwareMap.dcMotor.get("RF");
            BL = hardwareMap.dcMotor.get("LB");
            BR = hardwareMap.dcMotor.get("RB");
    
            FL.setDirection(DcMotor.Direction.FORWARD);
            FR.setDirection(DcMotor.Direction.REVERSE);
            BR.setDirection(DcMotor.Direction.REVERSE);
            BL.setDirection(DcMotor.Direction.FORWARD);
    
             FL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             FR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             BL.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             BR.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            waitForStart();
            double setPoint =90;  //  Angle to stop turning
            // function at bottom allows less typing
            //angles   = getAngles();  //Initially retrieve a new angle number from imu
    ​​​​
            angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
    
             FL.setPower(-.3);
             BL.setPower(-.3);
             FR.setPower(.3);
             BR.setPower(.3);
    
            while (opModeIsActive()&& angles.firstAngle < setPoint) {
                telemetry.update();
                // function at bottom allows less typing
                //angles   = getAngles();
                // get new angle reading  this returns an Orientation.   angles.firstAngle returns a float of the z axis gyro
                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                telemetry.addData("1", "angles.firstAngle " + angles.firstAngle);
            }
    
             FL.setPower(0);
             BL.setPower(0);
             FR.setPower(0);
             BR.setPower(0);
    
            // Leave opmode running to see what angle robot stopped
            while (opModeIsActive() ) {
                telemetry.update();
                // function at bottom allows less typing
                //angles   = getAngles();
                angles   = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                telemetry.addData("1", "angles.firstAngle " + angles.firstAngle);
            }
        }
    
        Orientation getAngles() {return imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); }
    }

    Comment

    Working...
    X