203 lines
11 KiB
Java
203 lines
11 KiB
Java
package org.firstinspires.ftc.robotcontroller.external.samples;
|
|
|
|
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
import com.qualcomm.robotcore.util.Range;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.ClassFactory;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
|
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
|
|
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
|
|
|
|
/**
|
|
* This OpMode illustrates using a webcam to locate and drive towards ANY Vuforia target.
|
|
* The code assumes a basic two-wheel Robot Configuration with motors named left_drive and right_drive.
|
|
* The motor directions must be set so a positive drive goes forward and a positive turn rotates to the right.
|
|
*
|
|
* Under manual control, the left stick will move forward/back, and the right stick will turn left/right.
|
|
* This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel).
|
|
* Manually drive the robot until it displays Target data on the Driver Station.
|
|
* Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
|
|
* Release the Left Bumper to return to manual driving mode.
|
|
*
|
|
* Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
|
|
* Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants.
|
|
*
|
|
* For more Vuforia details, or to adapt this OpMode for a phone camera, view the
|
|
* ConceptVuforiaFieldNavigation and ConceptVuforiaFieldNavigationWebcam samples.
|
|
*
|
|
* 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.
|
|
*
|
|
* IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
|
|
* is explained below.
|
|
*/
|
|
|
|
@TeleOp(name="Drive To Target", group = "Concept")
|
|
@Disabled
|
|
public class ConceptVuforiaDriveToTargetWebcam extends LinearOpMode
|
|
{
|
|
// Adjust these numbers to suit your robot.
|
|
final double DESIRED_DISTANCE = 8.0; // this is how close the camera should get to the target (inches)
|
|
// The GAIN constants set the relationship between the measured position error,
|
|
// and how much power is applied to the drive motors. Drive = Error * Gain
|
|
// Make these values smaller for smoother control.
|
|
final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
|
|
final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
|
|
|
|
final double MM_PER_INCH = 25.40 ; // Metric conversion
|
|
|
|
/*
|
|
* IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
|
|
* 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
|
|
* A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
|
|
* web site at https://developer.vuforia.com/license-manager.
|
|
*
|
|
* Vuforia license keys are always 380 characters long, and look as if they contain mostly
|
|
* random data. As an example, here is a example of a fragment of a valid key:
|
|
* ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
|
|
* Once you've obtained a license key, copy the string from the Vuforia web site
|
|
* and paste it in to your code on the next line, between the double quotes.
|
|
*/
|
|
private static final String VUFORIA_KEY =
|
|
" --- YOUR NEW VUFORIA KEY GOES HERE --- ";
|
|
|
|
VuforiaLocalizer vuforia = null;
|
|
OpenGLMatrix targetPose = null;
|
|
String targetName = "";
|
|
|
|
private DcMotor leftDrive = null;
|
|
private DcMotor rightDrive = null;
|
|
|
|
@Override public void runOpMode()
|
|
{
|
|
/*
|
|
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
|
|
* To get an on-phone camera preview, use the code below.
|
|
* If no camera preview is desired, use the parameter-less constructor instead (commented out below).
|
|
*/
|
|
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
|
|
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
|
// VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
|
|
|
|
parameters.vuforiaLicenseKey = VUFORIA_KEY;
|
|
|
|
// Turn off Extended tracking. Set this true if you want Vuforia to track beyond the target.
|
|
parameters.useExtendedTracking = false;
|
|
|
|
// Connect to the camera we are to use. This name must match what is set up in Robot Configuration
|
|
parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
|
|
this.vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
|
|
|
// Load the trackable objects from the Assets file, and give them meaningful names
|
|
VuforiaTrackables targetsFreightFrenzy = this.vuforia.loadTrackablesFromAsset("FreightFrenzy");
|
|
targetsFreightFrenzy.get(0).setName("Blue Storage");
|
|
targetsFreightFrenzy.get(1).setName("Blue Alliance Wall");
|
|
targetsFreightFrenzy.get(2).setName("Red Storage");
|
|
targetsFreightFrenzy.get(3).setName("Red Alliance Wall");
|
|
|
|
// Start tracking targets in the background
|
|
targetsFreightFrenzy.activate();
|
|
|
|
// Initialize the hardware variables. Note that the strings used here as parameters
|
|
// to 'get' must correspond to the names assigned during the robot configuration
|
|
// step (using the FTC Robot Controller app on the phone).
|
|
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.
|
|
// Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive.
|
|
leftDrive.setDirection(DcMotor.Direction.FORWARD);
|
|
rightDrive.setDirection(DcMotor.Direction.REVERSE);
|
|
|
|
telemetry.addData(">", "Press Play to start");
|
|
telemetry.update();
|
|
|
|
waitForStart();
|
|
|
|
boolean targetFound = false; // Set to true when a target is detected by Vuforia
|
|
double targetRange = 0; // Distance from camera to target in Inches
|
|
double targetBearing = 0; // Robot Heading, relative to target. Positive degrees means target is to the right.
|
|
double drive = 0; // Desired forward power (-1 to +1)
|
|
double turn = 0; // Desired turning power (-1 to +1)
|
|
|
|
while (opModeIsActive())
|
|
{
|
|
// Look for first visible target, and save its pose.
|
|
targetFound = false;
|
|
for (VuforiaTrackable trackable : targetsFreightFrenzy)
|
|
{
|
|
if (((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible())
|
|
{
|
|
targetPose = ((VuforiaTrackableDefaultListener)trackable.getListener()).getVuforiaCameraFromTarget();
|
|
|
|
// if we have a target, process the "pose" to determine the position of the target relative to the robot.
|
|
if (targetPose != null)
|
|
{
|
|
targetFound = true;
|
|
targetName = trackable.getName();
|
|
VectorF trans = targetPose.getTranslation();
|
|
|
|
// Extract the X & Y components of the offset of the target relative to the robot
|
|
double targetX = trans.get(0) / MM_PER_INCH; // Image X axis
|
|
double targetY = trans.get(2) / MM_PER_INCH; // Image Z axis
|
|
|
|
// target range is based on distance from robot position to origin (right triangle).
|
|
targetRange = Math.hypot(targetX, targetY);
|
|
|
|
// target bearing is based on angle formed between the X axis and the target range line
|
|
targetBearing = Math.toDegrees(Math.asin(targetX / targetRange));
|
|
|
|
break; // jump out of target tracking loop if we find a target.
|
|
}
|
|
}
|
|
}
|
|
|
|
// Tell the driver what we see, and what to do.
|
|
if (targetFound) {
|
|
telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
|
|
telemetry.addData("Target", " %s", targetName);
|
|
telemetry.addData("Range", "%5.1f inches", targetRange);
|
|
telemetry.addData("Bearing","%3.0f degrees", targetBearing);
|
|
} else {
|
|
telemetry.addData(">","Drive using joystick to find target\n");
|
|
}
|
|
|
|
// Drive to target Automatically if Left Bumper is being pressed, AND we have found a target.
|
|
if (gamepad1.left_bumper && targetFound) {
|
|
|
|
// Determine heading and range error so we can use them to control the robot automatically.
|
|
double rangeError = (targetRange - DESIRED_DISTANCE);
|
|
double headingError = targetBearing;
|
|
|
|
// Use the speed and turn "gains" to calculate how we want the robot to move.
|
|
drive = rangeError * SPEED_GAIN;
|
|
turn = headingError * TURN_GAIN ;
|
|
|
|
telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn);
|
|
} else {
|
|
|
|
// drive using manual POV Joystick mode.
|
|
drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
|
|
turn = gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%.
|
|
telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn);
|
|
}
|
|
telemetry.update();
|
|
|
|
// Calculate left and right wheel powers and send to them to the motors.
|
|
double leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
|
|
double rightPower = Range.clip(drive - turn, -1.0, 1.0) ;
|
|
leftDrive.setPower(leftPower);
|
|
rightDrive.setPower(rightPower);
|
|
|
|
sleep(10);
|
|
}
|
|
}
|
|
}
|