389 lines
17 KiB
Java
389 lines
17 KiB
Java
/* 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);
|
|
}
|
|
|
|
}
|