Autonomous Objectives
When we decided because of time constraints to drop our particle shooter, our main emphasis in autonomous mode became the claiming of beacons. Here are our objectives:
- Claim at least one and preferably two beacons (30 or 60 points).
- Move to knock the cap ball off of its starting position (5 points).
- Park with at least one wheel on the center or corner areas (5 points).
We examined the results of all FTC matches this season so far (about 900 of them), and discovered that only about 10% of the alliances were able to claim a beacon in autonomous mode. If we can claim two, we will be ahead of most other teams.
Sensors Used
- The primary sensors used for autonomous mode by our robot are the shaft position encoders on our four drive motors. By using them and the RUN_TO_POSITION control provided by the Modern Robotics Motor Controller, we can dead reckon navigate with surprising accuracy – within a couple of inches in translation and a few degrees in rotation.
- To position the robot to push a beacon button, we use an active light sensor located about 1/4″ above the floor to find the white tape stripe that extends two feet in front of each beacon’s center line. We use the Modern Robotics Optical Distance Sensor, although in this case we are sensing floor reflectance from a constant distance rather than sensing distance from a surface of constant reflectance.
- To determine whether the beacon color at the button the robot is positioned to push, we use a home made passive color sensor made from a Cadmium Sulfide photo resistor with a red filter in front of it. To this sensor, the red light is much brighter than the blue.
Key Algorithms
We developed a menu of algorithms and coded these as separate methods for the autonomous opmode to call:
A. moveForward(int howMuch, double speed)
howMuch is in inches, speed is from 0.1 (slow) to 0.5 (fast). A negative howMuch moves the robot backward.
B. moveRight(int howMuch, double speed)
The robot strafes to the right without rotating using the Mecanum drive. A negative howMuch moves to the left.
C. turnClockwise(int whatAngle, double speed)
The robot rotates whatAngle degrees about its drive center. A negative whatAngle turns counterclockwise.
D. moveToLine(int howMuch, double speed)
This method assumes the robot is facing the wall about 6″ away from it and located on a known side of the beacon white line. A positive howMuch causes the robot to strafe left. The distance should be greater than the expected range to the line. When the line is detected by the mrOds floor sensor, the robot immediately stops.
E. pushRedButton() pushBlueButton()
This method assumes that the robot is positioned in front of the left beacon button and about 6″ away. If the redEye sensor sees the wrong color, the robot is commanded to strafe right for 5″ to position in front of the other button. The robot is then moved slowly forward to claim the beacon. A slow speed/low drive power is used so that over-travel does not damage the beacon, wall, or robot.
Example Path
Complete listing of one of our autonomous mode programs:
package org.firstinspires.ftc.teamcode;
/*
*/
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
/**
* Team 9960 Revision 170119.0
* This program is the scaffold for autonomous operation.
* Designed to push the correct button on both beacons
*
* This robot uses four VEX Mecanum wheels, each direct driven by Neverest 40 motors.
* It is designed as a linear op mode, and uses RUN_TO_POSITION motor operation.
*
*/
@Autonomous(name="AutoRedBoth", group="Linear Opmode") // @TeleOp(...) is the other common choice
// @Disabled
public class AutoRedBoth extends LinearOpMode {
// Declare Devices
DcMotor leftFrontMotor = null;
DcMotor rightFrontMotor = null;
DcMotor leftRearMotor = null;
DcMotor rightRearMotor = null;
AnalogInput redEye = null;
OpticalDistanceSensor mrOds = null;
// drive motor position variables
private int lfPos; private int rfPos; private int lrPos; private int rrPos;
// operational constants
private double fast = 0.5; // Limit motor power to this value for Andymark RUN_USING_ENCODER mode
private double medium = 0.3; // medium speed
private double slow = 0.1; // slow speed
private double clicksPerInch = 87.5; // empirically measured
private double clicksPerDeg = 21.94; // empirically measured
private double lineThreshold = 0.7; // floor should be below this value, line above
private double redThreshold = 1.9; // red should be below this value, blue above
@Override
public void runOpMode() {
telemetry.setAutoClear(true);
// Initialize the hardware variables.
leftFrontMotor = hardwareMap.dcMotor.get("leftFront");
rightFrontMotor = hardwareMap.dcMotor.get("rightFront");
leftRearMotor = hardwareMap.dcMotor.get("leftRear");
rightRearMotor = hardwareMap.dcMotor.get("rightRear");
redEye = hardwareMap.analogInput.get("redEye");
mrOds = hardwareMap.opticalDistanceSensor.get("mrOds");
mrOds.enableLed(true);
// The right motors need reversing
rightFrontMotor.setDirection(DcMotor.Direction.REVERSE);
leftFrontMotor.setDirection(DcMotor.Direction.FORWARD);
rightRearMotor.setDirection(DcMotor.Direction.REVERSE);
leftRearMotor.setDirection(DcMotor.Direction.FORWARD);
// Set the drive motor run modes:
leftFrontMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightFrontMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftRearMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightRearMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftFrontMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rightFrontMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
leftRearMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rightRearMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// Wait for the game to start (driver presses PLAY)
waitForStart();
// *****************Dead reckoning list*************
// Distances in inches, angles in deg, speed 0.0 to 0.6
moveForward(16, fast);
turnClockwise(-45, fast);
moveForward(33, fast);
turnClockwise(-45, fast);
moveForward(24, fast);
moveToLine(24, medium);
pushRedButton();
moveForward(-6, fast);
turnClockwise(-3, medium); // aiming tweak
moveRight(36, fast);
moveToLine(24, medium);
pushRedButton();
moveForward(-12, fast);
turnClockwise(-135, fast);
moveForward(66, fast);
}
private void moveForward(int howMuch, double speed) {
// howMuch is in inches. A negative howMuch moves backward.
// fetch motor positions
lfPos = leftFrontMotor.getCurrentPosition();
rfPos = rightFrontMotor.getCurrentPosition();
lrPos = leftRearMotor.getCurrentPosition();
rrPos = rightRearMotor.getCurrentPosition();
// calculate new targets
lfPos += howMuch * clicksPerInch;
rfPos += howMuch * clicksPerInch;
lrPos += howMuch * clicksPerInch;
rrPos += howMuch * clicksPerInch;
// move robot to new position
leftFrontMotor.setTargetPosition(lfPos);
rightFrontMotor.setTargetPosition(rfPos);
leftRearMotor.setTargetPosition(lrPos);
rightRearMotor.setTargetPosition(rrPos);
leftFrontMotor.setPower(speed);
rightFrontMotor.setPower(speed);
leftRearMotor.setPower(speed);
rightRearMotor.setPower(speed);
// wait for move to complete
while (leftFrontMotor.isBusy() && rightFrontMotor.isBusy() &&
leftRearMotor.isBusy() && rightRearMotor.isBusy()) {
// Display it for the driver.
telemetry.addLine("Move Foward");
telemetry.addData("Target", "%7d :%7d", lfPos, rfPos, lrPos, rrPos);
telemetry.addData("Actual", "%7d :%7d", leftFrontMotor.getCurrentPosition(),
rightFrontMotor.getCurrentPosition(), leftRearMotor.getCurrentPosition(),
rightRearMotor.getCurrentPosition());
telemetry.update();
}
// Stop all motion;
leftFrontMotor.setPower(0);
rightFrontMotor.setPower(0);
leftRearMotor.setPower(0);
rightRearMotor.setPower(0);
}
private void moveRight(int howMuch, double speed) {
// howMuch is in inches. A negative howMuch moves backward.
// fetch motor positions
lfPos = leftFrontMotor.getCurrentPosition();
rfPos = rightFrontMotor.getCurrentPosition();
lrPos = leftRearMotor.getCurrentPosition();
rrPos = rightRearMotor.getCurrentPosition();
// calculate new targets
lfPos += howMuch * clicksPerInch;
rfPos -= howMuch * clicksPerInch;
lrPos -= howMuch * clicksPerInch;
rrPos += howMuch * clicksPerInch;
// move robot to new position
leftFrontMotor.setTargetPosition(lfPos);
rightFrontMotor.setTargetPosition(rfPos);
leftRearMotor.setTargetPosition(lrPos);
rightRearMotor.setTargetPosition(rrPos);
leftFrontMotor.setPower(speed);
rightFrontMotor.setPower(speed);
leftRearMotor.setPower(speed);
rightRearMotor.setPower(speed);
// wait for move to complete
while (leftFrontMotor.isBusy() && rightFrontMotor.isBusy() &&
leftRearMotor.isBusy() && rightRearMotor.isBusy()) {
// Display it for the driver.
telemetry.addLine("Strafe Right");
telemetry.addData("Target", "%7d :%7d", lfPos, rfPos, lrPos, rrPos);
telemetry.addData("Actual", "%7d :%7d", leftFrontMotor.getCurrentPosition(),
rightFrontMotor.getCurrentPosition(), leftRearMotor.getCurrentPosition(),
rightRearMotor.getCurrentPosition());
telemetry.update();
}
// Stop all motion;
leftFrontMotor.setPower(0);
rightFrontMotor.setPower(0);
leftRearMotor.setPower(0);
rightRearMotor.setPower(0);
}
private void turnClockwise(int whatAngle, double speed) {
// whatAngle is in degrees. A negative whatAngle turns counterclockwise.
// fetch motor positions
lfPos = leftFrontMotor.getCurrentPosition();
rfPos = rightFrontMotor.getCurrentPosition();
lrPos = leftRearMotor.getCurrentPosition();
rrPos = rightRearMotor.getCurrentPosition();
// calculate new targets
lfPos += whatAngle * clicksPerDeg;
rfPos -= whatAngle * clicksPerDeg;
lrPos += whatAngle * clicksPerDeg;
rrPos -= whatAngle * clicksPerDeg;
// move robot to new position
leftFrontMotor.setTargetPosition(lfPos);
rightFrontMotor.setTargetPosition(rfPos);
leftRearMotor.setTargetPosition(lrPos);
rightRearMotor.setTargetPosition(rrPos);
leftFrontMotor.setPower(speed);
rightFrontMotor.setPower(speed);
leftRearMotor.setPower(speed);
rightRearMotor.setPower(speed);
// wait for move to complete
while (leftFrontMotor.isBusy() && rightFrontMotor.isBusy() &&
leftRearMotor.isBusy() && rightRearMotor.isBusy()) {
// Display it for the driver.
telemetry.addLine("Turn Clockwise");
telemetry.addData("Target", "%7d :%7d", lfPos, rfPos, lrPos, rrPos);
telemetry.addData("Actual", "%7d :%7d", leftFrontMotor.getCurrentPosition(),
rightFrontMotor.getCurrentPosition(), leftRearMotor.getCurrentPosition(),
rightRearMotor.getCurrentPosition());
telemetry.update();
}
// Stop all motion;
leftFrontMotor.setPower(0);
rightFrontMotor.setPower(0);
leftRearMotor.setPower(0);
rightRearMotor.setPower(0);
}
private void moveToLine(int howMuch, double speed) {
// howMuch is in inches. The robot will stop if the line is found before
// this distance is reached. A negative howMuch moves left, positive moves right.
// fetch motor positions
lfPos = leftFrontMotor.getCurrentPosition();
rfPos = rightFrontMotor.getCurrentPosition();
lrPos = leftRearMotor.getCurrentPosition();
rrPos = rightRearMotor.getCurrentPosition();
// calculate new targets
lfPos += howMuch * clicksPerInch;
rfPos -= howMuch * clicksPerInch;
lrPos -= howMuch * clicksPerInch;
rrPos += howMuch * clicksPerInch;
// move robot to new position
leftFrontMotor.setTargetPosition(lfPos);
rightFrontMotor.setTargetPosition(rfPos);
leftRearMotor.setTargetPosition(lrPos);
rightRearMotor.setTargetPosition(rrPos);
leftFrontMotor.setPower(speed);
rightFrontMotor.setPower(speed);
leftRearMotor.setPower(speed);
rightRearMotor.setPower(speed);
// wait for move to complete
while (leftFrontMotor.isBusy() && rightFrontMotor.isBusy() &&
leftRearMotor.isBusy() && rightRearMotor.isBusy()) {
if (mrOds.getLightDetected() > lineThreshold) break;
// Display it for the driver.
telemetry.addLine("Move To Line");
telemetry.addData("Target", "%7d :%7d", lfPos, rfPos, lrPos, rrPos);
telemetry.addData("Actual", "%7d :%7d", leftFrontMotor.getCurrentPosition(),
rightFrontMotor.getCurrentPosition(), leftRearMotor.getCurrentPosition(),
rightRearMotor.getCurrentPosition());
telemetry.update();
}
// Stop all motion;
leftFrontMotor.setPower(0);
rightFrontMotor.setPower(0);
leftRearMotor.setPower(0);
rightRearMotor.setPower(0);
}
private void pushRedButton() {
// Red alliance version
// Assumes prepositioned in front of left button
// if beacon is red, push it, otherwise move right 5" and push the other button
if (redEye.getVoltage() > redThreshold) moveRight(5, slow); // we see a blue beacon
moveForward(8, slow);
}
private void pushBlueButton() {
// Blue alliance version
// Assumes prepositioned in front of left button
// if beacon is blue, push it, otherwise move right 5" and push the other button
if (redEye.getVoltage() < redThreshold) moveRight(5, slow); // we see a red beacon
moveForward(8, slow);
}
}