/* Copyright (c) 2017 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.modernrobotics.ModernRoboticsI2cGyro; 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; /** * This file illustrates the concept of driving a path based on Gyro heading and encoder counts. * The code is structured as a LinearOpMode * * The code REQUIRES that you DO have encoders on the wheels, * otherwise you would use: RobotAutoDriveByTime; * * This code ALSO requires that you have a Modern Robotics I2C gyro with the name "gyro" * otherwise you would use: RobotAutoDriveByEncoder; * * This code requires that the drive Motors have been configured such that a positive * power command moves them forward, and causes the encoders to count UP. * * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile * * In order to calibrate the Gyro correctly, the robot must remain stationary during calibration. * This is performed when the INIT button is pressed on the Driver Station. * This code assumes that the robot is stationary when the INIT button is pressed. * If this is not the case, then the INIT should be performed again. * * Note: in this example, all angles are referenced to the initial coordinate frame set during the * the Gyro Calibration process, or whenever the program issues a resetZAxisIntegrator() call on the Gyro. * * 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 * * Use Android Studio to Copy this Class, and Paste it into your team's code 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; ModernRoboticsI2cGyro gyro = null; // Additional Gyro device private double robotHeading = 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 = 1440 ; // eg: TETRIX Motor Encoder 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 // The can/should be tweaked to suite the specific robot drive train. static final double DRIVE_SPEED = 0.7; // Nominal speed for better accuracy. static final double TURN_SPEED = 0.5; // Nominal half speed for better accuracy. static final double HEADING_THRESHOLD = 1 ; // As tight as we can make it with an integer gyro static final double P_TURN_COEFF = 0.1; // Larger is more responsive, but also less stable static final double P_DRIVE_COEFF = 0.15; // 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); gyro = (ModernRoboticsI2cGyro)hardwareMap.gyroSensor.get("gyro"); // Ensure the robot it stationary, then reset the encoders and calibrate the gyro. leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); // Send telemetry message to alert driver that we are calibrating; telemetry.addData(">", "Calibrating Gyro"); // telemetry.update(); gyro.calibrate(); // make sure the gyro is calibrated before continuing while (!isStopRequested() && gyro.isCalibrating()) { sleep(50); idle(); } telemetry.addData(">", "Robot Ready."); // telemetry.update(); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // Wait for the game to start (Display Gyro value) while (opModeInInit()) { telemetry.addData(">", "Robot Heading = %4.0f", getHeading()); telemetry.update(); } // Reset gyro before we move.. gyro.resetZAxisIntegrator(); getHeading(); // Step through each leg of the path, // Note: Reverse movement is obtained by setting a negative distance (not speed) // Put a hold after each turn gyroDrive(DRIVE_SPEED, 48.0, 0.0); // Drive FWD 48 inches gyroTurn( TURN_SPEED, -45.0); // Turn CW to -45 Degrees gyroHold( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second gyroDrive(DRIVE_SPEED, 12.0, -45.0); // Drive FWD 12 inches at 45 degrees gyroTurn( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees gyroHold( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second gyroTurn( TURN_SPEED, 0.0); // Turn CW to 0 Degrees gyroHold( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for a 1 second gyroDrive(DRIVE_SPEED,-48.0, 0.0); // Drive REV 48 inches telemetry.addData("Path", "Complete"); telemetry.addData("Final Heading", "%5.0f", getHeading()); telemetry.update(); sleep(1000); // Pause to display last telemetry message. } /** * Method to drive on a fixed compass bearing (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 speed Target speed for forward motion. Should allow for +/- variance for adjusting heading * @param distance Distance (in inches) to move from current position. Negative distance means move backward. * @param angle Absolute 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 gyroDrive ( double speed, double distance, double angle) { int newLeftTarget; int newRightTarget; int moveCounts; double max; double error; double steer; double leftSpeed; double rightSpeed; // Ensure that the opmode is still active if (opModeIsActive()) { // Determine new target position, and pass to motor controller moveCounts = (int)(distance * COUNTS_PER_INCH); newLeftTarget = leftDrive.getCurrentPosition() + moveCounts; newRightTarget = rightDrive.getCurrentPosition() + moveCounts; // Set Target and Turn On RUN_TO_POSITION leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); // start motion. speed = Range.clip(Math.abs(speed), 0.0, 1.0); leftDrive.setPower(speed); rightDrive.setPower(speed); // keep looping while we are still active, and BOTH motors are running. while (opModeIsActive() && (leftDrive.isBusy() && rightDrive.isBusy())) { // adjust relative speed based on heading error. error = getError(angle); steer = getSteer(error, P_DRIVE_COEFF); // if driving in reverse, the motor correction also needs to be reversed if (distance < 0) steer *= -1.0; leftSpeed = speed - steer; rightSpeed = speed + steer; // Normalize speeds if either one exceeds +/- 1.0; max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); if (max > 1.0) { leftSpeed /= max; rightSpeed /= max; } leftDrive.setPower(leftSpeed); rightDrive.setPower(rightSpeed); // Display drive status for the driver. telemetry.addData("Angle Target:Current", "%5.2f:%5.0f", angle, robotHeading); telemetry.addData("Error:Steer", "%5.1f:%5.1f", error, steer); telemetry.addData("Target L:R", "%7d:%7d", newLeftTarget, newRightTarget); telemetry.addData("Actual L:R", "%7d:%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition()); telemetry.addData("Speed L:R", "%5.2f:%5.2f", leftSpeed, rightSpeed); telemetry.update(); } // Stop all motion; leftDrive.setPower(0); rightDrive.setPower(0); // Turn off RUN_TO_POSITION 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 speed Desired speed of turn. * @param angle Absolute 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 gyroTurn ( double speed, double angle) { // keep looping while we are still active, and not on heading. while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) { // Update telemetry & Allow time for other processes to run. telemetry.update(); } } /** * Method to obtain & hold a heading for a finite amount of time * Move will stop once the requested time has elapsed * * @param speed Desired speed of turn. * @param angle Absolute 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 gyroHold( double speed, double angle, double holdTime) { ElapsedTime holdTimer = new ElapsedTime(); // keep looping while we have time remaining. holdTimer.reset(); while (opModeIsActive() && (holdTimer.time() < holdTime)) { // Update telemetry & Allow time for other processes to run. onHeading(speed, angle, P_TURN_COEFF); telemetry.update(); } // Stop all motion; leftDrive.setPower(0); rightDrive.setPower(0); } /** * Perform one cycle of closed loop heading control. * * @param speed Desired speed of turn. * @param angle Absolute 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 PCoeff Proportional Gain coefficient * @return */ boolean onHeading(double speed, double angle, double PCoeff) { double error ; double steer ; boolean onTarget = false ; double leftSpeed; double rightSpeed; // determine turn power based on +/- error error = getError(angle); if (Math.abs(error) <= HEADING_THRESHOLD) { steer = 0.0; leftSpeed = 0.0; rightSpeed = 0.0; onTarget = true; } else { steer = getSteer(error, PCoeff); rightSpeed = speed * steer; leftSpeed = -rightSpeed; } // Send desired speeds to motors. leftDrive.setPower(leftSpeed); rightDrive.setPower(rightSpeed); // Display it for the driver. telemetry.addData("Target/Current", "%5.2f / %5.0f", angle, robotHeading); telemetry.addData("Error/Steer", "%5.2f / %5.2f", error, steer); telemetry.addData("Speed.", "%5.2f : %5.2f", leftSpeed, rightSpeed); return onTarget; } /** * getError determines the error between the target angle and the robot's current heading * @param targetAngle Desired angle (relative to global reference established at last Gyro Reset). * @return error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference * +ve error means the robot should turn LEFT (CCW) to reduce error. */ public double getError(double targetAngle) { double robotError; // calculate error in -179 to +180 range ( robotError = targetAngle - getHeading(); while (robotError > 180) robotError -= 360; while (robotError <= -180) robotError += 360; return robotError; } /** * read and save the current robot heading */ public double getHeading() { robotHeading = (double)gyro.getIntegratedZValue(); return robotHeading; } /** * returns desired steering force. +/- 1 range. +ve = steer left * @param error Error angle in robot relative degrees * @param PCoeff Proportional Gain Coefficient * @return */ public double getSteer(double error, double PCoeff) { return Range.clip(error * PCoeff, -1, 1); } }