/* Copyright (c) 2022 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 FIRST 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.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; 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; /** * This file illustrates the concept of driving an autonomous path based on Gyro heading and encoder counts. * The code is structured as a LinearOpMode * * The path to be followed by the robot is built from a series of drive, turn or pause steps. * Each step on the path is defined by a single function call, and these can be strung together in any order. * * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime; * * This code ALSO requires that you have a BOSCH BNO055 IMU, otherwise you would use: RobotAutoDriveByEncoder; * This IMU is found in REV Control/Expansion Hubs shipped prior to July 2022, and possibly also on later models. * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis. * * This sample requires that the drive Motors have been configured with names : left_drive and right_drive. * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP. * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction. * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor. * * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding. * Note: You must call setTargetPosition() at least once before switching to RUN_TO_POSITION mode. * * Notes: * * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called. * In this sample, the heading is reset when the Start button is touched on the Driver station. * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors. * * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis, * which means that a Positive rotation is Counter Clockwise, looking down on the field. * This is consistent with the FTC field coordinate conventions set out in the document: * ftc_app\doc\tutorial\FTC_FieldCoordinateSystemDefinition.pdf * * Control Approach. * * To reach, or maintain a required heading, this code implements a basic Proportional Controller where: * * Steering power = Heading Error * Proportional Gain. * * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading, * and then "normalizing" it by converting it to a value in the +/- 180 degree range. * * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response. * * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list */ @Autonomous(name="Robot: Auto Drive By Gyro", group="Robot") @Disabled public class RobotAutoDriveByGyro_Linear extends LinearOpMode { /* Declare OpMode members. */ private DcMotor leftDrive = null; private DcMotor rightDrive = null; private BNO055IMU imu = null; // Control/Expansion Hub IMU private double robotHeading = 0; private double headingOffset = 0; private double headingError = 0; // These variable are declared here (as class members) so they can be updated in various methods, // but still be displayed by sendTelemetry() private double targetHeading = 0; private double driveSpeed = 0; private double turnSpeed = 0; private double leftSpeed = 0; private double rightSpeed = 0; private int leftTarget = 0; private int rightTarget = 0; // Calculate the COUNTS_PER_INCH for your specific drive train. // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. // This is gearing DOWN for less speed and more torque. // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. static final double COUNTS_PER_MOTOR_REV = 537.7 ; // eg: GoBILDA 312 RPM Yellow Jacket static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415); // These constants define the desired driving/control characteristics // They can/should be tweaked to suit the specific robot drive train. static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy. static final double TURN_SPEED = 0.2; // Max Turn speed to limit turn rate static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step. // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position. // Define the Proportional control coefficient (or GAIN) for "heading control". // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors). // Increase these numbers if the heading does not corrects strongly enough (eg: a heavy robot or using tracks) // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels) static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable @Override public void runOpMode() { // Initialize the drive system variables. leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); // define initialization values for IMU, and then initialize it. BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; imu = hardwareMap.get(BNO055IMU.class, "imu"); imu.initialize(parameters); // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // Wait for the game to start (Display Gyro value while waiting) while (opModeInInit()) { telemetry.addData(">", "Robot Heading = %4.0f", getRawHeading()); telemetry.update(); } // Set the encoders for closed loop speed control, and reset the heading. leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); resetHeading(); // Step through each leg of the path, // Notes: Reverse movement is obtained by setting a negative distance (not speed) // holdHeading() is used after turns to let the heading stabilize // Add a sleep(2000) after any step to keep the telemetry data visible for review driveStraight(DRIVE_SPEED, 24.0, 0.0); // Drive Forward 24" turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees holdHeading( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y) turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y) turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position) telemetry.addData("Path", "Complete"); telemetry.update(); sleep(1000); // Pause to display last telemetry message. } /* * ==================================================================================================== * Driving "Helper" functions are below this line. * These provide the high and low level methods that handle driving straight and turning. * ==================================================================================================== */ // ********** HIGH Level driving functions. ******************** /** * Method to drive in a straight line, on a fixed compass heading (angle), based on encoder counts. * Move will stop if either of these conditions occur: * 1) Move gets to the desired position * 2) Driver stops the opmode running. * * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) . * @param distance Distance (in inches) to move from current position. Negative distance means move backward. * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. * If a relative angle is required, add/subtract from the current robotHeading. */ public void driveStraight(double maxDriveSpeed, double distance, double heading) { // Ensure that the opmode is still active if (opModeIsActive()) { // Determine new target position, and pass to motor controller int moveCounts = (int)(distance * COUNTS_PER_INCH); leftTarget = leftDrive.getCurrentPosition() + moveCounts; rightTarget = rightDrive.getCurrentPosition() + moveCounts; // Set Target FIRST, then turn on RUN_TO_POSITION leftDrive.setTargetPosition(leftTarget); rightDrive.setTargetPosition(rightTarget); leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); // Set the required driving speed (must be positive for RUN_TO_POSITION) // Start driving straight, and then enter the control loop maxDriveSpeed = Math.abs(maxDriveSpeed); moveRobot(maxDriveSpeed, 0); // keep looping while we are still active, and BOTH motors are running. while (opModeIsActive() && (leftDrive.isBusy() && rightDrive.isBusy())) { // Determine required steering to keep on heading turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); // if driving in reverse, the motor correction also needs to be reversed if (distance < 0) turnSpeed *= -1.0; // Apply the turning correction to the current driving speed. moveRobot(driveSpeed, turnSpeed); // Display drive status for the driver. sendTelemetry(true); } // Stop all motion & Turn off RUN_TO_POSITION moveRobot(0, 0); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } } /** * Method to spin on central axis to point in a new direction. * Move will stop if either of these conditions occur: * 1) Move gets to the heading (angle) * 2) Driver stops the opmode running. * * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0) * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. * If a relative angle is required, add/subtract from current heading. */ public void turnToHeading(double maxTurnSpeed, double heading) { // Run getSteeringCorrection() once to pre-calculate the current error getSteeringCorrection(heading, P_DRIVE_GAIN); // keep looping while we are still active, and not on heading. while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) { // Determine required steering to keep on heading turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); // Clip the speed to the maximum permitted value. turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); // Pivot in place by applying the turning correction moveRobot(0, turnSpeed); // Display drive status for the driver. sendTelemetry(false); } // Stop all motion; moveRobot(0, 0); } /** * Method to obtain & hold a heading for a finite amount of time * Move will stop once the requested time has elapsed * This function is useful for giving the robot a moment to stabilize it's heading between movements. * * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0) * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. * If a relative angle is required, add/subtract from current heading. * @param holdTime Length of time (in seconds) to hold the specified heading. */ public void holdHeading(double maxTurnSpeed, double heading, double holdTime) { ElapsedTime holdTimer = new ElapsedTime(); holdTimer.reset(); // keep looping while we have time remaining. while (opModeIsActive() && (holdTimer.time() < holdTime)) { // Determine required steering to keep on heading turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); // Clip the speed to the maximum permitted value. turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); // Pivot in place by applying the turning correction moveRobot(0, turnSpeed); // Display drive status for the driver. sendTelemetry(false); } // Stop all motion; moveRobot(0, 0); } // ********** LOW Level driving functions. ******************** /** * This method uses a Proportional Controller to determine how much steering correction is required. * * @param desiredHeading The desired absolute heading (relative to last heading reset) * @param proportionalGain Gain factor applied to heading error to obtain turning power. * @return Turning power needed to get to required heading. */ public double getSteeringCorrection(double desiredHeading, double proportionalGain) { targetHeading = desiredHeading; // Save for telemetry // Get the robot heading by applying an offset to the IMU heading robotHeading = getRawHeading() - headingOffset; // Determine the heading current error headingError = targetHeading - robotHeading; // Normalize the error to be within +/- 180 degrees while (headingError > 180) headingError -= 360; while (headingError <= -180) headingError += 360; // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 return Range.clip(headingError * proportionalGain, -1, 1); } /** * This method takes separate drive (fwd/rev) and turn (right/left) requests, * combines them, and applies the appropriate speed commands to the left and right wheel motors. * @param drive forward motor speed * @param turn clockwise turning motor speed. */ public void moveRobot(double drive, double turn) { driveSpeed = drive; // save this value as a class member so it can be used by telemetry. turnSpeed = turn; // save this value as a class member so it can be used by telemetry. leftSpeed = drive - turn; rightSpeed = drive + turn; // Scale speeds down if either one exceeds +/- 1.0; double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); if (max > 1.0) { leftSpeed /= max; rightSpeed /= max; } leftDrive.setPower(leftSpeed); rightDrive.setPower(rightSpeed); } /** * Display the various control parameters while driving * * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry. */ private void sendTelemetry(boolean straight) { if (straight) { telemetry.addData("Motion", "Drive Straight"); telemetry.addData("Target Pos L:R", "%7d:%7d", leftTarget, rightTarget); telemetry.addData("Actual Pos L:R", "%7d:%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition()); } else { telemetry.addData("Motion", "Turning"); } telemetry.addData("Angle Target:Current", "%5.2f:%5.0f", targetHeading, robotHeading); telemetry.addData("Error:Steer", "%5.1f:%5.1f", headingError, turnSpeed); telemetry.addData("Wheel Speeds L:R.", "%5.2f : %5.2f", leftSpeed, rightSpeed); telemetry.update(); } /** * read the raw (un-offset Gyro heading) directly from the IMU */ public double getRawHeading() { Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); return angles.firstAngle; } /** * Reset the "offset" heading back to zero */ public void resetHeading() { // Save a new heading offset equal to the current raw heading. headingOffset = getRawHeading(); robotHeading = 0; } }